/***
* rt_packet_recvmsg
*/
static ssize_t
rt_packet_recvmsg(struct rtdm_fd *fd, struct msghdr *msg, int msg_flags)
{
struct rtsocket *sock = rtdm_fd_to_private(fd);
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
size_t copy_len;
size_t real_len;
struct rtskb *rtskb;
struct sockaddr_ll *sll;
int ret;
nanosecs_rel_t timeout = sock->timeout;
/* 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) {
case -EWOULDBLOCK:
case -ETIMEDOUT:
case -EINTR:
return ret;
default:
return -EBADF; /* socket has been closed */
}
rtskb = rtskb_dequeue_chain(&sock->incoming);
RTNET_ASSERT(rtskb != NULL, return -EFAULT;);
/***
* rt_packet_recvmsg
*/
ssize_t rt_packet_recvmsg(struct rtdm_dev_context *sockctx,
rtdm_user_info_t *user_info, struct msghdr *msg,
int msg_flags)
{
struct rtsocket *sock = (struct rtsocket *)&sockctx->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
size_t copy_len;
size_t real_len;
struct rtskb *skb;
struct ethhdr *eth;
struct sockaddr_ll *sll;
int ret;
nanosecs_t timeout = sock->timeout;
/* non-blocking receive? */
if (testbits(msg_flags, MSG_DONTWAIT))
timeout = -1;
ret = rtdm_sem_timeddown(&sock->pending_sem, timeout, NULL);
if (unlikely(ret < 0)) {
if ((ret != -EWOULDBLOCK) && (ret != -ETIMEDOUT))
ret = -EBADF; /* socket has been closed */
return ret;
}
skb = rtskb_dequeue_chain(&sock->incoming);
RTNET_ASSERT(skb != NULL, return -EFAULT;);
/***
* rt_udp_recvmsg
*/
ssize_t rt_udp_recvmsg(struct rtdm_dev_context *context,
rtdm_user_info_t *user_info, struct msghdr *msg,
int msg_flags)
{
struct rtsocket *sock = (struct rtsocket *)&context->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
struct rtskb *skb;
struct rtskb *first_skb;
size_t copied = 0;
size_t block_size;
size_t data_len;
struct udphdr *uh;
struct sockaddr_in *sin;
nanosecs_t timeout = sock->timeout;
int ret;
/* non-blocking receive? */
if (test_bit(RT_SOCK_NONBLOCK, &context->context_flags) ||
(msg_flags & MSG_DONTWAIT))
timeout = -1;
ret = rtos_sem_down(&sock->pending_sem, timeout);
if (unlikely(ret < 0)) {
if (ret == -EWOULDBLOCK)
ret = -EAGAIN;
else if (ret != -ETIMEDOUT)
ret = -ENOTSOCK;
return ret;
}
skb = rtskb_dequeue_chain(&sock->incoming);
RTNET_ASSERT(skb != NULL, return -EFAULT;);
/***
* rt_packet_sendmsg
*/
ssize_t rt_packet_sendmsg(struct rtdm_dev_context *context, int call_flags,
const struct msghdr *msg, int flags)
{
struct rtsocket *sock = (struct rtsocket *)&context->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
struct sockaddr_ll *sll = (struct sockaddr_ll*)msg->msg_name;
struct rtnet_device *rtdev;
struct rtskb *rtskb;
int ret = 0;
if (flags & MSG_OOB) /* Mirror BSD error message compatibility */
return -EOPNOTSUPP;
/* a lot of sanity checks */
if ((flags & ~MSG_DONTWAIT) ||
(sll == NULL) || (msg->msg_namelen != sizeof(struct sockaddr_ll)) ||
((sll->sll_family != AF_PACKET) && (sll->sll_family != AF_UNSPEC)) ||
(sll->sll_ifindex <= 0))
return -EINVAL;
if ((rtdev = rtdev_get_by_index(sll->sll_ifindex)) == NULL)
return -ENODEV;
rtskb = alloc_rtskb(rtdev->hard_header_len + len, &sock->skb_pool);
if (rtskb == NULL) {
ret = -ENOBUFS;
goto out;
}
if ((len < 0) || (len > rtdev->mtu)) {
ret = -EMSGSIZE;
goto err;
}
if (sll->sll_halen != rtdev->addr_len) {
ret = -EINVAL;
goto err;
}
rtskb_reserve(rtskb, rtdev->hard_header_len);
rt_memcpy_fromkerneliovec(rtskb_put(rtskb, len), msg->msg_iov, len);
rtskb->rtdev = rtdev;
rtskb->priority = sock->priority;
if (rtdev->hard_header) {
ret = rtdev->hard_header(rtskb, rtdev, ntohs(sll->sll_protocol),
sll->sll_addr, rtdev->dev_addr, rtskb->len);
if (ret < 0)
goto err;
}
if ((rtdev->flags & IFF_UP) != 0) {
if (rtdev_xmit(rtskb) == 0)
ret = len;
else
ret = -EAGAIN;
} else {
ret = -ENETDOWN;
goto err;
}
out:
rtdev_dereference(rtdev);
return ret;
err:
kfree_rtskb(rtskb);
rtdev_dereference(rtdev);
return ret;
}
/***
* rt_packet_recvmsg
*/
ssize_t rt_packet_recvmsg(struct rtdm_dev_context *context, int call_flags,
struct msghdr *msg, int msg_flags)
{
struct rtsocket *sock = (struct rtsocket *)&context->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
size_t copy_len;
size_t real_len;
struct rtskb *skb;
struct ethhdr *eth;
struct sockaddr_ll *sll;
int ret;
unsigned long flags;
rtos_time_t timeout;
/* block on receive event */
if (!test_bit(RT_SOCK_NONBLOCK, &context->context_flags) &&
((msg_flags & MSG_DONTWAIT) == 0))
while ((skb = rtskb_dequeue_chain(&sock->incoming)) == NULL) {
rtos_spin_lock_irqsave(&sock->param_lock, flags);
memcpy(&timeout, &sock->timeout, sizeof(timeout));
rtos_spin_unlock_irqrestore(&sock->param_lock, flags);
if (!RTOS_TIME_IS_ZERO(&timeout)) {
ret = rtos_event_sem_wait_timed(&sock->wakeup_event, &timeout);
if (ret == RTOS_EVENT_TIMEOUT)
return -ETIMEDOUT;
} else
ret = rtos_event_sem_wait(&sock->wakeup_event);
if (RTOS_EVENT_ERROR(ret))
return -ENOTSOCK;
}
else {
skb = rtskb_dequeue_chain(&sock->incoming);
if (skb == NULL)
return -EAGAIN;
}
eth = skb->mac.ethernet;
sll = msg->msg_name;
/* copy the address */
msg->msg_namelen = sizeof(*sll);
if (sll != NULL) {
sll->sll_family = AF_PACKET;
sll->sll_protocol = skb->protocol;
sll->sll_ifindex = skb->rtdev->ifindex;
sll->sll_pkttype = skb->pkt_type;
/* Ethernet specific */
sll->sll_hatype = ARPHRD_ETHER;
sll->sll_halen = ETH_ALEN;
memcpy(sll->sll_addr, eth->h_source, ETH_ALEN);
}
copy_len = real_len = skb->len;
/* The data must not be longer than the available buffer size */
if (copy_len > len) {
copy_len = len;
msg->msg_flags |= MSG_TRUNC;
}
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, copy_len);
if ((msg_flags & MSG_PEEK) == 0) {
rtdev_dereference(skb->rtdev);
kfree_rtskb(skb);
} else
rtskb_queue_head(&sock->incoming, skb);
return real_len;
}
/***
* rt_udp_sendmsg
*/
ssize_t rt_udp_sendmsg(struct rtdm_dev_context *context, int call_flags,
const struct msghdr *msg, int msg_flags)
{
struct rtsocket *sock = (struct rtsocket *)&context->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
int ulen = len + sizeof(struct udphdr);
struct sockaddr_in *usin;
struct udpfakehdr ufh;
struct dest_route rt;
u32 saddr;
u32 daddr;
u16 dport;
int err;
unsigned long flags;
if ((len < 0) || (len > 0xFFFF-sizeof(struct iphdr)-sizeof(struct udphdr)))
return -EMSGSIZE;
if (msg_flags & MSG_OOB) /* Mirror BSD error message compatibility */
return -EOPNOTSUPP;
if (msg_flags & ~(MSG_DONTROUTE|MSG_DONTWAIT) )
return -EINVAL;
if ((msg->msg_name) && (msg->msg_namelen==sizeof(struct sockaddr_in))) {
usin = (struct sockaddr_in*) msg->msg_name;
if ((usin->sin_family != AF_INET) && (usin->sin_family != AF_UNSPEC))
return -EINVAL;
daddr = usin->sin_addr.s_addr;
dport = usin->sin_port;
rtos_spin_lock_irqsave(&sock->param_lock, flags);
} else {
rtos_spin_lock_irqsave(&sock->param_lock, flags);
if (sock->prot.inet.state != TCP_ESTABLISHED)
return -ENOTCONN;
daddr = sock->prot.inet.daddr;
dport = sock->prot.inet.dport;
}
saddr = sock->prot.inet.saddr;
ufh.uh.source = sock->prot.inet.sport;
rtos_spin_unlock_irqrestore(&sock->param_lock, flags);
if ((daddr | dport) == 0)
return -EINVAL;
/* get output route */
err = rt_ip_route_output(&rt, daddr);
if (err)
return err;
/* check if specified source address fits */
if ((saddr != INADDR_ANY) && (saddr != rt.rtdev->local_ip)) {
rtdev_dereference(rt.rtdev);
return -EHOSTUNREACH;
}
/* we found a route, remember the routing dest-addr could be the netmask */
ufh.saddr = rt.rtdev->local_ip;
ufh.daddr = daddr;
ufh.uh.dest = dport;
ufh.uh.len = htons(ulen);
ufh.uh.check = 0;
ufh.iov = msg->msg_iov;
ufh.iovlen = msg->msg_iovlen;
ufh.wcheck = 0;
err = rt_ip_build_xmit(sock, rt_udp_getfrag, &ufh, ulen, &rt, msg_flags);
rtdev_dereference(rt.rtdev);
if (!err)
return len;
else
return err;
}
/***
* rt_udp_recvmsg
*/
ssize_t rt_udp_recvmsg(struct rtdm_dev_context *context, int call_flags,
struct msghdr *msg, int msg_flags)
{
struct rtsocket *sock = (struct rtsocket *)&context->dev_private;
size_t len = rt_iovec_len(msg->msg_iov, msg->msg_iovlen);
struct rtskb *skb;
struct rtskb *first_skb;
size_t copied = 0;
size_t block_size;
size_t data_len;
struct udphdr *uh;
struct sockaddr_in *sin;
int ret;
unsigned long flags;
rtos_time_t timeout;
/* block on receive event */
if (!test_bit(RT_SOCK_NONBLOCK, &context->context_flags) &&
((msg_flags & MSG_DONTWAIT) == 0))
while ((skb = rtskb_dequeue_chain(&sock->incoming)) == NULL) {
rtos_spin_lock_irqsave(&sock->param_lock, flags);
memcpy(&timeout, &sock->timeout, sizeof(timeout));
rtos_spin_unlock_irqrestore(&sock->param_lock, flags);
if (!RTOS_TIME_IS_ZERO(&timeout)) {
ret = rtos_event_sem_wait_timed(&sock->wakeup_event, &timeout);
if (ret == RTOS_EVENT_TIMEOUT)
return -ETIMEDOUT;
} else
ret = rtos_event_sem_wait(&sock->wakeup_event);
if (RTOS_EVENT_ERROR(ret))
return -ENOTSOCK;
}
else {
skb = rtskb_dequeue_chain(&sock->incoming);
if (skb == NULL)
return -EAGAIN;
}
uh = skb->h.uh;
data_len = ntohs(uh->len) - sizeof(struct udphdr);
sin = msg->msg_name;
/* copy the address */
msg->msg_namelen = sizeof(*sin);
if (sin) {
sin->sin_family = AF_INET;
sin->sin_port = uh->source;
sin->sin_addr.s_addr = skb->nh.iph->saddr;
}
/* remove the UDP header */
__rtskb_pull(skb, sizeof(struct udphdr));
first_skb = skb;
/* iterate over all IP fragments */
do {
rtskb_trim(skb, data_len);
block_size = skb->len;
copied += block_size;
data_len -= block_size;
/* The data must not be longer than the available buffer size */
if (copied > len) {
block_size -= copied - len;
copied = len;
msg->msg_flags |= MSG_TRUNC;
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, block_size);
break;
}
/* copy the data */
rt_memcpy_tokerneliovec(msg->msg_iov, skb->data, block_size);
/* next fragment */
skb = skb->next;
} while (skb != NULL);
/* did we copied all bytes? */
if (data_len > 0)
msg->msg_flags |= MSG_TRUNC;
if ((msg_flags & MSG_PEEK) == 0)
kfree_rtskb(first_skb);
else {
__rtskb_push(first_skb, sizeof(struct udphdr));
rtskb_queue_head(&sock->incoming, first_skb);
}
return copied;
}
