Module: xenomai-3 Branch: master Commit: c643a7bce00c893243e23380b35e81a6cad270ea URL: http://git.xenomai.org/?p=xenomai-3.git;a=commit;h=c643a7bce00c893243e23380b35e81a6cad270ea
Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org> Date: Wed Oct 29 00:10:37 2014 +0100 rtnet: use callbacks for locking (rtpacket_type) this allows the af_packet sockets to be locked during the handler execution --- kernel/drivers/net/stack/include/stack_mgr.h | 13 ++ kernel/drivers/net/stack/ipv4/arp.c | 88 +++++++------ kernel/drivers/net/stack/ipv4/ip_output.c | 182 +++++++++++++------------- kernel/drivers/net/stack/packet/af_packet.c | 36 ++++- kernel/drivers/net/stack/stack_mgr.c | 16 ++- 5 files changed, 192 insertions(+), 143 deletions(-) diff --git a/kernel/drivers/net/stack/include/stack_mgr.h b/kernel/drivers/net/stack/include/stack_mgr.h index ed380e6..fd2893b 100644 --- a/kernel/drivers/net/stack/include/stack_mgr.h +++ b/kernel/drivers/net/stack/include/stack_mgr.h @@ -49,12 +49,25 @@ struct rtpacket_type { int (*handler)(struct rtskb *, struct rtpacket_type *); int (*err_handler)(struct rtskb *, struct rtnet_device *, struct rtpacket_type *); + bool (*trylock)(struct rtpacket_type *) + void (*unlock)(struct rtpacket_type *) }; int rtdev_add_pack(struct rtpacket_type *pt); int rtdev_remove_pack(struct rtpacket_type *pt); +static inline bool rtdev_lock_pack(struct rtpacket_type *pt) +{ + ++pt->recount; + return true; +} + +static inline void rtdev_unlock_pack(struct rtpacket_type *pt) +{ + --pt->refcount; +} + void rt_stack_connect(struct rtnet_device *rtdev, struct rtnet_mgr *mgr); void rt_stack_disconnect(struct rtnet_device *rtdev); diff --git a/kernel/drivers/net/stack/ipv4/arp.c b/kernel/drivers/net/stack/ipv4/arp.c index 0571682..ecb65f8 100644 --- a/kernel/drivers/net/stack/ipv4/arp.c +++ b/kernel/drivers/net/stack/ipv4/arp.c @@ -34,40 +34,40 @@ * we create a broadcast message. */ void rt_arp_send(int type, int ptype, u32 dest_ip, struct rtnet_device *rtdev, - u32 src_ip, unsigned char *dest_hw, unsigned char *src_hw, - unsigned char *target_hw) + u32 src_ip, unsigned char *dest_hw, unsigned char *src_hw, + unsigned char *target_hw) { struct rtskb *skb; struct arphdr *arp; unsigned char *arp_ptr; if (rtdev->flags & IFF_NOARP) - return; + return; if (!(skb=alloc_rtskb(sizeof(struct arphdr) + 2*(rtdev->addr_len+4) + - rtdev->hard_header_len+15, &global_pool))) - return; + rtdev->hard_header_len+15, &global_pool))) + return; rtskb_reserve(skb, (rtdev->hard_header_len+15)&~15); skb->nh.raw = skb->data; arp = (struct arphdr *)rtskb_put(skb, sizeof(struct arphdr) + - 2*(rtdev->addr_len+4)); + 2*(rtdev->addr_len+4)); skb->rtdev = rtdev; skb->protocol = __constant_htons (ETH_P_ARP); skb->priority = RT_ARP_SKB_PRIO; if (src_hw == NULL) - src_hw = rtdev->dev_addr; + src_hw = rtdev->dev_addr; if (dest_hw == NULL) - dest_hw = rtdev->broadcast; + dest_hw = rtdev->broadcast; /* * Fill the device header for the ARP frame */ if (rtdev->hard_header && - (rtdev->hard_header(skb,rtdev,ptype,dest_hw,src_hw,skb->len) < 0)) - goto out; + (rtdev->hard_header(skb,rtdev,ptype,dest_hw,src_hw,skb->len) < 0)) + goto out; arp->ar_hrd = htons(rtdev->type); arp->ar_pro = __constant_htons(ETH_P_IP); @@ -84,9 +84,9 @@ void rt_arp_send(int type, int ptype, u32 dest_ip, struct rtnet_device *rtdev, arp_ptr+=4; if (target_hw != NULL) - memcpy(arp_ptr, target_hw, rtdev->addr_len); + memcpy(arp_ptr, target_hw, rtdev->addr_len); else - memset(arp_ptr, 0, rtdev->addr_len); + memset(arp_ptr, 0, rtdev->addr_len); arp_ptr+=rtdev->addr_len; memcpy(arp_ptr, &dest_ip, 4); @@ -123,37 +123,37 @@ int rt_arp_rcv(struct rtskb *skb, struct rtpacket_type *pt) * it. */ if ((arp->ar_hln != rtdev->addr_len) || - (rtdev->flags & IFF_NOARP) || - (skb->pkt_type == PACKET_OTHERHOST) || - (skb->pkt_type == PACKET_LOOPBACK) || - (arp->ar_pln != 4)) - goto out; + (rtdev->flags & IFF_NOARP) || + (skb->pkt_type == PACKET_OTHERHOST) || + (skb->pkt_type == PACKET_LOOPBACK) || + (arp->ar_pln != 4)) + goto out; switch (dev_type) { - default: - if ((arp->ar_pro != __constant_htons(ETH_P_IP)) && - (htons(dev_type) != arp->ar_hrd)) - goto out; - break; - case ARPHRD_ETHER: - /* - * ETHERNET devices will accept ARP hardware types of either - * 1 (Ethernet) or 6 (IEEE 802.2). - */ - if ((arp->ar_hrd != __constant_htons(ARPHRD_ETHER)) && - (arp->ar_hrd != __constant_htons(ARPHRD_IEEE802))) { - goto out; - } - if (arp->ar_pro != __constant_htons(ETH_P_IP)) { - goto out; - } - break; + default: + if ((arp->ar_pro != __constant_htons(ETH_P_IP)) && + (htons(dev_type) != arp->ar_hrd)) + goto out; + break; + case ARPHRD_ETHER: + /* + * ETHERNET devices will accept ARP hardware types of either + * 1 (Ethernet) or 6 (IEEE 802.2). + */ + if ((arp->ar_hrd != __constant_htons(ARPHRD_ETHER)) && + (arp->ar_hrd != __constant_htons(ARPHRD_IEEE802))) { + goto out; + } + if (arp->ar_pro != __constant_htons(ETH_P_IP)) { + goto out; + } + break; } /* Understand only these message types */ if ((arp->ar_op != __constant_htons(ARPOP_REPLY)) && - (arp->ar_op != __constant_htons(ARPOP_REQUEST))) - goto out; + (arp->ar_op != __constant_htons(ARPOP_REQUEST))) + goto out; /* * Extract fields @@ -168,12 +168,12 @@ int rt_arp_rcv(struct rtskb *skb, struct rtpacket_type *pt) /* process only requests/replies directed to us */ if (tip == rtdev->local_ip) { - rt_ip_route_add_host(sip, sha, rtdev); + rt_ip_route_add_host(sip, sha, rtdev); #ifndef CONFIG_XENO_DRIVERS_NET_ADDON_PROXY_ARP - if (arp->ar_op == __constant_htons(ARPOP_REQUEST)) - rt_arp_send(ARPOP_REPLY, ETH_P_ARP, sip, rtdev, tip, sha, - rtdev->dev_addr, sha); + if (arp->ar_op == __constant_htons(ARPOP_REQUEST)) + rt_arp_send(ARPOP_REPLY, ETH_P_ARP, sip, rtdev, tip, sha, + rtdev->dev_addr, sha); #endif /* CONFIG_XENO_DRIVERS_NET_ADDON_PROXY_ARP */ } @@ -212,5 +212,9 @@ void __init rt_arp_init(void) */ void rt_arp_release(void) { - rtdev_remove_pack(&arp_packet_type); + while (rtdev_remove_pack(&arp_packet_type) == -EAGAIN) { + printk("RTnet ARP: waiting for protocol unregistration\n"); + set_current_state(TASK_UNINTERRUPTIBLE); + schedule_timeout(1*HZ); /* wait a second */ + } } diff --git a/kernel/drivers/net/stack/ipv4/ip_output.c b/kernel/drivers/net/stack/ipv4/ip_output.c index 03c45ae..cc47275 100644 --- a/kernel/drivers/net/stack/ipv4/ip_output.c +++ b/kernel/drivers/net/stack/ipv4/ip_output.c @@ -39,9 +39,9 @@ static u16 rt_ip_id_count = 0; * Slow path for fragmented packets */ int rt_ip_build_xmit_slow(struct rtsocket *sk, - int getfrag(const void *, char *, unsigned int, unsigned int), - const void *frag, unsigned length, struct dest_route *rt, - int msg_flags, unsigned int mtu, unsigned int prio) + int getfrag(const void *, char *, unsigned int, unsigned int), + const void *frag, unsigned length, struct dest_route *rt, + int msg_flags, unsigned int mtu, unsigned int prio) { int err, next_err; struct rtskb *skb; @@ -68,89 +68,89 @@ int rt_ip_build_xmit_slow(struct rtsocket *sk, rtskb_size = mtu + hh_len + 15; /* TODO: delay previous skb until ALL errors are catched which may occure - during next skb setup */ + during next skb setup */ /* Preallocate first rtskb */ skb = alloc_rtskb(rtskb_size, &sk->skb_pool); if (skb == NULL) - return -ENOBUFS; + return -ENOBUFS; for (offset = 0; offset < length; offset += fragdatalen) { - int fraglen; /* The length (IP, including ip-header) of this - very fragment */ - __u16 frag_off = offset >> 3 ; - - - next_err = 0; - if (offset >= length - fragdatalen) - { - /* last fragment */ - fraglen = FRAGHEADERLEN + length - offset ; - next_skb = NULL; - } - else - { - fraglen = FRAGHEADERLEN + fragdatalen; - frag_off |= IP_MF; - - next_skb = alloc_rtskb(rtskb_size, &sk->skb_pool); - if (next_skb == NULL) { - frag_off &= ~IP_MF; /* cut the chain */ - next_err = -ENOBUFS; - } - } - - rtskb_reserve(skb, hh_len); - - skb->rtdev = rtdev; - skb->nh.iph = iph = (struct iphdr *)rtskb_put(skb, fraglen); - skb->priority = prio; - - iph->version = 4; - iph->ihl = 5; /* 20 byte header - no options */ - iph->tos = sk->prot.inet.tos; - iph->tot_len = htons(fraglen); - iph->id = htons(msg_rt_ip_id); - iph->frag_off = htons(frag_off); - iph->ttl = 255; - iph->protocol = sk->protocol; - iph->saddr = rtdev->local_ip; - iph->daddr = rt->ip; - iph->check = 0; /* required! */ - iph->check = ip_fast_csum((unsigned char *)iph, 5 /*iph->ihl*/); - - if ( (err=getfrag(frag, ((char *)iph) + 5 /*iph->ihl*/ * 4, offset, - fraglen - FRAGHEADERLEN)) ) - goto error; - - if (rtdev->hard_header) { - err = rtdev->hard_header(skb, rtdev, ETH_P_IP, rt->dev_addr, - rtdev->dev_addr, skb->len); - if (err < 0) - goto error; - } - - err = rtdev_xmit(skb); - - skb = next_skb; - - if (err != 0) { - err = -EAGAIN; - goto error; - } - - if (next_err != 0) - return next_err; + int fraglen; /* The length (IP, including ip-header) of this + very fragment */ + __u16 frag_off = offset >> 3 ; + + + next_err = 0; + if (offset >= length - fragdatalen) + { + /* last fragment */ + fraglen = FRAGHEADERLEN + length - offset ; + next_skb = NULL; + } + else + { + fraglen = FRAGHEADERLEN + fragdatalen; + frag_off |= IP_MF; + + next_skb = alloc_rtskb(rtskb_size, &sk->skb_pool); + if (next_skb == NULL) { + frag_off &= ~IP_MF; /* cut the chain */ + next_err = -ENOBUFS; + } + } + + rtskb_reserve(skb, hh_len); + + skb->rtdev = rtdev; + skb->nh.iph = iph = (struct iphdr *)rtskb_put(skb, fraglen); + skb->priority = prio; + + iph->version = 4; + iph->ihl = 5; /* 20 byte header - no options */ + iph->tos = sk->prot.inet.tos; + iph->tot_len = htons(fraglen); + iph->id = htons(msg_rt_ip_id); + iph->frag_off = htons(frag_off); + iph->ttl = 255; + iph->protocol = sk->protocol; + iph->saddr = rtdev->local_ip; + iph->daddr = rt->ip; + iph->check = 0; /* required! */ + iph->check = ip_fast_csum((unsigned char *)iph, 5 /*iph->ihl*/); + + if ( (err=getfrag(frag, ((char *)iph) + 5 /*iph->ihl*/ * 4, offset, + fraglen - FRAGHEADERLEN)) ) + goto error; + + if (rtdev->hard_header) { + err = rtdev->hard_header(skb, rtdev, ETH_P_IP, rt->dev_addr, + rtdev->dev_addr, skb->len); + if (err < 0) + goto error; + } + + err = rtdev_xmit(skb); + + skb = next_skb; + + if (err != 0) { + err = -EAGAIN; + goto error; + } + + if (next_err != 0) + return next_err; } return 0; error: if (skb != NULL) { - kfree_rtskb(skb); + kfree_rtskb(skb); - if (next_skb != NULL) - kfree_rtskb(next_skb); + if (next_skb != NULL) + kfree_rtskb(next_skb); } return err; } @@ -161,9 +161,9 @@ int rt_ip_build_xmit_slow(struct rtsocket *sk, * Fast path for unfragmented packets. */ int rt_ip_build_xmit(struct rtsocket *sk, - int getfrag(const void *, char *, unsigned int, unsigned int), - const void *frag, unsigned length, struct dest_route *rt, - int msg_flags) + int getfrag(const void *, char *, unsigned int, unsigned int), + const void *frag, unsigned length, struct dest_route *rt, + int msg_flags) { int err = 0; struct rtskb *skb; @@ -189,9 +189,9 @@ int rt_ip_build_xmit(struct rtsocket *sk, length += sizeof(struct iphdr); if (length > mtu) - return rt_ip_build_xmit_slow(sk, getfrag, frag, - length - sizeof(struct iphdr), - rt, msg_flags, mtu, prio); + return rt_ip_build_xmit_slow(sk, getfrag, frag, + length - sizeof(struct iphdr), + rt, msg_flags, mtu, prio); /* Store id in local variable */ rtdm_lock_get_irqsave(&rt_ip_id_lock, context); @@ -202,7 +202,7 @@ int rt_ip_build_xmit(struct rtsocket *sk, skb = alloc_rtskb(length+hh_len+15, &sk->skb_pool); if (skb==NULL) - return -ENOBUFS; + return -ENOBUFS; rtskb_reserve(skb, hh_len); @@ -224,22 +224,22 @@ int rt_ip_build_xmit(struct rtsocket *sk, iph->check = ip_fast_csum((unsigned char *)iph, 5 /*iph->ihl*/); if ( (err=getfrag(frag, ((char *)iph) + 5 /*iph->ihl*/ * 4, 0, - length - 5 /*iph->ihl*/ * 4)) ) - goto error; + length - 5 /*iph->ihl*/ * 4)) ) + goto error; if (rtdev->hard_header) { - err = rtdev->hard_header(skb, rtdev, ETH_P_IP, rt->dev_addr, - rtdev->dev_addr, skb->len); - if (err < 0) - goto error; + err = rtdev->hard_header(skb, rtdev, ETH_P_IP, rt->dev_addr, + rtdev->dev_addr, skb->len); + if (err < 0) + goto error; } err = rtdev_xmit(skb); if (err) - return -EAGAIN; + return -EAGAIN; else - return 0; + return 0; error: kfree_rtskb(skb); @@ -276,5 +276,9 @@ void __init rt_ip_init(void) void rt_ip_release(void) { rt_ip_fragment_cleanup(); - rtdev_remove_pack(&ip_packet_type); + while (rtdev_remove_pack(&ip_packet_type) == -EAGAIN) { + printk("RTnet IP: waiting for protocol unregistration\n"); + set_current_state(TASK_UNINTERRUPTIBLE); + schedule_timeout(1*HZ); /* wait a second */ + } } diff --git a/kernel/drivers/net/stack/packet/af_packet.c b/kernel/drivers/net/stack/packet/af_packet.c index 97eaf1b..c848d0c 100644 --- a/kernel/drivers/net/stack/packet/af_packet.c +++ b/kernel/drivers/net/stack/packet/af_packet.c @@ -81,7 +81,26 @@ static int rt_packet_rcv(struct rtskb *skb, struct rtpacket_type *pt) 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 rtdev_lock_pack(pt); +} + +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); + rtdev_unlock_pack(pt); + rtdm_fd_unlock(fd); +} /*** * rt_packet_bind @@ -117,6 +136,8 @@ static int rt_packet_bind(struct rtsocket *sock, const struct sockaddr *addr, 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 @@ -183,8 +204,10 @@ static int rt_packet_socket(struct rtdm_fd *fd, int protocol) 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.type = protocol; + sock->prot.packet.ifindex = 0; + sock->prot.packet.trylock = rt_packet_trylock; + sock->prot.packet.unlock = rt_packet_unlock; /* if protocol is non-zero, register the packet type */ if (protocol != 0) { @@ -216,8 +239,10 @@ static int rt_packet_close(struct rtdm_fd *fd) rtdm_lock_get_irqsave(&sock->param_lock, context); - if ((pt->type != 0) && ((ret = rtdev_remove_pack(pt)) == 0)) + if (pt->type != 0) { + rtdev_remove_pack(pt); pt->type = 0; + } rtdm_lock_put_irqrestore(&sock->param_lock, context); @@ -227,10 +252,7 @@ static int rt_packet_close(struct rtdm_fd *fd) kfree_rtskb(del); } - if (ret == 0) - ret = rt_socket_cleanup(fd); - - return ret; + return rt_socket_cleanup(fd); } diff --git a/kernel/drivers/net/stack/stack_mgr.c b/kernel/drivers/net/stack/stack_mgr.c index 7e72abe..4286292 100644 --- a/kernel/drivers/net/stack/stack_mgr.c +++ b/kernel/drivers/net/stack/stack_mgr.c @@ -58,6 +58,10 @@ int rtdev_add_pack(struct rtpacket_type *pt) INIT_LIST_HEAD(&pt->list_entry); pt->refcount = 0; + if (pt->trylock == NULL) + pt->trylock = rtdev_lock_pack; + if (pt->unlock == NULL) + pt->unlock = rtdev_unlock_pack; rtdm_lock_get_irqsave(&rt_packets_lock, context); @@ -125,7 +129,7 @@ void rtnetif_rx(struct rtskb *skb) if (unlikely(rtskb_fifo_insert_inirq(&rx.fifo, skb) < 0)) { rtdm_printk("RTnet: dropping packet in %s()\n", __FUNCTION__); - kfree_rtskb(skb); + kfryee_rtskb(skb); rtdev_dereference(rtdev); } } @@ -158,13 +162,14 @@ __DELIVER_PREFIX void rt_stack_deliver(struct rtskb *rtskb) #ifdef CONFIG_XENO_DRIVERS_NET_ETH_P_ALL eth_p_all_hit = 0; list_for_each_entry(pt_entry, &rt_packets_all, list_entry) { - pt_entry->refcount++; + if (!pt_entry->trylock(pt_entry)) + continue; rtdm_lock_put_irqrestore(&rt_packets_lock, context); pt_entry->handler(rtskb, pt_entry); rtdm_lock_get_irqsave(&rt_packets_lock, context); - pt_entry->refcount--; + pt_entry->unlock(pt_entry); eth_p_all_hit = 1; } #endif /* CONFIG_XENO_DRIVERS_NET_ETH_P_ALL */ @@ -173,13 +178,14 @@ __DELIVER_PREFIX void rt_stack_deliver(struct rtskb *rtskb) list_for_each_entry(pt_entry, &rt_packets[hash], list_entry) if (pt_entry->type == rtskb->protocol) { - pt_entry->refcount++; + if (!pt_entry->trylock(pt_entry)) + continue; rtdm_lock_put_irqrestore(&rt_packets_lock, context); err = pt_entry->handler(rtskb, pt_entry); rtdm_lock_get_irqsave(&rt_packets_lock, context); - pt_entry->refcount--; + pt_entry->unlock(pt_entry); rtdev_dereference(rtdev); _______________________________________________ Xenomai-git mailing list Xenomai-git@xenomai.org http://www.xenomai.org/mailman/listinfo/xenomai-git