Diff: WizziCom.cpp

Revision:

9:0140247bab90

Parent:

8:42e00820df36

Child:

11:9b623641fd85

--- a/WizziCom.cpp	Tue Aug 20 13:48:15 2019 +0000
+++ b/WizziCom.cpp	Mon Jan 25 09:45:10 2021 +0000
@@ -201,8 +201,13 @@
     
     memset(_callback, 0, sizeof(_callback));
     
+    _cbuf = &_cbuf_h;
+    kal_buf_circ_create_static(&_cbuf_h, _cbuf_b, RX_BUF_SIZE, sizeof(uint8_t));
+    
     _serial = new RawSerial(tx, rx, 115200);
     _serial->format(8, SerialBase::None, 1);
+    //_serial->attach(callback(this, &WizziCom::_rx_isr), Serial::RxIrq);
+    _serial->read(&_rx_byte, sizeof(uint8_t), callback(this, &WizziCom::_rx_done_isr));
     _serial->set_flow_control(SerialBase::Disabled);
     
     _irq_out = (irq_out != NC)? new DigitalOut(irq_out) : NULL;
@@ -216,7 +221,7 @@
     {
         _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);
     
@@ -225,9 +230,6 @@
     
     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()
@@ -300,40 +302,29 @@
     _tx_done.release();
 }
 
+void WizziCom::_rx_isr(void)
+{
+    uint8_t byte;
+    
+    while (_serial->readable())
+    {
+        byte = _serial->getc();
+        kal_buf_circ_push(_cbuf, &byte);
+    }
+
+    _rx_done.release();
+}
+
 void WizziCom::_rx_done_isr(int event)
 {
-    if (SEARCH_HEADER == _state)
-    {
-        // 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();
-        }
-    }
-    // Got body
-    else
-    {
-        _rx_done.release();
-        _get_header(KAL_COM_HEADER_LEN);
-    }
+    uint8_t byte = _rx_byte;
+    
+    _serial->read(&_rx_byte, sizeof(uint8_t), callback(this, &WizziCom::_rx_done_isr));
+    
+    kal_buf_circ_push(_cbuf, &byte);
+
+    _rx_done.release();
 }
-
 /**
     CTS pin Interrupt Service Routine.
     For flow control (not yet inplemented)
@@ -347,18 +338,6 @@
     //_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.
 
@@ -453,62 +432,6 @@
     }
 }
 
-
-/**
-    Reads the Rx buffer, parses the packets
-
-    @param void
-    @return void
-*/
-void WizziCom::_parse_packet(void)
-{
-    COM_FPRINT("\r\n");
-    
-    uint8_t seqnum;
-    
-    // Check sync bytes
-    if(KAL_COM_SYNC_BYTE_0 == _rx_header[0] && KAL_COM_SYNC_BYTE_1 == _rx_header[1])
-    {
-        // 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;
-        
-        //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;
-            
-            if (_msg.blen)
-            {
-                memcpy(pkt->data, _rx_body, _msg.blen);
-            }
-    
-            // add packet to queue
-            _new_pkt(pkt);
-        }
-        else
-        {
-            // Ignore packet
-            COM_DPRINT("Ignore pkt id %02X\r\n", _msg.id);
-        }
-    }
-    else
-    {
-        // TODO Resync
-        PRINT("Needs Resync\n");
-    }
-}
-
 // Thread for calling callbacks
 // Like arg, arg thread is stalled by callbacks but not the parsing thread.
 void WizziCom::_thread_callback(void)
@@ -546,13 +469,82 @@
 // Thread for parsing packets from RX buffer.
 void WizziCom::_thread_rx(void)
 {
+    uint8_t seqnum;
+    uint8_t header[KAL_COM_HEADER_LEN];
+        
     COM_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
     while (true)
     {
         // wait for data available
         _rx_done.acquire();
         
-        _parse_packet();
+        // Do not start parsing before we got at least a header
+        if (kal_buf_circ_size(_cbuf) < KAL_COM_HEADER_LEN)
+        {
+            continue;
+        }
+        
+        // Copy header from buffer (data stays in buffer)
+        kal_buf_circ_fetch(_cbuf, header, KAL_COM_HEADER_LEN);
+        
+        // Check sync bytes
+        if(KAL_COM_SYNC_BYTE_0 == header[0] && KAL_COM_SYNC_BYTE_1 == header[1])
+        {
+            // Fill temp header
+            _msg.blen = header[2];
+            seqnum = header[3];
+            _msg.id = wizzicom_flow_to_type(header[4]);
+            
+            if (kal_buf_circ_size(_cbuf) < KAL_COM_HEADER_LEN + _msg.blen)
+            {
+                // Not enough bytes in buffer yet
+                //PRINT("COM: Not enough data (%d/%d)\n", kal_buf_circ_size(_cbuf), KAL_COM_HEADER_LEN + _msg.blen);
+                continue;
+            }
+            
+            // Packet valid, drop header from buffer
+            kal_buf_circ_get(_cbuf, NULL, KAL_COM_HEADER_LEN);
+            
+            // Update seqnum
+            WARNING(_rx_seq == seqnum, "COM Bad seqnum expected:%d got:%d\r\n", _rx_seq, seqnum);
+            _rx_seq = seqnum + 1;
+            
+            //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;
+                
+                if (_msg.blen)
+                {
+                    // Get payload from buffer
+                    kal_buf_circ_get(_cbuf, pkt->data, _msg.blen);
+                }
+        
+                // add packet to queue
+                _new_pkt(pkt);
+            }
+            else
+            {
+                // Ignore packet
+                COM_DPRINT("Ignore pkt id %02X\r\n", _msg.id);
+                
+                // Drop payload from buffer
+                kal_buf_circ_get(_cbuf, NULL, _msg.blen);
+            }
+        }
+        else
+        {
+            // Resync
+            PRINT("COM: Resync\n");
+            
+            // Drop a byte
+            kal_buf_circ_pop(_cbuf, NULL);
+        }
     }
 }