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_devices(struct sk_buff *skb,
  19                                       struct net_device *dev)
  20 {
  21         struct lowpan_dev_record *entry;
  22         struct sk_buff *skb_cp;
  23         int stat = NET_RX_SUCCESS;
  24
  25         skb->protocol = htons(ETH_P_IPV6);
  26         skb->pkt_type = PACKET_HOST;
  27
  28         rcu_read_lock();
  29         list_for_each_entry_rcu(entry, &lowpan_devices, list)
  30                 if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
  31                         skb_cp = skb_copy(skb, GFP_ATOMIC);
  32                         if (!skb_cp) {
  33                                 kfree_skb(skb);
  34                                 rcu_read_unlock();
  35                                 return NET_RX_DROP;
  36                         }
  37
  38                         skb_cp->dev = entry->ldev;
  39                         stat = netif_rx(skb_cp);
  40                         if (stat == NET_RX_DROP)
  41                                 break;
  42                 }
  43         rcu_read_unlock();
  44
  45         consume_skb(skb);
  46
  47         return stat;
  48 }
  49
  50 static int
  51 iphc_decompress(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
  52 {
  53         u8 iphc0, iphc1;
  54         struct ieee802154_addr_sa sa, da;
  55         void *sap, *dap;
  56
  57         raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
  58         /* at least two bytes will be used for the encoding */
  59         if (skb->len < 2)
  60                 return -EINVAL;
  61
  62         if (lowpan_fetch_skb_u8(skb, &iphc0))
  63                 return -EINVAL;
  64
  65         if (lowpan_fetch_skb_u8(skb, &iphc1))
  66                 return -EINVAL;
  67
  68         ieee802154_addr_to_sa(&sa, &hdr->source);
  69         ieee802154_addr_to_sa(&da, &hdr->dest);
  70
  71         if (sa.addr_type == IEEE802154_ADDR_SHORT)
  72                 sap = &sa.short_addr;
  73         else
  74                 sap = &sa.hwaddr;
  75
  76         if (da.addr_type == IEEE802154_ADDR_SHORT)
  77                 dap = &da.short_addr;
  78         else
  79                 dap = &da.hwaddr;
  80
  81         return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
  82                                         IEEE802154_ADDR_LEN, dap, da.addr_type,
  83                                         IEEE802154_ADDR_LEN, iphc0, iphc1);
  84 }
  85
  86 static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
  87                       struct packet_type *pt, struct net_device *orig_dev)
  88 {
  89         struct ieee802154_hdr hdr;
  90         int ret;
  91
  92         skb = skb_share_check(skb, GFP_ATOMIC);
  93         if (!skb)
  94                 goto drop;
  95
  96         if (!netif_running(dev))
  97                 goto drop_skb;
  98
  99         if (skb->pkt_type == PACKET_OTHERHOST)
 100                 goto drop_skb;
 101
 102         if (dev->type != ARPHRD_IEEE802154)
 103                 goto drop_skb;
 104
 105         if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
 106                 goto drop_skb;
 107
 108         /* check that it's our buffer */
 109         if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
 110                 /* Pull off the 1-byte of 6lowpan header. */
 111                 skb_pull(skb, 1);
 112                 return lowpan_give_skb_to_devices(skb, NULL);
 113         } else {
 114                 switch (skb->data[0] & 0xe0) {
 115                 case LOWPAN_DISPATCH_IPHC:      /* ipv6 datagram */
 116                         ret = iphc_decompress(skb, &hdr);
 117                         if (ret < 0)
 118                                 goto drop_skb;
 119
 120                         return lowpan_give_skb_to_devices(skb, NULL);
 121                 case LOWPAN_DISPATCH_FRAG1:     /* first fragment header */
 122                         ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
 123                         if (ret == 1) {
 124                                 ret = iphc_decompress(skb, &hdr);
 125                                 if (ret < 0)
 126                                         goto drop_skb;
 127
 128                                 return lowpan_give_skb_to_devices(skb, NULL);
 129                         } else if (ret == -1) {
 130                                 return NET_RX_DROP;
 131                         } else {
 132                                 return NET_RX_SUCCESS;
 133                         }
 134                 case LOWPAN_DISPATCH_FRAGN:     /* next fragments headers */
 135                         ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
 136                         if (ret == 1) {
 137                                 ret = iphc_decompress(skb, &hdr);
 138                                 if (ret < 0)
 139                                         goto drop_skb;
 140
 141                                 return lowpan_give_skb_to_devices(skb, NULL);
 142                         } else if (ret == -1) {
 143                                 return NET_RX_DROP;
 144                         } else {
 145                                 return NET_RX_SUCCESS;
 146                         }
 147                 default:
 148                         break;
 149                 }
 150         }
 151
 152 drop_skb:
 153         kfree_skb(skb);
 154 drop:
 155         return NET_RX_DROP;
 156 }
 157
 158 static struct packet_type lowpan_packet_type = {
 159         .type = htons(ETH_P_IEEE802154),
 160         .func = lowpan_rcv,
 161 };
 162
 163 void lowpan_rx_init(void)
 164 {
 165         dev_add_pack(&lowpan_packet_type);
 166 }
 167
 168 void lowpan_rx_exit(void)
 169 {
 170         dev_remove_pack(&lowpan_packet_type);
 171 }