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 } 172