sergiotarxz/nuttx
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
4fd506efaa
nuttx/net/can/can_recvfrom.c

839 lines
23 KiB
C
Raw Normal View History

net: Add SocketCAN support
2020-06-15 10:23:25 +02:00
/****************************************************************************
* net/can/can_recvfrom.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#ifdef CONFIG_NET_CAN
#include <sys/types.h>
#include <sys/socket.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <debug.h>
#include <assert.h>
#include <arch/irq.h>
#include <nuttx/semaphore.h>
#include <nuttx/net/net.h>
#include <nuttx/net/netdev.h>
#include "netdev/netdev.h"
#include "devif/devif.h"
#include "can/can.h"
#include "socket/socket.h"
#include <netpacket/packet.h>
#ifdef CONFIG_NET_TIMESTAMP
#include <sys/time.h>
#endif
/****************************************************************************
* Private Types
****************************************************************************/
struct can_recvfrom_s
{
FAR struct socket *pr_sock; /* The parent socket structure */
FAR struct devif_callback_s *pr_cb; /* Reference to callback instance */
sem_t pr_sem; /* Semaphore signals recv completion */
size_t pr_buflen; /* Length of receive buffer */
FAR uint8_t *pr_buffer; /* Pointer to receive buffer */
ssize_t pr_recvlen; /* The received length */
#ifdef CONFIG_NET_CMSG
size_t pr_msglen; /* Length of msg buffer */
FAR uint8_t *pr_msgbuf; /* Pointer to msg buffer */
#endif
int pr_result; /* Success:OK, failure:negated errno */
};
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: can_add_recvlen
*
* Description:
* Update information about space available for new data and update size
* of data in buffer, This logic accounts for the case where
* recvfrom_udpreadahead() sets state.pr_recvlen == -1 .
*
* Input Parameters:
* pstate recvfrom state structure
* recvlen size of new data appended to buffer
*
* Returned Value:
* None
*
****************************************************************************/
static inline void can_add_recvlen(FAR struct can_recvfrom_s *pstate,
size_t recvlen)
{
if (pstate->pr_recvlen < 0)
{
pstate->pr_recvlen = 0;
}
pstate->pr_recvlen += recvlen;
pstate->pr_buffer += recvlen;
pstate->pr_buflen -= recvlen;
}
/****************************************************************************
* Name: can_recvfrom_newdata
*
* Description:
* Copy the read data from the packet
*
* Input Parameters:
* dev The structure of the network driver that caused the event.
* pstate recvfrom state structure
*
* Returned Value:
* None.
*
* Assumptions:
* The network is locked.
*
****************************************************************************/
static size_t can_recvfrom_newdata(FAR struct net_driver_s *dev,
FAR struct can_recvfrom_s *pstate)
{
size_t recvlen;
if (dev->d_len > pstate->pr_buflen)
{
recvlen = pstate->pr_buflen;
}
else
{
recvlen = dev->d_len;
}
/* Copy the new packet data into the user buffer */
memcpy(pstate->pr_buffer, dev->d_buf, recvlen);
/* Update the accumulated size of the data read */
can_add_recvlen(pstate, recvlen);
return recvlen;
}
/****************************************************************************
* Name: can_newdata
*
* Description:
* Copy the read data from the packet
*
* Input Parameters:
* dev The structure of the network driver that generated the event
* pstate recvfrom state structure
*
* Returned Value:
* None.
*
* Assumptions:
* The network is locked.
*
****************************************************************************/
static inline void can_newdata(FAR struct net_driver_s *dev,
FAR struct can_recvfrom_s *pstate)
{
/* Take as much data from the packet as we can */
size_t recvlen = can_recvfrom_newdata(dev, pstate);
/* If there is more data left in the packet that we could not buffer, then
* add it to the read-ahead buffers.
*/
if (recvlen < dev->d_len)
{
FAR struct can_conn_s *conn =
(FAR struct can_conn_s *)pstate->pr_sock->s_conn;
FAR uint8_t *buffer = (FAR uint8_t *)dev->d_appdata + recvlen;
uint16_t buflen = dev->d_len - recvlen;
#ifdef CONFIG_DEBUG_NET
uint16_t nsaved;
nsaved = can_datahandler(conn, buffer, buflen);
#else
can_datahandler(conn, buffer, buflen);
#endif
/* There are complicated buffering issues that are not addressed fully
* here. For example, what if up_datahandler() cannot buffer the
* remainder of the packet? In that case, the data will be dropped but
* still ACKed. Therefore it would not be resent.
*
* This is probably not an issue here because we only get here if the
* read-ahead buffers are empty and there would have to be something
* serioulsy wrong with the configuration not to be able to buffer a
* partial packet in this context.
*/
#ifdef CONFIG_DEBUG_NET
if (nsaved < buflen)
{
nerr("ERROR: packet data not saved (%d bytes)\n", buflen - nsaved);
}
#endif
}
/* Indicate no data in the buffer */
dev->d_len = 0;
}
/****************************************************************************
* Name: can_readahead
*
* Description:
* Copy the read-ahead data from the packet
*
* Input Parameters:
* pstate recvfrom state structure
*
* Returned Value:
* None
*
* Assumptions:
* The network is locked.
*
****************************************************************************/
static inline int can_readahead(struct can_recvfrom_s *pstate)
{
FAR struct can_conn_s *conn =
(FAR struct can_conn_s *) pstate->pr_sock->s_conn;
FAR struct iob_s *iob;
int recvlen;
/* Check there is any CAN data already buffered in a read-ahead
* buffer.
*/
pstate->pr_recvlen = -1;
if ((iob = iob_peek_queue(&conn->readahead)) != NULL &&
pstate->pr_buflen > 0)
{
DEBUGASSERT(iob->io_pktlen > 0);
/* Transfer that buffered data from the I/O buffer chain into
* the user buffer.
*/
recvlen = iob_copyout(pstate->pr_buffer, iob, pstate->pr_buflen, 0);
/* If we took all of the data from the I/O buffer chain is empty, then
* release it. If there is still data available in the I/O buffer
* chain, then just trim the data that we have taken from the
* beginning of the I/O buffer chain.
*/
if (recvlen >= iob->io_pktlen)
{
FAR struct iob_s *tmp;
/* Remove the I/O buffer chain from the head of the read-ahead
* buffer queue.
*/
tmp = iob_remove_queue(&conn->readahead);
DEBUGASSERT(tmp == iob);
UNUSED(tmp);
/* And free the I/O buffer chain */
iob_free_chain(iob, IOBUSER_NET_CAN_READAHEAD);
}
else
{
/* The bytes that we have received from the head of the I/O
* buffer chain (probably changing the head of the I/O
* buffer queue).
*/
iob_trimhead_queue(&conn->readahead, recvlen,
IOBUSER_NET_CAN_READAHEAD);
}
/* do not pass frames with DLC > 8 to a legacy socket */
#if defined(CONFIG_NET_CANPROTO_OPTIONS) && defined(CONFIG_NET_CAN_CANFD)
if (!conn->fd_frames)
#endif
{
if (recvlen > sizeof(struct can_frame))
{
return 0;
}
}
return recvlen;
}
return 0;
}
/****************************************************************************
* Name: can_readahead
*
* Description:
* Copy the read-ahead data from the packet
*
* Input Parameters:
* pstate recvfrom state structure
*
* Returned Value:
* None
*
* Assumptions:
* The network is locked.
*
****************************************************************************/
#ifdef CONFIG_NET_TIMESTAMP
static inline int can_readahead_timestamp(struct can_conn_s *conn,
FAR uint8_t *buffer)
{
FAR struct iob_s *iob;
int recvlen;
if ((iob = iob_peek_queue(&conn->readahead)) != NULL)
{
DEBUGASSERT(iob->io_pktlen > 0);
/* Transfer that buffered data from the I/O buffer chain into
* the user buffer.
*/
recvlen = iob_copyout(buffer, iob, sizeof(struct timeval), 0);
/* If we took all of the data from the I/O buffer chain is empty, then
* release it. If there is still data available in the I/O buffer
* chain, then just trim the data that we have taken from the
* beginning of the I/O buffer chain.
*/
if (recvlen >= iob->io_pktlen)
{
FAR struct iob_s *tmp;
/* Remove the I/O buffer chain from the head of the read-ahead
* buffer queue.
*/
tmp = iob_remove_queue(&conn->readahead);
DEBUGASSERT(tmp == iob);
UNUSED(tmp);
/* And free the I/O buffer chain */
iob_free_chain(iob, IOBUSER_NET_CAN_READAHEAD);
}
else
{
/* The bytes that we have received from the head of the I/O
* buffer chain (probably changing the head of the I/O
* buffer queue).
*/
iob_trimhead_queue(&conn->readahead, recvlen,
IOBUSER_NET_CAN_READAHEAD);
}
return recvlen;
}
return 0;
}
#endif
#ifdef CONFIG_NET_CANPROTO_OPTIONS
static int can_recv_filter(struct can_conn_s *conn, canid_t id)
{
for (int i = 0; i < conn->filter_count; i++)
{
if (conn->filters[i].can_id & CAN_INV_FILTER)
{
if ((id & conn->filters[i].can_mask) !=
((conn->filters[i].can_id & ~CAN_INV_FILTER) &
conn->filters[i].can_mask))
{
return 1;
}
}
else
{
if ((id & conn->filters[i].can_mask) ==
(conn->filters[i].can_id & conn->filters[i].can_mask))
{
return 1;
}
}
}
return 0;
}
#endif
static uint16_t can_recvfrom_eventhandler(FAR struct net_driver_s *dev,
FAR void *pvconn,
FAR void *pvpriv, uint16_t flags)
{
struct can_recvfrom_s *pstate = (struct can_recvfrom_s *)pvpriv;
struct can_conn_s *conn = (struct can_conn_s *)pstate->pr_sock->s_conn;
/* 'priv' might be null in some race conditions (?) */
if (pstate)
{
if ((flags & CAN_NEWDATA) != 0)
{
/* If a new packet is available, check receive filters
* when is valid then complete the read action.
*/
#ifdef CONFIG_NET_CANPROTO_OPTIONS
if (can_recv_filter(conn, (canid_t) *dev->d_appdata) == 0)
{
flags &= ~CAN_NEWDATA;
return flags;
}
#endif
/* do not pass frames with DLC > 8 to a legacy socket */
#if defined(CONFIG_NET_CANPROTO_OPTIONS) && defined(CONFIG_NET_CAN_CANFD)
if (!conn->fd_frames)
#endif
{
if (dev->d_len > sizeof(struct can_frame))
{
/* DO WE NEED TO CLEAR FLAGS?? */
flags &= ~CAN_NEWDATA;
return flags;
}
}
/* Copy the packet */
can_newdata(dev, pstate);
#ifdef CONFIG_NET_TIMESTAMP
if (pstate->pr_sock->s_timestamp)
{
if (pstate->pr_msglen == sizeof(struct timeval))
{
can_readahead_timestamp(conn, pstate->pr_msgbuf);
}
else
{
/* We still have to consume the data
* otherwise IOB gets full
*/
uint8_t dummy_buf[sizeof(struct timeval)];
can_readahead_timestamp(conn, (uint8_t *)&dummy_buf);
}
}
#endif
/* We are finished. */
/* Don't allow any further call backs. */
pstate->pr_cb->flags = 0;
pstate->pr_cb->priv = NULL;
pstate->pr_cb->event = NULL;
/* indicate that the data has been consumed */
flags &= ~CAN_NEWDATA;
/* Wake up the waiting thread, returning the number of bytes
* actually read.
*/
nxsem_post(&pstate->pr_sem);
}
}
return flags;
}
/****************************************************************************
* Name: can_recvfrom_result
*
* Description:
* Evaluate the result of the recv operations
*
* Input Parameters:
* result The result of the net_lockedwait operation (may indicate EINTR)
* pstate A pointer to the state structure to be initialized
*
* Returned Value:
* The result of the recv operation with errno set appropriately
*
* Assumptions:
*
****************************************************************************/
static ssize_t can_recvfrom_result(int result,
FAR struct can_recvfrom_s *pstate)
{
/* Check for a error/timeout detected by the event handler. Errors are
* signaled by negative errno values for the rcv length
*/
if (pstate->pr_result < 0)
{
/* This might return EAGAIN on a timeout */
return pstate->pr_result;
}
/* If net_lockedwait failed, then we were probably reawakened by a signal.
* In this case, net_lockedwait will have returned negated errno
* appropriately.
*/
if (result < 0)
{
return result;
}
return pstate->pr_recvlen;
}
/****************************************************************************
* Name: can_recvfrom
*
* Description:
* recvfrom() receives messages from a socket, and may be used to receive
* data on a socket whether or not it is connection-oriented.
*
* If from is not NULL, and the underlying protocol provides the source
* address, this source address is filled in. The argument 'fromlen'
* initialized to the size of the buffer associated with from, and modified
* on return to indicate the actual size of the address stored there.
*
* Input Parameters:
* psock A pointer to a NuttX-specific, internal socket structure
* buf Buffer to receive data
* len Length of buffer
* flags Receive flags (ignored)
* from Address of source (may be NULL)
* fromlen The length of the address structure
*
****************************************************************************/
ssize_t can_recvfrom(FAR struct socket *psock, FAR void *buf,
size_t len, int flags,
FAR struct sockaddr *from,
FAR socklen_t *fromlen)
{
FAR struct can_conn_s *conn;
FAR struct net_driver_s *dev;
struct can_recvfrom_s state;
int ret;
DEBUGASSERT(psock != NULL && psock->s_conn != NULL && buf != NULL);
DEBUGASSERT(from == NULL ||
(fromlen != NULL && *fromlen >= sizeof(struct sockaddr_can)));
conn = (FAR struct can_conn_s *)psock->s_conn;
if (psock->s_type != SOCK_RAW)
{
nerr("ERROR: Unsupported socket type: %d\n", psock->s_type);
ret = -ENOSYS;
}
net_lock();
/* Initialize the state structure. */
memset(&state, 0, sizeof(struct can_recvfrom_s));
/* This semaphore is used for signaling and, hence, should not have
* priority inheritance enabled.
*/
nxsem_init(&state.pr_sem, 0, 0); /* Doesn't really fail */
nxsem_set_protocol(&state.pr_sem, SEM_PRIO_NONE);
state.pr_buflen = len;
state.pr_buffer = buf;
state.pr_sock = psock;
/* Handle any any CAN data already buffered in a read-ahead buffer. NOTE
* that there may be read-ahead data to be retrieved even after the
* socket has been disconnected.
*/
ret = can_readahead(&state);
if (ret > 0)
{
goto errout_with_state;
}
ret = state.pr_recvlen;
/* Handle non-blocking CAN sockets */
if (_SS_ISNONBLOCK(psock->s_flags) || (flags & MSG_DONTWAIT) != 0)
{
/* Return the number of bytes read from the read-ahead buffer if
* something was received (already in 'ret'); EAGAIN if not.
*/
if (ret < 0)
{
/* Nothing was received */
ret = -EAGAIN;
goto errout_with_state;
}
}
/* Get the device driver that will service this transfer */
dev = conn->dev;
if (dev == NULL)
{
ret = -ENODEV;
goto errout_with_state;
}
/* Set up the callback in the connection */
state.pr_cb = can_callback_alloc(dev, conn);
if (state.pr_cb)
{
state.pr_cb->flags = (CAN_NEWDATA | CAN_POLL);
state.pr_cb->priv = (FAR void *)&state;
state.pr_cb->event = can_recvfrom_eventhandler;
/* Wait for either the receive to complete or for an error/timeout to
* occur. NOTES: (1) net_lockedwait will also terminate if a signal
* is received, (2) the network is locked! It will be un-locked while
* the task sleeps and automatically re-locked when the task restarts.
*/
ret = net_lockedwait(&state.pr_sem);
/* Make sure that no further events are processed */
can_callback_free(dev, conn, state.pr_cb);
ret = can_recvfrom_result(ret, &state);
}
else
{
ret = -EBUSY;
}
errout_with_state:
net_unlock();
nxsem_destroy(&state.pr_sem);
return ret;
}
/****************************************************************************
* Name: can_recvmsg
*
* Description:
* recvmsg() receives messages from a socket, and may be used to receive
* data on a socket whether or not it is connection-oriented.
*
* If from is not NULL, and the underlying protocol provides the source
* address, this source address is filled in. The argument 'fromlen'
* initialized to the size of the buffer associated with from, and modified
* on return to indicate the actual size of the address stored there.
*
* Input Parameters:
* psock A pointer to a NuttX-specific, internal socket structure
* msg Buffer to receive msg
* flags Receive flags (ignored)
*
****************************************************************************/
#ifdef CONFIG_NET_CMSG
ssize_t can_recvmsg(FAR struct socket *psock, FAR struct msghdr *msg,
int flags)
{
FAR struct can_conn_s *conn;
FAR struct net_driver_s *dev;
struct can_recvfrom_s state;
int ret;
DEBUGASSERT(psock != NULL && psock->s_conn != NULL && msg != NULL);
conn = (FAR struct can_conn_s *)psock->s_conn;
if (psock->s_type != SOCK_RAW)
{
nerr("ERROR: Unsupported socket type: %d\n", psock->s_type);
ret = -ENOSYS;
}
net_lock();
/* Initialize the state structure. */
memset(&state, 0, sizeof(struct can_recvfrom_s));
/* This semaphore is used for signaling and, hence, should not have
* priority inheritance enabled.
*/
nxsem_init(&state.pr_sem, 0, 0); /* Doesn't really fail */
nxsem_set_protocol(&state.pr_sem, SEM_PRIO_NONE);
state.pr_buflen = msg->msg_iov->iov_len;
state.pr_buffer = msg->msg_iov->iov_base;
#ifdef CONFIG_NET_TIMESTAMP
if (psock->s_timestamp && msg->msg_controllen >=
(sizeof(struct cmsghdr) + sizeof(struct timeval)))
{
struct cmsghdr *cmsg = CMSG_FIRSTHDR(msg);
state.pr_msglen = sizeof(struct timeval);
state.pr_msgbuf = CMSG_DATA(cmsg);
cmsg->cmsg_level = SOL_SOCKET;
cmsg->cmsg_type = SO_TIMESTAMP;
cmsg->cmsg_len = state.pr_msglen;
msg->msg_controllen = sizeof(struct cmsghdr) + sizeof(struct timeval);
}
else
{
/* Expected behavior is that the msg_controllen becomes 0,
* otherwise CMSG_NXTHDR will go into a infinite loop
*/
msg->msg_controllen = 0;
}
#endif
state.pr_sock = psock;
/* Handle any any CAN data already buffered in a read-ahead buffer. NOTE
* that there may be read-ahead data to be retrieved even after the
* socket has been disconnected.
*/
ret = can_readahead(&state);
if (ret > 0)
{
#ifdef CONFIG_NET_TIMESTAMP
if (psock->s_timestamp)
{
if (state.pr_msglen == sizeof(struct timeval))
{
can_readahead_timestamp(conn, state.pr_msgbuf);
}
else
{
/* We still have to consume the data otherwise IOB gets full */
uint8_t dummy_buf[sizeof(struct timeval)];
can_readahead_timestamp(conn, (uint8_t *)&dummy_buf);
}
}
#endif
goto errout_with_state;
}
ret = state.pr_recvlen;
/* Handle non-blocking CAN sockets */
if (_SS_ISNONBLOCK(psock->s_flags) || (flags & MSG_DONTWAIT) != 0)
{
/* Return the number of bytes read from the read-ahead buffer if
* something was received (already in 'ret'); EAGAIN if not.
*/
if (ret < 0)
{
/* Nothing was received */
ret = -EAGAIN;
goto errout_with_state;
}
}
/* Get the device driver that will service this transfer */
dev = conn->dev;
if (dev == NULL)
{
ret = -ENODEV;
goto errout_with_state;
}
/* Set up the callback in the connection */
state.pr_cb = can_callback_alloc(dev, conn);
if (state.pr_cb)
{
state.pr_cb->flags = (CAN_NEWDATA | CAN_POLL);
state.pr_cb->priv = (FAR void *)&state;
state.pr_cb->event = can_recvfrom_eventhandler;
/* Wait for either the receive to complete or for an error/timeout to
* occur. NOTES: (1) net_lockedwait will also terminate if a signal
* is received, (2) the network is locked! It will be un-locked while
* the task sleeps and automatically re-locked when the task restarts.
*/
ret = net_lockedwait(&state.pr_sem);
/* Make sure that no further events are processed */
can_callback_free(dev, conn, state.pr_cb);
ret = can_recvfrom_result(ret, &state);
}
else
{
ret = -EBUSY;
}
errout_with_state:
net_unlock();
nxsem_destroy(&state.pr_sem);
return ret;
}
#endif
#endif /* CONFIG_NET_CAN */
Reference in New Issue Copy Permalink