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 wpan_dev *wpan_dev = lowpan_dev_info(dev)->real_dev->ieee802154_ptr; 194 struct ieee802154_addr sa, da; 195 struct ieee802154_mac_cb *cb = mac_cb_init(skb); 196 struct lowpan_addr_info info; 197 void *daddr, *saddr; 198 199 memcpy(&info, lowpan_skb_priv(skb), sizeof(info)); 200 201 /* TODO: Currently we only support extended_addr */ 202 daddr = &info.daddr.u.extended_addr; 203 saddr = &info.saddr.u.extended_addr; 204 205 lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len); 206 207 cb->type = IEEE802154_FC_TYPE_DATA; 208 209 /* prepare wpan address data */ 210 sa.mode = IEEE802154_ADDR_LONG; 211 sa.pan_id = wpan_dev->pan_id; 212 sa.extended_addr = ieee802154_devaddr_from_raw(saddr); 213 214 /* intra-PAN communications */ 215 da.pan_id = sa.pan_id; 216 217 /* if the destination address is the broadcast address, use the 218 * corresponding short address 219 */ 220 if (lowpan_is_addr_broadcast((const u8 *)daddr)) { 221 da.mode = IEEE802154_ADDR_SHORT; 222 da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST); 223 cb->ackreq = false; 224 } else { 225 da.mode = IEEE802154_ADDR_LONG; 226 da.extended_addr = ieee802154_devaddr_from_raw(daddr); 227 cb->ackreq = wpan_dev->frame_retries >= 0; 228 } 229 230 return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, 231 ETH_P_IPV6, (void *)&da, (void *)&sa, 0); 232 } 233 234 netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev) 235 { 236 struct ieee802154_hdr wpan_hdr; 237 int max_single, ret; 238 239 pr_debug("package xmit\n"); 240 241 /* We must take a copy of the skb before we modify/replace the ipv6 242 * header as the header could be used elsewhere 243 */ 244 skb = skb_unshare(skb, GFP_ATOMIC); 245 if (!skb) 246 return NET_XMIT_DROP; 247 248 ret = lowpan_header(skb, dev); 249 if (ret < 0) { 250 kfree_skb(skb); 251 return NET_XMIT_DROP; 252 } 253 254 if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) { 255 kfree_skb(skb); 256 return NET_XMIT_DROP; 257 } 258 259 max_single = ieee802154_max_payload(&wpan_hdr); 260 261 if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) { 262 skb->dev = lowpan_dev_info(dev)->real_dev; 263 return dev_queue_xmit(skb); 264 } else { 265 netdev_tx_t rc; 266 267 pr_debug("frame is too big, fragmentation is needed\n"); 268 rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr); 269 270 return rc < 0 ? NET_XMIT_DROP : rc; 271 } 272 } 273