Exportable version of WizziLab's modem driver.
src/alp_helpers.cpp
- Committer:
- Jeej
- Date:
- 2021-01-29
- Revision:
- 57:5444cfda9889
- Parent:
- 56:67e3d9608403
- Child:
- 59:3b38b5f499db
File content as of revision 57:5444cfda9889:
/// @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_size //---------------------------------------------------------------------- /// @brief Return payload length of most ALP operations /// @param op : ALP Action amongst alp_opcodes_t /// @param offset : Associated Offset if relevant /// @param size : Associated Size if relevant /// @retval payload size in bytes //====================================================================== uint alp_size(u8 op, u32 offset, u32 length) { uint len=2;// At least OP+FID / OP+TAG switch (op) { case ALP_OPCODE_NOP: case ALP_OPCODE_F_TOUCH: case ALP_OPCODE_F_EXIST: case ALP_OPCODE_F_DELETE: case ALP_OPCODE_F_FLUSH: case ALP_OPCODE_RSP_TAG: case ALP_OPCODE_F_RD_PROP: break; case ALP_OPCODE_RSP_STATUS: len += 1; break; case ALP_OPCODE_F_CREATE: case ALP_OPCODE_F_DECLARE: case ALP_OPCODE_RSP_F_PROP: len += ALP_FILE_HEADER_SIZE; break; case ALP_OPCODE_F_WR_PROP: case ALP_OPCODE_F_WR_DATA: case ALP_OPCODE_RSP_F_DATA: len = length; // Fallthrough case ALP_OPCODE_F_RD_DATA: len += ALP_LFIELD_SIZE(offset) + ALP_LFIELD_SIZE(length); break; default: //ALP_ASSERT(false, "ASSERT: ALP: alp_size unsupported op %d\n", op); break; } return len; } //====================================================================== // alp_encode_length //---------------------------------------------------------------------- /// @brief Encodes an ALP length/offset field /// @param p : pointer to the payload buffer /// @param len : value to be encoded /// @retval resulting payload size in bytes //====================================================================== u8 alp_encode_length(u8* p, u32 len) { if (len <= 0x3F) { *p++ = len; return 1; } else if (len <= 0x3FFF) { *p++ = 0x40 + (u8)(len >> 8); *p++ = (u8)(len & 0xFF); return 2; } else if (len <= 0x3FFFFF) { *p++ = 0x80 + (u8) (len >> 16); *p++ = (u8)((len >> 8) & 0xFF); *p++ = (u8) (len & 0xFF); return 3; } else { *p++ = 0xC0 + (u8) (len >> 24); *p++ = (u8)((len >> 16) & 0xFF); *p++ = (u8)((len >> 8) & 0xFF); *p++ = (u8) (len & 0xFF); return 4; } } //====================================================================== // alp_decode_length //---------------------------------------------------------------------- /// @brief Decodes an ALP length/offset field /// @param p : pointer to the pointer to payload buffer /// @param actp : pointer to ALP's Action Protocol Substitution flag /// Result amongst alp_actp_substitution_mode_t /// @retval decoded value //====================================================================== u32 alp_decode_length(u8** p, u8* actp) { u32 tmp = 0; switch ((**p) & 0xC0) { case 0xC0: // 0xCx xx xx xx tmp = ((*(*p)++) & 0x3F) << 24; tmp += (*(*p)++) << 16; tmp += (*(*p)++) << 8; tmp += (*(*p)++) << 0; break; case 0x80: // 0x8x xx xx : 16384 <= Len <4194303 tmp = ((*(*p)++) & 0x3F) << 16; tmp += (*(*p)++) << 8; if (tmp == 0) { // 0x8000 ActP special ActP code // Do not fetch the extra byte *actp = 2; } else { tmp += (*(*p)++) << 0; } break; case 0x40: // 0x4x xx : 64 <= Len < 16383 tmp = ((*(*p)++) & 0x3F) << 8; tmp += (*(*p)++) << 0; if (tmp == 0) { // 0x4000 ActP special ActP code *actp = 1; } break; case 0: // Len <63 tmp = (*(*p)++ & 0x3F) << 0; break; } 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 //---------------------------------------------------------------------- /// @brief Parses an ALP payload and extract a single chunk (action or /// response) to a more generic alp_parsed_chunk_t structure. /// @param payload : pointer to the pointer to payload buffer /// @param resp : pointer to alp_parsed_chunk_t structure /// @retval number of parsed bytes //====================================================================== int alp_parse_chunk(u8** payload, alp_parsed_chunk_t* resp) // {{{ { u8* p = *payload; u8 op = *p++; u8 actp; u32 bytes; resp->type = op & 0x3F; resp->data = (u8*)NULL; switch(resp->type) // XXX preemption bits squashed { case ALP_OPCODE_RSP_TAG: case ALP_OPCODE_TAG: resp->meta.tag.id = *p++; // PID resp->meta.tag.eop = !!(op & ALP_OPCODE_EOP); resp->meta.tag.err = !!(op & ALP_OPCODE_ERR); break; case ALP_OPCODE_RSP_F_DATA: case ALP_OPCODE_F_WR_DATA: resp->meta.f_data.fid = *p++; // FID resp->meta.f_data.offset = alp_decode_length(&p, &actp); // Offset resp->meta.f_data.length = alp_decode_length(&p, &actp); // Length resp->data = p; p += resp->meta.f_data.length; break; case ALP_OPCODE_F_RD_DATA: resp->meta.f_data.fid = *p++; // FID resp->meta.f_data.offset = alp_decode_length(&p, &actp); // Offset resp->meta.f_data.length = alp_decode_length(&p, &actp); // Length break; case ALP_OPCODE_F_RD_PROP: resp->meta.f_prop.fid = *p++; // FID resp->meta.f_prop.offset = 0; resp->meta.f_prop.length = ALP_FILE_HEADER_SIZE; // Hardcoded Length break; case ALP_OPCODE_RSP_F_PROP: resp->meta.f_prop.fid = *p++; // FID resp->meta.f_prop.offset = 0; resp->meta.f_prop.length = ALP_FILE_HEADER_SIZE; // Hardcoded Length resp->data = p; p += resp->meta.f_prop.length; break; case ALP_OPCODE_RSP_STATUS: if (ALP_OPCODE_RSP_ISTATUS == op) { resp->type = ALP_OPCODE_RSP_ISTATUS; resp->meta.itf.type = *p++; // ITF Type resp->meta.itf.length = alp_decode_length(&p, &actp); // Length resp->data = p; p += resp->meta.itf.length; } else if (ALP_OPCODE_RSP_EOPISTATUS == op) { resp->type = ALP_OPCODE_RSP_EOPISTATUS; resp->meta.istatus.itf = *p++; // ITF Type p++; // Length (always 1) resp->meta.istatus.err = *p++; // Interface error } else { resp->meta.status.id = *p++; // Action ID resp->meta.status.code = *p++; // status } break; case ALP_OPCODE_RSP_URC: resp->meta.urc.type = *p++; // Type resp->meta.urc.ifid = *p++; // Ifid if (resp->meta.urc.type == ALP_URC_TYPE_LQUAL) { resp->meta.urc.per = *p++; // Per } else if (resp->meta.urc.type == ALP_URC_TYPE_ITF_BUSY) { kal_ctf_t to; to.byte = *p++; // timeout resp->meta.urc.per = (to.byte == 0xff) ? MAX_U32 : kal_ctf_decode(to); } break; case ALP_OPCODE_F_DELETE: 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; return 0; } bytes = p - *payload; *payload = p; return bytes; } // }}} 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, u8 start) { static u32 action; if (NULL == alp) { return; } if (start) { action = 0; } 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, false); } void alp_payload_print(alp_payload_t* alp) { _alp_payload_print(alp, true); } 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; 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_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; 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_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; 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(); 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_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)); }