This is an automated email from the ASF dual-hosted git repository. xiaoxiang pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx.git
commit c61d7c7e8de4406cf98246438caea8973f4dc297 Author: Karel Kočí <[email protected]> AuthorDate: Tue Nov 4 14:38:48 2025 +0100 nuttx/can: add message alignment This adds ability for read and write operations to work with messages aligned to configured number of bytes. This has few different use cases. The alignment is specified as unsigned integer and can be changed with ioctl command CANIOC_SET_MSGALIGN. The current value can be queried by CANIOC_GET_MSGALIGN command. The default value for the message alignment is 1. This will provide behavior consistent with current one. Thus messages are placed to the buffer right after data of the previous one. The same applies for writes. The special alignment value 0 disables read and write of multiple frames. Thus read will always return at most one message and write will always write only one message even if larger buffer size is provided. Another use case is if message alignment is set to exactly message representation size (`sizeof(struct can_msg_s)`). This allows writing and reading arrays of messages. Other values provide even more advanced and specialized use cases, such as optimizations if architecture has to emulate some non-aligned accesses, there alignment of for example 4 bytes could provide performance boost. The original motivation behind this is was compatibility with socket CAN. It is easier to port applications to NuttX's CAN driver if only one frame is provided at the time. This solution was suggested by Pavel Pisa <[email protected]> as a more versatile variant of plain boolean disabling the multiple frame retrieval. Signed-off-by: Karel Kočí <[email protected]> --- Documentation/components/drivers/character/can.rst | 48 +++++++++++-- Documentation/guides/reading_can_msgs.rst | 84 ++++++++++++++-------- drivers/can/can.c | 59 +++++++++++++-- include/nuttx/can/can.h | 32 ++++++++- 4 files changed, 179 insertions(+), 44 deletions(-) diff --git a/Documentation/components/drivers/character/can.rst b/Documentation/components/drivers/character/can.rst index 86061c78b8e..d17643be2ac 100644 --- a/Documentation/components/drivers/character/can.rst +++ b/Documentation/components/drivers/character/can.rst @@ -31,11 +31,45 @@ Files supporting CAN can be found in the following locations: ``can_hdr_s`` structure depends on ``CONFIG_CAN_TIMESTAMP`` and is used to store the timestamp of the CAN message. -**Usage Note**: When reading from the CAN driver multiple messages -may be returned, depending on (1) the size the returned can -messages, and (2) the size of the buffer provided to receive CAN -messages. *Never assume that a single message will be returned*... -if you do this, *you will lose CAN data* under conditions where -your read buffer can hold more than one small message. Below is an -example about how you should think of the CAN read operation: +The upper half driver supports the following ``ioctl`` commands: + +- **CANIOC_RTR**: Send the given message (passed as ``ioctl`` argument) as a + remote request. On successful return, the passed message structure is updated + with the contents of the received message; i.e. the message ID and the + standard/extended ID indication bit stay the same, but the DLC and data bits + are updated with the contents of the received message. If no response is + received after the specified timeout, ioctl will return. +- **CANIOC_GET_BITTIMING**: Return the current bit timing settings. +- **CANIOC_SET_BITTIMING**: Set new current bit timing values. +- **CANIOC_ADD_STDFILTER**: Add an address filter for a standard 11 bit address. +- **CANIOC_ADD_EXTFILTER**: Add an address filter for a extended 29 bit address. +- **CANIOC_DEL_STDFILTER**: Remove an address filter for a standard 11 bit + address. +- **CANIOC_DEL_EXTFILTER**: Remove an address filter for a standard 29 bit + address. +- **CANIOC_GET_CONNMODES**: Get the current bus connection modes. +- **CANIOC_SET_CONNMODES**: Set new bus connection modes values. +- **CANIOC_BUSOFF_RECOVERY**: Initiates the BUS-OFF recovery sequence. +- **CANIOC_SET_NART**: Enable/Disable NART (No Automatic Retry). +- **CANIOC_SET_ABOM**: Enable/Disable ABOM (Automatic Bus-off Management). +- **CANIOC_IFLUSH**: Flush data received but not read. +- **CANIOC_OFLUSH**: Flush data written but not transmitted. +- **CANIOC_IOFLUSH**: Flush data received but not read and data written but not + transmitted. +- **CANIOC_SET_STATE**: Set specific can controller state. +- **CANIOC_GET_STATE**: Get specific can controller state. +- **CANIOC_SET_TRANSVSTATE**: Set specific can transceiver state. +- **CANIOC_GET_TRANSVSTATE**: Get specific can transceiver state. +- **CANIOC_SET_MSGALIGN**: Set messages alignment. Read and written messages can + be configured to be aligned to multiple of given bytes by this. The default + value is 1. The alignment affects both read and write operation. The value 0 + has a special meaning where write behaves the same way as with 1, but read + will always provide only a single message. +- **CANIOC_GET_MSGALIGN**: Get messages alignment. See CANIOC_SET_MSGALIGN for + explanation. + +**Usage Note**: The default behavior of the upper half driver is to return +multiple messages on ``read``. See the `guide on this subject +</guides/reading_can_msgs.html>`_. + **Examples**: ``drivers/can/mcp2515.c``. diff --git a/Documentation/guides/reading_can_msgs.rst b/Documentation/guides/reading_can_msgs.rst index 2e956a98aee..24929153dda 100644 --- a/Documentation/guides/reading_can_msgs.rst +++ b/Documentation/guides/reading_can_msgs.rst @@ -2,57 +2,44 @@ Reading CAN Messages ==================== -.. warning:: - Migrated from: - https://cwiki.apache.org/confluence/display/NUTTX/Reading+CAN+Messages +NuttX's CAN driver's default behavior is to return multiple messages for single +``read`` operation, if they fit. If your code (especially if migrated from +SocketCAN) doesn't count with it, then you will most likely encounter seemingly +lost frames. You have two options: Either implement your code to support this +behavior or you can switch this behavior off. -Twice now, there have been complaints or issues about reading messages from the -CAN driver. The usual concern is that the driver is somehow losing or dropping -CAN messages. In these cases, it is often discovered that the CAN driver is -being used incorrectly and, as is human nature, the driver itself is blamed for -the problem. - -When reading from the CAN driver, multiple messages may be returned, depending -on two factors: - -1. The size of the returned CAN messages. -2. The size of the buffer provided to receive CAN messages. - -It should never be assumed that a single message will be returned; making this -assumption can lead to lost CAN messages under conditions in which the read -buffer can hold more than one small message. The following example shows how to -properly handle the CAN read operation: +The following example shows how you can handle multiple messages: .. code-block:: c #define BUFLEN 128 /* Some arbitrary size for the CAN RX buffer */ - + FAR struct can_msg_s *msg; char rxbuffer[BUFLEN]; ssize_t nread; int nbytes; int msglen int i; - + /* Read messages into the RX buffer */ - + nread = read(fd, rxbuffer, BUFLEN); - + /* Check for read errors */ ... - + /* Process each message in the RX buffer */ - + for (i = 0; i <= nread - CAN_MSGLEN(0); i += msglen) { /* Get the next message from the RX buffer */ - + msg = (FAR struct can_msg_s *)&rxbuffer[i]; nbytes = can_dlc2bytes(msg->cm_hdr.ch_dlc); msglen = CAN_MSGLEN(nbytes); - + DEBUGASSERT(i + msglen < BUFLEN); - + /* Process the next CAN message */ ... } @@ -60,3 +47,44 @@ properly handle the CAN read operation: By looping over the read buffer and parsing out each CAN message, it is possible to avoid losing messages that are stored contiguously in the input buffer. + +The alternative is to use message alignment functionality. By setting the +message alignment to zero the driver will always return only a single message +for a single ``read`` operation: + +.. code-block:: c + + unsigned msgalign = 0; + ioctl(fd, CANIOC_SET_MSGALIGN, &msgalign); + +The message alignment functionality can be used to tweak the behavior event +further. It in general controls alignment of messages in the buffers passed to +both ``read`` and ``write`` operations. While the default behavior of packing as +many messages to the buffer as possible provides the most efficient exchange, +you might also want and easier usage where you pass array of messages. This can +be ensured with setting message align size to exactly size of the message: + +.. code-block:: c + + unsigned msgsiz = sizeof(struct can_msg_s); + struct can_msg_s msgs[5]; + ssize_t nread; + int i; + + /* Set message alignment to message size. */ + + ioctl(fd, CANIOC_SET_MSGALIGN, &msgsiz); + + /* Read messages to the array. */ + + nread = read(fd, msgs, sizeof(msgs)); + + /* Iterate over read messages */ + for (i = 0; i < nread / msgsiz; i--) + { + /* Process CAN message msgs[i] */ + } + +The same alignment rule applies to the ``write`` as well, so with alignment like +it is in the example you write array of message and not message packed right +after each other. diff --git a/drivers/can/can.c b/drivers/can/can.c index a61142b75f6..d3fb83151a3 100644 --- a/drivers/can/can.c +++ b/drivers/can/can.c @@ -27,6 +27,7 @@ #include <nuttx/config.h> #include <sys/types.h> +#include <sys/param.h> #include <inttypes.h> #include <stdint.h> #include <stdbool.h> @@ -193,6 +194,7 @@ static FAR struct can_reader_s *init_can_reader(FAR struct file *filep) DEBUGASSERT(reader != NULL); nxsem_init(&reader->fifo.rx_sem, 0, 0); + reader->msgalign = 1; filep->f_priv = reader; return reader; @@ -380,6 +382,7 @@ static ssize_t can_read(FAR struct file *filep, FAR char *buffer, { FAR struct can_reader_s *reader; FAR struct can_rxfifo_s *fifo; + unsigned int msgalign; irqstate_t flags; int ret = 0; @@ -395,6 +398,7 @@ static ssize_t can_read(FAR struct file *filep, FAR char *buffer, DEBUGASSERT(filep->f_priv != NULL); reader = (FAR struct can_reader_s *)filep->f_priv; fifo = &reader->fifo; + msgalign = reader->msgalign; /* Interrupts must be disabled while accessing the cd_recv FIFO */ @@ -482,6 +486,13 @@ static ssize_t can_read(FAR struct file *filep, FAR char *buffer, memcpy(&buffer[ret], msg, msglen); ret += msglen; + if (msgalign > 1) + { + ret = powerof2(msgalign) + ? roundup2(ret, msgalign) + : roundup(ret, msgalign); + } + /* Increment the head of the circular message buffer */ if (++fifo->rx_head >= CONFIG_CAN_RXFIFOSIZE) @@ -489,7 +500,7 @@ static ssize_t can_read(FAR struct file *filep, FAR char *buffer, fifo->rx_head = 0; } } - while (fifo->rx_head != fifo->rx_tail); + while (fifo->rx_head != fifo->rx_tail && msgalign != 0); if (fifo->rx_head != fifo->rx_tail) { @@ -505,7 +516,9 @@ return_with_irqdisabled: leave_critical_section(flags); } - return ret; + /* ret can be more than buflen due to roundup, so return at most buflen */ + + return ret ? MIN(ret, buflen) : -EMSGSIZE; } /**************************************************************************** @@ -595,9 +608,11 @@ static int can_xmit(FAR struct can_dev_s *dev) static ssize_t can_write(FAR struct file *filep, FAR const char *buffer, size_t buflen) { - FAR struct inode *inode = filep->f_inode; - FAR struct can_dev_s *dev = inode->i_private; - FAR struct can_txcache_s *sender = &dev->cd_sender; + FAR struct inode *inode = filep->f_inode; + FAR struct can_dev_s *dev = inode->i_private; + FAR struct can_txcache_s *sender = &dev->cd_sender; + FAR struct can_reader_s *reader = filep->f_priv; + unsigned int msgalign = reader->msgalign; FAR struct can_msg_s *msg; bool inactive; ssize_t nsent = 0; @@ -702,6 +717,18 @@ static ssize_t can_write(FAR struct file *filep, FAR const char *buffer, /* Increment the number of bytes that were sent */ nsent += msglen; + + if (msgalign > 1) + { + nsent = powerof2(msgalign) + ? roundup2(nsent, msgalign) + : roundup(nsent, msgalign); + } + + if (msgalign == 0) + { + break; + } } /* We get here after all messages have been added to the sender. Check if @@ -713,9 +740,11 @@ static ssize_t can_write(FAR struct file *filep, FAR const char *buffer, can_xmit(dev); } - /* Return the number of bytes that were sent */ + /* Return the number of bytes that were sent, but at most buflen as nsent + * can be more due to roundup. + */ - ret = nsent; + ret = MIN(nsent, buflen); return_with_irqdisabled: leave_critical_section(flags); @@ -964,6 +993,22 @@ static int can_ioctl(FAR struct file *filep, int cmd, unsigned long arg) } break; + /* Set message alignment for read and write operations */ + + case CANIOC_SET_MSGALIGN: + { + reader->msgalign = *(FAR unsigned int *)arg; + } + break; + + /* Get message alignment for read and write operations */ + + case CANIOC_GET_MSGALIGN: + { + *(FAR unsigned int *)arg = reader->msgalign; + } + break; + /* Not a "built-in" ioctl command.. perhaps it is unique to this * lower-half, device driver. */ diff --git a/include/nuttx/can/can.h b/include/nuttx/can/can.h index bbfea33f96d..517314c930a 100644 --- a/include/nuttx/can/can.h +++ b/include/nuttx/can/can.h @@ -305,6 +305,31 @@ * is returned with the errno variable set to indicate the * nature of the error. * Dependencies: None + * + * CANIOC_SET_MSGALIGN + * Description: Set messages alignment. Read and written messages can be + * configured to be aligned to multiple of given bytes by + * this. The default value is 1. The alignment affects both + * read and write operation. The value 0 has a special + * meaning where both write will always write only a single + * message and read will always provide only a single + * message. + * + * Argument: A pointer to an unsigned int type with alignment value. + * returned Value: Zero (OK) is returned on success. Otherwise -1 (ERROR) + * is returned with the errno variable set to indicate the + * nature of the error. + * Dependencies: None + * + * CANIOC_GET_MSGALIGN + * Description: Get messages alignment. See CANIOC_SET_MSGALIGN for + * explanation. + * + * Argument: A pointer to an unsigned int type for alignment value. + * returned Value: Zero (OK) is returned on success. Otherwise -1 (ERROR) + * is returned with the errno variable set to indicate the + * nature of the error. + * Dependencies: None */ #define CANIOC_RTR _CANIOC(1) @@ -326,9 +351,11 @@ #define CANIOC_GET_STATE _CANIOC(17) #define CANIOC_SET_TRANSVSTATE _CANIOC(18) #define CANIOC_GET_TRANSVSTATE _CANIOC(19) +#define CANIOC_SET_MSGALIGN _CANIOC(20) +#define CANIOC_GET_MSGALIGN _CANIOC(21) #define CAN_FIRST 0x0001 /* First common command */ -#define CAN_NCMDS 19 /* 20 common commands */ +#define CAN_NCMDS 21 /* 21 common commands */ /* User defined ioctl commands are also supported. These will be forwarded * by the upper-half CAN driver to the lower-half CAN driver via the @@ -796,13 +823,14 @@ struct can_ops_s * * The elements of 'cd_ops', and 'cd_priv' * - * The common logic will initialize all semaphores. + * The common logic will initialize all semaphores and set 'msgalign' to '1'. */ struct can_reader_s { struct list_node list; struct can_rxfifo_s fifo; /* Describes receive FIFO */ + unsigned int msgalign; FAR struct pollfd *cd_fds; };
