Skip to content

Commit 9ffe23b

Browse files
bugobliteratorbugobliterator
committed
add support for buffered CANFrames in TxQueue
1 parent 67e3455 commit 9ffe23b

File tree

3 files changed

+410
-37
lines changed

3 files changed

+410
-37
lines changed

‎canard.c‎

Lines changed: 310 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -375,19 +375,223 @@ int16_t canardRequestOrRespondObj(CanardInstance* ins, uint8_t destination_node_
375375
return result;
376376
}
377377

378-
CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins)
378+
#if CANARD_MAX_MTU > 8
379+
CANARD_INTERNAL int16_t canardBufferedCANFramePushBytes(CanardPoolAllocator* allocator,
380+
CanardTxQueueItem *queue_item,
381+
const uint8_t* data,
382+
uint16_t data_len)
383+
{
384+
if (data_len == 0) {
385+
return CANARD_OK;
386+
}
387+
388+
389+
390+
uint16_t global_data_index;
391+
if (CANARD_TX_QUEUE_PAYLOAD_HEAD_SIZE > queue_item->frame.data_len) {
392+
uint16_t head_space = CANARD_TX_QUEUE_PAYLOAD_HEAD_SIZE - queue_item->frame.data_len;
393+
// there might still be space in the head
394+
if (head_space >= data_len) {
395+
if (data != NULL) {
396+
memcpy(queue_item->payload_head + queue_item->frame.data_len, data, data_len);
397+
} else {
398+
// push zeros
399+
memset(queue_item->payload_head + queue_item->frame.data_len, 0, data_len);
400+
}
401+
queue_item->frame.data_len += data_len;
402+
return CANARD_OK;
403+
}
404+
memcpy(queue_item->payload_head + queue_item->frame.data_len, data, head_space);
405+
queue_item->frame.data_len += head_space;
406+
if (data != NULL) {
407+
data += head_space;
408+
}
409+
data_len -= head_space;
410+
}
411+
// we still have some data to copy
412+
global_data_index = CANARD_TX_QUEUE_PAYLOAD_HEAD_SIZE;
413+
CanardBufferBlock* block = NULL;
414+
if (queue_item->payload_next == CANARD_BUFFER_IDX_NONE) {
415+
// we need to allocate a new block
416+
block = createBufferBlock(allocator);
417+
if (block == NULL) {
418+
return -CANARD_ERROR_OUT_OF_MEMORY;
419+
}
420+
queue_item->payload_next = canardBufferToIdx(allocator, block);
421+
} else {
422+
block = canardBufferFromIdx(allocator, queue_item->payload_next);
423+
}
424+
// move data_index for the first block
425+
global_data_index += CANARD_BUFFER_BLOCK_DATA_SIZE;
426+
427+
CANARD_ASSERT(block != NULL);
428+
// look for the last empty block or the block that still has space
429+
while (block->next != NULL && (global_data_index < queue_item->frame.data_len)) {
430+
block = block->next;
431+
global_data_index += CANARD_BUFFER_BLOCK_DATA_SIZE;
432+
}
433+
434+
if (global_data_index > queue_item->frame.data_len) {
435+
// we have a block that still has space
436+
uint16_t block_space = (uint16_t)(global_data_index - queue_item->frame.data_len);
437+
uint16_t block_offset = (uint16_t)(CANARD_BUFFER_BLOCK_DATA_SIZE - block_space);
438+
if (block_space >= data_len) {
439+
if (data != NULL) {
440+
memcpy(block->data + block_offset, data, data_len);
441+
} else {
442+
// push zeros
443+
memset(block->data + block_offset, 0, data_len);
444+
}
445+
queue_item->frame.data_len += data_len;
446+
return CANARD_OK;
447+
}
448+
if (data != NULL) {
449+
memcpy(block->data + block_offset, data, block_space);
450+
} else {
451+
// push zeros
452+
memset(block->data + block_offset, 0, block_space);
453+
}
454+
queue_item->frame.data_len += block_space;
455+
if (data != NULL) {
456+
data += block_space;
457+
}
458+
data_len -= block_space;
459+
}
460+
461+
CANARD_ASSERT(block->next == NULL);
462+
// there is no next block, create one
463+
block->next = createBufferBlock(allocator);
464+
block = block->next;
465+
if (block == NULL) {
466+
CANARD_ASSERT(false);
467+
return -CANARD_ERROR_OUT_OF_MEMORY;
468+
}
469+
470+
// if there is more data to copy, allocate new block(s)
471+
uint16_t data_index = 0;
472+
while (data_index < data_len)
473+
{
474+
CANARD_ASSERT(block != NULL);
475+
for (uint16_t i = 0; (i < CANARD_BUFFER_BLOCK_DATA_SIZE) && (data_index < data_len); i++)
476+
{
477+
if (data != NULL) {
478+
block->data[i] = data[data_index];
479+
} else {
480+
block->data[i] = 0;
481+
}
482+
data_index++;
483+
}
484+
485+
if (data_index < data_len)
486+
{
487+
block->next = createBufferBlock(allocator);
488+
if (block->next == NULL)
489+
{
490+
CANARD_ASSERT(false);
491+
return -CANARD_ERROR_OUT_OF_MEMORY;
492+
}
493+
block = block->next;
494+
}
495+
}
496+
queue_item->frame.data_len += data_index;
497+
return CANARD_OK;
498+
}
499+
500+
#define CANARD_Q2F_COPY_VALUE(param1, param2, field) param1->field = param2->frame.field
501+
502+
CANARD_INTERNAL void canardBufferedCANFrameToCANFrame(CanardPoolAllocator *allocator,
503+
CanardCANFrame *in_out_frame,
504+
CanardTxQueueItem *queue_item)
505+
{
506+
// copy all fields except payload
507+
CANARD_Q2F_COPY_VALUE(in_out_frame, queue_item, id);
508+
#if CANARD_ENABLE_DEADLINE
509+
CANARD_Q2F_COPY_VALUE(in_out_frame, queue_item, deadline_usec);
510+
#endif
511+
CANARD_Q2F_COPY_VALUE(in_out_frame, queue_item, data_len);
512+
CANARD_Q2F_COPY_VALUE(in_out_frame, queue_item, iface_id);
513+
#if CANARD_MULTI_IFACE
514+
CANARD_Q2F_COPY_VALUE(in_out_frame, queue_item, iface_mask);
515+
#endif
516+
#if CANARD_ENABLE_CANFD
517+
CANARD_Q2F_COPY_VALUE(in_out_frame, queue_item, canfd);
518+
#endif
519+
520+
// copy payload
521+
if (CANARD_TX_QUEUE_PAYLOAD_HEAD_SIZE > queue_item->frame.data_len) {
522+
memcpy(in_out_frame->data, queue_item->payload_head, queue_item->frame.data_len);
523+
return;
524+
}
525+
memcpy(in_out_frame->data, queue_item->payload_head, CANARD_TX_QUEUE_PAYLOAD_HEAD_SIZE);
526+
CanardBufferBlock* block = canardBufferFromIdx(allocator, queue_item->payload_next);
527+
uint8_t data_index = CANARD_TX_QUEUE_PAYLOAD_HEAD_SIZE;
528+
while (block != NULL) {
529+
if (data_index + CANARD_BUFFER_BLOCK_DATA_SIZE > queue_item->frame.data_len) {
530+
CANARD_ASSERT(data_index < sizeof(in_out_frame->data));
531+
memcpy(in_out_frame->data + data_index, block->data, queue_item->frame.data_len - data_index);
532+
return;
533+
}
534+
CANARD_ASSERT(data_index < sizeof(in_out_frame->data));
535+
memcpy(in_out_frame->data + data_index, block->data, CANARD_BUFFER_BLOCK_DATA_SIZE);
536+
data_index += CANARD_BUFFER_BLOCK_DATA_SIZE;
537+
block = block->next;
538+
}
539+
CANARD_ASSERT(data_index == queue_item->frame.data_len);
540+
}
541+
542+
CANARD_INTERNAL void canardReleaseCANFrameBuffer(CanardPoolAllocator* allocator, CanardTxQueueItem *queue_item)
543+
{
544+
if (queue_item->payload_next == CANARD_BUFFER_IDX_NONE) {
545+
return;
546+
}
547+
CanardBufferBlock* block = canardBufferFromIdx(allocator, queue_item->payload_next);
548+
while (block != NULL) {
549+
CanardBufferBlock* next = block->next;
550+
freeBlock(allocator, block);
551+
block = next;
552+
}
553+
queue_item->payload_next = CANARD_BUFFER_IDX_NONE;
554+
}
555+
#endif
556+
557+
const CanardCANFrame* canardTxQueueItemToFrame(CanardInstance* ins, CanardTxQueueItem* item)
379558
{
380559
if (ins->tx_queue == NULL)
381560
{
382561
return NULL;
383562
}
384-
return &ins->tx_queue->frame;
563+
#if CANARD_MAX_MTU > 8
564+
// convert the buffered frame to a full frame
565+
canardBufferedCANFrameToCANFrame(&ins->allocator, &ins->tmp_frame, item);
566+
return &ins->tmp_frame;
567+
#else
568+
return &item->frame;
569+
#endif
570+
}
571+
572+
const CanardCANFrame* canardPeekTxQueue(CanardInstance* ins)
573+
{
574+
return canardTxQueueItemToFrame(ins, ins->tx_queue);
575+
}
576+
577+
#if CANARD_MULTI_IFACE
578+
void canardMarkFrameTransmitted(CanardTxQueueItem* obj, uint8_t iface)
579+
{
580+
CANARD_ASSERT(obj != NULL);
581+
if (obj == NULL) {
582+
return;
583+
}
584+
obj->frame.iface_mask &= ~(uint8_t)(1 << iface);
385585
}
586+
#endif
386587

387588
void canardPopTxQueue(CanardInstance* ins)
388589
{
389590
CanardTxQueueItem* item = ins->tx_queue;
390591
ins->tx_queue = item->next;
592+
#if CANARD_MAX_MTU > 8
593+
canardReleaseCANFrameBuffer(&ins->allocator, item);
594+
#endif
391595
freeBlock(&ins->allocator, item);
392596
}
393597

@@ -1098,9 +1302,9 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
10981302

10991303
int16_t result = 0;
11001304
#if CANARD_ENABLE_CANFD
1101-
uint8_t frame_max_data_len = transfer->canfd ? CANARD_CANFD_FRAME_MAX_DATA_LEN:CANARD_CAN_FRAME_MAX_DATA_LEN;
1305+
uint16_t frame_max_data_len = transfer->canfd ? CANARD_CANFD_FRAME_MAX_DATA_LEN:CANARD_CAN_FRAME_MAX_DATA_LEN;
11021306
#else
1103-
uint8_t frame_max_data_len = CANARD_CAN_FRAME_MAX_DATA_LEN;
1307+
uint16_t frame_max_data_len = CANARD_CAN_FRAME_MAX_DATA_LEN;
11041308
#endif
11051309
if (transfer->payload_len < frame_max_data_len) // Single frame transfer
11061310
{
@@ -1110,11 +1314,49 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
11101314
return -CANARD_ERROR_OUT_OF_MEMORY;
11111315
}
11121316

1317+
#if CANARD_MAX_MTU <= 8
11131318
memcpy(queue_item->frame.data, transfer->payload, transfer->payload_len);
1319+
#else
1320+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1321+
queue_item,
1322+
transfer->payload,
1323+
transfer->payload_len);
1324+
if (result != CANARD_OK) {
1325+
CANARD_ASSERT(false);
1326+
return result;
1327+
}
1328+
#endif
11141329

11151330
transfer->payload_len = dlcToDataLength(dataLengthToDlc(transfer->payload_len+1))-1;
1331+
#if CANARD_MAX_MTU > 8
1332+
if (transfer->canfd) {
1333+
uint8_t pad_bytes = (uint8_t)(transfer->payload_len - queue_item->frame.data_len);
1334+
if (pad_bytes > 0) {
1335+
// push zeroed padding bytes
1336+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1337+
queue_item,
1338+
NULL, // push zeroed bytes
1339+
pad_bytes);
1340+
if (result != CANARD_OK) {
1341+
CANARD_ASSERT(false);
1342+
return result;
1343+
}
1344+
}
1345+
}
1346+
// push tail byte
1347+
uint8_t tail_byte = (uint8_t)(0xC0U | (*transfer->inout_transfer_id & 31U));
1348+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1349+
queue_item,
1350+
&tail_byte,
1351+
1);
1352+
if (result != CANARD_OK) {
1353+
CANARD_ASSERT(false);
1354+
return result;
1355+
}
1356+
#else
11161357
queue_item->frame.data_len = (uint8_t)(transfer->payload_len + 1);
11171358
queue_item->frame.data[transfer->payload_len] = (uint8_t)(0xC0U | (*transfer->inout_transfer_id & 31U));
1359+
#endif
11181360
queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF;
11191361
#if CANARD_ENABLE_DEADLINE
11201362
queue_item->frame.deadline_usec = transfer->deadline_usec;
@@ -1132,7 +1374,7 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
11321374
{
11331375
uint16_t data_index = 0;
11341376
uint8_t toggle = 0;
1135-
uint8_t sot_eot = 0x80;
1377+
uint8_t tail_byte = 0x80; // intialized to mark as start of transfer (sot)
11361378

11371379
CanardTxQueueItem* queue_item = NULL;
11381380

@@ -1148,25 +1390,83 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
11481390
if (data_index == 0)
11491391
{
11501392
// add crc
1393+
#if CANARD_MAX_MTU > 8
1394+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1395+
queue_item,
1396+
(uint8_t*)&crc,
1397+
2);
1398+
if (result != CANARD_OK) {
1399+
CANARD_ASSERT(false);
1400+
return result;
1401+
}
1402+
#else
11511403
queue_item->frame.data[0] = (uint8_t) (crc);
11521404
queue_item->frame.data[1] = (uint8_t) (crc >> 8U);
1405+
#endif
11531406
i = 2;
11541407
}
11551408
else
11561409
{
11571410
i = 0;
11581411
}
11591412

1413+
#if CANARD_MAX_MTU > 8
1414+
if (data_index < transfer->payload_len) {
1415+
// push payload
1416+
uint16_t copy_length = (uint16_t)MIN(transfer->payload_len - data_index, frame_max_data_len - i - 1);
1417+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1418+
queue_item,
1419+
&transfer->payload[data_index],
1420+
copy_length);
1421+
if (result != CANARD_OK) {
1422+
CANARD_ASSERT(false);
1423+
return result;
1424+
}
1425+
data_index += copy_length;
1426+
i += copy_length;
1427+
}
1428+
#else
11601429
for (; i < (frame_max_data_len - 1) && data_index < transfer->payload_len; i++, data_index++)
11611430
{
11621431
queue_item->frame.data[i] = transfer->payload[data_index];
11631432
}
1164-
// tail byte
1165-
sot_eot = (data_index == transfer->payload_len) ? (uint8_t)0x40 : sot_eot;
1166-
1433+
#endif
1434+
// prepare tail byte
1435+
tail_byte = (data_index == transfer->payload_len) ? (uint8_t)0x40 : tail_byte; // mark sot/eot
1436+
tail_byte = (uint8_t)(tail_byte | ((uint32_t)toggle << 5U) | ((uint32_t)*transfer->inout_transfer_id & 31U));
11671437
i = dlcToDataLength(dataLengthToDlc(i+1))-1;
1168-
queue_item->frame.data[i] = (uint8_t)(sot_eot | ((uint32_t)toggle << 5U) | ((uint32_t)*transfer->inout_transfer_id & 31U));
1438+
#if CANARD_MAX_MTU > 8
1439+
// push padding bytes if canfd
1440+
if (transfer->canfd) {
1441+
uint8_t pad_bytes = (uint8_t)(i - queue_item->frame.data_len);
1442+
if (pad_bytes > 0) {
1443+
// push zeroed padding bytes
1444+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1445+
queue_item,
1446+
NULL, // push zeroed bytes
1447+
pad_bytes);
1448+
if (result != CANARD_OK) {
1449+
CANARD_ASSERT(false);
1450+
return result;
1451+
}
1452+
}
1453+
}
1454+
// push tail byte
1455+
result = canardBufferedCANFramePushBytes(&ins->allocator,
1456+
queue_item,
1457+
&tail_byte,
1458+
1);
1459+
if (result != CANARD_OK) {
1460+
CANARD_ASSERT(false);
1461+
return result;
1462+
}
1463+
#else
1464+
queue_item->frame.data[i] = tail_byte;
1465+
#endif
11691466
queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF;
1467+
#if CANARD_MAX_MTU > 8
1468+
CANARD_ASSERT(queue_item->frame.data_len == (i+1));
1469+
#endif
11701470
queue_item->frame.data_len = (uint8_t)(i + 1);
11711471
#if CANARD_ENABLE_DEADLINE
11721472
queue_item->frame.deadline_usec = transfer->deadline_usec;
@@ -1181,7 +1481,7 @@ CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
11811481

11821482
result++;
11831483
toggle ^= 1;
1184-
sot_eot = 0;
1484+
tail_byte = 0;
11851485
}
11861486
}
11871487

0 commit comments

Comments
 (0)