Module: xenomai-gch
Branch: for-forge
Commit: a8eaed38db0587057cb59d62bae46a1fedbd24bd
URL:    
http://git.xenomai.org/?p=xenomai-gch.git;a=commit;h=a8eaed38db0587057cb59d62bae46a1fedbd24bd

Author: Gilles Chanteperdrix <gilles.chanteperd...@xenomai.org>
Date:   Wed Oct 29 00:10:37 2014 +0100

rtpacket_type: use callbacks for locking

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

Reply via email to