528 lines
15 KiB
C
528 lines
15 KiB
C
// The MIT License (MIT)
|
|
//
|
|
// Copyright (C) 2026 Scott Duensing
|
|
//
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
// of this software and associated documentation files (the "Software"), to
|
|
// deal in the Software without restriction, including without limitation the
|
|
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
|
|
// sell copies of the Software, and to permit persons to whom the Software is
|
|
// furnished to do so, subject to the following conditions:
|
|
//
|
|
// The above copyright notice and this permission notice shall be included in
|
|
// all copies or substantial portions of the Software.
|
|
//
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
|
// IN THE SOFTWARE.
|
|
|
|
// In-process loopback rs232 driver with fault injection. See loopShim.h.
|
|
|
|
#include <string.h>
|
|
|
|
#include "loopShim.h"
|
|
|
|
// Per-port receive queue. Large enough for a whole burst of max-size
|
|
// frames from a blocking bulk send plus retransmissions.
|
|
#define RX_QUEUE_SIZE 65536
|
|
|
|
// Deferred bytes (reorder holds and delays) per direction.
|
|
#define HOLD_SIZE 256
|
|
|
|
// A reorder hold is released after this many following bytes by default.
|
|
#define DEFAULT_REORDER_WINDOW 4
|
|
|
|
// Delayed bytes are released after this many clock ticks by default (1ms
|
|
// at glibc's CLOCKS_PER_SEC of 1000000).
|
|
#define DEFAULT_DELAY_TICKS 1000
|
|
|
|
// Seed used when the caller passes zero -- xorshift cannot start from 0.
|
|
#define FALLBACK_SEED 0x9E3779B9u
|
|
|
|
typedef struct {
|
|
uint8_t byte;
|
|
uint32_t releaseAfter; // bytes still to pass (reorder) or clock tick (delay)
|
|
bool isDelay;
|
|
bool used;
|
|
} HoldT;
|
|
|
|
typedef struct {
|
|
uint32_t rate[LOOP_FAULT_COUNT];
|
|
uint32_t param[LOOP_FAULT_COUNT];
|
|
uint32_t rng;
|
|
uint32_t windowFirst;
|
|
uint32_t windowCount;
|
|
uint32_t seen; // bytes that have passed through this stage
|
|
LoopScopeE scope;
|
|
HoldT hold[HOLD_SIZE];
|
|
} FaultStageT;
|
|
|
|
typedef struct {
|
|
bool open;
|
|
bool dcd; // carrier detect as the test set it
|
|
bool breakOn; // line held in the break state
|
|
bool dtr; // DTR as the library set it (kept by rs232Close, like the real driver)
|
|
int32_t writeCap; // max bytes accepted per rs232Write (LOOP_CAP_NONE = all)
|
|
uint8_t rx[RX_QUEUE_SIZE];
|
|
uint32_t rxHead;
|
|
uint32_t rxTail;
|
|
uint32_t rxCount;
|
|
FaultStageT out; // faults applied to bytes leaving this port
|
|
LoopStatsT stats;
|
|
} PortT;
|
|
|
|
static PortT sPorts[RS232_NUM_PORTS];
|
|
static clock_t sClock;
|
|
static LoopIdleHookT sIdleHook;
|
|
static void *sIdleHookCtx;
|
|
static int32_t sHookDepth;
|
|
static int32_t sIdleCap = LOOP_CAP_NONE;
|
|
static int32_t sIdleCount;
|
|
|
|
static void deliverByte(PortT *dst, uint8_t byte);
|
|
static bool faultFires(FaultStageT *stage, LoopFaultE kind, bool roll);
|
|
static void holdByte(FaultStageT *stage, uint8_t byte, bool isDelay, uint32_t releaseAfter);
|
|
static bool inWindow(const FaultStageT *stage);
|
|
static bool portValid(int com);
|
|
static void releaseHolds(int com, bool afterByte);
|
|
static uint32_t rngNext(uint32_t *state);
|
|
static void stageInit(FaultStageT *stage);
|
|
static void transmitByte(int com, uint8_t byte, uint32_t forced);
|
|
|
|
|
|
static void deliverByte(PortT *dst, uint8_t byte) {
|
|
if (dst->rxCount >= RX_QUEUE_SIZE) {
|
|
return;
|
|
}
|
|
dst->rx[dst->rxTail] = byte;
|
|
dst->rxTail = (dst->rxTail + 1) % RX_QUEUE_SIZE;
|
|
dst->rxCount++;
|
|
}
|
|
|
|
|
|
// `roll` false answers the rate question without consuming the RNG or
|
|
// checking the byte window (used for the per-write scope decision).
|
|
static bool faultFires(FaultStageT *stage, LoopFaultE kind, bool roll) {
|
|
uint32_t rate = stage->rate[kind];
|
|
if (rate == 0) {
|
|
return false;
|
|
}
|
|
if (roll && !inWindow(stage)) {
|
|
return false;
|
|
}
|
|
if (rate >= LOOP_RATE_ONE) {
|
|
return true;
|
|
}
|
|
return (rngNext(&stage->rng) % LOOP_RATE_ONE) < rate;
|
|
}
|
|
|
|
|
|
static void holdByte(FaultStageT *stage, uint8_t byte, bool isDelay, uint32_t releaseAfter) {
|
|
for (int i = 0; i < HOLD_SIZE; i++) {
|
|
if (!stage->hold[i].used) {
|
|
stage->hold[i].byte = byte;
|
|
stage->hold[i].isDelay = isDelay;
|
|
stage->hold[i].releaseAfter = releaseAfter;
|
|
stage->hold[i].used = true;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
static bool inWindow(const FaultStageT *stage) {
|
|
return stage->seen >= stage->windowFirst && stage->seen < stage->windowFirst + stage->windowCount;
|
|
}
|
|
|
|
|
|
clock_t loopClock(void) {
|
|
return sClock;
|
|
}
|
|
|
|
|
|
void loopClockAdvance(clock_t ticks) {
|
|
sClock += ticks;
|
|
for (int com = 0; com < RS232_NUM_PORTS; com++) {
|
|
releaseHolds(com, false);
|
|
}
|
|
}
|
|
|
|
|
|
bool loopGetBreak(int com) {
|
|
return portValid(com) && sPorts[com].breakOn;
|
|
}
|
|
|
|
|
|
bool loopGetDtr(int com) {
|
|
return portValid(com) && sPorts[com].dtr;
|
|
}
|
|
|
|
|
|
void loopGetStats(int com, LoopStatsT *stats) {
|
|
if (!portValid(com) || !stats) {
|
|
return;
|
|
}
|
|
*stats = sPorts[com].stats;
|
|
}
|
|
|
|
|
|
bool loopIsOpen(int com) {
|
|
return portValid(com) && sPorts[com].open;
|
|
}
|
|
|
|
|
|
int loopPeer(int com) {
|
|
return com ^ 1;
|
|
}
|
|
|
|
|
|
void loopReset(void) {
|
|
memset(sPorts, 0, sizeof(sPorts));
|
|
for (int com = 0; com < RS232_NUM_PORTS; com++) {
|
|
stageInit(&sPorts[com].out);
|
|
}
|
|
sClock = 0;
|
|
for (int com = 0; com < RS232_NUM_PORTS; com++) {
|
|
sPorts[com].writeCap = LOOP_CAP_NONE;
|
|
}
|
|
sIdleHook = 0;
|
|
sIdleHookCtx = 0;
|
|
sHookDepth = 0;
|
|
sIdleCap = LOOP_CAP_NONE;
|
|
sIdleCount = 0;
|
|
}
|
|
|
|
|
|
void loopSetDcd(int com, bool dcd) {
|
|
if (portValid(com)) {
|
|
sPorts[com].dcd = dcd;
|
|
}
|
|
}
|
|
|
|
|
|
void loopSetFault(int com, LoopFaultE kind, uint32_t rate, uint32_t seed) {
|
|
if (!portValid(com) || kind >= LOOP_FAULT_COUNT) {
|
|
return;
|
|
}
|
|
sPorts[com].out.rate[kind] = rate;
|
|
sPorts[com].out.rng = seed ? seed : FALLBACK_SEED;
|
|
}
|
|
|
|
|
|
void loopSetFaultParam(int com, LoopFaultE kind, uint32_t param) {
|
|
if (!portValid(com) || kind >= LOOP_FAULT_COUNT) {
|
|
return;
|
|
}
|
|
sPorts[com].out.param[kind] = param;
|
|
}
|
|
|
|
|
|
// Faults only fire for the byte indices [firstByte, firstByte + count) counted
|
|
// from the port's open (or last loopReset). Lets a test corrupt one exact
|
|
// byte of one exact frame.
|
|
void loopSetFaultWindow(int com, uint32_t firstByte, uint32_t count) {
|
|
if (!portValid(com)) {
|
|
return;
|
|
}
|
|
sPorts[com].out.windowFirst = firstByte;
|
|
sPorts[com].out.windowCount = count;
|
|
}
|
|
|
|
|
|
// After `cap` consecutive empty reads (across all ports) every rs232Read
|
|
// returns RS232_ERR_UNKNOWN, which the packet layer reports as a disconnect.
|
|
// That turns a protocol hang inside a blocking send into a test failure.
|
|
void loopSetIdleCap(int32_t cap) {
|
|
sIdleCap = cap;
|
|
sIdleCount = 0;
|
|
}
|
|
|
|
|
|
// Called from rs232Read whenever the queue is empty. The test uses it to
|
|
// pump the peer and advance the clock while the library spins inside a
|
|
// blocking pktSend or secLinkHandshake. Invocations nest at most
|
|
// LOOP_HOOK_MAX_DEPTH deep so the hook may itself call rs232Read.
|
|
void loopSetIdleHook(LoopIdleHookT hook, void *ctx) {
|
|
sIdleHook = hook;
|
|
sIdleHookCtx = ctx;
|
|
}
|
|
|
|
|
|
void loopSetScope(int com, LoopScopeE scope) {
|
|
if (!portValid(com)) {
|
|
return;
|
|
}
|
|
sPorts[com].out.scope = scope;
|
|
}
|
|
|
|
|
|
// Accept at most `cap` bytes per rs232Write call (a short write, as the
|
|
// real driver returns while flow control holds the line).
|
|
void loopSetWriteCap(int com, int32_t cap) {
|
|
if (!portValid(com)) {
|
|
return;
|
|
}
|
|
sPorts[com].writeCap = cap;
|
|
}
|
|
|
|
|
|
static bool portValid(int com) {
|
|
return com >= 0 && com < RS232_NUM_PORTS;
|
|
}
|
|
|
|
|
|
static void releaseHolds(int com, bool afterByte) {
|
|
FaultStageT *stage = &sPorts[com].out;
|
|
PortT *dst = &sPorts[loopPeer(com)];
|
|
for (int i = 0; i < HOLD_SIZE; i++) {
|
|
HoldT *h = &stage->hold[i];
|
|
if (!h->used) {
|
|
continue;
|
|
}
|
|
if (h->isDelay) {
|
|
if ((clock_t)h->releaseAfter <= sClock) {
|
|
h->used = false;
|
|
deliverByte(dst, h->byte);
|
|
sPorts[com].stats.delivered++;
|
|
}
|
|
} else if (afterByte) {
|
|
if (h->releaseAfter == 0) {
|
|
h->used = false;
|
|
deliverByte(dst, h->byte);
|
|
sPorts[com].stats.delivered++;
|
|
} else {
|
|
h->releaseAfter--;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
static uint32_t rngNext(uint32_t *state) {
|
|
uint32_t x = *state;
|
|
x ^= x << 13;
|
|
x ^= x >> 17;
|
|
x ^= x << 5;
|
|
*state = x;
|
|
return x;
|
|
}
|
|
|
|
|
|
int rs232Close(int com) {
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
if (!sPorts[com].open) {
|
|
return RS232_ERR_NOT_OPEN;
|
|
}
|
|
sPorts[com].open = false;
|
|
return RS232_SUCCESS;
|
|
}
|
|
|
|
|
|
int32_t rs232GetDcd(int32_t com) {
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
return sPorts[com].dcd ? 1 : 0;
|
|
}
|
|
|
|
|
|
// Writes land in the peer at once, so nothing ever waits to go out.
|
|
int32_t rs232GetTxBuffered(int32_t com) {
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
int rs232Open(int com, int32_t bps, int dataBits, char parity, int stopBits, int handshake) {
|
|
(void)bps;
|
|
(void)dataBits;
|
|
(void)parity;
|
|
(void)stopBits;
|
|
(void)handshake;
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
if (sPorts[com].open) {
|
|
return RS232_ERR_ALREADY_OPEN;
|
|
}
|
|
sPorts[com].open = true;
|
|
sPorts[com].breakOn = false;
|
|
sPorts[com].dtr = true;
|
|
sPorts[com].rxHead = 0;
|
|
sPorts[com].rxTail = 0;
|
|
sPorts[com].rxCount = 0;
|
|
return RS232_SUCCESS;
|
|
}
|
|
|
|
|
|
int rs232Read(int com, char *data, int len) {
|
|
PortT *p;
|
|
int n = 0;
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
if (!data) {
|
|
return RS232_ERR_NULL_PTR;
|
|
}
|
|
p = &sPorts[com];
|
|
if (!p->open) {
|
|
return RS232_ERR_NOT_OPEN;
|
|
}
|
|
if (p->rxCount == 0) {
|
|
if (sIdleCap != LOOP_CAP_NONE && ++sIdleCount > sIdleCap) {
|
|
return RS232_ERR_UNKNOWN;
|
|
}
|
|
if (sIdleHook && sHookDepth < LOOP_HOOK_MAX_DEPTH) {
|
|
sHookDepth++;
|
|
sIdleHook(sIdleHookCtx);
|
|
sHookDepth--;
|
|
}
|
|
}
|
|
while (n < len && p->rxCount > 0) {
|
|
data[n++] = (char)p->rx[p->rxHead];
|
|
p->rxHead = (p->rxHead + 1) % RX_QUEUE_SIZE;
|
|
p->rxCount--;
|
|
}
|
|
if (n > 0) {
|
|
sIdleCount = 0;
|
|
}
|
|
return n;
|
|
}
|
|
|
|
|
|
int32_t rs232SetBreak(int32_t com, bool on) {
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
if (!sPorts[com].open) {
|
|
return RS232_ERR_NOT_OPEN;
|
|
}
|
|
if (on && !sPorts[com].breakOn) {
|
|
sPorts[com].stats.breaks++;
|
|
}
|
|
sPorts[com].breakOn = on;
|
|
return RS232_SUCCESS;
|
|
}
|
|
|
|
|
|
int32_t rs232SetDtr(int32_t com, bool dtr) {
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
if (!sPorts[com].open) {
|
|
return RS232_ERR_NOT_OPEN;
|
|
}
|
|
sPorts[com].dtr = dtr;
|
|
return RS232_SUCCESS;
|
|
}
|
|
|
|
|
|
int rs232Write(int com, const char *data, int len) {
|
|
if (!portValid(com)) {
|
|
return RS232_ERR_INVALID_PORT;
|
|
}
|
|
if (!data) {
|
|
return RS232_ERR_NULL_PTR;
|
|
}
|
|
if (!sPorts[com].open) {
|
|
return RS232_ERR_NOT_OPEN;
|
|
}
|
|
if (sPorts[com].writeCap != LOOP_CAP_NONE && len > sPorts[com].writeCap) {
|
|
len = sPorts[com].writeCap;
|
|
}
|
|
FaultStageT *stage = &sPorts[com].out;
|
|
int32_t target[LOOP_FAULT_COUNT];
|
|
for (int k = 0; k < LOOP_FAULT_COUNT; k++) {
|
|
target[k] = -1;
|
|
if (stage->scope == LOOP_SCOPE_WRITE && len > 0 && faultFires(stage, (LoopFaultE)k, false)) {
|
|
target[k] = (int32_t)(rngNext(&stage->rng) % (uint32_t)len);
|
|
}
|
|
}
|
|
for (int i = 0; i < len; i++) {
|
|
uint32_t forced = 0;
|
|
for (int k = 0; k < LOOP_FAULT_COUNT; k++) {
|
|
if (target[k] == i && inWindow(stage)) {
|
|
forced |= 1u << k;
|
|
}
|
|
}
|
|
transmitByte(com, (uint8_t)data[i], forced);
|
|
}
|
|
return len;
|
|
}
|
|
|
|
|
|
int32_t rs232WriteBuf(int32_t com, const char *data, int32_t len) {
|
|
return rs232Write(com, data, len);
|
|
}
|
|
|
|
|
|
static void stageInit(FaultStageT *stage) {
|
|
memset(stage, 0, sizeof(*stage));
|
|
stage->rng = FALLBACK_SEED;
|
|
stage->windowCount = UINT32_MAX;
|
|
stage->param[LOOP_FAULT_REORDER] = DEFAULT_REORDER_WINDOW;
|
|
stage->param[LOOP_FAULT_DELAY] = DEFAULT_DELAY_TICKS;
|
|
}
|
|
|
|
|
|
// Run one byte through the outgoing fault stage of `com` and into the
|
|
// peer's queue. `forced` is a bitmask of kinds decided per write; in byte
|
|
// scope each kind is rolled here. Bytes are delivered even when the peer
|
|
// is not open (a closed peer simply never reads them), as on a real wire.
|
|
static void transmitByte(int com, uint8_t byte, uint32_t forced) {
|
|
PortT *src = &sPorts[com];
|
|
PortT *dst = &sPorts[loopPeer(com)];
|
|
FaultStageT *stage = &src->out;
|
|
bool held = false;
|
|
bool fire[LOOP_FAULT_COUNT];
|
|
src->stats.written++;
|
|
for (int k = 0; k < LOOP_FAULT_COUNT; k++) {
|
|
if (stage->scope == LOOP_SCOPE_WRITE) {
|
|
fire[k] = (forced & (1u << k)) != 0;
|
|
} else {
|
|
fire[k] = faultFires(stage, (LoopFaultE)k, true);
|
|
}
|
|
}
|
|
if (fire[LOOP_FAULT_DROP]) {
|
|
src->stats.faults[LOOP_FAULT_DROP]++;
|
|
stage->seen++;
|
|
releaseHolds(com, true);
|
|
return;
|
|
}
|
|
if (fire[LOOP_FAULT_CORRUPT]) {
|
|
uint8_t mask = (uint8_t)(rngNext(&stage->rng) & 0xFF);
|
|
if (mask == 0) {
|
|
mask = 1;
|
|
}
|
|
byte ^= mask;
|
|
src->stats.faults[LOOP_FAULT_CORRUPT]++;
|
|
}
|
|
if (fire[LOOP_FAULT_REORDER]) {
|
|
holdByte(stage, byte, false, stage->param[LOOP_FAULT_REORDER]);
|
|
src->stats.faults[LOOP_FAULT_REORDER]++;
|
|
held = true;
|
|
} else if (fire[LOOP_FAULT_DELAY]) {
|
|
holdByte(stage, byte, true, (uint32_t)(sClock + stage->param[LOOP_FAULT_DELAY]));
|
|
src->stats.faults[LOOP_FAULT_DELAY]++;
|
|
held = true;
|
|
}
|
|
if (!held) {
|
|
deliverByte(dst, byte);
|
|
src->stats.delivered++;
|
|
if (fire[LOOP_FAULT_DUP]) {
|
|
deliverByte(dst, byte);
|
|
src->stats.delivered++;
|
|
src->stats.faults[LOOP_FAULT_DUP]++;
|
|
}
|
|
}
|
|
stage->seen++;
|
|
releaseHolds(com, true);
|
|
}
|