/ksrc/drivers/can/rtcan_raw.c
C | 1050 lines | 671 code | 229 blank | 150 comment | 168 complexity | 4fcd4511c50ef85a5fed5b23efbeac78 MD5 | raw file
Possible License(s): LGPL-2.1, GPL-2.0
- /*
- * Copyright (C) 2005, 2006 Sebastian Smolorz
- * <Sebastian.Smolorz@stud.uni-hannover.de>
- *
- * Copyright (C) 2006 Wolfgang Grandegger <wg@grandegger.com>
- *
- *
- * Parts of this software are based on the following:
- *
- * - RTAI CAN device driver for SJA1000 controllers by Jan Kiszka
- *
- * - linux-can.patch, a CAN socket framework for Linux,
- * Copyright (C) 2004, 2005, Robert Schwebel, Benedikt Spranger,
- * Marc Kleine-Budde, Sascha Hauer, Pengutronix
- *
- * - RTnet (www.rtnet.org)
- *
- * - serial device driver and profile included in Xenomai (RTDM),
- * Copyright (C) 2005 Jan Kiszka <jan.kiszka@web.de>.
- *
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software Foundation,
- * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
- #include <linux/module.h>
- #include <linux/delay.h>
- #include <rtdm/rtdm_driver.h>
- #include <rtdm/rtcan.h>
- #include "rtcan_version.h"
- #include "rtcan_socket.h"
- #include "rtcan_list.h"
- #include "rtcan_dev.h"
- #include "rtcan_raw.h"
- #include "rtcan_internal.h"
- /*
- * Set if socket wants to receive a high precision timestamp together with
- * CAN frames
- */
- #define RTCAN_GET_TIMESTAMP RTDM_USER_CONTEXT_FLAG
- MODULE_AUTHOR("RT-Socket-CAN Development Team");
- MODULE_DESCRIPTION("RTDM CAN raw socket device driver");
- MODULE_LICENSE("GPL");
- void rtcan_tx_push(struct rtcan_device *dev, struct rtcan_socket *sock,
- can_frame_t *frame);
- static struct rtdm_device rtcan_proto_raw_dev;
- static inline int rtcan_accept_msg(uint32_t can_id, can_filter_t *filter)
- {
- if ((filter->can_mask & CAN_INV_FILTER))
- return ((can_id & filter->can_mask) != filter->can_id);
- else
- return ((can_id & filter->can_mask) == filter->can_id);
- }
- static void rtcan_rcv_deliver(struct rtcan_recv *recv_listener,
- struct rtcan_skb *skb)
- {
- int size_free;
- size_t cpy_size, first_part_size;
- struct rtcan_rb_frame *frame = &skb->rb_frame;
- struct rtcan_socket *sock = recv_listener->sock;
- struct rtdm_dev_context *context = rtcan_socket_context(sock);
- cpy_size = skb->rb_frame_size;
- /* Check if socket wants to receive a timestamp */
- if (test_bit(RTCAN_GET_TIMESTAMP, &context->context_flags)) {
- cpy_size += RTCAN_TIMESTAMP_SIZE;
- frame->can_dlc |= RTCAN_HAS_TIMESTAMP;
- } else
- frame->can_dlc &= RTCAN_HAS_NO_TIMESTAMP;
- /* Calculate free size in the ring buffer */
- size_free = sock->recv_head - sock->recv_tail;
- if (size_free <= 0)
- size_free += RTCAN_RXBUF_SIZE;
- /* Test if ring buffer has enough space. */
- if (size_free > cpy_size) {
- /* Check if we must wrap around the end of buffer */
- if ((sock->recv_tail + cpy_size) > RTCAN_RXBUF_SIZE) {
- /* Wrap around: Two memcpy operations */
- first_part_size = RTCAN_RXBUF_SIZE - sock->recv_tail;
- memcpy(&sock->recv_buf[sock->recv_tail], (void *)frame,
- first_part_size);
- memcpy(&sock->recv_buf[0], (void *)frame +
- first_part_size, cpy_size - first_part_size);
- } else
- memcpy(&sock->recv_buf[sock->recv_tail], (void *)frame,
- cpy_size);
- /* Adjust tail */
- sock->recv_tail = (sock->recv_tail + cpy_size) &
- (RTCAN_RXBUF_SIZE - 1);
- /*Notify the delivery of the message */
- rtdm_sem_up(&sock->recv_sem);
- } else {
- /* Overflow of socket's ring buffer! */
- sock->rx_buf_full++;
- RTCAN_RTDM_DBG("%s: socket buffer overflow (fd=%d), message discarded\n",
- rtcan_proto_raw_dev.driver_name, context->fd);
- }
- }
- void rtcan_rcv(struct rtcan_device *dev, struct rtcan_skb *skb)
- {
- nanosecs_abs_t timestamp = rtdm_clock_read();
- /* Entry in reception list, begin with head */
- struct rtcan_recv *recv_listener = dev->recv_list;
- struct rtcan_rb_frame *frame = &skb->rb_frame;
- /* Copy timestamp to skb */
- memcpy((void *)&skb->rb_frame + skb->rb_frame_size,
- ×tamp, RTCAN_TIMESTAMP_SIZE);
- if ((frame->can_id & CAN_ERR_FLAG)) {
- dev->err_count++;
- while (recv_listener != NULL) {
- if ((frame->can_id & recv_listener->sock->err_mask)) {
- recv_listener->match_count++;
- rtcan_rcv_deliver(recv_listener, skb);
- }
- recv_listener = recv_listener->next;
- }
- } else {
- dev->rx_count++;
- while (recv_listener != NULL) {
- if (rtcan_accept_msg(frame->can_id, &recv_listener->can_filter)) {
- recv_listener->match_count++;
- rtcan_rcv_deliver(recv_listener, skb);
- }
- recv_listener = recv_listener->next;
- }
- }
- }
- #ifdef CONFIG_XENO_DRIVERS_CAN_LOOPBACK
- void rtcan_tx_push(struct rtcan_device *dev, struct rtcan_socket *sock,
- can_frame_t *frame)
- {
- struct rtcan_rb_frame *rb_frame = &dev->tx_skb.rb_frame;
- RTCAN_ASSERT(dev->tx_socket == 0,
- rtdm_printk("(%d) TX skb still in use", dev->ifindex););
- rb_frame->can_id = frame->can_id;
- rb_frame->can_dlc = frame->can_dlc;
- dev->tx_skb.rb_frame_size = EMPTY_RB_FRAME_SIZE;
- if (frame->can_dlc && !(frame->can_id & CAN_RTR_FLAG)) {
- memcpy(rb_frame->data, frame->data, frame->can_dlc);
- dev->tx_skb.rb_frame_size += frame->can_dlc;
- }
- rb_frame->can_ifindex = dev->ifindex;
- dev->tx_socket = sock;
- }
- void rtcan_loopback(struct rtcan_device *dev)
- {
- nanosecs_abs_t timestamp = rtdm_clock_read();
- /* Entry in reception list, begin with head */
- struct rtcan_recv *recv_listener = dev->recv_list;
- struct rtcan_rb_frame *frame = &dev->tx_skb.rb_frame;
- memcpy((void *)&dev->tx_skb.rb_frame + dev->tx_skb.rb_frame_size,
- ×tamp, RTCAN_TIMESTAMP_SIZE);
- while (recv_listener != NULL) {
- dev->rx_count++;
- if ((dev->tx_socket != recv_listener->sock) &&
- rtcan_accept_msg(frame->can_id, &recv_listener->can_filter)) {
- recv_listener->match_count++;
- rtcan_rcv_deliver(recv_listener, &dev->tx_skb);
- }
- recv_listener = recv_listener->next;
- }
- dev->tx_socket = NULL;
- }
- EXPORT_SYMBOL_GPL(rtcan_loopback);
- #endif /* CONFIG_XENO_DRIVERS_CAN_LOOPBACK */
- int rtcan_raw_socket(struct rtdm_dev_context *context,
- rtdm_user_info_t *user_info, int protocol)
- {
- /* Only protocol CAN_RAW is supported */
- if (protocol != CAN_RAW && protocol != 0)
- return -EPROTONOSUPPORT;
- rtcan_socket_init(context);
- return 0;
- }
- static inline void rtcan_raw_unbind(struct rtcan_socket *sock)
- {
- rtcan_raw_remove_filter(sock);
- if (!rtcan_flist_no_filter(sock->flist) && sock->flist)
- rtdm_free(sock->flist);
- sock->flist = NULL;
- sock->flistlen = RTCAN_SOCK_UNBOUND;
- atomic_set(&sock->ifindex, 0);
- }
- static int rtcan_raw_close(struct rtdm_dev_context *context,
- rtdm_user_info_t *user_info)
- {
- struct rtcan_socket *sock =
- (struct rtcan_socket *)&context->dev_private;
- rtdm_lockctx_t lock_ctx;
- /* Get lock for reception lists */
- rtdm_lock_get_irqsave(&rtcan_recv_list_lock, lock_ctx);
- /* Check if socket is bound */
- if (rtcan_sock_is_bound(sock))
- rtcan_raw_unbind(sock);
- rtdm_lock_put_irqrestore(&rtcan_recv_list_lock, lock_ctx);
- rtcan_socket_cleanup(context);
- return 0;
- }
- int rtcan_raw_bind(struct rtdm_dev_context *context,
- struct sockaddr_can *scan)
- {
- struct rtcan_socket *sock =
- (struct rtcan_socket *)&context->dev_private;
- rtdm_lockctx_t lock_ctx;
- int ret = 0;
- /* Check address family and
- check if given length of filter list is plausible */
- if (scan->can_family != AF_CAN)
- return -EINVAL;
- /* Check range of ifindex, must be between 0 and RTCAN_MAX_DEVICES */
- if (scan->can_ifindex < 0 || scan->can_ifindex > RTCAN_MAX_DEVICES)
- return -ENODEV;
- /* Get lock for reception lists */
- rtdm_lock_get_irqsave(&rtcan_recv_list_lock, lock_ctx);
- /* Test if socket is about to be closed */
- if (unlikely(test_bit(RTDM_CLOSING, &context->context_flags))) {
- ret = -EBADF;
- goto out;
- }
- if ((ret = rtcan_raw_check_filter(sock, scan->can_ifindex,
- sock->flist)))
- goto out;
- rtcan_raw_remove_filter(sock);
- /* Add filter and mark socket as bound */
- sock->flistlen = rtcan_raw_add_filter(sock, scan->can_ifindex);
- /* Set new interface index the socket is now bound to */
- atomic_set(&sock->ifindex, scan->can_ifindex);
- out:
- rtdm_lock_put_irqrestore(&rtcan_recv_list_lock, lock_ctx);
- return ret;
- }
- static int rtcan_raw_setsockopt(struct rtdm_dev_context *context,
- rtdm_user_info_t *user_info,
- struct _rtdm_setsockopt_args *so)
- {
- struct rtcan_socket *sock = (struct rtcan_socket *)&context->dev_private;
- struct rtcan_filter_list *flist;
- int ifindex = atomic_read(&sock->ifindex);
- rtdm_lockctx_t lock_ctx;
- can_err_mask_t err_mask;
- int val, ret = 0;
- if (so->level != SOL_CAN_RAW)
- return -ENOPROTOOPT;
- switch (so->optname) {
- case CAN_RAW_FILTER:
- if (so->optlen == 0) {
- flist = RTCAN_FLIST_NO_FILTER;
- } else {
- int flistlen;
- flistlen = so->optlen / sizeof(struct can_filter);
- if (flistlen < 1 || flistlen > RTCAN_MAX_RECEIVERS ||
- so->optlen % sizeof(struct can_filter) != 0)
- return -EINVAL;
- flist = (struct rtcan_filter_list *)rtdm_malloc(so->optlen + sizeof(int));
- if (flist == NULL)
- return -ENOMEM;
- if (user_info) {
- if (!rtdm_read_user_ok(user_info, so->optval, so->optlen) ||
- rtdm_copy_from_user(user_info, flist->flist,
- so->optval, so->optlen)) {
- rtdm_free(flist);
- return -EFAULT;
- }
- } else
- memcpy(flist->flist, so->optval, so->optlen);
- flist->flistlen = flistlen;
- }
- /* Get lock for reception lists */
- rtdm_lock_get_irqsave(&rtcan_recv_list_lock, lock_ctx);
- /* Check if there is space for the filter list if already bound */
- if (rtcan_sock_is_bound(sock)) {
- if (!rtcan_flist_no_filter(flist) &&
- (ret = rtcan_raw_check_filter(sock, ifindex, flist))) {
- rtdm_free(flist);
- goto out_filter;
- }
- rtcan_raw_remove_filter(sock);
- }
- /* Remove previous list and attach the new one */
- if (!rtcan_flist_no_filter(flist) && sock->flist)
- rtdm_free(sock->flist);
- sock->flist = flist;
- if (rtcan_sock_is_bound(sock))
- sock->flistlen = rtcan_raw_add_filter(sock, ifindex);
- out_filter:
- /* Release lock for reception lists */
- rtdm_lock_put_irqrestore(&rtcan_recv_list_lock, lock_ctx);
- break;
- case CAN_RAW_ERR_FILTER:
- if (so->optlen != sizeof(can_err_mask_t))
- return -EINVAL;
- if (user_info) {
- if (!rtdm_read_user_ok(user_info, so->optval, so->optlen) ||
- rtdm_copy_from_user(user_info, &err_mask, so->optval, so->optlen))
- return -EFAULT;
- } else
- memcpy(&err_mask, so->optval, so->optlen);
- /* Get lock for reception lists */
- rtdm_lock_get_irqsave(&rtcan_recv_list_lock, lock_ctx);
- sock->err_mask = err_mask;
- rtdm_lock_put_irqrestore(&rtcan_recv_list_lock, lock_ctx);
- break;
- case CAN_RAW_LOOPBACK:
- if (so->optlen != sizeof(int))
- return -EINVAL;
- if (user_info) {
- if (!rtdm_read_user_ok(user_info, so->optval, so->optlen) ||
- rtdm_copy_from_user(user_info, &val, so->optval, so->optlen))
- return -EFAULT;
- } else
- memcpy(&val, so->optval, so->optlen);
- #ifdef CONFIG_XENO_DRIVERS_CAN_LOOPBACK
- sock->loopback = val;
- #else
- if (val)
- return -EOPNOTSUPP;
- #endif
- break;
- default:
- ret = -ENOPROTOOPT;
- }
- return ret;
- }
- int rtcan_raw_ioctl(struct rtdm_dev_context *context,
- rtdm_user_info_t *user_info,
- unsigned int request, void *arg)
- {
- int ret = 0;
- switch (request) {
- case _RTIOC_BIND: {
- struct _rtdm_setsockaddr_args *setaddr, setaddr_buf;
- struct sockaddr_can *sockaddr, sockaddr_buf;
- if (user_info) {
- /* Copy argument structure from userspace */
- if (!rtdm_read_user_ok(user_info, arg,
- sizeof(struct _rtdm_setsockaddr_args)) ||
- rtdm_copy_from_user(user_info, &setaddr_buf, arg,
- sizeof(struct _rtdm_setsockaddr_args)))
- return -EFAULT;
- setaddr = &setaddr_buf;
- /* Check size */
- if (setaddr->addrlen != sizeof(struct sockaddr_can))
- return -EINVAL;
- /* Copy argument structure from userspace */
- if (!rtdm_read_user_ok(user_info, arg,
- sizeof(struct sockaddr_can)) ||
- rtdm_copy_from_user(user_info, &sockaddr_buf, setaddr->addr,
- sizeof(struct sockaddr_can)))
- return -EFAULT;
- sockaddr = &sockaddr_buf;
- } else {
- setaddr = (struct _rtdm_setsockaddr_args *)arg;
- sockaddr = (struct sockaddr_can *)setaddr->addr;
- }
- /* Now, all required data are in kernel space */
- ret = rtcan_raw_bind(context, sockaddr);
- break;
- }
- case _RTIOC_SETSOCKOPT: {
- struct _rtdm_setsockopt_args *setopt;
- struct _rtdm_setsockopt_args setopt_buf;
- if (user_info) {
- if (!rtdm_read_user_ok(user_info, arg,
- sizeof(struct _rtdm_setsockopt_args)) ||
- rtdm_copy_from_user(user_info, &setopt_buf, arg,
- sizeof(struct _rtdm_setsockopt_args)))
- return -EFAULT;
- setopt = &setopt_buf;
- } else
- setopt = (struct _rtdm_setsockopt_args *)arg;
- return rtcan_raw_setsockopt(context, user_info, setopt);
- }
- case RTCAN_RTIOC_TAKE_TIMESTAMP: {
- long timestamp_switch = (long)arg;
- if (timestamp_switch == RTCAN_TAKE_TIMESTAMPS)
- set_bit(RTCAN_GET_TIMESTAMP, &context->context_flags);
- else
- clear_bit(RTCAN_GET_TIMESTAMP, &context->context_flags);
- break;
- }
- case RTCAN_RTIOC_RCV_TIMEOUT:
- case RTCAN_RTIOC_SND_TIMEOUT: {
- /* Do some work these requests have in common. */
- struct rtcan_socket *sock =
- (struct rtcan_socket *)&context->dev_private;
- nanosecs_rel_t *timeout = (nanosecs_rel_t *)arg;
- nanosecs_rel_t timeo_buf;
- if (user_info) {
- /* Copy 64 bit timeout value from userspace */
- if (!rtdm_read_user_ok(user_info, arg,
- sizeof(nanosecs_rel_t)) ||
- rtdm_copy_from_user(user_info, &timeo_buf,
- arg, sizeof(nanosecs_rel_t)))
- return -EFAULT;
- timeout = &timeo_buf;
- }
- /* Now the differences begin between the requests. */
- if (request == RTCAN_RTIOC_RCV_TIMEOUT)
- sock->rx_timeout = *timeout;
- else
- sock->tx_timeout = *timeout;
- break;
- }
- default:
- ret = rtcan_raw_ioctl_dev(context, user_info, request, arg);
- break;
- }
- return ret;
- }
- #define MEMCPY_FROM_RING_BUF(to, len) \
- \
- if (unlikely((recv_buf_index + len) > RTCAN_RXBUF_SIZE)) { \
- /* Wrap around end of buffer */ \
- \
- first_part_size = RTCAN_RXBUF_SIZE - recv_buf_index; \
- \
- memcpy(to, &recv_buf[recv_buf_index], first_part_size); \
- memcpy((void *)to + first_part_size, recv_buf, \
- len - first_part_size); \
- \
- } else \
- \
- memcpy(to, &recv_buf[recv_buf_index], len); \
- \
- \
- recv_buf_index = (recv_buf_index + len) & (RTCAN_RXBUF_SIZE - 1);
- ssize_t rtcan_raw_recvmsg(struct rtdm_dev_context *context,
- rtdm_user_info_t *user_info,
- struct msghdr *msg, int flags)
- {
- struct rtcan_socket *sock =
- (struct rtcan_socket *)&context->dev_private;
- struct sockaddr_can scan;
- nanosecs_rel_t timeout;
- struct iovec *iov = (struct iovec *)msg->msg_iov;
- struct iovec iov_buf;
- can_frame_t frame;
- nanosecs_abs_t timestamp = 0;
- unsigned char ifindex;
- unsigned char can_dlc;
- unsigned char *recv_buf;
- int recv_buf_index;
- size_t first_part_size;
- size_t payload_size;
- rtdm_lockctx_t lock_ctx;
- int ret;
- /* Clear frame memory location */
- memset(&frame, 0, sizeof(can_frame_t));
- /* Check flags */
- if (flags & ~(MSG_DONTWAIT | MSG_PEEK))
- return -EINVAL;
- /* Check if msghdr entries are sane */
- if (msg->msg_name != NULL) {
- if (msg->msg_namelen < sizeof(struct sockaddr_can))
- return -EINVAL;
- if (user_info) {
- if (!rtdm_rw_user_ok(user_info, msg->msg_name, msg->msg_namelen))
- return -EFAULT;
- }
- } else {
- if (msg->msg_namelen != 0)
- return -EINVAL;
- }
- /* Check msg_iovlen, only one buffer allowed */
- if (msg->msg_iovlen != 1)
- return -EMSGSIZE;
- if (user_info) {
- /* Copy IO vector from userspace */
- if (!rtdm_rw_user_ok(user_info, msg->msg_iov,
- sizeof(struct iovec)) ||
- rtdm_copy_from_user(user_info, &iov_buf, msg->msg_iov,
- sizeof(struct iovec)))
- return -EFAULT;
- iov = &iov_buf;
- }
- /* Check size of buffer */
- if (iov->iov_len < sizeof(can_frame_t))
- return -EMSGSIZE;
- /* Check buffer if in user space */
- if (user_info) {
- if (!rtdm_rw_user_ok(user_info, iov->iov_base, iov->iov_len))
- return -EFAULT;
- }
- if (msg->msg_control != NULL) {
- if (msg->msg_controllen < sizeof(nanosecs_abs_t))
- return -EINVAL;
- if (user_info) {
- if (!rtdm_rw_user_ok(user_info, msg->msg_control,
- msg->msg_controllen))
- return -EFAULT;
- }
- } else {
- if (msg->msg_controllen != 0)
- return -EINVAL;
- }
- rtcan_raw_enable_bus_err(sock);
- /* Set RX timeout */
- timeout = (flags & MSG_DONTWAIT) ? RTDM_TIMEOUT_NONE : sock->rx_timeout;
- /* Fetch message (ok, try it ...) */
- ret = rtdm_sem_timeddown(&sock->recv_sem, timeout, NULL);
- /* Error code returned? */
- if (unlikely(ret)) {
- /* Which error code? */
- if (ret == -EIDRM)
- /* Socket was closed */
- return -EBADF;
- else if (ret == -EWOULDBLOCK)
- /* We would block but don't want to */
- return -EAGAIN;
- else
- /* Return all other error codes unmodified. */
- return ret;
- }
- /* OK, we've got mail. */
- rtdm_lock_get_irqsave(&rtcan_socket_lock, lock_ctx);
- /* Construct a struct can_frame with data from socket's ring buffer */
- recv_buf_index = sock->recv_head;
- recv_buf = sock->recv_buf;
- /* Begin with CAN ID */
- MEMCPY_FROM_RING_BUF(&frame.can_id, sizeof(uint32_t));
- /* Fetch interface index */
- ifindex = recv_buf[recv_buf_index];
- recv_buf_index = (recv_buf_index + 1) & (RTCAN_RXBUF_SIZE - 1);
- /* Fetch DLC (with indicator if a timestamp exists) */
- can_dlc = recv_buf[recv_buf_index];
- recv_buf_index = (recv_buf_index + 1) & (RTCAN_RXBUF_SIZE - 1);
- frame.can_dlc = can_dlc & RTCAN_HAS_NO_TIMESTAMP;
- payload_size = (frame.can_dlc > 8) ? 8 : frame.can_dlc;
- /* If frame is an RTR or one with no payload it's not necessary
- * to copy the data bytes. */
- if (!(frame.can_id & CAN_RTR_FLAG) && payload_size) {
- /* Copy data bytes */
- MEMCPY_FROM_RING_BUF(frame.data, payload_size);
- }
- /* Is a timestamp available and is the caller actually interested? */
- if (msg->msg_controllen && (can_dlc & RTCAN_HAS_TIMESTAMP)) {
- /* Copy timestamp */
- MEMCPY_FROM_RING_BUF(×tamp, RTCAN_TIMESTAMP_SIZE);
- }
- /* Message completely read from the socket's ring buffer. Now check if
- * caller is just peeking. */
- if (flags & MSG_PEEK)
- /* Next one, please! */
- rtdm_sem_up(&sock->recv_sem);
- else
- /* Adjust begin of first message in the ring buffer. */
- sock->recv_head = recv_buf_index;
- /* Release lock */
- rtdm_lock_put_irqrestore(&rtcan_socket_lock, lock_ctx);
- /* Create CAN socket address to give back */
- if (msg->msg_namelen) {
- scan.can_family = AF_CAN;
- scan.can_ifindex = ifindex;
- }
- /* Last duty: Copy all back to the caller's buffers. */
- if (user_info) {
- /* Copy to user space */
- /* Copy socket address */
- if (msg->msg_namelen) {
- if (rtdm_copy_to_user(user_info, msg->msg_name, &scan,
- sizeof(struct sockaddr_can)))
- return -EFAULT;
- msg->msg_namelen = sizeof(struct sockaddr_can);
- }
- /* Copy CAN frame */
- if (rtdm_copy_to_user(user_info, iov->iov_base, &frame,
- sizeof(can_frame_t)))
- return -EFAULT;
- /* Adjust iovec in the common way */
- iov->iov_base += sizeof(can_frame_t);
- iov->iov_len -= sizeof(can_frame_t);
- /* ... and copy it, too. */
- if (rtdm_copy_to_user(user_info, msg->msg_iov, iov,
- sizeof(struct iovec)))
- return -EFAULT;
- /* Copy timestamp if existent and wanted */
- if (msg->msg_controllen) {
- if (can_dlc & RTCAN_HAS_TIMESTAMP) {
- if (rtdm_copy_to_user(user_info, msg->msg_control,
- ×tamp, RTCAN_TIMESTAMP_SIZE))
- return -EFAULT;
- msg->msg_controllen = RTCAN_TIMESTAMP_SIZE;
- } else
- msg->msg_controllen = 0;
- }
- } else {
- /* Kernel space */
- /* Copy socket address */
- if (msg->msg_namelen) {
- memcpy(msg->msg_name, &scan, sizeof(struct sockaddr_can));
- msg->msg_namelen = sizeof(struct sockaddr_can);
- }
- /* Copy CAN frame */
- memcpy(iov->iov_base, &frame, sizeof(can_frame_t));
- /* Adjust iovec in the common way */
- iov->iov_base += sizeof(can_frame_t);
- iov->iov_len -= sizeof(can_frame_t);
- /* Copy timestamp if existent and wanted */
- if (msg->msg_controllen) {
- if (can_dlc & RTCAN_HAS_TIMESTAMP) {
- memcpy(msg->msg_control, ×tamp, RTCAN_TIMESTAMP_SIZE);
- msg->msg_controllen = RTCAN_TIMESTAMP_SIZE;
- } else
- msg->msg_controllen = 0;
- }
- }
- return sizeof(can_frame_t);
- }
- ssize_t rtcan_raw_sendmsg(struct rtdm_dev_context *context,
- rtdm_user_info_t *user_info,
- const struct msghdr *msg, int flags)
- {
- struct rtcan_socket *sock =
- (struct rtcan_socket *)&context->dev_private;
- struct sockaddr_can *scan = (struct sockaddr_can *)msg->msg_name;
- struct sockaddr_can scan_buf;
- struct iovec *iov = (struct iovec *)msg->msg_iov;
- struct iovec iov_buf;
- can_frame_t *frame;
- can_frame_t frame_buf;
- rtdm_lockctx_t lock_ctx;
- nanosecs_rel_t timeout = 0;
- struct tx_wait_queue tx_wait;
- struct rtcan_device *dev;
- int ifindex = 0;
- int ret = 0;
- if (flags & MSG_OOB) /* Mirror BSD error message compatibility */
- return -EOPNOTSUPP;
- /* Only MSG_DONTWAIT is a valid flag. */
- if (flags & ~MSG_DONTWAIT)
- return -EINVAL;
- /* Check msg_iovlen, only one buffer allowed */
- if (msg->msg_iovlen != 1)
- return -EMSGSIZE;
- if (scan == NULL) {
- /* No socket address. Will use bound interface for sending */
- if (msg->msg_namelen != 0)
- return -EINVAL;
- /* We only want a consistent value here, a spin lock would be
- * overkill. Nevertheless, the binding could change till we have
- * the chance to send. Blame the user, though. */
- ifindex = atomic_read(&sock->ifindex);
- if (!ifindex)
- /* Socket isn't bound or bound to all interfaces. Go out. */
- return -ENXIO;
- } else {
- /* Socket address given */
- if (msg->msg_namelen < sizeof(struct sockaddr_can))
- return -EINVAL;
- if (user_info) {
- /* Copy socket address from userspace */
- if (!rtdm_read_user_ok(user_info, msg->msg_name,
- sizeof(struct sockaddr_can)) ||
- rtdm_copy_from_user(user_info, &scan_buf, msg->msg_name,
- sizeof(struct sockaddr_can)))
- return -EFAULT;
- scan = &scan_buf;
- }
- /* Check address family */
- if (scan->can_family != AF_CAN)
- return -EINVAL;
- ifindex = scan->can_ifindex;
- }
- if (user_info) {
- /* Copy IO vector from userspace */
- if (!rtdm_rw_user_ok(user_info, msg->msg_iov,
- sizeof(struct iovec)) ||
- rtdm_copy_from_user(user_info, &iov_buf, msg->msg_iov,
- sizeof(struct iovec)))
- return -EFAULT;
- iov = &iov_buf;
- }
- /* Check size of buffer */
- if (iov->iov_len != sizeof(can_frame_t))
- return -EMSGSIZE;
- frame = (can_frame_t *)iov->iov_base;
- if (user_info) {
- /* Copy CAN frame from userspace */
- if (!rtdm_read_user_ok(user_info, iov->iov_base,
- sizeof(can_frame_t)) ||
- rtdm_copy_from_user(user_info, &frame_buf, iov->iov_base,
- sizeof(can_frame_t)))
- return -EFAULT;
- frame = &frame_buf;
- }
- /* Adjust iovec in the common way */
- iov->iov_base += sizeof(can_frame_t);
- iov->iov_len -= sizeof(can_frame_t);
- /* ... and copy it back to userspace if necessary */
- if (user_info) {
- if (rtdm_copy_to_user(user_info, msg->msg_iov, iov,
- sizeof(struct iovec)))
- return -EFAULT;
- }
- /* At last, we've got the frame ... */
- /* Check if DLC between 0 and 15 */
- if (frame->can_dlc > 15)
- return -EINVAL;
- /* Check if it is a standard frame and the ID between 0 and 2031 */
- if (!(frame->can_id & CAN_EFF_FLAG)) {
- u32 id = frame->can_id & CAN_EFF_MASK;
- if (id > (CAN_SFF_MASK - 16))
- return -EINVAL;
- }
- if ((dev = rtcan_dev_get_by_index(ifindex)) == NULL)
- return -ENXIO;
- timeout = (flags & MSG_DONTWAIT) ? RTDM_TIMEOUT_NONE : sock->tx_timeout;
- tx_wait.rt_task = rtdm_task_current();
- /* If socket was not closed recently, register the task at the
- * socket's TX wait queue and decrement the TX semaphore. This must be
- * atomic. Finally, the task must be deregistered again (also atomic). */
- RTDM_EXECUTE_ATOMICALLY(
- if (likely(!test_bit(RTDM_CLOSING, &context->context_flags))) {
- list_add(&tx_wait.tx_wait_list, &sock->tx_wait_head);
- /* Try to pass the guard in order to access the controller */
- ret = rtdm_sem_timeddown(&dev->tx_sem, timeout, NULL);
- /* Only dequeue task again if socket isn't being closed i.e. if
- * this task was not unblocked within the close() function. */
- if (likely(tx_wait.tx_wait_list.next != LIST_POISON1))
- /* Dequeue this task from the TX wait queue */
- list_del(&tx_wait.tx_wait_list);
- else
- /* The socket was closed. */
- ret = -EBADF;
- } else
- /* The socket was closed. */
- ret = -EBADF;
- );
- /* Error code returned? */
- if (ret != 0) {
- /* Which error code? */
- switch (ret) {
- case -EIDRM:
- /* Controller is stopped or bus-off */
- ret = -ENETDOWN;
- goto send_out1;
- case -EWOULDBLOCK:
- /* We would block but don't want to */
- ret = -EAGAIN;
- goto send_out1;
- default:
- /* Return all other error codes unmodified. */
- goto send_out1;
- }
- }
- /* We got access */
- /* Push message onto stack for loopback when TX done */
- if (rtcan_loopback_enabled(sock))
- rtcan_tx_push(dev, sock, frame);
- rtdm_lock_get_irqsave(&dev->device_lock, lock_ctx);
- /* Controller should be operating */
- if (!CAN_STATE_OPERATING(dev->state)) {
- if (dev->state == CAN_STATE_SLEEPING) {
- ret = -ECOMM;
- rtdm_lock_put_irqrestore(&dev->device_lock, lock_ctx);
- rtdm_sem_up(&dev->tx_sem);
- goto send_out1;
- }
- ret = -ENETDOWN;
- goto send_out2;
- }
- dev->tx_count++;
- ret = dev->hard_start_xmit(dev, frame);
- /* Return number of bytes sent upon successful completion */
- if (ret == 0)
- ret = sizeof(can_frame_t);
- send_out2:
- rtdm_lock_put_irqrestore(&dev->device_lock, lock_ctx);
- send_out1:
- rtcan_dev_dereference(dev);
- return ret;
- }
- static struct rtdm_device rtcan_proto_raw_dev = {
- struct_version: RTDM_DEVICE_STRUCT_VER,
- device_flags: RTDM_PROTOCOL_DEVICE,
- context_size: sizeof(struct rtcan_socket),
- protocol_family: PF_CAN,
- socket_type: SOCK_RAW,
- socket_nrt: rtcan_raw_socket,
- ops: {
- close_nrt: rtcan_raw_close,
- ioctl_rt: rtcan_raw_ioctl,
- ioctl_nrt: rtcan_raw_ioctl,
- read_rt: NULL,
- read_nrt: NULL,
- write_rt: NULL,
- write_nrt: NULL,
- recvmsg_rt: rtcan_raw_recvmsg,
- recvmsg_nrt: NULL,
- sendmsg_rt: rtcan_raw_sendmsg,
- sendmsg_nrt: NULL,
- },
- device_class: RTDM_CLASS_CAN,
- device_sub_class: RTDM_SUBCLASS_GENERIC,
- profile_version: RTCAN_PROFILE_VER,
- driver_name: "xeno_can",
- driver_version: RTDM_DRIVER_VER(RTCAN_MAJOR_VER,
- RTCAN_MINOR_VER,
- RTCAN_BUGFIX_VER),
- peripheral_name: "Real-Time CAN Raw Socket Interface",
- provider_name: "RT-Socket-CAN development team",
- proc_name: "rtcan"
- };
- int __init rtcan_raw_proto_register(void)
- {
- return rtdm_dev_register(&rtcan_proto_raw_dev);
- }
- void __exit rtcan_raw_proto_unregister(void)
- {
- rtdm_dev_unregister(&rtcan_proto_raw_dev, 1000);
- }
- EXPORT_SYMBOL_GPL(rtcan_rcv);