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 <net/6lowpan.h> 12 #include <net/ieee802154_netdev.h> 13 14 #include "6lowpan_i.h" 15 16 /* don't save pan id, it's intra pan */ 17 struct lowpan_addr { 18 u8 mode; 19 union { 20 /* IPv6 needs big endian here */ 21 __be64 extended_addr; 22 __be16 short_addr; 23 } u; 24 }; 25 26 struct lowpan_addr_info { 27 struct lowpan_addr daddr; 28 struct lowpan_addr saddr; 29 }; 30 31 static inline struct 32 lowpan_addr_info *lowpan_skb_priv(const struct sk_buff *skb) 33 { 34 WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct lowpan_addr_info)); 35 return (struct lowpan_addr_info *)(skb->data - 36 sizeof(struct lowpan_addr_info)); 37 } 38 39 int lowpan_header_create(struct sk_buff *skb, struct net_device *dev, 40 unsigned short type, const void *_daddr, 41 const void *_saddr, unsigned int len) 42 { 43 const u8 *saddr = _saddr; 44 const u8 *daddr = _daddr; 45 struct lowpan_addr_info *info; 46 47 /* TODO: 48 * if this package isn't ipv6 one, where should it be routed? 49 */ 50 if (type != ETH_P_IPV6) 51 return 0; 52 53 if (!saddr) 54 saddr = dev->dev_addr; 55 56 raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8); 57 raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8); 58 59 info = lowpan_skb_priv(skb); 60 61 /* TODO: Currently we only support extended_addr */ 62 info->daddr.mode = IEEE802154_ADDR_LONG; 63 memcpy(&info->daddr.u.extended_addr, daddr, 64 sizeof(info->daddr.u.extended_addr)); 65 info->saddr.mode = IEEE802154_ADDR_LONG; 66 memcpy(&info->saddr.u.extended_addr, saddr, 67 sizeof(info->daddr.u.extended_addr)); 68 69 return 0; 70 } 71 72 static struct sk_buff* 73 lowpan_alloc_frag(struct sk_buff *skb, int size, 74 const struct ieee802154_hdr *master_hdr) 75 { 76 struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev; 77 struct sk_buff *frag; 78 int rc; 79 80 frag = alloc_skb(real_dev->hard_header_len + 81 real_dev->needed_tailroom + size, 82 GFP_ATOMIC); 83 84 if (likely(frag)) { 85 frag->dev = real_dev; 86 frag->priority = skb->priority; 87 skb_reserve(frag, real_dev->hard_header_len); 88 skb_reset_network_header(frag); 89 *mac_cb(frag) = *mac_cb(skb); 90 91 rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest, 92 &master_hdr->source, size); 93 if (rc < 0) { 94 kfree_skb(frag); 95 return ERR_PTR(rc); 96 } 97 } else { 98 frag = ERR_PTR(-ENOMEM); 99 } 100 101 return frag; 102 } 103 104 static int 105 lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr, 106 u8 *frag_hdr, int frag_hdrlen, 107 int offset, int len) 108 { 109 struct sk_buff *frag; 110 111 raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen); 112 113 frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr); 114 if (IS_ERR(frag)) 115 return -PTR_ERR(frag); 116 117 memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen); 118 memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len); 119 120 raw_dump_table(__func__, " fragment dump", frag->data, frag->len); 121 122 return dev_queue_xmit(frag); 123 } 124 125 static int 126 lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev, 127 const struct ieee802154_hdr *wpan_hdr) 128 { 129 u16 dgram_size, dgram_offset; 130 __be16 frag_tag; 131 u8 frag_hdr[5]; 132 int frag_cap, frag_len, payload_cap, rc; 133 int skb_unprocessed, skb_offset; 134 135 dgram_size = lowpan_uncompress_size(skb, &dgram_offset) - 136 skb->mac_len; 137 frag_tag = htons(lowpan_dev_info(dev)->fragment_tag); 138 lowpan_dev_info(dev)->fragment_tag++; 139 140 frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07); 141 frag_hdr[1] = dgram_size & 0xff; 142 memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag)); 143 144 payload_cap = ieee802154_max_payload(wpan_hdr); 145 146 frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE - 147 skb_network_header_len(skb), 8); 148 149 skb_offset = skb_network_header_len(skb); 150 skb_unprocessed = skb->len - skb->mac_len - skb_offset; 151 152 rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr, 153 LOWPAN_FRAG1_HEAD_SIZE, 0, 154 frag_len + skb_network_header_len(skb)); 155 if (rc) { 156 pr_debug("%s unable to send FRAG1 packet (tag: %d)", 157 __func__, ntohs(frag_tag)); 158 goto err; 159 } 160 161 frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1; 162 frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN; 163 frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8); 164 165 do { 166 dgram_offset += frag_len; 167 skb_offset += frag_len; 168 skb_unprocessed -= frag_len; 169 frag_len = min(frag_cap, skb_unprocessed); 170 171 frag_hdr[4] = dgram_offset >> 3; 172 173 rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr, 174 LOWPAN_FRAGN_HEAD_SIZE, skb_offset, 175 frag_len); 176 if (rc) { 177 pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n", 178 __func__, ntohs(frag_tag), skb_offset); 179 goto err; 180 } 181 } while (skb_unprocessed > frag_cap); 182 183 consume_skb(skb); 184 return NET_XMIT_SUCCESS; 185 186 err: 187 kfree_skb(skb); 188 return rc; 189 } 190 191 static int lowpan_header(struct sk_buff *skb, struct net_device *dev) 192 { 193 struct ieee802154_addr sa, da; 194 struct ieee802154_mac_cb *cb = mac_cb_init(skb); 195 struct lowpan_addr_info info; 196 void *daddr, *saddr; 197 198 memcpy(&info, lowpan_skb_priv(skb), sizeof(info)); 199 200 /* TODO: Currently we only support extended_addr */ 201 daddr = &info.daddr.u.extended_addr; 202 saddr = &info.saddr.u.extended_addr; 203 204 lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len); 205 206 cb->type = IEEE802154_FC_TYPE_DATA; 207 208 /* prepare wpan address data */ 209 sa.mode = IEEE802154_ADDR_LONG; 210 sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev); 211 sa.extended_addr = ieee802154_devaddr_from_raw(saddr); 212 213 /* intra-PAN communications */ 214 da.pan_id = sa.pan_id; 215 216 /* if the destination address is the broadcast address, use the 217 * corresponding short address 218 */ 219 if (lowpan_is_addr_broadcast((const u8 *)daddr)) { 220 da.mode = IEEE802154_ADDR_SHORT; 221 da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST); 222 cb->ackreq = false; 223 } else { 224 da.mode = IEEE802154_ADDR_LONG; 225 da.extended_addr = ieee802154_devaddr_from_raw(daddr); 226 cb->ackreq = true; 227 } 228 229 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, 230 ETH_P_IPV6, (void *)&da, (void *)&sa, 0); 231 } 232 233 netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev) 234 { 235 struct ieee802154_hdr wpan_hdr; 236 int max_single, ret; 237 238 pr_debug("package xmit\n"); 239 240 /* We must take a copy of the skb before we modify/replace the ipv6 241 * header as the header could be used elsewhere 242 */ 243 skb = skb_unshare(skb, GFP_ATOMIC); 244 if (!skb) 245 return NET_XMIT_DROP; 246 247 ret = lowpan_header(skb, dev); 248 if (ret < 0) { 249 kfree_skb(skb); 250 return NET_XMIT_DROP; 251 } 252 253 if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) { 254 kfree_skb(skb); 255 return NET_XMIT_DROP; 256 } 257 258 max_single = ieee802154_max_payload(&wpan_hdr); 259 260 if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) { 261 skb->dev = lowpan_dev_info(dev)->real_dev; 262 return dev_queue_xmit(skb); 263 } else { 264 netdev_tx_t rc; 265 266 pr_debug("frame is too big, fragmentation is needed\n"); 267 rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr); 268 269 return rc < 0 ? NET_XMIT_DROP : rc; 270 } 271 } 272