WizziLab / modem_ref

Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

src/alp_payload.cpp@60:08efaaca0e83, 2021-02-19 (annotated)

Committer:
Jeej
Date:
Fri Feb 19 10:59:42 2021 +0000
Revision:
60:08efaaca0e83
Parent:
59:3b38b5f499db
Child:
61:820395fc5572
Fixed responses

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