File content as of revision 45:b85384e7d825:

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

void d7a_alp_open(void)
{
    FPRINT("\r\n");

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

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)
{
    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;
    
    if (pkt == NULL)
    {
        return rsp;
    }
    
    while ((p - t) < len)
    {
        
        uint8_t ctrl = *p++;
        switch (ctrl & 0x3F)
        {
            case ALP_OPCODE_RSP_STATUS:
                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;
        }
    }
    
    FREE(pkt);
    
    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(uint8_t* p, d7a_addressee_t* addressee, uint8_t retry, bool ack, bool resp)
{
    uint8_t* t = p;
    
    *p++ = ALP_OPCODE_FORWARD + ((resp)? ALP_CTRL_RESP : 0);
    *p++ = (retry << 3) | ((ack)? 2 : 0);
    *p++ = 0; // dorm_to
    *p++ = addressee->ctrl.bf.nls | (D7A_ID_UID<<4);
    *p++ = addressee->xcl.byte;
    p += d7a_alp_add(p, addressee->id, D7A_UID_LEN);
    
    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);
}


int8_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)
    {
        // Forward
        p += d7a_alp_forward(p, addressee, retry, true, 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;
    int8_t status;
    
    if (addressee)
    {
        pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
        if (pl->nb_status == 1
         && pl->status[0].status == 0
         && pl->tag_status == 0x01
         && pl->tag == g_alp_ctx.tag)
        {
            // OK
        }
        else
        {
            status = pl->status[0].status;
        }
        
        free_pl(pl);
        
        if (status)
        {
            return status;
        }
    }
    
    pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
    if (resp)
    {
        if (pl->nb_status == 1
         && pl->status[0].status == 0
         && pl->tag_status == 0x01 | ALP_CTRL_EOP
         && pl->tag == g_alp_ctx.tag
         )
        {
            // OK
        }
        else
        {
            
            if (pl->nb_status)
            {
                status = pl->status[0].status;
            }
            else
            {
                status = D7A_ERR_TX_FAILED;
            }
        }
    }
    else
    {
        if (pl->nb_status == 0
         && pl->tag_status == 0x01 | ALP_CTRL_EOP
         && pl->tag == g_alp_ctx.tag
         )
        {
            // OK
        }
        else
        {
            status = D7A_ERR_TX_FAILED;
        }
    }
    
    free_pl(pl);
    
    return status;
}

int8_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)
    {
        // Forward
        p += d7a_alp_forward(p, addressee, retry, true, 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;
    int8_t status;
    
    if (addressee)
    {
        pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));
        if (pl->nb_status == 1
         && pl->status[0].status == 0
         && pl->tag_status == 0x01
         && pl->tag == g_alp_ctx.tag)
        {
            // OK
        }
        else
        {
            status = pl->status[0].status;
        }
        
        free_pl(pl);
        
        if (status)
        {
            return status;
        }
    }
    
    pl = d7a_alp_parse_pl(d7a_alp_wait_pl(3000));

    if (pl->nb_status == 0
     && pl->data != NULL
     && pl->tag_status == 0x01 | ALP_CTRL_EOP
     && pl->tag == g_alp_ctx.tag
     )
    {
        // OK
        memcpy((void*)buf, (void*)pl->data, pl->length);
    }
    else
    {
        if (pl->nb_status)
        {
            status = pl->status[0].status;
        }
        else
        {
            status = D7A_ERR_TX_FAILED;
        }
    }
    
    free_pl(pl);
    
    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;
        }
    }
}