WizziLab / modem_ref

Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

src/alp_payload.cpp@65:c7749f9b6009, 2021-10-01 (annotated)

Committer:
Jeej
Date:
Fri Oct 01 08:46:44 2021 +0000
Revision:
65:c7749f9b6009
Parent:
64:7c338f388d82 
commit

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