Diff: WizziCom.cpp

Revision:

8:42e00820df36

Parent:

7:45fe755f52bc

Child:

9:0140247bab90

--- a/WizziCom.cpp	Thu Aug 01 14:53:04 2019 +0000
+++ b/WizziCom.cpp	Tue Aug 20 13:48:15 2019 +0000
@@ -29,9 +29,6 @@
 // (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
@@ -120,9 +117,7 @@
 
 enum {
     SEARCH_HEADER,
-    PARSE_HEADER,
     SEARCH_BODY,
-    PARSE_BODY,
 };
 
 void wizzi_com_rx_thread();
@@ -194,14 +189,13 @@
 }
 
 WizziCom::WizziCom(PinName tx, PinName rx, PinName irq_out, PinName irq_in) : 
-_data_parsing(0),
+_tx_done(0),
+_rx_done(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;
     
@@ -209,7 +203,7 @@
     
     _serial = new RawSerial(tx, rx, 115200);
     _serial->format(8, SerialBase::None, 1);
-    _serial->attach(callback(this, &WizziCom::_rx_isr), Serial::RxIrq);
+    _serial->set_flow_control(SerialBase::Disabled);
     
     _irq_out = (irq_out != NC)? new DigitalOut(irq_out) : NULL;
     
@@ -231,6 +225,9 @@
     
     err = _callback_thread.start(callback(this, &WizziCom::_thread_callback));
     ASSERT(err == osOK, "Failed to start WizziCom _thread_callback (err: %d)\r\n", err);
+    
+    // Get first header
+    _get_header(KAL_COM_HEADER_LEN);
 }
 
 WizziCom::~WizziCom()
@@ -252,16 +249,8 @@
 {
     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)
@@ -305,25 +294,43 @@
     @param void
     @return void
 */
-void WizziCom::_rx_isr()
+
+void WizziCom::_tx_done_isr(int event)
+{
+    _tx_done.release();
+}
+
+void WizziCom::_rx_done_isr(int event)
 {
-// Loop just in case more than one character is in UART's receive FIFO buffer
-    while (_serial->readable())
+    if (SEARCH_HEADER == _state)
     {
-        _rx_buf.push(_serial->getc());
-        //PRINT("-");
+        // Valid header
+        if (KAL_COM_SYNC_BYTE_0 == _rx_header[0] && KAL_COM_SYNC_BYTE_1 == _rx_header[1])
+        {
+            // Get body
+            if (_rx_header[2])
+            {
+                _get_body(_rx_header[2]);
+            }
+            // Empty body
+            else
+            {
+                _rx_done.release();
+                _get_header(KAL_COM_HEADER_LEN);
+            }
+        }
+        // Invalid header
+        else
+        {
+            // The parser should resynchronize the data
+            _rx_done.release();
+        }
     }
-    
-    // unlock data parsing thread
-    if (_state == SEARCH_HEADER && _rx_buf.available_data() >= KAL_COM_HEADER_LEN)
+    // Got body
+    else
     {
-        _state = PARSE_HEADER;
-        _data_parsing.release();
-    }
-    else if (_state == SEARCH_BODY && _rx_buf.available_data() >= _msg.blen)
-    {
-        _state = PARSE_BODY;
-        _data_parsing.release();
+        _rx_done.release();
+        _get_header(KAL_COM_HEADER_LEN);
     }
 }
 
@@ -340,6 +347,18 @@
     //_irq_in_int->release();
 }
 
+void WizziCom::_get_header(uint8_t length)
+{
+    _state = SEARCH_HEADER;
+    _serial->read(&(_rx_header[KAL_COM_HEADER_LEN - length]), length, callback(this, &WizziCom::_rx_done_isr));
+}
+
+void WizziCom::_get_body(uint8_t length)
+{
+    _state = SEARCH_BODY;
+    _serial->read(_rx_body, length, callback(this, &WizziCom::_rx_done_isr));
+}
+
 /**
     Wakes-up modem and send data throught Serial.
 
@@ -355,15 +374,13 @@
         ThisThread::sleep_for(3);
     }
     
-    for (uint32_t i=0 ; i<len ; i++)
-    {
-        _serial->putc(data[i]);
-    }
+    _serial->write(data, len, callback(this, &WizziCom::_tx_done_isr));
+    _tx_done.acquire();
    
     if (_irq_out)
     {
         // Important to not release the ressource too soon
-        ThisThread::sleep_for(2);   
+        ThisThread::sleep_for(2);
         *(_irq_out) = 0;
     }
 }
@@ -443,109 +460,52 @@
     @param void
     @return void
 */
-void WizziCom::_parse_packet_header(void)
+void WizziCom::_parse_packet(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)
+    // Check sync bytes
+    if(KAL_COM_SYNC_BYTE_0 == _rx_header[0] && KAL_COM_SYNC_BYTE_1 == _rx_header[1])
     {
-        header[1] = _rx_buf.pop();
+        // Fill temp header
+        _msg.blen = _rx_header[2];
+        seqnum = _rx_header[3];
+        _msg.id = wizzicom_flow_to_type(_rx_header[4]);
+        
+        // Update seqnum
+        WARNING(_rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", _rx_seq, seqnum);
+        _rx_seq = seqnum + 1;
         
-        // 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);
+        //PRINT("COM packet (id: %02X seq: %d body: %d bytes)\r\n", _msg.id, seqnum, _msg.blen);
+                    
+        if (_callback[_msg.id] || _callback[WizziComPacketUntreated])
+        {            
+            WizziComPacket_t* pkt = (WizziComPacket_t*)MALLOC(sizeof(WizziComPacket_t) - 1 + _msg.blen);
+    
+            // copy data to buffer
+            pkt->length = _msg.blen;
+            pkt->type = _msg.id;
             
-            // 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)
+            if (_msg.blen)
             {
-                _state = PARSE_BODY;
-                _data_parsing.release();
+                memcpy(pkt->data, _rx_body, _msg.blen);
             }
-            
-            //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;
+    
+            // add packet to queue
+            _new_pkt(pkt);
         }
         else
         {
-            // Shift by 1 byte
-            //WARNING(false, "COM Skipped byte 0x%02X.\r\n", header[0]);
-            _skipped_bytes++;
-            header[0] = header[1];
+            // Ignore packet
+            COM_DPRINT("Ignore pkt id %02X\r\n", _msg.id);
         }
     }
-    
-    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();
+        // TODO Resync
+        PRINT("Needs Resync\n");
     }
 }
 
@@ -576,8 +536,8 @@
             else
             {
                 EPRINT("Untreated pkt type %d in queue!\r\n", packet->type);
-                FREE(packet);
             }
+            FREE(packet);
         }
     }
 }
@@ -590,16 +550,9 @@
     while (true)
     {
         // wait for data available
-        _data_parsing.acquire();
+        _rx_done.acquire();
         
-        if (_state == PARSE_HEADER)
-        {
-            _parse_packet_header();
-        }
-        else if (_state == PARSE_BODY)
-        {
-            _parse_packet_body();
-        }
+        _parse_packet();
     }
 }