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         if (dev->type != ARPHRD_IEEE802154 ||
  71             !dev->ieee802154_ptr->lowpan_dev)
  72                 goto drop;
  73
  74         skb = skb_share_check(skb, GFP_ATOMIC);
  75         if (!skb)
  76                 goto drop;
  77
  78         if (!netif_running(dev))
  79                 goto drop_skb;
  80
  81         if (skb->pkt_type == PACKET_OTHERHOST)
  82                 goto drop_skb;
  83
  84         if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
  85                 goto drop_skb;
  86
  87         /* check that it's our buffer */
  88         if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
  89                 /* Pull off the 1-byte of 6lowpan header. */
  90                 skb_pull(skb, 1);
  91                 return lowpan_give_skb_to_device(skb, dev);
  92         } else {
  93                 switch (skb->data[0] & 0xe0) {
  94                 case LOWPAN_DISPATCH_IPHC:      /* ipv6 datagram */
  95                         ret = iphc_decompress(skb, &hdr);
  96                         if (ret < 0)
  97                                 goto drop_skb;
  98
  99                         return lowpan_give_skb_to_device(skb, dev);
 100                 case LOWPAN_DISPATCH_FRAG1:     /* first fragment header */
 101                         ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
 102                         if (ret == 1) {
 103                                 ret = iphc_decompress(skb, &hdr);
 104                                 if (ret < 0)
 105                                         goto drop_skb;
 106
 107                                 return lowpan_give_skb_to_device(skb, dev);
 108                         } else if (ret == -1) {
 109                                 return NET_RX_DROP;
 110                         } else {
 111                                 return NET_RX_SUCCESS;
 112                         }
 113                 case LOWPAN_DISPATCH_FRAGN:     /* next fragments headers */
 114                         ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
 115                         if (ret == 1) {
 116                                 ret = iphc_decompress(skb, &hdr);
 117                                 if (ret < 0)
 118                                         goto drop_skb;
 119
 120                                 return lowpan_give_skb_to_device(skb, dev);
 121                         } else if (ret == -1) {
 122                                 return NET_RX_DROP;
 123                         } else {
 124                                 return NET_RX_SUCCESS;
 125                         }
 126                 default:
 127                         break;
 128                 }
 129         }
 130
 131 drop_skb:
 132         kfree_skb(skb);
 133 drop:
 134         return NET_RX_DROP;
 135 }
 136
 137 static struct packet_type lowpan_packet_type = {
 138         .type = htons(ETH_P_IEEE802154),
 139         .func = lowpan_rcv,
 140 };
 141
 142 void lowpan_rx_init(void)
 143 {
 144         dev_add_pack(&lowpan_packet_type);
 145 }
 146
 147 void lowpan_rx_exit(void)
 148 {
 149         dev_remove_pack(&lowpan_packet_type);
 150 }