1 /* 2 * Copyright (C) 2014 Fraunhofer ITWM 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License version 2 6 * as published by the Free Software Foundation. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 * 13 * Written by: 14 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de> 15 */ 16 17 #include <linux/ieee802154.h> 18 19 #include <net/mac802154.h> 20 #include <net/ieee802154_netdev.h> 21 22 static int 23 ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr, 24 bool omit_pan) 25 { 26 int pos = 0; 27 28 if (addr->mode == IEEE802154_ADDR_NONE) 29 return 0; 30 31 if (!omit_pan) { 32 memcpy(buf + pos, &addr->pan_id, 2); 33 pos += 2; 34 } 35 36 switch (addr->mode) { 37 case IEEE802154_ADDR_SHORT: 38 memcpy(buf + pos, &addr->short_addr, 2); 39 pos += 2; 40 break; 41 42 case IEEE802154_ADDR_LONG: 43 memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN); 44 pos += IEEE802154_ADDR_LEN; 45 break; 46 47 default: 48 return -EINVAL; 49 } 50 51 return pos; 52 } 53 54 static int 55 ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr) 56 { 57 int pos = 5; 58 59 memcpy(buf, hdr, 1); 60 memcpy(buf + 1, &hdr->frame_counter, 4); 61 62 switch (hdr->key_id_mode) { 63 case IEEE802154_SCF_KEY_IMPLICIT: 64 return pos; 65 66 case IEEE802154_SCF_KEY_INDEX: 67 break; 68 69 case IEEE802154_SCF_KEY_SHORT_INDEX: 70 memcpy(buf + pos, &hdr->short_src, 4); 71 pos += 4; 72 break; 73 74 case IEEE802154_SCF_KEY_HW_INDEX: 75 memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN); 76 pos += IEEE802154_ADDR_LEN; 77 break; 78 } 79 80 buf[pos++] = hdr->key_id; 81 82 return pos; 83 } 84 85 int 86 ieee802154_hdr_push(struct sk_buff *skb, const struct ieee802154_hdr *hdr) 87 { 88 u8 buf[MAC802154_FRAME_HARD_HEADER_LEN]; 89 int pos = 2; 90 int rc; 91 struct ieee802154_hdr_fc fc = hdr->fc; 92 93 buf[pos++] = hdr->seq; 94 95 fc.dest_addr_mode = hdr->dest.mode; 96 97 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false); 98 if (rc < 0) 99 return -EINVAL; 100 pos += rc; 101 102 fc.source_addr_mode = hdr->source.mode; 103 104 if (hdr->source.pan_id == hdr->dest.pan_id && 105 hdr->dest.mode != IEEE802154_ADDR_NONE) 106 fc.intra_pan = true; 107 108 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc.intra_pan); 109 if (rc < 0) 110 return -EINVAL; 111 pos += rc; 112 113 if (fc.security_enabled) { 114 fc.version = 1; 115 116 rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec); 117 if (rc < 0) 118 return -EINVAL; 119 120 pos += rc; 121 } 122 123 memcpy(buf, &fc, 2); 124 125 memcpy(skb_push(skb, pos), buf, pos); 126 127 return pos; 128 } 129 EXPORT_SYMBOL_GPL(ieee802154_hdr_push); 130 131 static int 132 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan, 133 struct ieee802154_addr *addr) 134 { 135 int pos = 0; 136 137 addr->mode = mode; 138 139 if (mode == IEEE802154_ADDR_NONE) 140 return 0; 141 142 if (!omit_pan) { 143 memcpy(&addr->pan_id, buf + pos, 2); 144 pos += 2; 145 } 146 147 if (mode == IEEE802154_ADDR_SHORT) { 148 memcpy(&addr->short_addr, buf + pos, 2); 149 return pos + 2; 150 } else { 151 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN); 152 return pos + IEEE802154_ADDR_LEN; 153 } 154 } 155 156 static int ieee802154_hdr_addr_len(int mode, bool omit_pan) 157 { 158 int pan_len = omit_pan ? 0 : 2; 159 160 switch (mode) { 161 case IEEE802154_ADDR_NONE: return 0; 162 case IEEE802154_ADDR_SHORT: return 2 + pan_len; 163 case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len; 164 default: return -EINVAL; 165 } 166 } 167 168 static int 169 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr) 170 { 171 int pos = 5; 172 173 memcpy(hdr, buf, 1); 174 memcpy(&hdr->frame_counter, buf + 1, 4); 175 176 switch (hdr->key_id_mode) { 177 case IEEE802154_SCF_KEY_IMPLICIT: 178 return pos; 179 180 case IEEE802154_SCF_KEY_INDEX: 181 break; 182 183 case IEEE802154_SCF_KEY_SHORT_INDEX: 184 memcpy(&hdr->short_src, buf + pos, 4); 185 pos += 4; 186 break; 187 188 case IEEE802154_SCF_KEY_HW_INDEX: 189 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN); 190 pos += IEEE802154_ADDR_LEN; 191 break; 192 } 193 194 hdr->key_id = buf[pos++]; 195 196 return pos; 197 } 198 199 static int ieee802154_sechdr_lengths[4] = { 200 [IEEE802154_SCF_KEY_IMPLICIT] = 5, 201 [IEEE802154_SCF_KEY_INDEX] = 6, 202 [IEEE802154_SCF_KEY_SHORT_INDEX] = 10, 203 [IEEE802154_SCF_KEY_HW_INDEX] = 14, 204 }; 205 206 static int ieee802154_hdr_sechdr_len(u8 sc) 207 { 208 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)]; 209 } 210 211 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr) 212 { 213 int dlen, slen; 214 215 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false); 216 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode, 217 hdr->fc.intra_pan); 218 219 if (slen < 0 || dlen < 0) 220 return -EINVAL; 221 222 return 3 + dlen + slen + hdr->fc.security_enabled; 223 } 224 225 static int 226 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr) 227 { 228 int pos = 0; 229 230 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode, 231 false, &hdr->dest); 232 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode, 233 hdr->fc.intra_pan, &hdr->source); 234 235 if (hdr->fc.intra_pan) 236 hdr->source.pan_id = hdr->dest.pan_id; 237 238 return pos; 239 } 240 241 int 242 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr) 243 { 244 int pos = 3, rc; 245 246 if (!pskb_may_pull(skb, 3)) 247 return -EINVAL; 248 249 memcpy(hdr, skb->data, 3); 250 251 rc = ieee802154_hdr_minlen(hdr); 252 if (rc < 0 || !pskb_may_pull(skb, rc)) 253 return -EINVAL; 254 255 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr); 256 257 if (hdr->fc.security_enabled) { 258 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]); 259 260 if (!pskb_may_pull(skb, want)) 261 return -EINVAL; 262 263 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec); 264 } 265 266 skb_pull(skb, pos); 267 return pos; 268 } 269 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull); 270 271 int 272 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 273 { 274 const u8 *buf = skb_mac_header(skb); 275 int pos = 3, rc; 276 277 if (buf + 3 > skb_tail_pointer(skb)) 278 return -EINVAL; 279 280 memcpy(hdr, buf, 3); 281 282 rc = ieee802154_hdr_minlen(hdr); 283 if (rc < 0 || buf + rc > skb_tail_pointer(skb)) 284 return -EINVAL; 285 286 pos += ieee802154_hdr_get_addrs(buf + pos, hdr); 287 return pos; 288 } 289 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs); 290 291 int 292 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 293 { 294 const u8 *buf = skb_mac_header(skb); 295 int pos; 296 297 pos = ieee802154_hdr_peek_addrs(skb, hdr); 298 if (pos < 0) 299 return -EINVAL; 300 301 if (hdr->fc.security_enabled) { 302 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos)); 303 int want = pos + ieee802154_sechdr_lengths[key_id_mode]; 304 305 if (buf + want > skb_tail_pointer(skb)) 306 return -EINVAL; 307 308 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec); 309 } 310 311 return pos; 312 } 313 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek); 314 315 int ieee802154_max_payload(const struct ieee802154_hdr *hdr) 316 { 317 int hlen = ieee802154_hdr_minlen(hdr); 318 319 if (hdr->fc.security_enabled) { 320 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1; 321 hlen += ieee802154_sechdr_authtag_len(&hdr->sec); 322 } 323 324 return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE; 325 } 326 EXPORT_SYMBOL_GPL(ieee802154_max_payload); 327