Diff: src/d7a_alp.cpp

Revision:

45:b85384e7d825

Parent:

43:28202405094d

Child:

46:665391110051

--- a/src/d7a_alp.cpp	Tue Aug 30 17:15:31 2016 +0000
+++ b/src/d7a_alp.cpp	Thu Sep 01 09:35:27 2016 +0000
@@ -14,7 +14,7 @@
     uint8_t tag;
     uint8_t buffer[100];
     Queue<d7a_com_rx_msg_t, 16> pkt_queue;
-    Queue<void, 4> tag_queue;
+    Queue<d7a_com_rx_msg_t, 8> pl_queue;
     Thread* thread;
 } d7a_alp_ctx_t;
 
@@ -35,21 +35,27 @@
     ASSERT(g_alp_ctx.pkt_queue.put(pkt) == osOK, "ALP queue full!\r\n");
 }
 
-d7a_com_rx_msg_t* d7a_alp_wait_pkt( uint32_t millisec )
+void d7a_alp_new_pl(d7a_com_rx_msg_t* pl)
+{
+    FPRINT("\r\n");
+    ASSERT(g_alp_ctx.pl_queue.put(pl) == osOK, "ALP OP queue full!\r\n");
+}
+
+d7a_com_rx_msg_t* d7a_alp_wait_pkt(uint32_t millisec)
 {
     FPRINT("\r\n");
     osEvent evt = g_alp_ctx.pkt_queue.get(millisec);
     return (evt.status == osEventMessage)? (d7a_com_rx_msg_t*)evt.value.p : NULL;
 }
 
-bool d7a_alp_wait_tag( uint32_t millisec )
+d7a_com_rx_msg_t* d7a_alp_wait_pl(uint32_t millisec)
 {
     FPRINT("\r\n");
-    osEvent evt = g_alp_ctx.tag_queue.get(millisec);
-    return (evt.status == osEventMessage)? false : true;
+    osEvent evt = g_alp_ctx.pl_queue.get(millisec);
+    return (evt.status == osEventMessage)? (d7a_com_rx_msg_t*)evt.value.p : NULL;
 }
 
-uint8_t d7a_alp_encode_length(uint8_t* p, uint32_t len)
+uint32_t d7a_alp_encode_length(uint8_t* p, uint32_t len)
 {
     if (len <= 0x3F)
     {
@@ -79,30 +85,132 @@
     }
 }
 
+uint32_t alp_decode_length(uint8_t* p, uint32_t* len)
+{
+    uint32_t tmp = 0;
+    switch ((*p) & 0xC0)
+    {
+        case 0xC0: // 0xCx xx xx xx
+            tmp  =  (*p++ & 0x3F) << 24;
+            tmp +=   *p++         << 16;
+            tmp +=   *p++         <<  8;
+            tmp +=   *p++         <<  0;
+            *len = tmp;
+            return 4;
+        case 0x80: // 0x8x xx xx : 16384 <= Len <4194303
+            tmp  =  (*p++ & 0x3F) << 16;
+            tmp +=   *p++         <<  8;
+            if (tmp == 0)            {
+                // 0x8000 ActP special ActP code
+                // Do not fetch the extra byte
+                tmp = 2;
+            }
+            else
+            {
+                tmp += *p++       <<  0;
+            }
+            *len = tmp;
+            return 3;
+        case 0x40: // 0x4x xx : 64 <= Len < 16383
+            tmp  =  (*p++ & 0x3F)  <<  8;
+            tmp +=   *p++          <<  0;
+            if (tmp == 0)
+            {
+                // 0x4000 ActP special ActP code
+                tmp = 1;
+            }
+            *len = tmp;
+            return 2;
+        case 0: // Len <63
+            tmp  = (*p++ & 0x3F) <<  0;
+            *len = tmp;
+            return 1;
+    }
+    
+    return 0;
+}
 
-uint32_t d7a_alp_add(uint8_t* buf, const uint8_t* data, uint32_t len)
+uint32_t d7a_alp_add(uint8_t* p, const uint8_t* data, uint32_t len)
 {
-    memcpy(buf, data, len);
+    memcpy(p, data, len);
     
     return len;
 }
 
-uint32_t d7a_alp_tag(uint8_t* p)
+d7a_alp_rsp_t* d7a_alp_parse_pl(d7a_com_rx_msg_t* pkt)
+{
+    uint8_t* p = pkt->buffer;
+    uint8_t* t = p;
+    uint8_t len = pkt->blen;
+
+    d7a_alp_rsp_t* rsp = (d7a_alp_rsp_t*)MALLOC(sizeof(d7a_alp_rsp_t));
+    
+    rsp->nb_status = 0;
+    rsp->length = 0;
+    rsp->data = NULL;
+    
+    if (pkt == NULL)
+    {
+        return rsp;
+    }
+    
+    while ((p - t) < len)
+    {
+        
+        uint8_t ctrl = *p++;
+        switch (ctrl & 0x3F)
+        {
+            case ALP_OPCODE_RSP_STATUS:
+                rsp->status[rsp->nb_status].aid = *p++; // Action ID
+                rsp->status[rsp->nb_status].status = *p++; // Status
+                DPRINT("ALP RSP STATUS[%d] aid:%d Status:%d\r\n", rsp->nb_status, rsp->status[rsp->nb_status].aid, rsp->status[rsp->nb_status].status);
+                rsp->nb_status++;
+                break;
+            case ALP_OPCODE_RSP_TAG:
+                rsp->tag_status = (ctrl & ~0x3F) | 0x01;
+                rsp->tag = *p++; // TAG
+                DPRINT("ALP RSP TAG %d %02x\r\n", rsp->tag, rsp->tag_status);
+                break;
+            case ALP_OPCODE_RSP_F_DATA:
+                uint8_t fid = *p++; // File ID
+                uint32_t offset;
+                p += alp_decode_length(p, &offset); // offset
+                p += alp_decode_length(p, &(rsp->length)); // length
+                rsp->data = (uint8_t*)MALLOC(rsp->length);
+                p += d7a_alp_add(rsp->data, p, rsp->length);
+                DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, rsp->length);
+                //dbg_print_data("DATA: ", "%02X ", (uint8_t*)rsp->data, rsp->length, "\r\n");
+                break;
+            default:
+                WARNING(false, "ALP Untreated OP %d\r\n", ctrl);
+                break;
+        }
+    }
+    
+    FREE(pkt);
+    
+    ASSERT((p - t) == len, "Payload wrong size: %d expected %d\r\n", (p - t), len);
+    
+    return rsp;
+}
+
+
+uint32_t d7a_alp_tag(uint8_t* p, bool eop)
 {
     uint8_t* t = p;
     
-    *p++ = ALP_OPCODE_TAG + ALP_CTRL_EOP;
+    *p++ = ALP_OPCODE_TAG + ((eop)? ALP_CTRL_EOP : 0);
     *p++ = ++g_alp_ctx.tag;
     
     return (uint32_t)(p - t);
 }
 
-uint32_t d7a_alp_forward(uint8_t* p, d7a_addressee_t* addressee, uint8_t retry, bool resp)
+uint32_t d7a_alp_forward(uint8_t* p, d7a_addressee_t* addressee, uint8_t retry, bool ack, bool resp)
 {
     uint8_t* t = p;
     
-    *p++ = ALP_OPCODE_FORWARD + ALP_CTRL_RESP;
-    *p++ = (retry << 3) | ((resp)? 2 : 0);
+    *p++ = ALP_OPCODE_FORWARD + ((resp)? ALP_CTRL_RESP : 0);
+    *p++ = (retry << 3) | ((ack)? 2 : 0);
     *p++ = 0; // dorm_to
     *p++ = addressee->ctrl.bf.nls | (D7A_ID_UID<<4);
     *p++ = addressee->xcl.byte;
@@ -111,11 +219,11 @@
     return (uint32_t)(p - t);
 }
 
-uint32_t d7a_alp_write_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf)
+uint32_t d7a_alp_write_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, bool resp)
 {
     uint8_t* t = p;
     
-    *p++ = ALP_OPCODE_F_WR_DATA + ALP_CTRL_RESP;
+    *p++ = ALP_OPCODE_F_WR_DATA + ((resp)? ALP_CTRL_RESP : 0);
     *p++ = file_id;
     p += d7a_alp_encode_length(p, offset);
     p += d7a_alp_encode_length(p, size);
@@ -124,11 +232,11 @@
     return (uint32_t)(p - t);
 }
 
-uint32_t d7a_alp_read_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf)
+uint32_t d7a_alp_read_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, bool resp)
 {
     uint8_t* t = p;
     
-    *p++ = ALP_OPCODE_F_RD_DATA + ALP_CTRL_RESP;
+    *p++ = ALP_OPCODE_F_RD_DATA + ((resp)? ALP_CTRL_RESP : 0);
     *p++ = file_id;
     p += d7a_alp_encode_length(p, offset);
     p += d7a_alp_encode_length(p, size);
@@ -136,65 +244,176 @@
     return (uint32_t)(p - t);
 }
 
-
-bool 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)
+void free_pl(d7a_alp_rsp_t* pl)
 {
-    uint8_t* p = &g_alp_ctx.buffer[0];
-    uint8_t* t = p;
-    
-    // Tag action
-    p += d7a_alp_tag(p);
-    
-    if (addressee)
+    if (pl->data != NULL)
     {
-        // Forward
-        p += d7a_alp_forward(p, addressee, retry, resp);
+        FREE(pl->data);
     }
-    
-    // Write action
-    p += d7a_alp_write_action(p, file_id, offset, size, buf);
-    
-    d7a_com_dump(p, (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
-    
-    return d7a_alp_wait_tag(3000);
+    FREE(pl);
 }
 
-bool d7a_alp_read_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)
+
+int8_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)
 {
     uint8_t* p = &g_alp_ctx.buffer[0];
     uint8_t* t = p;
     
     // Tag action
-    p += d7a_alp_tag(p);
+    p += d7a_alp_tag(p, true);
+        
+    if (addressee)
+    {
+        // Forward
+        p += d7a_alp_forward(p, addressee, retry, true, true);
+    }
+    
+    // Write action
+    p += d7a_alp_write_action(p, file_id, offset, size, buf, resp);
+    
+    d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
+    
+    d7a_alp_rsp_t* pl;
+    int8_t status;
+    
+    if (addressee)
+    {
+        pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
+        if (pl->nb_status == 1
+         && pl->status[0].status == 0
+         && pl->tag_status == 0x01
+         && pl->tag == g_alp_ctx.tag)
+        {
+            // OK
+        }
+        else
+        {
+            status = pl->status[0].status;
+        }
+        
+        free_pl(pl);
+        
+        if (status)
+        {
+            return status;
+        }
+    }
+    
+    pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
+    if (resp)
+    {
+        if (pl->nb_status == 1
+         && pl->status[0].status == 0
+         && pl->tag_status == 0x01 | ALP_CTRL_EOP
+         && pl->tag == g_alp_ctx.tag
+         )
+        {
+            // OK
+        }
+        else
+        {
+            
+            if (pl->nb_status)
+            {
+                status = pl->status[0].status;
+            }
+            else
+            {
+                status = D7A_ERR_TX_FAILED;
+            }
+        }
+    }
+    else
+    {
+        if (pl->nb_status == 0
+         && pl->tag_status == 0x01 | ALP_CTRL_EOP
+         && pl->tag == g_alp_ctx.tag
+         )
+        {
+            // OK
+        }
+        else
+        {
+            status = D7A_ERR_TX_FAILED;
+        }
+    }
+    
+    free_pl(pl);
+    
+    return status;
+}
+
+int8_t d7a_alp_read_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)
+{
+    uint8_t* p = &g_alp_ctx.buffer[0];
+    uint8_t* t = p;
+    
+    // Tag action
+    p += d7a_alp_tag(p, true);
     
     if (addressee)
     {
         // Forward
-        p += d7a_alp_forward(p, addressee, retry, true);
+        p += d7a_alp_forward(p, addressee, retry, true, true);
     }
     
     // Read action
-    p += d7a_alp_read_action(p, file_id, offset, size, buf);
+    p += d7a_alp_read_action(p, file_id, offset, size, buf, true);
     
-    d7a_com_dump(p, (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
+    d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
+    
+    d7a_alp_rsp_t* pl;
+    int8_t status;
     
-    ASSERT(!d7a_alp_wait_tag(3000), "ALP TAG TO\r\n");
+    if (addressee)
+    {
+        pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
+        if (pl->nb_status == 1
+         && pl->status[0].status == 0
+         && pl->tag_status == 0x01
+         && pl->tag == g_alp_ctx.tag)
+        {
+            // OK
+        }
+        else
+        {
+            status = pl->status[0].status;
+        }
+        
+        free_pl(pl);
+        
+        if (status)
+        {
+            return status;
+        }
+    }
     
-    return 0;
-}
+    pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
 
-uint32_t d7a_alp_parse_tag(uint8_t* p, uint8_t* tag)
-{
-    uint8_t* t = p;
-
-    ASSERT(p != NULL, "ALP Parse Error: Payload TAG NULL\r\n");
+    if (pl->nb_status == 0
+     && pl->data != NULL
+     && pl->tag_status == 0x01 | ALP_CTRL_EOP
+     && pl->tag == g_alp_ctx.tag
+     )
+    {
+        // OK
+        memcpy((void*)buf, (void*)pl->data, pl->length);
+    }
+    else
+    {
+        if (pl->nb_status)
+        {
+            status = pl->status[0].status;
+        }
+        else
+        {
+            status = D7A_ERR_TX_FAILED;
+        }
+    }
     
-    uint8_t ctrl = *p++ & 0x3F;
-    ASSERT(ctrl == ALP_OPCODE_RSP_TAG, "ALP Parse Error: Wrong OP_CODE for TAG (%d)\r\n", ctrl);
-
-    *tag = *p++;
-
-    return (uint32_t)(p - t);
+    free_pl(pl);
+    
+    return status;
 }
 
 void d7a_alp_thread(const void *p)
@@ -210,24 +429,15 @@
         switch(pkt->id)
         {
             case KAL_COM_FLOW_AT_RESP:
-                PRINT("KAL_COM_FLOW_AT_RESP\r\n");
-                uint8_t* p = pkt->buffer;
-                uint8_t* t = p;
-                uint32_t eop = pkt->blen;
-                
-                uint8_t tag;
-                p += d7a_alp_parse_tag(p, &tag);
-                ASSERT(tag == g_alp_ctx.tag, "ALP Wrong tag %d expected %d\r\n", tag, g_alp_ctx.tag);
-                g_alp_ctx.tag_queue.put((void*)tag);
-                
-                
+                DPRINT("KAL_COM_FLOW_AT_RESP\r\n");
+
+                d7a_alp_new_pl(pkt);
                 
                 break;
             default:
                 EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
+                FREE(pkt);
                 break;
         }
-        
-        FREE(pkt);
     }
 }
\ No newline at end of file