WizziLab
Exportable version of WizziLab's modem driver.
Diff: src/alp_helpers.cpp
- 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