alp_payload.cpp

00001 /// @copyright
00002 /// ========================================================================={{{
00003 /// Copyright (c) 2012-2017 WizziLab                                           /
00004 /// All rights reserved                                                        /
00005 ///                                                                            /
00006 /// Should you have any questions regarding your right to use this Software,   /
00007 /// contact WizziLab at www.wizzilab.com.                                      /
00008 ///                                                                            /
00009 /// =========================================================================}}}
00010 /// @endcopyright
00011 
00012 //  ====================================================================
00013 //  @file           alp_helpers.c
00014 //  @brief          ALP helpers functions.
00015 //                  This code should be disclosable in source.
00016 //  ====================================================================
00017 
00018 
00019 #include "alp.h"
00020 #include "alp_dbg.h"
00021 #include "kal_math.h"
00022 #include "kal_crypto.h"
00023 #include "d7a_1x_fs.h"
00024 
00025 #include "WizziDebug.h"
00026 
00027 static int32_t g_payload_nb = 0;
00028 
00029 alp_payload_t* alp_payload_new(u32 size)
00030 {
00031     alp_payload_t* alp = (alp_payload_t*)MALLOC(sizeof(alp_payload_t) - 1 + size);
00032 
00033     alp->len = 0;
00034     alp->next = NULL;
00035     
00036     g_payload_nb++;
00037 
00038     return alp;
00039 }
00040 
00041 void alp_payload_free(alp_payload_t* alp)
00042 {
00043     while (NULL != alp)
00044     {
00045         alp_payload_t* alp_next = alp->next;   
00046         FREE(alp);
00047         
00048         g_payload_nb--;
00049 
00050         alp = alp_next;
00051     }
00052 }
00053 
00054 void alp_payload_print_nb(void)
00055 {
00056     PRINT("ALP: %d payloads assigned\n", g_payload_nb);
00057 }
00058 
00059 //======================================================================
00060 // alp_payload_append
00061 //----------------------------------------------------------------------
00062 /// @brief  Appends ALP payload 2 after payload 1
00063 /// @param  alp_payload_t*          Payload 1
00064 /// @param  alp_payload_t*          Payload 2
00065 /// @return alp_payload_t*          Appended payloads
00066 /// @note: XXX payloads MUST be malloced
00067 //======================================================================
00068 alp_payload_t* alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
00069 {
00070     alp_payload_t* alp = alp_1;
00071     
00072     if (NULL == alp_1)
00073     {
00074         return alp_2;
00075     }
00076 
00077     if (NULL == alp_2)
00078     {
00079         return alp_1;
00080     }
00081     
00082     while (NULL != alp)
00083     {
00084         if (NULL == alp->next)
00085         {
00086             alp->next = alp_2;
00087             return alp_1;
00088         }
00089         
00090         alp = alp->next;
00091     }
00092 
00093     return alp_1;
00094 }
00095 
00096 void alp_payload_print(alp_payload_t* alp)
00097 {
00098     u32 action = 0;
00099     u32 i;
00100     
00101     while (NULL != alp)
00102     {
00103         PRINT("ALP[%d]: (0x%x)", action, alp);
00104         PRINT_DATA("", " %02X", alp->d, alp->len, "\n");
00105         FLUSH();
00106         
00107         action++;
00108         
00109         alp = alp->next;
00110     }
00111     
00112     PRINT("---\n");
00113 }
00114 
00115 u32 alp_payload_to_buf(alp_payload_t* alp, u8* buf, u32 max)
00116 {
00117     u32 offset = 0;
00118     
00119     while (NULL != alp)
00120     {
00121         // End of payload
00122         ALP_ASSERT((offset + alp->len) <= max, "ALP payload too big for buffer (%d\%d)\n", offset + alp->len, max);
00123         
00124         // Copy into buffer
00125         memcpy(&(buf[offset]), alp->d, alp->len);
00126         offset += alp->len;
00127         
00128         alp = alp->next;
00129     }
00130     
00131     return offset;
00132 }
00133 
00134 //======================================================================
00135 // alp_payload_root_auth
00136 //----------------------------------------------------------------------
00137 /// @brief  Add the root access request to the given payload using the authentication protocol
00138 /// @param alp          alp_payload_t*          Payload
00139 /// @return             alp_payload_t*          Root access request + payload
00140 /// @note: XXX payloads MUST be malloced
00141 //======================================================================
00142 alp_payload_t* alp_payload_root_auth(alp_payload_t* alp, u8* root_key)
00143 {
00144     u8* p;
00145     alp_payload_t* new_alp;
00146     alp_payload_t* alp_hash = alp;
00147     u8 hash[32];
00148 
00149     // Calculate hash on payload
00150     kal_sha256_init();
00151     while (NULL != alp_hash)
00152     {
00153         kal_sha256_update(alp_hash->d, alp_hash->len);
00154         alp_hash = alp_hash->next;
00155     }
00156     kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
00157     kal_sha256_final(hash);
00158 
00159     new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));
00160 
00161     p = new_alp->d;
00162     ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_AUTH_PROTOCOL_ID);
00163 
00164     memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
00165     p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
00166 
00167     new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
00168 
00169     // XXX Prepend
00170     return alp_payload_append(new_alp, alp);
00171 }
00172 
00173 //======================================================================
00174 // alp_payload_root_sign
00175 //----------------------------------------------------------------------
00176 /// @brief  Sign payload with root key
00177 /// @param alp          alp_payload_t*          Payload
00178 /// @param iv           u8*                         Initialization vector
00179 /// @return             alp_payload_t*          Root access request + payload
00180 /// @note: XXX payloads MUST be malloced
00181 //======================================================================
00182 alp_payload_t* alp_payload_root_sign(alp_payload_t* alp, u8* root_key, u8* iv)
00183 {
00184     u8* p;
00185     alp_payload_t* new_alp;
00186     alp_payload_t* alp_hash = alp;
00187     u8 hash[32];
00188 
00189     // Calculate hash on payload
00190     kal_sha256_init();
00191     while (NULL != alp_hash)
00192     {
00193         kal_sha256_update(alp_hash->d, alp_hash->len);
00194         alp_hash = alp_hash->next;
00195     }
00196     kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
00197     kal_sha256_update(iv, ALP_ACTION_SECURED_IV_SIZE);
00198     kal_sha256_final(hash);
00199 
00200     new_alp = alp_payload_new(ALP_ACTION_SECURED_SIZE(0));
00201 
00202     p = new_alp->d;
00203     ALP_ACTION_SECURED(p, ALP_SECURED_ROOT, ALP_WIZZILAB_SIGN_PROTOCOL_ID);
00204 
00205     memcpy(p, iv, ALP_ACTION_SECURED_IV_SIZE);
00206     p += ALP_ACTION_SECURED_IV_SIZE;
00207 
00208     memcpy(p, hash, ALP_ACTION_SECURED_TOKEN_SIZE);
00209     p += ALP_ACTION_SECURED_TOKEN_SIZE;
00210 
00211     new_alp->len = ALP_ACTION_SECURED_SIZE(0);
00212 
00213     // XXX Prepend
00214     return alp_payload_append(new_alp, alp);
00215 }
00216 
00217 #if 0
00218 //======================================================================
00219 // alp_payload_root_auth_enc
00220 //----------------------------------------------------------------------
00221 /// @brief  Add the root access request to the given payload and encrypt using the challenge protocol
00222 /// @param alp          alp_payload_t*          Payload
00223 /// @param challenge    u8*                         32-byte challenge
00224 /// @return             alp_payload_t*          Root access request + encrypted payload
00225 /// @note: XXX payloads MUST be malloced
00226 //======================================================================
00227 alp_payload_t* alp_payload_root_auth_enc(alp_payload_t* alp, u8* root_key, u8* challenge)
00228 {
00229     u8* p;
00230     alp_payload_t* new_alp;
00231     alp_payload_t* alp_hash = alp;
00232     u8 hash[32];
00233 
00234     // encrypt
00235     u8 key[D7A_FS_ROOT_KEY_SIZE];
00236     memcpy(key, root_key, D7A_FS_ROOT_KEY_SIZE);
00237     kal_xor(key, challenge, D7A_FS_ROOT_KEY_SIZE);
00238     kal_aes128_init(key);
00239     kal_aes128_ctr(alp->d, alp->len, challenge + D7A_FS_ROOT_KEY_SIZE, TRUE);
00240 
00241     // Calculate hash on payload
00242     kal_sha256_init();
00243     while (NULL != alp_hash)
00244     {
00245         kal_sha256_update(alp_hash->d, alp_hash->len);
00246         alp_hash = alp_hash->next;
00247     }
00248     kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
00249     kal_sha256_update(challenge, 32);
00250     kal_sha256_final(hash);
00251 
00252     new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));
00253 
00254     p = new_alp->d;
00255 
00256     ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_CHAL_PROTOCOL_ID);
00257     memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
00258     p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;
00259 
00260     new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);
00261 
00262     // XXX Prepend
00263     return alp_payload_append(new_alp, alp);
00264 }
00265 #endif
00266 
00267 //======================================================================
00268 // alp_payload_rsp_f_data
00269 //----------------------------------------------------------------------
00270 /// @brief  Creates malloc'ed ALP payload
00271 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00272 /// @param  fid             u8                  file identifier
00273 /// @param  data            void*               file content
00274 /// @param  offset          u32                 file offset
00275 /// @param  length          u32                 file length
00276 /// @return                 alp_payload_t*  New ALP payload
00277 /// @revent                 NONE
00278 //======================================================================
00279 alp_payload_t* alp_payload_rsp_f_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length)
00280 {
00281     u8* p;
00282     alp_payload_t* new_alp;
00283 
00284     new_alp = alp_payload_new(ALP_ACTION_RSP_F_DATA_SIZE(offset, length));
00285 
00286     p = new_alp->d;
00287 
00288     ALP_ACTION_RSP_F_DATA(p, fid, offset, length, data);
00289 
00290     new_alp->len = (u32)(p - new_alp->d);
00291 
00292     return alp_payload_append(alp, new_alp);
00293 }
00294 
00295 //======================================================================
00296 // alp_payload_qbreak
00297 //----------------------------------------------------------------------
00298 /// @brief  Creates malloc'ed ALP payload
00299 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00300 /// @param  fid             u8                  file identifier
00301 /// @param  data            void*               file content
00302 /// @param  offset          u32                 file offset
00303 /// @param  length          u32                 file length
00304 /// @param  group           u8                  group with next OP
00305 /// @return                 alp_payload_t*  New ALP payload
00306 /// @revent                 NONE
00307 //======================================================================
00308 alp_payload_t* alp_payload_qbreak(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
00309 {
00310     u8* p;
00311     alp_payload_t* new_alp;
00312     u8 mask = 0;
00313 
00314     new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, mask));
00315 
00316     p = new_alp->d;
00317 
00318     ALP_ACTION_QBREAK_STRTOK(p, mask, data, fid, offset, length, group);
00319 
00320     new_alp->len = (u32)(p - new_alp->d);
00321 
00322     return alp_payload_append(alp, new_alp);
00323 }
00324 
00325 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)
00326 {
00327     u8* p;
00328     alp_payload_t* new_alp;
00329 
00330     new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, (mask)? 1 : 0));
00331 
00332     p = new_alp->d;
00333 
00334     ALP_ACTION_QBREAK_COMP(p, mask, data, comp, fid, offset, length, group);
00335 
00336     new_alp->len = (u32)(p - new_alp->d);
00337 
00338     return alp_payload_append(alp, new_alp);
00339 }
00340 
00341 alp_payload_t* alp_payload_qbreak_eq(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
00342 {
00343     return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_EQ);
00344 }
00345 
00346 alp_payload_t* alp_payload_qbreak_ne(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
00347 {
00348     return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_NE);
00349 }
00350 
00351 alp_payload_t* alp_payload_qbreak_gt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
00352 {
00353     return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GT);
00354 }
00355 
00356 alp_payload_t* alp_payload_qbreak_gte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
00357 {
00358     return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GTE);
00359 }
00360 
00361 alp_payload_t* alp_payload_qbreak_lt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
00362 {
00363     return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LT);
00364 }
00365 
00366 alp_payload_t* alp_payload_qbreak_lte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
00367 {
00368     return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LTE);
00369 }
00370 
00371 alp_payload_t* alp_payload_qbreak_eq_msk(alp_payload_t* alp, u8 fid, u32 mask, u32 data, u32 offset, u32 length, u8 group)
00372 {
00373     return _alp_payload_qbreak_comp(alp, fid, mask, data, offset, length, group, ALP_QCOMP_EQ);
00374 }
00375 
00376 //======================================================================
00377 // alp_payload_query
00378 //----------------------------------------------------------------------
00379 /// @brief  Creates malloc'ed ALP payload
00380 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00381 /// @param  fid             u8                  file identifier
00382 /// @param  data            void*               file content
00383 /// @param  offset          u32                 file offset
00384 /// @param  length          u32                 file length
00385 /// @param  group           u8                  group with next OP
00386 /// @return                 alp_payload_t*  New ALP payload
00387 /// @revent                 NONE
00388 //======================================================================
00389 alp_payload_t* alp_payload_query(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
00390 {
00391     u8* p;
00392     alp_payload_t* new_alp;
00393     u8 mask = 0;
00394 
00395     new_alp = alp_payload_new(ALP_ACTION_QUERY_SIZE(offset, length, mask));
00396 
00397     p = new_alp->d;
00398 
00399     ALP_ACTION_QUERY_STRTOK(p, mask, data, fid, offset, length, group);
00400 
00401     new_alp->len = (u32)(p - new_alp->d);
00402 
00403     return alp_payload_append(alp, new_alp);
00404 }
00405 
00406 //======================================================================
00407 // alp_payload_nop
00408 //----------------------------------------------------------------------
00409 /// @brief  Creates malloc'ed ALP payload
00410 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00411 /// @return                 alp_payload_t*  New ALP payload
00412 /// @revent                 NONE
00413 //======================================================================
00414 alp_payload_t* alp_payload_nop(alp_payload_t* alp, u8 group)
00415 {
00416     u8* p;
00417     alp_payload_t* new_alp;
00418 
00419     new_alp = alp_payload_new(ALP_ACTION_NOP_SIZE);
00420 
00421     p = new_alp->d;
00422 
00423     ALP_ACTION_NOP(p, true, group); // XXX RESP always true. If we use NOP, it's to get a response (istatus).
00424 
00425     new_alp->len = (u32)(p - new_alp->d);
00426 
00427     return alp_payload_append(alp, new_alp);
00428 }
00429 
00430 //======================================================================
00431 // alp_payload_f_touch
00432 //----------------------------------------------------------------------
00433 /// @brief  Creates malloc'ed ALP payload
00434 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00435 /// @return                 alp_payload_t*  New ALP payload
00436 /// @revent                 NONE
00437 //======================================================================
00438 alp_payload_t* alp_payload_f_touch(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
00439 {
00440     u8* p;
00441     alp_payload_t* new_alp;
00442 
00443     new_alp = alp_payload_new(ALP_ACTION_F_TOUCH_SIZE(offset, length));
00444 
00445     p = new_alp->d;
00446 
00447     ALP_ACTION_F_TOUCH(p, true, fid, offset, length, group);
00448 
00449     new_alp->len = (u32)(p - new_alp->d);
00450 
00451     return alp_payload_append(alp, new_alp);
00452 }
00453 
00454 //======================================================================
00455 // alp_payload_f_wr_data
00456 //----------------------------------------------------------------------
00457 /// @brief  Creates malloc'ed ALP payload
00458 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00459 /// @param  fid             u8                  file identifier
00460 /// @param  data            void*               file content
00461 /// @param  offset          u32                 file offset
00462 /// @param  length          u32                 file length
00463 /// @param  group           u8                  group with next OP
00464 /// @return                 alp_payload_t*  New ALP payload
00465 /// @revent                 NONE
00466 //======================================================================
00467 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)
00468 {
00469     u8* p;
00470     alp_payload_t* new_alp;
00471 
00472     new_alp = alp_payload_new(ALP_ACTION_F_WR_DATA_SIZE(offset, length));
00473 
00474     p = new_alp->d;
00475 
00476     ALP_ACTION_F_WR_DATA(p, resp, fid, offset, length, data, group);
00477 
00478     new_alp->len = (u32)(p - new_alp->d);
00479 
00480     return alp_payload_append(alp, new_alp);
00481 }
00482 
00483 alp_payload_t* alp_payload_f_wr_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
00484 {
00485     return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, true);
00486 }
00487 
00488 alp_payload_t* alp_payload_f_wr_data_nr(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
00489 {
00490     return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, false);
00491 }
00492 
00493 //======================================================================
00494 // alp_payload_f_rd_data
00495 //----------------------------------------------------------------------
00496 /// @brief  Creates malloc'ed ALP payload
00497 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00498 /// @param  fid             u8                  file identifier
00499 /// @param  offset          u32                 file offset
00500 /// @param  length          u32                 file length
00501 /// @param  group           u8                  group with next OP
00502 /// @return                 alp_payload_t*  New ALP payload
00503 /// @revent                 NONE
00504 //======================================================================
00505 alp_payload_t* alp_payload_f_rd_data(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
00506 {
00507     u8* p;
00508     alp_payload_t* new_alp;
00509 
00510     new_alp = alp_payload_new(ALP_ACTION_F_RD_DATA_SIZE(offset, length));
00511 
00512     p = new_alp->d;
00513 
00514     ALP_ACTION_F_RD_DATA(p, true, fid, offset, length, group);
00515 
00516     new_alp->len = (u32)(p - new_alp->d);
00517 
00518     return alp_payload_append(alp, new_alp);
00519 }
00520 
00521 //======================================================================
00522 // alp_payload_f_flush
00523 //----------------------------------------------------------------------
00524 /// @brief  Creates malloc'ed ALP payload
00525 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00526 /// @param  fid             u8                  file identifier
00527 /// @param  group           u8                  group with next OP
00528 /// @return                 alp_payload_t*  New ALP payload
00529 /// @revent                 NONE
00530 //======================================================================
00531 alp_payload_t* alp_payload_f_flush(alp_payload_t* alp, u8 fid, u8 group)
00532 {
00533     u8* p;
00534     alp_payload_t* new_alp;
00535 
00536     new_alp = alp_payload_new(ALP_ACTION_F_FLUSH_SIZE);
00537 
00538     p = new_alp->d;
00539 
00540     ALP_ACTION_F_FLUSH(p, true, fid, group);
00541 
00542     new_alp->len = (u32)(p - new_alp->d);
00543 
00544     return alp_payload_append(alp, new_alp);
00545 }
00546 
00547 
00548 //======================================================================
00549 // alp_payload_f_flush
00550 //----------------------------------------------------------------------
00551 /// @brief  Creates malloc'ed ALP payload
00552 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00553 /// @param  fid             u8                  file identifier
00554 /// @param  group           u8                  group with next OP
00555 /// @return                 alp_payload_t*  New ALP payload
00556 /// @revent                 NONE
00557 //======================================================================
00558 alp_payload_t* alp_payload_f_declare(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
00559 {
00560     u8* p;
00561     alp_payload_t* new_alp;
00562 
00563     new_alp = alp_payload_new(ALP_ACTION_F_DECLARE_SIZE);
00564 
00565     p = new_alp->d;
00566 
00567     ALP_ACTION_F_DECLARE(p, true, fid, hdr, true);
00568 
00569     new_alp->len = (u32)(p - new_alp->d);
00570 
00571     return alp_payload_append(alp, new_alp);
00572 }
00573 
00574 //======================================================================
00575 // alp_payload_forward
00576 //----------------------------------------------------------------------
00577 /// @brief  Creates malloc'ed ALP payload
00578 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00579 /// @param  itf             void*               Forward interface
00580 /// @return                 alp_payload_t*  New ALP payload
00581 /// @revent                 NONE
00582 //======================================================================
00583 alp_payload_t* alp_payload_forward(alp_payload_t* alp, void* itf)
00584 {
00585     u8* p;
00586     alp_payload_t* new_alp;
00587     u32 itf_length = alp_itf_size(itf);
00588 
00589     new_alp = alp_payload_new(ALP_ACTION_FORWARD_SIZE(itf_length));
00590 
00591     p = new_alp->d;
00592 
00593     ALP_ACTION_FORWARD(p, itf, itf_length);
00594 
00595     new_alp->len = (u32)(p - new_alp->d);
00596 
00597     return alp_payload_append(alp, new_alp);
00598 }
00599 
00600 //======================================================================
00601 // alp_payload_tag
00602 //----------------------------------------------------------------------
00603 /// @brief  Creates malloc'ed ALP payload
00604 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00605 /// @param  tag             u8                  Tag value
00606 /// @return                 alp_payload_t*  New ALP payload
00607 /// @revent                 NONE
00608 //======================================================================
00609 alp_payload_t* alp_payload_tag(alp_payload_t* alp, u8 tag)
00610 {
00611     u8* p;
00612     alp_payload_t* new_alp;
00613 
00614     new_alp = alp_payload_new(ALP_ACTION_TAG_SIZE);
00615 
00616     p = new_alp->d;
00617 
00618     ALP_ACTION_TAG(p, tag, true);
00619 
00620     new_alp->len = (u32)(p - new_alp->d);
00621 
00622     return alp_payload_append(alp, new_alp);
00623 }
00624 
00625 //======================================================================
00626 // alp_payload_tag
00627 //----------------------------------------------------------------------
00628 /// @brief  Creates malloc'ed ALP payload
00629 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00630 /// @param  tag             u8                  Tag value
00631 /// @return                 alp_payload_t*  New ALP payload
00632 /// @revent                 NONE
00633 //======================================================================
00634 alp_payload_t* alp_payload_rsp_tag(alp_payload_t* alp, u8 tag, u8 eop, u8 err)
00635 {
00636     u8* p;
00637     alp_payload_t* new_alp;
00638 
00639     new_alp = alp_payload_new(ALP_ACTION_RSP_TAG_SIZE);
00640 
00641     p = new_alp->d;
00642 
00643     ALP_ACTION_RSP_TAG(p, tag, eop, err);
00644 
00645     new_alp->len = (u32)(p - new_alp->d);
00646 
00647     return alp_payload_append(alp, new_alp);
00648 }
00649 
00650 //======================================================================
00651 // alp_payload_tag
00652 //----------------------------------------------------------------------
00653 /// @brief  Creates malloc'ed ALP payload
00654 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00655 /// @param  tag             u8                  Action index
00656 /// @param  tag             s8                  Status
00657 /// @return                 alp_payload_t*  New ALP payload
00658 /// @revent                 NONE
00659 //======================================================================
00660 alp_payload_t* alp_payload_rsp_status(alp_payload_t* alp, u8 action, s8 status)
00661 {
00662     u8* p;
00663     alp_payload_t* new_alp;
00664 
00665     new_alp = alp_payload_new(ALP_ACTION_RSP_STATUS_SIZE);
00666 
00667     p = new_alp->d;
00668 
00669     ALP_ACTION_RSP_STATUS(p, action, status);
00670 
00671     new_alp->len = (u32)(p - new_alp->d);
00672 
00673     return alp_payload_append(alp, new_alp);
00674 }
00675 
00676 //======================================================================
00677 // alp_payload_tag
00678 //----------------------------------------------------------------------
00679 /// @brief  Creates malloc'ed ALP payload
00680 /// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
00681 /// @param  tag             u8                  Action index
00682 /// @param  tag             s8                  Status
00683 /// @return                 alp_payload_t*  New ALP payload
00684 /// @revent                 NONE
00685 //======================================================================
00686 alp_payload_t* alp_payload_rsp_fprop(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
00687 {
00688     u8* p;
00689     alp_payload_t* new_alp;
00690 
00691     new_alp = alp_payload_new(ALP_ACTION_RSP_F_PROP_SIZE);
00692 
00693     p = new_alp->d;
00694 
00695     ALP_ACTION_RSP_F_PROP(p, fid, hdr);
00696 
00697     new_alp->len = (u32)(p - new_alp->d);
00698 
00699     return alp_payload_append(alp, new_alp);
00700 }
00701 
00702 alp_payload_t* alp_payload_activate_itf(alp_payload_t* alp, u8 type, u8 nb_dev, u8 ifid, u8 flags, u8 enable)
00703 {
00704     u8* p;
00705     alp_payload_t* new_alp;
00706 
00707     new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);
00708 
00709     p = new_alp->d;
00710 
00711     ALP_ACTION_ACTIVATE_ITF(p, true, enable, type, nb_dev, ifid, flags);
00712 
00713     new_alp->len = (u32)(p - new_alp->d);
00714 
00715     return alp_payload_append(alp, new_alp);
00716 }
00717 
00718 alp_payload_t* alp_payload_urcc_en(alp_payload_t* alp, u8 type, u8 ifid, u8 val)
00719 {
00720     u8* p;
00721     alp_payload_t* new_alp;
00722 
00723     new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);
00724 
00725     p = new_alp->d;
00726 
00727     ALP_ACTION_URCC_EN(p, true, type, ifid, val);
00728 
00729     new_alp->len = (u32)(p - new_alp->d);
00730 
00731     return alp_payload_append(alp, new_alp);
00732 }
00733 
00734 alp_payload_t* alp_payload_extract(alp_payload_t** alp, u8 op)
00735 {
00736     if (NULL == *alp)
00737     {
00738         return NULL;
00739     }
00740 
00741     if ((*alp)->d[0] == op)
00742     {
00743         // Save alp
00744         alp_payload_t* alp_old = *alp;
00745 
00746         // Remove it from chain
00747         *alp = alp_old->next;
00748         alp_old->next = NULL;
00749 
00750         // Return it
00751         return alp_old;
00752     }
00753 
00754     return alp_payload_extract((alp_payload_t**)&((*alp)->next), op);
00755 }
00756 
00757 alp_payload_t* alp_payload_get(alp_payload_t* alp, u8 op)
00758 {
00759     while (NULL != alp)
00760     {
00761         if (alp->d[0] == op)
00762         {
00763             // Return it
00764             return alp;
00765         }
00766         
00767         alp = alp->next;
00768     }
00769     
00770     return NULL;
00771 }
00772 
00773 bool alp_payload_extract_data(alp_payload_t** alp, u8 op, void* data)
00774 {
00775     alp_payload_t* alp_data = alp_payload_extract(alp, op);
00776 
00777     if (alp_data)
00778     {
00779         alp_parsed_chunk_t r;
00780         
00781         alp_payload_parse_chunk(alp_data, &r);
00782     
00783         switch (op)
00784         {
00785             case ALP_OPCODE_RSP_ISTATUS:
00786                 memcpy(data, r.data, r.meta.itf.length);
00787                 break;;
00788             case ALP_OPCODE_RSP_F_DATA:
00789                 memcpy(data, r.data, r.meta.f_data.length);
00790                 break;
00791             default:
00792                 alp_payload_free(alp_data);
00793                 return false;
00794                 break;
00795         }
00796     
00797         alp_payload_free(alp_data);
00798         
00799         return true;
00800     }
00801     
00802     return false;
00803 }
00804 
00805 enum {
00806     ERR_PRIO_EOPISTATUS,
00807     ERR_PRIO_STATUS,
00808     ERR_PRIO_TAG,
00809     ERR_PRIO_NONE,
00810 };
00811 
00812 int alp_payload_get_err(alp_payload_t* alp)
00813 {
00814     int err = ALP_ERR_NONE;
00815     uint8_t err_prio = ERR_PRIO_NONE;
00816     alp_parsed_chunk_t r;
00817 
00818     while (NULL != alp)
00819     {
00820         switch (alp->d[0])
00821         {
00822         case ALP_OPCODE_RSP_TAG: // Fallthrough
00823         case ALP_OPCODE_RSP_EOPTAG: // Fallthrough
00824         case ALP_OPCODE_RSP_ERRTAG:
00825         {
00826             alp_payload_parse_chunk(alp, &r);
00827             if (ERR_PRIO_TAG < err_prio && r.meta.tag.err)
00828             {
00829                 err = ALP_ERR_UNKNOWN;
00830                 err_prio = ERR_PRIO_TAG;
00831             }
00832             break;
00833         }
00834         case ALP_OPCODE_RSP_STATUS:
00835         {
00836             alp_payload_parse_chunk(alp, &r);
00837             if (ERR_PRIO_STATUS < err_prio && r.meta.status.code < ALP_ERR_NONE)
00838             {
00839                 err = r.meta.status.code;
00840                 err_prio = ERR_PRIO_STATUS;
00841             }
00842             break;
00843         }
00844         case ALP_OPCODE_RSP_EOPISTATUS:
00845         {
00846             alp_payload_parse_chunk(alp, &r);
00847             if (ERR_PRIO_EOPISTATUS < err_prio && r.meta.istatus.err < ALP_ERR_NONE)
00848             {
00849                 err = r.meta.istatus.err + ALP_ERR_ITF_START;
00850                 err_prio = ERR_PRIO_EOPISTATUS;
00851             }
00852             break;
00853         }
00854         default:
00855             break;
00856         }
00857         
00858         alp = alp->next;
00859     }
00860 
00861     return err;
00862 }
00863 
00864 int alp_payload_parse_chunk(alp_payload_t* alp, alp_parsed_chunk_t* r)
00865 {
00866     if (NULL == alp)
00867     {
00868         return 0;
00869     }
00870     
00871     uint8_t* p = alp->d;
00872     return alp_parse_chunk(&p, r);
00873 }
00874 
00875 alp_payload_t* alp_payload_parse(u8* d, int length)
00876 {
00877     u8* data_start;
00878     int len;
00879     alp_parsed_chunk_t r;
00880     alp_payload_t* alp = NULL;
00881     alp_payload_t* alp_new = NULL;
00882     
00883     while (length > 0)
00884     {
00885         data_start = d;
00886         len = (int)alp_parse_chunk(&d, &r);
00887         
00888         // Malloc payload
00889         alp_new = alp_payload_new(len);
00890     
00891         // Fill payload
00892         alp_new->len = len;
00893         memcpy(alp_new->d, data_start, len);
00894         
00895         alp = alp_payload_append(alp, alp_new);
00896         
00897         length -= len;
00898     }
00899 
00900     return alp;
00901 }