/**
 * @file    startup_bootloader.s
 * @brief   Bootloader startup file for STM32G474RET6
 *
 * Vector table at 0x08000000.
 * Stack pointer in first word, reset handler in second.
 */

.syntax unified
.cpu cortex-m4
.fpu softvfp
.thumb

/* Stack top from linker script */
.word   _estack
.word   Reset_Handler
.word   NMI_Handler
.word   HardFault_Handler
.word   MemManage_Handler
.word   BusFault_Handler
.word   UsageFault_Handler
.word   0               /* Reserved */
.word   0               /* Reserved */
.word   0               /* Reserved */
.word   0               /* Reserved */
.word   SVC_Handler
.word   DebugMon_Handler
.word   0               /* Reserved */
.word   PendSV_Handler
.word   SysTick_Handler

/* External interrupts (STM32G474RET6 - 87 IRQs) */
.rept 87
.word   Default_Handler
.endr

/* Reset handler */
.section .text.Reset_Handler
.weak   Reset_Handler
.type   Reset_Handler, %function
Reset_Handler:
    /* Set stack pointer */
    ldr   sp, =_estack

    /* Copy .data from flash to SRAM */
    ldr   r1, =_sidata
    ldr   r2, =_sdata
    ldr   r3, =_edata
    subs  r3, r2
    ble   .L_clear_bss
.L_copy_data:
    ldrb  r4, [r1], #1
    strb  r4, [r2], #1
    subs  r3, #1
    bgt   .L_copy_data

.L_clear_bss:
    ldr   r2, =_sbss
    ldr   r3, =_ebss
    movs  r0, #0
    subs  r3, r2
    ble   .L_init_done
.L_zero_bss:
    strb  r0, [r2], #1
    subs  r3, #1
    bgt   .L_zero_bss

.L_init_done:
    /* Set vector table (bootloader stays at 0x08000000) */
    ldr   r0, =0x08000000
    ldr   r1, =0xE000ED08  /* SCB->VTOR */
    str   r0, [r1]

    /* Initialize system clock */
    bl    SystemClock_Init

    /* Initialize SPI Flash */
    bl    FLASH_Init

    /* Call main bootloader */
    bl    bootloader_main

    /* Should never return, but just in case */
.L_loop:
    b     .L_loop
.size Reset_Handler, .-Reset_Handler

/* System clock init: HSE 8MHz -> PLL -> 72MHz */
.section .text.SystemClock_Init
.weak   SystemClock_Init
.type   SystemClock_Init, %function
SystemClock_Init:
    /* Enable HSE */
    ldr   r0, =0x40023800   /* RCC base */
    ldr   r1, [r0, #0x00]  /* RCC_CR */
    orr   r1, r1, #0x00010000  /* HSEON */
    str   r1, [r0, #0x00]

.L_wait_hse:
    ldr   r1, [r0, #0x00]
    tst   r1, #0x00020000  /* HSERDY */
    beq   .L_wait_hse

    /* Configure flash latency: 2 WS for 72MHz */
    ldr   r1, =0x40022000   /* FLASH_ACR */
    ldr   r2, [r1]
    orr   r2, r2, #0x05     /* PRFTEN | LATENCY=2 */
    str   r2, [r1]

    /* Configure PLL: HSE /2 *18 /2 = 72MHz */
    /* PLLM=2, PLLN=18, PLLP=0 (/2), PLLQ=2 */
    ldr   r1, =0x40023804   /* RCC_PLLCFG */
    ldr   r2, =(2 << 0) | (18 << 8) | (0 << 16) | (2 << 24)
    str   r2, [r1]

    /* Enable PLL */
    ldr   r1, [r0, #0x00]
    orr   r1, r1, #0x01000000  /* PLLON */
    str   r1, [r0, #0x00]
.L_wait_pll:
    ldr   r1, [r0, #0x00]
    tst   r1, #0x02000000  /* PLLRDY */
    beq   .L_wait_pll

    /* Switch system clock to PLL */
    ldr   r1, [r0, #0x08]  /* RCC_CFGR */
    bic   r1, r1, #0x03
    orr   r1, r1, #0x02    /* SW=PLL */
    str   r1, [r0, #0x08]
.L_wait_sw:
    ldr   r1, [r0, #0x08]
    and   r1, r1, #0x0C    /* SWS bits */
    cmp   r1, #0x08        /* PLL selected? */
    bne   .L_wait_sw

    /* Enable CRYP clock */
    ldr   r1, [r0, #0x34]  /* RCC_AHB2ENR */
    orr   r1, r1, #0x40    /* CRYPEN = bit 6 */
    str   r1, [r0, #0x34]

    bx    lr

/* Default weak handlers */
.macro weak_handler name
.section .text.Default_Handler
.weak \name
.type \name, %function
\name:
    b     .
.endm

weak_handler NMI_Handler
weak_handler HardFault_Handler
weak_handler MemManage_Handler
weak_handler BusFault_Handler
weak_handler UsageFault_Handler
weak_handler SVC_Handler
weak_handler DebugMon_Handler
weak_handler PendSV_Handler
weak_handler SysTick_Handler
weak_handler Default_Handler

.end