Diff: src/d7a_com.cpp

Revision:

69:18852c154df9

Parent:

66:492b1d7ba370

Child:

70:07b378285c95

--- a/src/d7a_com.cpp	Wed Oct 26 15:06:05 2016 +0000
+++ b/src/d7a_com.cpp	Thu Nov 03 09:45:18 2016 +0000
@@ -8,6 +8,7 @@
 #include "d7a_fs.h"
 #include "d7a_modem.h"
 #include "d7a_sys.h"
+#include "circular.h"
 
 
 #define XON_SIGNAL              (0x0001 << 0)
@@ -33,17 +34,9 @@
     
     DigitalOut*             rts;
     InterruptIn*            cts;
-        
-    // RX buffer address
-    uint8_t*                rx_buffer;
-    // RX buffer size
-    uint16_t                rx_buffer_size;
-    // Write index in RX buffer
-    volatile uint16_t       write_idx;
-    // Read index in RX buffer
-    uint16_t                read_idx;
-    // Number of bytes available in RX buffer
-    volatile uint16_t       data_available;
+    
+    CircularBuffer*         rx_buf;
+    
     // min data in buffer before parsing body
     d7a_com_rx_msg_t        msg;
     // Number of skipped bytes while parsing
@@ -100,31 +93,24 @@
     while (g_com_ctx.serial->readable())
     {
         
-        g_com_ctx.rx_buffer[g_com_ctx.write_idx] = g_com_ctx.serial->getc();
+        g_com_ctx.rx_buf->push(g_com_ctx.serial->getc());
         //PRINT("-");
         
         //if (g_com_ctx.rx_started)
         {
-            g_com_ctx.data_available++;
-            
-            g_com_ctx.write_idx = (g_com_ctx.write_idx >= (g_com_ctx.rx_buffer_size-1))? 0 : g_com_ctx.write_idx+1;
-            //g_com_ctx.write_idx = (g_com_ctx.write_idx + 1) % g_com_ctx.rx_buffer_size;
-            
             // unlock data parsing thread
-            if (g_com_ctx.state == SEARCH_HEADER && g_com_ctx.data_available >= KAL_COM_HEADER_LEN)
+            if (g_com_ctx.state == SEARCH_HEADER && g_com_ctx.rx_buf->available() >= KAL_COM_HEADER_LEN)
             {
                 g_com_ctx.state = PARSE_HEADER;
                 g_com_ctx.data_parsing->release();
             }
-            else if (g_com_ctx.state == SEARCH_BODY && g_com_ctx.data_available >= g_com_ctx.msg.blen)
+            else if (g_com_ctx.state == SEARCH_BODY && g_com_ctx.rx_buf->available() >= g_com_ctx.msg.blen)
             {
                 g_com_ctx.state = PARSE_BODY;
                 g_com_ctx.data_parsing->release();
             }
         }
     }
-    
-    ASSERT(g_com_ctx.data_available <= g_com_ctx.rx_buffer_size, "RX Buffer overflow! (%d/%d)\r\n", g_com_ctx.data_available, g_com_ctx.rx_buffer_size);
 }
 
 /**
@@ -151,15 +137,8 @@
 {
     FPRINT("\r\n");
     
-    g_com_ctx.rx_started = false;
-    g_com_ctx.tx_started = false;
     g_com_ctx.state = SEARCH_HEADER;
-    g_com_ctx.rx_buffer_size = config->rx_buffer_size;
-    g_com_ctx.rx_buffer = (uint8_t*)MALLOC(g_com_ctx.rx_buffer_size);
-    g_com_ctx.data_available = 0;
     g_com_ctx.skipped_bytes = 0;
-    g_com_ctx.write_idx = 0;
-    g_com_ctx.read_idx = 0;
     g_com_ctx.tx_seq = 0;
     g_com_ctx.rx_seq = 0;
     
@@ -170,6 +149,8 @@
     g_com_ctx.cts_int =         new Semaphore(0);
     
     g_com_ctx.cts->rise(&cts_isr);
+    
+    g_com_ctx.rx_buf =          new CircularBuffer(1024);
 
     /* XXX: Unknown bug:
     Baud rate can be found with high error rate (> 4%).
@@ -199,7 +180,7 @@
     delete g_com_ctx.serial;
     delete g_com_ctx.rts;
     delete g_com_ctx.cts;
-    FREE(g_com_ctx.rx_buffer);
+    delete g_com_ctx.rx_buf;
     
     return D7A_ERR_NONE;
 }
@@ -233,91 +214,20 @@
 void d7a_com_restart(void)
 {
     FPRINT("\r\n");
-    d7a_com_stop_rx();
-    d7a_com_stop_tx();
+    
+    g_com_ctx.serial->attach(NULL, Serial::RxIrq);
 
     g_com_ctx.state = SEARCH_HEADER;
-    g_com_ctx.data_available = 0;
     g_com_ctx.skipped_bytes = 0;
-    g_com_ctx.write_idx = 0;
-    g_com_ctx.read_idx = 0;
     g_com_ctx.tx_seq = 0;
     g_com_ctx.rx_seq = 0;
     
-    d7a_com_tx_buf_t* msg;
-    d7a_com_rx_msg_t* pkt;
-    osEvent evt;
-    
-    // Flush TX queue
-    do
-    {
-        // wait for data
-        evt = g_com_ctx.tx_queue.get(1);
-        msg = (evt.status == osEventMessage)? (d7a_com_tx_buf_t*)evt.value.p : NULL;
-        
-        // free message
-        if (msg != NULL)
-        {
-            FREE(msg->buf);
-            FREE(msg);
-        }
-    } while (msg != NULL);
+    g_com_ctx.rx_buf->reset();
     
-    // Flush RX
-    for (int i = 0 ; i < 3 ; i++)
-    {
-        do
-        {
-            // wait for data
-            if (i == 0)
-            {
-                pkt = d7a_modem_wait_pkt(1);
-            }
-            else if (i == 1)
-            {
-                pkt = d7a_fs_wait_pkt(1);
-            }
-            else
-            {
-                pkt = d7a_sys_wait_pkt(1);
-            }
-            
-            // free message
-            if (pkt != NULL)
-            {
-                FREE(pkt);
-            }
-        } while (pkt != NULL);
-    }
-    
-    d7a_com_start_rx();
-    d7a_com_start_tx();
+    g_com_ctx.serial->attach(&rx_isr, Serial::RxIrq);
 }
 
 
-static void d7a_com_copy_to_linear(uint8_t* linear_buffer, uint16_t read_start, uint16_t length)
-{
-    uint16_t first_part_size;
-    uint16_t second_part_size;
-    
-    ASSERT(length, "Can't copy 0 bytes\r\n");
-    ASSERT(length <= g_com_ctx.rx_buffer_size, "Length too long (%d) for buffer size (%d)\r\n", length, g_com_ctx.rx_buffer_size);
-    
-    read_start %= g_com_ctx.rx_buffer_size;
-    first_part_size = (length > (g_com_ctx.rx_buffer_size - read_start))? g_com_ctx.rx_buffer_size - read_start : length;
-    
-    //DPRINT("CPY @%08x %d %d %d\r\n", linear_buffer, length, read_start, first_part_size);
-    
-    memcpy((void*)&linear_buffer[0], (const void*)&g_com_ctx.rx_buffer[read_start], first_part_size);
-    
-    // The circular buffer is wrapping
-    if (length > first_part_size)
-    {
-        second_part_size = length - first_part_size;
-        memcpy((void*)&linear_buffer[first_part_size], (const void*)&g_com_ctx.rx_buffer[0], second_part_size);
-    }
-}
-
 /**
     Wakes-up modem and send data throught Serial.
 
@@ -326,7 +236,7 @@
     @return void
 */
 static void d7a_com_send(d7a_com_tx_buf_t* tx_buf)
-{    
+{
     FPRINT("\r\n");
     
     DPRINT("<-- (0x%02X) %d\r\n", tx_buf->buf[4], (tx_buf->len - KAL_COM_HEADER_LEN));
@@ -441,11 +351,6 @@
     }
 }
 
-void d7a_com_forward_buffer(uint8_t size)
-{
-    g_com_ctx.read_idx = (g_com_ctx.read_idx + size) % g_com_ctx.rx_buffer_size;
-    g_com_ctx.data_available -= size;
-}
 
 /**
     Reads the Rx buffer, parses the packets
@@ -458,59 +363,50 @@
     uint8_t header[KAL_COM_HEADER_LEN];
     uint8_t seqnum;
     
-    ASSERT(g_com_ctx.data_available >= KAL_COM_HEADER_LEN, "Not enough data for header\r\n");
+    ASSERT(g_com_ctx.rx_buf->available() >= KAL_COM_HEADER_LEN, "Not enough data for header\r\n");
     
     g_com_ctx.skipped_bytes = 0;
     
-    // Lookup fist sync byte
-    while (g_com_ctx.data_available >= KAL_COM_HEADER_LEN)
+    header[0] = g_com_ctx.rx_buf->pop();
+    
+    while (g_com_ctx.rx_buf->available() >= KAL_COM_HEADER_LEN - 1)
     {
-        if(KAL_COM_SYNC_BYTE_0 == g_com_ctx.rx_buffer[g_com_ctx.read_idx])
+        header[1] = g_com_ctx.rx_buf->pop();
+        
+        // Check sync bytes
+        if(KAL_COM_SYNC_BYTE_0 == header[0] && KAL_COM_SYNC_BYTE_1 == header[1])
         {
-            // copy following data
-            d7a_com_copy_to_linear(header, g_com_ctx.read_idx, KAL_COM_HEADER_LEN);
-            if (KAL_COM_SYNC_BYTE_1 == header[1])
+            // Copy header
+            g_com_ctx.rx_buf->get(&header[2], KAL_COM_HEADER_LEN - 2);
+            
+            // Fill temp header
+            g_com_ctx.msg.blen = header[2];
+            seqnum = header[3];
+            g_com_ctx.msg.id = header[4];
+            
+            // Update seqnum
+            WARNING(g_com_ctx.rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", g_com_ctx.rx_seq, seqnum);
+            g_com_ctx.rx_seq = seqnum + 1;
+            
+            // search for body
+            g_com_ctx.state = SEARCH_BODY;
+            
+            // Start parsing if data is already available
+            if (g_com_ctx.rx_buf->available() >= g_com_ctx.msg.blen)
             {
-                // Fill temp header
-                g_com_ctx.msg.blen = header[2];
-                seqnum = header[3];
-                g_com_ctx.msg.id = header[4];
-                
-                // Update seqnum
-                WARNING(g_com_ctx.rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", g_com_ctx.rx_seq, seqnum);
-                g_com_ctx.rx_seq = seqnum + 1;
-                
-                // search for body
-                d7a_com_forward_buffer(KAL_COM_HEADER_LEN);
-                g_com_ctx.state = SEARCH_BODY;
-                
-                // Start parsing if data is already available
-                if (g_com_ctx.data_available >= g_com_ctx.msg.blen)
-                {
-                    g_com_ctx.state = PARSE_BODY;
-                    g_com_ctx.data_parsing->release();
-                }
-                
-                //DPRINT("COM header found (id: %02X seq: %d body: %d/%d bytes)\r\n", g_com_ctx.msg.id, seqnum, g_com_ctx.data_available, g_com_ctx.msg.blen);
-                break;
+                g_com_ctx.state = PARSE_BODY;
+                g_com_ctx.data_parsing->release();
             }
+            
+            //DPRINT("COM header found (id: %02X seq: %d body: %d/%d bytes)\r\n", g_com_ctx.msg.id, seqnum, g_com_ctx.rx_buf->available(), g_com_ctx.msg.blen);
+            break;
         }
-        
-        // byte skipped
-        WARNING(g_com_ctx.skipped_bytes != 1, "COM not sync. Searching for header.\r\n");
-        d7a_com_forward_buffer(1);
-    }
-    
-    if (g_com_ctx.skipped_bytes)
-    {
-        WARNING(false, "COM Skipped %d bytes.\r\n", g_com_ctx.skipped_bytes);
-#if 0
-        uint8_t* temp = (uint8_t*)MALLOC(g_com_ctx.skipped_bytes);
-        d7a_com_copy_to_linear(temp, g_com_ctx.read_idx - g_com_ctx.skipped_bytes, g_com_ctx.skipped_bytes);
-        PRINT_DATA("", "%02X ", temp, g_com_ctx.skipped_bytes, "\r\n");
-        FREE(temp);
-#endif
-        g_com_ctx.skipped_bytes = 0;
+        else
+        {
+            // Shift by 1 byte
+            WARNING(false, "COM Skipped byte 0x%02X.\r\n", header[0]);
+            header[0] = header[1];
+        }
     }
 }
 
@@ -522,7 +418,7 @@
 */
 static void parse_packet_body(void)
 {
-    ASSERT(g_com_ctx.data_available >= g_com_ctx.msg.blen, "Not enough data for body\r\n");
+    ASSERT(g_com_ctx.rx_buf->available() >= g_com_ctx.msg.blen, "Not enough data for body\r\n");
     
     if (KAL_COM_FLOWID(g_com_ctx.msg.id) != KAL_COM_FLOWID_TRC)
     {
@@ -533,7 +429,7 @@
         // copy data to buffer
         pkt->blen = g_com_ctx.msg.blen;
         pkt->id = g_com_ctx.msg.id;
-        d7a_com_copy_to_linear(pkt->buffer, g_com_ctx.read_idx, g_com_ctx.msg.blen);
+        g_com_ctx.rx_buf->get(pkt->buffer, g_com_ctx.msg.blen);
 
         // add packet to queue
         d7a_com_new_pkt(pkt);
@@ -542,14 +438,14 @@
     {
         // Ignore packet
         //DPRINT("Ignore pkt id %02X\r\n", g_com_ctx.msg.id);
+        g_com_ctx.rx_buf->get(NULL, g_com_ctx.msg.blen);
     }
     
-    // update buffer indexes
-    d7a_com_forward_buffer(g_com_ctx.msg.blen);
+    // Seach for next header
     g_com_ctx.state = SEARCH_HEADER;
     
     // Start parsing if data is already available
-    if (g_com_ctx.data_available >= KAL_COM_HEADER_LEN)
+    if (g_com_ctx.rx_buf->available() >= KAL_COM_HEADER_LEN)
     {
         g_com_ctx.state = PARSE_HEADER;
         g_com_ctx.data_parsing->release();
@@ -563,11 +459,6 @@
     FPRINT("(id:0x%08x)\r\n", osThreadGetId());
     while (true)
     {
-        if (!g_com_ctx.rx_started)
-        {
-            g_com_ctx.rx_thread->signal_wait(START_SIGNAL);
-        }
-        
         // wait for data available
         g_com_ctx.data_parsing->wait();
         
@@ -592,11 +483,6 @@
     
     while (true)
     {
-        if (!g_com_ctx.tx_started)
-        {
-            g_com_ctx.tx_thread->signal_wait(START_SIGNAL);
-        }
-        
         // wait for data to send
         evt = g_com_ctx.tx_queue.get();
         msg = (evt.status == osEventMessage)? (d7a_com_tx_buf_t*)evt.value.p : NULL;