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
ieee802154_hdr_push_addr(u8 * buf,const struct ieee802154_addr * addr,bool omit_pan)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
ieee802154_hdr_push_sechdr(u8 * buf,const struct ieee802154_sechdr * hdr)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
ieee802154_hdr_push(struct sk_buff * skb,const struct ieee802154_hdr * hdr)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
ieee802154_hdr_get_addr(const u8 * buf,int mode,bool omit_pan,struct ieee802154_addr * addr)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
ieee802154_hdr_addr_len(int mode,bool omit_pan)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
ieee802154_hdr_get_sechdr(const u8 * buf,struct ieee802154_sechdr * hdr)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
ieee802154_hdr_sechdr_len(u8 sc)205 static int ieee802154_hdr_sechdr_len(u8 sc)
206 {
207 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
208 }
209
ieee802154_hdr_minlen(const struct ieee802154_hdr * hdr)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
ieee802154_hdr_get_addrs(const u8 * buf,struct ieee802154_hdr * hdr)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
ieee802154_hdr_pull(struct sk_buff * skb,struct ieee802154_hdr * hdr)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
ieee802154_hdr_peek_addrs(const struct sk_buff * skb,struct ieee802154_hdr * hdr)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
ieee802154_hdr_peek(const struct sk_buff * skb,struct ieee802154_hdr * hdr)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
ieee802154_max_payload(const struct ieee802154_hdr * hdr)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