WizziLab / modem_ref

Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

src/alp_helpers.cpp@57:5444cfda9889, 2021-01-29 (annotated)

Committer:
Jeej
Date:
Fri Jan 29 16:36:32 2021 +0000
Revision:
57:5444cfda9889
Parent:
56:67e3d9608403
Child:
59:3b38b5f499db
Sanity commit. Reworked driver version. Partially functional. To be tested with all demo applications.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jeej 0:027760f45e2c 1 /// @copyright
Jeej 0:027760f45e2c 2 /// ========================================================================={{{
Jeej 0:027760f45e2c 3 /// Copyright (c) 2012-2017 WizziLab /
Jeej 0:027760f45e2c 4 /// All rights reserved /
Jeej 0:027760f45e2c 5 /// /
Jeej 0:027760f45e2c 6 /// Should you have any questions regarding your right to use this Software, /
Jeej 0:027760f45e2c 7 /// contact WizziLab at www.wizzilab.com. /
Jeej 0:027760f45e2c 8 /// /
Jeej 0:027760f45e2c 9 /// =========================================================================}}}
Jeej 0:027760f45e2c 10 /// @endcopyright
Jeej 0:027760f45e2c 11
Jeej 0:027760f45e2c 12 // ====================================================================
Jeej 0:027760f45e2c 13 // @file alp_helpers.c
Jeej 56:67e3d9608403 14 // @brief ALP helpers functions.
Jeej 0:027760f45e2c 15 // This code should be disclosable in source.
Jeej 0:027760f45e2c 16 // ====================================================================
Jeej 0:027760f45e2c 17
Jeej 0:027760f45e2c 18
Jeej 0:027760f45e2c 19 #include "alp.h"
Jeej 56:67e3d9608403 20 #include "alp_dbg.h"
Jeej 41:6f83174ffed4 21 #include "kal_math.h"
Jeej 56:67e3d9608403 22 #include "kal_crypto.h"
Jeej 56:67e3d9608403 23 #include "d7a_1x_fs.h"
Jeej 0:027760f45e2c 24
Jeej 57:5444cfda9889 25 #include "WizziDebug.h"
Jeej 57:5444cfda9889 26
Jeej 0:027760f45e2c 27 //======================================================================
Jeej 0:027760f45e2c 28 // alp_size
Jeej 0:027760f45e2c 29 //----------------------------------------------------------------------
Jeej 0:027760f45e2c 30 /// @brief Return payload length of most ALP operations
Jeej 0:027760f45e2c 31 /// @param op : ALP Action amongst alp_opcodes_t
Jeej 0:027760f45e2c 32 /// @param offset : Associated Offset if relevant
Jeej 0:027760f45e2c 33 /// @param size : Associated Size if relevant
Jeej 0:027760f45e2c 34 /// @retval payload size in bytes
Jeej 0:027760f45e2c 35 //======================================================================
Jeej 56:67e3d9608403 36 uint alp_size(u8 op, u32 offset, u32 length)
Jeej 0:027760f45e2c 37 {
Jeej 0:027760f45e2c 38 uint len=2;// At least OP+FID / OP+TAG
Jeej 0:027760f45e2c 39 switch (op)
Jeej 0:027760f45e2c 40 {
Jeej 0:027760f45e2c 41 case ALP_OPCODE_NOP:
Jeej 0:027760f45e2c 42 case ALP_OPCODE_F_TOUCH:
Jeej 0:027760f45e2c 43 case ALP_OPCODE_F_EXIST:
Jeej 0:027760f45e2c 44 case ALP_OPCODE_F_DELETE:
Jeej 0:027760f45e2c 45 case ALP_OPCODE_F_FLUSH:
Jeej 0:027760f45e2c 46 case ALP_OPCODE_RSP_TAG:
Jeej 0:027760f45e2c 47 case ALP_OPCODE_F_RD_PROP:
Jeej 0:027760f45e2c 48 break;
Jeej 0:027760f45e2c 49 case ALP_OPCODE_RSP_STATUS:
Jeej 0:027760f45e2c 50 len += 1;
Jeej 0:027760f45e2c 51 break;
Jeej 0:027760f45e2c 52 case ALP_OPCODE_F_CREATE:
Jeej 0:027760f45e2c 53 case ALP_OPCODE_F_DECLARE:
Jeej 0:027760f45e2c 54 case ALP_OPCODE_RSP_F_PROP:
Jeej 0:027760f45e2c 55 len += ALP_FILE_HEADER_SIZE;
Jeej 0:027760f45e2c 56 break;
Jeej 0:027760f45e2c 57 case ALP_OPCODE_F_WR_PROP:
Jeej 0:027760f45e2c 58 case ALP_OPCODE_F_WR_DATA:
Jeej 0:027760f45e2c 59 case ALP_OPCODE_RSP_F_DATA:
Jeej 0:027760f45e2c 60 len = length;
Jeej 35:ac940cf8ebe6 61 // Fallthrough
Jeej 0:027760f45e2c 62 case ALP_OPCODE_F_RD_DATA:
Jeej 0:027760f45e2c 63 len += ALP_LFIELD_SIZE(offset) + ALP_LFIELD_SIZE(length);
Jeej 0:027760f45e2c 64 break;
Jeej 0:027760f45e2c 65 default:
Jeej 57:5444cfda9889 66 //ALP_ASSERT(false, "ASSERT: ALP: alp_size unsupported op %d\n", op);
Jeej 0:027760f45e2c 67 break;
Jeej 0:027760f45e2c 68 }
Jeej 0:027760f45e2c 69 return len;
Jeej 0:027760f45e2c 70 }
Jeej 0:027760f45e2c 71
Jeej 0:027760f45e2c 72 //======================================================================
Jeej 0:027760f45e2c 73 // alp_encode_length
Jeej 0:027760f45e2c 74 //----------------------------------------------------------------------
Jeej 0:027760f45e2c 75 /// @brief Encodes an ALP length/offset field
Jeej 0:027760f45e2c 76 /// @param p : pointer to the payload buffer
Jeej 0:027760f45e2c 77 /// @param len : value to be encoded
Jeej 0:027760f45e2c 78 /// @retval resulting payload size in bytes
Jeej 0:027760f45e2c 79 //======================================================================
Jeej 56:67e3d9608403 80 u8 alp_encode_length(u8* p, u32 len)
Jeej 0:027760f45e2c 81 {
Jeej 0:027760f45e2c 82 if (len <= 0x3F)
Jeej 0:027760f45e2c 83 {
Jeej 0:027760f45e2c 84 *p++ = len;
Jeej 0:027760f45e2c 85 return 1;
Jeej 0:027760f45e2c 86 }
Jeej 0:027760f45e2c 87 else if (len <= 0x3FFF)
Jeej 0:027760f45e2c 88 {
Jeej 0:027760f45e2c 89 *p++ = 0x40 + (u8)(len >> 8);
Jeej 0:027760f45e2c 90 *p++ = (u8)(len & 0xFF);
Jeej 0:027760f45e2c 91 return 2;
Jeej 0:027760f45e2c 92 }
Jeej 0:027760f45e2c 93 else if (len <= 0x3FFFFF)
Jeej 0:027760f45e2c 94 {
Jeej 0:027760f45e2c 95 *p++ = 0x80 + (u8) (len >> 16);
Jeej 0:027760f45e2c 96 *p++ = (u8)((len >> 8) & 0xFF);
Jeej 0:027760f45e2c 97 *p++ = (u8) (len & 0xFF);
Jeej 0:027760f45e2c 98 return 3;
Jeej 0:027760f45e2c 99 }
Jeej 0:027760f45e2c 100 else
Jeej 0:027760f45e2c 101 {
Jeej 0:027760f45e2c 102 *p++ = 0xC0 + (u8) (len >> 24);
Jeej 0:027760f45e2c 103 *p++ = (u8)((len >> 16) & 0xFF);
Jeej 0:027760f45e2c 104 *p++ = (u8)((len >> 8) & 0xFF);
Jeej 0:027760f45e2c 105 *p++ = (u8) (len & 0xFF);
Jeej 0:027760f45e2c 106 return 4;
Jeej 0:027760f45e2c 107 }
Jeej 0:027760f45e2c 108 }
Jeej 0:027760f45e2c 109
Jeej 0:027760f45e2c 110
Jeej 0:027760f45e2c 111 //======================================================================
Jeej 0:027760f45e2c 112 // alp_decode_length
Jeej 0:027760f45e2c 113 //----------------------------------------------------------------------
Jeej 0:027760f45e2c 114 /// @brief Decodes an ALP length/offset field
Jeej 0:027760f45e2c 115 /// @param p : pointer to the pointer to payload buffer
Jeej 0:027760f45e2c 116 /// @param actp : pointer to ALP's Action Protocol Substitution flag
Jeej 0:027760f45e2c 117 /// Result amongst alp_actp_substitution_mode_t
Jeej 0:027760f45e2c 118 /// @retval decoded value
Jeej 0:027760f45e2c 119 //======================================================================
Jeej 56:67e3d9608403 120 u32 alp_decode_length(u8** p, u8* actp)
Jeej 0:027760f45e2c 121 {
Jeej 0:027760f45e2c 122 u32 tmp = 0;
Jeej 0:027760f45e2c 123 switch ((**p) & 0xC0)
Jeej 0:027760f45e2c 124 {
Jeej 0:027760f45e2c 125 case 0xC0: // 0xCx xx xx xx
Jeej 0:027760f45e2c 126 tmp = ((*(*p)++) & 0x3F) << 24;
Jeej 0:027760f45e2c 127 tmp += (*(*p)++) << 16;
Jeej 0:027760f45e2c 128 tmp += (*(*p)++) << 8;
Jeej 0:027760f45e2c 129 tmp += (*(*p)++) << 0;
Jeej 0:027760f45e2c 130 break;
Jeej 0:027760f45e2c 131 case 0x80: // 0x8x xx xx : 16384 <= Len <4194303
Jeej 0:027760f45e2c 132 tmp = ((*(*p)++) & 0x3F) << 16;
Jeej 0:027760f45e2c 133 tmp += (*(*p)++) << 8;
Jeej 0:027760f45e2c 134 if (tmp == 0)
Jeej 0:027760f45e2c 135 {
Jeej 0:027760f45e2c 136 // 0x8000 ActP special ActP code
Jeej 0:027760f45e2c 137 // Do not fetch the extra byte
Jeej 0:027760f45e2c 138 *actp = 2;
Jeej 0:027760f45e2c 139 }
Jeej 0:027760f45e2c 140 else
Jeej 0:027760f45e2c 141 {
Jeej 0:027760f45e2c 142 tmp += (*(*p)++) << 0;
Jeej 0:027760f45e2c 143 }
Jeej 0:027760f45e2c 144 break;
Jeej 0:027760f45e2c 145 case 0x40: // 0x4x xx : 64 <= Len < 16383
Jeej 0:027760f45e2c 146 tmp = ((*(*p)++) & 0x3F) << 8;
Jeej 0:027760f45e2c 147 tmp += (*(*p)++) << 0;
Jeej 0:027760f45e2c 148 if (tmp == 0)
Jeej 0:027760f45e2c 149 {
Jeej 0:027760f45e2c 150 // 0x4000 ActP special ActP code
Jeej 0:027760f45e2c 151 *actp = 1;
Jeej 0:027760f45e2c 152 }
Jeej 0:027760f45e2c 153 break;
Jeej 0:027760f45e2c 154 case 0: // Len <63
Jeej 0:027760f45e2c 155 tmp = (*(*p)++ & 0x3F) << 0;
Jeej 0:027760f45e2c 156 break;
Jeej 0:027760f45e2c 157 }
Jeej 0:027760f45e2c 158 return tmp;
Jeej 0:027760f45e2c 159 }
Jeej 0:027760f45e2c 160
Jeej 56:67e3d9608403 161 //======================================================================
Jeej 56:67e3d9608403 162 // alp_itf_size
Jeej 56:67e3d9608403 163 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 164 /// @brief Get size of the interface configuration
Jeej 56:67e3d9608403 165 /// @param itf pointer to the configuration
Jeej 56:67e3d9608403 166 /// @return int configuration size
Jeej 56:67e3d9608403 167 //======================================================================
Jeej 56:67e3d9608403 168 int alp_itf_size(void* itf)
Jeej 56:67e3d9608403 169 {
Jeej 56:67e3d9608403 170 alp_itf_cfg_t* p = (alp_itf_cfg_t*)itf;
Jeej 56:67e3d9608403 171
Jeej 56:67e3d9608403 172 switch (p->type)
Jeej 56:67e3d9608403 173 {
Jeej 56:67e3d9608403 174 case ALP_ITF_TYPE_D7A:
Jeej 57:5444cfda9889 175 {
Jeej 57:5444cfda9889 176 alp_itf_d7a_cfg_t* d7a_itf = (alp_itf_d7a_cfg_t*)itf;
Jeej 57:5444cfda9889 177 return (1 + alp_itf_d7a_cfg_size((u8*)&d7a_itf->cfg));
Jeej 57:5444cfda9889 178 }
Jeej 56:67e3d9608403 179 case ALP_ITF_TYPE_COM:
Jeej 56:67e3d9608403 180 return sizeof(alp_itf_com_cfg_t);
Jeej 56:67e3d9608403 181 case ALP_ITF_TYPE_LWAN:
Jeej 56:67e3d9608403 182 return sizeof(alp_itf_lwan_cfg_t);
Jeej 56:67e3d9608403 183 default:
Jeej 56:67e3d9608403 184 ALP_ASSERT(FALSE, "ASSERT: ALP: Unknown ITF 0x%02X for itf size\n", p->type);
Jeej 56:67e3d9608403 185 break;
Jeej 56:67e3d9608403 186 }
Jeej 56:67e3d9608403 187
Jeej 56:67e3d9608403 188 return 0;
Jeej 56:67e3d9608403 189 }
Jeej 0:027760f45e2c 190
Jeej 0:027760f45e2c 191 //======================================================================
Jeej 0:027760f45e2c 192 // alp_parse_chunk
Jeej 0:027760f45e2c 193 //----------------------------------------------------------------------
Jeej 0:027760f45e2c 194 /// @brief Parses an ALP payload and extract a single chunk (action or
Jeej 0:027760f45e2c 195 /// response) to a more generic alp_parsed_chunk_t structure.
Jeej 0:027760f45e2c 196 /// @param payload : pointer to the pointer to payload buffer
Jeej 0:027760f45e2c 197 /// @param resp : pointer to alp_parsed_chunk_t structure
Jeej 0:027760f45e2c 198 /// @retval number of parsed bytes
Jeej 0:027760f45e2c 199 //======================================================================
Jeej 56:67e3d9608403 200 int alp_parse_chunk(u8** payload, alp_parsed_chunk_t* resp) // {{{
Jeej 0:027760f45e2c 201 {
Jeej 0:027760f45e2c 202 u8* p = *payload;
Jeej 0:027760f45e2c 203 u8 op = *p++;
Jeej 0:027760f45e2c 204 u8 actp;
Jeej 0:027760f45e2c 205 u32 bytes;
Jeej 0:027760f45e2c 206 resp->type = op & 0x3F;
Jeej 0:027760f45e2c 207 resp->data = (u8*)NULL;
Jeej 0:027760f45e2c 208 switch(resp->type) // XXX preemption bits squashed
Jeej 0:027760f45e2c 209 {
Jeej 0:027760f45e2c 210 case ALP_OPCODE_RSP_TAG:
Jeej 0:027760f45e2c 211 case ALP_OPCODE_TAG:
Jeej 0:027760f45e2c 212 resp->meta.tag.id = *p++; // PID
Jeej 0:027760f45e2c 213 resp->meta.tag.eop = !!(op & ALP_OPCODE_EOP);
Jeej 0:027760f45e2c 214 resp->meta.tag.err = !!(op & ALP_OPCODE_ERR);
Jeej 0:027760f45e2c 215 break;
Jeej 0:027760f45e2c 216 case ALP_OPCODE_RSP_F_DATA:
Jeej 0:027760f45e2c 217 case ALP_OPCODE_F_WR_DATA:
Jeej 0:027760f45e2c 218 resp->meta.f_data.fid = *p++; // FID
Jeej 57:5444cfda9889 219 resp->meta.f_data.offset = alp_decode_length(&p, &actp); // Offset
Jeej 57:5444cfda9889 220 resp->meta.f_data.length = alp_decode_length(&p, &actp); // Length
Jeej 0:027760f45e2c 221 resp->data = p;
Jeej 0:027760f45e2c 222 p += resp->meta.f_data.length;
Jeej 0:027760f45e2c 223 break;
Jeej 0:027760f45e2c 224 case ALP_OPCODE_F_RD_DATA:
Jeej 0:027760f45e2c 225 resp->meta.f_data.fid = *p++; // FID
Jeej 57:5444cfda9889 226 resp->meta.f_data.offset = alp_decode_length(&p, &actp); // Offset
Jeej 57:5444cfda9889 227 resp->meta.f_data.length = alp_decode_length(&p, &actp); // Length
Jeej 0:027760f45e2c 228 break;
Jeej 0:027760f45e2c 229 case ALP_OPCODE_F_RD_PROP:
Jeej 0:027760f45e2c 230 resp->meta.f_prop.fid = *p++; // FID
Jeej 0:027760f45e2c 231 resp->meta.f_prop.offset = 0;
Jeej 0:027760f45e2c 232 resp->meta.f_prop.length = ALP_FILE_HEADER_SIZE; // Hardcoded Length
Jeej 0:027760f45e2c 233 break;
Jeej 0:027760f45e2c 234 case ALP_OPCODE_RSP_F_PROP:
Jeej 0:027760f45e2c 235 resp->meta.f_prop.fid = *p++; // FID
Jeej 0:027760f45e2c 236 resp->meta.f_prop.offset = 0;
Jeej 0:027760f45e2c 237 resp->meta.f_prop.length = ALP_FILE_HEADER_SIZE; // Hardcoded Length
Jeej 0:027760f45e2c 238 resp->data = p;
Jeej 0:027760f45e2c 239 p += resp->meta.f_prop.length;
Jeej 0:027760f45e2c 240 break;
Jeej 0:027760f45e2c 241 case ALP_OPCODE_RSP_STATUS:
Jeej 56:67e3d9608403 242 if (ALP_OPCODE_RSP_ISTATUS == op)
Jeej 0:027760f45e2c 243 {
Jeej 0:027760f45e2c 244 resp->type = ALP_OPCODE_RSP_ISTATUS;
Jeej 0:027760f45e2c 245 resp->meta.itf.type = *p++; // ITF Type
Jeej 57:5444cfda9889 246 resp->meta.itf.length = alp_decode_length(&p, &actp); // Length
Jeej 0:027760f45e2c 247 resp->data = p;
Jeej 0:027760f45e2c 248 p += resp->meta.itf.length;
Jeej 0:027760f45e2c 249 }
Jeej 56:67e3d9608403 250 else if (ALP_OPCODE_RSP_EOPISTATUS == op)
Jeej 37:f5424d109c6d 251 {
Jeej 37:f5424d109c6d 252 resp->type = ALP_OPCODE_RSP_EOPISTATUS;
Jeej 37:f5424d109c6d 253 resp->meta.istatus.itf = *p++; // ITF Type
Jeej 37:f5424d109c6d 254 p++; // Length (always 1)
Jeej 37:f5424d109c6d 255 resp->meta.istatus.err = *p++; // Interface error
Jeej 37:f5424d109c6d 256 }
Jeej 0:027760f45e2c 257 else
Jeej 0:027760f45e2c 258 {
Jeej 0:027760f45e2c 259 resp->meta.status.id = *p++; // Action ID
Jeej 0:027760f45e2c 260 resp->meta.status.code = *p++; // status
Jeej 0:027760f45e2c 261 }
Jeej 0:027760f45e2c 262 break;
Jeej 0:027760f45e2c 263 case ALP_OPCODE_RSP_URC:
Jeej 0:027760f45e2c 264 resp->meta.urc.type = *p++; // Type
Jeej 0:027760f45e2c 265 resp->meta.urc.ifid = *p++; // Ifid
Jeej 0:027760f45e2c 266 if (resp->meta.urc.type == ALP_URC_TYPE_LQUAL)
Jeej 41:6f83174ffed4 267 {
Jeej 0:027760f45e2c 268 resp->meta.urc.per = *p++; // Per
Jeej 41:6f83174ffed4 269 }
Jeej 41:6f83174ffed4 270 else if (resp->meta.urc.type == ALP_URC_TYPE_ITF_BUSY)
Jeej 41:6f83174ffed4 271 {
Jeej 41:6f83174ffed4 272 kal_ctf_t to;
Jeej 41:6f83174ffed4 273 to.byte = *p++; // timeout
Jeej 41:6f83174ffed4 274 resp->meta.urc.per = (to.byte == 0xff) ? MAX_U32 : kal_ctf_decode(to);
Jeej 41:6f83174ffed4 275 }
Jeej 0:027760f45e2c 276 break;
Jeej 0:027760f45e2c 277 case ALP_OPCODE_F_DELETE:
Jeej 0:027760f45e2c 278 case ALP_OPCODE_F_FLUSH:
Jeej 0:027760f45e2c 279 resp->meta.f_data.fid = *p++; // FID
Jeej 0:027760f45e2c 280 break;
Jeej 56:67e3d9608403 281 case ALP_OPCODE_RSP_SECURED:
Jeej 56:67e3d9608403 282 resp->type = ALP_OPCODE_RSP_SECURED;
Jeej 56:67e3d9608403 283 *payload = (u8*)NULL;
Jeej 56:67e3d9608403 284 return 0; // XXX payload length unknown
Jeej 0:027760f45e2c 285 default:
Jeej 0:027760f45e2c 286 resp->type = ALP_OPCODE_UNKNOWN;
Jeej 0:027760f45e2c 287 *payload = (u8*)NULL;
Jeej 0:027760f45e2c 288 return 0;
Jeej 0:027760f45e2c 289 }
Jeej 0:027760f45e2c 290 bytes = p - *payload;
Jeej 0:027760f45e2c 291 *payload = p;
Jeej 0:027760f45e2c 292 return bytes;
Jeej 0:027760f45e2c 293 } // }}}
Jeej 56:67e3d9608403 294
Jeej 57:5444cfda9889 295
Jeej 57:5444cfda9889 296 alp_payload_t* alp_payload_new(u32 size)
Jeej 57:5444cfda9889 297 {
Jeej 57:5444cfda9889 298 alp_payload_t* alp = (alp_payload_t*)MALLOC(sizeof(alp_payload_t) - 1 + size);
Jeej 57:5444cfda9889 299
Jeej 57:5444cfda9889 300 alp->len = 0;
Jeej 57:5444cfda9889 301 alp->next = NULL;
Jeej 57:5444cfda9889 302
Jeej 57:5444cfda9889 303 return alp;
Jeej 57:5444cfda9889 304 }
Jeej 57:5444cfda9889 305
Jeej 57:5444cfda9889 306 void alp_payload_free(alp_payload_t* alp)
Jeej 57:5444cfda9889 307 {
Jeej 57:5444cfda9889 308 if (NULL == alp)
Jeej 57:5444cfda9889 309 {
Jeej 57:5444cfda9889 310 return;
Jeej 57:5444cfda9889 311 }
Jeej 57:5444cfda9889 312
Jeej 57:5444cfda9889 313 alp_payload_t* alp_next = alp->next;
Jeej 57:5444cfda9889 314
Jeej 57:5444cfda9889 315 FREE(alp);
Jeej 57:5444cfda9889 316
Jeej 57:5444cfda9889 317 alp_payload_free(alp_next);
Jeej 57:5444cfda9889 318 }
Jeej 57:5444cfda9889 319
Jeej 56:67e3d9608403 320 //======================================================================
Jeej 57:5444cfda9889 321 // alp_payload_append
Jeej 56:67e3d9608403 322 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 323 /// @brief Appends ALP payload 2 after payload 1
Jeej 57:5444cfda9889 324 /// @param alp_payload_t* Payload 1
Jeej 57:5444cfda9889 325 /// @param alp_payload_t* Payload 2
Jeej 57:5444cfda9889 326 /// @return alp_payload_t* Appended payloads
Jeej 56:67e3d9608403 327 /// @note: XXX payloads MUST be malloced
Jeej 56:67e3d9608403 328 //======================================================================
Jeej 57:5444cfda9889 329 static void _alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
Jeej 56:67e3d9608403 330 {
Jeej 57:5444cfda9889 331 if (NULL == alp_1->next)
Jeej 57:5444cfda9889 332 {
Jeej 57:5444cfda9889 333 // Append payload
Jeej 57:5444cfda9889 334 alp_1->next = alp_2;
Jeej 56:67e3d9608403 335
Jeej 57:5444cfda9889 336 return;
Jeej 57:5444cfda9889 337 }
Jeej 57:5444cfda9889 338
Jeej 57:5444cfda9889 339 alp_payload_append(alp_1->next, alp_2);
Jeej 57:5444cfda9889 340 }
Jeej 57:5444cfda9889 341
Jeej 57:5444cfda9889 342 alp_payload_t* alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
Jeej 57:5444cfda9889 343 {
Jeej 56:67e3d9608403 344 if (NULL == alp_1)
Jeej 56:67e3d9608403 345 {
Jeej 56:67e3d9608403 346 return alp_2;
Jeej 56:67e3d9608403 347 }
Jeej 56:67e3d9608403 348
Jeej 56:67e3d9608403 349 if (NULL == alp_2)
Jeej 56:67e3d9608403 350 {
Jeej 56:67e3d9608403 351 return alp_1;
Jeej 56:67e3d9608403 352 }
Jeej 56:67e3d9608403 353
Jeej 57:5444cfda9889 354 _alp_payload_append(alp_1, alp_2);
Jeej 57:5444cfda9889 355
Jeej 57:5444cfda9889 356 return alp_1;
Jeej 56:67e3d9608403 357 }
Jeej 56:67e3d9608403 358
Jeej 57:5444cfda9889 359 static void _alp_payload_print(alp_payload_t* alp, u8 start)
Jeej 56:67e3d9608403 360 {
Jeej 57:5444cfda9889 361 static u32 action;
Jeej 57:5444cfda9889 362
Jeej 56:67e3d9608403 363 if (NULL == alp)
Jeej 56:67e3d9608403 364 {
Jeej 56:67e3d9608403 365 return;
Jeej 56:67e3d9608403 366 }
Jeej 56:67e3d9608403 367
Jeej 57:5444cfda9889 368 if (start)
Jeej 57:5444cfda9889 369 {
Jeej 57:5444cfda9889 370 action = 0;
Jeej 57:5444cfda9889 371 }
Jeej 57:5444cfda9889 372
Jeej 56:67e3d9608403 373 u32 i;
Jeej 57:5444cfda9889 374 PRINT("ALP[%d]: ", action++);
Jeej 56:67e3d9608403 375 for (i = 0; i < alp->len; i++)
Jeej 56:67e3d9608403 376 {
Jeej 56:67e3d9608403 377 PRINT("%02X ", alp->d[i]);
Jeej 56:67e3d9608403 378 }
Jeej 56:67e3d9608403 379 PRINT("\n");
Jeej 57:5444cfda9889 380 FLUSH();
Jeej 57:5444cfda9889 381
Jeej 57:5444cfda9889 382 _alp_payload_print(alp->next, false);
Jeej 57:5444cfda9889 383 }
Jeej 57:5444cfda9889 384
Jeej 57:5444cfda9889 385 void alp_payload_print(alp_payload_t* alp)
Jeej 57:5444cfda9889 386 {
Jeej 57:5444cfda9889 387 _alp_payload_print(alp, true);
Jeej 57:5444cfda9889 388 }
Jeej 57:5444cfda9889 389
Jeej 57:5444cfda9889 390
Jeej 57:5444cfda9889 391 u32 alp_payload_to_buf(alp_payload_t* alp, u8* buf, u32 offset, u32 max)
Jeej 57:5444cfda9889 392 {
Jeej 57:5444cfda9889 393 if (NULL == alp)
Jeej 57:5444cfda9889 394 {
Jeej 57:5444cfda9889 395 // End of payload
Jeej 57:5444cfda9889 396 ALP_ASSERT((offset <= max), "ALP payload too big for buffer (%d\%d)\n", offset, max);
Jeej 57:5444cfda9889 397 return offset;
Jeej 57:5444cfda9889 398 }
Jeej 57:5444cfda9889 399
Jeej 57:5444cfda9889 400 if ((offset + alp->len) <= max)
Jeej 57:5444cfda9889 401 {
Jeej 57:5444cfda9889 402 // Copy into buffer
Jeej 57:5444cfda9889 403 memcpy(&(buf[offset]), alp->d, alp->len);
Jeej 57:5444cfda9889 404 }
Jeej 57:5444cfda9889 405
Jeej 57:5444cfda9889 406 // Next
Jeej 57:5444cfda9889 407 return alp_payload_to_buf(alp->next, buf, offset+alp->len, max);
Jeej 56:67e3d9608403 408 }
Jeej 56:67e3d9608403 409
Jeej 56:67e3d9608403 410 //======================================================================
Jeej 56:67e3d9608403 411 // alp_payload_root_auth
Jeej 56:67e3d9608403 412 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 413 /// @brief Add the root access request to the given payload using the authentication protocol
Jeej 57:5444cfda9889 414 /// @param alp alp_payload_t* Payload
Jeej 57:5444cfda9889 415 /// @return alp_payload_t* Root access request + payload
Jeej 56:67e3d9608403 416 /// @note: XXX payloads MUST be malloced
Jeej 56:67e3d9608403 417 //======================================================================
Jeej 57:5444cfda9889 418 alp_payload_t* alp_payload_root_auth(alp_payload_t* alp, u8* root_key)
Jeej 56:67e3d9608403 419 {
Jeej 56:67e3d9608403 420 u8* p;
Jeej 57:5444cfda9889 421 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 422 u8 hash[32];
Jeej 56:67e3d9608403 423
Jeej 56:67e3d9608403 424 // Calculate hash on payload
Jeej 56:67e3d9608403 425 kal_sha256_init();
Jeej 56:67e3d9608403 426 kal_sha256_update(alp->d, alp->len);
Jeej 56:67e3d9608403 427 kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 56:67e3d9608403 428 kal_sha256_final(hash);
Jeej 56:67e3d9608403 429
Jeej 57:5444cfda9889 430 new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));
Jeej 56:67e3d9608403 431
Jeej 56:67e3d9608403 432 p = new_alp->d;
Jeej 56:67e3d9608403 433 ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_AUTH_PROTOCOL_ID);
Jeej 56:67e3d9608403 434
Jeej 56:67e3d9608403 435 memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
Jeej 56:67e3d9608403 436 p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
Jeej 56:67e3d9608403 437
Jeej 56:67e3d9608403 438 new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
Jeej 57:5444cfda9889 439
Jeej 56:67e3d9608403 440 // XXX Prepend
Jeej 57:5444cfda9889 441 return alp_payload_append(new_alp, alp);
Jeej 56:67e3d9608403 442 }
Jeej 56:67e3d9608403 443
Jeej 56:67e3d9608403 444 //======================================================================
Jeej 56:67e3d9608403 445 // alp_payload_root_sign
Jeej 56:67e3d9608403 446 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 447 /// @brief Sign payload with root key
Jeej 57:5444cfda9889 448 /// @param alp alp_payload_t* Payload
Jeej 56:67e3d9608403 449 /// @param iv u8* Initialization vector
Jeej 57:5444cfda9889 450 /// @return alp_payload_t* Root access request + payload
Jeej 56:67e3d9608403 451 /// @note: XXX payloads MUST be malloced
Jeej 56:67e3d9608403 452 //======================================================================
Jeej 57:5444cfda9889 453 alp_payload_t* alp_payload_root_sign(alp_payload_t* alp, u8* root_key, u8* iv)
Jeej 56:67e3d9608403 454 {
Jeej 56:67e3d9608403 455 u8* p;
Jeej 57:5444cfda9889 456 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 457 u8 hash[32];
Jeej 56:67e3d9608403 458
Jeej 56:67e3d9608403 459 // Calculate hash on payload
Jeej 56:67e3d9608403 460 kal_sha256_init();
Jeej 56:67e3d9608403 461 kal_sha256_update(alp->d, alp->len);
Jeej 56:67e3d9608403 462 kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 56:67e3d9608403 463 kal_sha256_update(iv, ALP_ACTION_SECURED_IV_SIZE);
Jeej 56:67e3d9608403 464 kal_sha256_final(hash);
Jeej 56:67e3d9608403 465
Jeej 57:5444cfda9889 466 new_alp = alp_payload_new(ALP_ACTION_SECURED_SIZE(0));
Jeej 56:67e3d9608403 467
Jeej 56:67e3d9608403 468 p = new_alp->d;
Jeej 56:67e3d9608403 469 ALP_ACTION_SECURED(p, ALP_SECURED_ROOT, ALP_WIZZILAB_SIGN_PROTOCOL_ID);
Jeej 56:67e3d9608403 470
Jeej 56:67e3d9608403 471 memcpy(p, iv, ALP_ACTION_SECURED_IV_SIZE);
Jeej 56:67e3d9608403 472 p += ALP_ACTION_SECURED_IV_SIZE;
Jeej 56:67e3d9608403 473
Jeej 56:67e3d9608403 474 memcpy(p, hash, ALP_ACTION_SECURED_TOKEN_SIZE);
Jeej 56:67e3d9608403 475 p += ALP_ACTION_SECURED_TOKEN_SIZE;
Jeej 57:5444cfda9889 476
Jeej 56:67e3d9608403 477 new_alp->len = ALP_ACTION_SECURED_SIZE(0);
Jeej 56:67e3d9608403 478
Jeej 56:67e3d9608403 479 // XXX Prepend
Jeej 57:5444cfda9889 480 return alp_payload_append(new_alp, alp);
Jeej 56:67e3d9608403 481 }
Jeej 56:67e3d9608403 482
Jeej 57:5444cfda9889 483 #if 0
Jeej 56:67e3d9608403 484 //======================================================================
Jeej 56:67e3d9608403 485 // alp_payload_root_auth_enc
Jeej 56:67e3d9608403 486 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 487 /// @brief Add the root access request to the given payload and encrypt using the challenge protocol
Jeej 57:5444cfda9889 488 /// @param alp alp_payload_t* Payload
Jeej 56:67e3d9608403 489 /// @param challenge u8* 32-byte challenge
Jeej 57:5444cfda9889 490 /// @return alp_payload_t* Root access request + encrypted payload
Jeej 56:67e3d9608403 491 /// @note: XXX payloads MUST be malloced
Jeej 56:67e3d9608403 492 //======================================================================
Jeej 57:5444cfda9889 493 alp_payload_t* alp_payload_root_auth_enc(alp_payload_t* alp, u8* root_key, u8* challenge)
Jeej 56:67e3d9608403 494 {
Jeej 56:67e3d9608403 495 u8* p;
Jeej 57:5444cfda9889 496 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 497 u8 hash[32];
Jeej 56:67e3d9608403 498
Jeej 56:67e3d9608403 499 // encrypt
Jeej 57:5444cfda9889 500 u8 key[D7A_FS_ROOT_KEY_SIZE];
Jeej 56:67e3d9608403 501 memcpy(key, root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 56:67e3d9608403 502 kal_xor(key, challenge, D7A_FS_ROOT_KEY_SIZE);
Jeej 56:67e3d9608403 503 kal_aes128_init(key);
Jeej 56:67e3d9608403 504 kal_aes128_ctr(alp->d, alp->len, challenge + D7A_FS_ROOT_KEY_SIZE, TRUE);
Jeej 56:67e3d9608403 505
Jeej 56:67e3d9608403 506 // Calculate hash on payload
Jeej 56:67e3d9608403 507 kal_sha256_init();
Jeej 56:67e3d9608403 508 kal_sha256_update(alp->d, alp->len);
Jeej 56:67e3d9608403 509 kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 56:67e3d9608403 510 kal_sha256_update(challenge, 32);
Jeej 56:67e3d9608403 511 kal_sha256_final(hash);
Jeej 56:67e3d9608403 512
Jeej 57:5444cfda9889 513 new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));
Jeej 56:67e3d9608403 514
Jeej 56:67e3d9608403 515 p = new_alp->d;
Jeej 56:67e3d9608403 516
Jeej 56:67e3d9608403 517 ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_CHAL_PROTOCOL_ID);
Jeej 56:67e3d9608403 518 memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
Jeej 56:67e3d9608403 519 p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
Jeej 56:67e3d9608403 520
Jeej 56:67e3d9608403 521 new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
Jeej 57:5444cfda9889 522
Jeej 56:67e3d9608403 523 // XXX Prepend
Jeej 57:5444cfda9889 524 return alp_payload_append(new_alp, alp);
Jeej 56:67e3d9608403 525 }
Jeej 57:5444cfda9889 526 #endif
Jeej 56:67e3d9608403 527
Jeej 56:67e3d9608403 528 //======================================================================
Jeej 56:67e3d9608403 529 // alp_payload_rsp_f_data
Jeej 56:67e3d9608403 530 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 531 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 532 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 533 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 534 /// @param data void* file content
Jeej 56:67e3d9608403 535 /// @param offset u32 file offset
Jeej 56:67e3d9608403 536 /// @param length u32 file length
Jeej 57:5444cfda9889 537 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 538 /// @revent NONE
Jeej 56:67e3d9608403 539 //======================================================================
Jeej 57:5444cfda9889 540 alp_payload_t* alp_payload_rsp_f_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length)
Jeej 56:67e3d9608403 541 {
Jeej 56:67e3d9608403 542 u8* p;
Jeej 57:5444cfda9889 543 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 544
Jeej 57:5444cfda9889 545 new_alp = alp_payload_new(ALP_ACTION_RSP_F_DATA_SIZE(offset, length));
Jeej 56:67e3d9608403 546
Jeej 56:67e3d9608403 547 p = new_alp->d;
Jeej 56:67e3d9608403 548
Jeej 56:67e3d9608403 549 ALP_ACTION_RSP_F_DATA(p, fid, offset, length, data);
Jeej 56:67e3d9608403 550
Jeej 56:67e3d9608403 551 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 552
Jeej 57:5444cfda9889 553 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 554 }
Jeej 56:67e3d9608403 555
Jeej 56:67e3d9608403 556 //======================================================================
Jeej 56:67e3d9608403 557 // alp_payload_qbreak
Jeej 56:67e3d9608403 558 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 559 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 560 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 561 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 562 /// @param data void* file content
Jeej 56:67e3d9608403 563 /// @param offset u32 file offset
Jeej 56:67e3d9608403 564 /// @param length u32 file length
Jeej 56:67e3d9608403 565 /// @param group u8 group with next OP
Jeej 57:5444cfda9889 566 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 567 /// @revent NONE
Jeej 56:67e3d9608403 568 //======================================================================
Jeej 57:5444cfda9889 569 alp_payload_t* alp_payload_qbreak(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 570 {
Jeej 56:67e3d9608403 571 u8* p;
Jeej 57:5444cfda9889 572 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 573 u8 mask = 0;
Jeej 56:67e3d9608403 574
Jeej 57:5444cfda9889 575 new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, mask));
Jeej 56:67e3d9608403 576
Jeej 56:67e3d9608403 577 p = new_alp->d;
Jeej 56:67e3d9608403 578
Jeej 56:67e3d9608403 579 ALP_ACTION_QBREAK_STRTOK(p, mask, data, fid, offset, length, group);
Jeej 56:67e3d9608403 580
Jeej 56:67e3d9608403 581 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 582
Jeej 57:5444cfda9889 583 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 584 }
Jeej 56:67e3d9608403 585
Jeej 57:5444cfda9889 586 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)
Jeej 56:67e3d9608403 587 {
Jeej 56:67e3d9608403 588 u8* p;
Jeej 57:5444cfda9889 589 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 590
Jeej 57:5444cfda9889 591 new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, (mask)? 1 : 0));
Jeej 56:67e3d9608403 592
Jeej 56:67e3d9608403 593 p = new_alp->d;
Jeej 56:67e3d9608403 594
Jeej 56:67e3d9608403 595 ALP_ACTION_QBREAK_COMP(p, mask, data, comp, fid, offset, length, group);
Jeej 56:67e3d9608403 596
Jeej 56:67e3d9608403 597 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 598
Jeej 57:5444cfda9889 599 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 600 }
Jeej 56:67e3d9608403 601
Jeej 57:5444cfda9889 602 alp_payload_t* alp_payload_qbreak_eq(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 603 {
Jeej 56:67e3d9608403 604 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_EQ);
Jeej 56:67e3d9608403 605 }
Jeej 56:67e3d9608403 606
Jeej 57:5444cfda9889 607 alp_payload_t* alp_payload_qbreak_ne(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 608 {
Jeej 56:67e3d9608403 609 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_NE);
Jeej 56:67e3d9608403 610 }
Jeej 56:67e3d9608403 611
Jeej 57:5444cfda9889 612 alp_payload_t* alp_payload_qbreak_gt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 613 {
Jeej 56:67e3d9608403 614 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GT);
Jeej 56:67e3d9608403 615 }
Jeej 56:67e3d9608403 616
Jeej 57:5444cfda9889 617 alp_payload_t* alp_payload_qbreak_gte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 618 {
Jeej 56:67e3d9608403 619 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GTE);
Jeej 56:67e3d9608403 620 }
Jeej 56:67e3d9608403 621
Jeej 57:5444cfda9889 622 alp_payload_t* alp_payload_qbreak_lt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 623 {
Jeej 56:67e3d9608403 624 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LT);
Jeej 56:67e3d9608403 625 }
Jeej 56:67e3d9608403 626
Jeej 57:5444cfda9889 627 alp_payload_t* alp_payload_qbreak_lte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 628 {
Jeej 56:67e3d9608403 629 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LTE);
Jeej 56:67e3d9608403 630 }
Jeej 56:67e3d9608403 631
Jeej 57:5444cfda9889 632 alp_payload_t* alp_payload_qbreak_eq_msk(alp_payload_t* alp, u8 fid, u32 mask, u32 data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 633 {
Jeej 56:67e3d9608403 634 return _alp_payload_qbreak_comp(alp, fid, mask, data, offset, length, group, ALP_QCOMP_EQ);
Jeej 56:67e3d9608403 635 }
Jeej 56:67e3d9608403 636
Jeej 56:67e3d9608403 637 //======================================================================
Jeej 56:67e3d9608403 638 // alp_payload_query
Jeej 56:67e3d9608403 639 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 640 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 641 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 642 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 643 /// @param data void* file content
Jeej 56:67e3d9608403 644 /// @param offset u32 file offset
Jeej 56:67e3d9608403 645 /// @param length u32 file length
Jeej 56:67e3d9608403 646 /// @param group u8 group with next OP
Jeej 57:5444cfda9889 647 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 648 /// @revent NONE
Jeej 56:67e3d9608403 649 //======================================================================
Jeej 57:5444cfda9889 650 alp_payload_t* alp_payload_query(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 651 {
Jeej 56:67e3d9608403 652 u8* p;
Jeej 57:5444cfda9889 653 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 654 u8 mask = 0;
Jeej 56:67e3d9608403 655
Jeej 57:5444cfda9889 656 new_alp = alp_payload_new(ALP_ACTION_QUERY_SIZE(offset, length, mask));
Jeej 56:67e3d9608403 657
Jeej 56:67e3d9608403 658 p = new_alp->d;
Jeej 56:67e3d9608403 659
Jeej 56:67e3d9608403 660 ALP_ACTION_QUERY_STRTOK(p, mask, data, fid, offset, length, group);
Jeej 56:67e3d9608403 661
Jeej 56:67e3d9608403 662 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 663
Jeej 57:5444cfda9889 664 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 665 }
Jeej 56:67e3d9608403 666
Jeej 56:67e3d9608403 667 //======================================================================
Jeej 56:67e3d9608403 668 // alp_payload_nop
Jeej 56:67e3d9608403 669 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 670 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 671 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 57:5444cfda9889 672 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 673 /// @revent NONE
Jeej 56:67e3d9608403 674 //======================================================================
Jeej 57:5444cfda9889 675 alp_payload_t* alp_payload_nop(alp_payload_t* alp)
Jeej 56:67e3d9608403 676 {
Jeej 56:67e3d9608403 677 u8* p;
Jeej 57:5444cfda9889 678 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 679
Jeej 57:5444cfda9889 680 new_alp = alp_payload_new(ALP_ACTION_NOP_SIZE);
Jeej 56:67e3d9608403 681
Jeej 56:67e3d9608403 682 p = new_alp->d;
Jeej 56:67e3d9608403 683
Jeej 56:67e3d9608403 684 ALP_ACTION_NOP(p, true); // XXX RESP always true. If we use NOP, it's to get a response (istatus).
Jeej 56:67e3d9608403 685
Jeej 56:67e3d9608403 686 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 687
Jeej 57:5444cfda9889 688 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 689 }
Jeej 56:67e3d9608403 690
Jeej 56:67e3d9608403 691 //======================================================================
Jeej 56:67e3d9608403 692 // alp_payload_f_wr_data
Jeej 56:67e3d9608403 693 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 694 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 695 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 696 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 697 /// @param data void* file content
Jeej 56:67e3d9608403 698 /// @param offset u32 file offset
Jeej 56:67e3d9608403 699 /// @param length u32 file length
Jeej 56:67e3d9608403 700 /// @param group u8 group with next OP
Jeej 57:5444cfda9889 701 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 702 /// @revent NONE
Jeej 56:67e3d9608403 703 //======================================================================
Jeej 57:5444cfda9889 704 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)
Jeej 56:67e3d9608403 705 {
Jeej 56:67e3d9608403 706 u8* p;
Jeej 57:5444cfda9889 707 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 708
Jeej 57:5444cfda9889 709 new_alp = alp_payload_new(ALP_ACTION_F_WR_DATA_SIZE(offset, length));
Jeej 56:67e3d9608403 710
Jeej 56:67e3d9608403 711 p = new_alp->d;
Jeej 56:67e3d9608403 712
Jeej 56:67e3d9608403 713 ALP_ACTION_F_WR_DATA(p, resp, fid, offset, length, data, group);
Jeej 56:67e3d9608403 714
Jeej 56:67e3d9608403 715 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 716
Jeej 57:5444cfda9889 717 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 718 }
Jeej 56:67e3d9608403 719
Jeej 57:5444cfda9889 720 alp_payload_t* alp_payload_f_wr_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 721 {
Jeej 56:67e3d9608403 722 return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, true);
Jeej 56:67e3d9608403 723 }
Jeej 56:67e3d9608403 724
Jeej 57:5444cfda9889 725 alp_payload_t* alp_payload_f_wr_data_nr(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 726 {
Jeej 56:67e3d9608403 727 return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, false);
Jeej 56:67e3d9608403 728 }
Jeej 56:67e3d9608403 729
Jeej 56:67e3d9608403 730 //======================================================================
Jeej 56:67e3d9608403 731 // alp_payload_f_rd_data
Jeej 56:67e3d9608403 732 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 733 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 734 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 735 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 736 /// @param offset u32 file offset
Jeej 56:67e3d9608403 737 /// @param length u32 file length
Jeej 56:67e3d9608403 738 /// @param group u8 group with next OP
Jeej 57:5444cfda9889 739 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 740 /// @revent NONE
Jeej 56:67e3d9608403 741 //======================================================================
Jeej 57:5444cfda9889 742 alp_payload_t* alp_payload_f_rd_data(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
Jeej 56:67e3d9608403 743 {
Jeej 56:67e3d9608403 744 u8* p;
Jeej 57:5444cfda9889 745 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 746
Jeej 57:5444cfda9889 747 new_alp = alp_payload_new(ALP_ACTION_F_RD_DATA_SIZE(offset, length));
Jeej 56:67e3d9608403 748
Jeej 56:67e3d9608403 749 p = new_alp->d;
Jeej 56:67e3d9608403 750
Jeej 56:67e3d9608403 751 ALP_ACTION_F_RD_DATA(p, true, fid, offset, length, group);
Jeej 56:67e3d9608403 752
Jeej 56:67e3d9608403 753 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 754
Jeej 57:5444cfda9889 755 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 756 }
Jeej 56:67e3d9608403 757
Jeej 56:67e3d9608403 758 //======================================================================
Jeej 56:67e3d9608403 759 // alp_payload_f_flush
Jeej 56:67e3d9608403 760 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 761 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 762 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 763 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 764 /// @param group u8 group with next OP
Jeej 57:5444cfda9889 765 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 766 /// @revent NONE
Jeej 56:67e3d9608403 767 //======================================================================
Jeej 57:5444cfda9889 768 alp_payload_t* alp_payload_f_flush(alp_payload_t* alp, u8 fid, u8 group)
Jeej 56:67e3d9608403 769 {
Jeej 56:67e3d9608403 770 u8* p;
Jeej 57:5444cfda9889 771 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 772
Jeej 57:5444cfda9889 773 new_alp = alp_payload_new(ALP_ACTION_F_FLUSH_SIZE);
Jeej 56:67e3d9608403 774
Jeej 56:67e3d9608403 775 p = new_alp->d;
Jeej 56:67e3d9608403 776
Jeej 56:67e3d9608403 777 ALP_ACTION_F_FLUSH(p, true, fid, group);
Jeej 56:67e3d9608403 778
Jeej 56:67e3d9608403 779 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 780
Jeej 57:5444cfda9889 781 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 782 }
Jeej 56:67e3d9608403 783
Jeej 56:67e3d9608403 784
Jeej 56:67e3d9608403 785 //======================================================================
Jeej 56:67e3d9608403 786 // alp_payload_f_flush
Jeej 56:67e3d9608403 787 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 788 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 789 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 790 /// @param fid u8 file identifier
Jeej 56:67e3d9608403 791 /// @param group u8 group with next OP
Jeej 57:5444cfda9889 792 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 793 /// @revent NONE
Jeej 56:67e3d9608403 794 //======================================================================
Jeej 57:5444cfda9889 795 alp_payload_t* alp_payload_f_declare(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
Jeej 56:67e3d9608403 796 {
Jeej 56:67e3d9608403 797 u8* p;
Jeej 57:5444cfda9889 798 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 799
Jeej 57:5444cfda9889 800 new_alp = alp_payload_new(ALP_ACTION_F_DECLARE_SIZE);
Jeej 56:67e3d9608403 801
Jeej 56:67e3d9608403 802 p = new_alp->d;
Jeej 56:67e3d9608403 803
Jeej 56:67e3d9608403 804 ALP_ACTION_F_DECLARE(p, true, fid, hdr, true);
Jeej 56:67e3d9608403 805
Jeej 56:67e3d9608403 806 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 807
Jeej 57:5444cfda9889 808 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 809 }
Jeej 56:67e3d9608403 810
Jeej 56:67e3d9608403 811 //======================================================================
Jeej 56:67e3d9608403 812 // alp_payload_forward
Jeej 56:67e3d9608403 813 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 814 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 815 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 816 /// @param itf void* Forward interface
Jeej 57:5444cfda9889 817 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 818 /// @revent NONE
Jeej 56:67e3d9608403 819 //======================================================================
Jeej 57:5444cfda9889 820 alp_payload_t* alp_payload_forward(alp_payload_t* alp, void* itf)
Jeej 56:67e3d9608403 821 {
Jeej 56:67e3d9608403 822 u8* p;
Jeej 57:5444cfda9889 823 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 824 u32 itf_length = alp_itf_size(itf);
Jeej 56:67e3d9608403 825
Jeej 57:5444cfda9889 826 new_alp = alp_payload_new(ALP_ACTION_FORWARD_SIZE(itf_length));
Jeej 56:67e3d9608403 827
Jeej 56:67e3d9608403 828 p = new_alp->d;
Jeej 56:67e3d9608403 829
Jeej 56:67e3d9608403 830 ALP_ACTION_FORWARD(p, itf, itf_length);
Jeej 56:67e3d9608403 831
Jeej 56:67e3d9608403 832 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 833
Jeej 57:5444cfda9889 834 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 835 }
Jeej 56:67e3d9608403 836
Jeej 56:67e3d9608403 837 //======================================================================
Jeej 56:67e3d9608403 838 // alp_payload_tag
Jeej 56:67e3d9608403 839 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 840 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 841 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 842 /// @param tag u8 Tag value
Jeej 57:5444cfda9889 843 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 844 /// @revent NONE
Jeej 56:67e3d9608403 845 //======================================================================
Jeej 57:5444cfda9889 846 alp_payload_t* alp_payload_tag(alp_payload_t* alp, u8 tag)
Jeej 56:67e3d9608403 847 {
Jeej 56:67e3d9608403 848 u8* p;
Jeej 57:5444cfda9889 849 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 850
Jeej 57:5444cfda9889 851 new_alp = alp_payload_new(ALP_ACTION_TAG_SIZE);
Jeej 56:67e3d9608403 852
Jeej 56:67e3d9608403 853 p = new_alp->d;
Jeej 56:67e3d9608403 854
Jeej 56:67e3d9608403 855 ALP_ACTION_TAG(p, tag, true);
Jeej 56:67e3d9608403 856
Jeej 56:67e3d9608403 857 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 858
Jeej 57:5444cfda9889 859 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 860 }
Jeej 56:67e3d9608403 861
Jeej 56:67e3d9608403 862 //======================================================================
Jeej 56:67e3d9608403 863 // alp_payload_tag
Jeej 56:67e3d9608403 864 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 865 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 866 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 867 /// @param tag u8 Tag value
Jeej 57:5444cfda9889 868 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 869 /// @revent NONE
Jeej 56:67e3d9608403 870 //======================================================================
Jeej 57:5444cfda9889 871 alp_payload_t* alp_payload_rsp_tag(alp_payload_t* alp, u8 tag, u8 eop, u8 err)
Jeej 56:67e3d9608403 872 {
Jeej 56:67e3d9608403 873 u8* p;
Jeej 57:5444cfda9889 874 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 875
Jeej 57:5444cfda9889 876 new_alp = alp_payload_new(ALP_ACTION_RSP_TAG_SIZE);
Jeej 56:67e3d9608403 877
Jeej 56:67e3d9608403 878 p = new_alp->d;
Jeej 56:67e3d9608403 879
Jeej 56:67e3d9608403 880 ALP_ACTION_RSP_TAG(p, tag, eop, err);
Jeej 56:67e3d9608403 881
Jeej 56:67e3d9608403 882 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 883
Jeej 57:5444cfda9889 884 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 885 }
Jeej 56:67e3d9608403 886
Jeej 56:67e3d9608403 887 //======================================================================
Jeej 56:67e3d9608403 888 // alp_payload_tag
Jeej 56:67e3d9608403 889 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 890 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 891 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 892 /// @param tag u8 Action index
Jeej 56:67e3d9608403 893 /// @param tag s8 Status
Jeej 57:5444cfda9889 894 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 895 /// @revent NONE
Jeej 56:67e3d9608403 896 //======================================================================
Jeej 57:5444cfda9889 897 alp_payload_t* alp_payload_rsp_status(alp_payload_t* alp, u8 action, s8 status)
Jeej 56:67e3d9608403 898 {
Jeej 56:67e3d9608403 899 u8* p;
Jeej 57:5444cfda9889 900 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 901
Jeej 57:5444cfda9889 902 new_alp = alp_payload_new(ALP_ACTION_RSP_STATUS_SIZE);
Jeej 56:67e3d9608403 903
Jeej 56:67e3d9608403 904 p = new_alp->d;
Jeej 56:67e3d9608403 905
Jeej 56:67e3d9608403 906 ALP_ACTION_RSP_STATUS(p, action, status);
Jeej 56:67e3d9608403 907
Jeej 56:67e3d9608403 908 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 909
Jeej 57:5444cfda9889 910 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 911 }
Jeej 56:67e3d9608403 912
Jeej 56:67e3d9608403 913 //======================================================================
Jeej 56:67e3d9608403 914 // alp_payload_tag
Jeej 56:67e3d9608403 915 //----------------------------------------------------------------------
Jeej 56:67e3d9608403 916 /// @brief Creates malloc'ed ALP payload
Jeej 57:5444cfda9889 917 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 56:67e3d9608403 918 /// @param tag u8 Action index
Jeej 56:67e3d9608403 919 /// @param tag s8 Status
Jeej 57:5444cfda9889 920 /// @return alp_payload_t* New ALP payload
Jeej 56:67e3d9608403 921 /// @revent NONE
Jeej 56:67e3d9608403 922 //======================================================================
Jeej 57:5444cfda9889 923 alp_payload_t* alp_payload_rsp_fprop(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
Jeej 56:67e3d9608403 924 {
Jeej 56:67e3d9608403 925 u8* p;
Jeej 57:5444cfda9889 926 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 927
Jeej 57:5444cfda9889 928 new_alp = alp_payload_new(ALP_ACTION_RSP_F_PROP_SIZE);
Jeej 56:67e3d9608403 929
Jeej 56:67e3d9608403 930 p = new_alp->d;
Jeej 56:67e3d9608403 931
Jeej 56:67e3d9608403 932 ALP_ACTION_RSP_F_PROP(p, fid, hdr);
Jeej 56:67e3d9608403 933
Jeej 56:67e3d9608403 934 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 935
Jeej 57:5444cfda9889 936 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 937 }
Jeej 56:67e3d9608403 938
Jeej 57:5444cfda9889 939 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)
Jeej 56:67e3d9608403 940 {
Jeej 56:67e3d9608403 941 u8* p;
Jeej 57:5444cfda9889 942 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 943
Jeej 57:5444cfda9889 944 new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);
Jeej 56:67e3d9608403 945
Jeej 56:67e3d9608403 946 p = new_alp->d;
Jeej 56:67e3d9608403 947
Jeej 56:67e3d9608403 948 ALP_ACTION_ACTIVATE_ITF(p, true, enable, type, nb_dev, ifid, flags);
Jeej 56:67e3d9608403 949
Jeej 56:67e3d9608403 950 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 951
Jeej 57:5444cfda9889 952 return alp_payload_append(alp, new_alp);
Jeej 56:67e3d9608403 953 }
Jeej 56:67e3d9608403 954
Jeej 57:5444cfda9889 955 alp_payload_t* alp_payload_urcc_en(alp_payload_t* alp, uint8_t type, uint8_t ifid, uint8_t val)
Jeej 56:67e3d9608403 956 {
Jeej 56:67e3d9608403 957 u8* p;
Jeej 57:5444cfda9889 958 alp_payload_t* new_alp;
Jeej 56:67e3d9608403 959
Jeej 57:5444cfda9889 960 new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);
Jeej 56:67e3d9608403 961
Jeej 56:67e3d9608403 962 p = new_alp->d;
Jeej 56:67e3d9608403 963
Jeej 56:67e3d9608403 964 ALP_ACTION_URCC_EN(p, true, type, ifid, val);
Jeej 56:67e3d9608403 965
Jeej 56:67e3d9608403 966 new_alp->len = (u32)(p - new_alp->d);
Jeej 56:67e3d9608403 967
Jeej 57:5444cfda9889 968 return alp_payload_append(alp, new_alp);
Jeej 57:5444cfda9889 969 }
Jeej 57:5444cfda9889 970
Jeej 57:5444cfda9889 971 alp_payload_t* alp_payload_extract(alp_payload_t** alp, u8 op)
Jeej 57:5444cfda9889 972 {
Jeej 57:5444cfda9889 973 if (NULL == *alp)
Jeej 57:5444cfda9889 974 {
Jeej 57:5444cfda9889 975 return NULL;
Jeej 57:5444cfda9889 976 }
Jeej 57:5444cfda9889 977
Jeej 57:5444cfda9889 978 if ((*alp)->d[0] == op)
Jeej 57:5444cfda9889 979 {
Jeej 57:5444cfda9889 980 // Save alp
Jeej 57:5444cfda9889 981 alp_payload_t* alp_old = *alp;
Jeej 57:5444cfda9889 982
Jeej 57:5444cfda9889 983 // Remove it from chain
Jeej 57:5444cfda9889 984 *alp = alp_old->next;
Jeej 57:5444cfda9889 985 alp_old->next = NULL;
Jeej 57:5444cfda9889 986
Jeej 57:5444cfda9889 987 // Return it
Jeej 57:5444cfda9889 988 return alp_old;
Jeej 57:5444cfda9889 989 }
Jeej 57:5444cfda9889 990
Jeej 57:5444cfda9889 991 return alp_payload_extract((alp_payload_t**)&((*alp)->next), op);
Jeej 56:67e3d9608403 992 }
Jeej 56:67e3d9608403 993
Jeej 57:5444cfda9889 994 alp_payload_t* alp_payload_get(alp_payload_t* alp, u8 op)
Jeej 56:67e3d9608403 995 {
Jeej 57:5444cfda9889 996 FPRINT(": 0x%p\n", alp);
Jeej 57:5444cfda9889 997
Jeej 56:67e3d9608403 998 if (NULL == alp)
Jeej 56:67e3d9608403 999 {
Jeej 57:5444cfda9889 1000 return NULL;
Jeej 57:5444cfda9889 1001 }
Jeej 57:5444cfda9889 1002
Jeej 57:5444cfda9889 1003 if (alp->d[0] == op)
Jeej 57:5444cfda9889 1004 {
Jeej 57:5444cfda9889 1005 // Return it
Jeej 57:5444cfda9889 1006 return alp;
Jeej 57:5444cfda9889 1007 }
Jeej 57:5444cfda9889 1008
Jeej 57:5444cfda9889 1009 return alp_payload_get(alp->next, op);
Jeej 57:5444cfda9889 1010 }
Jeej 57:5444cfda9889 1011
Jeej 57:5444cfda9889 1012 enum {
Jeej 57:5444cfda9889 1013 ERR_PRIO_EOPISTATUS,
Jeej 57:5444cfda9889 1014 ERR_PRIO_STATUS,
Jeej 57:5444cfda9889 1015 ERR_PRIO_TAG,
Jeej 57:5444cfda9889 1016 ERR_PRIO_NONE,
Jeej 57:5444cfda9889 1017 };
Jeej 57:5444cfda9889 1018
Jeej 57:5444cfda9889 1019 static int _alp_payload_get_err(alp_payload_t* alp, int err, u8 err_prio)
Jeej 57:5444cfda9889 1020 {
Jeej 57:5444cfda9889 1021 alp_parsed_chunk_t r;
Jeej 57:5444cfda9889 1022 u8* p = alp->d;
Jeej 57:5444cfda9889 1023
Jeej 57:5444cfda9889 1024 if (NULL == alp)
Jeej 57:5444cfda9889 1025 {
Jeej 57:5444cfda9889 1026 return err;
Jeej 56:67e3d9608403 1027 }
Jeej 56:67e3d9608403 1028
Jeej 57:5444cfda9889 1029 switch (alp->d[0])
Jeej 57:5444cfda9889 1030 {
Jeej 57:5444cfda9889 1031 case ALP_OPCODE_RSP_TAG:
Jeej 56:67e3d9608403 1032 {
Jeej 57:5444cfda9889 1033 alp_parse_chunk(&p, &r);
Jeej 57:5444cfda9889 1034 if (ERR_PRIO_TAG < err_prio && r.meta.tag.err)
Jeej 57:5444cfda9889 1035 {
Jeej 57:5444cfda9889 1036 err = ALP_ERR_UNKNOWN;
Jeej 57:5444cfda9889 1037 err_prio = ERR_PRIO_TAG;
Jeej 57:5444cfda9889 1038 }
Jeej 57:5444cfda9889 1039 break;
Jeej 57:5444cfda9889 1040 }
Jeej 57:5444cfda9889 1041 case ALP_OPCODE_RSP_STATUS:
Jeej 57:5444cfda9889 1042 {
Jeej 57:5444cfda9889 1043 alp_parse_chunk(&p, &r);
Jeej 57:5444cfda9889 1044 if (ERR_PRIO_STATUS < err_prio && r.meta.status.code < ALP_ERR_NONE)
Jeej 56:67e3d9608403 1045 {
Jeej 57:5444cfda9889 1046 err = r.meta.status.code;
Jeej 57:5444cfda9889 1047 err_prio = ERR_PRIO_STATUS;
Jeej 56:67e3d9608403 1048 }
Jeej 57:5444cfda9889 1049 break;
Jeej 57:5444cfda9889 1050 }
Jeej 57:5444cfda9889 1051 case ALP_OPCODE_RSP_EOPISTATUS:
Jeej 57:5444cfda9889 1052 {
Jeej 57:5444cfda9889 1053 alp_parse_chunk(&p, &r);
Jeej 57:5444cfda9889 1054 if (ERR_PRIO_EOPISTATUS < err_prio && r.meta.istatus.err < ALP_ERR_NONE)
Jeej 57:5444cfda9889 1055 {
Jeej 57:5444cfda9889 1056 err = r.meta.istatus.err + ALP_ERR_ITF_START;
Jeej 57:5444cfda9889 1057 err_prio = ERR_PRIO_EOPISTATUS;
Jeej 57:5444cfda9889 1058 }
Jeej 57:5444cfda9889 1059 break;
Jeej 57:5444cfda9889 1060 }
Jeej 57:5444cfda9889 1061 default:
Jeej 57:5444cfda9889 1062 break;
Jeej 56:67e3d9608403 1063 }
Jeej 56:67e3d9608403 1064
Jeej 57:5444cfda9889 1065 return _alp_payload_get_err(alp->next, err, err_prio);
Jeej 57:5444cfda9889 1066 }
Jeej 57:5444cfda9889 1067
Jeej 57:5444cfda9889 1068 int alp_payload_get_err(alp_payload_t* alp)
Jeej 57:5444cfda9889 1069 {
Jeej 57:5444cfda9889 1070 return _alp_payload_get_err(alp, ALP_ERR_NONE, ERR_PRIO_NONE);
Jeej 56:67e3d9608403 1071 }
Jeej 56:67e3d9608403 1072
Jeej 57:5444cfda9889 1073 alp_payload_t* alp_payload_parse(u8* p, int length)
Jeej 56:67e3d9608403 1074 {
Jeej 57:5444cfda9889 1075 if (NULL == p)
Jeej 57:5444cfda9889 1076 {
Jeej 57:5444cfda9889 1077 return NULL;
Jeej 57:5444cfda9889 1078 }
Jeej 57:5444cfda9889 1079
Jeej 57:5444cfda9889 1080 if (length <= 0)
Jeej 57:5444cfda9889 1081 {
Jeej 57:5444cfda9889 1082 return NULL;
Jeej 57:5444cfda9889 1083 }
Jeej 57:5444cfda9889 1084
Jeej 57:5444cfda9889 1085 u8* d = p;
Jeej 57:5444cfda9889 1086 alp_parsed_chunk_t r;
Jeej 57:5444cfda9889 1087 int len;
Jeej 57:5444cfda9889 1088 alp_payload_t* alp;
Jeej 56:67e3d9608403 1089
Jeej 57:5444cfda9889 1090 // Parse payload
Jeej 57:5444cfda9889 1091 len = (int)alp_parse_chunk(&p, &r);
Jeej 57:5444cfda9889 1092
Jeej 57:5444cfda9889 1093 if (len <= 0)
Jeej 57:5444cfda9889 1094 {
Jeej 57:5444cfda9889 1095 return NULL;
Jeej 57:5444cfda9889 1096 }
Jeej 57:5444cfda9889 1097
Jeej 57:5444cfda9889 1098 // Malloc payload
Jeej 57:5444cfda9889 1099 alp = alp_payload_new(len);
Jeej 57:5444cfda9889 1100
Jeej 57:5444cfda9889 1101 // Fill payload
Jeej 57:5444cfda9889 1102 alp->len = len;
Jeej 57:5444cfda9889 1103 memcpy(alp->d, d, len);
Jeej 57:5444cfda9889 1104
Jeej 57:5444cfda9889 1105 // parse next
Jeej 57:5444cfda9889 1106 return alp_payload_append(alp, alp_payload_parse(p, length - len));
Jeej 56:67e3d9608403 1107 }