Diff: WizziCom.cpp

Revision:

0:95b73d0b37b7

Child:

1:ca1c9bfb1cf4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WizziCom.cpp	Wed Apr 26 12:29:35 2017 +0000
@@ -0,0 +1,628 @@
+#include "WizziCom.h"
+
+#if 1
+    #define COM_DPRINT(...)         DPRINT(__VA_ARGS__)
+    #define COM_DPRINT_DATA(...)    DPRINT_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
+};
+
+WizziCom::WizziCom(PinName tx, PinName rx, PinName irq_out, PinName irq_in) : 
+_data_parsing(0),
+_irq_in_int(0),
+_rx_thread(osPriorityRealtime, 512, NULL),
+_tx_thread(osPriorityHigh, 512, NULL),
+_callback_thread(osPriorityLow, 512, 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);
+    
+    FREE(packet);
+}
+
+
+/**
+    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 int                  Data length
+    @return void
+*/
+void WizziCom::_send_raw(wizzi_com_tx_buf_t* tx_buf)
+{
+    COM_FPRINT("\r\n");
+    
+    COM_DPRINT("<-- (%02d) %d\r\n", _flow_to_type(tx_buf->buf[4]), (tx_buf->len - KAL_COM_HEADER_LEN));
+    
+    if (_irq_out)
+    {
+        *(_irq_out) = 1;    
+        Thread::wait(3);
+    }
+    
+    for (uint32_t i=0 ; i<tx_buf->len ; i++)
+    {
+        _serial->putc(tx_buf->buf[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");
+}
+
+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;
+}
+
+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 = _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[WizziComPacketOther])
+    {
+        //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[WizziComPacketOther])
+            {
+                _callback[WizziComPacketOther].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];
+
+            _send_raw(msg);
+            FREE(msg);
+            
+            if (KAL_COM_FLOW_SYS_XACK == flow_id)
+            {
+                COM_DPRINT("XACK\r\n");
+               _tx_thread.signal_wait(XON_SIGNAL);
+            }
+        }
+    }
+}