/*** * * packet/af_packet.c * * RTnet - real-time networking subsystem * Copyright (C) 2003-2006 Jan Kiszka * Copyright (C) 2006 Jorge Almeida * * 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., 675 Mass Ave, Cambridge, MA 02139, USA. * */ #include #include #include #include #include #include MODULE_LICENSE("GPL"); /*** * rt_packet_rcv */ static int rt_packet_rcv(struct rtskb *skb, struct rtpacket_type *pt) { struct rtsocket *sock = container_of(pt, struct rtsocket, prot.packet.packet_type); int ifindex = sock->prot.packet.ifindex; void (*callback_func)(struct rtdm_fd *, void *); void *callback_arg; rtdm_lockctx_t context; if (unlikely((ifindex != 0) && (ifindex != skb->rtdev->ifindex))) return -EUNATCH; #ifdef CONFIG_XENO_DRIVERS_NET_ETH_P_ALL if (pt->type == htons(ETH_P_ALL)) { struct rtskb *clone_skb = rtskb_clone(skb, &sock->skb_pool); if (clone_skb == NULL) goto out; skb = clone_skb; } else #endif /* CONFIG_XENO_DRIVERS_NET_ETH_P_ALL */ if (unlikely(rtskb_acquire(skb, &sock->skb_pool) < 0)) { kfree_rtskb(skb); goto out; } rtskb_queue_tail(&sock->incoming, skb); rtdm_sem_up(&sock->pending_sem); rtdm_lock_get_irqsave(&sock->param_lock, context); callback_func = sock->callback_func; callback_arg = sock->callback_arg; rtdm_lock_put_irqrestore(&sock->param_lock, context); if (callback_func) callback_func(rt_socket_fd(sock), callback_arg); out: return 0; } static bool rt_packet_trylock(struct rtpacket_type *pt) { struct rtsocket *sock = container_of(pt, struct rtsocket, prot.packet.packet_type); struct rtdm_fd *fd = rtdm_private_to_fd(sock); if (rtdm_fd_lock(fd) < 0) return false; return true; } static void rt_packet_unlock(struct rtpacket_type *pt) { struct rtsocket *sock = container_of(pt, struct rtsocket, prot.packet.packet_type); struct rtdm_fd *fd = rtdm_private_to_fd(sock); rtdm_fd_unlock(fd); } /*** * rt_packet_bind */ static int rt_packet_bind(struct rtdm_fd *fd, struct rtsocket *sock, const struct sockaddr *addr, socklen_t addrlen) { struct sockaddr_ll _sll, *sll; struct rtpacket_type *pt = &sock->prot.packet.packet_type; int new_type; int ret; rtdm_lockctx_t context; if (addrlen < sizeof(struct sockaddr_ll)) return -EINVAL; sll = rtnet_get_arg(fd, &_sll, addr, sizeof(_sll)); if (IS_ERR(sll)) return PTR_ERR(sll); if (sll->sll_family != AF_PACKET) return -EINVAL; new_type = (sll->sll_protocol != 0) ? sll->sll_protocol : sock->protocol; rtdm_lock_get_irqsave(&sock->param_lock, context); /* release existing binding */ if (pt->type != 0) rtdev_remove_pack(pt); pt->type = new_type; sock->prot.packet.ifindex = sll->sll_ifindex; /* if protocol is non-zero, register the packet type */ if (new_type != 0) { pt->handler = rt_packet_rcv; pt->err_handler = NULL; pt->trylock = rt_packet_trylock; pt->unlock = rt_packet_unlock; ret = rtdev_add_pack(pt); } else ret = 0; rtdm_lock_put_irqrestore(&sock->param_lock, context); return ret; } /*** * rt_packet_getsockname */ static int rt_packet_getsockname(struct rtdm_fd *fd, struct rtsocket *sock, struct sockaddr *addr, socklen_t *addrlen) { struct sockaddr_ll _sll, *sll; struct rtnet_device *rtdev; rtdm_lockctx_t context; socklen_t _namelen, *namelen; int ret; namelen = rtnet_get_arg(fd, &_namelen, addrlen, sizeof(_namelen)); if (IS_ERR(namelen)) return PTR_ERR(namelen); if (*namelen < sizeof(struct sockaddr_ll)) return -EINVAL; sll = rtnet_get_arg(fd, &_sll, addr, sizeof(_sll)); if (IS_ERR(sll)) return PTR_ERR(sll); rtdm_lock_get_irqsave(&sock->param_lock, context); sll->sll_family = AF_PACKET; sll->sll_ifindex = sock->prot.packet.ifindex; sll->sll_protocol = sock->protocol; rtdm_lock_put_irqrestore(&sock->param_lock, context); rtdev = rtdev_get_by_index(sll->sll_ifindex); if (rtdev != NULL) { sll->sll_hatype = rtdev->type; sll->sll_halen = rtdev->addr_len; memcpy(sll->sll_addr, rtdev->dev_addr, rtdev->addr_len); rtdev_dereference(rtdev); } else { sll->sll_hatype = 0; sll->sll_halen = 0; } *namelen = sizeof(struct sockaddr_ll); ret = rtnet_put_arg(fd, addr, sll, sizeof(*sll)); if (ret) return ret; return rtnet_put_arg(fd, addrlen, namelen, sizeof(*namelen)); } /*** * rt_packet_socket - initialize a packet socket */ static int rt_packet_socket(struct rtdm_fd *fd, int protocol) { struct rtsocket *sock = rtdm_fd_to_private(fd); int ret; if ((ret = rt_socket_init(fd, protocol)) != 0) return ret; sock->prot.packet.packet_type.type = protocol; sock->prot.packet.ifindex = 0; sock->prot.packet.packet_type.trylock = rt_packet_trylock; sock->prot.packet.packet_type.unlock = rt_packet_unlock; /* if protocol is non-zero, register the packet type */ if (protocol != 0) { sock->prot.packet.packet_type.handler = rt_packet_rcv; sock->prot.packet.packet_type.err_handler = NULL; if ((ret = rtdev_add_pack(&sock->prot.packet.packet_type)) < 0) { rt_socket_cleanup(fd); return ret; } } return 0; } /*** * rt_packet_close */ static void rt_packet_close(struct rtdm_fd *fd) { struct rtsocket *sock = rtdm_fd_to_private(fd); struct rtpacket_type *pt = &sock->prot.packet.packet_type; struct rtskb *del; rtdm_lockctx_t context; rtdm_lock_get_irqsave(&sock->param_lock, context); if (pt->type != 0) { rtdev_remove_pack(pt); pt->type = 0; } rtdm_lock_put_irqrestore(&sock->param_lock, context); /* free packets in incoming queue */ while ((del = rtskb_dequeue(&sock->incoming)) != NULL) { kfree_rtskb(del); } rt_socket_cleanup(fd); } /*** * rt_packet_ioctl */ static int rt_packet_ioctl(struct rtdm_fd *fd, unsigned int request, void __user *arg) { struct rtsocket *sock = rtdm_fd_to_private(fd); const struct _rtdm_setsockaddr_args *setaddr; struct _rtdm_setsockaddr_args _setaddr; const struct _rtdm_getsockaddr_args *getaddr; struct _rtdm_getsockaddr_args _getaddr; /* fast path for common socket IOCTLs */ if (_IOC_TYPE(request) == RTIOC_TYPE_NETWORK) return rt_socket_common_ioctl(fd, request, arg); switch (request) { case _RTIOC_BIND: setaddr = rtnet_get_arg(fd, &_setaddr, arg, sizeof(_setaddr)); if (IS_ERR(setaddr)) return PTR_ERR(setaddr); return rt_packet_bind(fd, sock, setaddr->addr, setaddr->addrlen); case _RTIOC_GETSOCKNAME: getaddr = rtnet_get_arg(fd, &_getaddr, arg, sizeof(_getaddr)); if (IS_ERR(getaddr)) return PTR_ERR(getaddr); return rt_packet_getsockname(fd, sock, getaddr->addr, getaddr->addrlen); default: return rt_socket_if_ioctl(fd, request, arg); } } /*** * rt_packet_recvmsg */ static ssize_t rt_packet_recvmsg(struct rtdm_fd *fd, struct user_msghdr *msg, int msg_flags) { struct rtsocket *sock = rtdm_fd_to_private(fd); ssize_t len; size_t copy_len; struct rtskb *rtskb; struct sockaddr_ll sll; int ret; nanosecs_rel_t timeout = sock->timeout; socklen_t namelen; struct iovec iov_fast[RTDM_IOV_FASTMAX], *iov; if (msg->msg_iovlen < 0) return -EINVAL; if (msg->msg_iovlen == 0) return 0; ret = rtdm_get_iovec(fd, &iov, msg, iov_fast); if (ret) return ret; /* non-blocking receive? */ if (msg_flags & MSG_DONTWAIT) timeout = -1; ret = rtdm_sem_timeddown(&sock->pending_sem, timeout, NULL); if (unlikely(ret < 0)) switch (ret) { default: ret = -EBADF; /* socket has been closed */ fallthrough; case -EWOULDBLOCK: case -ETIMEDOUT: case -EINTR: rtdm_drop_iovec(iov, iov_fast); return ret; } rtskb = rtskb_dequeue_chain(&sock->incoming); RTNET_ASSERT(rtskb != NULL, return -EFAULT;); /* copy the address if required. */ if (msg->msg_name) { struct rtnet_device *rtdev = rtskb->rtdev; memset(&sll, 0, sizeof(sll)); sll.sll_family = AF_PACKET; sll.sll_hatype = rtdev->type; sll.sll_protocol = rtskb->protocol; sll.sll_pkttype = rtskb->pkt_type; sll.sll_ifindex = rtdev->ifindex; if (msg->msg_namelen < 0) { ret = -EINVAL; goto fail; } namelen = min(sizeof(sll), (size_t)msg->msg_namelen); /* Ethernet specific - we rather need some parse handler here */ memcpy(sll.sll_addr, rtskb->mac.ethernet->h_source, ETH_ALEN); sll.sll_halen = ETH_ALEN; ret = rtnet_put_arg(fd, msg->msg_name, &sll, namelen); if (ret) goto fail; msg->msg_namelen = sizeof(sll); } /* Include the header in raw delivery */ if (rtdm_fd_to_context(fd)->device->driver->socket_type != SOCK_DGRAM) rtskb_push(rtskb, rtskb->data - rtskb->mac.raw); /* The data must not be longer than the available buffer size */ copy_len = rtskb->len; len = rtdm_get_iov_flatlen(iov, msg->msg_iovlen); if (len < 0) { copy_len = len; goto out; } if (copy_len > len) { copy_len = len; msg->msg_flags |= MSG_TRUNC; } copy_len = rtnet_write_to_iov(fd, iov, msg->msg_iovlen, rtskb->data, copy_len); out: if ((msg_flags & MSG_PEEK) == 0) { kfree_rtskb(rtskb); } else { rtskb_queue_head(&sock->incoming, rtskb); rtdm_sem_up(&sock->pending_sem); } rtdm_drop_iovec(iov, iov_fast); return copy_len; fail: copy_len = ret; goto out; } /*** * rt_packet_sendmsg */ static ssize_t rt_packet_sendmsg(struct rtdm_fd *fd, const struct user_msghdr *msg, int msg_flags) { struct rtsocket *sock = rtdm_fd_to_private(fd); size_t len; struct sockaddr_ll _sll, *sll; struct rtnet_device *rtdev; struct rtskb *rtskb; unsigned short proto; unsigned char *addr; int ifindex; ssize_t ret; struct iovec iov_fast[RTDM_IOV_FASTMAX], *iov; if (msg_flags & MSG_OOB) /* Mirror BSD error message compatibility */ return -EOPNOTSUPP; if (msg_flags & ~MSG_DONTWAIT) return -EINVAL; if (msg->msg_iovlen < 0) return -EINVAL; if (msg->msg_iovlen == 0) return 0; ret = rtdm_get_iovec(fd, &iov, msg, iov_fast); if (ret) return ret; if (msg->msg_name == NULL) { /* Note: We do not care about races with rt_packet_bind here - the user has to do so. */ ifindex = sock->prot.packet.ifindex; proto = sock->prot.packet.packet_type.type; addr = NULL; sll = NULL; } else { sll = rtnet_get_arg(fd, &_sll, msg->msg_name, sizeof(_sll)); if (IS_ERR(sll)) { ret = PTR_ERR(sll); goto abort; } if ((msg->msg_namelen < sizeof(struct sockaddr_ll)) || (msg->msg_namelen < (sll->sll_halen + offsetof(struct sockaddr_ll, sll_addr))) || ((sll->sll_family != AF_PACKET) && (sll->sll_family != AF_UNSPEC))) { ret = -EINVAL; goto abort; } ifindex = sll->sll_ifindex; proto = sll->sll_protocol; addr = sll->sll_addr; } if ((rtdev = rtdev_get_by_index(ifindex)) == NULL) { ret = -ENODEV; goto abort; } len = rtdm_get_iov_flatlen(iov, msg->msg_iovlen); rtskb = alloc_rtskb(rtdev->hard_header_len + len, &sock->skb_pool); if (rtskb == NULL) { ret = -ENOBUFS; goto out; } /* If an RTmac discipline is active, this becomes a pure sanity check to avoid writing beyond rtskb boundaries. The hard check is then performed upon rtdev_xmit() by the discipline's xmit handler. */ if (len > rtdev->mtu + ((rtdm_fd_to_context(fd)->device->driver->socket_type == SOCK_RAW) ? rtdev->hard_header_len : 0)) { ret = -EMSGSIZE; goto err; } if ((sll != NULL) && (sll->sll_halen != rtdev->addr_len)) { ret = -EINVAL; goto err; } rtskb_reserve(rtskb, rtdev->hard_header_len); rtskb->rtdev = rtdev; rtskb->priority = sock->priority; if (rtdev->hard_header) { int hdr_len; ret = -EINVAL; hdr_len = rtdev->hard_header(rtskb, rtdev, ntohs(proto), addr, NULL, len); if (rtdm_fd_to_context(fd)->device->driver->socket_type != SOCK_DGRAM) { rtskb->tail = rtskb->data; rtskb->len = 0; } else if (hdr_len < 0) goto err; } ret = rtnet_read_from_iov(fd, iov, msg->msg_iovlen, rtskb_put(rtskb, len), len); if ((rtdev->flags & IFF_UP) != 0) { if ((ret = rtdev_xmit(rtskb)) == 0) ret = len; } else { ret = -ENETDOWN; goto err; } out: rtdev_dereference(rtdev); abort: rtdm_drop_iovec(iov, iov_fast); return ret; err: kfree_rtskb(rtskb); goto out; } static struct rtdm_driver packet_proto_drv = { .profile_info = RTDM_PROFILE_INFO(packet, RTDM_CLASS_NETWORK, RTDM_SUBCLASS_RTNET, RTNET_RTDM_VER), .device_flags = RTDM_PROTOCOL_DEVICE, .device_count = 1, .context_size = sizeof(struct rtsocket), .protocol_family = PF_PACKET, .socket_type = SOCK_DGRAM, .ops = { .socket = rt_packet_socket, .close = rt_packet_close, .ioctl_rt = rt_packet_ioctl, .ioctl_nrt = rt_packet_ioctl, .recvmsg_rt = rt_packet_recvmsg, .sendmsg_rt = rt_packet_sendmsg, .select = rt_socket_select_bind, }, }; static struct rtdm_device packet_proto_dev = { .driver = &packet_proto_drv, .label = "packet", }; static struct rtdm_driver raw_packet_proto_drv = { .profile_info = RTDM_PROFILE_INFO(raw_packet, RTDM_CLASS_NETWORK, RTDM_SUBCLASS_RTNET, RTNET_RTDM_VER), .device_flags = RTDM_PROTOCOL_DEVICE, .device_count = 1, .context_size = sizeof(struct rtsocket), .protocol_family = PF_PACKET, .socket_type = SOCK_RAW, .ops = { .socket = rt_packet_socket, .close = rt_packet_close, .ioctl_rt = rt_packet_ioctl, .ioctl_nrt = rt_packet_ioctl, .recvmsg_rt = rt_packet_recvmsg, .sendmsg_rt = rt_packet_sendmsg, .select = rt_socket_select_bind, }, }; static struct rtdm_device raw_packet_proto_dev = { .driver = &raw_packet_proto_drv, .label = "raw_packet", }; static int __init rt_packet_proto_init(void) { int err; err = rtdm_dev_register(&packet_proto_dev); if (err) return err; err = rtdm_dev_register(&raw_packet_proto_dev); if (err) rtdm_dev_unregister(&packet_proto_dev); return err; } static void rt_packet_proto_release(void) { rtdm_dev_unregister(&packet_proto_dev); rtdm_dev_unregister(&raw_packet_proto_dev); } module_init(rt_packet_proto_init); module_exit(rt_packet_proto_release); /********************************************************** * Utilities * **********************************************************/ static int hex2int(unsigned char hex_char) { if ((hex_char >= '0') && (hex_char <= '9')) return hex_char - '0'; else if ((hex_char >= 'a') && (hex_char <= 'f')) return hex_char - 'a' + 10; else if ((hex_char >= 'A') && (hex_char <= 'F')) return hex_char - 'A' + 10; else return -EINVAL; } int rt_eth_aton(unsigned char *addr_buf, const char *mac) { int i = 0; int nibble; while (1) { if (*mac == 0) return -EINVAL; if ((nibble = hex2int(*mac++)) < 0) return nibble; *addr_buf = nibble << 4; if (*mac == 0) return -EINVAL; if ((nibble = hex2int(*mac++)) < 0) return nibble; *addr_buf++ |= nibble; if (++i == 6) break; if ((*mac == 0) || (*mac++ != ':')) return -EINVAL; } return 0; } EXPORT_SYMBOL_GPL(rt_eth_aton);