Diff: src/d7a_com.cpp

Revision:

33:f9a542d3efaa

Parent:

31:ab9bfdbc6b44

Child:

34:1311cc53201a

--- a/src/d7a_com.cpp	Thu Jun 16 12:10:10 2016 +0000
+++ b/src/d7a_com.cpp	Fri Jun 24 10:11:19 2016 +0000
@@ -29,6 +29,11 @@
     uint16_t                read_idx;
     // Number of bytes available in RX buffer
     volatile uint16_t       data_available;
+    // Last Number of available data
+    volatile uint16_t       last_available;
+    // Number of skipped bytes while parsing
+    uint16_t                skipped_bytes;
+    
     // Port TX sequence number
     uint8_t                 tx_seq;
     // Port RX sequence number
@@ -85,20 +90,16 @@
 {
 // Loop just in case more than one character is in UART's receive FIFO buffer
 // Stop if buffer full
-    while ((g_com_ctx.serial->readable()) && (((g_com_ctx.write_idx + 1) % g_com_ctx.rx_buffer_size) != g_com_ctx.read_idx)) {
+    while (g_com_ctx.serial->readable()) {
         g_com_ctx.rx_buffer[g_com_ctx.write_idx] = g_com_ctx.serial->getc();
         g_com_ctx.data_available++;
-        //DPRINT(".");
+        //dbg_print(".");
 
         g_com_ctx.write_idx = (g_com_ctx.write_idx + 1) % g_com_ctx.rx_buffer_size;
     }
-            
+    
     // unlock data parsing thread
-    // do not parse if minimum packet size is not available
-    if (g_com_ctx.data_available > KAL_COM_HEADER_LEN)
-    {
-        g_com_ctx.data_parsing->release();
-    }
+    g_com_ctx.data_parsing->release();
     
     return;
 }
@@ -131,6 +132,8 @@
     g_com_ctx.rx_buffer = (uint8_t*)MALLOC(g_com_ctx.rx_buffer_size);
     g_com_ctx.cts->rise(&cts_isr);
     g_com_ctx.data_available = 0;
+    g_com_ctx.last_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;
@@ -151,7 +154,8 @@
     It is here manualy corrected after an oscilloscope measure.
     Normal Baudrate should be 115200.
     */
-    g_com_ctx.serial->baud(117000); // XXX
+    //g_com_ctx.serial->baud(117000); // XXX
+    g_com_ctx.serial->baud(115200); // XXX
     //g_com_ctx.serial->baud(58500); // XXX
     g_com_ctx.serial->format(8, SerialBase::None, 1);
     g_com_ctx.serial->attach(&rx_isr, Serial::RxIrq);
@@ -219,7 +223,6 @@
     uint16_t len = msg->alen + msg->plen;
     FPRINT("(len:%d)\r\n", len);
     
-    
     buf = (uint8_t*)MALLOC(KAL_COM_HEADER_LEN + len);
     
     // construct serial header
@@ -236,12 +239,6 @@
     memcpy(buf + KAL_COM_HEADER_LEN + msg->plen, msg->abuf, msg->alen);
     
     d7a_com_post_tx(buf, len);
-        
-    // Stats
-    g_com_ctx.tx_pkt_count++;
-    g_com_ctx.tx_byte_count += len;
-        
-    DPRINT("<-- %d (0x%02X)\r\n", (len - KAL_COM_HEADER_LEN), msg->id);
 }
 
 
@@ -256,14 +253,23 @@
     d7a_com_send_msg(&msg);
 }
 
-
+void d7a_com_flush_rx(void)
+{
+    g_com_ctx.data_available = 0;
+    g_com_ctx.last_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;
+}
 
 static void d7a_com_new_pkt(d7a_com_rx_msg_t* pkt)
 {
     //FPRINT("\r\n");
     if (KAL_COM_FLOWID(pkt->id) != KAL_COM_FLOWID_TRC)
     {
-        DPRINT("--> %d (0x%02X)\r\n", pkt->blen, pkt->id);
+        DPRINT("--> (0x%02X) %d\r\n", pkt->id, pkt->blen);
     }
     
     // Distribute packet types to processes
@@ -308,84 +314,117 @@
     }
 }
 
+
 /**
     Reads the Rx buffer, parses the packets and adds them to _pkt_queue
 
     @param void
     @return void
 */
-static bool parse_packet(const uint16_t start_idx, const uint16_t data_available)
+static void parse_packet(void)
 {
-    uint16_t data_read = 0;
-    uint8_t data = 0;
-    uint8_t state = 0;
+    uint16_t base = 0;
+    uint16_t current = 0;
+    uint8_t byte;
+    
+    // Packet fields
     uint8_t seqnum = 0;
     uint8_t len = 0;
     uint8_t id = 0;
+    
     uint8_t i = 0;
-    bool pkt_found = false;
+    
+    // Stats
+    uint16_t nb_pkt = 0;
+    
+    //dbg_print("Parsing %d bytes\r\n", g_com_ctx.data_available);
     
-    while (data_read < data_available)
+    while (g_com_ctx.data_available >= (base + KAL_COM_HEADER_LEN + 1))
     {
-        // do not parse if minimum packet size is not available
-        if (0 == state && (data_available - data_read) <= KAL_COM_HEADER_LEN)
+        // Pop data
+        byte = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + (base + current)) % g_com_ctx.rx_buffer_size];
+        //DPRINT("Treating base %d byte %d of %d: 0x%02X\r\n", base, current, g_com_ctx.data_available, byte);
+
+        // search first sync byte
+        if (0 == current)
         {
-            break;
-        }
+            WARNING(g_com_ctx.skipped_bytes != 1, "COM not sync. Searching for header.\r\n");
         
-        // Pop data
-        data = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + data_read) % g_com_ctx.rx_buffer_size];
-        data_read++;
-        //DPRINT("Read %d of %d bytes: 0x%02X\r\n", data_read, data_available, data);
-        
-        // search first sync byte
-        if (0 == state && KAL_COM_SYNC_BYTE_0 == data)
-        {
-            state++;
+            if(KAL_COM_SYNC_BYTE_0 == byte)
+            {
+                current++;
+            }
+            else
+            {
+                // Not a packet. Restart at base + 1
+                g_com_ctx.skipped_bytes++;
+                base++;
+            }
+            continue;
         }
         // look up second sync byte
-        else if (1 == state && KAL_COM_SYNC_BYTE_1 == data)
-        {
-            state++;
-        }
-        // look up packet size
-        else if (2 == state && 0 != data)
+        else if (1 == current)
         {
-            len = data;
-            // if not enough data is available
-            if (data_available < data_read + 1 + 1 + len) // data read + seqnum + id + data length
+            if(KAL_COM_SYNC_BYTE_1 == byte)
             {
-                // go back to begining of packet
-                data_read -= 3;
-                break;
+                current++;
             }
             else
             {
-                state++;
+                // Not a packet. Restart at base + 1
+                g_com_ctx.skipped_bytes++;
+                base++;
+                current = 0;
             }
+            continue;
+        }
+        // look up packet size
+        else if (2 == current)
+        {
+            len = byte;
+            // if not enough data is available
+            if ((g_com_ctx.data_available - base) < (KAL_COM_HEADER_LEN + len))
+            {
+                // exit parser
+                break;
+            }
+            
+            current++;
+            continue;
         }
         // get seqnum
-        else if (3 == state)
+        else if (3 == current)
+        {
+            seqnum = byte;
+            current++;
+            continue;
+        }
+        // get id
+        else if (4 == current)
         {
-            seqnum = data;
+            id = byte;
+            current++;
+            continue;
+        }
+        // we got a full packet
+        else if (5 == current)
+        {
+            if (g_com_ctx.skipped_bytes)
+            {
+                WARNING(false, "COM header found, skipped %d bytes.\r\n", g_com_ctx.skipped_bytes);
+                g_com_ctx.skipped_bytes = 0;
+            }
+            
+            // Update seqnum
             if (g_com_ctx.rx_seq != seqnum)
             {
-                WARNING(false, "COM Missing %d seqnum\r\n", g_com_ctx.rx_seq - seqnum);
+                ASSERT(false, "COM Bad seqnum expected:%d got:%d\r\n", seqnum, g_com_ctx.rx_seq);
                 g_com_ctx.rx_seq = seqnum;
             }
-
             g_com_ctx.rx_seq++;
-            state++;
-        }
-        // get id
-        else if (4 == state)
-        {
-            id = data;
-            state++;
-        }
-        // we got a full packet
-        else if (5 == state)
-        {
+            
+            nb_pkt++;
+            
             if (KAL_COM_FLOWID(id) != KAL_COM_FLOWID_TRC)
             {
                 d7a_com_rx_msg_t* pkt = NULL;
@@ -398,65 +437,38 @@
                 pkt->id = id;
                 for (i=0 ; i<len ; i++)
                 {
-                    pkt->buffer[i] = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + data_read + i - 1) % g_com_ctx.rx_buffer_size];
+                    pkt->buffer[i] = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + (base + current + i)) % g_com_ctx.rx_buffer_size];
                 }
-                data_read += i - 1;
                 
                 // add packet to queue
                 d7a_com_new_pkt(pkt);
-                
-                pkt_found = true;
             }
             else
             {
-                data_read += len - 1;
+                // Ignore packet
             }
-                        
-            // Stats
-            g_com_ctx.rx_pkt_count++;
-            g_com_ctx.rx_byte_count += (len + KAL_COM_HEADER_LEN);
+            
+            // Go to EOP
+            current += len - 1;
             
-            break;
+            // Search next packet
+            base += current + 1;
+            current = 0;
+            continue;
         }
         else
         {
-            // Restart search
-            state = 0;
+            ASSERT(false, "D7A COM parser!\r\n");
         }
     }
-
-    if (data_read)
-    {
-        uint16_t discarded_bytes;
-        if (pkt_found)
-        {
-            // If packet is found, bytes before the packet are discarded
-            discarded_bytes = data_read - len - KAL_COM_HEADER_LEN;
-        }
-        else
-        {
-            // Discard all bytes
-            discarded_bytes = data_read;
-        }
-        
-        if (discarded_bytes)
-        {
-            //EPRINT("Discarding %d bytes\r\n", discarded_bytes);
-#if 0
-            for (i=0 ; i<discarded_bytes ; i++)
-            {
-                DPRINT("%02X ", g_com_ctx.rx_buffer[(g_com_ctx.read_idx + i) % g_com_ctx.rx_buffer_size]);
-            }
-            DPRINT("\r\n");
-#endif
-        }
-        
-        // update buffer indexes
-        g_com_ctx.read_idx = (g_com_ctx.read_idx + data_read) % g_com_ctx.rx_buffer_size;
-        g_com_ctx.data_available -= data_read;
+    
+    // update buffer indexes
+    g_com_ctx.read_idx = (g_com_ctx.read_idx + base) % g_com_ctx.rx_buffer_size;
+    g_com_ctx.data_available -= base;
+    
+    if (base) {
+        DPRINT("Parsed %d bytes of %d (%d packets)\r\n", base, g_com_ctx.data_available + base, nb_pkt);
     }
-    
-    return pkt_found;
 }
 
 
@@ -468,8 +480,13 @@
     {
         // wait for data available
         g_com_ctx.data_parsing->wait();
+        ASSERT(g_com_ctx.data_available <= g_com_ctx.rx_buffer_size, "COM buffer overflow\r\n");
         // search for packets
-        parse_packet(g_com_ctx.read_idx, g_com_ctx.data_available);
+        if ((g_com_ctx.last_available != g_com_ctx.data_available) && (g_com_ctx.data_available >= (KAL_COM_HEADER_LEN + 1)))
+        {
+            g_com_ctx.last_available = g_com_ctx.data_available;
+            parse_packet();
+        }
     }
 }
 
@@ -493,6 +510,7 @@
         // send message
         if (msg != NULL)
         {
+            DPRINT("<-- (0x%02X) %d\r\n", msg->buf[4], (msg->len - KAL_COM_HEADER_LEN));
             d7a_com_send(msg->buf, msg->len);
             FREE(msg->buf);
             FREE(msg);