WizziLab / modem_ref

Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

src/alp_payload.cpp@62:75d819d68e5f, 2021-09-21 (annotated)

Committer:
Jeej
Date:
Tue Sep 21 08:47:52 2021 +0000
Revision:
62:75d819d68e5f
Parent:
61:820395fc5572
Child:
63:544a488a06d3
Use loops instead of recurence

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jeej 59:3b38b5f499db 1 /// @copyright
Jeej 59:3b38b5f499db 2 /// ========================================================================={{{
Jeej 59:3b38b5f499db 3 /// Copyright (c) 2012-2017 WizziLab /
Jeej 59:3b38b5f499db 4 /// All rights reserved /
Jeej 59:3b38b5f499db 5 /// /
Jeej 59:3b38b5f499db 6 /// Should you have any questions regarding your right to use this Software, /
Jeej 59:3b38b5f499db 7 /// contact WizziLab at www.wizzilab.com. /
Jeej 59:3b38b5f499db 8 /// /
Jeej 59:3b38b5f499db 9 /// =========================================================================}}}
Jeej 59:3b38b5f499db 10 /// @endcopyright
Jeej 59:3b38b5f499db 11
Jeej 59:3b38b5f499db 12 // ====================================================================
Jeej 59:3b38b5f499db 13 // @file alp_helpers.c
Jeej 59:3b38b5f499db 14 // @brief ALP helpers functions.
Jeej 59:3b38b5f499db 15 // This code should be disclosable in source.
Jeej 59:3b38b5f499db 16 // ====================================================================
Jeej 59:3b38b5f499db 17
Jeej 59:3b38b5f499db 18
Jeej 59:3b38b5f499db 19 #include "alp.h"
Jeej 59:3b38b5f499db 20 #include "alp_dbg.h"
Jeej 59:3b38b5f499db 21 #include "kal_math.h"
Jeej 59:3b38b5f499db 22 #include "kal_crypto.h"
Jeej 59:3b38b5f499db 23 #include "d7a_1x_fs.h"
Jeej 59:3b38b5f499db 24
Jeej 59:3b38b5f499db 25 #include "WizziDebug.h"
Jeej 59:3b38b5f499db 26
Jeej 59:3b38b5f499db 27 alp_payload_t* alp_payload_new(u32 size)
Jeej 59:3b38b5f499db 28 {
Jeej 59:3b38b5f499db 29 alp_payload_t* alp = (alp_payload_t*)MALLOC(sizeof(alp_payload_t) - 1 + size);
Jeej 59:3b38b5f499db 30
Jeej 59:3b38b5f499db 31 alp->len = 0;
Jeej 59:3b38b5f499db 32 alp->next = NULL;
Jeej 59:3b38b5f499db 33
Jeej 59:3b38b5f499db 34 return alp;
Jeej 59:3b38b5f499db 35 }
Jeej 59:3b38b5f499db 36
Jeej 59:3b38b5f499db 37 void alp_payload_free(alp_payload_t* alp)
Jeej 59:3b38b5f499db 38 {
Jeej 59:3b38b5f499db 39 if (NULL == alp)
Jeej 59:3b38b5f499db 40 {
Jeej 59:3b38b5f499db 41 return;
Jeej 59:3b38b5f499db 42 }
Jeej 59:3b38b5f499db 43
Jeej 59:3b38b5f499db 44 alp_payload_t* alp_next = alp->next;
Jeej 59:3b38b5f499db 45
Jeej 59:3b38b5f499db 46 FREE(alp);
Jeej 59:3b38b5f499db 47
Jeej 59:3b38b5f499db 48 alp_payload_free(alp_next);
Jeej 59:3b38b5f499db 49 }
Jeej 59:3b38b5f499db 50
Jeej 59:3b38b5f499db 51 //======================================================================
Jeej 59:3b38b5f499db 52 // alp_payload_append
Jeej 59:3b38b5f499db 53 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 54 /// @brief Appends ALP payload 2 after payload 1
Jeej 59:3b38b5f499db 55 /// @param alp_payload_t* Payload 1
Jeej 59:3b38b5f499db 56 /// @param alp_payload_t* Payload 2
Jeej 59:3b38b5f499db 57 /// @return alp_payload_t* Appended payloads
Jeej 59:3b38b5f499db 58 /// @note: XXX payloads MUST be malloced
Jeej 59:3b38b5f499db 59 //======================================================================
Jeej 59:3b38b5f499db 60 static void _alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
Jeej 59:3b38b5f499db 61 {
Jeej 59:3b38b5f499db 62 if (NULL == alp_1->next)
Jeej 59:3b38b5f499db 63 {
Jeej 59:3b38b5f499db 64 // Append payload
Jeej 59:3b38b5f499db 65 alp_1->next = alp_2;
Jeej 59:3b38b5f499db 66
Jeej 59:3b38b5f499db 67 return;
Jeej 59:3b38b5f499db 68 }
Jeej 59:3b38b5f499db 69
Jeej 59:3b38b5f499db 70 _alp_payload_append(alp_1->next, alp_2);
Jeej 59:3b38b5f499db 71 }
Jeej 59:3b38b5f499db 72
Jeej 59:3b38b5f499db 73 alp_payload_t* alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
Jeej 59:3b38b5f499db 74 {
Jeej 59:3b38b5f499db 75 if (NULL == alp_1)
Jeej 59:3b38b5f499db 76 {
Jeej 59:3b38b5f499db 77 return alp_2;
Jeej 59:3b38b5f499db 78 }
Jeej 59:3b38b5f499db 79
Jeej 59:3b38b5f499db 80 if (NULL == alp_2)
Jeej 59:3b38b5f499db 81 {
Jeej 59:3b38b5f499db 82 return alp_1;
Jeej 59:3b38b5f499db 83 }
Jeej 59:3b38b5f499db 84
Jeej 59:3b38b5f499db 85 _alp_payload_append(alp_1, alp_2);
Jeej 59:3b38b5f499db 86
Jeej 59:3b38b5f499db 87 return alp_1;
Jeej 59:3b38b5f499db 88 }
Jeej 59:3b38b5f499db 89
Jeej 59:3b38b5f499db 90 void alp_payload_print(alp_payload_t* alp)
Jeej 59:3b38b5f499db 91 {
Jeej 62:75d819d68e5f 92 u32 action = 0;
Jeej 62:75d819d68e5f 93 u32 i;
Jeej 62:75d819d68e5f 94
Jeej 62:75d819d68e5f 95 while (alp != NULL)
Jeej 62:75d819d68e5f 96 {
Jeej 62:75d819d68e5f 97 PRINT("ALP[%d]:", action);
Jeej 62:75d819d68e5f 98 PRINT_DATA("", " %02X", alp->d, alp->len, "\n");
Jeej 62:75d819d68e5f 99 FLUSH();
Jeej 62:75d819d68e5f 100
Jeej 62:75d819d68e5f 101 action++;
Jeej 62:75d819d68e5f 102
Jeej 62:75d819d68e5f 103 alp = alp->next;
Jeej 62:75d819d68e5f 104 }
Jeej 62:75d819d68e5f 105
Jeej 62:75d819d68e5f 106 PRINT("---\n");
Jeej 59:3b38b5f499db 107 }
Jeej 59:3b38b5f499db 108
Jeej 59:3b38b5f499db 109 u32 alp_payload_to_buf(alp_payload_t* alp, u8* buf, u32 offset, u32 max)
Jeej 59:3b38b5f499db 110 {
Jeej 59:3b38b5f499db 111 if (NULL == alp)
Jeej 59:3b38b5f499db 112 {
Jeej 59:3b38b5f499db 113 // End of payload
Jeej 59:3b38b5f499db 114 ALP_ASSERT((offset <= max), "ALP payload too big for buffer (%d\%d)\n", offset, max);
Jeej 59:3b38b5f499db 115 return offset;
Jeej 59:3b38b5f499db 116 }
Jeej 59:3b38b5f499db 117
Jeej 59:3b38b5f499db 118 if ((offset + alp->len) <= max)
Jeej 59:3b38b5f499db 119 {
Jeej 59:3b38b5f499db 120 // Copy into buffer
Jeej 59:3b38b5f499db 121 memcpy(&(buf[offset]), alp->d, alp->len);
Jeej 59:3b38b5f499db 122 }
Jeej 59:3b38b5f499db 123
Jeej 59:3b38b5f499db 124 // Next
Jeej 59:3b38b5f499db 125 return alp_payload_to_buf(alp->next, buf, offset+alp->len, max);
Jeej 59:3b38b5f499db 126 }
Jeej 59:3b38b5f499db 127
Jeej 59:3b38b5f499db 128 //======================================================================
Jeej 59:3b38b5f499db 129 // alp_payload_root_auth
Jeej 59:3b38b5f499db 130 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 131 /// @brief Add the root access request to the given payload using the authentication protocol
Jeej 59:3b38b5f499db 132 /// @param alp alp_payload_t* Payload
Jeej 59:3b38b5f499db 133 /// @return alp_payload_t* Root access request + payload
Jeej 59:3b38b5f499db 134 /// @note: XXX payloads MUST be malloced
Jeej 59:3b38b5f499db 135 //======================================================================
Jeej 59:3b38b5f499db 136 alp_payload_t* alp_payload_root_auth(alp_payload_t* alp, u8* root_key)
Jeej 59:3b38b5f499db 137 {
Jeej 59:3b38b5f499db 138 u8* p;
Jeej 59:3b38b5f499db 139 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 140 alp_payload_t* alp_hash = alp;
Jeej 59:3b38b5f499db 141 u8 hash[32];
Jeej 59:3b38b5f499db 142
Jeej 59:3b38b5f499db 143 // Calculate hash on payload
Jeej 59:3b38b5f499db 144 kal_sha256_init();
Jeej 59:3b38b5f499db 145 while (alp_hash != NULL)
Jeej 59:3b38b5f499db 146 {
Jeej 59:3b38b5f499db 147 kal_sha256_update(alp_hash->d, alp_hash->len);
Jeej 59:3b38b5f499db 148 alp_hash = alp_hash->next;
Jeej 59:3b38b5f499db 149 }
Jeej 59:3b38b5f499db 150 kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 59:3b38b5f499db 151 kal_sha256_final(hash);
Jeej 59:3b38b5f499db 152
Jeej 59:3b38b5f499db 153 new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));
Jeej 59:3b38b5f499db 154
Jeej 59:3b38b5f499db 155 p = new_alp->d;
Jeej 59:3b38b5f499db 156 ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_AUTH_PROTOCOL_ID);
Jeej 59:3b38b5f499db 157
Jeej 59:3b38b5f499db 158 memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
Jeej 59:3b38b5f499db 159 p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
Jeej 59:3b38b5f499db 160
Jeej 59:3b38b5f499db 161 new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
Jeej 59:3b38b5f499db 162
Jeej 59:3b38b5f499db 163 // XXX Prepend
Jeej 59:3b38b5f499db 164 return alp_payload_append(new_alp, alp);
Jeej 59:3b38b5f499db 165 }
Jeej 59:3b38b5f499db 166
Jeej 59:3b38b5f499db 167 //======================================================================
Jeej 59:3b38b5f499db 168 // alp_payload_root_sign
Jeej 59:3b38b5f499db 169 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 170 /// @brief Sign payload with root key
Jeej 59:3b38b5f499db 171 /// @param alp alp_payload_t* Payload
Jeej 59:3b38b5f499db 172 /// @param iv u8* Initialization vector
Jeej 59:3b38b5f499db 173 /// @return alp_payload_t* Root access request + payload
Jeej 59:3b38b5f499db 174 /// @note: XXX payloads MUST be malloced
Jeej 59:3b38b5f499db 175 //======================================================================
Jeej 59:3b38b5f499db 176 alp_payload_t* alp_payload_root_sign(alp_payload_t* alp, u8* root_key, u8* iv)
Jeej 59:3b38b5f499db 177 {
Jeej 59:3b38b5f499db 178 u8* p;
Jeej 59:3b38b5f499db 179 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 180 alp_payload_t* alp_hash = alp;
Jeej 59:3b38b5f499db 181 u8 hash[32];
Jeej 59:3b38b5f499db 182
Jeej 59:3b38b5f499db 183 // Calculate hash on payload
Jeej 59:3b38b5f499db 184 kal_sha256_init();
Jeej 59:3b38b5f499db 185 while (alp_hash != NULL)
Jeej 59:3b38b5f499db 186 {
Jeej 59:3b38b5f499db 187 kal_sha256_update(alp_hash->d, alp_hash->len);
Jeej 59:3b38b5f499db 188 alp_hash = alp_hash->next;
Jeej 59:3b38b5f499db 189 }
Jeej 59:3b38b5f499db 190 kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 59:3b38b5f499db 191 kal_sha256_update(iv, ALP_ACTION_SECURED_IV_SIZE);
Jeej 59:3b38b5f499db 192 kal_sha256_final(hash);
Jeej 59:3b38b5f499db 193
Jeej 59:3b38b5f499db 194 new_alp = alp_payload_new(ALP_ACTION_SECURED_SIZE(0));
Jeej 59:3b38b5f499db 195
Jeej 59:3b38b5f499db 196 p = new_alp->d;
Jeej 59:3b38b5f499db 197 ALP_ACTION_SECURED(p, ALP_SECURED_ROOT, ALP_WIZZILAB_SIGN_PROTOCOL_ID);
Jeej 59:3b38b5f499db 198
Jeej 59:3b38b5f499db 199 memcpy(p, iv, ALP_ACTION_SECURED_IV_SIZE);
Jeej 59:3b38b5f499db 200 p += ALP_ACTION_SECURED_IV_SIZE;
Jeej 59:3b38b5f499db 201
Jeej 59:3b38b5f499db 202 memcpy(p, hash, ALP_ACTION_SECURED_TOKEN_SIZE);
Jeej 59:3b38b5f499db 203 p += ALP_ACTION_SECURED_TOKEN_SIZE;
Jeej 59:3b38b5f499db 204
Jeej 59:3b38b5f499db 205 new_alp->len = ALP_ACTION_SECURED_SIZE(0);
Jeej 59:3b38b5f499db 206
Jeej 59:3b38b5f499db 207 // XXX Prepend
Jeej 59:3b38b5f499db 208 return alp_payload_append(new_alp, alp);
Jeej 59:3b38b5f499db 209 }
Jeej 59:3b38b5f499db 210
Jeej 59:3b38b5f499db 211 #if 0
Jeej 59:3b38b5f499db 212 //======================================================================
Jeej 59:3b38b5f499db 213 // alp_payload_root_auth_enc
Jeej 59:3b38b5f499db 214 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 215 /// @brief Add the root access request to the given payload and encrypt using the challenge protocol
Jeej 59:3b38b5f499db 216 /// @param alp alp_payload_t* Payload
Jeej 59:3b38b5f499db 217 /// @param challenge u8* 32-byte challenge
Jeej 59:3b38b5f499db 218 /// @return alp_payload_t* Root access request + encrypted payload
Jeej 59:3b38b5f499db 219 /// @note: XXX payloads MUST be malloced
Jeej 59:3b38b5f499db 220 //======================================================================
Jeej 59:3b38b5f499db 221 alp_payload_t* alp_payload_root_auth_enc(alp_payload_t* alp, u8* root_key, u8* challenge)
Jeej 59:3b38b5f499db 222 {
Jeej 59:3b38b5f499db 223 u8* p;
Jeej 59:3b38b5f499db 224 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 225 alp_payload_t* alp_hash = alp;
Jeej 59:3b38b5f499db 226 u8 hash[32];
Jeej 59:3b38b5f499db 227
Jeej 59:3b38b5f499db 228 // encrypt
Jeej 59:3b38b5f499db 229 u8 key[D7A_FS_ROOT_KEY_SIZE];
Jeej 59:3b38b5f499db 230 memcpy(key, root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 59:3b38b5f499db 231 kal_xor(key, challenge, D7A_FS_ROOT_KEY_SIZE);
Jeej 59:3b38b5f499db 232 kal_aes128_init(key);
Jeej 59:3b38b5f499db 233 kal_aes128_ctr(alp->d, alp->len, challenge + D7A_FS_ROOT_KEY_SIZE, TRUE);
Jeej 59:3b38b5f499db 234
Jeej 59:3b38b5f499db 235 // Calculate hash on payload
Jeej 59:3b38b5f499db 236 kal_sha256_init();
Jeej 59:3b38b5f499db 237 while (alp_hash != NULL)
Jeej 59:3b38b5f499db 238 {
Jeej 59:3b38b5f499db 239 kal_sha256_update(alp_hash->d, alp_hash->len);
Jeej 59:3b38b5f499db 240 alp_hash = alp_hash->next;
Jeej 59:3b38b5f499db 241 }
Jeej 59:3b38b5f499db 242 kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
Jeej 59:3b38b5f499db 243 kal_sha256_update(challenge, 32);
Jeej 59:3b38b5f499db 244 kal_sha256_final(hash);
Jeej 59:3b38b5f499db 245
Jeej 59:3b38b5f499db 246 new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));
Jeej 59:3b38b5f499db 247
Jeej 59:3b38b5f499db 248 p = new_alp->d;
Jeej 59:3b38b5f499db 249
Jeej 59:3b38b5f499db 250 ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_CHAL_PROTOCOL_ID);
Jeej 59:3b38b5f499db 251 memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
Jeej 59:3b38b5f499db 252 p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
Jeej 59:3b38b5f499db 253
Jeej 59:3b38b5f499db 254 new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
Jeej 59:3b38b5f499db 255
Jeej 59:3b38b5f499db 256 // XXX Prepend
Jeej 59:3b38b5f499db 257 return alp_payload_append(new_alp, alp);
Jeej 59:3b38b5f499db 258 }
Jeej 59:3b38b5f499db 259 #endif
Jeej 59:3b38b5f499db 260
Jeej 59:3b38b5f499db 261 //======================================================================
Jeej 59:3b38b5f499db 262 // alp_payload_rsp_f_data
Jeej 59:3b38b5f499db 263 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 264 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 265 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 266 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 267 /// @param data void* file content
Jeej 59:3b38b5f499db 268 /// @param offset u32 file offset
Jeej 59:3b38b5f499db 269 /// @param length u32 file length
Jeej 59:3b38b5f499db 270 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 271 /// @revent NONE
Jeej 59:3b38b5f499db 272 //======================================================================
Jeej 59:3b38b5f499db 273 alp_payload_t* alp_payload_rsp_f_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length)
Jeej 59:3b38b5f499db 274 {
Jeej 59:3b38b5f499db 275 u8* p;
Jeej 59:3b38b5f499db 276 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 277
Jeej 59:3b38b5f499db 278 new_alp = alp_payload_new(ALP_ACTION_RSP_F_DATA_SIZE(offset, length));
Jeej 59:3b38b5f499db 279
Jeej 59:3b38b5f499db 280 p = new_alp->d;
Jeej 59:3b38b5f499db 281
Jeej 59:3b38b5f499db 282 ALP_ACTION_RSP_F_DATA(p, fid, offset, length, data);
Jeej 59:3b38b5f499db 283
Jeej 59:3b38b5f499db 284 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 285
Jeej 59:3b38b5f499db 286 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 287 }
Jeej 59:3b38b5f499db 288
Jeej 59:3b38b5f499db 289 //======================================================================
Jeej 59:3b38b5f499db 290 // alp_payload_qbreak
Jeej 59:3b38b5f499db 291 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 292 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 293 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 294 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 295 /// @param data void* file content
Jeej 59:3b38b5f499db 296 /// @param offset u32 file offset
Jeej 59:3b38b5f499db 297 /// @param length u32 file length
Jeej 59:3b38b5f499db 298 /// @param group u8 group with next OP
Jeej 59:3b38b5f499db 299 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 300 /// @revent NONE
Jeej 59:3b38b5f499db 301 //======================================================================
Jeej 59:3b38b5f499db 302 alp_payload_t* alp_payload_qbreak(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 303 {
Jeej 59:3b38b5f499db 304 u8* p;
Jeej 59:3b38b5f499db 305 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 306 u8 mask = 0;
Jeej 59:3b38b5f499db 307
Jeej 59:3b38b5f499db 308 new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, mask));
Jeej 59:3b38b5f499db 309
Jeej 59:3b38b5f499db 310 p = new_alp->d;
Jeej 59:3b38b5f499db 311
Jeej 59:3b38b5f499db 312 ALP_ACTION_QBREAK_STRTOK(p, mask, data, fid, offset, length, group);
Jeej 59:3b38b5f499db 313
Jeej 59:3b38b5f499db 314 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 315
Jeej 59:3b38b5f499db 316 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 317 }
Jeej 59:3b38b5f499db 318
Jeej 59:3b38b5f499db 319 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 59:3b38b5f499db 320 {
Jeej 59:3b38b5f499db 321 u8* p;
Jeej 59:3b38b5f499db 322 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 323
Jeej 59:3b38b5f499db 324 new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, (mask)? 1 : 0));
Jeej 59:3b38b5f499db 325
Jeej 59:3b38b5f499db 326 p = new_alp->d;
Jeej 59:3b38b5f499db 327
Jeej 59:3b38b5f499db 328 ALP_ACTION_QBREAK_COMP(p, mask, data, comp, fid, offset, length, group);
Jeej 59:3b38b5f499db 329
Jeej 59:3b38b5f499db 330 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 331
Jeej 59:3b38b5f499db 332 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 333 }
Jeej 59:3b38b5f499db 334
Jeej 59:3b38b5f499db 335 alp_payload_t* alp_payload_qbreak_eq(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 336 {
Jeej 59:3b38b5f499db 337 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_EQ);
Jeej 59:3b38b5f499db 338 }
Jeej 59:3b38b5f499db 339
Jeej 59:3b38b5f499db 340 alp_payload_t* alp_payload_qbreak_ne(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 341 {
Jeej 59:3b38b5f499db 342 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_NE);
Jeej 59:3b38b5f499db 343 }
Jeej 59:3b38b5f499db 344
Jeej 59:3b38b5f499db 345 alp_payload_t* alp_payload_qbreak_gt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 346 {
Jeej 59:3b38b5f499db 347 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GT);
Jeej 59:3b38b5f499db 348 }
Jeej 59:3b38b5f499db 349
Jeej 59:3b38b5f499db 350 alp_payload_t* alp_payload_qbreak_gte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 351 {
Jeej 59:3b38b5f499db 352 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GTE);
Jeej 59:3b38b5f499db 353 }
Jeej 59:3b38b5f499db 354
Jeej 59:3b38b5f499db 355 alp_payload_t* alp_payload_qbreak_lt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 356 {
Jeej 59:3b38b5f499db 357 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LT);
Jeej 59:3b38b5f499db 358 }
Jeej 59:3b38b5f499db 359
Jeej 59:3b38b5f499db 360 alp_payload_t* alp_payload_qbreak_lte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 361 {
Jeej 59:3b38b5f499db 362 return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LTE);
Jeej 59:3b38b5f499db 363 }
Jeej 59:3b38b5f499db 364
Jeej 59:3b38b5f499db 365 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 59:3b38b5f499db 366 {
Jeej 59:3b38b5f499db 367 return _alp_payload_qbreak_comp(alp, fid, mask, data, offset, length, group, ALP_QCOMP_EQ);
Jeej 59:3b38b5f499db 368 }
Jeej 59:3b38b5f499db 369
Jeej 59:3b38b5f499db 370 //======================================================================
Jeej 59:3b38b5f499db 371 // alp_payload_query
Jeej 59:3b38b5f499db 372 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 373 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 374 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 375 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 376 /// @param data void* file content
Jeej 59:3b38b5f499db 377 /// @param offset u32 file offset
Jeej 59:3b38b5f499db 378 /// @param length u32 file length
Jeej 59:3b38b5f499db 379 /// @param group u8 group with next OP
Jeej 59:3b38b5f499db 380 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 381 /// @revent NONE
Jeej 59:3b38b5f499db 382 //======================================================================
Jeej 59:3b38b5f499db 383 alp_payload_t* alp_payload_query(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 384 {
Jeej 59:3b38b5f499db 385 u8* p;
Jeej 59:3b38b5f499db 386 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 387 u8 mask = 0;
Jeej 59:3b38b5f499db 388
Jeej 59:3b38b5f499db 389 new_alp = alp_payload_new(ALP_ACTION_QUERY_SIZE(offset, length, mask));
Jeej 59:3b38b5f499db 390
Jeej 59:3b38b5f499db 391 p = new_alp->d;
Jeej 59:3b38b5f499db 392
Jeej 59:3b38b5f499db 393 ALP_ACTION_QUERY_STRTOK(p, mask, data, fid, offset, length, group);
Jeej 59:3b38b5f499db 394
Jeej 59:3b38b5f499db 395 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 396
Jeej 59:3b38b5f499db 397 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 398 }
Jeej 59:3b38b5f499db 399
Jeej 59:3b38b5f499db 400 //======================================================================
Jeej 59:3b38b5f499db 401 // alp_payload_nop
Jeej 59:3b38b5f499db 402 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 403 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 404 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 405 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 406 /// @revent NONE
Jeej 59:3b38b5f499db 407 //======================================================================
Jeej 60:08efaaca0e83 408 alp_payload_t* alp_payload_nop(alp_payload_t* alp, u8 group)
Jeej 59:3b38b5f499db 409 {
Jeej 59:3b38b5f499db 410 u8* p;
Jeej 59:3b38b5f499db 411 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 412
Jeej 59:3b38b5f499db 413 new_alp = alp_payload_new(ALP_ACTION_NOP_SIZE);
Jeej 59:3b38b5f499db 414
Jeej 59:3b38b5f499db 415 p = new_alp->d;
Jeej 59:3b38b5f499db 416
Jeej 60:08efaaca0e83 417 ALP_ACTION_NOP(p, true, group); // XXX RESP always true. If we use NOP, it's to get a response (istatus).
Jeej 60:08efaaca0e83 418
Jeej 60:08efaaca0e83 419 new_alp->len = (u32)(p - new_alp->d);
Jeej 60:08efaaca0e83 420
Jeej 60:08efaaca0e83 421 return alp_payload_append(alp, new_alp);
Jeej 60:08efaaca0e83 422 }
Jeej 60:08efaaca0e83 423
Jeej 60:08efaaca0e83 424 //======================================================================
Jeej 60:08efaaca0e83 425 // alp_payload_f_touch
Jeej 60:08efaaca0e83 426 //----------------------------------------------------------------------
Jeej 60:08efaaca0e83 427 /// @brief Creates malloc'ed ALP payload
Jeej 60:08efaaca0e83 428 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 60:08efaaca0e83 429 /// @return alp_payload_t* New ALP payload
Jeej 60:08efaaca0e83 430 /// @revent NONE
Jeej 60:08efaaca0e83 431 //======================================================================
Jeej 60:08efaaca0e83 432 alp_payload_t* alp_payload_f_touch(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
Jeej 60:08efaaca0e83 433 {
Jeej 60:08efaaca0e83 434 u8* p;
Jeej 60:08efaaca0e83 435 alp_payload_t* new_alp;
Jeej 60:08efaaca0e83 436
Jeej 60:08efaaca0e83 437 new_alp = alp_payload_new(ALP_ACTION_F_TOUCH_SIZE(offset, length));
Jeej 60:08efaaca0e83 438
Jeej 60:08efaaca0e83 439 p = new_alp->d;
Jeej 60:08efaaca0e83 440
Jeej 60:08efaaca0e83 441 ALP_ACTION_F_TOUCH(p, true, fid, offset, length, group);
Jeej 59:3b38b5f499db 442
Jeej 59:3b38b5f499db 443 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 444
Jeej 59:3b38b5f499db 445 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 446 }
Jeej 59:3b38b5f499db 447
Jeej 59:3b38b5f499db 448 //======================================================================
Jeej 59:3b38b5f499db 449 // alp_payload_f_wr_data
Jeej 59:3b38b5f499db 450 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 451 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 452 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 453 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 454 /// @param data void* file content
Jeej 59:3b38b5f499db 455 /// @param offset u32 file offset
Jeej 59:3b38b5f499db 456 /// @param length u32 file length
Jeej 59:3b38b5f499db 457 /// @param group u8 group with next OP
Jeej 59:3b38b5f499db 458 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 459 /// @revent NONE
Jeej 59:3b38b5f499db 460 //======================================================================
Jeej 59:3b38b5f499db 461 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 59:3b38b5f499db 462 {
Jeej 59:3b38b5f499db 463 u8* p;
Jeej 59:3b38b5f499db 464 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 465
Jeej 59:3b38b5f499db 466 new_alp = alp_payload_new(ALP_ACTION_F_WR_DATA_SIZE(offset, length));
Jeej 59:3b38b5f499db 467
Jeej 59:3b38b5f499db 468 p = new_alp->d;
Jeej 59:3b38b5f499db 469
Jeej 59:3b38b5f499db 470 ALP_ACTION_F_WR_DATA(p, resp, fid, offset, length, data, group);
Jeej 59:3b38b5f499db 471
Jeej 59:3b38b5f499db 472 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 473
Jeej 59:3b38b5f499db 474 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 475 }
Jeej 59:3b38b5f499db 476
Jeej 59:3b38b5f499db 477 alp_payload_t* alp_payload_f_wr_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 478 {
Jeej 59:3b38b5f499db 479 return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, true);
Jeej 59:3b38b5f499db 480 }
Jeej 59:3b38b5f499db 481
Jeej 59:3b38b5f499db 482 alp_payload_t* alp_payload_f_wr_data_nr(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 483 {
Jeej 59:3b38b5f499db 484 return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, false);
Jeej 59:3b38b5f499db 485 }
Jeej 59:3b38b5f499db 486
Jeej 59:3b38b5f499db 487 //======================================================================
Jeej 59:3b38b5f499db 488 // alp_payload_f_rd_data
Jeej 59:3b38b5f499db 489 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 490 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 491 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 492 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 493 /// @param offset u32 file offset
Jeej 59:3b38b5f499db 494 /// @param length u32 file length
Jeej 59:3b38b5f499db 495 /// @param group u8 group with next OP
Jeej 59:3b38b5f499db 496 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 497 /// @revent NONE
Jeej 59:3b38b5f499db 498 //======================================================================
Jeej 59:3b38b5f499db 499 alp_payload_t* alp_payload_f_rd_data(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
Jeej 59:3b38b5f499db 500 {
Jeej 59:3b38b5f499db 501 u8* p;
Jeej 59:3b38b5f499db 502 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 503
Jeej 59:3b38b5f499db 504 new_alp = alp_payload_new(ALP_ACTION_F_RD_DATA_SIZE(offset, length));
Jeej 59:3b38b5f499db 505
Jeej 59:3b38b5f499db 506 p = new_alp->d;
Jeej 59:3b38b5f499db 507
Jeej 59:3b38b5f499db 508 ALP_ACTION_F_RD_DATA(p, true, fid, offset, length, group);
Jeej 59:3b38b5f499db 509
Jeej 59:3b38b5f499db 510 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 511
Jeej 59:3b38b5f499db 512 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 513 }
Jeej 59:3b38b5f499db 514
Jeej 59:3b38b5f499db 515 //======================================================================
Jeej 59:3b38b5f499db 516 // alp_payload_f_flush
Jeej 59:3b38b5f499db 517 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 518 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 519 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 520 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 521 /// @param group u8 group with next OP
Jeej 59:3b38b5f499db 522 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 523 /// @revent NONE
Jeej 59:3b38b5f499db 524 //======================================================================
Jeej 59:3b38b5f499db 525 alp_payload_t* alp_payload_f_flush(alp_payload_t* alp, u8 fid, u8 group)
Jeej 59:3b38b5f499db 526 {
Jeej 59:3b38b5f499db 527 u8* p;
Jeej 59:3b38b5f499db 528 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 529
Jeej 59:3b38b5f499db 530 new_alp = alp_payload_new(ALP_ACTION_F_FLUSH_SIZE);
Jeej 59:3b38b5f499db 531
Jeej 59:3b38b5f499db 532 p = new_alp->d;
Jeej 59:3b38b5f499db 533
Jeej 59:3b38b5f499db 534 ALP_ACTION_F_FLUSH(p, true, fid, group);
Jeej 59:3b38b5f499db 535
Jeej 59:3b38b5f499db 536 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 537
Jeej 59:3b38b5f499db 538 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 539 }
Jeej 59:3b38b5f499db 540
Jeej 59:3b38b5f499db 541
Jeej 59:3b38b5f499db 542 //======================================================================
Jeej 59:3b38b5f499db 543 // alp_payload_f_flush
Jeej 59:3b38b5f499db 544 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 545 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 546 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 547 /// @param fid u8 file identifier
Jeej 59:3b38b5f499db 548 /// @param group u8 group with next OP
Jeej 59:3b38b5f499db 549 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 550 /// @revent NONE
Jeej 59:3b38b5f499db 551 //======================================================================
Jeej 59:3b38b5f499db 552 alp_payload_t* alp_payload_f_declare(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
Jeej 59:3b38b5f499db 553 {
Jeej 59:3b38b5f499db 554 u8* p;
Jeej 59:3b38b5f499db 555 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 556
Jeej 59:3b38b5f499db 557 new_alp = alp_payload_new(ALP_ACTION_F_DECLARE_SIZE);
Jeej 59:3b38b5f499db 558
Jeej 59:3b38b5f499db 559 p = new_alp->d;
Jeej 59:3b38b5f499db 560
Jeej 59:3b38b5f499db 561 ALP_ACTION_F_DECLARE(p, true, fid, hdr, true);
Jeej 59:3b38b5f499db 562
Jeej 59:3b38b5f499db 563 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 564
Jeej 59:3b38b5f499db 565 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 566 }
Jeej 59:3b38b5f499db 567
Jeej 59:3b38b5f499db 568 //======================================================================
Jeej 59:3b38b5f499db 569 // alp_payload_forward
Jeej 59:3b38b5f499db 570 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 571 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 572 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 573 /// @param itf void* Forward interface
Jeej 59:3b38b5f499db 574 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 575 /// @revent NONE
Jeej 59:3b38b5f499db 576 //======================================================================
Jeej 59:3b38b5f499db 577 alp_payload_t* alp_payload_forward(alp_payload_t* alp, void* itf)
Jeej 59:3b38b5f499db 578 {
Jeej 59:3b38b5f499db 579 u8* p;
Jeej 59:3b38b5f499db 580 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 581 u32 itf_length = alp_itf_size(itf);
Jeej 59:3b38b5f499db 582
Jeej 59:3b38b5f499db 583 new_alp = alp_payload_new(ALP_ACTION_FORWARD_SIZE(itf_length));
Jeej 59:3b38b5f499db 584
Jeej 59:3b38b5f499db 585 p = new_alp->d;
Jeej 59:3b38b5f499db 586
Jeej 59:3b38b5f499db 587 ALP_ACTION_FORWARD(p, itf, itf_length);
Jeej 59:3b38b5f499db 588
Jeej 59:3b38b5f499db 589 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 590
Jeej 59:3b38b5f499db 591 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 592 }
Jeej 59:3b38b5f499db 593
Jeej 59:3b38b5f499db 594 //======================================================================
Jeej 59:3b38b5f499db 595 // alp_payload_tag
Jeej 59:3b38b5f499db 596 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 597 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 598 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 599 /// @param tag u8 Tag value
Jeej 59:3b38b5f499db 600 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 601 /// @revent NONE
Jeej 59:3b38b5f499db 602 //======================================================================
Jeej 59:3b38b5f499db 603 alp_payload_t* alp_payload_tag(alp_payload_t* alp, u8 tag)
Jeej 59:3b38b5f499db 604 {
Jeej 59:3b38b5f499db 605 u8* p;
Jeej 59:3b38b5f499db 606 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 607
Jeej 59:3b38b5f499db 608 new_alp = alp_payload_new(ALP_ACTION_TAG_SIZE);
Jeej 59:3b38b5f499db 609
Jeej 59:3b38b5f499db 610 p = new_alp->d;
Jeej 59:3b38b5f499db 611
Jeej 59:3b38b5f499db 612 ALP_ACTION_TAG(p, tag, true);
Jeej 59:3b38b5f499db 613
Jeej 59:3b38b5f499db 614 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 615
Jeej 59:3b38b5f499db 616 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 617 }
Jeej 59:3b38b5f499db 618
Jeej 59:3b38b5f499db 619 //======================================================================
Jeej 59:3b38b5f499db 620 // alp_payload_tag
Jeej 59:3b38b5f499db 621 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 622 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 623 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 624 /// @param tag u8 Tag value
Jeej 59:3b38b5f499db 625 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 626 /// @revent NONE
Jeej 59:3b38b5f499db 627 //======================================================================
Jeej 59:3b38b5f499db 628 alp_payload_t* alp_payload_rsp_tag(alp_payload_t* alp, u8 tag, u8 eop, u8 err)
Jeej 59:3b38b5f499db 629 {
Jeej 59:3b38b5f499db 630 u8* p;
Jeej 59:3b38b5f499db 631 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 632
Jeej 59:3b38b5f499db 633 new_alp = alp_payload_new(ALP_ACTION_RSP_TAG_SIZE);
Jeej 59:3b38b5f499db 634
Jeej 59:3b38b5f499db 635 p = new_alp->d;
Jeej 59:3b38b5f499db 636
Jeej 59:3b38b5f499db 637 ALP_ACTION_RSP_TAG(p, tag, eop, err);
Jeej 59:3b38b5f499db 638
Jeej 59:3b38b5f499db 639 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 640
Jeej 59:3b38b5f499db 641 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 642 }
Jeej 59:3b38b5f499db 643
Jeej 59:3b38b5f499db 644 //======================================================================
Jeej 59:3b38b5f499db 645 // alp_payload_tag
Jeej 59:3b38b5f499db 646 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 647 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 648 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 649 /// @param tag u8 Action index
Jeej 59:3b38b5f499db 650 /// @param tag s8 Status
Jeej 59:3b38b5f499db 651 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 652 /// @revent NONE
Jeej 59:3b38b5f499db 653 //======================================================================
Jeej 59:3b38b5f499db 654 alp_payload_t* alp_payload_rsp_status(alp_payload_t* alp, u8 action, s8 status)
Jeej 59:3b38b5f499db 655 {
Jeej 59:3b38b5f499db 656 u8* p;
Jeej 59:3b38b5f499db 657 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 658
Jeej 59:3b38b5f499db 659 new_alp = alp_payload_new(ALP_ACTION_RSP_STATUS_SIZE);
Jeej 59:3b38b5f499db 660
Jeej 59:3b38b5f499db 661 p = new_alp->d;
Jeej 59:3b38b5f499db 662
Jeej 59:3b38b5f499db 663 ALP_ACTION_RSP_STATUS(p, action, status);
Jeej 59:3b38b5f499db 664
Jeej 59:3b38b5f499db 665 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 666
Jeej 59:3b38b5f499db 667 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 668 }
Jeej 59:3b38b5f499db 669
Jeej 59:3b38b5f499db 670 //======================================================================
Jeej 59:3b38b5f499db 671 // alp_payload_tag
Jeej 59:3b38b5f499db 672 //----------------------------------------------------------------------
Jeej 59:3b38b5f499db 673 /// @brief Creates malloc'ed ALP payload
Jeej 59:3b38b5f499db 674 /// @param alp alp_payload_t* Payload to append the ALP message to. A new payload will be malloc'ed if NULL
Jeej 59:3b38b5f499db 675 /// @param tag u8 Action index
Jeej 59:3b38b5f499db 676 /// @param tag s8 Status
Jeej 59:3b38b5f499db 677 /// @return alp_payload_t* New ALP payload
Jeej 59:3b38b5f499db 678 /// @revent NONE
Jeej 59:3b38b5f499db 679 //======================================================================
Jeej 59:3b38b5f499db 680 alp_payload_t* alp_payload_rsp_fprop(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
Jeej 59:3b38b5f499db 681 {
Jeej 59:3b38b5f499db 682 u8* p;
Jeej 59:3b38b5f499db 683 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 684
Jeej 59:3b38b5f499db 685 new_alp = alp_payload_new(ALP_ACTION_RSP_F_PROP_SIZE);
Jeej 59:3b38b5f499db 686
Jeej 59:3b38b5f499db 687 p = new_alp->d;
Jeej 59:3b38b5f499db 688
Jeej 59:3b38b5f499db 689 ALP_ACTION_RSP_F_PROP(p, fid, hdr);
Jeej 59:3b38b5f499db 690
Jeej 59:3b38b5f499db 691 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 692
Jeej 59:3b38b5f499db 693 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 694 }
Jeej 59:3b38b5f499db 695
Jeej 60:08efaaca0e83 696 alp_payload_t* alp_payload_activate_itf(alp_payload_t* alp, u8 type, u8 nb_dev, u8 ifid, u8 flags, u8 enable)
Jeej 59:3b38b5f499db 697 {
Jeej 59:3b38b5f499db 698 u8* p;
Jeej 59:3b38b5f499db 699 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 700
Jeej 59:3b38b5f499db 701 new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);
Jeej 59:3b38b5f499db 702
Jeej 59:3b38b5f499db 703 p = new_alp->d;
Jeej 59:3b38b5f499db 704
Jeej 59:3b38b5f499db 705 ALP_ACTION_ACTIVATE_ITF(p, true, enable, type, nb_dev, ifid, flags);
Jeej 59:3b38b5f499db 706
Jeej 59:3b38b5f499db 707 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 708
Jeej 59:3b38b5f499db 709 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 710 }
Jeej 59:3b38b5f499db 711
Jeej 60:08efaaca0e83 712 alp_payload_t* alp_payload_urcc_en(alp_payload_t* alp, u8 type, u8 ifid, u8 val)
Jeej 59:3b38b5f499db 713 {
Jeej 59:3b38b5f499db 714 u8* p;
Jeej 59:3b38b5f499db 715 alp_payload_t* new_alp;
Jeej 59:3b38b5f499db 716
Jeej 59:3b38b5f499db 717 new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);
Jeej 59:3b38b5f499db 718
Jeej 59:3b38b5f499db 719 p = new_alp->d;
Jeej 59:3b38b5f499db 720
Jeej 59:3b38b5f499db 721 ALP_ACTION_URCC_EN(p, true, type, ifid, val);
Jeej 59:3b38b5f499db 722
Jeej 59:3b38b5f499db 723 new_alp->len = (u32)(p - new_alp->d);
Jeej 59:3b38b5f499db 724
Jeej 59:3b38b5f499db 725 return alp_payload_append(alp, new_alp);
Jeej 59:3b38b5f499db 726 }
Jeej 59:3b38b5f499db 727
Jeej 59:3b38b5f499db 728 alp_payload_t* alp_payload_extract(alp_payload_t** alp, u8 op)
Jeej 59:3b38b5f499db 729 {
Jeej 59:3b38b5f499db 730 if (NULL == *alp)
Jeej 59:3b38b5f499db 731 {
Jeej 59:3b38b5f499db 732 return NULL;
Jeej 59:3b38b5f499db 733 }
Jeej 59:3b38b5f499db 734
Jeej 59:3b38b5f499db 735 if ((*alp)->d[0] == op)
Jeej 59:3b38b5f499db 736 {
Jeej 59:3b38b5f499db 737 // Save alp
Jeej 59:3b38b5f499db 738 alp_payload_t* alp_old = *alp;
Jeej 59:3b38b5f499db 739
Jeej 59:3b38b5f499db 740 // Remove it from chain
Jeej 59:3b38b5f499db 741 *alp = alp_old->next;
Jeej 59:3b38b5f499db 742 alp_old->next = NULL;
Jeej 59:3b38b5f499db 743
Jeej 59:3b38b5f499db 744 // Return it
Jeej 59:3b38b5f499db 745 return alp_old;
Jeej 59:3b38b5f499db 746 }
Jeej 59:3b38b5f499db 747
Jeej 59:3b38b5f499db 748 return alp_payload_extract((alp_payload_t**)&((*alp)->next), op);
Jeej 59:3b38b5f499db 749 }
Jeej 59:3b38b5f499db 750
Jeej 59:3b38b5f499db 751 alp_payload_t* alp_payload_get(alp_payload_t* alp, u8 op)
Jeej 61:820395fc5572 752 {
Jeej 59:3b38b5f499db 753 if (NULL == alp)
Jeej 59:3b38b5f499db 754 {
Jeej 59:3b38b5f499db 755 return NULL;
Jeej 59:3b38b5f499db 756 }
Jeej 59:3b38b5f499db 757
Jeej 59:3b38b5f499db 758 if (alp->d[0] == op)
Jeej 59:3b38b5f499db 759 {
Jeej 59:3b38b5f499db 760 // Return it
Jeej 59:3b38b5f499db 761 return alp;
Jeej 59:3b38b5f499db 762 }
Jeej 59:3b38b5f499db 763
Jeej 59:3b38b5f499db 764 return alp_payload_get(alp->next, op);
Jeej 59:3b38b5f499db 765 }
Jeej 59:3b38b5f499db 766
Jeej 61:820395fc5572 767 bool alp_payload_extract_data(alp_payload_t** alp, u8 op, void* data)
Jeej 61:820395fc5572 768 {
Jeej 61:820395fc5572 769 alp_payload_t* alp_data = alp_payload_extract(alp, op);
Jeej 61:820395fc5572 770
Jeej 61:820395fc5572 771 if (alp_data)
Jeej 61:820395fc5572 772 {
Jeej 61:820395fc5572 773 alp_parsed_chunk_t r;
Jeej 61:820395fc5572 774 u8* p = alp_data->d;
Jeej 61:820395fc5572 775
Jeej 61:820395fc5572 776 alp_parse_chunk(&p, &r);
Jeej 61:820395fc5572 777
Jeej 61:820395fc5572 778 switch (op)
Jeej 61:820395fc5572 779 {
Jeej 61:820395fc5572 780 case ALP_OPCODE_RSP_ISTATUS:
Jeej 61:820395fc5572 781 memcpy(data, r.data, r.meta.itf.length);
Jeej 61:820395fc5572 782 break;;
Jeej 61:820395fc5572 783 case ALP_OPCODE_RSP_F_DATA:
Jeej 61:820395fc5572 784 memcpy(data, r.data, r.meta.f_data.length);
Jeej 61:820395fc5572 785 break;
Jeej 61:820395fc5572 786 default:
Jeej 61:820395fc5572 787 alp_payload_free(alp_data);
Jeej 61:820395fc5572 788 return false;
Jeej 61:820395fc5572 789 break;
Jeej 61:820395fc5572 790 }
Jeej 61:820395fc5572 791
Jeej 61:820395fc5572 792 alp_payload_free(alp_data);
Jeej 61:820395fc5572 793
Jeej 61:820395fc5572 794 return true;
Jeej 61:820395fc5572 795 }
Jeej 61:820395fc5572 796
Jeej 61:820395fc5572 797 return false;
Jeej 61:820395fc5572 798 }
Jeej 61:820395fc5572 799
Jeej 59:3b38b5f499db 800 enum {
Jeej 59:3b38b5f499db 801 ERR_PRIO_EOPISTATUS,
Jeej 59:3b38b5f499db 802 ERR_PRIO_STATUS,
Jeej 59:3b38b5f499db 803 ERR_PRIO_TAG,
Jeej 59:3b38b5f499db 804 ERR_PRIO_NONE,
Jeej 59:3b38b5f499db 805 };
Jeej 59:3b38b5f499db 806
Jeej 59:3b38b5f499db 807 static int _alp_payload_get_err(alp_payload_t* alp, int err, u8 err_prio)
Jeej 59:3b38b5f499db 808 {
Jeej 59:3b38b5f499db 809 alp_parsed_chunk_t r;
Jeej 59:3b38b5f499db 810 u8* p = alp->d;
Jeej 59:3b38b5f499db 811
Jeej 59:3b38b5f499db 812 if (NULL == alp)
Jeej 59:3b38b5f499db 813 {
Jeej 59:3b38b5f499db 814 return err;
Jeej 59:3b38b5f499db 815 }
Jeej 59:3b38b5f499db 816
Jeej 59:3b38b5f499db 817 switch (alp->d[0])
Jeej 59:3b38b5f499db 818 {
Jeej 59:3b38b5f499db 819 case ALP_OPCODE_RSP_TAG:
Jeej 59:3b38b5f499db 820 {
Jeej 59:3b38b5f499db 821 alp_parse_chunk(&p, &r);
Jeej 59:3b38b5f499db 822 if (ERR_PRIO_TAG < err_prio && r.meta.tag.err)
Jeej 59:3b38b5f499db 823 {
Jeej 59:3b38b5f499db 824 err = ALP_ERR_UNKNOWN;
Jeej 59:3b38b5f499db 825 err_prio = ERR_PRIO_TAG;
Jeej 59:3b38b5f499db 826 }
Jeej 59:3b38b5f499db 827 break;
Jeej 59:3b38b5f499db 828 }
Jeej 59:3b38b5f499db 829 case ALP_OPCODE_RSP_STATUS:
Jeej 59:3b38b5f499db 830 {
Jeej 59:3b38b5f499db 831 alp_parse_chunk(&p, &r);
Jeej 59:3b38b5f499db 832 if (ERR_PRIO_STATUS < err_prio && r.meta.status.code < ALP_ERR_NONE)
Jeej 59:3b38b5f499db 833 {
Jeej 59:3b38b5f499db 834 err = r.meta.status.code;
Jeej 59:3b38b5f499db 835 err_prio = ERR_PRIO_STATUS;
Jeej 59:3b38b5f499db 836 }
Jeej 59:3b38b5f499db 837 break;
Jeej 59:3b38b5f499db 838 }
Jeej 59:3b38b5f499db 839 case ALP_OPCODE_RSP_EOPISTATUS:
Jeej 59:3b38b5f499db 840 {
Jeej 59:3b38b5f499db 841 alp_parse_chunk(&p, &r);
Jeej 59:3b38b5f499db 842 if (ERR_PRIO_EOPISTATUS < err_prio && r.meta.istatus.err < ALP_ERR_NONE)
Jeej 59:3b38b5f499db 843 {
Jeej 59:3b38b5f499db 844 err = r.meta.istatus.err + ALP_ERR_ITF_START;
Jeej 59:3b38b5f499db 845 err_prio = ERR_PRIO_EOPISTATUS;
Jeej 59:3b38b5f499db 846 }
Jeej 59:3b38b5f499db 847 break;
Jeej 59:3b38b5f499db 848 }
Jeej 59:3b38b5f499db 849 default:
Jeej 59:3b38b5f499db 850 break;
Jeej 59:3b38b5f499db 851 }
Jeej 59:3b38b5f499db 852
Jeej 59:3b38b5f499db 853 return _alp_payload_get_err(alp->next, err, err_prio);
Jeej 59:3b38b5f499db 854 }
Jeej 59:3b38b5f499db 855
Jeej 59:3b38b5f499db 856 int alp_payload_get_err(alp_payload_t* alp)
Jeej 59:3b38b5f499db 857 {
Jeej 59:3b38b5f499db 858 return _alp_payload_get_err(alp, ALP_ERR_NONE, ERR_PRIO_NONE);
Jeej 59:3b38b5f499db 859 }
Jeej 59:3b38b5f499db 860
Jeej 59:3b38b5f499db 861 alp_payload_t* alp_payload_parse(u8* p, int length)
Jeej 59:3b38b5f499db 862 {
Jeej 59:3b38b5f499db 863 if (NULL == p)
Jeej 59:3b38b5f499db 864 {
Jeej 59:3b38b5f499db 865 return NULL;
Jeej 59:3b38b5f499db 866 }
Jeej 59:3b38b5f499db 867
Jeej 59:3b38b5f499db 868 if (length <= 0)
Jeej 59:3b38b5f499db 869 {
Jeej 59:3b38b5f499db 870 return NULL;
Jeej 59:3b38b5f499db 871 }
Jeej 59:3b38b5f499db 872
Jeej 59:3b38b5f499db 873 u8* d = p;
Jeej 59:3b38b5f499db 874 alp_parsed_chunk_t r;
Jeej 59:3b38b5f499db 875 int len;
Jeej 59:3b38b5f499db 876 alp_payload_t* alp;
Jeej 59:3b38b5f499db 877
Jeej 59:3b38b5f499db 878 // Parse payload
Jeej 59:3b38b5f499db 879 len = (int)alp_parse_chunk(&p, &r);
Jeej 59:3b38b5f499db 880
Jeej 59:3b38b5f499db 881 if (len <= 0)
Jeej 59:3b38b5f499db 882 {
Jeej 59:3b38b5f499db 883 return NULL;
Jeej 59:3b38b5f499db 884 }
Jeej 59:3b38b5f499db 885
Jeej 59:3b38b5f499db 886 // Malloc payload
Jeej 59:3b38b5f499db 887 alp = alp_payload_new(len);
Jeej 59:3b38b5f499db 888
Jeej 59:3b38b5f499db 889 // Fill payload
Jeej 59:3b38b5f499db 890 alp->len = len;
Jeej 59:3b38b5f499db 891 memcpy(alp->d, d, len);
Jeej 59:3b38b5f499db 892
Jeej 59:3b38b5f499db 893 // parse next
Jeej 59:3b38b5f499db 894 return alp_payload_append(alp, alp_payload_parse(p, length - len));
Jeej 59:3b38b5f499db 895 }