#include "drill_clock.h"
/* Finite startup poll count, not a calibrated time interval. Failure is latched.
 * Cold reset and hard-off GPIO are prerequisites. No hot clock reconfiguration. */
static bool clock_started,clock_failed;
#define CLOCK_POLL_LIMIT 65536u
static bool clock_wait(volatile uint32_t *reg,uint32_t mask,uint32_t expect) {
 for(uint32_t i=0;i<CLOCK_POLL_LIMIT;++i){__DSB();if((*reg&mask)==expect)return true;}
 return false;
}
static bool clock_pin(GPIO_TypeDef *p,unsigned n,bool high) {
 uint32_t bit=1u<<n;
 return ((p->MODER>>(2u*n))&3u)==1u && !(p->OTYPER&bit) &&
 (bool)(p->ODR&bit)==high && (bool)(p->IDR&bit)==high;
}
static bool clock_pins_off(void) {
 return (RCC->AHB2ENR&(RCC_AHB2ENR_GPIOAEN|RCC_AHB2ENR_GPIOBEN|RCC_AHB2ENR_GPIOFEN))==(RCC_AHB2ENR_GPIOAEN|RCC_AHB2ENR_GPIOBEN|RCC_AHB2ENR_GPIOFEN) &&
 clock_pin(GPIOA,5,false)&&clock_pin(GPIOA,7,false)&&clock_pin(GPIOA,8,false)&&clock_pin(GPIOA,9,false)&&clock_pin(GPIOA,10,false)&&clock_pin(GPIOB,0,false)&&clock_pin(GPIOB,1,false)&&clock_pin(GPIOF,1,false)&&clock_pin(GPIOA,12,true);
}
static bool clock_fail(void){clock_failed=true;return false;}
bool drill_clock_init(void) {
 if(clock_failed)return false;
 if(clock_started)return drill_clock_available()&&clock_pins_off();
 if(!clock_pins_off() || (RCC->CR&(RCC_CR_HSEON|RCC_CR_HSEBYP|RCC_CR_PLLON|RCC_CR_PLLRDY)) ||
 (RCC->CR&(RCC_CR_HSION|RCC_CR_HSIRDY))!=(RCC_CR_HSION|RCC_CR_HSIRDY) ||
 (RCC->CFGR&(RCC_CFGR_SW|RCC_CFGR_SWS|RCC_CFGR_HPRE|RCC_CFGR_PPRE1|RCC_CFGR_PPRE2))!=(RCC_CFGR_SW_HSI|RCC_CFGR_SWS_HSI) ||
 (RCC->APB1ENR1&(RCC_APB1ENR1_TIM2EN|RCC_APB1ENR1_SPI2EN)) || (RCC->APB2ENR&RCC_APB2ENR_TIM1EN) ||
 (RCC->AHB2ENR&RCC_AHB2ENR_ADC12EN) || (RCC->AHB1ENR&(RCC_AHB1ENR_DMA1EN|RCC_AHB1ENR_DMA2EN|RCC_AHB1ENR_DMAMUX1EN)) ||
 (RCC->APB1RSTR1&RCC_APB1RSTR1_PWRRST) || (SysTick->CTRL&SysTick_CTRL_ENABLE_Msk))return clock_fail();
 RCC->APB1ENR1|=RCC_APB1ENR1_PWREN;(void)RCC->APB1ENR1;__DSB();
 PWR->CR1=(PWR->CR1&~(PWR_CR1_VOS|PWR_CR1_LPR))|PWR_CR1_VOS_0;
 PWR->CR5|=PWR_CR5_R1MODE;
 if(!clock_wait(&PWR->SR2,PWR_SR2_VOSF,0))return clock_fail();
 if((PWR->CR1&(PWR_CR1_VOS|PWR_CR1_LPR))!=PWR_CR1_VOS_0 || !(PWR->CR5&PWR_CR5_R1MODE))return clock_fail();
 /* Raise latency before either frequency increase; retain existing cache state. */
 FLASH->ACR=(FLASH->ACR&~FLASH_ACR_LATENCY)|FLASH_ACR_LATENCY_4WS;
 if(!clock_wait(&FLASH->ACR,FLASH_ACR_LATENCY,FLASH_ACR_LATENCY_4WS))return clock_fail();
 RCC->PLLCFGR=DRILL_PLL_CONFIG;__DSB();
 if(RCC->PLLCFGR!=DRILL_PLL_CONFIG)return clock_fail();
 RCC->CR|=RCC_CR_PLLON;
 if(!clock_wait(&RCC->CR,RCC_CR_PLLRDY,RCC_CR_PLLRDY))return clock_fail();
 /* ST LL Utils uses intermediate AHB /2 above 80MHz to avoid undershoot. */
 RCC->CFGR=(RCC->CFGR&~RCC_CFGR_HPRE)|RCC_CFGR_HPRE_DIV2;__DSB();
 if((RCC->CFGR&RCC_CFGR_HPRE)!=RCC_CFGR_HPRE_DIV2)return clock_fail();
 RCC->CFGR=(RCC->CFGR&~RCC_CFGR_SW)|RCC_CFGR_SW_PLL;
 if(!clock_wait(&RCC->CFGR,RCC_CFGR_SWS,RCC_CFGR_SWS_PLL))return clock_fail();
 SystemCoreClock=DRILL_CORE_HZ/2u;
 RCC->CFGR&=~RCC_CFGR_HPRE;__DSB();__ISB();
 SystemCoreClock=DRILL_CORE_HZ;clock_started=true;
 if(!drill_clock_available()||!clock_pins_off())return clock_fail();
 return true;
}
