WizziLab
Exportable version of WizziLab's modem driver.
src/alp_helpers.cpp
- Committer:
- Jeej
- Date:
- 2021-01-27
- Revision:
- 56:67e3d9608403
- Parent:
- 41:6f83174ffed4
- Child:
- 57:5444cfda9889
File content as of revision 56:67e3d9608403:
/// @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"
//======================================================================
// 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_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);
}