WizziLab
Exportable version of WizziLab's modem driver.
Diff: src/alp_payload.cpp
- Revision:
- 59:3b38b5f499db
- Child:
- 60:08efaaca0e83
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/src/alp_payload.cpp Wed Feb 17 11:10:41 2021 +0000
@@ -0,0 +1,845 @@
+/// @copyright
+/// ========================================================================={{{
+/// Copyright (c) 2012-2017 WizziLab /
+/// All rights reserved /
+/// /
+/// Should you have any questions regarding your right to use this Software, /
+/// contact WizziLab at www.wizzilab.com. /
+/// /
+/// =========================================================================}}}
+/// @endcopyright
+
+// ====================================================================
+// @file alp_helpers.c
+// @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"
+
+#include "WizziDebug.h"
+
+alp_payload_t* alp_payload_new(u32 size)
+{
+ alp_payload_t* alp = (alp_payload_t*)MALLOC(sizeof(alp_payload_t) - 1 + size);
+
+ alp->len = 0;
+ alp->next = NULL;
+
+ return alp;
+}
+
+void alp_payload_free(alp_payload_t* alp)
+{
+ if (NULL == alp)
+ {
+ return;
+ }
+
+ alp_payload_t* alp_next = alp->next;
+
+ FREE(alp);
+
+ alp_payload_free(alp_next);
+}
+
+//======================================================================
+// alp_payload_append
+//----------------------------------------------------------------------
+/// @brief Appends ALP payload 2 after payload 1
+/// @param alp_payload_t* Payload 1
+/// @param alp_payload_t* Payload 2
+/// @return alp_payload_t* Appended payloads
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+static void _alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
+{
+ if (NULL == alp_1->next)
+ {
+ // Append payload
+ alp_1->next = alp_2;
+
+ return;
+ }
+
+ _alp_payload_append(alp_1->next, alp_2);
+}
+
+alp_payload_t* alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
+{
+ if (NULL == alp_1)
+ {
+ return alp_2;
+ }
+
+ if (NULL == alp_2)
+ {
+ return alp_1;
+ }
+
+ _alp_payload_append(alp_1, alp_2);
+
+ return alp_1;
+}
+
+static void _alp_payload_print(alp_payload_t* alp, u32 action)
+{
+ if (NULL == alp)
+ {
+ return;
+ }
+
+ u32 i;
+ PRINT("ALP[%d]: ", action);
+ for (i = 0; i < alp->len; i++)
+ {
+ PRINT("%02X ", alp->d[i]);
+ }
+ PRINT("\n");
+ FLUSH();
+
+ _alp_payload_print(alp->next, ++action);
+}
+
+void alp_payload_print(alp_payload_t* alp)
+{
+ _alp_payload_print(alp, 0);
+}
+
+u32 alp_payload_to_buf(alp_payload_t* alp, u8* buf, u32 offset, u32 max)
+{
+ if (NULL == alp)
+ {
+ // End of payload
+ ALP_ASSERT((offset <= max), "ALP payload too big for buffer (%d\%d)\n", offset, max);
+ return offset;
+ }
+
+ if ((offset + alp->len) <= max)
+ {
+ // Copy into buffer
+ memcpy(&(buf[offset]), alp->d, alp->len);
+ }
+
+ // Next
+ return alp_payload_to_buf(alp->next, buf, offset+alp->len, max);
+}
+
+//======================================================================
+// alp_payload_root_auth
+//----------------------------------------------------------------------
+/// @brief Add the root access request to the given payload using the authentication protocol
+/// @param alp alp_payload_t* Payload
+/// @return alp_payload_t* Root access request + payload
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_payload_t* alp_payload_root_auth(alp_payload_t* alp, u8* root_key)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+ alp_payload_t* alp_hash = alp;
+ u8 hash[32];
+
+ // Calculate hash on payload
+ kal_sha256_init();
+ while (alp_hash != NULL)
+ {
+ kal_sha256_update(alp_hash->d, alp_hash->len);
+ alp_hash = alp_hash->next;
+ }
+ kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
+ kal_sha256_final(hash);
+
+ new_alp = alp_payload_new(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_payload_append(new_alp, alp);
+}
+
+//======================================================================
+// alp_payload_root_sign
+//----------------------------------------------------------------------
+/// @brief Sign payload with root key
+/// @param alp alp_payload_t* Payload
+/// @param iv u8* Initialization vector
+/// @return alp_payload_t* Root access request + payload
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_payload_t* alp_payload_root_sign(alp_payload_t* alp, u8* root_key, u8* iv)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+ alp_payload_t* alp_hash = alp;
+ u8 hash[32];
+
+ // Calculate hash on payload
+ kal_sha256_init();
+ while (alp_hash != NULL)
+ {
+ kal_sha256_update(alp_hash->d, alp_hash->len);
+ alp_hash = alp_hash->next;
+ }
+ 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_payload_new(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_payload_append(new_alp, alp);
+}
+
+#if 0
+//======================================================================
+// alp_payload_root_auth_enc
+//----------------------------------------------------------------------
+/// @brief Add the root access request to the given payload and encrypt using the challenge protocol
+/// @param alp alp_payload_t* Payload
+/// @param challenge u8* 32-byte challenge
+/// @return alp_payload_t* Root access request + encrypted payload
+/// @note: XXX payloads MUST be malloced
+//======================================================================
+alp_payload_t* alp_payload_root_auth_enc(alp_payload_t* alp, u8* root_key, u8* challenge)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+ alp_payload_t* alp_hash = alp;
+ u8 hash[32];
+
+ // encrypt
+ u8 key[D7A_FS_ROOT_KEY_SIZE];
+ 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();
+ while (alp_hash != NULL)
+ {
+ kal_sha256_update(alp_hash->d, alp_hash->len);
+ alp_hash = alp_hash->next;
+ }
+ kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
+ kal_sha256_update(challenge, 32);
+ kal_sha256_final(hash);
+
+ new_alp = alp_payload_new(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_payload_append(new_alp, alp);
+}
+#endif
+
+//======================================================================
+// alp_payload_rsp_f_data
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_rsp_f_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_qbreak
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_qbreak(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+ u8 mask = 0;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+static alp_payload_t* _alp_payload_qbreak_comp(alp_payload_t* alp, u8 fid, u32 mask, u32 data, u32 offset, u32 length, u8 group, u8 comp)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+alp_payload_t* alp_payload_qbreak_eq(alp_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_payload_t* alp_payload_qbreak_ne(alp_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_payload_t* alp_payload_qbreak_gt(alp_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_payload_t* alp_payload_qbreak_gte(alp_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_payload_t* alp_payload_qbreak_lt(alp_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_payload_t* alp_payload_qbreak_lte(alp_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_payload_t* alp_payload_qbreak_eq_msk(alp_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_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_query(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+ u8 mask = 0;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_nop
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
+/// @return alp_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_nop(alp_payload_t* alp)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_f_wr_data
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+static alp_payload_t* _alp_payload_f_wr_data_resp(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group, u8 resp)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+alp_payload_t* alp_payload_f_wr_data(alp_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_payload_t* alp_payload_f_wr_data_nr(alp_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_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_f_rd_data(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_f_flush
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_f_flush(alp_payload_t* alp, u8 fid, u8 group)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+
+//======================================================================
+// alp_payload_f_flush
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_f_declare(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_forward
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_forward(alp_payload_t* alp, void* itf)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+ u32 itf_length = alp_itf_size(itf);
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_tag(alp_payload_t* alp, u8 tag)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(ALP_ACTION_TAG_SIZE);
+
+ p = new_alp->d;
+
+ ALP_ACTION_TAG(p, tag, true);
+
+ new_alp->len = (u32)(p - new_alp->d);
+
+ return alp_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_rsp_tag(alp_payload_t* alp, u8 tag, u8 eop, u8 err)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_rsp_status(alp_payload_t* alp, u8 action, s8 status)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+//======================================================================
+// alp_payload_tag
+//----------------------------------------------------------------------
+/// @brief Creates malloc'ed ALP payload
+/// @param alp alp_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_payload_t* New ALP payload
+/// @revent NONE
+//======================================================================
+alp_payload_t* alp_payload_rsp_fprop(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+alp_payload_t* alp_payload_activate_itf(alp_payload_t* alp, uint8_t type, uint8_t nb_dev, uint8_t ifid, uint8_t flags, uint8_t enable)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+alp_payload_t* alp_payload_urcc_en(alp_payload_t* alp, uint8_t type, uint8_t ifid, uint8_t val)
+{
+ u8* p;
+ alp_payload_t* new_alp;
+
+ new_alp = alp_payload_new(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_payload_append(alp, new_alp);
+}
+
+alp_payload_t* alp_payload_extract(alp_payload_t** alp, u8 op)
+{
+ if (NULL == *alp)
+ {
+ return NULL;
+ }
+
+ if ((*alp)->d[0] == op)
+ {
+ // Save alp
+ alp_payload_t* alp_old = *alp;
+
+ // Remove it from chain
+ *alp = alp_old->next;
+ alp_old->next = NULL;
+
+ // Return it
+ return alp_old;
+ }
+
+ return alp_payload_extract((alp_payload_t**)&((*alp)->next), op);
+}
+
+alp_payload_t* alp_payload_get(alp_payload_t* alp, u8 op)
+{
+ FPRINT(": 0x%p\n", alp);
+
+ if (NULL == alp)
+ {
+ return NULL;
+ }
+
+ if (alp->d[0] == op)
+ {
+ // Return it
+ return alp;
+ }
+
+ return alp_payload_get(alp->next, op);
+}
+
+enum {
+ ERR_PRIO_EOPISTATUS,
+ ERR_PRIO_STATUS,
+ ERR_PRIO_TAG,
+ ERR_PRIO_NONE,
+};
+
+static int _alp_payload_get_err(alp_payload_t* alp, int err, u8 err_prio)
+{
+ alp_parsed_chunk_t r;
+ u8* p = alp->d;
+
+ if (NULL == alp)
+ {
+ return err;
+ }
+
+ switch (alp->d[0])
+ {
+ case ALP_OPCODE_RSP_TAG:
+ {
+ alp_parse_chunk(&p, &r);
+ if (ERR_PRIO_TAG < err_prio && r.meta.tag.err)
+ {
+ err = ALP_ERR_UNKNOWN;
+ err_prio = ERR_PRIO_TAG;
+ }
+ break;
+ }
+ case ALP_OPCODE_RSP_STATUS:
+ {
+ alp_parse_chunk(&p, &r);
+ if (ERR_PRIO_STATUS < err_prio && r.meta.status.code < ALP_ERR_NONE)
+ {
+ err = r.meta.status.code;
+ err_prio = ERR_PRIO_STATUS;
+ }
+ break;
+ }
+ case ALP_OPCODE_RSP_EOPISTATUS:
+ {
+ alp_parse_chunk(&p, &r);
+ if (ERR_PRIO_EOPISTATUS < err_prio && r.meta.istatus.err < ALP_ERR_NONE)
+ {
+ err = r.meta.istatus.err + ALP_ERR_ITF_START;
+ err_prio = ERR_PRIO_EOPISTATUS;
+ }
+ break;
+ }
+ default:
+ break;
+ }
+
+ return _alp_payload_get_err(alp->next, err, err_prio);
+}
+
+int alp_payload_get_err(alp_payload_t* alp)
+{
+ return _alp_payload_get_err(alp, ALP_ERR_NONE, ERR_PRIO_NONE);
+}
+
+alp_payload_t* alp_payload_parse(u8* p, int length)
+{
+ if (NULL == p)
+ {
+ return NULL;
+ }
+
+ if (length <= 0)
+ {
+ return NULL;
+ }
+
+ u8* d = p;
+ alp_parsed_chunk_t r;
+ int len;
+ alp_payload_t* alp;
+
+ // Parse payload
+ len = (int)alp_parse_chunk(&p, &r);
+
+ if (len <= 0)
+ {
+ return NULL;
+ }
+
+ // Malloc payload
+ alp = alp_payload_new(len);
+
+ // Fill payload
+ alp->len = len;
+ memcpy(alp->d, d, len);
+
+ // parse next
+ return alp_payload_append(alp, alp_payload_parse(p, length - len));
+}
\ No newline at end of file