539 lines
16 KiB
C
539 lines
16 KiB
C
// Packetized serial transport with HDLC-style framing and sliding window
|
|
//
|
|
// Frame format (before byte stuffing):
|
|
// [0x7E] [SEQ] [TYPE] [LEN] [PAYLOAD...] [CRC_LO] [CRC_HI]
|
|
//
|
|
// Byte stuffing:
|
|
// 0x7E -> 0x7D 0x5E
|
|
// 0x7D -> 0x7D 0x5D
|
|
//
|
|
// CRC-16-CCITT over SEQ+TYPE+LEN+PAYLOAD
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <pc.h>
|
|
#include "packet.h"
|
|
#include "../rs232/rs232.h"
|
|
|
|
|
|
// ========================================================================
|
|
// Internal defines
|
|
// ========================================================================
|
|
|
|
#define FLAG_BYTE 0x7E
|
|
#define ESC_BYTE 0x7D
|
|
#define ESC_XOR 0x20
|
|
|
|
// Frame types
|
|
#define FRAME_DATA 0x00
|
|
#define FRAME_ACK 0x01
|
|
#define FRAME_NAK 0x02
|
|
#define FRAME_RST 0x03
|
|
|
|
// Header size: SEQ + TYPE + LEN
|
|
#define HEADER_SIZE 3
|
|
// CRC size
|
|
#define CRC_SIZE 2
|
|
// Minimum frame size (header + CRC, no payload)
|
|
#define MIN_FRAME_SIZE (HEADER_SIZE + CRC_SIZE)
|
|
// Maximum raw frame size (header + max payload + CRC)
|
|
#define MAX_FRAME_SIZE (HEADER_SIZE + PKT_MAX_PAYLOAD + CRC_SIZE)
|
|
// Maximum stuffed frame size (worst case: every byte stuffed + flag)
|
|
#define MAX_STUFFED_SIZE (1 + MAX_FRAME_SIZE * 2)
|
|
|
|
// Receive buffer must hold at least one max-size stuffed frame
|
|
#define RX_BUF_SIZE (MAX_STUFFED_SIZE + 64)
|
|
|
|
// Retransmit timeout in milliseconds
|
|
#define RETRANSMIT_TIMEOUT_MS 500
|
|
|
|
// Receive state machine
|
|
#define RX_STATE_HUNT 0 // scanning for FLAG_BYTE
|
|
#define RX_STATE_ACTIVE 1 // receiving frame data
|
|
#define RX_STATE_ESCAPE 2 // next byte is escaped
|
|
|
|
|
|
// ========================================================================
|
|
// Types
|
|
// ========================================================================
|
|
|
|
// Transmit window slot
|
|
typedef struct {
|
|
uint8_t data[PKT_MAX_PAYLOAD];
|
|
int len;
|
|
uint8_t seq;
|
|
clock_t timer;
|
|
} TxSlotT;
|
|
|
|
// Connection state
|
|
struct PktConnS {
|
|
int com;
|
|
int windowSize;
|
|
PktRecvCallbackT callback;
|
|
void *callbackCtx;
|
|
|
|
// Transmit state
|
|
uint8_t txNextSeq; // next sequence number to assign
|
|
uint8_t txAckSeq; // oldest unacknowledged sequence
|
|
TxSlotT txSlots[PKT_MAX_WINDOW];
|
|
int txCount; // number of slots in use
|
|
|
|
// Receive state
|
|
uint8_t rxExpectSeq; // next expected sequence number
|
|
uint8_t rxState; // RX_STATE_*
|
|
uint8_t rxFrame[MAX_FRAME_SIZE];
|
|
int rxFrameLen;
|
|
|
|
};
|
|
|
|
|
|
// ========================================================================
|
|
// CRC-16-CCITT lookup table
|
|
// ========================================================================
|
|
|
|
static const uint16_t sCrcTable[256] = {
|
|
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
|
|
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
|
|
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
|
|
0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
|
|
0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x54A5,
|
|
0xA54A, 0xB56B, 0x8508, 0x9529, 0xE5CE, 0xF5EF, 0xC58C, 0xD5AD,
|
|
0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
|
|
0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
|
|
0x4864, 0x5845, 0x6826, 0x7807, 0x08E0, 0x18C1, 0x28A2, 0x38A3,
|
|
0xC94C, 0xD96D, 0xE90E, 0xF92F, 0x89C8, 0x99E9, 0xA98A, 0xB9AB,
|
|
0x5A55, 0x4A74, 0x7A17, 0x6A36, 0x1AD1, 0x0AF0, 0x3A93, 0x2AB2,
|
|
0xDB5D, 0xCB7C, 0xFB1F, 0xEB3E, 0x9BD9, 0x8BF8, 0xBB9B, 0xAB9A,
|
|
0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
|
|
0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
|
|
0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
|
|
0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
|
|
0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
|
|
0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
|
|
0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
|
|
0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
|
|
0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
|
|
0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
|
|
0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
|
|
0x26D3, 0x36F2, 0x06B1, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
|
|
0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
|
|
0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
|
|
0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
|
|
0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
|
|
0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
|
|
0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
|
|
0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
|
|
0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
|
|
};
|
|
|
|
|
|
// ========================================================================
|
|
// Static prototypes (alphabetical)
|
|
// ========================================================================
|
|
|
|
static uint16_t crcCalc(const uint8_t *data, int len);
|
|
static void processFrame(PktConnT *conn, const uint8_t *frame, int len);
|
|
static void retransmitCheck(PktConnT *conn);
|
|
static void rxProcessByte(PktConnT *conn, uint8_t byte);
|
|
static void sendAck(PktConnT *conn, uint8_t seq);
|
|
static void sendFrame(PktConnT *conn, uint8_t seq, uint8_t type, const uint8_t *payload, int len);
|
|
static void sendNak(PktConnT *conn, uint8_t seq);
|
|
static void sendRst(PktConnT *conn);
|
|
static int seqInWindow(uint8_t seq, uint8_t base, int size);
|
|
static int txSlotIndex(PktConnT *conn, uint8_t seq);
|
|
|
|
|
|
// ========================================================================
|
|
// CRC computation
|
|
// ========================================================================
|
|
|
|
static uint16_t crcCalc(const uint8_t *data, int len) {
|
|
uint16_t crc = 0xFFFF;
|
|
|
|
for (int i = 0; i < len; i++) {
|
|
crc = (crc << 8) ^ sCrcTable[(crc >> 8) ^ data[i]];
|
|
}
|
|
|
|
return crc;
|
|
}
|
|
|
|
|
|
// ========================================================================
|
|
// Frame transmission
|
|
// ========================================================================
|
|
|
|
static void sendFrame(PktConnT *conn, uint8_t seq, uint8_t type, const uint8_t *payload, int len) {
|
|
uint8_t raw[MAX_FRAME_SIZE];
|
|
uint8_t stuffed[MAX_STUFFED_SIZE];
|
|
int rawLen;
|
|
uint16_t crc;
|
|
int out;
|
|
|
|
// Build raw frame: SEQ + TYPE + LEN + PAYLOAD
|
|
raw[0] = seq;
|
|
raw[1] = type;
|
|
raw[2] = (uint8_t)len;
|
|
if (payload && len > 0) {
|
|
memcpy(&raw[3], payload, len);
|
|
}
|
|
rawLen = HEADER_SIZE + len;
|
|
|
|
// Append CRC
|
|
crc = crcCalc(raw, rawLen);
|
|
raw[rawLen] = (uint8_t)(crc & 0xFF);
|
|
raw[rawLen + 1] = (uint8_t)((crc >> 8) & 0xFF);
|
|
rawLen += CRC_SIZE;
|
|
|
|
// Byte-stuff into output buffer with leading flag
|
|
out = 0;
|
|
stuffed[out++] = FLAG_BYTE;
|
|
for (int i = 0; i < rawLen; i++) {
|
|
if (raw[i] == FLAG_BYTE) {
|
|
stuffed[out++] = ESC_BYTE;
|
|
stuffed[out++] = FLAG_BYTE ^ ESC_XOR;
|
|
} else if (raw[i] == ESC_BYTE) {
|
|
stuffed[out++] = ESC_BYTE;
|
|
stuffed[out++] = ESC_BYTE ^ ESC_XOR;
|
|
} else {
|
|
stuffed[out++] = raw[i];
|
|
}
|
|
}
|
|
|
|
// Trailing flag to close the frame immediately
|
|
stuffed[out++] = FLAG_BYTE;
|
|
|
|
// Send via serial port (blocking write)
|
|
rs232Write(conn->com, (const char *)stuffed, out);
|
|
}
|
|
|
|
|
|
static void sendAck(PktConnT *conn, uint8_t seq) {
|
|
sendFrame(conn, seq, FRAME_ACK, 0, 0);
|
|
}
|
|
|
|
|
|
static void sendNak(PktConnT *conn, uint8_t seq) {
|
|
sendFrame(conn, seq, FRAME_NAK, 0, 0);
|
|
}
|
|
|
|
|
|
static void sendRst(PktConnT *conn) {
|
|
sendFrame(conn, 0, FRAME_RST, 0, 0);
|
|
}
|
|
|
|
|
|
// ========================================================================
|
|
// Sequence number helpers
|
|
// ========================================================================
|
|
|
|
static int seqInWindow(uint8_t seq, uint8_t base, int size) {
|
|
uint8_t diff = seq - base;
|
|
return diff < (uint8_t)size;
|
|
}
|
|
|
|
|
|
static int txSlotIndex(PktConnT *conn, uint8_t seq) {
|
|
uint8_t diff = seq - conn->txAckSeq;
|
|
if (diff >= (uint8_t)conn->txCount) {
|
|
return -1;
|
|
}
|
|
return diff;
|
|
}
|
|
|
|
|
|
// ========================================================================
|
|
// Frame processing
|
|
// ========================================================================
|
|
|
|
static void processFrame(PktConnT *conn, const uint8_t *frame, int len) {
|
|
uint8_t seq;
|
|
uint8_t type;
|
|
int payloadLen;
|
|
uint16_t rxCrc;
|
|
uint16_t calcCrc;
|
|
|
|
if (len < MIN_FRAME_SIZE) {
|
|
return;
|
|
}
|
|
|
|
// Verify CRC
|
|
calcCrc = crcCalc(frame, len - CRC_SIZE);
|
|
rxCrc = frame[len - 2] | ((uint16_t)frame[len - 1] << 8);
|
|
if (calcCrc != rxCrc) {
|
|
// CRC mismatch — request retransmit of what we expect
|
|
sendNak(conn, conn->rxExpectSeq);
|
|
return;
|
|
}
|
|
|
|
seq = frame[0];
|
|
type = frame[1];
|
|
payloadLen = frame[2];
|
|
|
|
// Validate payload length against actual frame size
|
|
if (payloadLen + MIN_FRAME_SIZE != len) {
|
|
return;
|
|
}
|
|
|
|
switch (type) {
|
|
case FRAME_DATA:
|
|
if (seq == conn->rxExpectSeq) {
|
|
// In-order delivery
|
|
if (conn->callback) {
|
|
conn->callback(conn->callbackCtx, &frame[HEADER_SIZE], payloadLen);
|
|
}
|
|
conn->rxExpectSeq++;
|
|
sendAck(conn, conn->rxExpectSeq);
|
|
} else if (seqInWindow(seq, conn->rxExpectSeq, conn->windowSize)) {
|
|
// Out of order but in window — NAK the one we want
|
|
sendNak(conn, conn->rxExpectSeq);
|
|
}
|
|
// else: duplicate or out of window, silently discard
|
|
break;
|
|
|
|
case FRAME_ACK: {
|
|
// ACK carries the next expected sequence number (cumulative)
|
|
// Advance txAckSeq and free all slots up to seq
|
|
while (conn->txCount > 0 && conn->txAckSeq != seq) {
|
|
conn->txAckSeq++;
|
|
conn->txCount--;
|
|
}
|
|
break;
|
|
}
|
|
|
|
case FRAME_NAK: {
|
|
// Retransmit from the requested sequence
|
|
int idx = txSlotIndex(conn, seq);
|
|
if (idx >= 0) {
|
|
// Retransmit this slot and all after it (go-back-N)
|
|
for (int i = idx; i < conn->txCount; i++) {
|
|
TxSlotT *slot = &conn->txSlots[i];
|
|
sendFrame(conn, slot->seq, FRAME_DATA, slot->data, slot->len);
|
|
slot->timer = clock();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case FRAME_RST:
|
|
// Remote requested reset — clear state and respond with RST
|
|
conn->txNextSeq = 0;
|
|
conn->txAckSeq = 0;
|
|
conn->txCount = 0;
|
|
conn->rxExpectSeq = 0;
|
|
sendRst(conn);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
// ========================================================================
|
|
// Receive byte processing (state machine)
|
|
// ========================================================================
|
|
|
|
static void rxProcessByte(PktConnT *conn, uint8_t byte) {
|
|
switch (conn->rxState) {
|
|
case RX_STATE_HUNT:
|
|
if (byte == FLAG_BYTE) {
|
|
conn->rxState = RX_STATE_ACTIVE;
|
|
conn->rxFrameLen = 0;
|
|
}
|
|
break;
|
|
|
|
case RX_STATE_ACTIVE:
|
|
if (byte == FLAG_BYTE) {
|
|
// End of frame (or start of next)
|
|
if (conn->rxFrameLen >= MIN_FRAME_SIZE) {
|
|
processFrame(conn, conn->rxFrame, conn->rxFrameLen);
|
|
}
|
|
// Reset for next frame
|
|
conn->rxFrameLen = 0;
|
|
} else if (byte == ESC_BYTE) {
|
|
conn->rxState = RX_STATE_ESCAPE;
|
|
} else {
|
|
if (conn->rxFrameLen < MAX_FRAME_SIZE) {
|
|
conn->rxFrame[conn->rxFrameLen++] = byte;
|
|
} else {
|
|
// Frame too large, discard and hunt for next flag
|
|
conn->rxState = RX_STATE_HUNT;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case RX_STATE_ESCAPE:
|
|
conn->rxState = RX_STATE_ACTIVE;
|
|
byte ^= ESC_XOR;
|
|
if (conn->rxFrameLen < MAX_FRAME_SIZE) {
|
|
conn->rxFrame[conn->rxFrameLen++] = byte;
|
|
} else {
|
|
conn->rxState = RX_STATE_HUNT;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
// ========================================================================
|
|
// Retransmit check
|
|
// ========================================================================
|
|
|
|
static void retransmitCheck(PktConnT *conn) {
|
|
clock_t now = clock();
|
|
clock_t timeout = (clock_t)RETRANSMIT_TIMEOUT_MS * CLOCKS_PER_SEC / 1000;
|
|
|
|
for (int i = 0; i < conn->txCount; i++) {
|
|
TxSlotT *slot = &conn->txSlots[i];
|
|
if (now - slot->timer >= timeout) {
|
|
sendFrame(conn, slot->seq, FRAME_DATA, slot->data, slot->len);
|
|
slot->timer = now;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// ========================================================================
|
|
// Public functions (alphabetical)
|
|
// ========================================================================
|
|
|
|
void pktClose(PktConnT *conn) {
|
|
if (conn) {
|
|
free(conn);
|
|
}
|
|
}
|
|
|
|
|
|
bool pktCanSend(PktConnT *conn) {
|
|
if (!conn) {
|
|
return false;
|
|
}
|
|
return conn->txCount < conn->windowSize;
|
|
}
|
|
|
|
|
|
int pktGetPending(PktConnT *conn) {
|
|
if (!conn) {
|
|
return PKT_ERR_INVALID_PARAM;
|
|
}
|
|
return conn->txCount;
|
|
}
|
|
|
|
|
|
PktConnT *pktOpen(int com, int windowSize, PktRecvCallbackT callback, void *callbackCtx) {
|
|
PktConnT *conn;
|
|
|
|
if (windowSize <= 0) {
|
|
windowSize = PKT_DEFAULT_WINDOW;
|
|
}
|
|
if (windowSize > PKT_MAX_WINDOW) {
|
|
windowSize = PKT_MAX_WINDOW;
|
|
}
|
|
|
|
conn = (PktConnT *)calloc(1, sizeof(PktConnT));
|
|
if (!conn) {
|
|
return 0;
|
|
}
|
|
|
|
conn->com = com;
|
|
conn->windowSize = windowSize;
|
|
conn->callback = callback;
|
|
conn->callbackCtx = callbackCtx;
|
|
conn->txNextSeq = 0;
|
|
conn->txAckSeq = 0;
|
|
conn->txCount = 0;
|
|
conn->rxExpectSeq = 0;
|
|
conn->rxState = RX_STATE_HUNT;
|
|
conn->rxFrameLen = 0;
|
|
|
|
return conn;
|
|
}
|
|
|
|
|
|
int pktPoll(PktConnT *conn) {
|
|
char buf[128];
|
|
int nRead;
|
|
int delivered = 0;
|
|
|
|
if (!conn) {
|
|
return PKT_ERR_INVALID_PARAM;
|
|
}
|
|
|
|
// Read available serial data and feed to state machine
|
|
while ((nRead = rs232Read(conn->com, buf, sizeof(buf))) > 0) {
|
|
for (int i = 0; i < nRead; i++) {
|
|
uint8_t prevExpect = conn->rxExpectSeq;
|
|
rxProcessByte(conn, (uint8_t)buf[i]);
|
|
if (conn->rxExpectSeq != prevExpect) {
|
|
delivered++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Detect disconnected socket/port
|
|
if (nRead < 0) {
|
|
return PKT_ERR_DISCONNECTED;
|
|
}
|
|
|
|
// Check for retransmit timeouts
|
|
retransmitCheck(conn);
|
|
|
|
return delivered;
|
|
}
|
|
|
|
|
|
int pktReset(PktConnT *conn) {
|
|
if (!conn) {
|
|
return PKT_ERR_INVALID_PARAM;
|
|
}
|
|
|
|
conn->txNextSeq = 0;
|
|
conn->txAckSeq = 0;
|
|
conn->txCount = 0;
|
|
conn->rxExpectSeq = 0;
|
|
conn->rxState = RX_STATE_HUNT;
|
|
conn->rxFrameLen = 0;
|
|
|
|
sendRst(conn);
|
|
|
|
return PKT_SUCCESS;
|
|
}
|
|
|
|
|
|
int pktSend(PktConnT *conn, const uint8_t *data, int len, bool block) {
|
|
TxSlotT *slot;
|
|
|
|
if (!conn) {
|
|
return PKT_ERR_INVALID_PARAM;
|
|
}
|
|
if (!data || len <= 0 || len > PKT_MAX_PAYLOAD) {
|
|
return PKT_ERR_INVALID_PARAM;
|
|
}
|
|
|
|
// Wait for window space if blocking
|
|
if (block) {
|
|
while (conn->txCount >= conn->windowSize) {
|
|
if (pktPoll(conn) == PKT_ERR_DISCONNECTED) {
|
|
return PKT_ERR_DISCONNECTED;
|
|
}
|
|
}
|
|
} else {
|
|
if (conn->txCount >= conn->windowSize) {
|
|
return PKT_ERR_TX_FULL;
|
|
}
|
|
}
|
|
|
|
// Store in retransmit buffer
|
|
slot = &conn->txSlots[conn->txCount];
|
|
memcpy(slot->data, data, len);
|
|
slot->len = len;
|
|
slot->seq = conn->txNextSeq;
|
|
slot->timer = clock();
|
|
conn->txCount++;
|
|
conn->txNextSeq++;
|
|
|
|
// Transmit
|
|
sendFrame(conn, slot->seq, FRAME_DATA, slot->data, slot->len);
|
|
|
|
return PKT_SUCCESS;
|
|
}
|