Diff: src/d7a_com.cpp

Revision:

34:1311cc53201a

Parent:

33:f9a542d3efaa

Child:

35:1fe2975c5a63

--- a/src/d7a_com.cpp	Fri Jun 24 10:11:19 2016 +0000
+++ b/src/d7a_com.cpp	Mon Aug 22 13:47:39 2016 +0000
@@ -7,7 +7,8 @@
 #include "d7a_modem.h"
 #include "d7a_sys.h"
 
-static volatile bool g_xon;
+
+#define XON_SIGNAL      0x0001
 
 typedef struct {
     uint32_t len;
@@ -16,6 +17,8 @@
 
 
 typedef struct {
+    bool                    started;
+    
     DigitalOut*             rts;
     InterruptIn*            cts;
         
@@ -41,15 +44,10 @@
     // Response to command maximum time
     uint32_t                timeout;
     
-    // Packet reception queue
-    Queue<d7a_com_rx_msg_t, 32> pkt_queue;
-    
     // Waiting for data available in RX buffer
     Semaphore*              data_parsing;
     // CTS management
     Semaphore*              cts_int;
-    // XON/XOFF management
-    Semaphore*              xonoff;
     
     // Data treatment threads
     Thread*                 rx_thread;
@@ -59,10 +57,8 @@
     Thread*                 tx_thread;
     Queue<d7a_com_tx_buf_t, 32> tx_queue;
     
-    uint32_t                rx_pkt_count;
-    uint32_t                rx_byte_count;
-    uint32_t                tx_pkt_count;
-    uint32_t                tx_byte_count;
+    Timer                   tim;
+
 } d7a_com_ctx_t;
 
 
@@ -78,7 +74,6 @@
 void d7a_com_tx_thread( void const *p );
 
 
-
 /**
     Serial Rx Interrupt Service Routine.
     Add recevied bytes to the RX buffer.
@@ -90,18 +85,25 @@
 {
 // 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()) {
+    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++;
-        //dbg_print(".");
-
-        g_com_ctx.write_idx = (g_com_ctx.write_idx + 1) % g_com_ctx.rx_buffer_size;
+        //PRINT("-");
+        
+        if (g_com_ctx.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;
+        }
     }
     
+    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);
+    
     // unlock data parsing thread
     g_com_ctx.data_parsing->release();
-    
-    return;
 }
 
 /**
@@ -113,8 +115,8 @@
 */
 void cts_isr()
 {
-    g_com_ctx.cts_int->release();
-    return;
+    //PRINT("CTS_INT\r\n");
+    //g_com_ctx.cts_int->release();
 }
 
 // D7a_com constructor.
@@ -128,9 +130,9 @@
 {
     FPRINT("\r\n");
     
+    g_com_ctx.started = false;
     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.cts->rise(&cts_isr);
     g_com_ctx.data_available = 0;
     g_com_ctx.last_available = 0;
     g_com_ctx.skipped_bytes = 0;
@@ -142,13 +144,11 @@
     g_com_ctx.serial =          new Serial(config->tx, config->rx);
     g_com_ctx.rts =             new DigitalOut(config->rts);
     g_com_ctx.cts =             new InterruptIn(config->cts);
-    g_com_ctx.data_parsing =    new Semaphore(1);
-    g_com_ctx.cts_int =         new Semaphore(1);
-    g_com_ctx.xonoff =          new Semaphore(1);
-        
-    g_xon = true;
-    g_com_ctx.xonoff->wait();
+    g_com_ctx.data_parsing =    new Semaphore(0);
+    g_com_ctx.cts_int =         new Semaphore(0);
     
+    g_com_ctx.cts->rise(&cts_isr);
+
     /* XXX: Unknown bug:
     Baud rate can be found with high error rate (> 4%).
     It is here manualy corrected after an oscilloscope measure.
@@ -156,7 +156,7 @@
     */
     //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->baud(57600); // XXX
     g_com_ctx.serial->format(8, SerialBase::None, 1);
     g_com_ctx.serial->attach(&rx_isr, Serial::RxIrq);
     
@@ -178,6 +178,92 @@
     FREE(g_com_ctx.rx_buffer);
 }
 
+void d7a_com_start(void)
+{
+    FPRINT("\r\n");
+    g_com_ctx.started = true;
+}
+
+void d7a_com_restart(void)
+{
+    FPRINT("\r\n");
+    g_com_ctx.started = false;
+    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;
+    
+    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);
+    
+    // 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);
+    }
+    
+    g_com_ctx.started = true;
+}
+
+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 <= 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;
+    
+    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.
@@ -188,15 +274,16 @@
 */
 static void d7a_com_send(const uint8_t* buffer, int length)
 {    
-    int i;
     FPRINT("(len:%d)\r\n", length);
+    ASSERT(g_com_ctx.started, "COM not started!\r\n");
+    
     *(g_com_ctx.rts) = 1;
     
     //dbg_print_data("%02X ", (uint8_t*)buffer, length);
     //DPRINT("\r\n");
     Thread::wait(5);
     
-    for (i=0 ; i<length ; i++)
+    for (uint32_t i=0 ; i<length ; i++)
     {
         g_com_ctx.serial->putc(buffer[i]);
     }
@@ -208,6 +295,8 @@
 static void d7a_com_post_tx(uint8_t* buffer, uint32_t length)
 {
     FPRINT("(len:%d)\r\n", length);
+    ASSERT(g_com_ctx.started, "COM not started!\r\n");
+    
     d7a_com_tx_buf_t* tx_buf = (d7a_com_tx_buf_t*)MALLOC(sizeof(d7a_com_tx_buf_t));
     
     tx_buf->len = length;
@@ -218,7 +307,33 @@
 
 // Formats and send packet throught Serial.
 void d7a_com_send_msg(d7a_com_tx_msg_t* msg)
-{
+{    
+    uint8_t* buf;
+    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
+    // concatenate and update tx_seq ID
+    buf[0] = (uint8_t)KAL_COM_SYNC_BYTE_0;
+    buf[1] = (uint8_t)KAL_COM_SYNC_BYTE_1;
+    buf[2] = (uint8_t)len;
+    buf[3] = (uint8_t)g_com_ctx.tx_seq++;
+    buf[4] = (uint8_t)msg->id;
+    len += KAL_COM_HEADER_LEN;
+
+    // copy payload and parameters
+    memcpy(buf + KAL_COM_HEADER_LEN, msg->pbuf, msg->plen);
+    memcpy(buf + KAL_COM_HEADER_LEN + msg->plen, msg->abuf, msg->alen);
+    
+    DPRINT("<-- (0x%02X) %d\r\n", buf[4], (len - KAL_COM_HEADER_LEN));
+    
+    d7a_com_send(buf, len);
+}
+
+void d7a_com_post_msg(d7a_com_tx_msg_t* msg)
+{    
     uint8_t* buf;
     uint16_t len = msg->alen + msg->plen;
     FPRINT("(len:%d)\r\n", len);
@@ -250,18 +365,18 @@
     msg.pbuf = buf;
     msg.plen = len;
     msg.alen = 0;
-    d7a_com_send_msg(&msg);
+    d7a_com_post_msg(&msg);
 }
 
-void d7a_com_flush_rx(void)
+void d7a_com_force_dump(uint8_t* buf, uint8_t len, d7a_com_flow_t flow)
 {
-    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;
+    d7a_com_tx_msg_t msg;
+
+    msg.id = flow;
+    msg.pbuf = buf;
+    msg.plen = len;
+    msg.alen = 0;
+    d7a_com_send_msg(&msg);
 }
 
 static void d7a_com_new_pkt(d7a_com_rx_msg_t* pkt)
@@ -285,22 +400,22 @@
             // This has to be here to avoid going to another process
             if (pkt->id == KAL_COM_FLOW_SYS_XON)
             {
-                DPRINT("XON\r\n");
-                g_xon = true;
-                // Free all threads
-                g_com_ctx.xonoff->release();
+                g_com_ctx.tx_thread->signal_set(XON_SIGNAL);
                 FREE(pkt);
             }
             else if (pkt->id == KAL_COM_FLOW_SYS_XOFF)
             {
-                uint8_t buf[] = "X";
-                d7a_com_dump(buf, 1, KAL_COM_FLOW_SYS_XACK);
-                g_xon = false;
+                d7a_sys_xack();
                 FREE(pkt);
-                DPRINT("XOFF\r\n");
             }
             else
             {
+                if (KAL_COM_FLOW_SYS_PONG == pkt->id)
+                {
+                    g_com_ctx.tim.stop();
+                    uint32_t time_us = g_com_ctx.tim.read_us();
+                    IPRINT("Ping in %d.%03dms\r\n", time_us/1000, time_us%1000);
+                }
                 d7a_sys_new_pkt(pkt);
             }
             break;
@@ -316,7 +431,7 @@
 
 
 /**
-    Reads the Rx buffer, parses the packets and adds them to _pkt_queue
+    Reads the Rx buffer, parses the packets
 
     @param void
     @return void
@@ -332,18 +447,14 @@
     uint8_t len = 0;
     uint8_t id = 0;
     
-    uint8_t i = 0;
-    
     // Stats
     uint16_t nb_pkt = 0;
     
-    //dbg_print("Parsing %d bytes\r\n", g_com_ctx.data_available);
-    
     while (g_com_ctx.data_available >= (base + KAL_COM_HEADER_LEN + 1))
     {
         // 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);
+        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)
@@ -360,7 +471,6 @@
                 g_com_ctx.skipped_bytes++;
                 base++;
             }
-            continue;
         }
         // look up second sync byte
         else if (1 == current)
@@ -376,35 +486,32 @@
                 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))
+            if (g_com_ctx.data_available < (KAL_COM_HEADER_LEN + len + base))
             {
                 // exit parser
+                DPRINT("len exit\r\n");
                 break;
             }
             
             current++;
-            continue;
         }
         // get seqnum
         else if (3 == current)
         {
             seqnum = byte;
             current++;
-            continue;
         }
         // get id
         else if (4 == current)
         {
             id = byte;
             current++;
-            continue;
         }
         // we got a full packet
         else if (5 == current)
@@ -412,33 +519,34 @@
             if (g_com_ctx.skipped_bytes)
             {
                 WARNING(false, "COM header found, skipped %d bytes.\r\n", g_com_ctx.skipped_bytes);
+#if 1
+                uint8_t* temp = (uint8_t*)MALLOC(g_com_ctx.skipped_bytes);
+                d7a_com_copy_to_linear(temp, g_com_ctx.read_idx + (base - 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;
             }
             
             // Update seqnum
-            if (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++;
+            WARNING(g_com_ctx.rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d (base:%d curr:%d avail:%d read:%d)\r\n",
+                g_com_ctx.rx_seq, seqnum, base, current, g_com_ctx.data_available, g_com_ctx.read_idx);
+                
+            g_com_ctx.rx_seq = seqnum + 1;
             
             nb_pkt++;
             
+            //DPRINT("Got packet id 0x%02X len %d\r\n", id, len);
+            
             if (KAL_COM_FLOWID(id) != KAL_COM_FLOWID_TRC)
             {
                 d7a_com_rx_msg_t* pkt = NULL;
                 pkt = (d7a_com_rx_msg_t*)MALLOC(sizeof(d7a_com_rx_msg_t) - 1 + len);
-                
-                //DPRINT("Got packet id 0x%02X len %d\r\n", id, len);
     
                 // copy data to buffer
                 pkt->blen = len;
                 pkt->id = id;
-                for (i=0 ; i<len ; i++)
-                {
-                    pkt->buffer[i] = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + (base + current + i)) % g_com_ctx.rx_buffer_size];
-                }
+                d7a_com_copy_to_linear(pkt->buffer, g_com_ctx.read_idx + (base + current), len);
                 
                 // add packet to queue
                 d7a_com_new_pkt(pkt);
@@ -448,13 +556,9 @@
                 // Ignore packet
             }
             
-            // Go to EOP
-            current += len - 1;
-            
             // Search next packet
-            base += current + 1;
+            base += current + len;
             current = 0;
-            continue;
         }
         else
         {
@@ -480,7 +584,6 @@
     {
         // 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
         if ((g_com_ctx.last_available != g_com_ctx.data_available) && (g_com_ctx.data_available >= (KAL_COM_HEADER_LEN + 1)))
         {
@@ -496,13 +599,10 @@
     
     d7a_com_tx_buf_t* msg;
     osEvent evt;
+    uint8_t flow_id;
+    
     while (true)
     {
-        if (g_xon == false)
-        {
-            g_com_ctx.xonoff->wait();
-        }
-        
         // 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;
@@ -510,10 +610,25 @@
         // send message
         if (msg != NULL)
         {
-            DPRINT("<-- (0x%02X) %d\r\n", msg->buf[4], (msg->len - KAL_COM_HEADER_LEN));
+            flow_id = msg->buf[4];
+            DPRINT("<-- (0x%02X) %d\r\n", flow_id, (msg->len - KAL_COM_HEADER_LEN));
+
             d7a_com_send(msg->buf, msg->len);
             FREE(msg->buf);
             FREE(msg);
+                        
+            if (KAL_COM_FLOW_SYS_PING == flow_id)
+            {
+                g_com_ctx.tim.reset();
+                g_com_ctx.tim.start();
+            }
+            
+            if (KAL_COM_FLOW_SYS_XACK == flow_id)
+            {
+                DPRINT("XOFF\r\n");
+                g_com_ctx.tx_thread->signal_wait(XON_SIGNAL);
+                DPRINT("XON\r\n");
+            }
         }
     }
 }