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