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