Skip to content

Commit 5220b05

Browse files
serges147serges147
authored
introduce deadline queue #sonar (#238)
1 parent 5b8226f commit 5220b05

File tree

4 files changed

+318
-63
lines changed

4 files changed

+318
-63
lines changed

‎libcanard/canard.c

Lines changed: 133 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
/// Author: Pavel Kirienko <[email protected]>
44

55
#include "canard.h"
6+
#include <stddef.h>
67
#include <string.h>
78

89
// --------------------------------------------- BUILD CONFIGURATION ---------------------------------------------
@@ -70,6 +71,11 @@
7071

7172
#define INITIAL_TOGGLE_STATE true
7273

74+
#define CONTAINER_OF(type, ptr, member) \
75+
((const type*) (((ptr) == NULL) ? NULL : (const void*) (((const char*) (ptr)) - offsetof(type, member))))
76+
#define MUTABLE_CONTAINER_OF(type, ptr, member) \
77+
((type*) (((ptr) == NULL) ? NULL : (void*) (((char*) (ptr)) - offsetof(type, member))))
78+
7379
/// Used for inserting new items into AVL trees.
7480
CANARD_PRIVATE struct CanardTreeNode* avlTrivialFactory(void* const user_reference)
7581
{
@@ -299,10 +305,15 @@ CANARD_PRIVATE struct CanardTxQueueItem* txAllocateQueueItem(struct CanardTxQueu
299305
struct CanardTxQueueItem* out = ins->memory.allocate(ins->memory.user_reference, sizeof(struct CanardTxQueueItem));
300306
if (out != NULL)
301307
{
302-
out->base.up = NULL;
303-
out->base.lr[0] = NULL;
304-
out->base.lr[1] = NULL;
305-
out->base.bf = 0;
308+
out->priority_base.up = NULL;
309+
out->priority_base.lr[0] = NULL;
310+
out->priority_base.lr[1] = NULL;
311+
out->priority_base.bf = 0;
312+
313+
out->deadline_base.up = NULL;
314+
out->deadline_base.lr[0] = NULL;
315+
out->deadline_base.lr[1] = NULL;
316+
out->deadline_base.bf = 0;
306317

307318
out->next_in_transfer = NULL; // Last by default.
308319
out->tx_deadline_usec = deadline_usec;
@@ -329,15 +340,34 @@ CANARD_PRIVATE struct CanardTxQueueItem* txAllocateQueueItem(struct CanardTxQueu
329340
/// Frames with identical CAN ID that are added later always compare greater than their counterparts with same CAN ID.
330341
/// This ensures that CAN frames with the same CAN ID are transmitted in the FIFO order.
331342
/// Frames that should be transmitted earlier compare smaller (i.e., put on the left side of the tree).
332-
CANARD_PRIVATE int8_t txAVLPredicate(void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
333-
const struct CanardTreeNode* const node)
343+
CANARD_PRIVATE int8_t txAVLPriorityPredicate( //
344+
void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
345+
const struct CanardTreeNode* const node)
334346
{
335-
const struct CanardTxQueueItem* const target = (const struct CanardTxQueueItem*) user_reference;
336-
const struct CanardTxQueueItem* const other = (const struct CanardTxQueueItem*) (const void*) node;
347+
typedef struct CanardTxQueueItem TxItem;
348+
349+
const TxItem* const target = CONTAINER_OF(TxItem, user_reference, priority_base);
350+
const TxItem* const other = CONTAINER_OF(TxItem, node, priority_base);
337351
CANARD_ASSERT((target != NULL) && (other != NULL));
338352
return (target->frame.extended_can_id >= other->frame.extended_can_id) ? +1 : -1;
339353
}
340354

355+
/// Frames with identical deadline
356+
/// that are added later always compare greater than their counterparts with the same deadline.
357+
/// This ensures that CAN frames with the same deadline are, when timed out, dropped in the FIFO order.
358+
/// Frames that should be dropped earlier compare smaller (i.e., put on the left side of the tree).
359+
CANARD_PRIVATE int8_t txAVLDeadlinePredicate( //
360+
void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
361+
const struct CanardTreeNode* const node)
362+
{
363+
typedef struct CanardTxQueueItem TxItem;
364+
365+
const TxItem* const target = CONTAINER_OF(TxItem, user_reference, deadline_base);
366+
const TxItem* const other = CONTAINER_OF(TxItem, node, deadline_base);
367+
CANARD_ASSERT((target != NULL) && (other != NULL));
368+
return (target->tx_deadline_usec >= other->tx_deadline_usec) ? +1 : -1;
369+
}
370+
341371
/// Returns the number of frames enqueued or error (i.e., =1 or <0).
342372
CANARD_PRIVATE int32_t txPushSingleFrame(struct CanardTxQueue* const que,
343373
const struct CanardInstance* const ins,
@@ -369,11 +399,19 @@ CANARD_PRIVATE int32_t txPushSingleFrame(struct CanardTxQueue* const que,
369399
uint8_t* const frame_bytes = tqi->frame.payload.data;
370400
(void) memset(frame_bytes + payload.size, PADDING_BYTE_VALUE, padding_size); // NOLINT
371401
*(frame_bytes + (frame_payload_size - 1U)) = txMakeTailByte(true, true, true, transfer_id);
372-
// Insert the newly created TX item into the queue.
373-
const struct CanardTreeNode* const res =
374-
cavlSearch(&que->root, &tqi->base, &txAVLPredicate, &avlTrivialFactory);
375-
(void) res;
376-
CANARD_ASSERT(res == &tqi->base);
402+
403+
// Insert the newly created TX item into the priority queue.
404+
const struct CanardTreeNode* const priority_queue_res =
405+
cavlSearch(&que->priority_root, &tqi->priority_base, &txAVLPriorityPredicate, &avlTrivialFactory);
406+
(void) priority_queue_res;
407+
CANARD_ASSERT(priority_queue_res == &tqi->priority_base);
408+
409+
// Insert the newly created TX item into the deadline queue.
410+
const struct CanardTreeNode* const deadline_queue_res =
411+
cavlSearch(&que->deadline_root, &tqi->deadline_base, &txAVLDeadlinePredicate, &avlTrivialFactory);
412+
(void) deadline_queue_res;
413+
CANARD_ASSERT(deadline_queue_res == &tqi->deadline_base);
414+
377415
que->size++;
378416
CANARD_ASSERT(que->size <= que->capacity);
379417
out = 1; // One frame enqueued.
@@ -514,11 +552,18 @@ CANARD_PRIVATE int32_t txPushMultiFrame(struct CanardTxQueue* const que,
514552
struct CanardTxQueueItem* next = sq.head;
515553
do
516554
{
517-
const struct CanardTreeNode* const res =
518-
cavlSearch(&que->root, &next->base, &txAVLPredicate, &avlTrivialFactory);
519-
(void) res;
520-
CANARD_ASSERT(res == &next->base);
521-
CANARD_ASSERT(que->root != NULL);
555+
const struct CanardTreeNode* const priority_queue_res =
556+
cavlSearch(&que->priority_root, &next->priority_base, &txAVLPriorityPredicate, &avlTrivialFactory);
557+
(void) priority_queue_res;
558+
CANARD_ASSERT(priority_queue_res == &next->priority_base);
559+
CANARD_ASSERT(que->priority_root != NULL);
560+
561+
const struct CanardTreeNode* const deadline_queue_res =
562+
cavlSearch(&que->deadline_root, &next->deadline_base, &txAVLDeadlinePredicate, &avlTrivialFactory);
563+
(void) deadline_queue_res;
564+
CANARD_ASSERT(deadline_queue_res == &next->deadline_base);
565+
CANARD_ASSERT(que->deadline_root != NULL);
566+
522567
next = next->next_in_transfer;
523568
} while (next != NULL);
524569
CANARD_ASSERT(num_frames == sq.size);
@@ -547,6 +592,35 @@ CANARD_PRIVATE int32_t txPushMultiFrame(struct CanardTxQueue* const que,
547592
return out;
548593
}
549594

595+
/// Flushes expired transfers by comparing deadline timestamps of the pending transfers with the current time.
596+
CANARD_PRIVATE void txFlushExpiredTransfers(struct CanardTxQueue* const que,
597+
const struct CanardInstance* const ins,
598+
const CanardMicrosecond now_usec)
599+
{
600+
struct CanardTxQueueItem* tx_item = NULL;
601+
while (NULL != (tx_item = MUTABLE_CONTAINER_OF( //
602+
struct CanardTxQueueItem,
603+
cavlFindExtremum(que->deadline_root, false),
604+
deadline_base)))
605+
{
606+
if (now_usec <= tx_item->tx_deadline_usec)
607+
{
608+
// The queue is sorted by deadline, so we can stop here.
609+
break;
610+
}
611+
612+
// All frames of the transfer are released at once b/c they all have the same deadline.
613+
struct CanardTxQueueItem* tx_item_to_free = NULL;
614+
while (NULL != (tx_item_to_free = canardTxPop(que, tx_item)))
615+
{
616+
tx_item = tx_item_to_free->next_in_transfer;
617+
canardTxFree(que, ins, tx_item_to_free);
618+
619+
que->stats.dropped_frames++;
620+
}
621+
}
622+
}
623+
550624
// --------------------------------------------- RECEPTION ---------------------------------------------
551625

552626
#define RX_SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U)
@@ -1005,8 +1079,9 @@ CANARD_PRIVATE int8_t
10051079
rxSubscriptionPredicateOnPortID(void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
10061080
const struct CanardTreeNode* const node)
10071081
{
1082+
CANARD_ASSERT((user_reference != NULL) && (node != NULL));
10081083
const CanardPortID sought = *((const CanardPortID*) user_reference);
1009-
const CanardPortID other = ((const struct CanardRxSubscription*) (const void*) node)->port_id;
1084+
const CanardPortID other = CONTAINER_OF(struct CanardRxSubscription, node, base)->port_id;
10101085
static const int8_t NegPos[2] = {-1, +1};
10111086
// Clang-Tidy mistakenly identifies a narrowing cast to int8_t here, which is incorrect.
10121087
return (sought == other) ? 0 : NegPos[sought > other]; // NOLINT no narrowing conversion is taking place here
@@ -1016,7 +1091,9 @@ CANARD_PRIVATE int8_t
10161091
rxSubscriptionPredicateOnStruct(void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
10171092
const struct CanardTreeNode* const node)
10181093
{
1019-
return rxSubscriptionPredicateOnPortID(&((struct CanardRxSubscription*) user_reference)->port_id, node);
1094+
return rxSubscriptionPredicateOnPortID( //
1095+
&MUTABLE_CONTAINER_OF(struct CanardRxSubscription, user_reference, base)->port_id,
1096+
node);
10201097
}
10211098

10221099
// --------------------------------------------- PUBLIC API ---------------------------------------------
@@ -1056,7 +1133,8 @@ struct CanardTxQueue canardTxInit(const size_t capacity,
10561133
.capacity = capacity,
10571134
.mtu_bytes = mtu_bytes,
10581135
.size = 0,
1059-
.root = NULL,
1136+
.priority_root = NULL,
1137+
.deadline_root = NULL,
10601138
.memory = memory,
10611139
.user_reference = NULL,
10621140
};
@@ -1067,8 +1145,20 @@ int32_t canardTxPush(struct CanardTxQueue* const que,
10671145
const struct CanardInstance* const ins,
10681146
const CanardMicrosecond tx_deadline_usec,
10691147
const struct CanardTransferMetadata* const metadata,
1070-
const struct CanardPayload payload)
1148+
const struct CanardPayload payload,
1149+
const CanardMicrosecond now_usec)
10711150
{
1151+
// Before pushing payload (potentially in multiple frames), we need to try to flush any expired transfers.
1152+
// This is necessary to ensure that we don't exhaust the capacity of the queue by holding outdated frames.
1153+
// The flushing is done by comparing deadline timestamps of the pending transfers with the current time,
1154+
// which makes sense only if the current time is known (bigger than zero).
1155+
if (now_usec > 0)
1156+
{
1157+
txFlushExpiredTransfers(que, ins, now_usec);
1158+
}
1159+
1160+
(void) now_usec;
1161+
10721162
int32_t out = -CANARD_ERROR_INVALID_ARGUMENT;
10731163
if ((ins != NULL) && (que != NULL) && (metadata != NULL) && ((payload.data != NULL) || (0U == payload.size)))
10741164
{
@@ -1114,7 +1204,8 @@ struct CanardTxQueueItem* canardTxPeek(const struct CanardTxQueue* const que)
11141204
{
11151205
// Paragraph 6.7.2.1.15 of the C standard says:
11161206
// A pointer to a structure object, suitably converted, points to its initial member, and vice versa.
1117-
out = (struct CanardTxQueueItem*) (void*) cavlFindExtremum(que->root, false);
1207+
struct CanardTreeNode* const priority_node = cavlFindExtremum(que->priority_root, false);
1208+
out = MUTABLE_CONTAINER_OF(struct CanardTxQueueItem, priority_node, priority_base);
11181209
}
11191210
return out;
11201211
}
@@ -1127,7 +1218,8 @@ struct CanardTxQueueItem* canardTxPop(struct CanardTxQueue* const que, struct Ca
11271218
// A pointer to a structure object, suitably converted, points to its initial member, and vice versa.
11281219
// Note that the highest-priority frame is always a leaf node in the AVL tree, which means that it is very
11291220
// cheap to remove.
1130-
cavlRemove(&que->root, &item->base);
1221+
cavlRemove(&que->priority_root, &item->priority_base);
1222+
cavlRemove(&que->deadline_root, &item->deadline_base);
11311223
que->size--;
11321224
}
11331225
return item;
@@ -1169,11 +1261,13 @@ int8_t canardRxAccept(struct CanardInstance* const ins,
11691261
// This is the reason the function has a logarithmic time complexity of the number of subscriptions.
11701262
// Note also that this one of the two variable-complexity operations in the RX pipeline; the other one
11711263
// is memcpy(). Excepting these two cases, the entire RX pipeline contains neither loops nor recursion.
1172-
struct CanardRxSubscription* const sub = (struct CanardRxSubscription*) (void*)
1264+
struct CanardRxSubscription* const sub = MUTABLE_CONTAINER_OF( //
1265+
struct CanardRxSubscription,
11731266
cavlSearch(&ins->rx_subscriptions[(size_t) model.transfer_kind],
11741267
&model.port_id,
11751268
&rxSubscriptionPredicateOnPortID,
1176-
NULL);
1269+
NULL),
1270+
base);
11771271
if (out_subscription != NULL)
11781272
{
11791273
*out_subscription = sub; // Expose selected instance to the caller.
@@ -1230,7 +1324,7 @@ int8_t canardRxSubscribe(struct CanardInstance* const ins,
12301324
out_subscription->sessions[i] = NULL;
12311325
}
12321326
const struct CanardTreeNode* const res = cavlSearch(&ins->rx_subscriptions[tk],
1233-
out_subscription,
1327+
&out_subscription->base,
12341328
&rxSubscriptionPredicateOnStruct,
12351329
&avlTrivialFactory);
12361330
(void) res;
@@ -1249,9 +1343,12 @@ int8_t canardRxUnsubscribe(struct CanardInstance* const ins,
12491343
const size_t tk = (size_t) transfer_kind;
12501344
if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS))
12511345
{
1252-
CanardPortID port_id_mutable = port_id;
1253-
struct CanardRxSubscription* const sub = (struct CanardRxSubscription*) (void*)
1254-
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL);
1346+
CanardPortID port_id_mutable = port_id;
1347+
1348+
struct CanardRxSubscription* const sub = MUTABLE_CONTAINER_OF( //
1349+
struct CanardRxSubscription,
1350+
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL),
1351+
base);
12551352
if (sub != NULL)
12561353
{
12571354
cavlRemove(&ins->rx_subscriptions[tk], &sub->base);
@@ -1287,9 +1384,12 @@ int8_t canardRxGetSubscription(struct CanardInstance* const ins,
12871384
const size_t tk = (size_t) transfer_kind;
12881385
if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS))
12891386
{
1290-
CanardPortID port_id_mutable = port_id;
1291-
struct CanardRxSubscription* const sub = (struct CanardRxSubscription*) (void*)
1292-
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL);
1387+
CanardPortID port_id_mutable = port_id;
1388+
1389+
struct CanardRxSubscription* const sub = MUTABLE_CONTAINER_OF( //
1390+
struct CanardRxSubscription,
1391+
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL),
1392+
base);
12931393
if (sub != NULL)
12941394
{
12951395
CANARD_ASSERT(sub->port_id == port_id);

0 commit comments

Comments
 (0)