File content as of revision 59:b42eae56b51b:

#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"

typedef struct {
    uint8_t tag;
    uint8_t buffer[100];
    Queue<d7a_com_rx_msg_t, 16> pkt_queue;
    Queue<d7a_com_rx_msg_t, 8> pl_queue;
    
    UnsolicitedMsgFunction uns_msg;
    Thread* thread;
} d7a_alp_ctx_t;

static d7a_alp_ctx_t g_alp_ctx;

void d7a_alp_thread(const void *p);

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

    g_alp_ctx.uns_msg = uns_msg;
    g_alp_ctx.thread = new Thread(d7a_alp_thread, NULL, osPriorityHigh, DEFAULT_STACK_SIZE);
    
    return D7A_ERR_NONE;
}

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

    g_alp_ctx.thread->terminate();
    
    return D7A_ERR_NONE;
}

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

void d7a_alp_new_pl(d7a_com_rx_msg_t* pl)
{
    FPRINT("\r\n");
    ASSERT(g_alp_ctx.pl_queue.put(pl) == osOK, "ALP OP queue full!\r\n");
}

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

d7a_com_rx_msg_t* d7a_alp_wait_pl(uint32_t millisec)
{
    FPRINT("\r\n");
    osEvent evt = g_alp_ctx.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);
}

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;
    }
}

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;
}

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)
{
    if (msg->data)
    {
        FREE(msg->data);
    }
    FREE(msg);
}

d7a_msg_t* d7a_alp_new_msg(void)
{
    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;
}

d7a_alp_rsp_t* d7a_alp_parse_pl(d7a_com_rx_msg_t* pkt)
{    
    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
                    memcpy(rsp->msg->id, res->addressee.id, D7A_UID_LEN); // Get UID
                    
                    DPRINT("ALP RSP ISTATUS type:%02X lb: %3d ", type, rsp->msg->lb);
                    DPRINT_DATA("UID:", "%02X", rsp->msg->id, D7A_UID_LEN, "\r\n");
                }
                else
                {
                    uint8_t aid = *p++; // Action ID
                    rsp->msg->err = *p++; // Status
                    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
                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);
                DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, length);
                //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;
}


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_ctx.tag;
    
    return (uint32_t)(p - t);
}

uint32_t d7a_alp_forward_action(uint8_t* p, alp_d7a_itf_t* itf, bool resp)
{
    uint8_t* t = p;
    
    uint32_t itf_size = sizeof(alp_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);
}

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);
}

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);
}

void d7a_alp_construct_resp(d7a_msg_t** ret, uint8_t current_tag, uint32_t max_responses)
{
    int i = 0;
    d7a_alp_rsp_t* pl = NULL;
    d7a_com_rx_msg_t* pkt = NULL;
    
    // Parse responses
    do
    {
        pkt = d7a_alp_wait_pl(60000);
        
        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)
        {
            DPRINT("EOP\r\n");
            
            // If tag only
            if (!pl->msg->data && !pl->msg->lb)
            {
                // Ignore response
                d7a_alp_free_msg(pl->msg);
            }
            else
            {
                DPRINT("last response (err %d)\r\n", pl->msg->err);
                ret[i] = pl->msg;
            }
            
            FREE(pl);
            break;
        }
        // Wait for new msg
        else
        {
            DPRINT("next response (err %d)\r\n", pl->msg->err);
            ret[i] = pl->msg;
            FREE(pl);
            i++;
            ASSERT(i <= max_responses, "Too much responses! max: %d\r\n", max_responses);
        }
        
    } while (1);
}

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, d7a_addressee_t* addressee, uint8_t retry, bool resp)
{
    // Get command buffer
    uint8_t* p = &g_alp_ctx.buffer[0];
    // Save initial position of the command buffer
    uint8_t* t = p;
    
    bool broadcast = false;
    uint8_t current_tag;
    d7a_msg_t** ret = NULL;
    uint32_t max_responses = 1;
    uint8_t i;
    
    if (addressee)
    {
        if (addressee->ctrl.bf.idf == D7A_ID_NBID)
        {
            broadcast = true;
            d7a_ctf_t ctf;
            ctf.byte = addressee->id[0];
            max_responses = ((1 << (2*ctf.bf.exp)) * ctf.bf.mant);
        }
        else if (addressee->ctrl.bf.idf == D7A_ID_NOID)
        {
            broadcast = true;
            max_responses = 32;
        }
    }
    
    // malloc and init pointer array
    ret = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * (max_responses + 1));
    for (i = 0; i < (max_responses + 1); i++)
    {
        ret[i] = NULL;
    }
        
    // Construct command
    
    // Tag action
    p += d7a_alp_tag(p, true);
    
    // get tag
    current_tag = g_alp_ctx.tag;
    
    if (addressee)
    {
        // Construct interface
        alp_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.dorm_to.byte = 0,
        };
        memcpy(&itf.cfg.addressee, addressee, sizeof(d7a_addressee_t));
        
        // Forward action
        p += d7a_alp_forward_action(p, &itf, true);
    }
    
    // Write action
    p += d7a_alp_write_action(p, file_id, offset, size, buf, resp);
    
    // Send command
    d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);

    i = 0; // start with msg 0
    
    // Parse responses
    d7a_alp_construct_resp(ret, current_tag, max_responses);
    
    return ret;
}

d7a_msg_t** d7a_alp_read_file(const uint8_t file_id, const uint32_t offset, const uint32_t size, d7a_addressee_t* addressee, uint8_t retry)
{
    // Get command buffer
    uint8_t* p = &g_alp_ctx.buffer[0];
    // Save initial position of the command buffer
    uint8_t* t = p;
    
    bool broadcast = false;
    uint8_t current_tag;
    d7a_msg_t** ret = NULL;
    uint32_t max_responses = 1;
    uint8_t i;
    
    if (addressee)
    {
        if (addressee->ctrl.bf.idf == D7A_ID_NBID)
        {
            broadcast = true;
            d7a_ctf_t ctf;
            ctf.byte = addressee->id[0];
            max_responses = ((1 << (2*ctf.bf.exp)) * ctf.bf.mant);
        }
        else if (addressee->ctrl.bf.idf == D7A_ID_NOID)
        {
            broadcast = true;
            max_responses = 32;
        }
    }
    
    // malloc and init pointer array
    ret = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * (max_responses + 1));
    for (i = 0; i < (max_responses + 1); i++)
    {
        ret[i] = NULL;
    }
    
    // malloc and init first response
    ret[0] = d7a_alp_new_msg();
    
    // Construct command
    
    // Tag action
    p += d7a_alp_tag(p, true);
    
    // get tag
    current_tag = g_alp_ctx.tag;
    
    if (addressee)
    {
        // Construct interface
        alp_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.resp = D7A_RESP_ALL,
            .cfg.qos.bf.retry = retry,
            .cfg.qos.bf.record = 0,
            .cfg.qos.bf.stop_on_err = 0,
            .cfg.dorm_to.byte = 0,
        };
        memcpy(&itf.cfg.addressee, addressee, sizeof(d7a_addressee_t));
        
        // Forward action
        p += d7a_alp_forward_action(p, &itf, true);
    }
    
    // Read action
    p += d7a_alp_read_action(p, file_id, offset, size, true);
    
    // Send command
    d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);

    i = 0; // start with msg 0
    
    // Parse responses
    d7a_alp_construct_resp(ret, current_tag, max_responses);
    
    return ret;
}

void d7a_alp_thread(const void *p)
{
    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:
                DPRINT("KAL_COM_FLOW_AT_RESP\r\n");

                d7a_alp_new_pl(pkt);
                
                break;
            case KAL_COM_FLOW_AT_UNS:
                DPRINT("KAL_COM_FLOW_AT_UNS\r\n");
                d7a_msg_t** ret = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * 2);
                
                d7a_alp_rsp_t* pl = d7a_alp_parse_pl(pkt);
                
                ret[0] = pl->msg;
                ret[1] = NULL;
                
                FREE(pkt);
                FREE(pl);
                
                // Callback
                g_alp_ctx.uns_msg(ret);
                
                break;
            default:
                EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
                FREE(pkt);
                break;
        }
    }
}