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