Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

Revision:
56:67e3d9608403
Parent:
41:6f83174ffed4
Child:
57:5444cfda9889
--- a/src/alp_helpers.cpp	Mon Jan 25 11:13:27 2021 +0100
+++ b/src/alp_helpers.cpp	Wed Jan 27 14:45:28 2021 +0000
@@ -11,13 +11,16 @@
 
 //  ====================================================================
 //  @file           alp_helpers.c
-//  @brief          ALP public helpers functions.
+//  @brief          ALP helpers functions.
 //                  This code should be disclosable in source.
 //  ====================================================================
 
 
 #include "alp.h"
+#include "alp_dbg.h"
 #include "kal_math.h"
+#include "kal_crypto.h"
+#include "d7a_1x_fs.h"
 
 //======================================================================
 // alp_size
@@ -28,7 +31,7 @@
 /// @param size     : Associated Size if relevant
 /// @retval payload size in bytes
 //======================================================================
-public uint alp_size(u8 op, u32 offset, u32 length)
+uint alp_size(u8 op, u32 offset, u32 length)
 {
     uint len=2;// At least OP+FID / OP+TAG
     switch (op)
@@ -72,7 +75,7 @@
 /// @param len      : value to be encoded
 /// @retval resulting payload size in bytes
 //======================================================================
-public u8 alp_encode_length(u8* p, u32 len)
+u8 alp_encode_length(u8* p, u32 len)
 {
     if (len <= 0x3F)
     {
@@ -112,7 +115,7 @@
 ///                   Result amongst alp_actp_substitution_mode_t
 /// @retval decoded value
 //======================================================================
-public u32 alp_decode_length(u8** p, u8* actp)
+u32 alp_decode_length(u8** p, u8* actp)
 {
     u32 tmp = 0;
     switch ((**p) & 0xC0)
@@ -153,6 +156,35 @@
     return tmp;
 }
 
+//======================================================================
+// alp_itf_size
+//----------------------------------------------------------------------
+/// @brief  Get size of the interface configuration
+/// @param  itf pointer to the configuration
+/// @return int configuration size
+//======================================================================
+int alp_itf_size(void* itf)
+{
+    alp_itf_cfg_t* p = (alp_itf_cfg_t*)itf;
+
+    switch (p->type)
+    {
+        case ALP_ITF_TYPE_D7A:
+        {
+            alp_itf_d7a_cfg_t* d7a_itf = (alp_itf_d7a_cfg_t*)itf;
+            return (1 + alp_itf_d7a_cfg_size((u8*)&d7a_itf->cfg));
+        }
+        case ALP_ITF_TYPE_COM:
+            return sizeof(alp_itf_com_cfg_t);
+        case ALP_ITF_TYPE_LWAN:
+            return sizeof(alp_itf_lwan_cfg_t);
+        default:
+            ALP_ASSERT(FALSE, "ASSERT: ALP: Unknown ITF 0x%02X for itf size\n", p->type);
+            break;
+    }
+
+    return 0;
+}
 
 //======================================================================
 // alp_parse_chunk
@@ -163,7 +195,7 @@
 /// @param resp     : pointer to alp_parsed_chunk_t structure
 /// @retval number of parsed bytes
 //======================================================================
-public int alp_parse_chunk(u8** payload, alp_parsed_chunk_t* resp) // {{{
+int alp_parse_chunk(u8** payload, alp_parsed_chunk_t* resp) // {{{
 {
     u8* p = *payload;
     u8 op = *p++;
@@ -205,7 +237,7 @@
             p += resp->meta.f_prop.length;
             break;
         case ALP_OPCODE_RSP_STATUS:
-            if (op == ALP_OPCODE_RSP_ISTATUS)
+            if (ALP_OPCODE_RSP_ISTATUS == op)
             {
                 resp->type = ALP_OPCODE_RSP_ISTATUS;
                 resp->meta.itf.type = *p++; // ITF Type
@@ -213,7 +245,7 @@
                 resp->data = p;
                 p += resp->meta.itf.length;
             }
-            else if (op == ALP_OPCODE_RSP_EOPISTATUS)
+            else if (ALP_OPCODE_RSP_EOPISTATUS == op)
             {
                 resp->type = ALP_OPCODE_RSP_EOPISTATUS;
                 resp->meta.istatus.itf = *p++; // ITF Type
@@ -244,6 +276,10 @@
         case ALP_OPCODE_F_FLUSH:
             resp->meta.f_data.fid = *p++; // FID
             break;
+        case ALP_OPCODE_RSP_SECURED:
+            resp->type = ALP_OPCODE_RSP_SECURED;
+            *payload = (u8*)NULL;
+            return 0; // XXX payload length unknown
         default:
             resp->type = ALP_OPCODE_UNKNOWN;
             *payload = (u8*)NULL;
@@ -253,3 +289,708 @@
     *payload = p;
     return bytes;
 } // }}}
+
+//======================================================================
+// alp_append
+//----------------------------------------------------------------------
+/// @brief  Appends ALP payload 2 after payload 1
+/// @param  alp_pub_payload_t*          Payload 1
+/// @param  alp_pub_payload_t*          Payload 2
+/// @return alp_pub_payload_t*          Appended payloads
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_pub_payload_t* alp_append(alp_pub_payload_t* alp_1, alp_pub_payload_t* alp_2)
+{
+    alp_pub_payload_t* alp;
+
+    if (NULL == alp_1)
+    {
+        return alp_2;
+    }
+
+    if (NULL == alp_2)
+    {
+        return alp_1;
+    }
+
+    alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1 + alp_1->len + alp_2->len);
+    alp->len = alp_1->len + alp_2->len;
+
+    memcpy(alp->d, alp_1->d, alp_1->len);
+    memcpy(&(alp->d[alp_1->len]), alp_2->d, alp_2->len);
+
+    FREE(alp_1);
+    FREE(alp_2);
+
+    return alp;
+}
+
+void alp_payload_print(alp_pub_payload_t* alp)
+{
+    if (NULL == alp)
+    {
+        return;
+    }
+
+    u32 i;
+    for (i = 0; i < alp->len; i++)
+    {
+        PRINT("%02X ", alp->d[i]);
+        FLUSH();
+    }
+    PRINT("\n");
+}
+
+//======================================================================
+// alp_payload_root_auth
+//----------------------------------------------------------------------
+/// @brief  Add the root access request to the given payload using the authentication protocol
+/// @param alp          alp_pub_payload_t*          Payload
+/// @return             alp_pub_payload_t*          Root access request + payload
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_pub_payload_t* alp_payload_root_auth(alp_pub_payload_t* alp, u8* root_key)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+    u8 hash[32];
+
+    // Calculate hash on payload
+    kal_sha256_init();
+    kal_sha256_update(alp->d, alp->len);
+    kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
+    kal_sha256_final(hash);
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_PERM_REQ_SIZE(0)
+            );
+
+    p = new_alp->d;
+    ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_AUTH_PROTOCOL_ID);
+
+    memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
+    p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
+
+    new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
+    
+    // XXX Prepend
+    return alp_append(new_alp, alp);
+}
+
+//======================================================================
+// alp_payload_root_sign
+//----------------------------------------------------------------------
+/// @brief  Sign payload with root key
+/// @param alp          alp_pub_payload_t*          Payload
+/// @param iv           u8*                         Initialization vector
+/// @return             alp_pub_payload_t*          Root access request + payload
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_pub_payload_t* alp_payload_root_sign(alp_pub_payload_t* alp, u8* root_key, u8* iv)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+    u8 hash[32];
+
+    // Calculate hash on payload
+    kal_sha256_init();
+    kal_sha256_update(alp->d, alp->len);
+    kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
+    kal_sha256_update(iv, ALP_ACTION_SECURED_IV_SIZE);
+    kal_sha256_final(hash);
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_SECURED_SIZE(0)
+            );
+
+    p = new_alp->d;
+    ALP_ACTION_SECURED(p, ALP_SECURED_ROOT, ALP_WIZZILAB_SIGN_PROTOCOL_ID);
+
+    memcpy(p, iv, ALP_ACTION_SECURED_IV_SIZE);
+    p += ALP_ACTION_SECURED_IV_SIZE;
+
+    memcpy(p, hash, ALP_ACTION_SECURED_TOKEN_SIZE);
+    p += ALP_ACTION_SECURED_TOKEN_SIZE;
+    
+    new_alp->len = ALP_ACTION_SECURED_SIZE(0);
+
+    // XXX Prepend
+    return alp_append(new_alp, alp);
+}
+
+//======================================================================
+// alp_payload_root_auth_enc
+//----------------------------------------------------------------------
+/// @brief  Add the root access request to the given payload and encrypt using the challenge protocol
+/// @param alp          alp_pub_payload_t*          Payload
+/// @param challenge    u8*                         32-byte challenge
+/// @return             alp_pub_payload_t*          Root access request + encrypted payload
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_pub_payload_t* alp_payload_root_auth_enc(alp_pub_payload_t* alp, u8* root_key, u8* challenge)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+    u8 hash[32];
+
+    // encrypt
+    u8 key[16];
+    memcpy(key, root_key, D7A_FS_ROOT_KEY_SIZE);
+    kal_xor(key, challenge, D7A_FS_ROOT_KEY_SIZE);
+    kal_aes128_init(key);
+    kal_aes128_ctr(alp->d, alp->len, challenge + D7A_FS_ROOT_KEY_SIZE, TRUE);
+
+    // Calculate hash on payload
+    kal_sha256_init();
+    kal_sha256_update(alp->d, alp->len);
+    kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
+    kal_sha256_update(challenge, 32);
+    kal_sha256_final(hash);
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_PERM_REQ_SIZE(0)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_CHAL_PROTOCOL_ID);
+    memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
+    p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
+
+    new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
+    
+    // XXX Prepend
+    return alp_append(new_alp, alp);
+}
+
+//======================================================================
+// alp_payload_rsp_f_data
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  data            void*               file content
+/// @param  offset          u32                 file offset
+/// @param  length          u32                 file length
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_rsp_f_data(alp_pub_payload_t* alp, u8 fid, void* data, u32 offset, u32 length)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_RSP_F_DATA_SIZE(offset, length)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_RSP_F_DATA(p, fid, offset, length, data);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_qbreak
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  data            void*               file content
+/// @param  offset          u32                 file offset
+/// @param  length          u32                 file length
+/// @param  group           u8                  group with next OP
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_qbreak(alp_pub_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+    u8 mask = 0;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_QBREAK_SIZE(offset, length, mask)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_QBREAK_STRTOK(p, mask, data, fid, offset, length, group);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+static alp_pub_payload_t* _alp_payload_qbreak_comp(alp_pub_payload_t* alp, u8 fid, u32 mask, u32 data, u32 offset, u32 length, u8 group, u8 comp)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_QBREAK_SIZE(offset, length, (mask)? 1 : 0)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_QBREAK_COMP(p, mask, data, comp, fid, offset, length, group);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_eq(alp_pub_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_EQ);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_ne(alp_pub_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_NE);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_gt(alp_pub_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GT);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_gte(alp_pub_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GTE);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_lt(alp_pub_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LT);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_lte(alp_pub_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LTE);
+}
+
+alp_pub_payload_t* alp_payload_qbreak_eq_msk(alp_pub_payload_t* alp, u8 fid, u32 mask, u32 data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_qbreak_comp(alp, fid, mask, data, offset, length, group, ALP_QCOMP_EQ);
+}
+
+//======================================================================
+// alp_payload_query
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  data            void*               file content
+/// @param  offset          u32                 file offset
+/// @param  length          u32                 file length
+/// @param  group           u8                  group with next OP
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_query(alp_pub_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+    u8 mask = 0;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_QUERY_SIZE(offset, length, mask)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_QUERY_STRTOK(p, mask, data, fid, offset, length, group);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_nop
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_nop(alp_pub_payload_t* alp)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_NOP_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_NOP(p, true); // XXX RESP always true. If we use NOP, it's to get a response (istatus).
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_f_wr_data
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  data            void*               file content
+/// @param  offset          u32                 file offset
+/// @param  length          u32                 file length
+/// @param  group           u8                  group with next OP
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+static alp_pub_payload_t* _alp_payload_f_wr_data_resp(alp_pub_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group, u8 resp)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_F_WR_DATA_SIZE(offset, length)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_F_WR_DATA(p, resp, fid, offset, length, data, group);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+alp_pub_payload_t* alp_payload_f_wr_data(alp_pub_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, true);
+}
+
+alp_pub_payload_t* alp_payload_f_wr_data_nr(alp_pub_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
+{
+    return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, false);
+}
+
+//======================================================================
+// alp_payload_f_rd_data
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  offset          u32                 file offset
+/// @param  length          u32                 file length
+/// @param  group           u8                  group with next OP
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_f_rd_data(alp_pub_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_F_RD_DATA_SIZE(offset, length)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_F_RD_DATA(p, true, fid, offset, length, group);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_f_flush
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  group           u8                  group with next OP
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_f_flush(alp_pub_payload_t* alp, u8 fid, u8 group)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_F_FLUSH_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_F_FLUSH(p, true, fid, group);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+
+//======================================================================
+// alp_payload_f_flush
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  fid             u8                  file identifier
+/// @param  group           u8                  group with next OP
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_f_declare(alp_pub_payload_t* alp, u8 fid, alp_file_header_t* hdr)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_F_DECLARE_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_F_DECLARE(p, true, fid, hdr, true);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_forward
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  itf             void*               Forward interface
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_forward(alp_pub_payload_t* alp, void* itf)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+    u32 itf_length = alp_itf_size(itf);
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_FORWARD_SIZE(itf_length)
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_FORWARD(p, itf, itf_length);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  tag             u8                  Tag value
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_tag(alp_pub_payload_t* alp, u8 tag)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_TAG_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_TAG(p, tag, true);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  tag             u8                  Tag value
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_rsp_tag(alp_pub_payload_t* alp, u8 tag, u8 eop, u8 err)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_RSP_TAG_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_RSP_TAG(p, tag, eop, err);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  tag             u8                  Action index
+/// @param  tag             s8                  Status
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_rsp_status(alp_pub_payload_t* alp, u8 action, s8 status)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_RSP_STATUS_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_RSP_STATUS(p, action, status);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief  Creates malloc'ed ALP payload
+/// @param  alp             alp_pub_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @param  tag             u8                  Action index
+/// @param  tag             s8                  Status
+/// @return                 alp_pub_payload_t*  New ALP payload
+/// @revent                 NONE
+//======================================================================
+alp_pub_payload_t* alp_payload_rsp_fprop(alp_pub_payload_t* alp, u8 fid, alp_file_header_t* hdr)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_RSP_F_PROP_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_RSP_F_PROP(p, fid, hdr);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+alp_pub_payload_t* alp_payload_activate_itf(alp_pub_payload_t* alp, uint8_t type, uint8_t nb_dev, uint8_t ifid, uint8_t flags, uint8_t enable)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_ACTIVATE_ITF_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_ACTIVATE_ITF(p, true, enable, type, nb_dev, ifid, flags);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+alp_pub_payload_t* alp_payload_urcc_en(alp_pub_payload_t* alp, uint8_t type, uint8_t ifid, uint8_t val)
+{
+    u8* p;
+    alp_pub_payload_t* new_alp;
+
+    new_alp = (alp_pub_payload_t*)MALLOC(sizeof(alp_pub_payload_t) - 1
+            + ALP_ACTION_ACTIVATE_ITF_SIZE
+            );
+
+    p = new_alp->d;
+
+    ALP_ACTION_URCC_EN(p, true, type, ifid, val);
+
+    new_alp->len = (u32)(p - new_alp->d);
+
+    return alp_append(alp, new_alp);
+}
+
+static int _alp_payload_parse(alp_pub_payload_t* alp, u8 op, void* buffer, u8 extract)
+{
+    if (NULL == alp)
+    {
+        return 0;
+    }
+
+    u8* p = alp->d;
+    u8* d;
+    int rem = alp->len;
+    int len;
+    alp_parsed_chunk_t r;
+
+    while (rem > 0)
+    {
+        // Parse payload
+        d = p;
+        len = alp_parse_chunk(&p, &r);
+        rem -= len;
+
+        if (r.type == op)
+        {
+            // Extract wanted payload
+            if (buffer)
+            {
+                memcpy(buffer, d, len);
+            }
+
+            if (extract)
+            {
+                // Remove parsed OP from total length
+                alp->len -= len;
+
+                // Shift end of payload to delete parsed OP
+                if (rem)
+                {
+                    memcpy(d, p, rem);
+                }
+            }
+
+            return len;
+        }
+    }
+
+    return 0;
+}
+
+int alp_payload_get(alp_pub_payload_t* alp, u8 op, void* buffer)
+{
+    return _alp_payload_parse(alp, op, buffer, false);
+}
+
+int alp_payload_extract(alp_pub_payload_t* alp, u8 op, void* buffer)
+{
+    return _alp_payload_parse(alp, op, buffer, true);
+}
\ No newline at end of file