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