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