File content as of revision 5:a06d239f3b3e:

#include "WizziCom.h"

#if 0
    #define COM_DPRINT(...)         PRINT(__VA_ARGS__)
    #define COM_DPRINT_DATA(...)    PRINT_DATA(__VA_ARGS__)
    #define COM_FPRINT(...)         FPRINT(__VA_ARGS__)
#else
    #define COM_DPRINT(...);
    #define COM_DPRINT_DATA(...);
    #define COM_FPRINT(...);
#endif

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

// +--------------+--------+--------+--------+---- - - - - - - - - - - --------+
// |     SYNC     |   LEN  |   SEQ  |   ID   |             PAYLOAD             |
// +--------------+--------+--------+--------+---- - - - - - - - - - - --------+
//
//      2 bytes     1 byte   1 byte   1 byte              LEN bytes
// |<------------>|<------>|<------>|<------>|<--- - - - - - - - - - - ------->|


// first byte of the sync word
// (ASCII start of heading)
#define KAL_COM_SYNC_BYTE_0          0x01

// second byte of the sync word
// (ASCII group separator)
#define KAL_COM_SYNC_BYTE_1          0x1F

// message header length in byte
#define KAL_COM_HEADER_LEN           5


//======================================================================
// wizzi_com_fid_t
//----------------------------------------------------------------------
// Enumerator of serial Flow-ids
//======================================================================
typedef enum
{
    // Trace channel
    KAL_COM_FLOWID_TRC = 0,
    // General purpose Command channel
    KAL_COM_FLOWID_CMD,
    // ALP channel
    KAL_COM_FLOWID_ALP,
    // System notifications
    KAL_COM_FLOWID_SYS,
    // File System channel
    KAL_COM_FLOWID_FS,

    KAL_COM_FLOWID_QTY

} wizzi_com_fid_t;

#define KAL_COM_FLOW(fid,type)      ((((fid)&0x7)<<4) | ((type)&0xF))
#define KAL_COM_FLOWID(id)          (((id)>>4)&0x7)
#define KAL_COM_FLOWID_REDIRECT     0x80

//======================================================================
// wizzi_com_flow_t
//----------------------------------------------------------------------
// Enumerator of serial flows
//======================================================================
typedef enum
{
    // Default printf type
    KAL_COM_FLOW_PRINTF                     = KAL_COM_FLOW(KAL_COM_FLOWID_TRC,0),
    // Substitute the string by a codeword
    // interpreted by the PC com tool
    KAL_COM_FLOW_PRINTF_COMPRESSED          = KAL_COM_FLOW(KAL_COM_FLOWID_TRC,1),
    // Display the payload as hex data
    KAL_COM_FLOW_PRINT_HEX                  = KAL_COM_FLOW(KAL_COM_FLOWID_TRC,2),

    // AT command
    KAL_COM_FLOW_AT_CMD                     = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,0),
    // AT command response
    KAL_COM_FLOW_AT_RESP                    = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,1),
    // AT unsolicited message
    KAL_COM_FLOW_AT_UNS                     = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,2),
    // AT unsolicited error
    KAL_COM_FLOW_AT_ERR                     = KAL_COM_FLOW(KAL_COM_FLOWID_ALP,3),

    // Remote Commands
    KAL_COM_FLOW_CMD                        = KAL_COM_FLOW(KAL_COM_FLOWID_CMD,0),

    // Remote System reset
    KAL_COM_FLOW_SYS_RST                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,0),
    // Button Emulator
    KAL_COM_FLOW_SYS_BUTTON                 = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,1),
    // Dump Debug parameters
    KAL_COM_FLOW_SYS_INFO                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,2),
    // CUP signalisation
    KAL_COM_FLOW_SYS_CUP                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,3),
    // Ping distant COM
    KAL_COM_FLOW_SYS_PING                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,4),
    // Pong from distant COM
    KAL_COM_FLOW_SYS_PONG                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,5),
    // Enable system config from distant COM
    KAL_COM_FLOW_SYS_CFG                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,6),
    // Configure Output Trace level from distant COM
    KAL_COM_FLOW_SYS_TLEV                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,7),
    // Configure COM port redirection
    KAL_COM_FLOW_SYS_REDIR                  = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,8),
    // Flow control signalling
    KAL_COM_FLOW_SYS_XON                    = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,9),
    KAL_COM_FLOW_SYS_XOFF                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,10),
    KAL_COM_FLOW_SYS_XACK                   = KAL_COM_FLOW(KAL_COM_FLOWID_SYS,11),

    // File System Command/Response
    KAL_COM_FLOW_FS_CMD                     = KAL_COM_FLOW(KAL_COM_FLOWID_FS,0),
    KAL_COM_FLOW_FS_RESP                    = KAL_COM_FLOW(KAL_COM_FLOWID_FS,1),
    
} wizzi_com_flow_t;




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

void wizzi_com_rx_thread();
void wizzi_com_tx_thread();

const uint8_t g_type_to_flow[WIZZICOM_PKT_QTY] = {
    // Default printf type
    KAL_COM_FLOW_PRINTF,
    // Substitute the string by a codeword
    // interpreted by the PC com tool
    KAL_COM_FLOW_PRINTF_COMPRESSED,
    // Display the payload as hex data
    KAL_COM_FLOW_PRINT_HEX,

    // AT command
    KAL_COM_FLOW_AT_CMD,
    // AT command response
    KAL_COM_FLOW_AT_RESP,
    // AT unsolicited message
    KAL_COM_FLOW_AT_UNS,
    // AT unsolicited error
    KAL_COM_FLOW_AT_ERR,

    // Remote Commands
    KAL_COM_FLOW_CMD,
    
    // Remote System reset
    KAL_COM_FLOW_SYS_RST,
    // Button Emulator
    KAL_COM_FLOW_SYS_BUTTON,
    // Dump Debug parameters
    KAL_COM_FLOW_SYS_INFO,
    // CUP signalisation
    KAL_COM_FLOW_SYS_CUP,
    // Ping distant COM
    KAL_COM_FLOW_SYS_PING,
    // Pong from distant COM
    KAL_COM_FLOW_SYS_PONG,
    // Enable system config from distant COM
    KAL_COM_FLOW_SYS_CFG,
    // Configure Output Trace level from distant COM
    KAL_COM_FLOW_SYS_TLEV,
    // Configure COM port redirection
    KAL_COM_FLOW_SYS_REDIR,
    // Flow control signalling
    KAL_COM_FLOW_SYS_XON,
    KAL_COM_FLOW_SYS_XOFF,
    KAL_COM_FLOW_SYS_XACK,

    // File System Command/Response
    KAL_COM_FLOW_FS_CMD,
    KAL_COM_FLOW_FS_RESP
};

uint8_t wizzicom_type_to_flow(uint8_t packet_type)
{
    return g_type_to_flow[packet_type];
}

uint8_t wizzicom_flow_to_type(uint8_t flow_id)
{
    // Get packet type from flow_id
    uint8_t packet_type = 0;
    while ((g_type_to_flow[packet_type] != flow_id) && (packet_type < WIZZICOM_PKT_QTY))
    {
        packet_type++;
    }
    return packet_type;
}

WizziCom::WizziCom(PinName tx, PinName rx, PinName irq_out, PinName irq_in) : 
_data_parsing(0),
_irq_in_int(0),
_rx_thread(osPriorityHigh, 512, NULL),
_tx_thread(osPriorityHigh, 512, NULL),
_callback_thread(osPriorityRealtime, 1024, NULL)
{
    _state = SEARCH_HEADER;
    _skipped_bytes = 0;
    _tx_seq = 0;
    _rx_seq = 0;
    
    memset(_callback, 0, sizeof(_callback));
    
    _serial = new RawSerial(tx, rx, 115200);
    _serial->format(8, SerialBase::None, 1);
    _serial->attach(callback(this, &WizziCom::_rx_isr), Serial::RxIrq);
    
    _irq_out = (irq_out != NC)? new DigitalOut(irq_out) : NULL;
    
    if (irq_in != NC)
    {
        _irq_in = new InterruptIn(irq_in);
        _irq_in->rise(callback(this, &WizziCom::_irq_in_isr));
    }
    else
    {
        _irq_in = NULL;
    }
    
    osStatus err = _rx_thread.start(callback(this, &WizziCom::_thread_rx));
    ASSERT(err == osOK, "Failed to start WizziCom _thread_rx (err: %d)\r\n", err);
    
    err = _tx_thread.start(callback(this, &WizziCom::_thread_tx));
    ASSERT(err == osOK, "Failed to start WizziCom _thread_tx (err: %d)\r\n", err);
    
    err = _callback_thread.start(callback(this, &WizziCom::_thread_callback));
    ASSERT(err == osOK, "Failed to start WizziCom _thread_callback (err: %d)\r\n", err);
}

WizziCom::~WizziCom()
{  
    _rx_thread.terminate();
    _tx_thread.terminate();
    _callback_thread.terminate();
    delete _serial;
    delete _irq_out;
    delete _irq_in;
}

void WizziCom::attach(WizziComCallback function, WizziComPacketType packet_type)
{
    _callback[packet_type] = callback(function);
}

void WizziCom::reset(void)
{
    COM_FPRINT("\r\n");
    
    _serial->attach(NULL, Serial::RxIrq);

    _state = SEARCH_HEADER;
    _skipped_bytes = 0;
    _tx_seq = 0;
    _rx_seq = 0;
    
    _rx_buf.reset();
    
    _serial->attach(callback(this, &WizziCom::_rx_isr), Serial::RxIrq);
}

void WizziCom::send(WizziComPacketType type, uint8_t length, uint8_t* data)
{
    wizzi_com_tx_msg_t msg;

    msg.id = g_type_to_flow[type];
    msg.pbuf = data;
    msg.plen = length;
    msg.alen = 0;
    
    _post_msg(&msg);
}

void WizziCom::send(WizziComPacket_t* packet)
{
    wizzi_com_tx_msg_t msg;

    msg.id = g_type_to_flow[packet->type];
    msg.pbuf = packet->data;
    msg.plen = packet->length;
    msg.alen = 0;
    
    _post_msg(&msg);
}

void WizziCom::print_raw(char* str)
{
    _send_raw((uint8_t*)str, (uint32_t)strlen(str));
}

void WizziCom::send_raw(uint8_t* data, uint32_t len)
{
    _send_raw(data, len);
}

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

    @param void
    @return void
*/
void WizziCom::_rx_isr()
{
// Loop just in case more than one character is in UART's receive FIFO buffer
    while (_serial->readable())
    {
        _rx_buf.push(_serial->getc());
        //PRINT("-");
    }
    
    // unlock data parsing thread
    if (_state == SEARCH_HEADER && _rx_buf.available_data() >= KAL_COM_HEADER_LEN)
    {
        _state = PARSE_HEADER;
        _data_parsing.release();
    }
    else if (_state == SEARCH_BODY && _rx_buf.available_data() >= _msg.blen)
    {
        _state = PARSE_BODY;
        _data_parsing.release();
    }
}

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

    @param void
    @return void
*/
void WizziCom::_irq_in_isr()
{
    //PRINT("IRQ_IN_ISR\r\n");
    //_irq_in_int->release();
}

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

    @param const uint8_t*       Pointer to data buffer
    @param uint32_t             Data length
    @return void
*/
void WizziCom::_send_raw(uint8_t* data, uint32_t len)
{
    if (_irq_out)
    {
        *(_irq_out) = 1;
        Thread::wait(3);
    }
    
    for (uint32_t i=0 ; i<len ; i++)
    {
        _serial->putc(data[i]);
    }
   
    if (_irq_out)
    {
        // Important to not release the ressource too soon
        Thread::wait(2);   
        *(_irq_out) = 0;
    }
}

void WizziCom::_sys_xack(void)
{
    send(WizziComPacketSysXack, 0, NULL);
}

// Formats to send packet throught Serial.
wizzi_com_tx_buf_t* WizziCom::_new_msg(wizzi_com_tx_msg_t* msg)
{    
    uint8_t len = KAL_COM_HEADER_LEN + msg->alen + msg->plen;
    COM_FPRINT("(len:%d)\r\n", len);
    
    wizzi_com_tx_buf_t* tx_buf = (wizzi_com_tx_buf_t*)MALLOC(sizeof(wizzi_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)_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 WizziCom::_post_msg(wizzi_com_tx_msg_t* msg)
{
    COM_FPRINT("\r\n");
    
    ASSERT(_tx_queue.put(_new_msg(msg)) == osOK, "WizziCom TX queue full!\r\n");
}

void WizziCom::_new_pkt(WizziComPacket_t* packet)
{
    //COM_COM_FPRINT("\r\n");

    COM_DPRINT("--> (%02d) %d\r\n", packet->type, packet->length);

    // Distribute packet types to callbacks
    switch (packet->type)
    {
        case WizziComPacketSysXon:
            FREE(packet);
            COM_DPRINT("XON\r\n");
            _tx_thread.signal_set(XON_SIGNAL);
            break;
        case WizziComPacketSysXoff:
            FREE(packet);
            COM_DPRINT("XOFF\r\n");
            _sys_xack();
            break;
        default:
            ASSERT(_rx_queue.put(packet) == osOK, "WizziCom RX queue full!\r\n");
            break;
    }
}


/**
    Reads the Rx buffer, parses the packets

    @param void
    @return void
*/
void WizziCom::_parse_packet_header(void)
{
    COM_FPRINT("\r\n");
    
    uint8_t header[KAL_COM_HEADER_LEN];
    uint8_t seqnum;
    
    ASSERT(_rx_buf.available_data() >= KAL_COM_HEADER_LEN, "Not enough data for header\r\n");
    
    _skipped_bytes = 0;
    
    header[0] = _rx_buf.pop();
    
    while (_rx_buf.available_data() >= KAL_COM_HEADER_LEN - 1)
    {
        header[1] = _rx_buf.pop();
        
        // Check sync bytes
        if(KAL_COM_SYNC_BYTE_0 == header[0] && KAL_COM_SYNC_BYTE_1 == header[1])
        {
            // Copy header
            _rx_buf.get(&header[2], KAL_COM_HEADER_LEN - 2);
            
            // Fill temp header
            _msg.blen = header[2];
            seqnum = header[3];
            _msg.id = wizzicom_flow_to_type(header[4]);
            
            // Update seqnum
            WARNING(_rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", _rx_seq, seqnum);
            _rx_seq = seqnum + 1;
            
            // search for body
            _state = SEARCH_BODY;
            
            // Start parsing if data is already available
            if (_rx_buf.available_data() >= _msg.blen)
            {
                _state = PARSE_BODY;
                _data_parsing.release();
            }
            
            //COM_DPRINT("COM header found (id: %02X seq: %d body: %d/%d bytes)\r\n", _msg.id, seqnum, _rx_buf.available_data(), _msg.blen);
            break;
        }
        else
        {
            // Shift by 1 byte
            //WARNING(false, "COM Skipped byte 0x%02X.\r\n", header[0]);
            _skipped_bytes++;
            header[0] = header[1];
        }
    }
    
    WARNING(!_skipped_bytes, "COM Skipped %d bytes.\r\n", _skipped_bytes);
}

/**
    Reads the Rx buffer, parses the packets

    @param void
    @return void
*/
void WizziCom::_parse_packet_body(void)
{
    ASSERT(_rx_buf.available_data() >= _msg.blen, "Not enough data for body\r\n");
    
    if (_callback[_msg.id] || _callback[WizziComPacketUntreated])
    {
        //COM_DPRINT("COM body found (%d bytes)\r\n", _msg.blen);
        
        WizziComPacket_t* pkt = (WizziComPacket_t*)MALLOC(sizeof(WizziComPacket_t) - 1 + _msg.blen);

        // copy data to buffer
        pkt->length = _msg.blen;
        pkt->type = _msg.id;
        
        if (_msg.blen)
        {
            _rx_buf.get(pkt->data, _msg.blen);
        }

        // add packet to queue
        _new_pkt(pkt);
    }
    else
    {
        // Ignore packet
        //COM_DPRINT("Ignore pkt id %02X\r\n", _msg.id);
        if (_msg.blen)
        {
            _rx_buf.get(NULL, _msg.blen);
        }
    }
    
    // Seach for next header
    _state = SEARCH_HEADER;
    
    // Start parsing if data is already available
    if (_rx_buf.available_data() >= KAL_COM_HEADER_LEN)
    {
        _state = PARSE_HEADER;
        _data_parsing.release();
    }
}

// Thread for calling callbacks
// Like arg, arg thread is stalled by callbacks but not the parsing thread.
void WizziCom::_thread_callback(void)
{
    osEvent evt;
    WizziComPacket_t* packet;
    
    COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
    while (true)
    {
        // wait for available packet
        evt = _rx_queue.get();
        packet = (evt.status == osEventMessage)? (WizziComPacket_t*)evt.value.p : NULL;
        
        if (packet != NULL)
        {
            if (_callback[packet->type])
            {
                _callback[packet->type].call(this, packet);
            }
            else if (_callback[WizziComPacketUntreated])
            {
                _callback[WizziComPacketUntreated].call(this, packet);
            }
            else
            {
                EPRINT("Untreated pkt type %d in queue!\r\n", packet->type);
                FREE(packet);
            }
        }
    }
}


// Thread for parsing packets from RX buffer.
void WizziCom::_thread_rx(void)
{
    COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
    while (true)
    {
        // wait for data available
        _data_parsing.wait();
        
        if (_state == PARSE_HEADER)
        {
            _parse_packet_header();
        }
        else if (_state == PARSE_BODY)
        {
            _parse_packet_body();
        }
    }
}

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

        
        // send message
        if (msg != NULL)
        {
            flow_id = msg->buf[4];
                
            COM_DPRINT("<-- (%02d) %d\r\n", wizzicom_flow_to_type(flow_id), (msg->len - KAL_COM_HEADER_LEN));

            _send_raw(msg->buf, msg->len);
            FREE(msg);
            
            if (KAL_COM_FLOW_SYS_XACK == flow_id)
            {
                COM_DPRINT("XACK\r\n");
               _tx_thread.signal_wait(XON_SIGNAL);
            }
        }
    }
}