Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

introduce deadline queue #sonar #238

Merged
merged 7 commits into from
Dec 5, 2024
Merged

introduce deadline queue #sonar #238

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
5670ba5
introduce deadline queue #sonar
serges147 Dec 3, 2024
3c1535f
pr review and build fixes #sonar
serges147 Dec 3, 2024
4f1a28c
build fixes #sonar
serges147 Dec 3, 2024
7ec9051
minor fix
serges147 Dec 3, 2024
c67a59c
style fix
serges147 Dec 3, 2024
ee67adc
`canardTxPush` now flushes expired frames
serges147 Dec 4, 2024
78b62fa
build fix
serges147 Dec 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
166 changes: 133 additions & 33 deletions libcanard/canard.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
/// Author: Pavel Kirienko <[email protected]>

#include "canard.h"
#include <stddef.h>
#include <string.h>

// --------------------------------------------- BUILD CONFIGURATION ---------------------------------------------
Expand Down Expand Up @@ -70,6 +71,11 @@

#define INITIAL_TOGGLE_STATE true

#define CONTAINER_OF(type, ptr, member) \
((const type*) (((ptr) == NULL) ? NULL : (const void*) (((const char*) (ptr)) - offsetof(type, member))))
#define MUTABLE_CONTAINER_OF(type, ptr, member) \
((type*) (((ptr) == NULL) ? NULL : (void*) (((char*) (ptr)) - offsetof(type, member))))

/// Used for inserting new items into AVL trees.
CANARD_PRIVATE struct CanardTreeNode* avlTrivialFactory(void* const user_reference)
{
Expand Down Expand Up @@ -299,10 +305,15 @@ CANARD_PRIVATE struct CanardTxQueueItem* txAllocateQueueItem(struct CanardTxQueu
struct CanardTxQueueItem* out = ins->memory.allocate(ins->memory.user_reference, sizeof(struct CanardTxQueueItem));
if (out != NULL)
{
out->base.up = NULL;
out->base.lr[0] = NULL;
out->base.lr[1] = NULL;
out->base.bf = 0;
out->priority_base.up = NULL;
out->priority_base.lr[0] = NULL;
out->priority_base.lr[1] = NULL;
out->priority_base.bf = 0;

out->deadline_base.up = NULL;
out->deadline_base.lr[0] = NULL;
out->deadline_base.lr[1] = NULL;
out->deadline_base.bf = 0;

out->next_in_transfer = NULL; // Last by default.
out->tx_deadline_usec = deadline_usec;
Expand All @@ -329,15 +340,34 @@ CANARD_PRIVATE struct CanardTxQueueItem* txAllocateQueueItem(struct CanardTxQueu
/// Frames with identical CAN ID that are added later always compare greater than their counterparts with same CAN ID.
/// This ensures that CAN frames with the same CAN ID are transmitted in the FIFO order.
/// Frames that should be transmitted earlier compare smaller (i.e., put on the left side of the tree).
CANARD_PRIVATE int8_t txAVLPredicate(void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
const struct CanardTreeNode* const node)
CANARD_PRIVATE int8_t txAVLPriorityPredicate( //
void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
const struct CanardTreeNode* const node)
{
const struct CanardTxQueueItem* const target = (const struct CanardTxQueueItem*) user_reference;
const struct CanardTxQueueItem* const other = (const struct CanardTxQueueItem*) (const void*) node;
typedef struct CanardTxQueueItem TxItem;

const TxItem* const target = CONTAINER_OF(TxItem, user_reference, priority_base);
const TxItem* const other = CONTAINER_OF(TxItem, node, priority_base);
CANARD_ASSERT((target != NULL) && (other != NULL));
return (target->frame.extended_can_id >= other->frame.extended_can_id) ? +1 : -1;
}

/// Frames with identical deadline
/// that are added later always compare greater than their counterparts with the same deadline.
/// This ensures that CAN frames with the same deadline are, when timed out, dropped in the FIFO order.
/// Frames that should be dropped earlier compare smaller (i.e., put on the left side of the tree).
CANARD_PRIVATE int8_t txAVLDeadlinePredicate( //
void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
const struct CanardTreeNode* const node)
{
typedef struct CanardTxQueueItem TxItem;

const TxItem* const target = CONTAINER_OF(TxItem, user_reference, deadline_base);
const TxItem* const other = CONTAINER_OF(TxItem, node, deadline_base);
CANARD_ASSERT((target != NULL) && (other != NULL));
return (target->tx_deadline_usec >= other->tx_deadline_usec) ? +1 : -1;
}

/// Returns the number of frames enqueued or error (i.e., =1 or <0).
CANARD_PRIVATE int32_t txPushSingleFrame(struct CanardTxQueue* const que,
const struct CanardInstance* const ins,
Expand Down Expand Up @@ -369,11 +399,19 @@ CANARD_PRIVATE int32_t txPushSingleFrame(struct CanardTxQueue* const que,
uint8_t* const frame_bytes = tqi->frame.payload.data;
(void) memset(frame_bytes + payload.size, PADDING_BYTE_VALUE, padding_size); // NOLINT
*(frame_bytes + (frame_payload_size - 1U)) = txMakeTailByte(true, true, true, transfer_id);
// Insert the newly created TX item into the queue.
const struct CanardTreeNode* const res =
cavlSearch(&que->root, &tqi->base, &txAVLPredicate, &avlTrivialFactory);
(void) res;
CANARD_ASSERT(res == &tqi->base);

// Insert the newly created TX item into the priority queue.
const struct CanardTreeNode* const priority_queue_res =
cavlSearch(&que->priority_root, &tqi->priority_base, &txAVLPriorityPredicate, &avlTrivialFactory);
(void) priority_queue_res;
CANARD_ASSERT(priority_queue_res == &tqi->priority_base);

// Insert the newly created TX item into the deadline queue.
const struct CanardTreeNode* const deadline_queue_res =
cavlSearch(&que->deadline_root, &tqi->deadline_base, &txAVLDeadlinePredicate, &avlTrivialFactory);
(void) deadline_queue_res;
CANARD_ASSERT(deadline_queue_res == &tqi->deadline_base);

que->size++;
CANARD_ASSERT(que->size <= que->capacity);
out = 1; // One frame enqueued.
Expand Down Expand Up @@ -514,11 +552,18 @@ CANARD_PRIVATE int32_t txPushMultiFrame(struct CanardTxQueue* const que,
struct CanardTxQueueItem* next = sq.head;
do
{
const struct CanardTreeNode* const res =
cavlSearch(&que->root, &next->base, &txAVLPredicate, &avlTrivialFactory);
(void) res;
CANARD_ASSERT(res == &next->base);
CANARD_ASSERT(que->root != NULL);
const struct CanardTreeNode* const priority_queue_res =
cavlSearch(&que->priority_root, &next->priority_base, &txAVLPriorityPredicate, &avlTrivialFactory);
(void) priority_queue_res;
CANARD_ASSERT(priority_queue_res == &next->priority_base);
CANARD_ASSERT(que->priority_root != NULL);

const struct CanardTreeNode* const deadline_queue_res =
cavlSearch(&que->deadline_root, &next->deadline_base, &txAVLDeadlinePredicate, &avlTrivialFactory);
(void) deadline_queue_res;
CANARD_ASSERT(deadline_queue_res == &next->deadline_base);
CANARD_ASSERT(que->deadline_root != NULL);

next = next->next_in_transfer;
} while (next != NULL);
CANARD_ASSERT(num_frames == sq.size);
Expand Down Expand Up @@ -547,6 +592,35 @@ CANARD_PRIVATE int32_t txPushMultiFrame(struct CanardTxQueue* const que,
return out;
}

/// Flushes expired transfers by comparing deadline timestamps of the pending transfers with the current time.
CANARD_PRIVATE void txFlushExpiredTransfers(struct CanardTxQueue* const que,
const struct CanardInstance* const ins,
const CanardMicrosecond now_usec)
{
struct CanardTxQueueItem* tx_item = NULL;
while (NULL != (tx_item = MUTABLE_CONTAINER_OF( //
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Assignment in an expression violates some MISRA C rule whose number I forgot (ChatGPT can tell you which one); please split this into the longer form.

struct CanardTxQueueItem,
cavlFindExtremum(que->deadline_root, false),
deadline_base)))
{
if (now_usec <= tx_item->tx_deadline_usec)
{
// The queue is sorted by deadline, so we can stop here.
break;
}

// All frames of the transfer are released at once b/c they all have the same deadline.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we really need this special case here? All frames in a transfer have the same timeout (IIRC it is mentioned in the docs, too), so either all or none will be kept even if you don't include this special case. On the other hand, one might argue that the current solution is more robust because it doesn't make assumptions about timeout equality. So maybe we do need this special case, after all.

There is also a related question: do we really need to locate the extremum at every iteration? This looks like an avoidable Shliemel the painter problem. What we have right now is good enough but perhaps an explanatory comment is warranted, saying that it should be possible to avoid extremum call at every iteration by simply fetching the neighboring node from the current one before deleting it. Obviously this will only work if we rely on the assumption that the timeout is the same for all frames in the transfer (otherwise the loop below will invalidate the tree linkage).

Copy link
Contributor Author

@serges147 serges147 Dec 5, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe that such flushing is more like for robustness and "self healing" for a special corner case, namely when media stopped to work for some time (f.e. stopped to answer/callback at all), whole TX capacity exhausted, then media recovered but it won't be engaged/triggered anymore b/c we cannot push anything new due to full capacity. Flushing by timeout will resolve such deadlock.

really need to locate the extremum

I think it is okey, especially if you think about nominal use case in which there will be only one call to cavlFindExtremum, it will return NULL (b/c normally everything properly transmitted before deadline), and that is it.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

While the current solution is acceptable, I must point out that in real-time programming, one should focus not only on the general case but also on the worst case.

struct CanardTxQueueItem* tx_item_to_free = NULL;
while (NULL != (tx_item_to_free = canardTxPop(que, tx_item)))
{
tx_item = tx_item_to_free->next_in_transfer;
canardTxFree(que, ins, tx_item_to_free);

que->stats.dropped_frames++;
}
}
}

// --------------------------------------------- RECEPTION ---------------------------------------------

#define RX_SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U)
Expand Down Expand Up @@ -1005,8 +1079,9 @@ CANARD_PRIVATE int8_t
rxSubscriptionPredicateOnPortID(void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
const struct CanardTreeNode* const node)
{
CANARD_ASSERT((user_reference != NULL) && (node != NULL));
const CanardPortID sought = *((const CanardPortID*) user_reference);
const CanardPortID other = ((const struct CanardRxSubscription*) (const void*) node)->port_id;
const CanardPortID other = CONTAINER_OF(struct CanardRxSubscription, node, base)->port_id;
static const int8_t NegPos[2] = {-1, +1};
// Clang-Tidy mistakenly identifies a narrowing cast to int8_t here, which is incorrect.
return (sought == other) ? 0 : NegPos[sought > other]; // NOLINT no narrowing conversion is taking place here
Expand All @@ -1016,7 +1091,9 @@ CANARD_PRIVATE int8_t
rxSubscriptionPredicateOnStruct(void* const user_reference, // NOSONAR Cavl API requires pointer to non-const.
const struct CanardTreeNode* const node)
{
return rxSubscriptionPredicateOnPortID(&((struct CanardRxSubscription*) user_reference)->port_id, node);
return rxSubscriptionPredicateOnPortID( //
&MUTABLE_CONTAINER_OF(struct CanardRxSubscription, user_reference, base)->port_id,
node);
}

// --------------------------------------------- PUBLIC API ---------------------------------------------
Expand Down Expand Up @@ -1056,7 +1133,8 @@ struct CanardTxQueue canardTxInit(const size_t capacity,
.capacity = capacity,
.mtu_bytes = mtu_bytes,
.size = 0,
.root = NULL,
.priority_root = NULL,
.deadline_root = NULL,
.memory = memory,
.user_reference = NULL,
};
Expand All @@ -1067,8 +1145,20 @@ int32_t canardTxPush(struct CanardTxQueue* const que,
const struct CanardInstance* const ins,
const CanardMicrosecond tx_deadline_usec,
const struct CanardTransferMetadata* const metadata,
const struct CanardPayload payload)
const struct CanardPayload payload,
const CanardMicrosecond now_usec)
{
// Before pushing payload (potentially in multiple frames), we need to try to flush any expired transfers.
// This is necessary to ensure that we don't exhaust the capacity of the queue by holding outdated frames.
// The flushing is done by comparing deadline timestamps of the pending transfers with the current time,
// which makes sense only if the current time is known (bigger than zero).
if (now_usec > 0)
{
txFlushExpiredTransfers(que, ins, now_usec);
}

(void) now_usec;
pavel-kirienko marked this conversation as resolved.
Show resolved Hide resolved

int32_t out = -CANARD_ERROR_INVALID_ARGUMENT;
if ((ins != NULL) && (que != NULL) && (metadata != NULL) && ((payload.data != NULL) || (0U == payload.size)))
{
Expand Down Expand Up @@ -1114,7 +1204,8 @@ struct CanardTxQueueItem* canardTxPeek(const struct CanardTxQueue* const que)
{
// Paragraph 6.7.2.1.15 of the C standard says:
// A pointer to a structure object, suitably converted, points to its initial member, and vice versa.
out = (struct CanardTxQueueItem*) (void*) cavlFindExtremum(que->root, false);
struct CanardTreeNode* const priority_node = cavlFindExtremum(que->priority_root, false);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The comment above (citing the standard) is no longer applicable so it should be removed.

out = MUTABLE_CONTAINER_OF(struct CanardTxQueueItem, priority_node, priority_base);
}
return out;
}
Expand All @@ -1127,7 +1218,8 @@ struct CanardTxQueueItem* canardTxPop(struct CanardTxQueue* const que, struct Ca
// A pointer to a structure object, suitably converted, points to its initial member, and vice versa.
// Note that the highest-priority frame is always a leaf node in the AVL tree, which means that it is very
// cheap to remove.
cavlRemove(&que->root, &item->base);
cavlRemove(&que->priority_root, &item->priority_base);
cavlRemove(&que->deadline_root, &item->deadline_base);
que->size--;
}
return item;
Expand Down Expand Up @@ -1169,11 +1261,13 @@ int8_t canardRxAccept(struct CanardInstance* const ins,
// This is the reason the function has a logarithmic time complexity of the number of subscriptions.
// Note also that this one of the two variable-complexity operations in the RX pipeline; the other one
// is memcpy(). Excepting these two cases, the entire RX pipeline contains neither loops nor recursion.
struct CanardRxSubscription* const sub = (struct CanardRxSubscription*) (void*)
struct CanardRxSubscription* const sub = MUTABLE_CONTAINER_OF( //
struct CanardRxSubscription,
cavlSearch(&ins->rx_subscriptions[(size_t) model.transfer_kind],
&model.port_id,
&rxSubscriptionPredicateOnPortID,
NULL);
NULL),
base);
if (out_subscription != NULL)
{
*out_subscription = sub; // Expose selected instance to the caller.
Expand Down Expand Up @@ -1230,7 +1324,7 @@ int8_t canardRxSubscribe(struct CanardInstance* const ins,
out_subscription->sessions[i] = NULL;
}
const struct CanardTreeNode* const res = cavlSearch(&ins->rx_subscriptions[tk],
out_subscription,
&out_subscription->base,
&rxSubscriptionPredicateOnStruct,
&avlTrivialFactory);
(void) res;
Expand All @@ -1249,9 +1343,12 @@ int8_t canardRxUnsubscribe(struct CanardInstance* const ins,
const size_t tk = (size_t) transfer_kind;
if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS))
{
CanardPortID port_id_mutable = port_id;
struct CanardRxSubscription* const sub = (struct CanardRxSubscription*) (void*)
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL);
CanardPortID port_id_mutable = port_id;

struct CanardRxSubscription* const sub = MUTABLE_CONTAINER_OF( //
struct CanardRxSubscription,
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL),
base);
if (sub != NULL)
{
cavlRemove(&ins->rx_subscriptions[tk], &sub->base);
Expand Down Expand Up @@ -1287,9 +1384,12 @@ int8_t canardRxGetSubscription(struct CanardInstance* const ins,
const size_t tk = (size_t) transfer_kind;
if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS))
{
CanardPortID port_id_mutable = port_id;
struct CanardRxSubscription* const sub = (struct CanardRxSubscription*) (void*)
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL);
CanardPortID port_id_mutable = port_id;

struct CanardRxSubscription* const sub = MUTABLE_CONTAINER_OF( //
struct CanardRxSubscription,
cavlSearch(&ins->rx_subscriptions[tk], &port_id_mutable, &rxSubscriptionPredicateOnPortID, NULL),
base);
if (sub != NULL)
{
CANARD_ASSERT(sub->port_id == port_id);
Expand Down
Loading
Loading