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_beacon_push(struct sk_buff *skb, 124 struct ieee802154_beacon_frame *beacon) 125 { 126 struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl; 127 struct ieee802154_hdr *mhr = &beacon->mhr; 128 int ret; 129 130 skb_reserve(skb, sizeof(*mhr)); 131 ret = ieee802154_hdr_push(skb, mhr); 132 if (ret < 0) 133 return ret; 134 135 skb_reset_mac_header(skb); 136 skb->mac_len = ret; 137 138 skb_put_data(skb, mac_pl, sizeof(*mac_pl)); 139 140 if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count) 141 return -EOPNOTSUPP; 142 143 return 0; 144 } 145 EXPORT_SYMBOL_GPL(ieee802154_beacon_push); 146 147 static int 148 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan, 149 struct ieee802154_addr *addr) 150 { 151 int pos = 0; 152 153 addr->mode = mode; 154 155 if (mode == IEEE802154_ADDR_NONE) 156 return 0; 157 158 if (!omit_pan) { 159 memcpy(&addr->pan_id, buf + pos, 2); 160 pos += 2; 161 } 162 163 if (mode == IEEE802154_ADDR_SHORT) { 164 memcpy(&addr->short_addr, buf + pos, 2); 165 return pos + 2; 166 } else { 167 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN); 168 return pos + IEEE802154_ADDR_LEN; 169 } 170 } 171 172 static int ieee802154_hdr_addr_len(int mode, bool omit_pan) 173 { 174 int pan_len = omit_pan ? 0 : 2; 175 176 switch (mode) { 177 case IEEE802154_ADDR_NONE: return 0; 178 case IEEE802154_ADDR_SHORT: return 2 + pan_len; 179 case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len; 180 default: return -EINVAL; 181 } 182 } 183 184 static int 185 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr) 186 { 187 int pos = 5; 188 189 memcpy(hdr, buf, 1); 190 memcpy(&hdr->frame_counter, buf + 1, 4); 191 192 switch (hdr->key_id_mode) { 193 case IEEE802154_SCF_KEY_IMPLICIT: 194 return pos; 195 196 case IEEE802154_SCF_KEY_INDEX: 197 break; 198 199 case IEEE802154_SCF_KEY_SHORT_INDEX: 200 memcpy(&hdr->short_src, buf + pos, 4); 201 pos += 4; 202 break; 203 204 case IEEE802154_SCF_KEY_HW_INDEX: 205 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN); 206 pos += IEEE802154_ADDR_LEN; 207 break; 208 } 209 210 hdr->key_id = buf[pos++]; 211 212 return pos; 213 } 214 215 static int ieee802154_sechdr_lengths[4] = { 216 [IEEE802154_SCF_KEY_IMPLICIT] = 5, 217 [IEEE802154_SCF_KEY_INDEX] = 6, 218 [IEEE802154_SCF_KEY_SHORT_INDEX] = 10, 219 [IEEE802154_SCF_KEY_HW_INDEX] = 14, 220 }; 221 222 static int ieee802154_hdr_sechdr_len(u8 sc) 223 { 224 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)]; 225 } 226 227 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr) 228 { 229 int dlen, slen; 230 231 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false); 232 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode, 233 hdr->fc.intra_pan); 234 235 if (slen < 0 || dlen < 0) 236 return -EINVAL; 237 238 return 3 + dlen + slen + hdr->fc.security_enabled; 239 } 240 241 static int 242 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr) 243 { 244 int pos = 0; 245 246 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode, 247 false, &hdr->dest); 248 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode, 249 hdr->fc.intra_pan, &hdr->source); 250 251 if (hdr->fc.intra_pan) 252 hdr->source.pan_id = hdr->dest.pan_id; 253 254 return pos; 255 } 256 257 int 258 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr) 259 { 260 int pos = 3, rc; 261 262 if (!pskb_may_pull(skb, 3)) 263 return -EINVAL; 264 265 memcpy(hdr, skb->data, 3); 266 267 rc = ieee802154_hdr_minlen(hdr); 268 if (rc < 0 || !pskb_may_pull(skb, rc)) 269 return -EINVAL; 270 271 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr); 272 273 if (hdr->fc.security_enabled) { 274 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]); 275 276 if (!pskb_may_pull(skb, want)) 277 return -EINVAL; 278 279 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec); 280 } 281 282 skb_pull(skb, pos); 283 return pos; 284 } 285 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull); 286 287 int 288 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 289 { 290 const u8 *buf = skb_mac_header(skb); 291 int pos = 3, rc; 292 293 if (buf + 3 > skb_tail_pointer(skb)) 294 return -EINVAL; 295 296 memcpy(hdr, buf, 3); 297 298 rc = ieee802154_hdr_minlen(hdr); 299 if (rc < 0 || buf + rc > skb_tail_pointer(skb)) 300 return -EINVAL; 301 302 pos += ieee802154_hdr_get_addrs(buf + pos, hdr); 303 return pos; 304 } 305 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs); 306 307 int 308 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 309 { 310 const u8 *buf = skb_mac_header(skb); 311 int pos; 312 313 pos = ieee802154_hdr_peek_addrs(skb, hdr); 314 if (pos < 0) 315 return -EINVAL; 316 317 if (hdr->fc.security_enabled) { 318 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos)); 319 int want = pos + ieee802154_sechdr_lengths[key_id_mode]; 320 321 if (buf + want > skb_tail_pointer(skb)) 322 return -EINVAL; 323 324 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec); 325 } 326 327 return pos; 328 } 329 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek); 330 331 int ieee802154_max_payload(const struct ieee802154_hdr *hdr) 332 { 333 int hlen = ieee802154_hdr_minlen(hdr); 334 335 if (hdr->fc.security_enabled) { 336 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1; 337 hlen += ieee802154_sechdr_authtag_len(&hdr->sec); 338 } 339 340 return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE; 341 } 342 EXPORT_SYMBOL_GPL(ieee802154_max_payload); 343