1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * Copyright (C) 2014 Fraunhofer ITWM 4 * 5 * Written by: 6 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de> 7 */ 8 9 #include <linux/ieee802154.h> 10 11 #include <net/mac802154.h> 12 #include <net/ieee802154_netdev.h> 13 14 static int 15 ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr, 16 bool omit_pan) 17 { 18 int pos = 0; 19 20 if (addr->mode == IEEE802154_ADDR_NONE) 21 return 0; 22 23 if (!omit_pan) { 24 memcpy(buf + pos, &addr->pan_id, 2); 25 pos += 2; 26 } 27 28 switch (addr->mode) { 29 case IEEE802154_ADDR_SHORT: 30 memcpy(buf + pos, &addr->short_addr, 2); 31 pos += 2; 32 break; 33 34 case IEEE802154_ADDR_LONG: 35 memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN); 36 pos += IEEE802154_ADDR_LEN; 37 break; 38 39 default: 40 return -EINVAL; 41 } 42 43 return pos; 44 } 45 46 static int 47 ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr) 48 { 49 int pos = 5; 50 51 memcpy(buf, hdr, 1); 52 memcpy(buf + 1, &hdr->frame_counter, 4); 53 54 switch (hdr->key_id_mode) { 55 case IEEE802154_SCF_KEY_IMPLICIT: 56 return pos; 57 58 case IEEE802154_SCF_KEY_INDEX: 59 break; 60 61 case IEEE802154_SCF_KEY_SHORT_INDEX: 62 memcpy(buf + pos, &hdr->short_src, 4); 63 pos += 4; 64 break; 65 66 case IEEE802154_SCF_KEY_HW_INDEX: 67 memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN); 68 pos += IEEE802154_ADDR_LEN; 69 break; 70 } 71 72 buf[pos++] = hdr->key_id; 73 74 return pos; 75 } 76 77 int 78 ieee802154_hdr_push(struct sk_buff *skb, struct ieee802154_hdr *hdr) 79 { 80 u8 buf[IEEE802154_MAX_HEADER_LEN]; 81 int pos = 2; 82 int rc; 83 struct ieee802154_hdr_fc *fc = &hdr->fc; 84 85 buf[pos++] = hdr->seq; 86 87 fc->dest_addr_mode = hdr->dest.mode; 88 89 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false); 90 if (rc < 0) 91 return -EINVAL; 92 pos += rc; 93 94 fc->source_addr_mode = hdr->source.mode; 95 96 if (hdr->source.pan_id == hdr->dest.pan_id && 97 hdr->dest.mode != IEEE802154_ADDR_NONE) 98 fc->intra_pan = true; 99 100 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan); 101 if (rc < 0) 102 return -EINVAL; 103 pos += rc; 104 105 if (fc->security_enabled) { 106 fc->version = 1; 107 108 rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec); 109 if (rc < 0) 110 return -EINVAL; 111 112 pos += rc; 113 } 114 115 memcpy(buf, fc, 2); 116 117 memcpy(skb_push(skb, pos), buf, pos); 118 119 return pos; 120 } 121 EXPORT_SYMBOL_GPL(ieee802154_hdr_push); 122 123 int ieee802154_mac_cmd_push(struct sk_buff *skb, void *f, 124 const void *pl, unsigned int pl_len) 125 { 126 struct ieee802154_mac_cmd_frame *frame = f; 127 struct ieee802154_mac_cmd_pl *mac_pl = &frame->mac_pl; 128 struct ieee802154_hdr *mhr = &frame->mhr; 129 int ret; 130 131 skb_reserve(skb, sizeof(*mhr)); 132 ret = ieee802154_hdr_push(skb, mhr); 133 if (ret < 0) 134 return ret; 135 136 skb_reset_mac_header(skb); 137 skb->mac_len = ret; 138 139 skb_put_data(skb, mac_pl, sizeof(*mac_pl)); 140 skb_put_data(skb, pl, pl_len); 141 142 return 0; 143 } 144 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_push); 145 146 int ieee802154_beacon_push(struct sk_buff *skb, 147 struct ieee802154_beacon_frame *beacon) 148 { 149 struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl; 150 struct ieee802154_hdr *mhr = &beacon->mhr; 151 int ret; 152 153 skb_reserve(skb, sizeof(*mhr)); 154 ret = ieee802154_hdr_push(skb, mhr); 155 if (ret < 0) 156 return ret; 157 158 skb_reset_mac_header(skb); 159 skb->mac_len = ret; 160 161 skb_put_data(skb, mac_pl, sizeof(*mac_pl)); 162 163 if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count) 164 return -EOPNOTSUPP; 165 166 return 0; 167 } 168 EXPORT_SYMBOL_GPL(ieee802154_beacon_push); 169 170 static int 171 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan, 172 struct ieee802154_addr *addr) 173 { 174 int pos = 0; 175 176 if (mode == IEEE802154_ADDR_NONE) { 177 memset(addr, 0, sizeof(*addr)); 178 addr->mode = IEEE802154_ADDR_NONE; 179 return 0; 180 } 181 182 addr->mode = mode; 183 184 if (!omit_pan) { 185 memcpy(&addr->pan_id, buf + pos, 2); 186 pos += 2; 187 } 188 189 if (mode == IEEE802154_ADDR_SHORT) { 190 memcpy(&addr->short_addr, buf + pos, 2); 191 return pos + 2; 192 } else { 193 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN); 194 return pos + IEEE802154_ADDR_LEN; 195 } 196 } 197 198 static int ieee802154_hdr_addr_len(int mode, bool omit_pan) 199 { 200 int pan_len = omit_pan ? 0 : 2; 201 202 switch (mode) { 203 case IEEE802154_ADDR_NONE: return 0; 204 case IEEE802154_ADDR_SHORT: return 2 + pan_len; 205 case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len; 206 default: return -EINVAL; 207 } 208 } 209 210 static int 211 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr) 212 { 213 int pos = 5; 214 215 memcpy(hdr, buf, 1); 216 memcpy(&hdr->frame_counter, buf + 1, 4); 217 218 switch (hdr->key_id_mode) { 219 case IEEE802154_SCF_KEY_IMPLICIT: 220 return pos; 221 222 case IEEE802154_SCF_KEY_INDEX: 223 break; 224 225 case IEEE802154_SCF_KEY_SHORT_INDEX: 226 memcpy(&hdr->short_src, buf + pos, 4); 227 pos += 4; 228 break; 229 230 case IEEE802154_SCF_KEY_HW_INDEX: 231 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN); 232 pos += IEEE802154_ADDR_LEN; 233 break; 234 } 235 236 hdr->key_id = buf[pos++]; 237 238 return pos; 239 } 240 241 static int ieee802154_sechdr_lengths[4] = { 242 [IEEE802154_SCF_KEY_IMPLICIT] = 5, 243 [IEEE802154_SCF_KEY_INDEX] = 6, 244 [IEEE802154_SCF_KEY_SHORT_INDEX] = 10, 245 [IEEE802154_SCF_KEY_HW_INDEX] = 14, 246 }; 247 248 static int ieee802154_hdr_sechdr_len(u8 sc) 249 { 250 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)]; 251 } 252 253 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr) 254 { 255 int dlen, slen; 256 257 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false); 258 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode, 259 hdr->fc.intra_pan); 260 261 if (slen < 0 || dlen < 0) 262 return -EINVAL; 263 264 return 3 + dlen + slen + hdr->fc.security_enabled; 265 } 266 267 static int 268 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr) 269 { 270 int pos = 0; 271 272 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode, 273 false, &hdr->dest); 274 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode, 275 hdr->fc.intra_pan, &hdr->source); 276 277 if (hdr->fc.intra_pan) 278 hdr->source.pan_id = hdr->dest.pan_id; 279 280 return pos; 281 } 282 283 int 284 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr) 285 { 286 int pos = 3, rc; 287 288 if (!pskb_may_pull(skb, 3)) 289 return -EINVAL; 290 291 memcpy(hdr, skb->data, 3); 292 293 rc = ieee802154_hdr_minlen(hdr); 294 if (rc < 0 || !pskb_may_pull(skb, rc)) 295 return -EINVAL; 296 297 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr); 298 299 if (hdr->fc.security_enabled) { 300 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]); 301 302 if (!pskb_may_pull(skb, want)) 303 return -EINVAL; 304 305 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec); 306 } 307 308 skb_pull(skb, pos); 309 return pos; 310 } 311 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull); 312 313 int ieee802154_mac_cmd_pl_pull(struct sk_buff *skb, 314 struct ieee802154_mac_cmd_pl *mac_pl) 315 { 316 if (!pskb_may_pull(skb, sizeof(*mac_pl))) 317 return -EINVAL; 318 319 memcpy(mac_pl, skb->data, sizeof(*mac_pl)); 320 skb_pull(skb, sizeof(*mac_pl)); 321 322 return 0; 323 } 324 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_pl_pull); 325 326 int 327 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 328 { 329 const u8 *buf = skb_mac_header(skb); 330 int pos = 3, rc; 331 332 if (buf + 3 > skb_tail_pointer(skb)) 333 return -EINVAL; 334 335 memcpy(hdr, buf, 3); 336 337 rc = ieee802154_hdr_minlen(hdr); 338 if (rc < 0 || buf + rc > skb_tail_pointer(skb)) 339 return -EINVAL; 340 341 pos += ieee802154_hdr_get_addrs(buf + pos, hdr); 342 return pos; 343 } 344 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs); 345 346 int 347 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 348 { 349 const u8 *buf = skb_mac_header(skb); 350 int pos; 351 352 pos = ieee802154_hdr_peek_addrs(skb, hdr); 353 if (pos < 0) 354 return -EINVAL; 355 356 if (hdr->fc.security_enabled) { 357 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos)); 358 int want = pos + ieee802154_sechdr_lengths[key_id_mode]; 359 360 if (buf + want > skb_tail_pointer(skb)) 361 return -EINVAL; 362 363 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec); 364 } 365 366 return pos; 367 } 368 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek); 369 370 int ieee802154_max_payload(const struct ieee802154_hdr *hdr) 371 { 372 int hlen = ieee802154_hdr_minlen(hdr); 373 374 if (hdr->fc.security_enabled) { 375 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1; 376 hlen += ieee802154_sechdr_authtag_len(&hdr->sec); 377 } 378 379 return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE; 380 } 381 EXPORT_SYMBOL_GPL(ieee802154_max_payload); 382