From e59645042d9a8ff2892e30de348a3c20f750dec9 Mon Sep 17 00:00:00 2001 From: Petro Karashchenko Date: Fri, 11 Mar 2022 16:46:20 +0100 Subject: [PATCH] [BACKPORT] drivers/can: optimize can driver reader side Signed-off-by: Petro Karashchenko --- arch/arm/src/lpc43xx/lpc43_can.c | 2 +- drivers/can/can.c | 96 ++++++++++++++++++-------------- include/nuttx/can/can.h | 6 +- 3 files changed, 60 insertions(+), 44 deletions(-) diff --git a/arch/arm/src/lpc43xx/lpc43_can.c b/arch/arm/src/lpc43xx/lpc43_can.c index 48e0b90ec90..07cf9f631db 100644 --- a/arch/arm/src/lpc43xx/lpc43_can.c +++ b/arch/arm/src/lpc43xx/lpc43_can.c @@ -186,7 +186,7 @@ static int can_bittiming(struct up_dev_s *priv); static const struct can_ops_s g_canops = { .co_reset = can_reset, - .co_setup = can_setup, + .co_setup = can_setup, .co_shutdown = can_shutdown, .co_rxint = can_rxint, .co_txint = can_txint, diff --git a/drivers/can/can.c b/drivers/can/can.c index 76369c82fe9..981c077b56b 100644 --- a/drivers/can/can.c +++ b/drivers/can/can.c @@ -411,7 +411,6 @@ static int can_open(FAR struct file *filep) FAR struct inode *inode = filep->f_inode; FAR struct can_dev_s *dev = inode->i_private; irqstate_t flags; - int tmp; int ret; /* If the port is the middle of closing, wait until the close is finished */ @@ -426,46 +425,48 @@ static int can_open(FAR struct file *filep) * for this device, then perform hardware initialization. */ - if (list_is_empty(&dev->cd_readers)) + caninfo("ocount: %u\n", dev->cd_crefs); + + if (dev->cd_crefs >= 255) { - caninfo("ocount: %d\n", 0); + /* Limit to no more than 255 opens */ + ret = -EMFILE; + goto errout; + } + else + { flags = enter_critical_section(); - ret = dev_setup(dev); - if (ret >= 0) + + if (dev->cd_crefs == 0) { - /* Mark the FIFOs empty */ + ret = dev_setup(dev); + if (ret == OK) + { + /* Mark the FIFOs empty */ - dev->cd_xmit.tx_head = 0; - dev->cd_xmit.tx_queue = 0; - dev->cd_xmit.tx_tail = 0; + dev->cd_xmit.tx_head = 0; + dev->cd_xmit.tx_queue = 0; + dev->cd_xmit.tx_tail = 0; - /* Finally, Enable the CAN RX interrupt */ + /* Finally, Enable the CAN RX interrupt */ - dev_rxint(dev, true); + dev_rxint(dev, true); + } } - list_add_head(&dev->cd_readers, - (FAR struct list_node *)init_can_reader(filep)); - - leave_critical_section(flags); - } - else - { - tmp = list_length(&dev->cd_readers); - caninfo("ocount: %d\n", tmp); - - if (tmp >= 255) + if (ret == OK) { - /* Limit to no more than 255 opens */ + dev->cd_crefs++; - ret = -EMFILE; - goto errout; - } + /* Update the reader list only if driver was open for reading */ - flags = enter_critical_section(); - list_add_head(&dev->cd_readers, - (FAR struct list_node *)init_can_reader(filep)); + if ((filep->f_oflags & O_RDOK) != 0) + { + list_add_head(&dev->cd_readers, + (FAR struct list_node *)init_can_reader(filep)); + } + } leave_critical_section(flags); } @@ -494,7 +495,7 @@ static int can_close(FAR struct file *filep) int ret; #ifdef CONFIG_DEBUG_CAN_INFO - caninfo("ocount: %d\n", list_length(&dev->cd_readers)); + caninfo("ocount: %u\n", dev->cd_crefs); #endif ret = can_takesem(&dev->cd_closesem); @@ -515,10 +516,11 @@ static int can_close(FAR struct file *filep) } filep->f_priv = NULL; + dev->cd_crefs--; - /* Uninitialize the driver if there are no more readers */ + /* De-initialize the driver if there are no more readers */ - if (!list_is_empty(&dev->cd_readers)) + if (dev->cd_crefs > 0) { goto errout; } @@ -563,7 +565,7 @@ static int can_close(FAR struct file *filep) static ssize_t can_read(FAR struct file *filep, FAR char *buffer, size_t buflen) { - FAR struct can_reader_s *reader = NULL; + FAR struct can_reader_s *reader; FAR struct can_rxfifo_s *fifo; size_t nread; irqstate_t flags; @@ -582,11 +584,25 @@ static ssize_t can_read(FAR struct file *filep, FAR char *buffer, if (buflen >= CAN_MSGLEN(0)) { + DEBUGASSERT(filep->f_priv != NULL); + reader = (FAR struct can_reader_s *)filep->f_priv; + + fifo = &reader->fifo; + /* Interrupts must be disabled while accessing the cd_recv FIFO */ flags = enter_critical_section(); #ifdef CONFIG_CAN_ERRORS + + /* Check for reader fifo overflow */ + + if (fifo->rx_overflow) + { + dev->cd_error |= CAN_ERROR5_RXOVERFLOW; + fifo->rx_overflow = false; + } + /* Check for internal errors */ if (dev->cd_error != 0) @@ -623,12 +639,7 @@ static ssize_t can_read(FAR struct file *filep, FAR char *buffer, } #endif /* CONFIG_CAN_ERRORS */ - DEBUGASSERT(filep->f_priv != NULL); - reader = (FAR struct can_reader_s *)filep->f_priv; - - fifo = &reader->fifo; - - if (filep->f_oflags & O_NONBLOCK) + if ((filep->f_oflags & O_NONBLOCK) != 0) { ret = nxsem_trywait(&fifo->rx_sem); } @@ -1248,12 +1259,13 @@ int can_register(FAR const char *path, FAR struct can_dev_s *dev) /* Initialize the CAN device structure */ - list_initialize(&dev->cd_readers); - dev->cd_ntxwaiters = 0; + dev->cd_crefs = 0; dev->cd_npendrtr = 0; + dev->cd_ntxwaiters = 0; #ifdef CONFIG_CAN_ERRORS dev->cd_error = 0; #endif + list_initialize(&dev->cd_readers); /* Initialize semaphores */ @@ -1446,7 +1458,7 @@ int can_receive(FAR struct can_dev_s *dev, FAR struct can_hdr_s *hdr, { /* Report rx overflow error */ - dev->cd_error |= CAN_ERROR5_RXOVERFLOW; + fifo->rx_overflow = true; } #endif } diff --git a/include/nuttx/can/can.h b/include/nuttx/can/can.h index 0f7db306a54..0b5d28287a7 100644 --- a/include/nuttx/can/can.h +++ b/include/nuttx/can/can.h @@ -498,6 +498,9 @@ struct can_rxfifo_s sem_t rx_sem; +#ifdef CONFIG_CAN_ERRORS + bool rx_overflow; /* Indicates the RX FIFO overflow event */ +#endif uint8_t rx_head; /* Index to the head [IN] in the circular buffer */ uint8_t rx_tail; /* Index to the tail [OUT] in the circular buffer */ /* Circular buffer of CAN messages */ @@ -606,12 +609,13 @@ struct can_reader_s struct can_dev_s { + uint8_t cd_crefs; /* References counts on number of opens */ uint8_t cd_npendrtr; /* Number of pending RTR messages */ volatile uint8_t cd_ntxwaiters; /* Number of threads waiting to enqueue a message */ - struct list_node cd_readers; /* List of readers */ #ifdef CONFIG_CAN_ERRORS uint8_t cd_error; /* Flags to indicate internal device errors */ #endif + struct list_node cd_readers; /* List of readers */ sem_t cd_closesem; /* Locks out new opens while close is in progress */ sem_t cd_pollsem; /* Manages exclusive access to cd_fds[] */ struct can_txfifo_s cd_xmit; /* Describes transmit FIFO */