From 358e83c419e70548e29de39fee228dfd33efaaf2 Mon Sep 17 00:00:00 2001
From: jaseg <git@jaseg.de>
Date: Thu, 1 Jun 2023 18:15:29 +0200
Subject: [PATCH] USB WIP

---
 include/proto.h |  4 ++--
 src/main.c      | 24 ------------------------
 src/proto.c     | 42 +++++++++++++++++++++++++++++++++++++++---
 src/usb_if.c    |  4 +++-
 4 files changed, 44 insertions(+), 30 deletions(-)

diff --git a/include/proto.h b/include/proto.h
index 6149861..2a3d451 100644
--- a/include/proto.h
+++ b/include/proto.h
@@ -5,7 +5,7 @@
 #include <serial.h>
 
 
-#define PROTO_RESPONSE_TIMEOUT_MS 5
+#define PROTO_RESPONSE_TIMEOUT_MS 7
 #define PROTO_UPDATE_INTERVAL_MS 50
 
 
@@ -40,7 +40,7 @@ struct proto_state {
     bool rx_lid_open;
     bool rx_estop;
 
-    uint32_t adc_samples[2][2][512];
+    int32_t adc_samples[2][2][512];
     size_t adc_sp_wr_idx[2];
     size_t adc_sp_rd_idx[2];
     uint64_t adc_sample_timestamp[2];
diff --git a/src/main.c b/src/main.c
index aa4ee44..ce06559 100644
--- a/src/main.c
+++ b/src/main.c
@@ -8,8 +8,6 @@
 #include <lcd.h>
 #include <led.h>
 #include <led7seg.h>
-#include <usb_if.h>
-#include <usb_proto.h>
 
 static void update_leds(void);
 static void loop_display(void);
@@ -360,28 +358,6 @@ uint64_t get_sync_time() {
     return sync_time_us + SysTick->VAL * SYSTICK_INTERVAL_US / SysTick->LOAD;
 }
 
-void handle_usb_packet(void *packet, size_t len) {
-    switch (*(uint8_t *)packet) {
-        case USBP_GET_STATUS: {
-                struct usbp_status_pkt response = {
-                    .packet_type = USBP_GET_STATUS,
-                    .has_adc = st_adc.has_adc,
-                    .has_lcd = st_lcd.has_lcd,
-                };
-                usb_write_response(&response, sizeof(response));
-                break;
-            }
-
-        case USBP_GET_MEASUREMENTS: {
-                struct usbp_measurement_pkt response = {
-                    .packet_type = USBP_GET_MEASUREMENTS,
-                };
-                usb_write_response(&response, sizeof(response));
-                break;
-            }
-    }
-}
-
 void DMA1_Channel2_3_IRQHandler() {
     int flags = DMA1->ISR;
     DMA1->IFCR = DMA_IFCR_CGIF2 | DMA_IFCR_CGIF3;
diff --git a/src/proto.c b/src/proto.c
index c7f638e..460594f 100644
--- a/src/proto.c
+++ b/src/proto.c
@@ -49,7 +49,43 @@ void uart_handle_user_packet(struct ll_pkt *pkt, size_t len) {
 
     if (board_config == BCFG_MOTOR) { /* We are the protocol host */
         if (pkt->type == PK_FETCH_RESPONSE) {
-            /* ... */
+
+            size_t index = (pkt->src - BADDR_MEAS_BASE) & 1;
+            struct pkt_adc_data *pkt = (struct pkt_adc_data *)pkt;
+            size_t offset = st_proto.adc_sp_wr_idx[index];
+            size_t num_new = pkt->sample_count - st_proto.adc_sample_count[index];
+
+            if (num_new > COUNT_OF(pkt->samples)) {
+                st_proto.last_error = sys_time_us;
+                st_proto.error = ERR_BUFFER_OVERFLOW;
+                num_new = COUNT_OF(pkt->samples);
+            }
+
+            for (ssize_t i=num_new-1; i>=0; i--) {
+                for (size_t channel=0; channel<2; channel++) {
+                    st_proto.adc_samples[index][channel][offset] = 
+                        pkt->samples[i][channel][2]    | 
+                        pkt->samples[i][channel][1]<<8 |
+                        (((int32_t)(int8_t)pkt->samples[i][channel][0])<<16);
+                }
+
+                offset ++;
+                if (offset >= COUNT_OF(st_proto.adc_samples[0][0])) {
+                    offset = 0;
+                }
+            }
+
+            size_t old_wr_pos = st_proto.adc_sp_wr_idx[index];
+            size_t rd_pos = st_proto.adc_sp_rd_idx[index];
+            if ((old_wr_pos < rd_pos && rd_pos < offset) || !(offset < rd_pos && rd_pos < old_wr_pos)) {
+                st_proto.last_error = sys_time_us;
+                st_proto.error = ERR_BUFFER_OVERFLOW;
+            }
+
+            st_proto.adc_sp_wr_idx[index] = offset;
+            st_proto.adc_sample_timestamp[index] = pkt->timestamp;
+            st_proto.adc_sampling_interval[index] = pkt->sampling_interval_us;
+            st_proto.adc_sample_count[index] = pkt->sample_count;
 
             proto_handle_host(false);
 
@@ -82,12 +118,12 @@ void proto_handle_host(bool timeout) {
             break;
 
         case ST_TIME_SYNC:
-            rc = tx_fetch_adc(0);
+            rc = tx_fetch_adc(2);
             st_proto.fsm = ST_FETCH_0;
             break;
 
         case ST_FETCH_0:
-            rc = tx_fetch_adc(1);
+            rc = tx_fetch_adc(3);
             st_proto.fsm = ST_FETCH_1;
             break;
 
diff --git a/src/usb_if.c b/src/usb_if.c
index eb952fd..1b49c8e 100644
--- a/src/usb_if.c
+++ b/src/usb_if.c
@@ -225,7 +225,9 @@ ErrorCode usb_write_response(void *data, size_t len) {
 }
 
 static void usb_cobs_tx(usbd_device *dev, uint8_t event, uint8_t ep) {
-    /* do nothing, we enqueue packets manually in usb_write_response. */
+    uint8_t t = dev->driver->frame_no();
+    memset(fifo, t, CDC_DATA_SZ);
+    usbd_ep_write(dev, ep, fifo, CDC_DATA_SZ);
 }
 
 static usbd_respond usb_setconf (usbd_device *dev, uint8_t cfg) {