Skip to content

Commit 8e6404b

Browse files
keningatamazonkeningatamazon
authored
Merge branch 'OpenCyphal-Garage:main' into main
2 parents a2da0e0 + bb9052f commit 8e6404b

File tree

5 files changed

+261
-148
lines changed

5 files changed

+261
-148
lines changed

‎libudpard/udpard.c‎

Lines changed: 97 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -54,34 +54,51 @@
5454
#define UDPARD_END_OF_TRANSFER_OFFSET 31U
5555
#define UDPARD_MAX_FRAME_INDEX ((1U << UDPARD_END_OF_TRANSFER_OFFSET) - 1U)
5656

57-
#define UDPARD_NODE_ID_MASK 255U /// 8 bits for now
57+
#define UDPARD_NODE_ID_MASK 65535U /// 0xFFFF
5858

5959
/*
60-
fixed reserved
61-
(9 bits) (3 bits)
62-
________ _
63-
/ \ / \
64-
11101111.0ddddddd.000sssss.ssssssss
65-
\__/ \_____/ \____________/
66-
(4 bits) (7 bits) (13 bits)
67-
IPv4 subnet-ID subject-ID
68-
multicast \_______________________/
69-
prefix (23 bits)
70-
collision-free multicast
71-
addressing limit of
72-
Ethernet MAC for IPv4
60+
fixed Cyphal/UDP
61+
(9 bits) Ipv4 Addr
62+
Version Destination Node ID
63+
________________(1 bit) SNM _______________________________
64+
/ \ | | / \
65+
1 1 1 0 1 1 1 1 . 0 c d d d d d m . 0 s s s s s s s . s s s s s s s s
66+
\_____/ \______/ \_______/ | \_____________________________/
67+
(4 bits) (4 bits) Subnet | (15 bits) subject-ID
68+
IPv4 Scope Reserved Reserved
69+
multicast \_______________________________________________/
70+
prefix (23 bits)
71+
collision-free multicast
72+
addressing limit of
73+
Ethernet MAC for IPv4
7374
*/
7475

75-
/// The multicast message transfer IP address node ID is formed of 3 reserved 0 bits and 13 bits for a subject id.
76-
#define UDPARD_SUBJECT_ID_MASK 8191U /// 0x1FFF
77-
#define UDPARD_SUBNET_OFFSET 16U
78-
#define UDPARD_SUBNET_MASK (127U << UDPARD_SUBNET_OFFSET)
79-
#define UDPARD_RESERVED_3BITS_OFFSET 13U
80-
#define UDPARD_RESERVED_3BITS_MASK (7U << UDPARD_RESERVED_3BITS_OFFSET)
76+
/// The multicast message transfer IP address node ID is formed of 1 reserved 0 bits and 15 bits for a subject id.
77+
#define UDPARD_SUBJECT_ID_MASK 32767U /// 0x7FFF
78+
#define UDPARD_SUBNET_OFFSET 17U
79+
#define UDPARD_SUBNET_MASK (31U << UDPARD_SUBNET_OFFSET)
80+
#define UDPARD_RESERVED_1BIT_OFFSET 15U
81+
#define UDPARD_RESERVED_1BIT_MASK (1U << UDPARD_RESERVED_1BIT_OFFSET)
82+
#define UDPARD_SERVICE_NOT_MESSAGE_OFFSET 16U
83+
#define UDPARD_SERVICE_NOT_MESSAGE_MASK (1U << UDPARD_SERVICE_NOT_MESSAGE_OFFSET)
8184
#define UDPARD_MULTICAST_OFFSET 23U
8285
#define UDPARD_MULTICAST_PREFIX (478U << UDPARD_MULTICAST_OFFSET)
8386
#define UDPARD_MULTICAST_ADDRESS_MASK ((1U << UDPARD_MULTICAST_OFFSET) - 1U)
8487

88+
/* The 16 bit data specifier in the Cyphal header consists of
89+
SNM + 15 bit Subject-ID (Message)
90+
SNM + IRNR + Service-ID (Service Request/Response)
91+
92+
SNM - Service, Not Message
93+
IRNR - Is Request, Not Response
94+
*/
95+
#define UDPARD_SERVICE_NOT_MESSAGE_DATA_SPECIFIER_OFFSET 15U
96+
#define UDPARD_IRNR_DATA_SPECIFIER_OFFSET 14U
97+
#define UDPARD_SERVICE_ID_MASK 16383U /// 0x3FFF
98+
#define UPDARD_DATA_SPECIFIER_MESSAGE (0xFFFF >> 1) // SNM (0) + SubjectID
99+
#define UDPARD_DATA_SPECIFIER_SERVICE_RESPONSE (2U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) // Set SNM in Cyphal data specifier - SNM (1) + IRNR (0) + ServiceID
100+
#define UDPARD_DATA_SPECIFIER_SERVICE_REQUEST (3U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) // Set SNM and IRNR in Cyphal data specifier - SNM (1) + IRNR (1) + ServiceID
101+
85102
/// Ports align with subject and service ids
86103
/// Subjects use multicast and always use port 16383
87104
/// Services use unicast and start with port 16384
@@ -91,6 +108,8 @@
91108
#define UDPARD_SERVICE_ID_INITIAL_PORT 16384U
92109
#define UDPARD_SERVICE_ID_RESPONSE_MASK 1U
93110

111+
#define UDPARD_UDP_PORT 9382U
112+
94113
/// Used for inserting new items into AVL trees.
95114
UDPARD_PRIVATE UdpardTreeNode* avlTrivialFactory(void* const user_reference)
96115
{
@@ -188,12 +207,15 @@ UDPARD_PRIVATE int32_t txMakeMessageSessionSpecifier(const UdpardPortID
188207
UDPARD_ASSERT(subject_id <= UDPARD_SUBJECT_ID_MAX);
189208
/// Just the local ip address + source node id
190209
out_spec->source_route_specifier =
191-
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) | (UdpardIPv4Addr) src_node_id;
210+
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) |
211+
(UdpardIPv4Addr) src_node_id;
192212
out_spec->destination_route_specifier =
193-
((local_node_addr & (UdpardIPv4Addr) UDPARD_SUBNET_MASK) | (UdpardIPv4Addr) UDPARD_MULTICAST_PREFIX |
194-
((UdpardIPv4Addr) UDPARD_SUBJECT_ID_MASK & (UdpardIPv4Addr) subject_id)) &
195-
~(UdpardIPv4Addr) UDPARD_RESERVED_3BITS_MASK;
196-
out_spec->data_specifier = (UdpardUdpPortID) UDPARD_SUBJECT_ID_PORT;
213+
((local_node_addr & (UdpardIPv4Addr) UDPARD_SUBNET_MASK) |
214+
(UdpardIPv4Addr) UDPARD_MULTICAST_PREFIX |
215+
((UdpardIPv4Addr) UDPARD_SUBJECT_ID_MASK & (UdpardIPv4Addr) subject_id)) &
216+
~(UdpardIPv4Addr) UDPARD_SERVICE_NOT_MESSAGE_MASK &
217+
~(UdpardIPv4Addr) UDPARD_RESERVED_1BIT_MASK;
218+
out_spec->data_specifier = (UdpardUdpPortID) UDPARD_UDP_PORT;
197219
return UDPARD_SUCCESS;
198220
}
199221

@@ -208,11 +230,14 @@ UDPARD_PRIVATE int32_t txMakeServiceSessionSpecifier(const UdpardPortID
208230
UDPARD_ASSERT(service_id < UDPARD_SERVICE_ID_MAX);
209231
/// Just the local ip address + source node id
210232
out_spec->source_route_specifier =
211-
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) | (UdpardIPv4Addr) src_node_id;
233+
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) |
234+
(UdpardIPv4Addr) src_node_id;
212235
out_spec->destination_route_specifier =
213-
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) | (UdpardIPv4Addr) dst_node_id;
214-
out_spec->data_specifier = (UdpardUdpPortID) (UDPARD_SERVICE_ID_INITIAL_PORT + (UdpardUdpPortID) (service_id * 2U) +
215-
(request_not_response ? 0U : 1U));
236+
((local_node_addr & (UdpardIPv4Addr) UDPARD_SUBNET_MASK) |
237+
(UdpardIPv4Addr) UDPARD_MULTICAST_PREFIX |
238+
((UdpardIPv4Addr) UDPARD_NODE_ID_MASK & (UdpardIPv4Addr) dst_node_id)) |
239+
(UdpardIPv4Addr) UDPARD_SERVICE_NOT_MESSAGE_MASK;
240+
out_spec->data_specifier = (UdpardUdpPortID) UDPARD_UDP_PORT;
216241
return UDPARD_SUCCESS;
217242
}
218243

@@ -295,6 +320,10 @@ UDPARD_PRIVATE size_t txRoundFramePayloadSizeUp(const size_t x)
295320
}
296321

297322
UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
323+
const UdpardNodeID src_node_id,
324+
const UdpardNodeID dst_node_id,
325+
const UdpardPortID port_id,
326+
const UdpardTransferKind transfer_kind,
298327
const UdpardPriority priority,
299328
const UdpardTransferID transfer_id,
300329
const bool end_of_transfer,
@@ -305,6 +334,18 @@ UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
305334
header->transfer_id = transfer_id;
306335
header->priority = (uint8_t) priority;
307336
header->frame_index_eot = end_of_transfer_mask | frame_index;
337+
header->source_node_id = src_node_id;
338+
header->destination_node_id = dst_node_id;
339+
if (transfer_kind == UdpardTransferKindMessage)
340+
{
341+
header->data_specifier = (uint16_t) UPDARD_DATA_SPECIFIER_MESSAGE & port_id; // SNM (0) + Subject ID
342+
}
343+
else
344+
{
345+
header->data_specifier =
346+
(transfer_kind == UdpardTransferKindRequest) ? UDPARD_DATA_SPECIFIER_SERVICE_REQUEST | port_id
347+
: UDPARD_DATA_SPECIFIER_SERVICE_RESPONSE | port_id; // SNM (1) + IRNR + ServiceID
348+
}
308349
}
309350

310351
/// The item is only allocated and initialized, but NOT included into the queue! The caller needs to do that.
@@ -360,6 +401,10 @@ UDPARD_PRIVATE int32_t txPushSingleFrame(UdpardTxQueue* const que
360401
UdpardInstance* const ins,
361402
const UdpardMicrosecond deadline_usec,
362403
const UdpardSessionSpecifier* const specifier,
404+
const UdpardNodeID src_node_id,
405+
const UdpardNodeID dst_node_id,
406+
const UdpardPortID port_id,
407+
const UdpardTransferKind transfer_kind,
363408
const UdpardPriority priority,
364409
const UdpardTransferID transfer_id,
365410
const size_t payload_size,
@@ -389,7 +434,7 @@ UDPARD_PRIVATE int32_t txPushSingleFrame(UdpardTxQueue* const que
389434
// We ignore it because the safe functions are poorly supported; reliance on them may limit the portability.
390435
(void) memset(&tqi->payload_buffer[payload_size], PADDING_BYTE_VALUE, padding_size); // NOLINT
391436
/// Create the FrameHeader
392-
txMakeFrameHeader(&tqi->base.frame.udp_cyphal_header, priority, transfer_id, true, 1);
437+
txMakeFrameHeader(&tqi->base.frame.udp_cyphal_header, src_node_id, dst_node_id, port_id, transfer_kind, priority, transfer_id, true, 1);
393438
// Clang-Tidy raises an error recommending the use of memcpy_s() instead.
394439
// We ignore it because the safe functions are poorly supported; reliance on them may limit the portability.
395440
(void) memcpy(&tqi->payload_buffer[0],
@@ -483,14 +528,22 @@ UDPARD_PRIVATE UdpardPortID getPortIdFromRouteAndDataSpecifiers(UdpardIPv4Addr
483528
: (UdpardPortID) ((data_specifier - UDPARD_SERVICE_ID_INITIAL_PORT) / 2);
484529
}
485530

531+
UDPARD_PRIVATE UdpardPortID getPortIdFromDataSpecifiers(UdpardUdpPortID data_specifier)
532+
{
533+
if ((data_specifier >> (UDPARD_SERVICE_NOT_MESSAGE_DATA_SPECIFIER_OFFSET)) & 1U)
534+
{
535+
return (UdpardPortID) (data_specifier & UDPARD_SERVICE_ID_MASK);
536+
}
537+
return (UdpardPortID) (data_specifier & UDPARD_SUBJECT_ID_MASK);
538+
}
539+
486540
UDPARD_PRIVATE UdpardTransferKind getTransferKindFromDataSpecifier(UdpardUdpPortID data_specifier)
487541
{
488-
UDPARD_ASSERT(data_specifier >= UDPARD_SUBJECT_ID_PORT);
489-
if (data_specifier == UDPARD_SUBJECT_ID_PORT)
542+
if ((data_specifier >> (UDPARD_SERVICE_NOT_MESSAGE_DATA_SPECIFIER_OFFSET)) & 1U)
490543
{
491-
return UdpardTransferKindMessage;
544+
return ((data_specifier >> UDPARD_IRNR_DATA_SPECIFIER_OFFSET) & 1U) ? UdpardTransferKindRequest : UdpardTransferKindResponse;
492545
}
493-
return (data_specifier % 2 == 1) ? UdpardTransferKindResponse : UdpardTransferKindRequest;
546+
return UdpardTransferKindMessage;
494547
}
495548

496549
/// Returns truth if the frame is valid and parsed successfully. False if the frame is not a valid Cyphal/UDP frame.
@@ -499,6 +552,7 @@ UDPARD_PRIVATE bool rxTryParseFrame(const UdpardMicrosecond timestam
499552
UdpardFrame* const frame,
500553
RxFrameModel* const out)
501554
{
555+
502556
UDPARD_ASSERT(frame != NULL);
503557
UDPARD_ASSERT(out != NULL);
504558
UDPARD_ASSERT(specifier != NULL);
@@ -507,18 +561,17 @@ UDPARD_PRIVATE bool rxTryParseFrame(const UdpardMicrosecond timestam
507561
return false;
508562
}
509563
bool valid = true;
564+
510565
// Get the Header out of the frame
511566
UDPARD_ASSERT(frame->payload != NULL);
512567
(void) memcpy(&frame->udp_cyphal_header, frame->payload, sizeof(frame->udp_cyphal_header)); // NOLINT
513568
out->timestamp_usec = timestamp_usec;
514569

515570
out->priority = (UdpardPriority) frame->udp_cyphal_header.priority;
516-
out->source_node_id = getNodeIdFromRouteSpecifier(specifier->source_route_specifier);
517-
out->transfer_kind = getTransferKindFromDataSpecifier(specifier->data_specifier);
518-
out->port_id =
519-
getPortIdFromRouteAndDataSpecifiers(specifier->destination_route_specifier, specifier->data_specifier);
520-
out->destination_node_id =
521-
getNodeIdFromRouteAndDataSpecifiers(specifier->destination_route_specifier, specifier->data_specifier);
571+
out -> source_node_id = frame->udp_cyphal_header.source_node_id;
572+
out->transfer_kind = getTransferKindFromDataSpecifier(frame->udp_cyphal_header.data_specifier);
573+
out ->port_id = getPortIdFromDataSpecifiers(frame->udp_cyphal_header.data_specifier);
574+
out ->destination_node_id = frame->udp_cyphal_header.destination_node_id;
522575
// Payload parsing.
523576
out->payload_size = frame->payload_size - sizeof(frame->udp_cyphal_header); // Cut off the header size.
524577
out->payload = (void*) ((uint8_t*) frame->payload + sizeof(frame->udp_cyphal_header));
@@ -890,6 +943,10 @@ int32_t udpardTxPush(UdpardTxQueue* const que,
890943
ins,
891944
tx_deadline_usec,
892945
&specifier,
946+
ins->node_id,
947+
metadata->remote_node_id,
948+
metadata->port_id,
949+
metadata->transfer_kind,
893950
metadata->priority,
894951
metadata->transfer_id,
895952
payload_size,

‎libudpard/udpard.h‎

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -52,16 +52,15 @@ extern "C" {
5252
#define UDPARD_MTU_MAX UDPARD_MTU_UDP_IPV6 /// We may want to set this to 1400/1500 for Ethernet MTU
5353

5454
/// Parameter ranges are inclusive; the lower bound is zero for all. See Cyphal/UDP Specification for background.
55-
#define UDPARD_SUBJECT_ID_MAX 8191U /// 13 bits subject ID
56-
#define UDPARD_SERVICE_ID_MAX 65535U /// The hard limit for ports
57-
#define UDPARD_NODE_SUBNET_MAX 128U /// 7 bits for subnet
58-
// #define UDPARD_NODE_ID_MAX 65535U /// 16 bits is the hard limit. But this may change pending implementations
59-
#define UDPARD_NODE_ID_MAX 255U /// Setting to a lower value until we can determine a way to pre-allocate sessions
55+
#define UDPARD_SUBJECT_ID_MAX 32767U /// 15 bits subject ID
56+
#define UDPARD_SERVICE_ID_MAX 65535U /// The hard limit for ports
57+
#define UDPARD_NODE_SUBNET_MAX 31U /// 5 bits for subnet
58+
#define UDPARD_NODE_ID_MAX 65535U /// 16 bits is the hard limit. But this may change pending implementations
6059
#define UDPARD_PRIORITY_MAX 7U
6160
#define UDPARD_TRANSFER_ID_BIT_LENGTH 63ULL
6261
#define UDPARD_TRANSFER_ID_MAX ((1ULL << UDPARD_TRANSFER_ID_BIT_LENGTH) - 1ULL)
6362

64-
#define UDPARD_NODE_ID_UNSET 0U /// For UDP No ID is the anonymous ID
63+
#define UDPARD_NODE_ID_UNSET 65535U /// For UDP 0xFFFF is the anonymous ID
6564

6665
/// This is the recommended transfer-ID timeout value given in the Cyphal Specification. The application may choose
6766
/// different values per subscription (i.e., per data specifier) depending on its timing requirements.
@@ -114,10 +113,13 @@ typedef struct
114113
{
115114
uint8_t version;
116115
uint8_t priority;
117-
uint16_t _reserved_a;
118-
uint32_t frame_index_eot;
116+
uint16_t source_node_id;
117+
uint16_t destination_node_id;
118+
uint16_t data_specifier;
119119
uint64_t transfer_id;
120-
uint64_t _reserved_b;
120+
uint32_t frame_index_eot;
121+
uint16_t _opaque;
122+
uint16_t cyphal_header_checksum;
121123
} UdpardFrameHeader;
122124

123125
typedef struct

0 commit comments

Comments
 (0)