Diff: src/d7a_alp.cpp

Revision:

47:19f22e1a448a

Parent:

46:665391110051

Child:

49:81d5bddb02f0

--- a/src/d7a_alp.cpp	Thu Sep 01 18:13:46 2016 +0000
+++ b/src/d7a_alp.cpp	Fri Sep 02 09:07:03 2016 +0000
@@ -9,12 +9,12 @@
 #include "d7a_sys.h"
 #include "d7a_alp.h"
 
-
 typedef struct {
     uint8_t tag;
     uint8_t buffer[100];
     Queue<d7a_com_rx_msg_t, 16> pkt_queue;
     Queue<d7a_com_rx_msg_t, 8> pl_queue;
+    
     Thread* thread;
 } d7a_alp_ctx_t;
 
@@ -198,8 +198,6 @@
         }
     }
     
-    FREE(pkt);
-    
     ASSERT((p - t) == len, "Payload wrong size: %d expected %d\r\n", (p - t), len);
     
     return rsp;
@@ -294,23 +292,42 @@
     
     d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
     
-    d7a_alp_rsp_t* pl;
-    d7a_errors_t status;
+    d7a_alp_rsp_t* pl = NULL;
+    d7a_com_rx_msg_t* pkt = NULL;
+    d7a_errors_t status = D7A_ERR_UNKNOWN;
+    uint8_t current_tag = g_alp_ctx.tag;
     
     do
     {
-        pl = d7a_alp_parse_pl(d7a_alp_wait_pl(60000));
+        pkt = d7a_alp_wait_pl(60000);
         
-        if (pl == NULL)
+        if (pkt == NULL)
         {
             status = D7A_ERR_CMD_TO;
-            free_pl(pl);
             break;
         }
         
-        ASSERT(pl->tag_status & 0x01, "No tag in payload expected %d\r\n", g_alp_ctx.tag);
-        ASSERT(pl->tag == g_alp_ctx.tag, "Wrong tag %d expected %d\r\n", pl->tag, g_alp_ctx.tag);
+        pl = d7a_alp_parse_pl(pkt);
+        
+        if (!(pl->tag_status & 0x01))
+        {
+            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
+            FREE(pkt);
+            free_pl(pl);
+            status = D7A_ERR_UNKNOWN;
+            break;
+        }
         
+        if (pl->tag != current_tag)
+        {
+            //WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
+            free_pl(pl);
+            d7a_alp_new_pl(pkt);
+            continue;
+        }
+        
+        FREE(pkt);
+                
         if (pl->tag_status & ALP_CTRL_ERR)
         {
             if (pl->nb_status)
@@ -378,22 +395,41 @@
     
     d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
     
-    d7a_alp_rsp_t* pl;
-    d7a_errors_t status;
+    d7a_alp_rsp_t* pl = NULL;
+    d7a_com_rx_msg_t* pkt = NULL;
+    d7a_errors_t status = D7A_ERR_UNKNOWN;
+    uint8_t current_tag = g_alp_ctx.tag;
     
     do
     {
-        pl = d7a_alp_parse_pl(d7a_alp_wait_pl(60000));
+        pkt = d7a_alp_wait_pl(60000);
         
-        if (pl == NULL)
+        if (pkt == NULL)
         {
             status = D7A_ERR_CMD_TO;
-            free_pl(pl);
             break;
         }
         
-        ASSERT(pl->tag_status & 0x01, "No tag in payload expected %d\r\n", g_alp_ctx.tag);
-        ASSERT(pl->tag == g_alp_ctx.tag, "Wrong tag %d expected %d\r\n", pl->tag, g_alp_ctx.tag);
+        pl = d7a_alp_parse_pl(pkt);
+        
+        if (!(pl->tag_status & 0x01))
+        {
+            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
+            FREE(pkt);
+            free_pl(pl);
+            status = D7A_ERR_UNKNOWN;
+            break;
+        }
+        
+        if (pl->tag != current_tag)
+        {
+            //WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
+            free_pl(pl);
+            d7a_alp_new_pl(pkt);
+            continue;
+        }
+        
+        FREE(pkt);
         
         if (pl->tag_status & ALP_CTRL_ERR)
         {
@@ -410,6 +446,7 @@
         if (pl->data != NULL)
         {
             memcpy((void*)buf, (void*)pl->data, pl->length);
+            status = D7A_ERR_NONE;
         }
     
         if (pl->tag_status & ALP_CTRL_EOP)