Diff: src/d7a_alp.cpp

Revision:

43:28202405094d

Child:

45:b85384e7d825

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/d7a_alp.cpp	Tue Aug 30 16:59:12 2016 +0000
@@ -0,0 +1,233 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "dbg.h"
+#include "d7a.h"
+#include "d7a_com.h"
+#include "d7a_common.h"
+#include "d7a_fs.h"
+#include "d7a_modem.h"
+#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<void, 4> tag_queue;
+    Thread* thread;
+} d7a_alp_ctx_t;
+
+static d7a_alp_ctx_t g_alp_ctx;
+
+void d7a_alp_thread(const void *p);
+
+void d7a_alp_open(void)
+{
+    FPRINT("\r\n");
+
+    g_alp_ctx.thread = new Thread(d7a_alp_thread, NULL, osPriorityHigh, DEFAULT_STACK_SIZE);
+}
+
+void d7a_alp_new_pkt(d7a_com_rx_msg_t* pkt)
+{
+    FPRINT("\r\n");
+    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 )
+{
+    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 )
+{
+    FPRINT("\r\n");
+    osEvent evt = g_alp_ctx.tag_queue.get(millisec);
+    return (evt.status == osEventMessage)? false : true;
+}
+
+uint8_t d7a_alp_encode_length(uint8_t* p, uint32_t len)
+{
+    if (len <= 0x3F)
+    {
+        *p++ = len;
+        return 1;
+    }
+    else if (len <= 0x3FFF)
+    {
+        *p++ = 0x40 + (uint8_t)(len >> 8);
+        *p++ =        (uint8_t)(len & 0xFF);
+        return 2;
+    }
+    else if (len <= 0x3FFFFF)
+    {
+        *p++ = 0x80 + (uint8_t) (len >> 16);
+        *p++ =        (uint8_t)((len >> 8) & 0xFF);
+        *p++ =        (uint8_t) (len       & 0xFF);
+        return 3;
+    }
+    else
+    {
+        *p++ = 0xC0 + (uint8_t) (len >> 24);
+        *p++ =        (uint8_t)((len >> 16) & 0xFF);
+        *p++ =        (uint8_t)((len >>  8) & 0xFF);
+        *p++ =        (uint8_t) (len        & 0xFF);
+        return 4;
+    }
+}
+
+
+uint32_t d7a_alp_add(uint8_t* buf, const uint8_t* data, uint32_t len)
+{
+    memcpy(buf, data, len);
+    
+    return len;
+}
+
+uint32_t d7a_alp_tag(uint8_t* p)
+{
+    uint8_t* t = p;
+    
+    *p++ = ALP_OPCODE_TAG + ALP_CTRL_EOP;
+    *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)
+{
+    uint8_t* t = p;
+    
+    *p++ = ALP_OPCODE_FORWARD + ALP_CTRL_RESP;
+    *p++ = (retry << 3) | ((resp)? 2 : 0);
+    *p++ = 0; // dorm_to
+    *p++ = addressee->ctrl.bf.nls | (D7A_ID_UID<<4);
+    *p++ = addressee->xcl.byte;
+    p += d7a_alp_add(p, addressee->id, D7A_UID_LEN);
+    
+    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)
+{
+    uint8_t* t = p;
+    
+    *p++ = ALP_OPCODE_F_WR_DATA + ALP_CTRL_RESP;
+    *p++ = file_id;
+    p += d7a_alp_encode_length(p, offset);
+    p += d7a_alp_encode_length(p, size);
+    p += d7a_alp_add(p, buf, size);
+    
+    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)
+{
+    uint8_t* t = p;
+    
+    *p++ = ALP_OPCODE_F_RD_DATA + ALP_CTRL_RESP;
+    *p++ = file_id;
+    p += d7a_alp_encode_length(p, offset);
+    p += d7a_alp_encode_length(p, size);
+    
+    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)
+{
+    uint8_t* p = &g_alp_ctx.buffer[0];
+    uint8_t* t = p;
+    
+    // Tag action
+    p += d7a_alp_tag(p);
+    
+    if (addressee)
+    {
+        // Forward
+        p += d7a_alp_forward(p, addressee, retry, resp);
+    }
+    
+    // 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);
+}
+
+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)
+{
+    uint8_t* p = &g_alp_ctx.buffer[0];
+    uint8_t* t = p;
+    
+    // Tag action
+    p += d7a_alp_tag(p);
+    
+    if (addressee)
+    {
+        // Forward
+        p += d7a_alp_forward(p, addressee, retry, true);
+    }
+    
+    // Read action
+    p += d7a_alp_read_action(p, file_id, offset, size, buf);
+    
+    d7a_com_dump(p, (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
+    
+    ASSERT(!d7a_alp_wait_tag(3000), "ALP TAG TO\r\n");
+    
+    return 0;
+}
+
+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");
+    
+    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);
+}
+
+void d7a_alp_thread(const void *p)
+{
+    FPRINT("\r\n");
+    d7a_com_rx_msg_t* pkt;
+    
+    while (true)
+    {
+        pkt = d7a_alp_wait_pkt();
+        ASSERT(pkt != NULL, "ALP NULL pkt\r\n");
+
+        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);
+                
+                
+                
+                break;
+            default:
+                EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
+                break;
+        }
+        
+        FREE(pkt);
+    }
+}
\ No newline at end of file