Basic JTAG working

1 files changed, 46 insertions, 4 deletions

diff --git a/controller/fw/src/main.c b/controller/fw/src/main.c
index d2795df..aec1fdc 100644
--- a/controller/fw/src/main.c
+++ b/controller/fw/src/main.c
@@ -14,6 +14,7 @@
 #include "freq_meas.h"
 #include "dsss_demod.h"
 #include "con_usart.h"
+#include "mspdebug_wrapper.h"
 
 static struct spi_flash_if spif;
 
@@ -26,6 +27,8 @@ unsigned int apb2_timer_speed = 0;
 
 struct leds leds;
 
+ssize_t jt_spi_flash_read_block(void *usr, int addr, size_t len, uint8_t *out);
+
 void __libc_init_array(void) { /* we don't need this. */ }
 void __assert_func (unused_a const char *file, unused_a int line, unused_a const char *function, unused_a const char *expr) {
     asm volatile ("bkpt");
@@ -116,8 +119,11 @@ static void clock_setup(void)
 
 static void led_setup(void)
 {
-    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN;
+    /* onboard leds */
     GPIOA->MODER |= (1<<GPIO_MODER_MODER6_Pos) | (1<<GPIO_MODER_MODER7_Pos);
+    GPIOB->MODER |= (1<<GPIO_MODER_MODER11_Pos) | (1<<GPIO_MODER_MODER12_Pos) | (1<<GPIO_MODER_MODER13_Pos)| (1<<GPIO_MODER_MODER14_Pos);
+    GPIOB->BSRR = 0xf << 11;
 }
 
 static void spi_flash_if_set_cs(bool val) {
@@ -174,6 +180,21 @@ void spi_flash_test(void) {
 }
 #endif
 
+static struct jtag_img_descriptor {
+    size_t devmem_img_start;
+    size_t spiflash_img_start;
+    size_t img_len;
+} jtag_img = {
+    .devmem_img_start = 0x00c000,
+    .spiflash_img_start = 0x000000,
+    .img_len = 0x060000,
+};
+
+ssize_t jt_spi_flash_read_block(void *usr, int addr, size_t len, uint8_t *out) {
+    struct jtag_img_descriptor *desc = (struct jtag_img_descriptor *)usr;
+    return spif_read(&spif, desc->spiflash_img_start + addr, len, (char *)out);
+}
+
 static unsigned int measurement_errors = 0;
 static struct dsss_demod_state demod_state;
 static uint32_t freq_sample_ts = 0;
@@ -216,6 +237,15 @@ int main(void)
     dsss_demod_init(&demod_state);
 
     con_printf("Booted.\r\n");
+    int i=0;
+    while (23) {
+        mspd_jtag_init();
+        con_printf("%d flash result: %d\r\n", i,
+            mspd_jtag_flash_and_reset(jtag_img.devmem_img_start, jtag_img.img_len, jt_spi_flash_read_block, &jtag_img));
+        for (int i=0; i<168*1000*5; i++)
+            asm volatile ("nop");
+        i++;
+    }
     while (23) {
         if (adc_fft_buf_ready_idx != -1) {
             for (int i=0; i<168*1000*2; i++)
@@ -223,6 +253,18 @@ int main(void)
             GPIOA->BSRR = 1<<11;
             memcpy(adc_fft_buf[!adc_fft_buf_ready_idx], adc_fft_buf[adc_fft_buf_ready_idx] + FMEAS_FFT_LEN/2, sizeof(adc_fft_buf[0][0]) * FMEAS_FFT_LEN/2);
             GPIOA->BSRR = 1<<11<<16;
+            GPIOB->ODR ^= 1<<14;
+
+            bool clip_low=false, clip_high=false;
+            const int clip_thresh = 100;
+            for (size_t i=FMEAS_FFT_LEN/2; i<FMEAS_FFT_LEN; i++) {
+                int val = adc_fft_buf[adc_fft_buf_ready_idx][i];
+                if (val < clip_thresh)
+                    clip_low = true;
+                if (val > FMEAS_ADC_MAX-clip_thresh)
+                    clip_high = true;
+            }
+            GPIOB->ODR = (GPIOB->ODR & ~(3<<11)) | (!clip_low<<11) | (!clip_high<<12);
 
             for (int i=0; i<168*1000*2; i++)
                 asm volatile ("nop");
@@ -232,7 +274,7 @@ int main(void)
             if (adc_buf_measure_freq(adc_fft_buf[adc_fft_buf_ready_idx], &out)) {
                 con_printf("%012d: measurement error\r\n", freq_sample_ts);
                 measurement_errors++;
-                GPIOA->BSRR = 1<<7;
+                GPIOB->BSRR = 1<<13;
                 debug_last_freq = NAN;
 
             } else {
@@ -240,9 +282,9 @@ int main(void)
                 con_printf("%012d: %2d.%03d Hz\r\n", freq_sample_ts, (int)out, (int)(out * 1000) % 1000);
                 /* frequency ok led */
                 if (48 < out && out < 52)
-                    GPIOA->BSRR = 1<<7<<16;
+                    GPIOB->BSRR = 1<<13<<16;
                 else
-                    GPIOA->BSRR = 1<<7;
+                    GPIOB->BSRR = 1<<13;
 
                 GPIOA->BSRR = 1<<12;
                 dsss_demod_step(&demod_state, out, freq_sample_ts);