File content as of revision 57:fd9c8b67ffdc:

#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;
    
    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(void)
{
    FPRINT("\r\n");

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

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

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->nb_status = 0;
    rsp->length = 0;
    rsp->data = NULL;
    
    while ((p - t) < len)
    {
        
        uint8_t ctrl = *p++;
        switch (ctrl & 0x3F)
        {
            case ALP_OPCODE_RSP_STATUS:
                if (ctrl & ALP_OPCODE_INDIRECT)
                {
                    uint8_t type = *p++; // ITF Type
                    uint32_t length;
                    p += alp_decode_length(p, &length); // Length
                    p += length; // Data
                    DPRINT("ALP RSP ISTATUS %02X\r\n", type);
                }
                else
                {
                    rsp->status[rsp->nb_status].aid = *p++; // Action ID
                    rsp->status[rsp->nb_status].status = *p++; // Status
                    DPRINT("ALP RSP STATUS[%d] aid:%d Status:%d\r\n", rsp->nb_status, rsp->status[rsp->nb_status].aid, rsp->status[rsp->nb_status].status);
                    rsp->nb_status++;
                }
                break;
            case ALP_OPCODE_RSP_TAG:
                rsp->tag_status = (ctrl & ~0x3F) | 0x01;
                rsp->tag = *p++; // TAG
                DPRINT("ALP RSP TAG %d %02x\r\n", rsp->tag, rsp->tag_status);
                break;
            case ALP_OPCODE_RSP_F_DATA:
                uint8_t fid = *p++; // File ID
                uint32_t offset;
                p += alp_decode_length(p, &offset); // offset
                p += alp_decode_length(p, &(rsp->length)); // length
                rsp->data = (uint8_t*)MALLOC(rsp->length);
                p += d7a_alp_add(rsp->data, p, rsp->length);
                DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, rsp->length);
                //dbg_print_data("DATA: ", "%02X ", (uint8_t*)rsp->data, rsp->length, "\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;
    
    *p++ = ALP_OPCODE_FORWARD + ((resp)? ALP_CTRL_RESP : 0);
    p += d7a_alp_add(p, (uint8_t*)itf, sizeof(alp_d7a_itf_t));
    
    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, const uint8_t* const buf, 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 free_pl(d7a_alp_rsp_t* pl)
{
    if (pl->data != NULL)
    {
        FREE(pl->data);
    }
    FREE(pl);
}


d7a_errors_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)
{
    uint8_t* p = &g_alp_ctx.buffer[0];
    uint8_t* t = p;
    
    // Tag action
    p += d7a_alp_tag(p, true);
        
    if (addressee)
    {
        // Construct interface
        alp_d7a_itf_t itf = {
            .type = 0xD7, // Dash7 interface
            .cfg.qos.bf.resp = 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));
        itf.cfg.addressee.ctrl.bf.idf = D7A_ID_UID;
            
        // Forward
        p += d7a_alp_forward_action(p, &itf, true);
    }
    
    // Write action
    p += d7a_alp_write_action(p, file_id, offset, size, buf, resp);
    
    d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
    
    d7a_alp_rsp_t* pl = NULL;
    d7a_com_rx_msg_t* pkt = NULL;
    d7a_errors_t status = D7A_ERR_UNKNOWN;
    uint8_t current_tag = g_alp_ctx.tag;
    
    do
    {
        pkt = d7a_alp_wait_pl(60000);
        
        if (pkt == NULL)
        {
            status = D7A_ERR_CMD_TO;
            break;
        }
        
        pl = d7a_alp_parse_pl(pkt);
        
        if (!(pl->tag_status & 0x01))
        {
            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
            FREE(pkt);
            free_pl(pl);
            status = D7A_ERR_UNKNOWN;
            break;
        }
        
        if (pl->tag != current_tag)
        {
            //WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
            free_pl(pl);
            d7a_alp_new_pl(pkt);
            continue;
        }
        
        FREE(pkt);
        
        if (pl->nb_status)
        {
            status = (d7a_errors_t)pl->status[0].status;
        }
        else
        {
            if (pl->tag_status & ALP_CTRL_ERR)
            {
                status = D7A_ERR_TX_FAILED;
            }
            else
            {
                status = D7A_ERR_NONE;
            }
        }
    
        if (pl->tag_status & ALP_CTRL_EOP)
        {
            free_pl(pl);
            break;
        }
        
        free_pl(pl);
    } while (1);
    
    return status;
}

d7a_errors_t d7a_alp_read_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)
{
    uint8_t* p = &g_alp_ctx.buffer[0];
    uint8_t* t = p;
    
    // Tag action
    p += d7a_alp_tag(p, true);
    
    if (addressee)
    {
        // Construct interface
        alp_d7a_itf_t itf = {
            .type = 0xD7, // Dash7 interface
            .cfg.qos.bf.resp = 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));
        itf.cfg.addressee.ctrl.bf.idf = D7A_ID_UID;
            
        // Forward
        p += d7a_alp_forward_action(p, &itf, true);
    }
    
    // Read action
    p += d7a_alp_read_action(p, file_id, offset, size, buf, true);
    
    d7a_com_dump(&g_alp_ctx.buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
    
    d7a_alp_rsp_t* pl = NULL;
    d7a_com_rx_msg_t* pkt = NULL;
    d7a_errors_t status = D7A_ERR_UNKNOWN;
    uint8_t current_tag = g_alp_ctx.tag;
    
    do
    {
        pkt = d7a_alp_wait_pl(60000);
        
        if (pkt == NULL)
        {
            status = D7A_ERR_CMD_TO;
            break;
        }
        
        pl = d7a_alp_parse_pl(pkt);
        
        if (!(pl->tag_status & 0x01))
        {
            WARNING(false, "No tag in payload expected %d\r\n", current_tag);
            FREE(pkt);
            free_pl(pl);
            status = D7A_ERR_UNKNOWN;
            break;
        }
        
        if (pl->tag != current_tag)
        {
            //WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
            free_pl(pl);
            d7a_alp_new_pl(pkt);
            continue;
        }
        
        FREE(pkt);
        
        if (pl->nb_status)
        {
            status = (d7a_errors_t)pl->status[0].status;
        }
        else
        {
            if (pl->tag_status & ALP_CTRL_ERR)
            {
                status = D7A_ERR_TX_FAILED;
            }
            else
            {
                status = D7A_ERR_NONE;
            }
        }
        
        if (pl->data != NULL)
        {
            memcpy((void*)buf, (void*)pl->data, pl->length);
            status = D7A_ERR_NONE;
        }
    
        if (pl->tag_status & ALP_CTRL_EOP)
        {
            free_pl(pl);
            break;
        }
        
        free_pl(pl);
    } while (1);
    
    return status;
}

void d7a_alp_thread(const void *p)
{
    FPRINT("\r\n");
    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;
            default:
                EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
                FREE(pkt);
                break;
        }
    }
}