Diff: src/d7a_com.cpp

Revision:

30:d775c1409849

Parent:

28:0376b97b4b55

Child:

31:ab9bfdbc6b44

--- a/src/d7a_com.cpp	Tue May 03 16:53:52 2016 +0000
+++ b/src/d7a_com.cpp	Wed May 25 08:23:15 2016 +0000
@@ -7,6 +7,11 @@
 #include "d7a_modem.h"
 #include "d7a_sys.h"
 
+#define MAX_WAITING_THREADS     16
+static volatile bool g_xon;
+static volatile uint8_t g_waiting_nb;
+
+
 typedef struct {
     DigitalOut*             rts;
     InterruptIn*            cts;
@@ -35,6 +40,8 @@
     Semaphore*              data_parsing;
     // CTS management
     Semaphore*              cts_int;
+    // XON/XOFF management
+    Semaphore*              xonoff;
     
     // Data treatment threads
     Thread*                 thread;
@@ -124,6 +131,7 @@
     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(MAX_WAITING_THREADS);
     g_com_ctx.thread =          new Thread(d7a_com_thread, NULL, osPriorityBelowNormal, DEFAULT_STACK_SIZE*2);
         
     /* XXX: Unknown bug:
@@ -132,8 +140,17 @@
     Normal Baudrate should be 115200.
     */
     g_com_ctx.serial->baud(117000); // 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);
+    
+    // Consume Semaphore
+    for(uint8_t i=0 ; i<MAX_WAITING_THREADS ; i++)
+    {
+        g_com_ctx.xonoff->wait();
+    }
+    g_xon = true;
+    g_waiting_nb = 0;
 }
 
 // Destructor
@@ -161,6 +178,14 @@
 {    
     int i;
     
+    if (!g_xon)
+    {
+        // Wait if COM port is OFF
+        g_waiting_nb++;
+        DPRINT("COM WAITING (%d threads)\r\n", g_waiting_nb);
+        g_com_ctx.xonoff->wait();
+    }
+    
     *(g_com_ctx.rts) = 1;
     
     Thread::wait(5);
@@ -176,9 +201,9 @@
 // Formats and send packet throught Serial.
 void d7a_com_send_msg(d7a_com_tx_msg_t* msg)
 {
-    FPRINT("\r\n");
     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);
     
@@ -206,10 +231,27 @@
     DPRINT("<-- %d (0x%02X)\r\n", (len - KAL_COM_HEADER_LEN), msg->id);
 }
 
+void d7a_com_dump(uint8_t* buf, uint8_t len, d7a_com_flow_t flow)
+{
+    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)
 {
     //FPRINT("\r\n");
+    if (KAL_COM_FLOWID(pkt->id) != KAL_COM_FLOWID_TRC)
+    {
+        DPRINT("--> %d (0x%02X)\r\n", pkt->blen, pkt->id);
+    }
+    
     // Distribute packet types to processes
     switch (KAL_COM_FLOWID(pkt->id))
     {
@@ -220,7 +262,33 @@
             d7a_modem_new_pkt(pkt);
             break;
         case KAL_COM_FLOWID_SYS:
-            d7a_sys_new_pkt(pkt);
+            // This has to be here to avoid going to another process
+            if (pkt->id == KAL_COM_FLOW_SYS_XON)
+            {
+                DPRINT("XON (%d threads)\r\n", g_waiting_nb);
+                g_xon = true;
+                // Free all threads
+                for(uint8_t i=0 ; i<g_waiting_nb ; i++)
+                {
+                    g_com_ctx.xonoff->release();
+                }
+                g_waiting_nb = 0;
+                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;
+                FREE(pkt);
+                DPRINT("XOFF\r\n");
+            }
+            else
+            {
+                d7a_sys_new_pkt(pkt);
+            }
+            break;
+        case KAL_COM_FLOWID_TRC:
             break;
         default:
             EPRINT("Untreated pkt type 0x%02X\r\n", pkt->id);
@@ -322,7 +390,6 @@
             data_read += i - 1;
             
             // add packet to queue
-            DPRINT("--> %d (0x%02X)\r\n", pkt->blen, pkt->id);
             d7a_com_new_pkt(pkt);
             
             pkt_found = true;