Skip to content

Commit

Permalink
[BACKPORT] drivers/can: optimize can driver reader side
Browse files Browse the repository at this point in the history
Signed-off-by: Petro Karashchenko <petro.karashchenko@gmail.com>
  • Loading branch information
pkarashchenko authored and JacobCrabill committed Jul 28, 2022
1 parent a0c05c2 commit e596450
Show file tree
Hide file tree
Showing 3 changed files with 60 additions and 44 deletions.
2 changes: 1 addition & 1 deletion arch/arm/src/lpc43xx/lpc43_can.c
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
96 changes: 54 additions & 42 deletions drivers/can/can.c
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand All @@ -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);
}
Expand Down Expand Up @@ -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);
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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 */

Expand Down Expand Up @@ -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
}
Expand Down
6 changes: 5 additions & 1 deletion include/nuttx/can/can.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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 */
Expand Down

0 comments on commit e596450

Please sign in to comment.