1 files changed, 62 insertions, 73 deletions
diff --git a/gm_platform/fw/serial.c b/gm_platform/fw/serial.c
index 178d6f9..5a6009c 100644
--- a/gm_platform/fw/serial.c
+++ b/gm_platform/fw/serial.c
@@ -41,15 +41,18 @@ static volatile uint8_t rx_buf[32];
 
 static void usart_schedule_dma(void);
 static int usart_putc_nonblocking(uint8_t c);
-static int usart_retransmit_packet(uint8_t idx);
 
 
 void usart_dma_init() {
     usart_tx_buf.xfr_start = -1;
     usart_tx_buf.xfr_end = 0;
     usart_tx_buf.wr_pos = 0;
-    for (size_t i=0; i<ARRAY_LEN(usart_tx_buf.packet_start); i++)
-        usart_tx_buf.packet_start[i] = -1;
+    usart_tx_buf.wr_idx = 0;
+    usart_tx_buf.cur_packet = -1;
+    usart_tx_buf.retransmit_rq = 0;
+    usart_tx_buf.wraparound = 0;
+    for (size_t i=0; i<ARRAY_LEN(usart_tx_buf.packet_end); i++)
+        usart_tx_buf.packet_end[i] = -1;
 
     cobs_decode_incremental_initialize(&cobs_state);
 
@@ -134,8 +137,8 @@ void USART1_IRQHandler() {
 
         switch (pkt->type) {
             case CTRL_PKT_RESET:
-                for (size_t i=0; i<ARRAY_LEN(usart_tx_buf.packet_start); i++)
-                    usart_tx_buf.packet_start[i] = -1;
+                for (size_t i=0; i<ARRAY_LEN(usart_tx_buf.packet_end); i++)
+                    usart_tx_buf.packet_end[i] = -1;
                 break;
 
             case CTRL_PKT_ACK:
@@ -144,8 +147,9 @@ void USART1_IRQHandler() {
                 break;
 
             case CTRL_PKT_RETRANSMIT:
-                if (usart_retransmit_packet(pkt->orig_id))
-                    rx_protocol_errors++;
+                usart_tx_buf.retransmit_rq = 1;
+                if (!(DMA1_Channel2->CCR & DMA_CCR_EN))
+                    usart_schedule_dma();
                 break;
 
             default:
@@ -161,26 +165,56 @@ void usart_schedule_dma() {
      * disables. */
     volatile struct dma_tx_buf *buf = &usart_tx_buf;
 
-    ssize_t xfr_len, xfr_start = buf->xfr_end;
-    if (buf->wr_pos > xfr_start) /* no wraparound */
-        xfr_len = buf->wr_pos - xfr_start;
+    ssize_t next_start, next_idx;
+    if (buf->wraparound) {
+        buf->wraparound = 0;
+        next_idx = buf->cur_packet;
+        next_start = 0;
+
+    } else if (buf->retransmit_rq) {
+        buf->retransmit_rq = 0;
+        next_idx = buf->cur_packet;
+        next_start = buf->xfr_start;
+
+    } else {
+        next_idx = (buf->cur_packet + 1) % ARRAY_LEN(usart_tx_buf.packet_end);
+        next_start = buf->xfr_end;
+    }
+
+    ssize_t next_end = buf->packet_end[next_idx];
+
+    /* Nothing to trasnmit */
+    if (next_end == -1)
+        return;
+
+    ssize_t xfr_len;
+    if (next_end > next_start) /* no wraparound */
+        xfr_len = next_end - next_start;
     else /* wraparound */
-        xfr_len = sizeof(buf->data) - xfr_start; /* schedule transfer until end of buffer */
+        xfr_len = sizeof(buf->data) - next_start; /* schedule transfer until end of buffer */
 
-    buf->xfr_start = xfr_start;
-    buf->xfr_end = (xfr_start + xfr_len) % sizeof(buf->data); /* handle wraparound */
+    buf->xfr_start = next_start;
+    buf->xfr_end = (next_start + xfr_len) % sizeof(buf->data); /* handle wraparound */
+    buf->cur_packet = next_idx;
 
     /* initiate transmission of new buffer */
-    DMA1_Channel2->CMAR = (uint32_t)(buf->data + xfr_start);
+    DMA1_Channel2->CMAR = (uint32_t)(buf->data + next_start);
     DMA1_Channel2->CNDTR = xfr_len;
     DMA1_Channel2->CCR |= DMA_CCR_EN;
 }
 
 int usart_ack_packet(uint8_t idx) {
-    if (idx > ARRAY_LEN(usart_tx_buf.packet_start))
+    if (idx > ARRAY_LEN(usart_tx_buf.packet_end))
         return -EINVAL;
 
-    usart_tx_buf.packet_start[idx] = -1;
+    if (idx != usart_tx_buf.cur_packet)
+        return -EINVAL;
+
+    usart_tx_buf.packet_end[idx] = -1;
+
+    /* If the DMA stream is idle right now, schedule the next transfer */
+    if (!(DMA1_Channel2->CCR & DMA_CCR_EN))
+        usart_schedule_dma();
     return 0;
 }
 
@@ -188,16 +222,11 @@ int usart_dma_fifo_push(volatile struct dma_tx_buf *buf, uint8_t c) {
     /* This function must be guarded by IRQ disable since the IRQ may schedule a new transfer and charge pos/start. */
     NVIC_DisableIRQ(DMA1_Channel2_3_IRQn);
 
-    /* If the write pointer hit any unacknowledged packet start position we can't advance it.
-     * Packet start positions are unordered and we have to scan here. */
-    for (size_t i=0; i<ARRAY_LEN(buf->packet_start); i++) {
-        if (buf->wr_pos == buf->packet_start[i]) {
-            NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);
-            return -EBUSY;
-        }
+    if (buf->wr_pos == buf->xfr_start) {
+        NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);
+        return -EBUSY;
     }
 
-    /* write byte, then increment to avoid racing the DMA ISR reading wr_pos */
     buf->data[buf->wr_pos] = c;
     buf->wr_pos = (buf->wr_pos + 1) % sizeof(buf->data);
 
@@ -223,55 +252,20 @@ void DMA1_Channel2_3_IRQHandler(void) {
     DMA1->IFCR |= DMA_IFCR_CTCIF2;
 
     DMA1_Channel2->CCR &= ~DMA_CCR_EN;
-    if (usart_tx_buf.wr_pos != usart_tx_buf.xfr_end) /* buffer not empty */
+    if (usart_tx_buf.retransmit_rq || usart_tx_buf.wraparound)
         usart_schedule_dma();
 }
 
-int usart_retransmit_packet(uint8_t idx) {
-    /* Disable ADC DMA IRQ to prevent write races */
-    NVIC_DisableIRQ(DMA1_Channel1_IRQn);
-
-    ssize_t i = usart_tx_buf.packet_start[idx];
-    ssize_t start = i;
-
-    /* Copy packet */
-    uint8_t c;
-    while ((c = usart_tx_buf.data[i++])) {
-        if (usart_putc_nonblocking(c)) {
-            tx_overruns++;
-            return -EBUSY;
-        }
-    }
+/* len is the packet length including headers */
+int usart_send_packet_nonblocking(struct ll_pkt *pkt, size_t pkt_len) {
 
-    /* Terminating null byte */
-    if (usart_putc_nonblocking(0)) {
+    if (usart_tx_buf.packet_end[usart_tx_buf.wr_idx] != -1) {
+        /* Find a free slot for this packet */
         tx_overruns++;
         return -EBUSY;
     }
 
-    /* Update start index */
-    usart_tx_buf.packet_start[idx] = start;
-
-    NVIC_EnableIRQ(DMA1_Channel1_IRQn);
-    return 0;
-}
-
-/* len is the packet length including headers */
-int usart_send_packet_nonblocking(struct ll_pkt *pkt, size_t pkt_len) {
-
-    ssize_t start = usart_tx_buf.wr_pos;
-    /* Find a free slot for this packet */
-    size_t packet_idx = 0;
-    do {
-        if (usart_tx_buf.packet_start[packet_idx] == -1)
-            goto success;
-    } while (++packet_idx <ARRAY_LEN(usart_tx_buf.packet_start));
-
-    tx_overruns++;
-    return -EBUSY;
-
-success:
-    pkt->pid = packet_idx;
+    pkt->pid = usart_tx_buf.wr_idx;
     pkt->_pad = 0;
 
     /* make the value this wonky-ass CRC implementation produces match zlib etc. */
@@ -280,19 +274,14 @@ success:
         CRC->DR = ((uint8_t *)pkt)[i];
 
     pkt->crc32 = ~CRC->DR;
+
     int rc = cobs_encode_usart((int (*)(char))usart_putc_nonblocking, (char *)pkt, pkt_len);
     if (rc)
         return rc;
 
-    /* Checkpoint packet start index to prevent overwriting before ack */
-    usart_tx_buf.packet_start[packet_idx] = start;
-    /* FIXME debug code
-    static uint8_t x = 0;
-    for (size_t i=0; i<351; i++)
-        usart_putc_nonblocking(x++);
-    */
+    usart_tx_buf.packet_end[usart_tx_buf.wr_idx] = usart_tx_buf.wr_pos;
+    usart_tx_buf.wr_idx = (usart_tx_buf.wr_idx + 1) % ARRAY_LEN(usart_tx_buf.packet_end);
 
-    /* If the DMA stream is idle right now, schedule a transfer */
     if (!(DMA1_Channel2->CCR & DMA_CCR_EN))
         usart_schedule_dma();
     return 0;