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 addr->mode = mode; 177 178 if (mode == IEEE802154_ADDR_NONE) 179 return 0; 180 181 if (!omit_pan) { 182 memcpy(&addr->pan_id, buf + pos, 2); 183 pos += 2; 184 } 185 186 if (mode == IEEE802154_ADDR_SHORT) { 187 memcpy(&addr->short_addr, buf + pos, 2); 188 return pos + 2; 189 } else { 190 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN); 191 return pos + IEEE802154_ADDR_LEN; 192 } 193 } 194 195 static int ieee802154_hdr_addr_len(int mode, bool omit_pan) 196 { 197 int pan_len = omit_pan ? 0 : 2; 198 199 switch (mode) { 200 case IEEE802154_ADDR_NONE: return 0; 201 case IEEE802154_ADDR_SHORT: return 2 + pan_len; 202 case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len; 203 default: return -EINVAL; 204 } 205 } 206 207 static int 208 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr) 209 { 210 int pos = 5; 211 212 memcpy(hdr, buf, 1); 213 memcpy(&hdr->frame_counter, buf + 1, 4); 214 215 switch (hdr->key_id_mode) { 216 case IEEE802154_SCF_KEY_IMPLICIT: 217 return pos; 218 219 case IEEE802154_SCF_KEY_INDEX: 220 break; 221 222 case IEEE802154_SCF_KEY_SHORT_INDEX: 223 memcpy(&hdr->short_src, buf + pos, 4); 224 pos += 4; 225 break; 226 227 case IEEE802154_SCF_KEY_HW_INDEX: 228 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN); 229 pos += IEEE802154_ADDR_LEN; 230 break; 231 } 232 233 hdr->key_id = buf[pos++]; 234 235 return pos; 236 } 237 238 static int ieee802154_sechdr_lengths[4] = { 239 [IEEE802154_SCF_KEY_IMPLICIT] = 5, 240 [IEEE802154_SCF_KEY_INDEX] = 6, 241 [IEEE802154_SCF_KEY_SHORT_INDEX] = 10, 242 [IEEE802154_SCF_KEY_HW_INDEX] = 14, 243 }; 244 245 static int ieee802154_hdr_sechdr_len(u8 sc) 246 { 247 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)]; 248 } 249 250 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr) 251 { 252 int dlen, slen; 253 254 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false); 255 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode, 256 hdr->fc.intra_pan); 257 258 if (slen < 0 || dlen < 0) 259 return -EINVAL; 260 261 return 3 + dlen + slen + hdr->fc.security_enabled; 262 } 263 264 static int 265 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr) 266 { 267 int pos = 0; 268 269 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode, 270 false, &hdr->dest); 271 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode, 272 hdr->fc.intra_pan, &hdr->source); 273 274 if (hdr->fc.intra_pan) 275 hdr->source.pan_id = hdr->dest.pan_id; 276 277 return pos; 278 } 279 280 int 281 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr) 282 { 283 int pos = 3, rc; 284 285 if (!pskb_may_pull(skb, 3)) 286 return -EINVAL; 287 288 memcpy(hdr, skb->data, 3); 289 290 rc = ieee802154_hdr_minlen(hdr); 291 if (rc < 0 || !pskb_may_pull(skb, rc)) 292 return -EINVAL; 293 294 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr); 295 296 if (hdr->fc.security_enabled) { 297 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]); 298 299 if (!pskb_may_pull(skb, want)) 300 return -EINVAL; 301 302 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec); 303 } 304 305 skb_pull(skb, pos); 306 return pos; 307 } 308 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull); 309 310 int ieee802154_mac_cmd_pl_pull(struct sk_buff *skb, 311 struct ieee802154_mac_cmd_pl *mac_pl) 312 { 313 if (!pskb_may_pull(skb, sizeof(*mac_pl))) 314 return -EINVAL; 315 316 memcpy(mac_pl, skb->data, sizeof(*mac_pl)); 317 skb_pull(skb, sizeof(*mac_pl)); 318 319 return 0; 320 } 321 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_pl_pull); 322 323 int 324 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 325 { 326 const u8 *buf = skb_mac_header(skb); 327 int pos = 3, rc; 328 329 if (buf + 3 > skb_tail_pointer(skb)) 330 return -EINVAL; 331 332 memcpy(hdr, buf, 3); 333 334 rc = ieee802154_hdr_minlen(hdr); 335 if (rc < 0 || buf + rc > skb_tail_pointer(skb)) 336 return -EINVAL; 337 338 pos += ieee802154_hdr_get_addrs(buf + pos, hdr); 339 return pos; 340 } 341 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs); 342 343 int 344 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 345 { 346 const u8 *buf = skb_mac_header(skb); 347 int pos; 348 349 pos = ieee802154_hdr_peek_addrs(skb, hdr); 350 if (pos < 0) 351 return -EINVAL; 352 353 if (hdr->fc.security_enabled) { 354 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos)); 355 int want = pos + ieee802154_sechdr_lengths[key_id_mode]; 356 357 if (buf + want > skb_tail_pointer(skb)) 358 return -EINVAL; 359 360 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec); 361 } 362 363 return pos; 364 } 365 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek); 366 367 int ieee802154_max_payload(const struct ieee802154_hdr *hdr) 368 { 369 int hlen = ieee802154_hdr_minlen(hdr); 370 371 if (hdr->fc.security_enabled) { 372 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1; 373 hlen += ieee802154_sechdr_authtag_len(&hdr->sec); 374 } 375 376 return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE; 377 } 378 EXPORT_SYMBOL_GPL(ieee802154_max_payload); 379