/***
|
*
|
* packet/af_packet.c
|
*
|
* RTnet - real-time networking subsystem
|
* Copyright (C) 2003-2006 Jan Kiszka <jan.kiszka@web.de>
|
* Copyright (C) 2006 Jorge Almeida <j-almeida@criticalsoftware.com>
|
*
|
* 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 <linux/module.h>
|
#include <linux/sched.h>
|
#include <linux/err.h>
|
|
#include <rtnet_iovec.h>
|
#include <rtnet_socket.h>
|
#include <stack_mgr.h>
|
|
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);
|
