File content as of revision 59:3b38b5f499db:

/// @copyright
/// ========================================================================={{{
/// Copyright (c) 2012-2017 WizziLab                                           /
/// All rights reserved                                                        /
///                                                                            /
/// Should you have any questions regarding your right to use this Software,   /
/// contact WizziLab at www.wizzilab.com.                                      /
///                                                                            /
/// =========================================================================}}}
/// @endcopyright

//  ====================================================================
//  @file           alp_helpers.c
//  @brief          ALP helpers functions.
//                  This code should be disclosable in source.
//  ====================================================================


#include "alp.h"
#include "alp_dbg.h"
#include "kal_math.h"
#include "kal_crypto.h"
#include "d7a_1x_fs.h"

#include "WizziDebug.h"

alp_payload_t* alp_payload_new(u32 size)
{
    alp_payload_t* alp = (alp_payload_t*)MALLOC(sizeof(alp_payload_t) - 1 + size);

    alp->len = 0;
    alp->next = NULL;

    return alp;
}

void alp_payload_free(alp_payload_t* alp)
{    
    if (NULL == alp)
    {
        return;
    }

    alp_payload_t* alp_next = alp->next;
    
    FREE(alp);

    alp_payload_free(alp_next);
}

//======================================================================
// alp_payload_append
//----------------------------------------------------------------------
/// @brief  Appends ALP payload 2 after payload 1
/// @param  alp_payload_t*          Payload 1
/// @param  alp_payload_t*          Payload 2
/// @return alp_payload_t*          Appended payloads
/// @note: XXX payloads MUST be malloced
//======================================================================
static void _alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
{
    if (NULL == alp_1->next)
    {
        // Append payload
        alp_1->next = alp_2;

        return;
    }

    _alp_payload_append(alp_1->next, alp_2);
}

alp_payload_t* alp_payload_append(alp_payload_t* alp_1, alp_payload_t* alp_2)
{    
    if (NULL == alp_1)
    {
        return alp_2;
    }

    if (NULL == alp_2)
    {
        return alp_1;
    }

    _alp_payload_append(alp_1, alp_2);
    
    return alp_1;
}

static void _alp_payload_print(alp_payload_t* alp, u32 action)
{
    if (NULL == alp)
    {
        return;
    }

    u32 i;
    PRINT("ALP[%d]: ", action);
    for (i = 0; i < alp->len; i++)
    {
        PRINT("%02X ", alp->d[i]);
    }
    PRINT("\n");
    FLUSH();

    _alp_payload_print(alp->next, ++action);
}

void alp_payload_print(alp_payload_t* alp)
{
    _alp_payload_print(alp, 0);
}

u32 alp_payload_to_buf(alp_payload_t* alp, u8* buf, u32 offset, u32 max)
{
    if (NULL == alp)
    {
        // End of payload
        ALP_ASSERT((offset <= max), "ALP payload too big for buffer (%d\%d)\n", offset, max);
        return offset;
    }
    
    if ((offset + alp->len) <= max)
    {
        // Copy into buffer
        memcpy(&(buf[offset]), alp->d, alp->len);
    }
    
    // Next
    return alp_payload_to_buf(alp->next, buf, offset+alp->len, max);
}

//======================================================================
// alp_payload_root_auth
//----------------------------------------------------------------------
/// @brief  Add the root access request to the given payload using the authentication protocol
/// @param alp          alp_payload_t*          Payload
/// @return             alp_payload_t*          Root access request + payload
/// @note: XXX payloads MUST be malloced
//======================================================================
alp_payload_t* alp_payload_root_auth(alp_payload_t* alp, u8* root_key)
{
    u8* p;
    alp_payload_t* new_alp;
    alp_payload_t* alp_hash = alp;
    u8 hash[32];

    // Calculate hash on payload
    kal_sha256_init();
    while (alp_hash != NULL)
    {
        kal_sha256_update(alp_hash->d, alp_hash->len);
        alp_hash = alp_hash->next;
    }
    kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
    kal_sha256_final(hash);

    new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));

    p = new_alp->d;
    ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_AUTH_PROTOCOL_ID);

    memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
    p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;

    new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);

    // XXX Prepend
    return alp_payload_append(new_alp, alp);
}

//======================================================================
// alp_payload_root_sign
//----------------------------------------------------------------------
/// @brief  Sign payload with root key
/// @param alp          alp_payload_t*          Payload
/// @param iv           u8*                         Initialization vector
/// @return             alp_payload_t*          Root access request + payload
/// @note: XXX payloads MUST be malloced
//======================================================================
alp_payload_t* alp_payload_root_sign(alp_payload_t* alp, u8* root_key, u8* iv)
{
    u8* p;
    alp_payload_t* new_alp;
    alp_payload_t* alp_hash = alp;
    u8 hash[32];

    // Calculate hash on payload
    kal_sha256_init();
    while (alp_hash != NULL)
    {
        kal_sha256_update(alp_hash->d, alp_hash->len);
        alp_hash = alp_hash->next;
    }
    kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
    kal_sha256_update(iv, ALP_ACTION_SECURED_IV_SIZE);
    kal_sha256_final(hash);

    new_alp = alp_payload_new(ALP_ACTION_SECURED_SIZE(0));

    p = new_alp->d;
    ALP_ACTION_SECURED(p, ALP_SECURED_ROOT, ALP_WIZZILAB_SIGN_PROTOCOL_ID);

    memcpy(p, iv, ALP_ACTION_SECURED_IV_SIZE);
    p += ALP_ACTION_SECURED_IV_SIZE;

    memcpy(p, hash, ALP_ACTION_SECURED_TOKEN_SIZE);
    p += ALP_ACTION_SECURED_TOKEN_SIZE;

    new_alp->len = ALP_ACTION_SECURED_SIZE(0);

    // XXX Prepend
    return alp_payload_append(new_alp, alp);
}

#if 0
//======================================================================
// alp_payload_root_auth_enc
//----------------------------------------------------------------------
/// @brief  Add the root access request to the given payload and encrypt using the challenge protocol
/// @param alp          alp_payload_t*          Payload
/// @param challenge    u8*                         32-byte challenge
/// @return             alp_payload_t*          Root access request + encrypted payload
/// @note: XXX payloads MUST be malloced
//======================================================================
alp_payload_t* alp_payload_root_auth_enc(alp_payload_t* alp, u8* root_key, u8* challenge)
{
    u8* p;
    alp_payload_t* new_alp;
    alp_payload_t* alp_hash = alp;
    u8 hash[32];

    // encrypt
    u8 key[D7A_FS_ROOT_KEY_SIZE];
    memcpy(key, root_key, D7A_FS_ROOT_KEY_SIZE);
    kal_xor(key, challenge, D7A_FS_ROOT_KEY_SIZE);
    kal_aes128_init(key);
    kal_aes128_ctr(alp->d, alp->len, challenge + D7A_FS_ROOT_KEY_SIZE, TRUE);

    // Calculate hash on payload
    kal_sha256_init();
    while (alp_hash != NULL)
    {
        kal_sha256_update(alp_hash->d, alp_hash->len);
        alp_hash = alp_hash->next;
    }
    kal_sha256_update(root_key, D7A_FS_ROOT_KEY_SIZE);
    kal_sha256_update(challenge, 32);
    kal_sha256_final(hash);

    new_alp = alp_payload_new(ALP_ACTION_PERM_REQ_SIZE(0));

    p = new_alp->d;

    ALP_ACTION_PERM_REQ(p, FALSE, ALP_PERM_REQ_ROOT, ALP_WIZZILAB_CHAL_PROTOCOL_ID);
    memcpy(p, hash, ALP_ACTION_PERM_REQ_TOKEN_SIZE);
    p += ALP_ACTION_PERM_REQ_TOKEN_SIZE;

    new_alp->len = ALP_ACTION_PERM_REQ_SIZE(0);

    // XXX Prepend
    return alp_payload_append(new_alp, alp);
}
#endif

//======================================================================
// alp_payload_rsp_f_data
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  data            void*               file content
/// @param  offset          u32                 file offset
/// @param  length          u32                 file length
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_rsp_f_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_RSP_F_DATA_SIZE(offset, length));

    p = new_alp->d;

    ALP_ACTION_RSP_F_DATA(p, fid, offset, length, data);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_qbreak
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  data            void*               file content
/// @param  offset          u32                 file offset
/// @param  length          u32                 file length
/// @param  group           u8                  group with next OP
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_qbreak(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
{
    u8* p;
    alp_payload_t* new_alp;
    u8 mask = 0;

    new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, mask));

    p = new_alp->d;

    ALP_ACTION_QBREAK_STRTOK(p, mask, data, fid, offset, length, group);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

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)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_QBREAK_SIZE(offset, length, (mask)? 1 : 0));

    p = new_alp->d;

    ALP_ACTION_QBREAK_COMP(p, mask, data, comp, fid, offset, length, group);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

alp_payload_t* alp_payload_qbreak_eq(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_EQ);
}

alp_payload_t* alp_payload_qbreak_ne(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_NE);
}

alp_payload_t* alp_payload_qbreak_gt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GT);
}

alp_payload_t* alp_payload_qbreak_gte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_GTE);
}

alp_payload_t* alp_payload_qbreak_lt(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LT);
}

alp_payload_t* alp_payload_qbreak_lte(alp_payload_t* alp, u8 fid, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, 0, data, offset, length, group, ALP_QCOMP_LTE);
}

alp_payload_t* alp_payload_qbreak_eq_msk(alp_payload_t* alp, u8 fid, u32 mask, u32 data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_qbreak_comp(alp, fid, mask, data, offset, length, group, ALP_QCOMP_EQ);
}

//======================================================================
// alp_payload_query
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  data            void*               file content
/// @param  offset          u32                 file offset
/// @param  length          u32                 file length
/// @param  group           u8                  group with next OP
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_query(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
{
    u8* p;
    alp_payload_t* new_alp;
    u8 mask = 0;

    new_alp = alp_payload_new(ALP_ACTION_QUERY_SIZE(offset, length, mask));

    p = new_alp->d;

    ALP_ACTION_QUERY_STRTOK(p, mask, data, fid, offset, length, group);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_nop
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_nop(alp_payload_t* alp)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_NOP_SIZE);

    p = new_alp->d;

    ALP_ACTION_NOP(p, true); // XXX RESP always true. If we use NOP, it's to get a response (istatus).

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_f_wr_data
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  data            void*               file content
/// @param  offset          u32                 file offset
/// @param  length          u32                 file length
/// @param  group           u8                  group with next OP
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
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)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_F_WR_DATA_SIZE(offset, length));

    p = new_alp->d;

    ALP_ACTION_F_WR_DATA(p, resp, fid, offset, length, data, group);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

alp_payload_t* alp_payload_f_wr_data(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, true);
}

alp_payload_t* alp_payload_f_wr_data_nr(alp_payload_t* alp, u8 fid, void* data, u32 offset, u32 length, u8 group)
{
    return _alp_payload_f_wr_data_resp(alp, fid, data, offset, length, group, false);
}

//======================================================================
// alp_payload_f_rd_data
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  offset          u32                 file offset
/// @param  length          u32                 file length
/// @param  group           u8                  group with next OP
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_f_rd_data(alp_payload_t* alp, u8 fid, u32 offset, u32 length, u8 group)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_F_RD_DATA_SIZE(offset, length));

    p = new_alp->d;

    ALP_ACTION_F_RD_DATA(p, true, fid, offset, length, group);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_f_flush
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  group           u8                  group with next OP
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_f_flush(alp_payload_t* alp, u8 fid, u8 group)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_F_FLUSH_SIZE);

    p = new_alp->d;

    ALP_ACTION_F_FLUSH(p, true, fid, group);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}


//======================================================================
// alp_payload_f_flush
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  fid             u8                  file identifier
/// @param  group           u8                  group with next OP
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_f_declare(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_F_DECLARE_SIZE);

    p = new_alp->d;

    ALP_ACTION_F_DECLARE(p, true, fid, hdr, true);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_forward
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  itf             void*               Forward interface
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_forward(alp_payload_t* alp, void* itf)
{
    u8* p;
    alp_payload_t* new_alp;
    u32 itf_length = alp_itf_size(itf);

    new_alp = alp_payload_new(ALP_ACTION_FORWARD_SIZE(itf_length));

    p = new_alp->d;

    ALP_ACTION_FORWARD(p, itf, itf_length);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_tag
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  tag             u8                  Tag value
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_tag(alp_payload_t* alp, u8 tag)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_TAG_SIZE);

    p = new_alp->d;

    ALP_ACTION_TAG(p, tag, true);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_tag
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  tag             u8                  Tag value
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_rsp_tag(alp_payload_t* alp, u8 tag, u8 eop, u8 err)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_RSP_TAG_SIZE);

    p = new_alp->d;

    ALP_ACTION_RSP_TAG(p, tag, eop, err);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_tag
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  tag             u8                  Action index
/// @param  tag             s8                  Status
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_rsp_status(alp_payload_t* alp, u8 action, s8 status)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_RSP_STATUS_SIZE);

    p = new_alp->d;

    ALP_ACTION_RSP_STATUS(p, action, status);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

//======================================================================
// alp_payload_tag
//----------------------------------------------------------------------
/// @brief  Creates malloc'ed ALP payload
/// @param  alp             alp_payload_t*  Payload to append the ALP message to. A new payload will be malloc'ed if NULL
/// @param  tag             u8                  Action index
/// @param  tag             s8                  Status
/// @return                 alp_payload_t*  New ALP payload
/// @revent                 NONE
//======================================================================
alp_payload_t* alp_payload_rsp_fprop(alp_payload_t* alp, u8 fid, alp_file_header_t* hdr)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_RSP_F_PROP_SIZE);

    p = new_alp->d;

    ALP_ACTION_RSP_F_PROP(p, fid, hdr);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

alp_payload_t* alp_payload_activate_itf(alp_payload_t* alp, uint8_t type, uint8_t nb_dev, uint8_t ifid, uint8_t flags, uint8_t enable)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);

    p = new_alp->d;

    ALP_ACTION_ACTIVATE_ITF(p, true, enable, type, nb_dev, ifid, flags);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

alp_payload_t* alp_payload_urcc_en(alp_payload_t* alp, uint8_t type, uint8_t ifid, uint8_t val)
{
    u8* p;
    alp_payload_t* new_alp;

    new_alp = alp_payload_new(ALP_ACTION_ACTIVATE_ITF_SIZE);

    p = new_alp->d;

    ALP_ACTION_URCC_EN(p, true, type, ifid, val);

    new_alp->len = (u32)(p - new_alp->d);

    return alp_payload_append(alp, new_alp);
}

alp_payload_t* alp_payload_extract(alp_payload_t** alp, u8 op)
{
    if (NULL == *alp)
    {
        return NULL;
    }

    if ((*alp)->d[0] == op)
    {
        // Save alp
        alp_payload_t* alp_old = *alp;

        // Remove it from chain
        *alp = alp_old->next;
        alp_old->next = NULL;

        // Return it
        return alp_old;
    }

    return alp_payload_extract((alp_payload_t**)&((*alp)->next), op);
}

alp_payload_t* alp_payload_get(alp_payload_t* alp, u8 op)
{
    FPRINT(": 0x%p\n", alp);
    
    if (NULL == alp)
    {
        return NULL;
    }

    if (alp->d[0] == op)
    {
        // Return it
        return alp;
    }

    return alp_payload_get(alp->next, op);
}

enum {
    ERR_PRIO_EOPISTATUS,
    ERR_PRIO_STATUS,
    ERR_PRIO_TAG,
    ERR_PRIO_NONE,
};

static int _alp_payload_get_err(alp_payload_t* alp, int err, u8 err_prio)
{
    alp_parsed_chunk_t r;
    u8* p = alp->d;
    
    if (NULL == alp)
    {
        return err;
    }

    switch (alp->d[0])
    {
    case ALP_OPCODE_RSP_TAG:
    {
        alp_parse_chunk(&p, &r);
        if (ERR_PRIO_TAG < err_prio && r.meta.tag.err)
        {
            err = ALP_ERR_UNKNOWN;
            err_prio = ERR_PRIO_TAG;
        }
        break;
    }
    case ALP_OPCODE_RSP_STATUS:
    {
        alp_parse_chunk(&p, &r);
        if (ERR_PRIO_STATUS < err_prio && r.meta.status.code < ALP_ERR_NONE)
        {
            err = r.meta.status.code;
            err_prio = ERR_PRIO_STATUS;
        }
        break;
    }
    case ALP_OPCODE_RSP_EOPISTATUS:
    {
        alp_parse_chunk(&p, &r);
        if (ERR_PRIO_EOPISTATUS < err_prio && r.meta.istatus.err < ALP_ERR_NONE)
        {
            err = r.meta.istatus.err + ALP_ERR_ITF_START;
            err_prio = ERR_PRIO_EOPISTATUS;
        }
        break;
    }
    default:
        break;
    }

    return _alp_payload_get_err(alp->next, err, err_prio);
}

int alp_payload_get_err(alp_payload_t* alp)
{
    return _alp_payload_get_err(alp, ALP_ERR_NONE, ERR_PRIO_NONE);
}

alp_payload_t* alp_payload_parse(u8* p, int length)
{
    if (NULL == p)
    {
        return NULL;
    }

    if (length <= 0)
    {
        return NULL;
    }

    u8* d = p;
    alp_parsed_chunk_t r;
    int len;
    alp_payload_t* alp;

    // Parse payload
    len = (int)alp_parse_chunk(&p, &r);

    if (len <= 0)
    {
        return NULL;
    }

    // Malloc payload
    alp = alp_payload_new(len);

    // Fill payload
    alp->len = len;
    memcpy(alp->d, d, len);

    // parse next
    return alp_payload_append(alp, alp_payload_parse(p, length - len));
}