44/// Author: Pavel Kirienko <[email protected] > 55/// Copyright 2022 Amazon.com, Inc. or its affiliates. All Rights Reserved.
66
7- /// @todo Replace with subrepo of official libudpard
8-
97#include "udpard.h"
108#include "cavl.h"
119#include <string.h>
4745static const uint8_t BITS_PER_BYTE = 8U ;
4846static const uint8_t BYTE_MAX = 0xFFU ;
4947
50- /// TODO - determine the minimum payload size for udp non-last frame. It should be similar if not the same as CAN
51- /// #define MFT_NON_LAST_FRAME_PAYLOAD_MIN 7U /// The minimum payload size for the non-last frame in a multi-frame
52- /// transfer
53-
5448static const uint8_t UDPARD_END_OF_TRANSFER_OFFSET = 31U ;
55- static const uint32_t UDPARD_MAX_FRAME_INDEX = 0x7FFFFFFF ; // ((1U << UDPARD_END_OF_TRANSFER_OFFSET) - 1U);
49+ static const uint32_t UDPARD_MAX_FRAME_INDEX = 0x7FFFFFFFU ; // ((1U << UDPARD_END_OF_TRANSFER_OFFSET) - 1U);
5650static const uint16_t UDPARD_NODE_ID_MASK = 65535U ; /// 0xFFFF
5751
5852/*
@@ -73,17 +67,12 @@ static const uint16_t UDPARD_NODE_ID_MASK = 65535U; /// 0xFFFF
7367*/
7468
7569/// The multicast message transfer IP address node ID is formed of 1 reserved 0 bits and 15 bits for a subject id.
76- static const uint16_t UDPARD_SUBJECT_ID_MASK = 32767U ; /// 0x7FFF
77- // static const uint32_t UDPARD_SUBNET_OFFSET = 17U;
78- static const uint32_t UDPARD_SUBNET_MASK = 0x3E0000 ; // (31U << UDPARD_SUBNET_OFFSET);
70+ static const uint16_t UDPARD_SUBJECT_ID_MASK = 0x7FFFU ;
71+ static const uint32_t UDPARD_SUBNET_MASK = 0x3E0000U ; // (31U << UDPARD_SUBNET_OFFSET (17));
7972static const uint8_t UDPARD_TRANSMIT_SUBNET_VALUE = 0U ;
80- // static const uint8_t UDPARD_RESERVED_1BIT_OFFSET = 15U;
81- static const uint16_t UDPARD_RESERVED_1BIT_MASK = 0x8000 ; // (1U << UDPARD_RESERVED_1BIT_OFFSET);
82- // static const uint8_t UDPARD_SERVICE_NOT_MESSAGE_OFFSET = 16U;
83- static const uint32_t UDPARD_SERVICE_NOT_MESSAGE_MASK = 0x10000 ; // (1U << UDPARD_SERVICE_NOT_MESSAGE_OFFSET);
84- // static const uint8_t UDPARD_MULTICAST_OFFSET = 23U;
85- static const uint32_t UDPARD_MULTICAST_PREFIX = 0xEF000000 ; // (478U << UDPARD_MULTICAST_OFFSET)
86- // static const uint32_t UDPARD_MULTICAST_ADDRESS_MASK = 0x7fffff; // ((1U << UDPARD_MULTICAST_OFFSET) - 1U)
73+ static const uint16_t UDPARD_RESERVED_1BIT_MASK = 0x8000U ; // (1U << UDPARD_RESERVED_1BIT_OFFSET (15));
74+ static const uint32_t UDPARD_SERVICE_NOT_MESSAGE_MASK = 0x10000U ; // (1U << UDPARD_SERVICE_NOT_MESSAGE_OFFSET (16));
75+ static const uint32_t UDPARD_MULTICAST_PREFIX = 0xEF000000U ; // (478U << UDPARD_MULTICAST_OFFSET (23))
8776
8877/* The 16 bit data specifier in the Cyphal header consists of
8978SNM + 15 bit Subject-ID (Message)
@@ -94,10 +83,10 @@ IRNR - Is Request, Not Response
9483*/
9584static const uint16_t UDPARD_SERVICE_NOT_MESSAGE_DATA_SPECIFIER_OFFSET = 15U ;
9685static const uint16_t UDPARD_IRNR_DATA_SPECIFIER_OFFSET = 14U ;
97- static const uint16_t UDPARD_SERVICE_ID_MASK = 16383U ; // 0x3FFF
98- static const uint16_t UPDARD_DATA_SPECIFIER_MESSAGE = 0x7FFFU ; // SNM (0) + SubjectID
99- static const uint16_t UDPARD_DATA_SPECIFIER_SERVICE_RESPONSE = 0x8000 ; // (2U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) // Set SNM in Cyphal data specifier - SNM (1) + IRNR (0) + ServiceID
100- static const uint16_t UDPARD_DATA_SPECIFIER_SERVICE_REQUEST = 0xC000 ; // (3U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) // Set SNM and IRNR in Cyphal data specifier - SNM (1) + IRNR (1) + ServiceID
86+ static const uint16_t UDPARD_SERVICE_ID_MASK = 0x3FFFU ;
87+ static const uint16_t UDPARD_DATA_SPECIFIER_MESSAGE_MASK = 0x7FFFU ; // SNM (0) + SubjectID
88+ static const uint16_t UDPARD_DATA_SPECIFIER_SERVICE_RESPONSE = 0x8000U ; // (2U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) // Set SNM in Cyphal data specifier - SNM (1) + IRNR (0) + ServiceID
89+ static const uint16_t UDPARD_DATA_SPECIFIER_SERVICE_REQUEST = 0xC000U ; // (3U << UDPARD_IRNR_DATA_SPECIFIER_OFFSET) // Set SNM and IRNR in Cyphal data specifier - SNM (1) + IRNR (1) + ServiceID
10190
10291static const uint16_t UDPARD_UDP_PORT = 9382U ;
10392
@@ -293,8 +282,13 @@ UDPARD_PRIVATE int32_t txMakeSessionSpecifier(const UdpardTransferMetadata* cons
293282 if ((tr -> transfer_kind == UdpardTransferKindMessage ) && (UDPARD_NODE_ID_UNSET == tr -> remote_node_id ) &&
294283 (tr -> port_id <= UDPARD_SUBJECT_ID_MAX ))
295284 {
296- out = txMakeMessageSessionSpecifier (tr -> port_id , local_node_id , local_node_addr , spec );
297- UDPARD_ASSERT (out >= 0 );
285+ if ((local_node_id == UDPARD_NODE_ID_UNSET ) || (local_node_id <= UDPARD_NODE_ID_MAX )) {
286+
287+ out = txMakeMessageSessionSpecifier (tr -> port_id , local_node_id , local_node_addr , spec );
288+ UDPARD_ASSERT (out >= 0 );
289+ } else {
290+ out = - UDPARD_ERROR_INVALID_ARGUMENT ; // Node must be at least 1 less than the unset id
291+ }
298292 }
299293 else if (((tr -> transfer_kind == UdpardTransferKindRequest ) || (tr -> transfer_kind == UdpardTransferKindResponse )) &&
300294 (tr -> port_id < UDPARD_SERVICE_ID_MAX ) && (tr -> remote_node_id != UDPARD_NODE_ID_UNSET ))
@@ -350,7 +344,8 @@ UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
350344 header -> cyphal_header_checksum = cyphalHeaderCrcAdd (CYPHAL_HEADER_CRC_INITIAL , cyphal_header_size_without_crc , header );
351345 if (transfer_kind == UdpardTransferKindMessage )
352346 {
353- header -> data_specifier = (uint16_t )(port_id & UDPARD_SUBJECT_ID_MASK ) & UPDARD_DATA_SPECIFIER_MESSAGE ; // SNM (0) + Subject ID
347+ // both port_id and the data_specifier start at bit-0. No shift of the port_id value is necessary.
348+ header -> data_specifier = port_id & UDPARD_DATA_SPECIFIER_MESSAGE_MASK ;
354349 }
355350 else
356351 {
@@ -1030,7 +1025,7 @@ UDPARD_PRIVATE int8_t rxAcceptFrame(UdpardInstance* const ins,
10301025 UDPARD_ASSERT (out_transfer != NULL );
10311026
10321027 int8_t out = 0 ;
1033- if (frame -> source_node_id != UDPARD_NODE_ID_UNSET )
1028+ if (( frame -> source_node_id <= UDPARD_NODE_ID_MAX ) && ( frame -> source_node_id != UDPARD_NODE_ID_UNSET ) )
10341029 {
10351030 // If such session does not exist, create it. This only makes sense if this is the first frame of a
10361031 // transfer, otherwise, we won't be able to receive the transfer anyway so we don't bother.
0 commit comments