#include "stm32g4xx.h"
#include "drill_clock.h"
#include "drill_timebase.h"
#include "drill_timing.h"
#include <stddef.h>
#define DT_US 30u
#define DT_CYCLES (DRILL_CORE_HZ/1000000u*DT_US)
_Static_assert(sizeof(dt_record)==64u,"SWD timing record ABI");
volatile dt_record drill_debug_timing;
static bool timing_owned,timing_failed,timing_open,previous_busy,have_previous;
static uint32_t begin_cycles,begin_us,previous_cycles,previous_us;
static uint32_t timing_inc(uint32_t n){return n==UINT32_MAX?n:n+1u;}
static bool timing_counter_ok(void){return (CoreDebug->DEMCR&CoreDebug_DEMCR_TRCENA_Msk)&&(DWT->CTRL&DWT_CTRL_CYCCNTENA_Msk)&&!(DWT->CTRL&DWT_CTRL_NOCYCCNT_Msk);}
static bool timing_fail(dt_fault f){
 if(!(drill_debug_timing.epoch&1u)){++drill_debug_timing.epoch;__DMB();}
 timing_failed=true;drill_debug_timing.fault=(uint32_t)f;drill_debug_timing.flags&=~2u;
 if(drill_debug_timing.epoch&1u){__DMB();++drill_debug_timing.epoch;}
 timing_open=false;return false;
}
bool drill_timing_init(void){
 if(timing_failed)return false;
 if(timing_owned){if(timing_counter_ok())return true;return timing_fail(DT_COUNTER);}
 uint32_t now;
 drill_debug_timing=(dt_record){.magic=0x544d3830u,.version=1,.core_hz=DRILL_CORE_HZ,.limit_cycles=DT_CYCLES,.limit_us=DT_US};
 if(!drill_timebase_get(&now))return timing_fail(DT_CLOCK);
 if(DWT->CTRL&DWT_CTRL_NOCYCCNT_Msk)return timing_fail(DT_COUNTER);
 /* Preserve a debugger's counter value and other trace settings; no IRQ mask,
  * no counter reset, no SWO/ITM output, no DWT comparators configured. */
 CoreDebug->DEMCR|=CoreDebug_DEMCR_TRCENA_Msk;DWT->CTRL|=DWT_CTRL_CYCCNTENA_Msk;__DSB();__ISB();
 if(!timing_counter_ok())return timing_fail(DT_COUNTER);
 uint32_t first=DWT->CYCCNT;
 for(unsigned i=0;i<32u;++i){__DSB();if(DWT->CYCCNT!=first){timing_owned=true;drill_debug_timing.flags=1u;return true;}}
 return timing_fail(DT_COUNTER);
}
bool drill_timing_begin(uint32_t *now){
 if(timing_failed)return false;
 if(!timing_owned||timing_open||!now)return timing_fail(DT_SEQUENCE);
 begin_cycles=DWT->CYCCNT;++drill_debug_timing.epoch;__DMB();timing_open=true;
 if(!timing_counter_ok())return timing_fail(DT_COUNTER);
 if(!drill_timebase_get(&begin_us))return timing_fail(DT_CLOCK);
 if(have_previous&&previous_busy){
  uint32_t gap=begin_us-previous_us,cycles=begin_cycles-previous_cycles;
  drill_debug_timing.last_gap_us=gap;if(gap>drill_debug_timing.max_gap_us)drill_debug_timing.max_gap_us=gap;
  if(gap>DT_US||cycles>DT_CYCLES)return timing_fail(DT_LIMIT);
 }
 *now=begin_us;return true;
}
bool drill_timing_end(bool idle){
 if(timing_failed)return false;
 if(!timing_owned||!timing_open)return timing_fail(DT_SEQUENCE);
 uint32_t ended;
 if(!timing_counter_ok())return timing_fail(DT_COUNTER);
 if(!drill_timebase_get(&ended))return timing_fail(DT_CLOCK);
 uint32_t cycles=DWT->CYCCNT-begin_cycles,us=ended-begin_us;
 drill_debug_timing.last_cycles=cycles;drill_debug_timing.last_us=us;
 if(cycles>drill_debug_timing.max_cycles)drill_debug_timing.max_cycles=cycles;
 if(us>drill_debug_timing.max_us)drill_debug_timing.max_us=us;
 if(cycles==0u)return timing_fail(DT_COUNTER);
 if(cycles>DT_CYCLES||us>DT_US)return timing_fail(DT_LIMIT);
 drill_debug_timing.windows=timing_inc(drill_debug_timing.windows);
 if(!idle)drill_debug_timing.active_windows=timing_inc(drill_debug_timing.active_windows);
 previous_us=begin_us;previous_cycles=begin_cycles;previous_busy=!idle;have_previous=true;
 drill_debug_timing.flags|=2u;timing_open=false;__DMB();++drill_debug_timing.epoch;return true;
}
