Diff: src/d7a_com.cpp

Revision:

45:b85384e7d825

Parent:

44:8ebe6b70f932

Child:

46:665391110051

--- a/src/d7a_com.cpp	Tue Aug 30 17:15:31 2016 +0000
+++ b/src/d7a_com.cpp	Thu Sep 01 09:35:27 2016 +0000
@@ -2,6 +2,7 @@
 #include "rtos.h"
 #include "dbg.h"
 #include "d7a_com.h"
+#include "d7a_alp.h"
 #include "d7a_common.h"
 #include "d7a_fs.h"
 #include "d7a_modem.h"
@@ -13,7 +14,7 @@
 
 typedef struct {
     uint32_t len;
-    uint8_t* buf;
+    uint8_t buf[1];
 } d7a_com_tx_buf_t;
 
 enum {
@@ -308,25 +309,7 @@
         memcpy((void*)&linear_buffer[first_part_size], (const void*)&g_com_ctx.rx_buffer[0], second_part_size);
     }
 }
-
-
-void d7a_com_print_stats(void)
-{
-    PRINT("State: %d\r\n", g_com_ctx.state);
-    PRINT("Data: %d\r\n", g_com_ctx.data_available);
-    
-#if 0
-    if (g_com_ctx.data_available)
-    {
-        uint8_t* temp = (uint8_t*)MALLOC(g_com_ctx.data_available);
-        d7a_com_copy_to_linear(temp, g_com_ctx.read_idx, g_com_ctx.data_available);
-        PRINT_DATA("", "%02X ", temp, g_com_ctx.data_available, "\r\n");
-        FREE(temp);
-    }
-#endif
-}
             
-
 /**
     Wakes-up modem and send data throught Serial.
 
@@ -334,19 +317,21 @@
     @param int                  Data length
     @return void
 */
-static void d7a_com_send(const uint8_t* buffer, int length)
+static void d7a_com_send(d7a_com_tx_buf_t* tx_buf)
 {    
-    FPRINT("(len:%d)\r\n", length);
+    FPRINT("\r\n");
+    
+    DPRINT("<-- (0x%02X) %d\r\n", tx_buf->buf[4], (tx_buf->len - KAL_COM_HEADER_LEN));
     
     *(g_com_ctx.rts) = 1;
     
-    //dbg_print_data("%02X ", (uint8_t*)buffer, length);
-    //DPRINT("\r\n");
+    //dbg_print_data("", "%02X ", (uint8_t*)tx_buf->buf, tx_buf->len, "\r\n");
+    
     Thread::wait(2);
     
-    for (uint32_t i=0 ; i<length ; i++)
+    for (uint32_t i=0 ; i<tx_buf->len ; i++)
     {
-        g_com_ctx.serial->putc(buffer[i]);
+        g_com_ctx.serial->putc(tx_buf->buf[i]);
     }
    
     //Thread::wait(2);
@@ -354,71 +339,44 @@
     *(g_com_ctx.rts) = 0;
 }
 
-
-static void d7a_com_post_tx(uint8_t* buffer, uint32_t length)
-{
-    FPRINT("(len:%d)\r\n", length);
-    
-    d7a_com_tx_buf_t* tx_buf = (d7a_com_tx_buf_t*)MALLOC(sizeof(d7a_com_tx_buf_t));
-    
-    tx_buf->len = length;
-    tx_buf->buf = buffer;
-    
-    g_com_ctx.tx_queue.put(tx_buf);
-}
-
 // Formats and send packet throught Serial.
-void d7a_com_send_msg(d7a_com_tx_msg_t* msg)
+d7a_com_tx_buf_t* d7a_com_new_msg(d7a_com_tx_msg_t* msg)
 {    
-    uint8_t* buf;
-    uint16_t len = msg->alen + msg->plen;
+    uint8_t len = KAL_COM_HEADER_LEN + msg->alen + msg->plen;
     FPRINT("(len:%d)\r\n", len);
     
-    buf = (uint8_t*)MALLOC(KAL_COM_HEADER_LEN + len);
+    d7a_com_tx_buf_t* tx_buf = (d7a_com_tx_buf_t*)MALLOC(sizeof(d7a_com_tx_buf_t) - 1 + 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;
+    uint8_t* p = tx_buf->buf;
+    uint8_t* t = p;
+    *p++ = (uint8_t)KAL_COM_SYNC_BYTE_0;
+    *p++ = (uint8_t)KAL_COM_SYNC_BYTE_1;
+    *p++ = (uint8_t)msg->alen + msg->plen;
+    *p++ = (uint8_t)g_com_ctx.tx_seq++;
+    *p++ = (uint8_t)msg->id;
 
     // 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);
+    memcpy(p, msg->pbuf, msg->plen);
+    p += msg->plen;
+    memcpy(p, msg->abuf, msg->alen);
+    p += msg->alen;
     
-    DPRINT("<-- (0x%02X) %d\r\n", buf[4], (len - KAL_COM_HEADER_LEN));
+    tx_buf->len = (uint32_t)(p - t);
     
-    d7a_com_send(buf, len);
+    ASSERT(tx_buf->len == len, "New msg wrong length %d expected %d\r\n", tx_buf->len, len);
+    
+    return tx_buf;
 }
 
 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);
-    
-    buf = (uint8_t*)MALLOC(KAL_COM_HEADER_LEN + len);
+{
+    FPRINT("\r\n");
     
-    // 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);
-    
-    d7a_com_post_tx(buf, len);
+    g_com_ctx.tx_queue.put(d7a_com_new_msg(msg));
 }
 
-
 void d7a_com_dump(uint8_t* buf, uint8_t len, d7a_com_flow_t flow)
 {
     d7a_com_tx_msg_t msg;
@@ -430,17 +388,6 @@
     d7a_com_post_msg(&msg);
 }
 
-void d7a_com_force_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");
@@ -456,6 +403,9 @@
         case KAL_COM_FLOWID_CMD:
             d7a_modem_new_pkt(pkt);
             break;
+        case KAL_COM_FLOWID_ALP:
+            d7a_alp_new_pkt(pkt);
+            break;
         case KAL_COM_FLOWID_SYS:
             // This has to be here to avoid going to another process
             if (pkt->id == KAL_COM_FLOW_SYS_XON)
@@ -538,7 +488,7 @@
                     g_com_ctx.data_parsing->release();
                 }
                 
-                DPRINT("COM header found (body %d/%d bytes)\r\n", g_com_ctx.data_available, g_com_ctx.msg.blen);
+                //DPRINT("COM header found (body %d/%d bytes)\r\n", g_com_ctx.data_available, g_com_ctx.msg.blen);
                 break;
             }
         }
@@ -550,7 +500,7 @@
     
     if (g_com_ctx.skipped_bytes)
     {
-        WARNING(false, "Skipped %d bytes.\r\n", 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);
@@ -653,10 +603,8 @@
         if (msg != NULL)
         {
             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);
+            d7a_com_send(msg);
             FREE(msg);
                         
             if (KAL_COM_FLOW_SYS_PING == flow_id)