1 files changed, 83 insertions, 52 deletions

diff --git a/net/bluetooth/6lowpan.c b/net/bluetooth/6lowpan.c
index 5d3e6202da3d..2ec7c84c2000 100644
--- a/net/bluetooth/6lowpan.c
+++ b/net/bluetooth/6lowpan.c
@@ -426,38 +426,33 @@ static void convert_dest_bdaddr(struct in6_addr *ip6_daddr,
         *addr_type = get_addr_type_from_eui64(addr->b[5]);
 }
 
-static int header_create(struct sk_buff *skb, struct net_device *netdev,
-                         unsigned short type, const void *_daddr,
-                         const void *_saddr, unsigned int len)
+static int setup_header(struct sk_buff *skb, struct net_device *netdev,
+                        bdaddr_t *peer_addr, u8 *peer_addr_type)
 {
-        struct ipv6hdr *hdr;
+        struct in6_addr ipv6_daddr;
         struct lowpan_dev *dev;
         struct lowpan_peer *peer;
         bdaddr_t addr, *any = BDADDR_ANY;
-        u8 *saddr, *daddr = any->b;
-        u8 addr_type;
-
-        if (type != ETH_P_IPV6)
-                return -EINVAL;
-
-        hdr = ipv6_hdr(skb);
+        u8 *daddr = any->b;
+        int err, status = 0;
 
         dev = lowpan_dev(netdev);
 
-        if (ipv6_addr_is_multicast(&hdr->daddr)) {
-                memcpy(&lowpan_cb(skb)->addr, &hdr->daddr,
-                       sizeof(struct in6_addr));
+        memcpy(&ipv6_daddr, &lowpan_cb(skb)->addr, sizeof(ipv6_daddr));
+
+        if (ipv6_addr_is_multicast(&ipv6_daddr)) {
                 lowpan_cb(skb)->chan = NULL;
         } else {
                 unsigned long flags;
+                u8 addr_type;
 
                 /* Get destination BT device from skb.
                  * If there is no such peer then discard the packet.
                  */
-                convert_dest_bdaddr(&hdr->daddr, &addr, &addr_type);
+                convert_dest_bdaddr(&ipv6_daddr, &addr, &addr_type);
 
                 BT_DBG("dest addr %pMR type %d IP %pI6c", &addr,
-                       addr_type, &hdr->daddr);
+                       addr_type, &ipv6_daddr);
 
                 read_lock_irqsave(&devices_lock, flags);
                 peer = peer_lookup_ba(dev, &addr, addr_type);
@@ -470,7 +465,7 @@ static int header_create(struct sk_buff *skb, struct net_device *netdev,
                          * the destination address.
                          */
                         read_lock_irqsave(&devices_lock, flags);
-                        peer = peer_lookup_dst(dev, &hdr->daddr, skb);
+                        peer = peer_lookup_dst(dev, &ipv6_daddr, skb);
                         read_unlock_irqrestore(&devices_lock, flags);
                         if (!peer) {
                                 BT_DBG("no such peer %pMR found", &addr);
@@ -479,29 +474,56 @@ static int header_create(struct sk_buff *skb, struct net_device *netdev,
                 }
 
                 daddr = peer->eui64_addr;
-
-                memcpy(&lowpan_cb(skb)->addr, &hdr->daddr,
-                       sizeof(struct in6_addr));
+                *peer_addr = addr;
+                *peer_addr_type = addr_type;
                 lowpan_cb(skb)->chan = peer->chan;
+
+                status = 1;
         }
 
-        saddr = dev->netdev->dev_addr;
+        lowpan_header_compress(skb, netdev, ETH_P_IPV6, daddr,
+                               dev->netdev->dev_addr, skb->len);
+
+        err = dev_hard_header(skb, netdev, ETH_P_IPV6, NULL, NULL, 0);
+        if (err < 0)
+                return err;
+
+        return status;
+}
+
+static int header_create(struct sk_buff *skb, struct net_device *netdev,
+                         unsigned short type, const void *_daddr,
+                         const void *_saddr, unsigned int len)
+{
+        struct ipv6hdr *hdr;
+
+        if (type != ETH_P_IPV6)
+                return -EINVAL;
+
+        hdr = ipv6_hdr(skb);
+
+        memcpy(&lowpan_cb(skb)->addr, &hdr->daddr, sizeof(struct in6_addr));
 
-        return lowpan_header_compress(skb, netdev, type, daddr, saddr, len);
+        return 0;
 }
 
 /* Packet to BT LE device */
 static int send_pkt(struct l2cap_chan *chan, struct sk_buff *skb,
-                    struct net_device *netdev)
+                    struct net_device *netdev, bool is_mcast)
 {
         struct msghdr msg;
         struct kvec iv;
         int err;
 
         /* Remember the skb so that we can send EAGAIN to the caller if
-         * we run out of credits.
+         * we run out of credits. This is not done for multicast packets
+         * because we generate mcast packet in this module and are not
+         * really able to remember the skb after this packet is sent.
          */
-        chan->data = skb;
+        if (is_mcast)
+                chan->data = NULL;
+        else
+                chan->data = skb;
 
         memset(&msg, 0, sizeof(msg));
         msg.msg_iov = (struct iovec *) &iv;
@@ -549,7 +571,11 @@ static void send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev)
                 list_for_each_entry_safe(pentry, ptmp, &dev->peers, list) {
                         local_skb = skb_clone(skb, GFP_ATOMIC);
 
-                        send_pkt(pentry->chan, local_skb, netdev);
+                        BT_DBG("xmit %s to %pMR type %d IP %pI6c chan %p",
+                               netdev->name,
+                               &pentry->chan->dst, pentry->chan->dst_type,
+                               &pentry->peer_addr, pentry->chan);
+                        send_pkt(pentry->chan, local_skb, netdev, true);
 
                         kfree_skb(local_skb);
                 }
@@ -561,43 +587,48 @@ static void send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev)
 static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev)
 {
         int err = 0;
-        struct lowpan_dev *dev;
-        struct lowpan_peer *peer;
         bdaddr_t addr;
         u8 addr_type;
 
-        if (ipv6_addr_is_multicast(&lowpan_cb(skb)->addr)) {
-                /* We need to send the packet to every device
-                 * behind this interface.
-                 */
-                send_mcast_pkt(skb, netdev);
-        } else {
-                unsigned long flags;
-
-                convert_dest_bdaddr(&lowpan_cb(skb)->addr, &addr, &addr_type);
-                dev = lowpan_dev(netdev);
-
-                read_lock_irqsave(&devices_lock, flags);
-                peer = peer_lookup_ba(dev, &addr, addr_type);
-                if (!peer)
-                        peer = peer_lookup_dst(dev, &lowpan_cb(skb)->addr, skb);
-                read_unlock_irqrestore(&devices_lock, flags);
+        /* We must take a copy of the skb before we modify/replace the ipv6
+         * header as the header could be used elsewhere
+         */
+        skb = skb_unshare(skb, GFP_ATOMIC);
+        if (!skb)
+                return NET_XMIT_DROP;
 
-                BT_DBG("xmit %s to %pMR type %d IP %pI6c peer %p",
-                       netdev->name, &addr, addr_type,
-                       &lowpan_cb(skb)->addr, peer);
+        /* Return values from setup_header()
+         *  <0 - error, packet is dropped
+         *   0 - this is a multicast packet
+         *   1 - this is unicast packet
+         */
+        err = setup_header(skb, netdev, &addr, &addr_type);
+        if (err < 0) {
+                kfree_skb(skb);
+                return NET_XMIT_DROP;
+        }
 
-                if (peer && peer->chan)
-                        err = send_pkt(peer->chan, skb, netdev);
-                else
+        if (err) {
+                if (lowpan_cb(skb)->chan) {
+                        BT_DBG("xmit %s to %pMR type %d IP %pI6c chan %p",
+                               netdev->name, &addr, addr_type,
+                               &lowpan_cb(skb)->addr, lowpan_cb(skb)->chan);
+                        err = send_pkt(lowpan_cb(skb)->chan, skb, netdev,
+                                       false);
+                } else {
                         err = -ENOENT;
+                }
+        } else {
+                /* We need to send the packet to every device behind this
+                 * interface.
+                 */
+                send_mcast_pkt(skb, netdev);
         }
-        dev_kfree_skb(skb);
 
         if (err)
                 BT_DBG("ERROR: xmit failed (%d)", err);
 
-        return (err < 0) ? NET_XMIT_DROP : err;
+        return err < 0 ? NET_XMIT_DROP : err;
 }
 
 static const struct net_device_ops netdev_ops = {