Exportable version of WizziLab's modem driver.
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?
User | Revision | Line number | New 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 | } |