Skip to content

Commit 4fdd031

Browse files
pavel-kirienkopavel-kirienko
committed
Implement v1/v0 service request-response APIs and align unicast CAN-ID encoding
1 parent 4ca9dd1 commit 4fdd031

File tree

3 files changed

+389
-14
lines changed

3 files changed

+389
-14
lines changed

‎libcanard/canard.c‎

Lines changed: 124 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -842,14 +842,13 @@ bool canard_new(canard_t* const self,
842842
const size_t filter_count,
843843
canard_filter_t* const filter_storage)
844844
{
845-
const bool has_valid_node_id = (node_id <= CANARD_NODE_ID_MAX) || (node_id == CANARD_NODE_ID_ANONYMOUS);
846-
bool ok = (self != NULL) && (vtable != NULL) && (vtable->now != NULL) && (vtable->tx != NULL) &&
845+
bool ok = (self != NULL) && (vtable != NULL) && (vtable->now != NULL) && (vtable->tx != NULL) &&
847846
(vtable->filter != NULL) && mem_valid(memory.tx_transfer) && mem_valid(memory.tx_frame) &&
848847
mem_valid(memory.rx_session) && mem_valid(memory.rx_payload) &&
849-
((filter_count == 0U) || (filter_storage != NULL)) && has_valid_node_id;
848+
((filter_count == 0U) || (filter_storage != NULL)) && (node_id <= CANARD_NODE_ID_MAX);
850849
if (ok) {
851850
(void)memset(self, 0, sizeof(*self));
852-
self->node_id = (node_id <= CANARD_NODE_ID_MAX) ? node_id : 0U;
851+
self->node_id = (node_id <= CANARD_NODE_ID_MAX) ? node_id : CANARD_NODE_ID_ANONYMOUS;
853852
self->tx.fd = true;
854853
self->tx.queue_capacity = tx_queue_capacity;
855854
self->rx.filter_count = filter_count;
@@ -900,6 +899,17 @@ uint_least8_t canard_pending_ifaces(const canard_t* const self)
900899
return out;
901900
}
902901

902+
// Common Cyphal v1.0 service TX path for both request and response.
903+
static bool tx_1v0_service(canard_t* const self,
904+
const canard_us_t deadline,
905+
const canard_prio_t priority,
906+
const uint16_t service_id,
907+
const uint_least8_t destination_node_id,
908+
const bool request_not_response,
909+
const uint_least8_t transfer_id,
910+
const canard_bytes_chain_t payload,
911+
const canard_user_context_t context);
912+
903913
bool canard_publish(canard_t* const self,
904914
const canard_us_t deadline,
905915
const uint_least8_t iface_bitmap,
@@ -928,16 +938,21 @@ bool canard_unicast(canard_t* const self,
928938
const canard_bytes_chain_t payload,
929939
const canard_user_context_t context)
930940
{
941+
// Keep transfer-ID map behavior: increment only if basic argument validation succeeds.
931942
bool ok = (self != NULL) && (priority < CANARD_PRIO_COUNT) && bytes_chain_valid(payload) &&
932943
(destination_node_id <= CANARD_NODE_ID_MAX);
933944
if (ok) {
934-
const byte_t tr_id = (byte_t)self->unicast_transfer_id[destination_node_id]++;
935-
const uint32_t can_id = (((uint32_t)priority) << PRIO_SHIFT) |
936-
(((uint32_t)CANARD_SERVICE_ID_UNICAST) << 14U) | //
937-
(1UL << 23U) | // request
938-
(((uint32_t)destination_node_id) << 7U);
939-
canard_txfer_t* const tr = txfer_new(self, deadline, tr_id, can_id, self->tx.fd, context);
940-
ok = (tr != NULL) && tx_push(self, tr, false, CANARD_IFACE_BITMAP_ALL, payload, CRC_INITIAL);
945+
const byte_t tr_id = (byte_t)self->unicast_transfer_id[destination_node_id]++;
946+
// Per Cyphal v1.1, unicast is a service request with fixed service-ID 511.
947+
ok = tx_1v0_service(self,
948+
deadline,
949+
priority,
950+
(uint16_t)CANARD_SERVICE_ID_UNICAST,
951+
destination_node_id,
952+
true,
953+
tr_id,
954+
payload,
955+
context);
941956
}
942957
return ok;
943958
}
@@ -963,6 +978,53 @@ bool canard_1v0_publish(canard_t* const self,
963978
return ok;
964979
}
965980

981+
static bool tx_1v0_service(canard_t* const self,
982+
const canard_us_t deadline,
983+
const canard_prio_t priority,
984+
const uint16_t service_id,
985+
const uint_least8_t destination_node_id,
986+
const bool request_not_response,
987+
const uint_least8_t transfer_id,
988+
const canard_bytes_chain_t payload,
989+
const canard_user_context_t context)
990+
{
991+
bool ok = (self != NULL) && (priority < CANARD_PRIO_COUNT) && bytes_chain_valid(payload) &&
992+
(service_id <= CANARD_SERVICE_ID_MAX) && (destination_node_id <= CANARD_NODE_ID_MAX);
993+
if (ok) {
994+
const uint32_t can_id = (((uint32_t)priority) << PRIO_SHIFT) | //
995+
(1UL << 25U) | // service
996+
(request_not_response ? (1UL << 24U) : 0U) | (((uint32_t)service_id) << 14U) |
997+
(((uint32_t)destination_node_id) << 7U);
998+
canard_txfer_t* const tr = txfer_new(self, deadline, transfer_id, can_id, self->tx.fd, context);
999+
ok = (tr != NULL) && tx_push(self, tr, false, CANARD_IFACE_BITMAP_ALL, payload, CRC_INITIAL);
1000+
}
1001+
return ok;
1002+
}
1003+
1004+
bool canard_1v0_request(canard_t* const self,
1005+
const canard_us_t deadline,
1006+
const canard_prio_t priority,
1007+
const uint16_t service_id,
1008+
const uint_least8_t server_node_id,
1009+
const uint_least8_t transfer_id,
1010+
const canard_bytes_chain_t payload,
1011+
const canard_user_context_t context)
1012+
{
1013+
return tx_1v0_service(self, deadline, priority, service_id, server_node_id, true, transfer_id, payload, context);
1014+
}
1015+
1016+
bool canard_1v0_respond(canard_t* const self,
1017+
const canard_us_t deadline,
1018+
const canard_prio_t priority,
1019+
const uint16_t service_id,
1020+
const uint_least8_t client_node_id,
1021+
const uint_least8_t transfer_id,
1022+
const canard_bytes_chain_t payload,
1023+
const canard_user_context_t context)
1024+
{
1025+
return tx_1v0_service(self, deadline, priority, service_id, client_node_id, false, transfer_id, payload, context);
1026+
}
1027+
9661028
bool canard_0v1_publish(canard_t* const self,
9671029
const canard_us_t deadline,
9681030
const uint_least8_t iface_bitmap,
@@ -985,6 +1047,57 @@ bool canard_0v1_publish(canard_t* const self,
9851047
return ok;
9861048
}
9871049

1050+
static bool tx_0v1_service(canard_t* const self,
1051+
const canard_us_t deadline,
1052+
const canard_prio_t priority,
1053+
const uint_least8_t data_type_id,
1054+
const uint16_t crc_seed,
1055+
const uint_least8_t destination_node_id,
1056+
const bool request_not_response,
1057+
const uint_least8_t transfer_id,
1058+
const canard_bytes_chain_t payload,
1059+
const canard_user_context_t context)
1060+
{
1061+
bool ok = (self != NULL) && (priority < CANARD_PRIO_COUNT) && bytes_chain_valid(payload) && (self->node_id != 0U) &&
1062+
(destination_node_id > 0U) && (destination_node_id <= CANARD_NODE_ID_MAX);
1063+
if (ok) {
1064+
const uint32_t can_id = (((((uint32_t)priority) << 2U) | 3UL) << 24U) | //
1065+
(((uint32_t)data_type_id) << 16U) | (request_not_response ? (1UL << 15U) : 0U) |
1066+
(((uint32_t)destination_node_id) << 8U) | (1UL << 7U); // service
1067+
canard_txfer_t* const tr = txfer_new(self, deadline, transfer_id, can_id, false, context);
1068+
ok = (tr != NULL) && tx_push(self, tr, true, CANARD_IFACE_BITMAP_ALL, payload, crc_seed);
1069+
}
1070+
return ok;
1071+
}
1072+
1073+
bool canard_0v1_request(canard_t* const self,
1074+
const canard_us_t deadline,
1075+
const canard_prio_t priority,
1076+
const uint_least8_t data_type_id,
1077+
const uint16_t crc_seed,
1078+
const uint_least8_t server_node_id,
1079+
const uint_least8_t transfer_id,
1080+
const canard_bytes_chain_t payload,
1081+
const canard_user_context_t context)
1082+
{
1083+
return tx_0v1_service(
1084+
self, deadline, priority, data_type_id, crc_seed, server_node_id, true, transfer_id, payload, context);
1085+
}
1086+
1087+
bool canard_0v1_respond(canard_t* const self,
1088+
const canard_us_t deadline,
1089+
const canard_prio_t priority,
1090+
const uint_least8_t data_type_id,
1091+
const uint16_t crc_seed,
1092+
const uint_least8_t client_node_id,
1093+
const uint_least8_t transfer_id,
1094+
const canard_bytes_chain_t payload,
1095+
const canard_user_context_t context)
1096+
{
1097+
return tx_0v1_service(
1098+
self, deadline, priority, data_type_id, crc_seed, client_node_id, false, transfer_id, payload, context);
1099+
}
1100+
9881101
uint16_t canard_0v1_crc_seed_from_data_type_signature(const uint64_t data_type_signature)
9891102
{
9901103
uint16_t crc = CRC_INITIAL;

‎tests/src/test_api_tx.cpp‎

Lines changed: 76 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -113,16 +113,21 @@ static canard_mem_set_t make_std_memory()
113113
};
114114
}
115115

116-
static void init_with_capture(canard_t* const self, tx_capture_t* const capture)
116+
static void init_with_capture_node_id(canard_t* const self, tx_capture_t* const capture, const uint_least8_t node_id)
117117
{
118118
*capture = tx_capture_t{};
119119
capture->now = 0;
120120
capture->accept_tx = true;
121121
capture->count = 0;
122-
TEST_ASSERT_TRUE(canard_new(self, &kCaptureVtable, make_std_memory(), 16U, 42U, 1234U, 0U, nullptr));
122+
TEST_ASSERT_TRUE(canard_new(self, &kCaptureVtable, make_std_memory(), 16U, node_id, 1234U, 0U, nullptr));
123123
self->user_context = capture;
124124
}
125125

126+
static void init_with_capture(canard_t* const self, tx_capture_t* const capture)
127+
{
128+
init_with_capture_node_id(self, capture, 42U);
129+
}
130+
126131
// Basic constructor argument validation.
127132
static void test_canard_new_validation()
128133
{
@@ -402,8 +407,10 @@ static void test_canard_unicast_encoding_and_transfer_id()
402407
TEST_ASSERT_EQUAL_UINT64(1000U, (uint64_t)cap.records[i].deadline);
403408
TEST_ASSERT_TRUE(cap.records[i].fd);
404409
TEST_ASSERT_EQUAL_UINT8((uint8_t)canard_prio_high, (uint8_t)((can_id >> 26U) & 7U));
410+
TEST_ASSERT_EQUAL_UINT8(1U, (uint8_t)((can_id >> 25U) & 1U));
411+
TEST_ASSERT_EQUAL_UINT8(1U, (uint8_t)((can_id >> 24U) & 1U));
412+
TEST_ASSERT_EQUAL_UINT8(0U, (uint8_t)((can_id >> 23U) & 1U));
405413
TEST_ASSERT_EQUAL_UINT32(CANARD_SERVICE_ID_UNICAST, (can_id >> 14U) & CANARD_SERVICE_ID_MAX);
406-
TEST_ASSERT_EQUAL_UINT8(1U, (uint8_t)((can_id >> 23U) & 1U));
407414
TEST_ASSERT_EQUAL_UINT8(expected_dest[i], (uint8_t)((can_id >> 7U) & CANARD_NODE_ID_MAX));
408415
TEST_ASSERT_EQUAL_UINT8(42U, (uint8_t)(can_id & CANARD_NODE_ID_MAX));
409416
TEST_ASSERT_EQUAL_UINT8(expected_tid[i], (uint8_t)(cap.records[i].tail & CANARD_TRANSFER_ID_MAX));
@@ -412,6 +419,70 @@ static void test_canard_unicast_encoding_and_transfer_id()
412419
canard_destroy(&self);
413420
}
414421

422+
// Validate Cyphal v1.0 service CAN-ID encoding against specification examples.
423+
static void test_canard_1v0_service_can_id_golden()
424+
{
425+
const canard_bytes_chain_t payload = { .bytes = { .size = 0U, .data = nullptr }, .next = nullptr };
426+
427+
canard_t req_self = {};
428+
tx_capture_t req_cap = {};
429+
init_with_capture_node_id(&req_self, &req_cap, 123U);
430+
TEST_ASSERT_TRUE(
431+
canard_1v0_request(&req_self, 1000, canard_prio_nominal, 430U, 42U, 1U, payload, CANARD_USER_CONTEXT_NULL));
432+
canard_poll(&req_self, 1U);
433+
TEST_ASSERT_EQUAL_size_t(1U, req_cap.count);
434+
TEST_ASSERT_EQUAL_UINT32(0x136B957BU, req_cap.records[0].can_id);
435+
TEST_ASSERT_TRUE(req_cap.records[0].fd);
436+
TEST_ASSERT_EQUAL_UINT8(0xE1U, req_cap.records[0].tail);
437+
canard_destroy(&req_self);
438+
439+
canard_t res_self = {};
440+
tx_capture_t res_cap = {};
441+
init_with_capture_node_id(&res_self, &res_cap, 42U);
442+
TEST_ASSERT_TRUE(
443+
canard_1v0_respond(&res_self, 1000, canard_prio_nominal, 430U, 123U, 1U, payload, CANARD_USER_CONTEXT_NULL));
444+
canard_poll(&res_self, 1U);
445+
TEST_ASSERT_EQUAL_size_t(1U, res_cap.count);
446+
TEST_ASSERT_EQUAL_UINT32(0x126BBDAAU, res_cap.records[0].can_id);
447+
TEST_ASSERT_TRUE(res_cap.records[0].fd);
448+
TEST_ASSERT_EQUAL_UINT8(0xE1U, res_cap.records[0].tail);
449+
canard_destroy(&res_self);
450+
}
451+
452+
// Validate legacy v0 service rules and CAN-ID encoding.
453+
static void test_canard_0v1_service_node_id_rule_and_encoding()
454+
{
455+
const canard_bytes_chain_t payload = { .bytes = { .size = 0U, .data = nullptr }, .next = nullptr };
456+
457+
canard_t self_zero = {};
458+
TEST_ASSERT_FALSE(canard_0v1_request(
459+
&self_zero, 0, canard_prio_nominal, 0x37U, 0xBEEFU, 24U, 5U, payload, CANARD_USER_CONTEXT_NULL));
460+
TEST_ASSERT_FALSE(canard_0v1_respond(
461+
&self_zero, 0, canard_prio_nominal, 0x37U, 0xBEEFU, 24U, 6U, payload, CANARD_USER_CONTEXT_NULL));
462+
463+
canard_t self = {};
464+
tx_capture_t cap = {};
465+
init_with_capture_node_id(&self, &cap, 11U);
466+
467+
TEST_ASSERT_TRUE(
468+
canard_0v1_request(&self, 1000, canard_prio_nominal, 0x37U, 0xBEEFU, 24U, 5U, payload, CANARD_USER_CONTEXT_NULL));
469+
canard_poll(&self, 1U);
470+
471+
TEST_ASSERT_TRUE(
472+
canard_0v1_respond(&self, 1000, canard_prio_nominal, 0x37U, 0xBEEFU, 24U, 6U, payload, CANARD_USER_CONTEXT_NULL));
473+
canard_poll(&self, 1U);
474+
475+
TEST_ASSERT_EQUAL_size_t(2U, cap.count);
476+
TEST_ASSERT_EQUAL_UINT32(0x1337988BU, cap.records[0].can_id);
477+
TEST_ASSERT_EQUAL_UINT32(0x1337188BU, cap.records[1].can_id);
478+
TEST_ASSERT_FALSE(cap.records[0].fd);
479+
TEST_ASSERT_FALSE(cap.records[1].fd);
480+
TEST_ASSERT_EQUAL_UINT8(0xC5U, cap.records[0].tail);
481+
TEST_ASSERT_EQUAL_UINT8(0xC6U, cap.records[1].tail);
482+
483+
canard_destroy(&self);
484+
}
485+
415486
extern "C" void setUp() {}
416487
extern "C" void tearDown() {}
417488

@@ -435,6 +506,8 @@ int main()
435506
RUN_TEST(test_canard_poll_expiration);
436507
RUN_TEST(test_canard_unicast_validation);
437508
RUN_TEST(test_canard_unicast_encoding_and_transfer_id);
509+
RUN_TEST(test_canard_1v0_service_can_id_golden);
510+
RUN_TEST(test_canard_0v1_service_node_id_rule_and_encoding);
438511

439512
return UNITY_END();
440513
}

0 commit comments

Comments
 (0)