Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of d7a_1x by
Diff: src/d7a_com.cpp
- Revision:
- 33:f9a542d3efaa
- Parent:
- 31:ab9bfdbc6b44
- Child:
- 34:1311cc53201a
--- a/src/d7a_com.cpp Thu Jun 16 12:10:10 2016 +0000 +++ b/src/d7a_com.cpp Fri Jun 24 10:11:19 2016 +0000 @@ -29,6 +29,11 @@ uint16_t read_idx; // Number of bytes available in RX buffer volatile uint16_t data_available; + // Last Number of available data + volatile uint16_t last_available; + // Number of skipped bytes while parsing + uint16_t skipped_bytes; + // Port TX sequence number uint8_t tx_seq; // Port RX sequence number @@ -85,20 +90,16 @@ { // Loop just in case more than one character is in UART's receive FIFO buffer // Stop if buffer full - while ((g_com_ctx.serial->readable()) && (((g_com_ctx.write_idx + 1) % g_com_ctx.rx_buffer_size) != g_com_ctx.read_idx)) { + while (g_com_ctx.serial->readable()) { g_com_ctx.rx_buffer[g_com_ctx.write_idx] = g_com_ctx.serial->getc(); g_com_ctx.data_available++; - //DPRINT("."); + //dbg_print("."); g_com_ctx.write_idx = (g_com_ctx.write_idx + 1) % g_com_ctx.rx_buffer_size; } - + // unlock data parsing thread - // do not parse if minimum packet size is not available - if (g_com_ctx.data_available > KAL_COM_HEADER_LEN) - { - g_com_ctx.data_parsing->release(); - } + g_com_ctx.data_parsing->release(); return; } @@ -131,6 +132,8 @@ g_com_ctx.rx_buffer = (uint8_t*)MALLOC(g_com_ctx.rx_buffer_size); g_com_ctx.cts->rise(&cts_isr); g_com_ctx.data_available = 0; + g_com_ctx.last_available = 0; + g_com_ctx.skipped_bytes = 0; g_com_ctx.write_idx = 0; g_com_ctx.read_idx = 0; g_com_ctx.tx_seq = 0; @@ -151,7 +154,8 @@ It is here manualy corrected after an oscilloscope measure. Normal Baudrate should be 115200. */ - g_com_ctx.serial->baud(117000); // XXX + //g_com_ctx.serial->baud(117000); // XXX + g_com_ctx.serial->baud(115200); // XXX //g_com_ctx.serial->baud(58500); // XXX g_com_ctx.serial->format(8, SerialBase::None, 1); g_com_ctx.serial->attach(&rx_isr, Serial::RxIrq); @@ -219,7 +223,6 @@ uint16_t len = msg->alen + msg->plen; FPRINT("(len:%d)\r\n", len); - buf = (uint8_t*)MALLOC(KAL_COM_HEADER_LEN + len); // construct serial header @@ -236,12 +239,6 @@ memcpy(buf + KAL_COM_HEADER_LEN + msg->plen, msg->abuf, msg->alen); d7a_com_post_tx(buf, len); - - // Stats - g_com_ctx.tx_pkt_count++; - g_com_ctx.tx_byte_count += len; - - DPRINT("<-- %d (0x%02X)\r\n", (len - KAL_COM_HEADER_LEN), msg->id); } @@ -256,14 +253,23 @@ d7a_com_send_msg(&msg); } - +void d7a_com_flush_rx(void) +{ + g_com_ctx.data_available = 0; + g_com_ctx.last_available = 0; + g_com_ctx.skipped_bytes = 0; + g_com_ctx.write_idx = 0; + g_com_ctx.read_idx = 0; + g_com_ctx.tx_seq = 0; + g_com_ctx.rx_seq = 0; +} static void d7a_com_new_pkt(d7a_com_rx_msg_t* pkt) { //FPRINT("\r\n"); if (KAL_COM_FLOWID(pkt->id) != KAL_COM_FLOWID_TRC) { - DPRINT("--> %d (0x%02X)\r\n", pkt->blen, pkt->id); + DPRINT("--> (0x%02X) %d\r\n", pkt->id, pkt->blen); } // Distribute packet types to processes @@ -308,84 +314,117 @@ } } + /** Reads the Rx buffer, parses the packets and adds them to _pkt_queue @param void @return void */ -static bool parse_packet(const uint16_t start_idx, const uint16_t data_available) +static void parse_packet(void) { - uint16_t data_read = 0; - uint8_t data = 0; - uint8_t state = 0; + uint16_t base = 0; + uint16_t current = 0; + uint8_t byte; + + // Packet fields uint8_t seqnum = 0; uint8_t len = 0; uint8_t id = 0; + uint8_t i = 0; - bool pkt_found = false; + + // Stats + uint16_t nb_pkt = 0; + + //dbg_print("Parsing %d bytes\r\n", g_com_ctx.data_available); - while (data_read < data_available) + while (g_com_ctx.data_available >= (base + KAL_COM_HEADER_LEN + 1)) { - // do not parse if minimum packet size is not available - if (0 == state && (data_available - data_read) <= KAL_COM_HEADER_LEN) + // Pop data + byte = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + (base + current)) % g_com_ctx.rx_buffer_size]; + //DPRINT("Treating base %d byte %d of %d: 0x%02X\r\n", base, current, g_com_ctx.data_available, byte); + + // search first sync byte + if (0 == current) { - break; - } + WARNING(g_com_ctx.skipped_bytes != 1, "COM not sync. Searching for header.\r\n"); - // Pop data - data = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + data_read) % g_com_ctx.rx_buffer_size]; - data_read++; - //DPRINT("Read %d of %d bytes: 0x%02X\r\n", data_read, data_available, data); - - // search first sync byte - if (0 == state && KAL_COM_SYNC_BYTE_0 == data) - { - state++; + if(KAL_COM_SYNC_BYTE_0 == byte) + { + current++; + } + else + { + // Not a packet. Restart at base + 1 + g_com_ctx.skipped_bytes++; + base++; + } + continue; } // look up second sync byte - else if (1 == state && KAL_COM_SYNC_BYTE_1 == data) - { - state++; - } - // look up packet size - else if (2 == state && 0 != data) + else if (1 == current) { - len = data; - // if not enough data is available - if (data_available < data_read + 1 + 1 + len) // data read + seqnum + id + data length + if(KAL_COM_SYNC_BYTE_1 == byte) { - // go back to begining of packet - data_read -= 3; - break; + current++; } else { - state++; + // Not a packet. Restart at base + 1 + g_com_ctx.skipped_bytes++; + base++; + current = 0; } + continue; + } + // look up packet size + else if (2 == current) + { + len = byte; + // if not enough data is available + if ((g_com_ctx.data_available - base) < (KAL_COM_HEADER_LEN + len)) + { + // exit parser + break; + } + + current++; + continue; } // get seqnum - else if (3 == state) + else if (3 == current) + { + seqnum = byte; + current++; + continue; + } + // get id + else if (4 == current) { - seqnum = data; + id = byte; + current++; + continue; + } + // we got a full packet + else if (5 == current) + { + if (g_com_ctx.skipped_bytes) + { + WARNING(false, "COM header found, skipped %d bytes.\r\n", g_com_ctx.skipped_bytes); + g_com_ctx.skipped_bytes = 0; + } + + // Update seqnum if (g_com_ctx.rx_seq != seqnum) { - WARNING(false, "COM Missing %d seqnum\r\n", g_com_ctx.rx_seq - seqnum); + ASSERT(false, "COM Bad seqnum expected:%d got:%d\r\n", seqnum, g_com_ctx.rx_seq); g_com_ctx.rx_seq = seqnum; } - g_com_ctx.rx_seq++; - state++; - } - // get id - else if (4 == state) - { - id = data; - state++; - } - // we got a full packet - else if (5 == state) - { + + nb_pkt++; + if (KAL_COM_FLOWID(id) != KAL_COM_FLOWID_TRC) { d7a_com_rx_msg_t* pkt = NULL; @@ -398,65 +437,38 @@ pkt->id = id; for (i=0 ; i<len ; i++) { - pkt->buffer[i] = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + data_read + i - 1) % g_com_ctx.rx_buffer_size]; + pkt->buffer[i] = g_com_ctx.rx_buffer[(g_com_ctx.read_idx + (base + current + i)) % g_com_ctx.rx_buffer_size]; } - data_read += i - 1; // add packet to queue d7a_com_new_pkt(pkt); - - pkt_found = true; } else { - data_read += len - 1; + // Ignore packet } - - // Stats - g_com_ctx.rx_pkt_count++; - g_com_ctx.rx_byte_count += (len + KAL_COM_HEADER_LEN); + + // Go to EOP + current += len - 1; - break; + // Search next packet + base += current + 1; + current = 0; + continue; } else { - // Restart search - state = 0; + ASSERT(false, "D7A COM parser!\r\n"); } } - - if (data_read) - { - uint16_t discarded_bytes; - if (pkt_found) - { - // If packet is found, bytes before the packet are discarded - discarded_bytes = data_read - len - KAL_COM_HEADER_LEN; - } - else - { - // Discard all bytes - discarded_bytes = data_read; - } - - if (discarded_bytes) - { - //EPRINT("Discarding %d bytes\r\n", discarded_bytes); -#if 0 - for (i=0 ; i<discarded_bytes ; i++) - { - DPRINT("%02X ", g_com_ctx.rx_buffer[(g_com_ctx.read_idx + i) % g_com_ctx.rx_buffer_size]); - } - DPRINT("\r\n"); -#endif - } - - // update buffer indexes - g_com_ctx.read_idx = (g_com_ctx.read_idx + data_read) % g_com_ctx.rx_buffer_size; - g_com_ctx.data_available -= data_read; + + // update buffer indexes + g_com_ctx.read_idx = (g_com_ctx.read_idx + base) % g_com_ctx.rx_buffer_size; + g_com_ctx.data_available -= base; + + if (base) { + DPRINT("Parsed %d bytes of %d (%d packets)\r\n", base, g_com_ctx.data_available + base, nb_pkt); } - - return pkt_found; } @@ -468,8 +480,13 @@ { // wait for data available g_com_ctx.data_parsing->wait(); + ASSERT(g_com_ctx.data_available <= g_com_ctx.rx_buffer_size, "COM buffer overflow\r\n"); // search for packets - parse_packet(g_com_ctx.read_idx, g_com_ctx.data_available); + if ((g_com_ctx.last_available != g_com_ctx.data_available) && (g_com_ctx.data_available >= (KAL_COM_HEADER_LEN + 1))) + { + g_com_ctx.last_available = g_com_ctx.data_available; + parse_packet(); + } } } @@ -493,6 +510,7 @@ // send message if (msg != NULL) { + DPRINT("<-- (0x%02X) %d\r\n", msg->buf[4], (msg->len - KAL_COM_HEADER_LEN)); d7a_com_send(msg->buf, msg->len); FREE(msg->buf); FREE(msg);