Diff: src/d7a_alp.cpp

Revision:

59:b42eae56b51b

Parent:

58:38a366236bda

Child:

60:8d4133fbc060

--- a/src/d7a_alp.cpp	Mon Sep 12 16:36:09 2016 +0000
+++ b/src/d7a_alp.cpp	Fri Sep 23 15:00:41 2016 +0000
@@ -15,6 +15,7 @@
     Queue<d7a_com_rx_msg_t, 16> pkt_queue;
     Queue<d7a_com_rx_msg_t, 8> pl_queue;
     
+    UnsolicitedMsgFunction uns_msg;
     Thread* thread;
 } d7a_alp_ctx_t;
 
@@ -22,10 +23,11 @@
 
 void d7a_alp_thread(const void *p);
 
-d7a_errors_t d7a_alp_open(void)
+d7a_errors_t d7a_alp_open(UnsolicitedMsgFunction uns_msg)
 {
     FPRINT("\r\n");
 
+    g_alp_ctx.uns_msg = uns_msg;
     g_alp_ctx.thread = new Thread(d7a_alp_thread, NULL, osPriorityHigh, DEFAULT_STACK_SIZE);
     
     return D7A_ERR_NONE;
@@ -153,6 +155,23 @@
     return len;
 }
 
+void d7a_alp_free_msg(d7a_msg_t* msg)
+{
+    if (msg->data)
+    {
+        FREE(msg->data);
+    }
+    FREE(msg);
+}
+
+d7a_msg_t* d7a_alp_new_msg(void)
+{
+    d7a_msg_t* msg = (d7a_msg_t*)MALLOC(sizeof(d7a_msg_t));
+    memset(msg, 0, sizeof(d7a_msg_t));
+    msg->err = D7A_ERR_NONE;
+    
+    return msg;
+}
 
 d7a_alp_rsp_t* d7a_alp_parse_pl(d7a_com_rx_msg_t* pkt)
 {    
@@ -166,11 +185,12 @@
     uint8_t len = pkt->blen;
 
     d7a_alp_rsp_t* rsp = (d7a_alp_rsp_t*)MALLOC(sizeof(d7a_alp_rsp_t));
-    memset(rsp, 0, sizeof(d7a_alp_rsp_t));
+    rsp->tag = NO_TAG;
+    rsp->eop = false;
+    rsp->msg = d7a_alp_new_msg();
     
     while ((p - t) < len)
     {
-        
         uint8_t ctrl = *p++;
         switch (ctrl & 0x3F)
         {
@@ -187,35 +207,38 @@
                     p += length;
                     
                     // Fill corresponding fields
-                    rsp->s_istatus = 0x01; // IStatus present
-                    rsp->lb = res->lb; // Get Link Budget
-                    memcpy(rsp->id, res->addressee.id, D7A_UID_LEN); // Get UID
+                    rsp->msg->lb = res->lb; // Get Link Budget
+                    memcpy(rsp->msg->id, res->addressee.id, D7A_UID_LEN); // Get UID
                     
-                    DPRINT("ALP RSP ISTATUS type:%02X lb: %3d ", type, rsp->lb);
-                    PRINT_DATA("UID:", "%02X", rsp->id, D7A_UID_LEN, "\r\n");
+                    DPRINT("ALP RSP ISTATUS type:%02X lb: %3d ", type, rsp->msg->lb);
+                    DPRINT_DATA("UID:", "%02X", rsp->msg->id, D7A_UID_LEN, "\r\n");
                 }
                 else
                 {
-                    rsp->s_status = 0x01; // Status present
-                    rsp->status.aid = *p++; // Action ID
-                    rsp->status.status = *p++; // Status
-                    DPRINT("ALP RSP STATUS aid:%d Status:%d\r\n", rsp->status.aid, rsp->status.status);
+                    uint8_t aid = *p++; // Action ID
+                    rsp->msg->err = *p++; // Status
+                    DPRINT("ALP RSP STATUS aid:%d Status:%d\r\n", aid, rsp->msg->err);
                 }
                 break;
             case ALP_OPCODE_RSP_TAG:
-                rsp->s_tag = (ctrl & ~0x3F) | 0x01;
+                rsp->eop = !!(ctrl & ALP_CTRL_EOP);
                 rsp->tag = *p++; // TAG
-                DPRINT("ALP RSP TAG %d %02x\r\n", rsp->tag, rsp->s_tag);
+                DPRINT("ALP RSP TAG %d EOP %d\r\n", rsp->tag, rsp->eop);
                 break;
             case ALP_OPCODE_RSP_F_DATA:
-                uint8_t fid = *p++; // File ID
+                uint8_t fid;
                 uint32_t offset;
+                uint32_t length;
+                fid = *p++; // File ID
                 p += alp_decode_length(p, &offset); // offset
-                p += alp_decode_length(p, &(rsp->data_len)); // length
-                rsp->data = (uint8_t*)MALLOC(rsp->data_len);
-                p += d7a_alp_add(rsp->data, p, rsp->data_len);
-                DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, rsp->data_len);
-                //dbg_print_data("DATA: ", "%02X ", (uint8_t*)rsp->data, rsp->data_len, "\r\n");
+                p += alp_decode_length(p, &length); // length
+                rsp->msg->data = (d7a_data_t*)MALLOC(sizeof(d7a_data_t) - 1 + length);
+                rsp->msg->data->fid = fid;
+                rsp->msg->data->offset = offset;
+                rsp->msg->data->length = length;
+                p += d7a_alp_add(rsp->msg->data->buf, p, length);
+                DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, length);
+                //DPRINT_DATA("DATA: ", "%02X ", (uint8_t*)rsp->data, rsp->data_len, "\r\n");
                 break;
             default:
                 WARNING(false, "ALP Untreated OP %d\r\n", ctrl);
@@ -292,22 +315,80 @@
     return (uint32_t)(p - t);
 }
 
-void free_pl(d7a_alp_rsp_t* pl)
+void d7a_alp_construct_resp(d7a_msg_t** ret, uint8_t current_tag, uint32_t max_responses)
 {
-    if (pl->data_len)
+    int i = 0;
+    d7a_alp_rsp_t* pl = NULL;
+    d7a_com_rx_msg_t* pkt = NULL;
+    
+    // Parse responses
+    do
     {
-        FREE(pl->data);
-    }
-    FREE(pl);
-}
-
-d7a_msg_t* new_msg(void)
-{
-    d7a_msg_t* msg = (d7a_msg_t*)MALLOC(sizeof(d7a_msg_t));
-    memset(msg, 0, sizeof(d7a_msg_t));
-    msg->err = D7A_ERR_UNKNOWN;
-    
-    return msg;
+        pkt = d7a_alp_wait_pl(60000);
+        
+        if (pkt == NULL)
+        {
+            ret[i] = d7a_alp_new_msg();
+            ret[i]->err = D7A_ERR_CMD_TO;
+            break;
+        }
+        
+        pl = d7a_alp_parse_pl(pkt);
+        
+        // Check TAG
+        if (pl->tag == NO_TAG)
+        {
+            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
+            FREE(pkt);
+            d7a_alp_free_msg(pl->msg);
+            FREE(pl);
+            ret[i] = d7a_alp_new_msg();
+            ret[i]->err = D7A_ERR_UNKNOWN;
+            break;
+        }
+        
+        if (pl->tag != current_tag)
+        {
+            WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
+            d7a_alp_free_msg(pl->msg);
+            FREE(pl);
+            d7a_alp_new_pl(pkt);
+            continue;
+        }
+        
+        FREE(pkt);
+        
+        // Check for END OF PAYLOAD
+        if (pl->eop)
+        {
+            DPRINT("EOP\r\n");
+            
+            // If tag only
+            if (!pl->msg->data && !pl->msg->lb)
+            {
+                // Ignore response
+                d7a_alp_free_msg(pl->msg);
+            }
+            else
+            {
+                DPRINT("last response (err %d)\r\n", pl->msg->err);
+                ret[i] = pl->msg;
+            }
+            
+            FREE(pl);
+            break;
+        }
+        // Wait for new msg
+        else
+        {
+            DPRINT("next response (err %d)\r\n", pl->msg->err);
+            ret[i] = pl->msg;
+            FREE(pl);
+            i++;
+            ASSERT(i <= max_responses, "Too much responses! max: %d\r\n", max_responses);
+        }
+        
+    } while (1);
 }
 
 d7a_msg_t** d7a_alp_write_file(const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, d7a_addressee_t* addressee, uint8_t retry, bool resp)
@@ -318,8 +399,6 @@
     uint8_t* t = p;
     
     bool broadcast = false;
-    d7a_alp_rsp_t* pl = NULL;
-    d7a_com_rx_msg_t* pkt = NULL;
     uint8_t current_tag;
     d7a_msg_t** ret = NULL;
     uint32_t max_responses = 1;
@@ -347,10 +426,7 @@
     {
         ret[i] = NULL;
     }
-    
-    // malloc and init first response
-    ret[0] = new_msg();
-    
+        
     // Construct command
     
     // Tag action
@@ -385,96 +461,9 @@
     d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
 
     i = 0; // start with msg 0
+    
     // Parse responses
-    do
-    {
-        pkt = d7a_alp_wait_pl(60000);
-        
-        if (pkt == NULL)
-        {
-            ret[i]->err = D7A_ERR_CMD_TO;
-            break;
-        }
-        
-        pl = d7a_alp_parse_pl(pkt);
-        
-        // Check TAG
-        if (!(pl->s_tag & 0x01))
-        {
-            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
-            FREE(pkt);
-            free_pl(pl);
-            ret[i]->err = 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);
-        
-        // Get STATUS
-        if (pl->s_status)
-        {
-            ret[i]->err = (d7a_errors_t)pl->status.status;
-        }
-        else
-        {
-            if (pl->s_tag & ALP_CTRL_ERR)
-            {
-                ret[i]->err = D7A_ERR_TX_FAILED;
-            }
-            else
-            {
-                ret[i]->err = D7A_ERR_NONE;
-            }
-        }
-        
-        // Get DATA
-        if (pl->data_len)
-        {
-            ret[i]->data_len = pl->data_len;
-            ret[i]->data = (uint8_t*)MALLOC(ret[i]->data_len);
-            memcpy((void*)ret[i]->data, (void*)pl->data, ret[i]->data_len);
-        }
-        
-        // Get ISTATUS
-        if (pl->s_istatus)
-        {
-            ret[i]->lb = pl->lb;
-            memcpy(ret[i]->id, pl->id, D7A_UID_LEN); // Get UID
-        }
-    
-        // Check for END OF PAYLOAD
-        if (pl->s_tag & ALP_CTRL_EOP)
-        {
-            // If tag only
-            if (!pl->s_status && !pl->s_istatus && !pl->data_len)
-            {
-                // Ignore response
-                FREE(ret[i]);
-                ret[i] = NULL;
-            }
-            
-            free_pl(pl);
-            break;
-        }
-        
-        // Wait for new msg
-        if (pl->s_istatus)
-        {
-            i++;
-            ASSERT(i <= max_responses, "Too much responses! max: %d\r\n", max_responses);
-            ret[i] = new_msg();
-        }
-        
-        free_pl(pl);
-    } while (1);
+    d7a_alp_construct_resp(ret, current_tag, max_responses);
     
     return ret;
 }
@@ -487,8 +476,6 @@
     uint8_t* t = p;
     
     bool broadcast = false;
-    d7a_alp_rsp_t* pl = NULL;
-    d7a_com_rx_msg_t* pkt = NULL;
     uint8_t current_tag;
     d7a_msg_t** ret = NULL;
     uint32_t max_responses = 1;
@@ -518,7 +505,7 @@
     }
     
     // malloc and init first response
-    ret[0] = new_msg();
+    ret[0] = d7a_alp_new_msg();
     
     // Construct command
     
@@ -555,103 +542,9 @@
     d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
 
     i = 0; // start with msg 0
+    
     // Parse responses
-    do
-    {
-        pkt = d7a_alp_wait_pl(60000);
-        
-        if (pkt == NULL)
-        {
-            ret[i]->err = D7A_ERR_CMD_TO;
-            break;
-        }
-        
-        pl = d7a_alp_parse_pl(pkt);
-        
-        // Check TAG
-        if (!(pl->s_tag & 0x01))
-        {
-            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
-            FREE(pkt);
-            free_pl(pl);
-            ret[i]->err = 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);
-        
-        // Get STATUS
-        if (pl->s_status)
-        {
-            DPRINT("err status\r\n");
-            ret[i]->err = (d7a_errors_t)pl->status.status;
-        }
-        else
-        {
-            DPRINT("err not status\r\n");
-            if (pl->s_tag & ALP_CTRL_ERR)
-            {
-                ret[i]->err = D7A_ERR_TX_FAILED;
-            }
-            else
-            {
-                ret[i]->err = D7A_ERR_NONE;
-            }
-        }
-        
-        // Get DATA
-        if (pl->data_len)
-        {
-            DPRINT("get data\r\n");
-            ret[i]->data_len = pl->data_len;
-            ret[i]->data = (uint8_t*)MALLOC(ret[i]->data_len);
-            memcpy((void*)ret[i]->data, (void*)pl->data, ret[i]->data_len);
-        }
-        
-        // Get ISTATUS
-        if (pl->s_istatus)
-        {
-            DPRINT("get istatus\r\n");
-            ret[i]->lb = pl->lb;
-            memcpy(ret[i]->id, pl->id, D7A_UID_LEN); // Get UID
-        }
-    
-        // Check for END OF PAYLOAD
-        if (pl->s_tag & ALP_CTRL_EOP)
-        {
-            DPRINT("EOP\r\n");
-            
-            // If tag only
-            if (!pl->s_status && !pl->s_istatus && !pl->data_len)
-            {
-                // Ignore response
-                FREE(ret[i]);
-                ret[i] = NULL;
-            }
-            
-            free_pl(pl);
-            break;
-        }
-        
-        // Wait for new msg
-        if (pl->s_istatus)
-        {
-            DPRINT("next response\r\n");
-            i++;
-            ASSERT(i <= max_responses, "Too much responses! max: %d\r\n", max_responses);
-            ret[i] = new_msg();
-        }
-        
-        free_pl(pl);
-    } while (1);
+    d7a_alp_construct_resp(ret, current_tag, max_responses);
     
     return ret;
 }
@@ -674,6 +567,22 @@
                 d7a_alp_new_pl(pkt);
                 
                 break;
+            case KAL_COM_FLOW_AT_UNS:
+                DPRINT("KAL_COM_FLOW_AT_UNS\r\n");
+                d7a_msg_t** ret = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * 2);
+                
+                d7a_alp_rsp_t* pl = d7a_alp_parse_pl(pkt);
+                
+                ret[0] = pl->msg;
+                ret[1] = NULL;
+                
+                FREE(pkt);
+                FREE(pl);
+                
+                // Callback
+                g_alp_ctx.uns_msg(ret);
+                
+                break;
             default:
                 EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
                 FREE(pkt);