Skip to content

Commit cc454f4

Browse files
pavel-kirienkopavel-kirienko
committed
fix AVL tiebreaking
1 parent d43d242 commit cc454f4

File tree

2 files changed

+255
-5
lines changed

2 files changed

+255
-5
lines changed

‎libcanard/canard.c‎

Lines changed: 71 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -601,10 +601,29 @@ static void txfer_free_payload(canard_t* const self, canard_txfer_t* const tr)
601601
}
602602
}
603603

604+
// Key for the staged index ordering.
605+
typedef struct
606+
{
607+
canard_us_t when;
608+
const canard_txfer_t* tr;
609+
} tx_staged_key_t;
610+
604611
static int32_t tx_cavl_compare_staged_until(const void* const user, const canard_tree_t* const node)
605612
{
606-
return ((*(const canard_us_t*)user) >= txfer_staged_until(CAVL2_TO_OWNER(node, canard_txfer_t, index_staged))) ? +1
607-
: -1;
613+
// Use the transfer pointer as a stable tiebreaker for equal timestamps.
614+
const canard_txfer_t* const rhs = CAVL2_TO_OWNER(node, canard_txfer_t, index_staged);
615+
const canard_us_t rhs_when = txfer_staged_until(rhs);
616+
const tx_staged_key_t* const key = (const tx_staged_key_t*)user;
617+
if (key->when < rhs_when) {
618+
return -1;
619+
}
620+
if (key->when > rhs_when) {
621+
return +1;
622+
}
623+
if (key->tr < rhs) {
624+
return -1;
625+
}
626+
return (key->tr > rhs) ? +1 : 0;
608627
}
609628

610629
/// Updates the next attempt time and inserts the transfer into the staged index, unless the next scheduled
@@ -630,19 +649,66 @@ static void tx_stage_reliable_if(canard_t* const self, canard_txfer_t* const tr)
630649
if ((tr->deadline - timeout) >= new_staged_until) {
631650
const canard_us_t delta = min_i64(tr->deadline - new_staged_until, (canard_us_t)STAGED_UNTIL_DELTA_MAX);
632651
CANARD_ASSERT(delta > 0);
633-
tr->staged_until_delta = ((uint64_t)delta) & STAGED_UNTIL_DELTA_MAX;
652+
tr->staged_until_delta = ((uint64_t)delta) & STAGED_UNTIL_DELTA_MAX;
653+
// Composite key ensures equal timestamps are ordered deterministically.
654+
const tx_staged_key_t key = { .when = new_staged_until, .tr = tr };
634655
const canard_tree_t* const tree = cavl2_find_or_insert(
635-
&self->tx.staged, &new_staged_until, tx_cavl_compare_staged_until, &tr->index_staged, cavl2_trivial_factory);
656+
&self->tx.staged, &key, tx_cavl_compare_staged_until, &tr->index_staged, cavl2_trivial_factory);
636657
CANARD_ASSERT(tree == &tr->index_staged);
637658
(void)tree;
638659
}
639660
}
640661

662+
// Compare transfer-ID with wraparound support (5-bit, +/-16).
663+
static int8_t tx_compare_transfer_id(const byte_t lhs, const byte_t rhs)
664+
{
665+
const uint8_t diff_u = (uint8_t)((lhs - rhs) & CANARD_TRANSFER_ID_MAX);
666+
int16_t diff = (int16_t)diff_u;
667+
if (diff > (int16_t)(CANARD_TRANSFER_ID_MAX / 2U)) {
668+
diff = (int16_t)(diff - (int16_t)(CANARD_TRANSFER_ID_MAX + 1U));
669+
}
670+
return (int8_t)diff;
671+
}
672+
641673
static int32_t tx_cavl_compare_pending_order(const void* const user, const canard_tree_t* const node)
642674
{
643675
const canard_txfer_t* const lhs = (const canard_txfer_t*)user;
644676
const canard_txfer_t* const rhs = CAVL2_TO_OWNER(node, canard_txfer_t, index_pending[0]);
645-
return (lhs->can_id_msb >= rhs->can_id_msb) ? +1 : -1;
677+
// Same topic hash: order by priority then FIFO by transfer-ID (wraparound), then by deadline.
678+
if (lhs->topic_hash == rhs->topic_hash) {
679+
const canard_prio_t lhs_prio = txfer_prio(lhs);
680+
const canard_prio_t rhs_prio = txfer_prio(rhs);
681+
if (lhs_prio < rhs_prio) {
682+
return -1;
683+
}
684+
if (lhs_prio > rhs_prio) {
685+
return +1;
686+
}
687+
const int8_t tid_diff = tx_compare_transfer_id((byte_t)lhs->transfer_id, (byte_t)rhs->transfer_id);
688+
if (tid_diff != 0) {
689+
return (tid_diff < 0) ? -1 : +1;
690+
}
691+
// Best-effort transfers may reuse transfer-ID; use deadline as a tiebreaker.
692+
if (lhs->deadline < rhs->deadline) {
693+
return -1;
694+
}
695+
if (lhs->deadline > rhs->deadline) {
696+
return +1;
697+
}
698+
} else {
699+
// Different topic hash: sooner deadline first.
700+
if (lhs->deadline < rhs->deadline) {
701+
return -1;
702+
}
703+
if (lhs->deadline > rhs->deadline) {
704+
return +1;
705+
}
706+
}
707+
// Final arbitrary tiebreaker to enforce strict ordering.
708+
if (lhs < rhs) {
709+
return -1;
710+
}
711+
return (lhs > rhs) ? +1 : 0;
646712
}
647713

648714
static void tx_make_pending(canard_t* const self, canard_txfer_t* const tr)

‎tests/src/test_intrusive_tx.c‎

Lines changed: 184 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1160,6 +1160,152 @@ static void test_tx_push_fifo_same_priority(void)
11601160
TEST_ASSERT_EQUAL_size_t(0, alloc_tr.allocated_fragments);
11611161
}
11621162

1163+
// Ensure pending order wraps transfer-ID for same topic and priority.
1164+
static void test_tx_pending_same_topic_tid_wrap(void)
1165+
{
1166+
canard_t self;
1167+
instrumented_allocator_t alloc_tr;
1168+
instrumented_allocator_t alloc_fr;
1169+
test_context_t ctx;
1170+
setup_canard_for_tx_push(&self, &alloc_tr, &alloc_fr, &ctx);
1171+
1172+
const canard_bytes_chain_t payload = { .bytes = { .size = 0, .data = NULL }, .next = NULL };
1173+
const uint32_t can_id = ((uint32_t)canard_prio_nominal << PRIO_SHIFT);
1174+
const uint64_t topic = 0xAABBCCDDU;
1175+
canard_txfer_t* tr1 = make_test_transfer(
1176+
self.mem.tx_transfer, transfer_kind_message, true, topic, can_id, 30, CANARD_USER_CONTEXT_NULL);
1177+
canard_txfer_t* tr2 =
1178+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, topic, can_id, 2, CANARD_USER_CONTEXT_NULL);
1179+
TEST_ASSERT_NOT_NULL(tr1);
1180+
TEST_ASSERT_NOT_NULL(tr2);
1181+
1182+
TEST_ASSERT_TRUE(tx_push(&self, tr1, false, false, 1, payload, CRC_INITIAL));
1183+
TEST_ASSERT_TRUE(tx_push(&self, tr2, false, false, 1, payload, CRC_INITIAL));
1184+
1185+
canard_txfer_t* const head = CAVL2_TO_OWNER(cavl2_min(self.tx.pending[0]), canard_txfer_t, index_pending[0]);
1186+
TEST_ASSERT_EQUAL_PTR(tr1, head);
1187+
1188+
txfer_retire(&self, tr2, true);
1189+
txfer_retire(&self, tr1, true);
1190+
}
1191+
1192+
// Same transfer-ID uses deadline as a tiebreaker.
1193+
static void test_tx_pending_same_topic_tid_deadline(void)
1194+
{
1195+
canard_t self;
1196+
instrumented_allocator_t alloc_tr;
1197+
instrumented_allocator_t alloc_fr;
1198+
test_context_t ctx;
1199+
setup_canard_for_tx_push(&self, &alloc_tr, &alloc_fr, &ctx);
1200+
1201+
const canard_bytes_chain_t payload = { .bytes = { .size = 0, .data = NULL }, .next = NULL };
1202+
const uint32_t can_id = ((uint32_t)canard_prio_nominal << PRIO_SHIFT);
1203+
const uint64_t topic = 0xDEADBEEFULL;
1204+
canard_txfer_t* tr1 =
1205+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, topic, can_id, 5, CANARD_USER_CONTEXT_NULL);
1206+
canard_txfer_t* tr2 =
1207+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, topic, can_id, 5, CANARD_USER_CONTEXT_NULL);
1208+
TEST_ASSERT_NOT_NULL(tr1);
1209+
TEST_ASSERT_NOT_NULL(tr2);
1210+
tr1->deadline = 10;
1211+
tr2->deadline = 20;
1212+
1213+
TEST_ASSERT_TRUE(tx_push(&self, tr1, false, false, 1, payload, CRC_INITIAL));
1214+
TEST_ASSERT_TRUE(tx_push(&self, tr2, false, false, 1, payload, CRC_INITIAL));
1215+
1216+
canard_txfer_t* const head = CAVL2_TO_OWNER(cavl2_min(self.tx.pending[0]), canard_txfer_t, index_pending[0]);
1217+
TEST_ASSERT_EQUAL_PTR(tr1, head);
1218+
1219+
txfer_retire(&self, tr2, true);
1220+
txfer_retire(&self, tr1, true);
1221+
}
1222+
1223+
// Different topic hash uses deadline ordering.
1224+
static void test_tx_pending_diff_topic_deadline(void)
1225+
{
1226+
canard_t self;
1227+
instrumented_allocator_t alloc_tr;
1228+
instrumented_allocator_t alloc_fr;
1229+
test_context_t ctx;
1230+
setup_canard_for_tx_push(&self, &alloc_tr, &alloc_fr, &ctx);
1231+
1232+
const canard_bytes_chain_t payload = { .bytes = { .size = 0, .data = NULL }, .next = NULL };
1233+
const uint32_t can_id = ((uint32_t)canard_prio_nominal << PRIO_SHIFT);
1234+
canard_txfer_t* tr1 =
1235+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, 1, can_id, 1, CANARD_USER_CONTEXT_NULL);
1236+
canard_txfer_t* tr2 =
1237+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, 2, can_id, 2, CANARD_USER_CONTEXT_NULL);
1238+
TEST_ASSERT_NOT_NULL(tr1);
1239+
TEST_ASSERT_NOT_NULL(tr2);
1240+
tr1->deadline = 20;
1241+
tr2->deadline = 10;
1242+
1243+
TEST_ASSERT_TRUE(tx_push(&self, tr1, false, false, 1, payload, CRC_INITIAL));
1244+
TEST_ASSERT_TRUE(tx_push(&self, tr2, false, false, 1, payload, CRC_INITIAL));
1245+
1246+
canard_txfer_t* const head = CAVL2_TO_OWNER(cavl2_min(self.tx.pending[0]), canard_txfer_t, index_pending[0]);
1247+
TEST_ASSERT_EQUAL_PTR(tr2, head);
1248+
1249+
txfer_retire(&self, tr2, true);
1250+
txfer_retire(&self, tr1, true);
1251+
}
1252+
1253+
// Cover comparator branches for staged and pending trees.
1254+
static void test_tx_comparator_branches(void)
1255+
{
1256+
canard_txfer_t tr[2];
1257+
memset(&tr, 0, sizeof(tr));
1258+
1259+
// Staged compare: when < rhs.
1260+
tr[1].deadline = 100;
1261+
tr[1].staged_until_delta = 10; // staged_until = 90
1262+
tx_staged_key_t key = { .when = 80, .tr = &tr[0] };
1263+
TEST_ASSERT_EQUAL_INT32(-1, tx_cavl_compare_staged_until(&key, &tr[1].index_staged));
1264+
1265+
// Staged compare: when > rhs.
1266+
key.when = 100;
1267+
TEST_ASSERT_EQUAL_INT32(+1, tx_cavl_compare_staged_until(&key, &tr[1].index_staged));
1268+
1269+
// Staged compare: pointer tiebreak.
1270+
key.when = 90;
1271+
TEST_ASSERT_EQUAL_INT32(-1, tx_cavl_compare_staged_until(&key, &tr[1].index_staged));
1272+
1273+
// Transfer-ID wraparound compare.
1274+
TEST_ASSERT_TRUE(tx_compare_transfer_id(30, 2) < 0);
1275+
1276+
// Pending compare: priority order.
1277+
tr[0].topic_hash = 1;
1278+
tr[1].topic_hash = 1;
1279+
tr[0].can_id_msb = (uint32_t)(1U << (CAN_ID_MSb_BITS - 3U));
1280+
tr[1].can_id_msb = (uint32_t)(2U << (CAN_ID_MSb_BITS - 3U));
1281+
TEST_ASSERT_EQUAL_INT32(-1, tx_cavl_compare_pending_order(&tr[0], &tr[1].index_pending[0]));
1282+
1283+
// Pending compare: same transfer-ID uses deadline.
1284+
tr[0].can_id_msb = tr[1].can_id_msb;
1285+
tr[0].transfer_id = 5;
1286+
tr[1].transfer_id = 5;
1287+
tr[0].deadline = 10;
1288+
tr[1].deadline = 20;
1289+
TEST_ASSERT_EQUAL_INT32(-1, tx_cavl_compare_pending_order(&tr[0], &tr[1].index_pending[0]));
1290+
1291+
// Pending compare: different topic uses deadline.
1292+
tr[0].topic_hash = 1;
1293+
tr[1].topic_hash = 2;
1294+
tr[0].deadline = 30;
1295+
tr[1].deadline = 20;
1296+
TEST_ASSERT_EQUAL_INT32(+1, tx_cavl_compare_pending_order(&tr[0], &tr[1].index_pending[0]));
1297+
1298+
// Pending compare: pointer tiebreakers.
1299+
tr[0].topic_hash = 3;
1300+
tr[1].topic_hash = 3;
1301+
tr[0].transfer_id = 7;
1302+
tr[1].transfer_id = 7;
1303+
tr[0].deadline = 40;
1304+
tr[1].deadline = 40;
1305+
TEST_ASSERT_EQUAL_INT32(-1, tx_cavl_compare_pending_order(&tr[0], &tr[1].index_pending[0]));
1306+
TEST_ASSERT_EQUAL_INT32(+1, tx_cavl_compare_pending_order(&tr[1], &tr[0].index_pending[0]));
1307+
}
1308+
11631309
static void test_tx_make_pending_orders(void)
11641310
{
11651311
canard_t self;
@@ -1334,6 +1480,39 @@ static void test_tx_stage_reliable_if_inserts(void)
13341480
TEST_ASSERT_EQUAL_size_t(0, alloc_tr.allocated_fragments);
13351481
}
13361482

1483+
// Equal staged timestamps must not collide in the staged tree.
1484+
static void test_tx_stage_reliable_if_tiebreak(void)
1485+
{
1486+
canard_t self;
1487+
instrumented_allocator_t alloc_tr;
1488+
instrumented_allocator_t alloc_fr;
1489+
test_context_t ctx;
1490+
setup_canard_for_tx_push(&self, &alloc_tr, &alloc_fr, &ctx);
1491+
self.ack_baseline_timeout = 1;
1492+
ctx.now = 100;
1493+
1494+
const canard_bytes_chain_t payload = { .bytes = { .size = 0, .data = NULL }, .next = NULL };
1495+
canard_txfer_t* tr1 =
1496+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, 1, 0, 1, CANARD_USER_CONTEXT_NULL);
1497+
canard_txfer_t* tr2 =
1498+
make_test_transfer(self.mem.tx_transfer, transfer_kind_message, true, 1, 0, 2, CANARD_USER_CONTEXT_NULL);
1499+
TEST_ASSERT_NOT_NULL(tr1);
1500+
TEST_ASSERT_NOT_NULL(tr2);
1501+
tr1->deadline = 1000000;
1502+
tr2->deadline = 1000000;
1503+
1504+
TEST_ASSERT_TRUE(tx_push(&self, tr1, true, false, 1, payload, CRC_INITIAL));
1505+
TEST_ASSERT_TRUE(tx_push(&self, tr2, true, false, 1, payload, CRC_INITIAL));
1506+
1507+
tx_stage_reliable_if(&self, tr1);
1508+
tx_stage_reliable_if(&self, tr2);
1509+
TEST_ASSERT_TRUE(cavl2_is_inserted(self.tx.staged, &tr1->index_staged));
1510+
TEST_ASSERT_TRUE(cavl2_is_inserted(self.tx.staged, &tr2->index_staged));
1511+
1512+
txfer_retire(&self, tr2, true);
1513+
txfer_retire(&self, tr1, true);
1514+
}
1515+
13371516
static void test_tx_stage_reliable_if_orders(void)
13381517
{
13391518
canard_t self;
@@ -1576,6 +1755,10 @@ int main(void)
15761755
RUN_TEST(test_tx_push_sacrifice_oldest);
15771756
RUN_TEST(test_tx_push_duplicate_reliable_transfer);
15781757
RUN_TEST(test_tx_push_fifo_same_priority);
1758+
RUN_TEST(test_tx_pending_same_topic_tid_wrap);
1759+
RUN_TEST(test_tx_pending_same_topic_tid_deadline);
1760+
RUN_TEST(test_tx_pending_diff_topic_deadline);
1761+
RUN_TEST(test_tx_comparator_branches);
15791762
RUN_TEST(test_tx_make_pending_orders);
15801763
RUN_TEST(test_tx_receive_ack_retires_feedback);
15811764
RUN_TEST(test_tx_receive_ack_scan_miss);
@@ -1584,6 +1767,7 @@ int main(void)
15841767
RUN_TEST(test_txfer_is_pending_false);
15851768
RUN_TEST(test_txfer_retire_updates_iter);
15861769
RUN_TEST(test_tx_stage_reliable_if_inserts);
1770+
RUN_TEST(test_tx_stage_reliable_if_tiebreak);
15871771
RUN_TEST(test_tx_stage_reliable_if_orders);
15881772
RUN_TEST(test_tx_stage_reliable_if_deadline_too_close);
15891773
RUN_TEST(test_tx_promote_staged_requeues);

0 commit comments

Comments
 (0)