File content as of revision 78:f1c0affd99e7:

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

#define XON_SIGNAL              (0x0001 << 0)
#define START_SIGNAL            (0x0001 << 1)

typedef struct {
    uint32_t len;
    uint8_t buf[1];
} d7a_com_tx_buf_t;

static bool                         g_com_rx_started;
static bool                         g_com_tx_started;
static uint8_t                      g_com_state;
static DigitalOut*                  g_com_rts;
static InterruptIn*                 g_com_cts;
static CBuffer<uint8_t, 512>        g_com_rx_buf;
static d7a_com_rx_msg_t             g_com_msg;
static uint16_t                     g_com_skipped_bytes;
static uint8_t                      g_com_tx_seq;
static uint8_t                      g_com_rx_seq;
static Semaphore                    g_com_data_parsing(0);
static Semaphore                    g_com_cts_int(0);
static RawSerial*                   g_com_serial;
static Thread                       g_com_rx_thread(osPriorityRealtime, 512, NULL);
static Thread                       g_com_tx_thread(osPriorityHigh, 512, NULL);
static Queue<d7a_com_tx_buf_t, 8>   g_com_tx_queue;
static Timer                        g_com_tim;


enum {
    SEARCH_HEADER,
    PARSE_HEADER,
    SEARCH_BODY,
    PARSE_BODY,
};


/**
    Thread for parsing packets from RX buffer.

    @param void
    @return void
*/
void d7a_com_rx_thread();
void d7a_com_tx_thread();


/**
    Serial Rx Interrupt Service Routine.
    Add recevied bytes to the RX buffer.

    @param void
    @return void
*/
void rx_isr()
{
// Loop just in case more than one character is in UART's receive FIFO buffer
    while (g_com_serial->readable())
    {
        g_com_rx_buf.push(g_com_serial->getc());
        //PRINT("-");
    }
    
    // unlock data parsing thread
    if (g_com_state == SEARCH_HEADER && g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN)
    {
        g_com_state = PARSE_HEADER;
        g_com_data_parsing.release();
    }
    else if (g_com_state == SEARCH_BODY && g_com_rx_buf.available_data() >= g_com_msg.blen)
    {
        g_com_state = PARSE_BODY;
        g_com_data_parsing.release();
    }
}

/**
    CTS pin Interrupt Service Routine.
    For flow control (not yet inplemented)

    @param void
    @return void
*/
void cts_isr()
{
    //PRINT("CTS_INT\r\n");
    //g_com_cts_int->release();
}

// D7a_com constructor.
// Opens a serial port and monitors the input for ALP packets.
// Pins are those of the host, not the modem:
// TX-host -> RX-modem
// RX-host <- TX-modem
// RTS-host -> CTS-modem
// CTS-host <- RTS-modem
d7a_errors_t d7a_com_open( const d7a_com_config_t* config )
{
    FPRINT("\r\n");
    
    g_com_state = SEARCH_HEADER;
    g_com_skipped_bytes = 0;
    g_com_tx_seq = 0;
    g_com_rx_seq = 0;
    
    g_com_serial =          new RawSerial(config->tx, config->rx, 115200);
    g_com_rts =             new DigitalOut(config->rts);
    g_com_cts =             new InterruptIn(config->cts);
    
    g_com_cts->rise(&cts_isr);
    
    g_com_serial->format(8, SerialBase::None, 1);
    g_com_serial->attach(&rx_isr, Serial::RxIrq);
    
    osStatus err = g_com_rx_thread.start(d7a_com_rx_thread);
    ASSERT(err == osOK, "Failed to start d7a_com_rx_thread (err: %d)\r\n", err);
    
    err = g_com_tx_thread.start(d7a_com_tx_thread);
    ASSERT(err == osOK, "Failed to start d7a_com_tx_thread (err: %d)\r\n", err);
    
    return D7A_ERR_NONE;
}

// Destructor
d7a_errors_t d7a_com_close(void)
{
    FPRINT("\r\n");
    g_com_rx_thread.terminate();
    g_com_tx_thread.terminate();
    delete g_com_serial;
    delete g_com_rts;
    delete g_com_cts;
    
    return D7A_ERR_NONE;
}


void d7a_com_restart(void)
{
    FPRINT("\r\n");
    
    g_com_serial->attach(NULL, Serial::RxIrq);

    g_com_state = SEARCH_HEADER;
    g_com_skipped_bytes = 0;
    g_com_tx_seq = 0;
    g_com_rx_seq = 0;
    
    g_com_rx_buf.reset();
    
    g_com_serial->attach(&rx_isr, Serial::RxIrq);
}


/**
    Wakes-up modem and send data throught Serial.

    @param const uint8_t*       Pointer to data buffer
    @param int                  Data length
    @return void
*/
static void d7a_com_send(d7a_com_tx_buf_t* tx_buf)
{
    FPRINT("\r\n");
    
    DPRINT("<-- (0x%02X) %d\r\n", tx_buf->buf[4], (tx_buf->len - KAL_COM_HEADER_LEN));
    
    *(g_com_rts) = 1;
    
    //dbg_print_data("", "%02X ", (uint8_t*)tx_buf->buf, tx_buf->len, "\r\n");
    
    Thread::wait(3);
    
    for (uint32_t i=0 ; i<tx_buf->len ; i++)
    {
        g_com_serial->putc(tx_buf->buf[i]);
    }
   
    // Important to not release the ressource too soon
    Thread::wait(2);
    
    *(g_com_rts) = 0;
}

// Formats and send packet throught Serial.
d7a_com_tx_buf_t* d7a_com_new_msg(d7a_com_tx_msg_t* msg)
{    
    uint8_t len = KAL_COM_HEADER_LEN + msg->alen + msg->plen;
    FPRINT("(len:%d)\r\n", len);
    
    d7a_com_tx_buf_t* tx_buf = (d7a_com_tx_buf_t*)MALLOC(sizeof(d7a_com_tx_buf_t) - 1 + len);
    
    // construct serial header
    // concatenate and update tx_seq ID
    uint8_t* p = tx_buf->buf;
    uint8_t* t = p;
    *p++ = (uint8_t)KAL_COM_SYNC_BYTE_0;
    *p++ = (uint8_t)KAL_COM_SYNC_BYTE_1;
    *p++ = (uint8_t)msg->alen + msg->plen;
    *p++ = (uint8_t)g_com_tx_seq++;
    *p++ = (uint8_t)msg->id;

    // copy payload and parameters
    memcpy(p, msg->pbuf, msg->plen);
    p += msg->plen;
    memcpy(p, msg->abuf, msg->alen);
    p += msg->alen;
    
    tx_buf->len = (uint32_t)(p - t);
    
    ASSERT(tx_buf->len == len, "New msg wrong length %d expected %d\r\n", tx_buf->len, len);
    
    return tx_buf;
}

void d7a_com_post_msg(d7a_com_tx_msg_t* msg)
{
    FPRINT("\r\n");
    
    g_com_tx_queue.put(d7a_com_new_msg(msg));
}

void d7a_com_dump(uint8_t* buf, uint8_t len, d7a_com_flow_t flow)
{
    d7a_com_tx_msg_t msg;

    msg.id = flow;
    msg.pbuf = buf;
    msg.plen = len;
    msg.alen = 0;
    d7a_com_post_msg(&msg);
}

static void d7a_com_new_pkt(d7a_com_rx_msg_t* pkt)
{
    //FPRINT("\r\n");

    DPRINT("--> (0x%02X) %d\r\n", pkt->id, pkt->blen);

    // Distribute packet types to processes
    switch (KAL_COM_FLOWID(pkt->id))
    {
        case KAL_COM_FLOWID_FS:
            d7a_fs_new_pkt(pkt);
            break;
        case KAL_COM_FLOWID_CMD:
            d7a_modem_new_pkt(pkt);
            break;
        case KAL_COM_FLOWID_ALP:
            d7a_alp_new_pkt(pkt);
            break;
        case KAL_COM_FLOWID_SYS:
            // This has to be here to avoid going to another process
            if (pkt->id == KAL_COM_FLOW_SYS_XON)
            {
                FREE(pkt);
                DPRINT("XON\r\n");
                g_com_tx_thread.signal_set(XON_SIGNAL);
            }
            else if (pkt->id == KAL_COM_FLOW_SYS_XOFF)
            {
                FREE(pkt);
                DPRINT("XOFF\r\n");
                d7a_sys_xack();
            }
            else
            {
                d7a_sys_new_pkt(pkt);
            }
            break;
        default:
            EPRINT("Untreated pkt type 0x%02X\r\n", pkt->id);
            FREE(pkt);
            break;
    }
}


/**
    Reads the Rx buffer, parses the packets

    @param void
    @return void
*/
static void parse_packet_header(void)
{
    FPRINT("\r\n");
    
    uint8_t header[KAL_COM_HEADER_LEN];
    uint8_t seqnum;
    
    ASSERT(g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN, "Not enough data for header\r\n");
    
    g_com_skipped_bytes = 0;
    
    header[0] = g_com_rx_buf.pop();
    
    while (g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN - 1)
    {
        header[1] = g_com_rx_buf.pop();
        
        // Check sync bytes
        if(KAL_COM_SYNC_BYTE_0 == header[0] && KAL_COM_SYNC_BYTE_1 == header[1])
        {
            // Copy header
            g_com_rx_buf.get(&header[2], KAL_COM_HEADER_LEN - 2);
            
            // Fill temp header
            g_com_msg.blen = header[2];
            seqnum = header[3];
            g_com_msg.id = header[4];
            
            // Update seqnum
            WARNING(g_com_rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", g_com_rx_seq, seqnum);
            g_com_rx_seq = seqnum + 1;
            
            // search for body
            g_com_state = SEARCH_BODY;
            
            // Start parsing if data is already available
            if (g_com_rx_buf.available_data() >= g_com_msg.blen)
            {
                g_com_state = PARSE_BODY;
                g_com_data_parsing.release();
            }
            
            //DPRINT("COM header found (id: %02X seq: %d body: %d/%d bytes)\r\n", g_com_msg.id, seqnum, g_com_rx_buf.available_data(), g_com_msg.blen);
            break;
        }
        else
        {
            // Shift by 1 byte
            WARNING(false, "COM Skipped byte 0x%02X.\r\n", header[0]);
            header[0] = header[1];
        }
    }
}

/**
    Reads the Rx buffer, parses the packets

    @param void
    @return void
*/
static void parse_packet_body(void)
{
    ASSERT(g_com_rx_buf.available_data() >= g_com_msg.blen, "Not enough data for body\r\n");
    
    if (KAL_COM_FLOWID(g_com_msg.id) != KAL_COM_FLOWID_TRC)
    {
        //DPRINT("COM body found (%d bytes)\r\n", g_com_msg.blen);
        
        d7a_com_rx_msg_t* pkt = (d7a_com_rx_msg_t*)MALLOC(sizeof(d7a_com_rx_msg_t) - 1 + g_com_msg.blen);

        // copy data to buffer
        pkt->blen = g_com_msg.blen;
        pkt->id = g_com_msg.id;
        g_com_rx_buf.get(pkt->buffer, g_com_msg.blen);

        // add packet to queue
        d7a_com_new_pkt(pkt);
    }
    else
    {
        // Ignore packet
        //DPRINT("Ignore pkt id %02X\r\n", g_com_msg.id);
        g_com_rx_buf.get(NULL, g_com_msg.blen);
    }
    
    // Seach for next header
    g_com_state = SEARCH_HEADER;
    
    // Start parsing if data is already available
    if (g_com_rx_buf.available_data() >= KAL_COM_HEADER_LEN)
    {
        g_com_state = PARSE_HEADER;
        g_com_data_parsing.release();
    }
}


// Thread for parsing packets from RX buffer.
void d7a_com_rx_thread()
{
    FPRINT("(id:0x%08x)\r\n", osThreadGetId());
    while (true)
    {
        // wait for data available
        g_com_data_parsing.wait();
        
        if (g_com_state == PARSE_HEADER)
        {
            parse_packet_header();
        }
        else if (g_com_state == PARSE_BODY)
        {
            parse_packet_body();
        }
    }
}

void d7a_com_tx_thread()
{
    FPRINT("(id:0x%08x)\r\n", osThreadGetId());
    
    d7a_com_tx_buf_t* msg;
    osEvent evt;
    uint8_t flow_id;
    
    while (true)
    {
        // wait for data to send
        evt = g_com_tx_queue.get();
        msg = (evt.status == osEventMessage)? (d7a_com_tx_buf_t*)evt.value.p : NULL;

        
        // send message
        if (msg != NULL)
        {
            flow_id = msg->buf[4];

            d7a_com_send(msg);
            FREE(msg);
            
            if (KAL_COM_FLOW_SYS_XACK == flow_id)
            {
                DPRINT("XACK\r\n");
                g_com_tx_thread.signal_wait(XON_SIGNAL);
            }
        }
    }
}