WizziLab / modem_ref

Exportable version of WizziLab's modem driver.

Dependents:   modem_ref_helper

src/alp_payload.cpp@64:7c338f388d82, 2021-09-23 (annotated)

Committer:
Jeej
Date:
Thu Sep 23 15:06:02 2021 +0000
Revision:
64:7c338f388d82
Parent:
63:544a488a06d3
Child:
65:c7749f9b6009
Added print to follow number of malloc'ed ALP payloads.

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