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