File content as of revision 90:eaf980532613:

#include "mbed.h"
#include "rtos.h"
#include "dbg.h"
#include "d7a.h"
#include "d7a_com.h"
#include "d7a_common.h"
#include "d7a_fs.h"
#include "d7a_modem.h"
#include "d7a_sys.h"
#include "d7a_alp.h"
#include "sha.h"
#include "d7a_typedefs.h"

#if 1
    #define ALP_DPRINT(...)         DPRINT(__VA_ARGS__)
    #define ALP_DPRINT_DATA(...)    DPRINT_DATA(__VA_ARGS__)
    #define ALP_FPRINT(...)         FPRINT(__VA_ARGS__)
#else
    #define ALP_DPRINT(...);
    #define ALP_DPRINT_DATA(...);
    #define ALP_FPRINT(...);
#endif

#define ALP_CMD_MAX_LENGHT          (256)
#define MAX_RESPONSES               (32)

static uint8_t                      g_alp_tag;
static uint8_t                      g_alp_root_key_size;
static uint8_t                      g_alp_buffer[ALP_CMD_MAX_LENGHT];
static UnsolicitedMsgFunction       g_alp_uns_msg;

static OS_Queue<d7a_com_rx_msg_t, 8>   g_alp_pkt_queue;
static OS_Queue<d7a_com_rx_msg_t, 8>   g_alp_pl_queue;
static OS_Thread                       g_alp_thread(osPriorityHigh, 512, NULL);

void d7a_alp_thread();

d7a_errors_t d7a_alp_open(UnsolicitedMsgFunction uns_msg)
{
    ALP_FPRINT("\r\n");

    g_alp_uns_msg = uns_msg;
    g_alp_root_key_size = D7A_ROOT_KEY_SIZE;
    
    osStatus err = g_alp_thread.start(d7a_alp_thread);
    ASSERT(err == osOK, "Failed to start d7a_alp_thread (err: %d)\r\n", err);
    
    return D7A_ERR_NONE;
}

d7a_errors_t d7a_alp_close(void)
{
    ALP_FPRINT("\r\n");

    g_alp_thread.terminate();
    
    return D7A_ERR_NONE;
}

void d7a_alp_new_pkt(d7a_com_rx_msg_t* pkt)
{
    ALP_FPRINT("\r\n");
    ASSERT(g_alp_pkt_queue.put(pkt) == osOK, "ALP queue full!\r\n");
}

static void d7a_alp_new_pl(d7a_com_rx_msg_t* pl)
{
    ALP_FPRINT("\r\n");
    ASSERT(g_alp_pl_queue.put(pl) == osOK, "ALP PL queue full!\r\n");
}

static d7a_com_rx_msg_t* d7a_alp_wait_pkt(uint32_t millisec)
{
    ALP_FPRINT("\r\n");
    osEvent evt = g_alp_pkt_queue.get(millisec);
    return (evt.status == osEventMessage)? (d7a_com_rx_msg_t*)evt.value.p : NULL;
}

static d7a_com_rx_msg_t* d7a_alp_wait_pl(uint32_t millisec)
{
    ALP_FPRINT("\r\n");
    osEvent evt = g_alp_pl_queue.get(millisec);
    return (evt.status == osEventMessage)? (d7a_com_rx_msg_t*)evt.value.p : NULL;
}

static uint32_t d7a_ctf_to_ti(d7a_ctf_t ctf)
{
    return ((1 << (2*ctf.bf.exp)) * ctf.bf.mant);
}

static uint32_t d7a_alp_encode_length(uint8_t* p, uint32_t len)
{
    if (len <= 0x3F)
    {
        *p++ = len;
        return 1;
    }
    else if (len <= 0x3FFF)
    {
        *p++ = 0x40 + (uint8_t)(len >> 8);
        *p++ =        (uint8_t)(len & 0xFF);
        return 2;
    }
    else if (len <= 0x3FFFFF)
    {
        *p++ = 0x80 + (uint8_t) (len >> 16);
        *p++ =        (uint8_t)((len >> 8) & 0xFF);
        *p++ =        (uint8_t) (len       & 0xFF);
        return 3;
    }
    else
    {
        *p++ = 0xC0 + (uint8_t) (len >> 24);
        *p++ =        (uint8_t)((len >> 16) & 0xFF);
        *p++ =        (uint8_t)((len >>  8) & 0xFF);
        *p++ =        (uint8_t) (len        & 0xFF);
        return 4;
    }
}

static uint32_t alp_decode_length(uint8_t* p, uint32_t* len)
{
    uint32_t tmp = 0;
    switch ((*p) & 0xC0)
    {
        case 0xC0: // 0xCx xx xx xx
            tmp  =  (*p++ & 0x3F) << 24;
            tmp +=   *p++         << 16;
            tmp +=   *p++         <<  8;
            tmp +=   *p++         <<  0;
            *len = tmp;
            return 4;
        case 0x80: // 0x8x xx xx : 16384 <= Len <4194303
            tmp  =  (*p++ & 0x3F) << 16;
            tmp +=   *p++         <<  8;
            if (tmp == 0)            {
                // 0x8000 ActP special ActP code
                // Do not fetch the extra byte
                tmp = 2;
            }
            else
            {
                tmp += *p++       <<  0;
            }
            *len = tmp;
            return 3;
        case 0x40: // 0x4x xx : 64 <= Len < 16383
            tmp  =  (*p++ & 0x3F)  <<  8;
            tmp +=   *p++          <<  0;
            if (tmp == 0)
            {
                // 0x4000 ActP special ActP code
                tmp = 1;
            }
            *len = tmp;
            return 2;
        case 0: // Len <63
            tmp  = (*p++ & 0x3F) <<  0;
            *len = tmp;
            return 1;
    }
    
    return 0;
}

static uint32_t d7a_alp_add(uint8_t* p, const uint8_t* data, uint32_t len)
{
    memcpy(p, data, len);
    
    return len;
}

void d7a_alp_free_msg(d7a_msg_t* msg)
{
    ALP_FPRINT("\r\n");
    
    if (msg->data)
    {
        FREE(msg->data);
    }
    FREE(msg);
}

static d7a_msg_t* d7a_alp_new_msg(void)
{
    ALP_FPRINT("\r\n");
    
    d7a_msg_t* msg = (d7a_msg_t*)MALLOC(sizeof(d7a_msg_t));
    memset(msg, 0, sizeof(d7a_msg_t));
    msg->err = D7A_ERR_NONE;
    
    return msg;
}

static d7a_alp_rsp_t* d7a_alp_parse_pl(d7a_com_rx_msg_t* pkt)
{
    ALP_FPRINT("\r\n");
    
    if (pkt == NULL)
    {
        return NULL;
    }
    
    uint8_t* p = pkt->buffer;
    uint8_t* t = p;
    uint8_t len = pkt->blen;

    d7a_alp_rsp_t* rsp = (d7a_alp_rsp_t*)MALLOC(sizeof(d7a_alp_rsp_t));
    rsp->tag = NO_TAG;
    rsp->eop = false;
    rsp->msg = d7a_alp_new_msg();
    
    while ((p - t) < len)
    {
        uint8_t ctrl = *p++;
        switch (ctrl & 0x3F)
        {
            case ALP_OPCODE_RSP_STATUS:
                if (ctrl & ALP_OPCODE_INDIRECT)
                {
                    // ITF Type
                    uint8_t type = *p++;
                    // Length
                    uint32_t length;
                    p += alp_decode_length(p, &length);
                    // Data
                    d7a_sp_res_t* res = (d7a_sp_res_t*)p;
                    p += length;
                    
                    // Fill corresponding fields
                    rsp->msg->lb = res->lb; // Get Link Budget
                    rsp->msg->rxlev = res->rxlev; // Get RXLEV
                    memcpy(rsp->msg->id, res->addressee.id, D7A_UID_LEN); // Get UID
                    
                    ALP_DPRINT("ALP RSP ISTATUS type:%02X lb: %3d ", type, rsp->msg->lb);
                    ALP_DPRINT_DATA("UID:", "%02X", rsp->msg->id, D7A_UID_LEN, "\r\n");
                }
                else
                {
                    uint8_t aid = *p++; // Action ID
                    rsp->msg->err = *p++; // Status
                    ALP_DPRINT("ALP RSP STATUS aid:%d Status:%d\r\n", aid, rsp->msg->err);
                }
                break;
            case ALP_OPCODE_RSP_TAG:
                rsp->eop = !!(ctrl & ALP_CTRL_EOP);
                rsp->tag = *p++; // TAG
                ALP_DPRINT("ALP RSP TAG %d EOP %d\r\n", rsp->tag, rsp->eop);
                break;
            case ALP_OPCODE_RSP_F_DATA:
                uint8_t fid;
                uint32_t offset;
                uint32_t length;
                fid = *p++; // File ID
                p += alp_decode_length(p, &offset); // offset
                p += alp_decode_length(p, &length); // length
                rsp->msg->data = (d7a_data_t*)MALLOC(sizeof(d7a_data_t) - 1 + length);
                rsp->msg->data->fid = fid;
                rsp->msg->data->offset = offset;
                rsp->msg->data->length = length;
                p += d7a_alp_add(rsp->msg->data->buf, p, length);
                ALP_DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, length);
                //ALP_DPRINT_DATA("DATA: ", "%02X ", (uint8_t*)rsp->data, rsp->data_len, "\r\n");
                break;
            default:
                WARNING(false, "ALP Untreated OP %d\r\n", ctrl);
                break;
        }
    }
    
    ASSERT((p - t) == len, "Payload wrong size: %d expected %d\r\n", (p - t), len);

    return rsp;
}


static uint32_t d7a_alp_tag(uint8_t* p, bool eop)
{
    uint8_t* t = p;
    
    *p++ = ALP_OPCODE_TAG + ((eop)? ALP_CTRL_EOP : 0);
    *p++ = ++g_alp_tag;
    
    return (uint32_t)(p - t);
}

static uint32_t d7a_alp_forward_action(uint8_t* p, d7a_itf_t* itf, bool resp)
{
    uint8_t* t = p;
    
    uint32_t itf_size = sizeof(d7a_itf_t);
    switch (itf->cfg.addressee.ctrl.bf.idf)
    {
        case D7A_ID_NBID:
            itf_size -= 7;
            break;
        case D7A_ID_NOID:
            itf_size -= 8;
            break;
        case D7A_ID_UID:
            break;
        case D7A_ID_VID:
            itf_size -= 4;
            break;
        default:
            break;
    }
    
    *p++ = ALP_OPCODE_FORWARD + ((resp)? ALP_CTRL_RESP : 0);
    p += d7a_alp_add(p, (uint8_t*)itf, itf_size);
    
    return (uint32_t)(p - t);
}

static uint32_t d7a_alp_write_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, bool resp)
{
    uint8_t* t = p;
    
    *p++ = ALP_OPCODE_F_WR_DATA + ((resp)? ALP_CTRL_RESP : 0);
    *p++ = file_id;
    p += d7a_alp_encode_length(p, offset);
    p += d7a_alp_encode_length(p, size);
    p += d7a_alp_add(p, buf, size);
    
    return (uint32_t)(p - t);
}

static uint32_t d7a_alp_read_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, bool resp)
{
    uint8_t* t = p;
    
    *p++ = ALP_OPCODE_F_RD_DATA + ((resp)? ALP_CTRL_RESP : 0);
    *p++ = file_id;
    p += d7a_alp_encode_length(p, offset);
    p += d7a_alp_encode_length(p, size);
    
    return (uint32_t)(p - t);
}

static uint32_t d7a_alp_perm_request_action(uint8_t* p, uint8_t* req, uint32_t req_size, const uint8_t* root_key, bool resp)
{
    uint8_t* t = p;
    uint8_t hash[32];
    
    *p++ = ALP_OPCODE_PERM_REQ + ((resp)? ALP_CTRL_RESP : 0);
    *p++ = 1; // ROOT request
    *p++ = 42; // Auth protocol ID
    sha256_init();
    sha256_update(req, req_size);
    sha256_update((uint8_t*)root_key, g_alp_root_key_size);
    sha256_final(hash);
    //PRINT_DATA("Req  : ", "%02X ", (uint8_t*)req, req_size, "\r\n");
    //PRINT_DATA("Key  : ", "%d ", (uint8_t*)root_key, g_alp_root_key_size, "\r\n");
    //PRINT_DATA("Token: ", "%02X", hash, D7A_AUTH_PROTOCOLE_TOKEN_SIZE, "\r\n");
    p += d7a_alp_add(p, hash, D7A_AUTH_PROTOCOLE_TOKEN_SIZE);
    
    return (uint32_t)(p - t);
}

static uint32_t d7a_alp_flush_action(uint8_t* p, uint8_t fid, bool resp)
{
    uint8_t* t = p;
    
    *p++ = ALP_OPCODE_F_FLUSH + ((resp)? ALP_CTRL_RESP : 0);
    *p++ = fid;
    
    return (uint32_t)(p - t);
}

static void d7a_alp_construct_resp(d7a_msg_t** ret, uint8_t current_tag)
{
    ALP_FPRINT("\r\n");
    
    int i = 0;
    d7a_alp_rsp_t* pl = NULL;
    d7a_com_rx_msg_t* pkt = NULL;
    int32_t time;
    Timer timeout;
    
    timeout.start();
    
    // Parse responses
    do
    {
        time = D7A_ALP_RESP_TO - timeout.read_ms();
        if (time < 0) time = 0;
        
        pkt = d7a_alp_wait_pl(time);
        
        if (pkt == NULL)
        {
            ret[i] = d7a_alp_new_msg();
            ret[i]->err = D7A_ERR_CMD_TO;
            break;
        }
        
        pl = d7a_alp_parse_pl(pkt);

        // Check TAG
        if (pl->tag == NO_TAG)
        {
            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
            FREE(pkt);
            d7a_alp_free_msg(pl->msg);
            FREE(pl);
            ret[i] = d7a_alp_new_msg();
            ret[i]->err = D7A_ERR_UNKNOWN;
            break;
        }
        
        if (pl->tag != current_tag)
        {
            WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
            d7a_alp_free_msg(pl->msg);
            FREE(pl);
            d7a_alp_new_pl(pkt);
            continue;
        }
        
        FREE(pkt);
        
        // Check for END OF PAYLOAD
        if (pl->eop)
        {
            ALP_DPRINT("EOP\r\n");
            
            // If tag only
            if (!pl->msg->data && !pl->msg->lb && i != 0)
            {
                // Ignore response
                d7a_alp_free_msg(pl->msg);
            }
            else
            {
                ALP_DPRINT("last response (err %d)\r\n", pl->msg->err);
                ret[i] = pl->msg;
            }
            
            FREE(pl);
            break;
        }
        // Wait for new msg
        else
        {
            ALP_DPRINT("next response (err %d)\r\n", pl->msg->err);
            
            if (i > MAX_RESPONSES)
            {
                WARNING(false, "Too much responses! max: %d\r\n", MAX_RESPONSES);
                FREE(pl->msg);
            }
            else
            {
                ret[i] = pl->msg;
                i++;
            }
            
            FREE(pl);
        }
        
    } while (1);
}

static uint32_t d7a_alp_construct_itf(uint8_t* p, d7a_addressee_t* addressee, uint8_t retry, bool resp)
{
    bool broadcast = false;
    
    if (addressee)
    {
        if (addressee->ctrl.bf.idf == D7A_ID_NBID || addressee->ctrl.bf.idf == D7A_ID_NOID)
        {
            broadcast = true;
        }

        // Construct interface
         d7a_itf_t itf = {
            // Dash7 interface
            .type = 0xD7,
            // Switch response type if broadcast
            .cfg.qos.bf.resp = (broadcast)? D7A_RESP_ALL : D7A_RESP_ANY,
            .cfg.qos.bf.retry = retry,
            .cfg.qos.bf.record = 0,
            .cfg.qos.bf.stop_on_err = 0,
            .cfg.to.byte = 0,
            .cfg.te.byte = 0,
        };
        memcpy(&itf.cfg.addressee, addressee, sizeof(d7a_addressee_t));
        
        // Forward action
        return d7a_alp_forward_action(p, &itf, true);
    }
    
    return 0;
}

d7a_msg_t** d7a_alp_init_ret(void)
{
    d7a_msg_t** ret = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * (MAX_RESPONSES + 1));
    for (uint32_t i = 0; i < (MAX_RESPONSES + 1); i++)
    {
        ret[i] = NULL;
    }
    
    return ret;
}

d7a_msg_t** d7a_alp_write_file(const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, const uint8_t* root_key, d7a_addressee_t* addressee, uint8_t retry, bool resp)
{
    ALP_FPRINT("\r\n");
    
    // Get command buffer
    uint8_t* p = &g_alp_buffer[0];
    // Save initial position of the command buffer
    uint8_t* t = p;
    
    uint8_t current_tag;
    d7a_msg_t** ret = NULL;
    
    // malloc and init pointer array
    ret = d7a_alp_init_ret();
    
    // Tag action
    p += d7a_alp_tag(p, true);
    
    // Eventual forward
    p += d7a_alp_construct_itf(p, addressee, retry, resp);
    
    // get tag
    current_tag = g_alp_tag;
       
    // Ask for root permissions
    if (root_key)
    {
        uint8_t req[100];
        uint8_t req_size = d7a_alp_write_action(req, file_id, offset, size, buf, resp);
        p += d7a_alp_perm_request_action(p, req, req_size, root_key, false);
    }
   
    // Write action
    p += d7a_alp_write_action(p, file_id, offset, size, buf, resp);
    
    // Send command
    d7a_com_dump(&g_alp_buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
    
    // Parse responses
    d7a_alp_construct_resp(ret, current_tag);
    
    return ret;
}

d7a_msg_t** d7a_alp_read_file(const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* root_key, d7a_addressee_t* addressee, uint8_t retry)
{
    ALP_FPRINT("\r\n");
    
    // Get command buffer
    uint8_t* p = &g_alp_buffer[0];
    // Save initial position of the command buffer
    uint8_t* t = p;
    
    uint8_t current_tag;
    d7a_msg_t** ret = NULL;
    
    // malloc and init pointer array
    ret = d7a_alp_init_ret();
        
    // Tag action
    p += d7a_alp_tag(p, true);
    
    // Eventual forward
    p += d7a_alp_construct_itf(p, addressee, retry, true);

    // get tag
    current_tag = g_alp_tag;
           
    // Ask for root permissions
    if (root_key)
    {
        uint8_t req[100];
        uint8_t req_size = d7a_alp_read_action(req, file_id, offset, size, true);
        p += d7a_alp_perm_request_action(p, req, req_size, root_key, false);
    }
    
    // Read action
    p += d7a_alp_read_action(p, file_id, offset, size, true);
    
    // Send command
    d7a_com_dump(&g_alp_buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
    
    // Parse responses
    d7a_alp_construct_resp(ret, current_tag);
    
    return ret;
}


d7a_msg_t** d7a_alp_flush_file(const uint8_t file_id, const uint8_t* root_key, d7a_addressee_t* addressee, uint8_t retry, bool resp)
{
    ALP_FPRINT("\r\n");
    
    // Get command buffer
    uint8_t* p = &g_alp_buffer[0];
    // Save initial position of the command buffer
    uint8_t* t = p;
    
    uint8_t current_tag;
    d7a_msg_t** ret = NULL;
    
    // malloc and init pointer array
    ret = d7a_alp_init_ret();
    
    // Tag action
    p += d7a_alp_tag(p, true);
    
    // Eventual forward
    p += d7a_alp_construct_itf(p, addressee, retry, resp);
    
    // get tag
    current_tag = g_alp_tag;
               
    // Ask for root permissions
    if (root_key)
    {
        uint8_t req[100];
        uint8_t req_size = d7a_alp_flush_action(req, file_id, resp);
        p += d7a_alp_perm_request_action(p, req, req_size, root_key, false);
    }
    
    // Flush action
    p += d7a_alp_flush_action(p, file_id, resp);
    
    // Send command
    d7a_com_dump(&g_alp_buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
    
    // Parse responses
    d7a_alp_construct_resp(ret, current_tag);
    
    return ret;
}

void d7a_alp_set_root_key_size(uint8_t size)
{
    g_alp_root_key_size = size;
}

void d7a_alp_thread()
{
    ALP_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
    d7a_com_rx_msg_t* pkt;
    
    while (true)
    {
        pkt = d7a_alp_wait_pkt();
        ASSERT(pkt != NULL, "ALP NULL pkt\r\n");

        switch(pkt->id)
        {
            case KAL_COM_FLOW_AT_RESP:
                ALP_DPRINT("KAL_COM_FLOW_AT_RESP\r\n");

                d7a_alp_new_pl(pkt);
                
                break;
            case KAL_COM_FLOW_AT_UNS:
                ALP_DPRINT("KAL_COM_FLOW_AT_UNS\r\n");
                if (g_alp_uns_msg)
                {
                    d7a_msg_t** uns = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * 2);
                    
                    d7a_alp_rsp_t* pl = d7a_alp_parse_pl(pkt);
                    
                    uns[0] = pl->msg;
                    uns[1] = NULL;
                    
                    FREE(pl);
                    
                    // Callback
                    g_alp_uns_msg(uns);
                }

                FREE(pkt);
                break;
            default:
                EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
                FREE(pkt);
                break;
        }
    }
}