net/ieee802154/6lowpan/rx.c

   1 /* This program is free software; you can redistribute it and/or modify
   2  * it under the terms of the GNU General Public License version 2
   3  * as published by the Free Software Foundation.
   4  *
   5  * This program is distributed in the hope that it will be useful,
   6  * but WITHOUT ANY WARRANTY; without even the implied warranty of
   7  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   8  * GNU General Public License for more details.
   9  */
  10
  11 #include <linux/if_arp.h>
  12
  13 #include <net/6lowpan.h>
  14 #include <net/ieee802154_netdev.h>
  15
  16 #include "6lowpan_i.h"
  17
  18 static int lowpan_give_skb_to_device(struct sk_buff *skb,
  19                                      struct net_device *dev)
  20 {
  21         skb->dev = dev->ieee802154_ptr->lowpan_dev;
  22         skb->protocol = htons(ETH_P_IPV6);
  23         skb->pkt_type = PACKET_HOST;
  24
  25         return netif_rx(skb);
  26 }
  27
  28 static int
  29 iphc_decompress(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
  30 {
  31         u8 iphc0, iphc1;
  32         struct ieee802154_addr_sa sa, da;
  33         void *sap, *dap;
  34
  35         raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
  36         /* at least two bytes will be used for the encoding */
  37         if (skb->len < 2)
  38                 return -EINVAL;
  39
  40         if (lowpan_fetch_skb_u8(skb, &iphc0))
  41                 return -EINVAL;
  42
  43         if (lowpan_fetch_skb_u8(skb, &iphc1))
  44                 return -EINVAL;
  45
  46         ieee802154_addr_to_sa(&sa, &hdr->source);
  47         ieee802154_addr_to_sa(&da, &hdr->dest);
  48
  49         if (sa.addr_type == IEEE802154_ADDR_SHORT)
  50                 sap = &sa.short_addr;
  51         else
  52                 sap = &sa.hwaddr;
  53
  54         if (da.addr_type == IEEE802154_ADDR_SHORT)
  55                 dap = &da.short_addr;
  56         else
  57                 dap = &da.hwaddr;
  58
  59         return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
  60                                         IEEE802154_ADDR_LEN, dap, da.addr_type,
  61                                         IEEE802154_ADDR_LEN, iphc0, iphc1);
  62 }
  63
  64 static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
  65                       struct packet_type *pt, struct net_device *orig_dev)
  66 {
  67         struct ieee802154_hdr hdr;
  68         int ret;
  69
  70         skb = skb_share_check(skb, GFP_ATOMIC);
  71         if (!skb)
  72                 goto drop;
  73
  74         if (!netif_running(dev))
  75                 goto drop_skb;
  76
  77         if (skb->pkt_type == PACKET_OTHERHOST)
  78                 goto drop_skb;
  79
  80         if (dev->type != ARPHRD_IEEE802154)
  81                 goto drop_skb;
  82
  83         if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
  84                 goto drop_skb;
  85
  86         /* check that it's our buffer */
  87         if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
  88                 /* Pull off the 1-byte of 6lowpan header. */
  89                 skb_pull(skb, 1);
  90                 return lowpan_give_skb_to_device(skb, dev);
  91         } else {
  92                 switch (skb->data[0] & 0xe0) {
  93                 case LOWPAN_DISPATCH_IPHC:      /* ipv6 datagram */
  94                         ret = iphc_decompress(skb, &hdr);
  95                         if (ret < 0)
  96                                 goto drop_skb;
  97
  98                         return lowpan_give_skb_to_device(skb, dev);
  99                 case LOWPAN_DISPATCH_FRAG1:     /* first fragment header */
 100                         ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
 101                         if (ret == 1) {
 102                                 ret = iphc_decompress(skb, &hdr);
 103                                 if (ret < 0)
 104                                         goto drop_skb;
 105
 106                                 return lowpan_give_skb_to_device(skb, dev);
 107                         } else if (ret == -1) {
 108                                 return NET_RX_DROP;
 109                         } else {
 110                                 return NET_RX_SUCCESS;
 111                         }
 112                 case LOWPAN_DISPATCH_FRAGN:     /* next fragments headers */
 113                         ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
 114                         if (ret == 1) {
 115                                 ret = iphc_decompress(skb, &hdr);
 116                                 if (ret < 0)
 117                                         goto drop_skb;
 118
 119                                 return lowpan_give_skb_to_device(skb, dev);
 120                         } else if (ret == -1) {
 121                                 return NET_RX_DROP;
 122                         } else {
 123                                 return NET_RX_SUCCESS;
 124                         }
 125                 default:
 126                         break;
 127                 }
 128         }
 129
 130 drop_skb:
 131         kfree_skb(skb);
 132 drop:
 133         return NET_RX_DROP;
 134 }
 135
 136 static struct packet_type lowpan_packet_type = {
 137         .type = htons(ETH_P_IEEE802154),
 138         .func = lowpan_rcv,
 139 };
 140
 141 void lowpan_rx_init(void)
 142 {
 143         dev_add_pack(&lowpan_packet_type);
 144 }
 145
 146 void lowpan_rx_exit(void)
 147 {
 148         dev_remove_pack(&lowpan_packet_type);
 149 }