#include "main.h"
#include "usb_device.h"
#include "usbd_dfu_bootloader.h"

typedef void (*ApplicationEntry)(void);

static void Bootloader_SystemClockConfig(void);
static void Bootloader_GPIOInit(void);
static uint8_t Bootloader_IsApplicationValid(uint32_t address);
static void Bootloader_JumpToApplication(uint32_t address);

int main(void)
{
  uint32_t selectedAddress = 0U;

  HAL_Init();
  Bootloader_GPIOInit();
  HAL_Delay(20U);

  /* Holding START during reset always forces USB DFU mode. */
  if (HAL_GPIO_ReadPin(Start_GPIO_Port, Start_Pin) != GPIO_PIN_RESET)
  {
    GPIO_PinState pa4 = HAL_GPIO_ReadPin(DINPUT_GPIO_Port, DINPUT_Pin);
    GPIO_PinState pa5 = HAL_GPIO_ReadPin(XINPUT_GPIO_Port, XINPUT_Pin);

    /* Actual board wiring: PA4 LOW selects XInput, PA5 LOW selects DInput. */
    if ((pa4 == GPIO_PIN_RESET) && (pa5 == GPIO_PIN_SET))
    {
      selectedAddress = DFU_APP1_ADDRESS;
    }
    else if ((pa4 == GPIO_PIN_SET) && (pa5 == GPIO_PIN_RESET))
    {
      selectedAddress = DFU_APP2_ADDRESS;
    }
  }

  if ((selectedAddress != 0U) && Bootloader_IsApplicationValid(selectedAddress))
  {
    Bootloader_JumpToApplication(selectedAddress);
  }

  Bootloader_SystemClockConfig();
  MX_USB_DEVICE_Init();

  while (1)
  {
  }
}

static void Bootloader_GPIOInit(void)
{
  GPIO_InitTypeDef gpio = {0};

  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();

  gpio.Pin = DINPUT_Pin | XINPUT_Pin;
  gpio.Mode = GPIO_MODE_INPUT;
  gpio.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(GPIOA, &gpio);

  gpio.Pin = Start_Pin;
  HAL_GPIO_Init(GPIOC, &gpio);
}

static uint8_t Bootloader_IsApplicationValid(uint32_t address)
{
  uint32_t stackPointer = *(volatile uint32_t *)address;
  uint32_t resetHandler = *(volatile uint32_t *)(address + 4U);
  uint32_t slotEnd = address + DFU_APP_SLOT_SIZE;

  return ((stackPointer >= SRAM_BASE) &&
          (stackPointer <= (SRAM_BASE + (20U * 1024U))) &&
          ((resetHandler & 1U) != 0U) &&
          ((resetHandler & ~1U) >= address) &&
          ((resetHandler & ~1U) < slotEnd)) ? 1U : 0U;
}

static void Bootloader_JumpToApplication(uint32_t address)
{
  uint32_t stackPointer = *(volatile uint32_t *)address;
  uint32_t resetHandler = *(volatile uint32_t *)(address + 4U);

  __disable_irq();
  SysTick->CTRL = 0U;
  SysTick->LOAD = 0U;
  SysTick->VAL = 0U;
  HAL_DeInit();

  NVIC->ICER[0] = 0xFFFFFFFFU;
  NVIC->ICER[1] = 0xFFFFFFFFU;
  NVIC->ICPR[0] = 0xFFFFFFFFU;
  NVIC->ICPR[1] = 0xFFFFFFFFU;

  SCB->VTOR = address;
  __DSB();
  __ISB();
  __enable_irq();

  __asm volatile (
      "msr msp, %0\n"
      "bx %1\n"
      :
      : "r" (stackPointer), "r" (resetHandler)
      : "memory");

  while (1)
  {
  }
}

static void Bootloader_SystemClockConfig(void)
{
  RCC_OscInitTypeDef oscillator = {0};
  RCC_ClkInitTypeDef clocks = {0};
  RCC_PeriphCLKInitTypeDef peripheral = {0};

  oscillator.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  oscillator.HSEState = RCC_HSE_ON;
  oscillator.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  oscillator.HSIState = RCC_HSI_ON;
  oscillator.PLL.PLLState = RCC_PLL_ON;
  oscillator.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  oscillator.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&oscillator) != HAL_OK)
  {
    Error_Handler();
  }

  clocks.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
                     RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  clocks.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  clocks.AHBCLKDivider = RCC_SYSCLK_DIV1;
  clocks.APB1CLKDivider = RCC_HCLK_DIV2;
  clocks.APB2CLKDivider = RCC_HCLK_DIV1;
  if (HAL_RCC_ClockConfig(&clocks, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }

  peripheral.PeriphClockSelection = RCC_PERIPHCLK_USB;
  peripheral.UsbClockSelection = RCC_USBCLKSOURCE_PLL_DIV1_5;
  if (HAL_RCCEx_PeriphCLKConfig(&peripheral) != HAL_OK)
  {
    Error_Handler();
  }
}

void Error_Handler(void)
{
  __disable_irq();
  while (1)
  {
  }
}