void event_handler(int arg)
{
struct {
nanosecs_t time_stamp;
unsigned long count;
} packet;
while (1) {
rtos_event_sem_wait(&event_sem);
ioctl_rt(tdma, RTMAC_RTIOC_TIMEOFFSET, &packet.time_stamp);
rtos_irq_disable(&irq_handle);
packet.time_stamp += time_stamp;
packet.count = irq_count;
rtos_irq_enable(&irq_handle);
ioctl_rt(tdma, RTMAC_RTIOC_WAITONCYCLE, RTMAC_WAIT_ON_DEFAULT);
if (sendto_rt(sock, &packet, sizeof(packet), 0,
(struct sockaddr*)&dest_addr,
sizeof(struct sockaddr_in)) < 0)
break;
}
}
static void rtpc_dispatch_handler(int arg)
{
struct rt_proc_call *call;
int ret;
while (1) {
rtos_event_sem_wait(&dispatch_event);
call = rtpc_dequeue_pending_call();
ret = call->proc(call);
if (ret != -CALL_PENDING)
rtpc_complete_call(call, ret);
}
}
void nrt_xmit_task(int arg)
{
struct rtskb *rtskb;
struct rtnet_device *rtdev;
while (!shutdown) {
if ((rtskb = rtskb_dequeue(&nrt_rtskb_queue))) {
rtdev = rtskb->rtdev;
/* no MAC: we simply transmit the packet under xmit_lock */
rtos_res_lock(&rtdev->xmit_lock);
rtmac_xmit(rtskb);
rtos_res_unlock(&rtdev->xmit_lock);
}
rtos_event_sem_wait(&wakeup_sem);
}
}
static void rtcfg_rx_task(int arg)
{
struct rtskb *rtskb;
struct rtcfg_frm_head *frm_head;
struct rtnet_device *rtdev;
while (1) {
if (RTOS_EVENT_ERROR(rtos_event_sem_wait(&rx_event)))
return;
rtskb = rtskb_dequeue(&rx_queue);
rtdev = rtskb->rtdev;
if (rtskb->pkt_type == PACKET_OTHERHOST) {
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
continue;
}
if (rtskb->len < sizeof(struct rtcfg_frm_head)) {
RTCFG_DEBUG(1, "RTcfg: %s() received an invalid frame\n",
__FUNCTION__);
rtdev_dereference(rtdev);
kfree_rtskb(rtskb);
continue;
}
frm_head = (struct rtcfg_frm_head *)rtskb->data;
if (rtcfg_do_main_event(rtskb->rtdev->ifindex,
frm_head->id + RTCFG_FRM_STAGE_1_CFG,
rtskb) < 0)
kfree_rtskb(rtskb);
rtdev_dereference(rtdev);
}
}
/***
* 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_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;
}
