Skip to content

Commit 067d687

Browse files
SaqibAZSaqibAZ
committed
Update UDP and Cyphal Header
1 parent c888d48 commit 067d687

File tree

5 files changed

+258
-148
lines changed

5 files changed

+258
-148
lines changed

‎libudpard/udpard.c‎

Lines changed: 94 additions & 40 deletions
Original file line numberDiff line numberDiff line change
@@ -54,34 +54,48 @@
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+
8599
/// Ports align with subject and service ids
86100
/// Subjects use multicast and always use port 16383
87101
/// Services use unicast and start with port 16384
@@ -91,6 +105,8 @@
91105
#define UDPARD_SERVICE_ID_INITIAL_PORT 16384U
92106
#define UDPARD_SERVICE_ID_RESPONSE_MASK 1U
93107

108+
#define UDPARD_UDP_PORT 9382U
109+
94110
/// Used for inserting new items into AVL trees.
95111
UDPARD_PRIVATE UdpardTreeNode* avlTrivialFactory(void* const user_reference)
96112
{
@@ -188,12 +204,15 @@ UDPARD_PRIVATE int32_t txMakeMessageSessionSpecifier(const UdpardPortID
188204
UDPARD_ASSERT(subject_id <= UDPARD_SUBJECT_ID_MAX);
189205
/// Just the local ip address + source node id
190206
out_spec->source_route_specifier =
191-
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) | (UdpardIPv4Addr) src_node_id;
207+
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) |
208+
(UdpardIPv4Addr) src_node_id;
192209
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;
210+
((local_node_addr & (UdpardIPv4Addr) UDPARD_SUBNET_MASK) |
211+
(UdpardIPv4Addr) UDPARD_MULTICAST_PREFIX |
212+
((UdpardIPv4Addr) UDPARD_SUBJECT_ID_MASK & (UdpardIPv4Addr) subject_id)) &
213+
~(UdpardIPv4Addr) UDPARD_SERVICE_NOT_MESSAGE_MASK &
214+
~(UdpardIPv4Addr) UDPARD_RESERVED_1BIT_MASK;
215+
out_spec->data_specifier = (UdpardUdpPortID) UDPARD_UDP_PORT;
197216
return UDPARD_SUCCESS;
198217
}
199218

@@ -208,11 +227,14 @@ UDPARD_PRIVATE int32_t txMakeServiceSessionSpecifier(const UdpardPortID
208227
UDPARD_ASSERT(service_id < UDPARD_SERVICE_ID_MAX);
209228
/// Just the local ip address + source node id
210229
out_spec->source_route_specifier =
211-
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) | (UdpardIPv4Addr) src_node_id;
230+
(local_node_addr & ~(UdpardIPv4Addr) UDPARD_NODE_ID_MASK) |
231+
(UdpardIPv4Addr) src_node_id;
212232
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));
233+
((local_node_addr & (UdpardIPv4Addr) UDPARD_SUBNET_MASK) |
234+
(UdpardIPv4Addr) UDPARD_MULTICAST_PREFIX |
235+
((UdpardIPv4Addr) UDPARD_NODE_ID_MASK & (UdpardIPv4Addr) dst_node_id)) |
236+
(UdpardIPv4Addr) UDPARD_SERVICE_NOT_MESSAGE_MASK;
237+
out_spec->data_specifier = (UdpardUdpPortID) UDPARD_UDP_PORT;
216238
return UDPARD_SUCCESS;
217239
}
218240

@@ -295,6 +317,10 @@ UDPARD_PRIVATE size_t txRoundFramePayloadSizeUp(const size_t x)
295317
}
296318

297319
UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
320+
const UdpardNodeID src_node_id,
321+
const UdpardNodeID dst_node_id,
322+
const UdpardPortID port_id,
323+
const UdpardTransferKind transfer_kind,
298324
const UdpardPriority priority,
299325
const UdpardTransferID transfer_id,
300326
const bool end_of_transfer,
@@ -305,6 +331,18 @@ UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
305331
header->transfer_id = transfer_id;
306332
header->priority = (uint8_t) priority;
307333
header->frame_index_eot = end_of_transfer_mask | frame_index;
334+
header->source_node_id = src_node_id;
335+
header->destination_node_id = dst_node_id;
336+
if (transfer_kind == UdpardTransferKindMessage)
337+
{
338+
header->data_specifier = (uint16_t) (0xFFFF >> 1) & port_id; // SNM (0) + Subject ID
339+
}
340+
else
341+
{
342+
header->data_specifier =
343+
(transfer_kind == UdpardTransferKindRequest) ? (3U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) | port_id
344+
: (2U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) | port_id; // SNM (1) + IRNR + ServiceID
345+
}
308346
}
309347

310348
/// The item is only allocated and initialized, but NOT included into the queue! The caller needs to do that.
@@ -360,6 +398,10 @@ UDPARD_PRIVATE int32_t txPushSingleFrame(UdpardTxQueue* const que
360398
UdpardInstance* const ins,
361399
const UdpardMicrosecond deadline_usec,
362400
const UdpardSessionSpecifier* const specifier,
401+
const UdpardNodeID src_node_id,
402+
const UdpardNodeID dst_node_id,
403+
const UdpardPortID port_id,
404+
const UdpardTransferKind transfer_kind,
363405
const UdpardPriority priority,
364406
const UdpardTransferID transfer_id,
365407
const size_t payload_size,
@@ -389,7 +431,7 @@ UDPARD_PRIVATE int32_t txPushSingleFrame(UdpardTxQueue* const que
389431
// We ignore it because the safe functions are poorly supported; reliance on them may limit the portability.
390432
(void) memset(&tqi->payload_buffer[payload_size], PADDING_BYTE_VALUE, padding_size); // NOLINT
391433
/// Create the FrameHeader
392-
txMakeFrameHeader(&tqi->base.frame.udp_cyphal_header, priority, transfer_id, true, 1);
434+
txMakeFrameHeader(&tqi->base.frame.udp_cyphal_header, src_node_id, dst_node_id, port_id, transfer_kind, priority, transfer_id, true, 1);
393435
// Clang-Tidy raises an error recommending the use of memcpy_s() instead.
394436
// We ignore it because the safe functions are poorly supported; reliance on them may limit the portability.
395437
(void) memcpy(&tqi->payload_buffer[0],
@@ -483,14 +525,22 @@ UDPARD_PRIVATE UdpardPortID getPortIdFromRouteAndDataSpecifiers(UdpardIPv4Addr
483525
: (UdpardPortID) ((data_specifier - UDPARD_SERVICE_ID_INITIAL_PORT) / 2);
484526
}
485527

528+
UDPARD_PRIVATE UdpardPortID getPortIdFromDataSpecifiers(UdpardUdpPortID data_specifier)
529+
{
530+
if ((data_specifier >> (UDPARD_SERVICE_NOT_MESSAGE_DATA_SPECIFIER_OFFSET)) & 1U)
531+
{
532+
return (UdpardPortID) (data_specifier & UDPARD_SERVICE_ID_MASK);
533+
}
534+
return (UdpardPortID) (data_specifier & UDPARD_SUBJECT_ID_MASK);
535+
}
536+
486537
UDPARD_PRIVATE UdpardTransferKind getTransferKindFromDataSpecifier(UdpardUdpPortID data_specifier)
487538
{
488-
UDPARD_ASSERT(data_specifier >= UDPARD_SUBJECT_ID_PORT);
489-
if (data_specifier == UDPARD_SUBJECT_ID_PORT)
539+
if ((data_specifier >> (UDPARD_SERVICE_NOT_MESSAGE_DATA_SPECIFIER_OFFSET)) & 1U)
490540
{
491-
return UdpardTransferKindMessage;
541+
return ((data_specifier >> UDPARD_IRNR_DATA_SPECIFIER_OFFSET) & 1U) ? UdpardTransferKindRequest : UdpardTransferKindResponse;
492542
}
493-
return (data_specifier % 2 == 1) ? UdpardTransferKindResponse : UdpardTransferKindRequest;
543+
return UdpardTransferKindMessage;
494544
}
495545

496546
/// Returns truth if the frame is valid and parsed successfully. False if the frame is not a valid Cyphal/UDP frame.
@@ -499,6 +549,7 @@ UDPARD_PRIVATE bool rxTryParseFrame(const UdpardMicrosecond timestam
499549
UdpardFrame* const frame,
500550
RxFrameModel* const out)
501551
{
552+
502553
UDPARD_ASSERT(frame != NULL);
503554
UDPARD_ASSERT(out != NULL);
504555
UDPARD_ASSERT(specifier != NULL);
@@ -507,18 +558,17 @@ UDPARD_PRIVATE bool rxTryParseFrame(const UdpardMicrosecond timestam
507558
return false;
508559
}
509560
bool valid = true;
561+
510562
// Get the Header out of the frame
511563
UDPARD_ASSERT(frame->payload != NULL);
512564
(void) memcpy(&frame->udp_cyphal_header, frame->payload, sizeof(frame->udp_cyphal_header)); // NOLINT
513565
out->timestamp_usec = timestamp_usec;
514566

515567
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);
568+
out -> source_node_id = frame->udp_cyphal_header.source_node_id;
569+
out->transfer_kind = getTransferKindFromDataSpecifier(frame->udp_cyphal_header.data_specifier);
570+
out ->port_id = getPortIdFromDataSpecifiers(frame->udp_cyphal_header.data_specifier);
571+
out ->destination_node_id = frame->udp_cyphal_header.destination_node_id;
522572
// Payload parsing.
523573
out->payload_size = frame->payload_size - sizeof(frame->udp_cyphal_header); // Cut off the header size.
524574
out->payload = (void*) ((uint8_t*) frame->payload + sizeof(frame->udp_cyphal_header));
@@ -890,6 +940,10 @@ int32_t udpardTxPush(UdpardTxQueue* const que,
890940
ins,
891941
tx_deadline_usec,
892942
&specifier,
943+
ins->node_id,
944+
metadata->remote_node_id,
945+
metadata->port_id,
946+
metadata->transfer_kind,
893947
metadata->priority,
894948
metadata->transfer_id,
895949
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)