
#include <global.h>
#include <string.h>

int main(void) {
    /* Enable HSE w/ 12 MHz crystal */
    RCC->CR |= RCC_CR_HSEON;
    while (!(RCC->CR & RCC_CR_HSERDY)) {
        /* Wait for HSE to stabilize */
    }

    /* We're already in voltage scaling range 1. Enter boost mode (set main internal VDD from 1.20V to 1.28V) */
    RCC->APB1ENR1 |= RCC_APB1ENR1_PWREN;
    PWR->CR5 &= ~PWR_CR5_R1MODE;

    /* Increase flash wait states to 5 required for operation above 136 MHz */
    FLASH->ACR = (FLASH->ACR & ~FLASH_ACR_LATENCY_Msk) | (4<<FLASH_ACR_LATENCY_Pos);
    while ((FLASH->ACR & FLASH_ACR_LATENCY_Msk) != (4<<FLASH_ACR_LATENCY_Pos)) {
        /* wait for flash controller to acknowledge change. */
    }
    
    /* Configure PLL with multiplier 28, divisor 2 for "R" output, and enable "R" (sysclk) output */
    RCC->PLLCFGR = (RCC->PLLCFGR & ~RCC_PLLCFGR_PLLN_Msk) | (28<<RCC_PLLCFGR_PLLN_Pos) | (3<<RCC_PLLCFGR_PLLSRC_Pos) | RCC_PLLCFGR_PLLREN;
    RCC->CR |= RCC_CR_PLLON;
    while (!(RCC->CR & RCC_CR_PLLRDY)) {
        /* wait for PLL to stabilize. */
    }
    /* PLL clocks now:
     *   R = 168 MHz
     *   Q = 168 MHz
     *   P = 19.765 MHz
     *
     *   System clocks:
     *   All undivided at 168 MHz.
     */
    
    /* Switch SYSCLK to PLL source. */
    RCC->CFGR |= (3<<RCC_CFGR_SW_Pos);
    while ((RCC->CFGR & RCC_CFGR_SWS_Msk) != (3<<RCC_CFGR_SWS_Pos)) {
        /* wait for RCC to switch over. */
    }

    /* Enable peripheral clocks */
    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN | RCC_AHB2ENR_GPIOBEN | RCC_AHB2ENR_GPIOCEN | RCC_AHB2ENR_DAC2EN;
    /* SYSCFGEN for VREFBUF */
    RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM4EN;
    SystemCoreClockUpdate();

    /* GPIOA:
     *  0 -  (testpoint)
     *  1 -  LINK_PD_RAW
     *  2 -  LINK_PD_PGA
     *  3 -  LMH_OUT
     *  4 -  ~LMH_IDC
     *  5 -  (testpoint)
     *  6 -  VBIAS_DAC (DAC2 Channel 1)
     *  7 -  LMH_COMP
     *  8 -  (testpoint)
     *  9 -  LINK_PD_COMP
     * 10 -  LINK_PD_COMP
     * 11 -  (testpoint)
     * 12 -  (testpoint)
     * 13 -  SWDIO
     * 14 -  SWCLK
     * 15 -  (testpoint)
     */

    GPIOA->MODER &= (~(CLEAR(15) | CLEAR(1))); /* Clear JTAG TDI pin mode */
    GPIOA->MODER |= IN(0) | IN(1) | IN(2) | IN(3) | OUT(4) | IN(5) | IN(6) | IN(7) | IN(8) |
        IN(9) | IN(10) | IN(11) | IN(12) | AF(13) | AF(14) | IN(15);
    GPIOA->AFR[1] = AFRH(13, 0) | /* SWDIO */
                    AFRH(14, 0);  /* SWCLK */
    GPIOA->OSPEEDR = 0xffffffff;
    GPIOA->BRR = (1<<4); /* enable LMH34400 ambient light (I_DC) cancellation */

    /* GPIOB: 
     *  0 -  (testpoint)
     *  1 -  LINK_PD_PGA
     *  2 -  (testpoint)
     *  3 -  SWO
     *  4 -  (testpoint)
     *  5 -  (testpoint)
     *  6 -  (testpoint)
     *  7 -  SDA
     *  8 -  SCL
     *  9 -  LINK_LED_OUT
     * 10 -  (testpoint)
     * 11 -  LMH_COMP
     * 12 -  (testpoint)
     * 13 -  (testpoint)
     * 14 -  (testpoint)
     * 15 -  (testpoint)
     */

    GPIOB->MODER = IN(0) | IN(1) | IN(2) | IN(3) | IN(4) | IN(5) | IN(6) | IN(7) | IN(8) | AF(9) | IN(10) |
        IN(11) | IN(12) | IN(13) | IN(14) | IN(15);
    GPIOB->AFR[1] = AFRH( 9, 2);  /* TIM4 Channel 4 */
    GPIOB->OSPEEDR = 0xffffffff;

    /* GPIOC:
     * 13 -  unused
     * 14 -  unused
     * 15 -  unused
     *  
     * (nothing to configure)
     */

    /* GPIOF:
     *  0 -  RCC_OSC_IN
     *  1 -  RCC_OSC_OUT
     *  
     * (nothing to configure)
     */

    /* GPIOG:
     * 10 -  SWD NRST
     *  
     * (nothing to configure)
     */

    VREFBUF->CSR = (2<<VREFBUF_CSR_VRS_Pos) | VREFBUF_CSR_ENVR;
    DAC2->CR = DAC_CR_EN1;
    DAC2->DHR12R1 = 0x800; /* VBIAS_DAC */

    /* Note: TIM4 has no BDTR or MOE bit. */
    TIM4->ARR = 167;
    TIM4->CCMR2 = (6<<TIM_CCMR2_OC4M_Pos);
    TIM4->CCR4 = 84;
    TIM4->CCER = TIM_CCER_CC4E;
    TIM4->CR1 = TIM_CR1_CEN;

    while (23) {
    }
}

void HardFault_Handler() {
    asm volatile ("bkpt");
}

void delay_us(int duration_us) {
    while (duration_us--) {
        for (int i=0; i<60; i++) {
            asm volatile ("nop");
        }
    }
}

void *memcpy(void *restrict dest, const void *restrict src, size_t n)
{
	unsigned char *d = dest;
	const unsigned char *s = src;

	for (; n; n--) {
        *d++ = *s++;
    }
	return dest;
}

void *memmove(void *dest, const void *src, size_t n)
{
    return memcpy(dest, src, n);
}

void *memset(void *dest, int c, size_t n)
{
    unsigned char *d = dest;
    while (n--) {
        *d++ = c;
    }
	return dest;
}

size_t strlen(const char *s)
{
    const char *start = s;
    while (*s) {
        s++;
    }
    return s - start;
}

void __libc_init_array (void) __attribute__((weak));
void __libc_init_array () {
}
