1
2
3
4
5
6
7
8
9#include <linux/ieee802154.h>
10
11#include <net/mac802154.h>
12#include <net/ieee802154_netdev.h>
13
14static int
15ieee802154_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
46static int
47ieee802154_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
77int
78ieee802154_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}
121EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
122
123static int
124ieee802154_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
148static 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
160static int
161ieee802154_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
191static 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
198static int ieee802154_hdr_sechdr_len(u8 sc)
199{
200 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
201}
202
203static 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
217static int
218ieee802154_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
233int
234ieee802154_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}
261EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
262
263int
264ieee802154_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}
281EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
282
283int
284ieee802154_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}
305EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
306
307int 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}
318EXPORT_SYMBOL_GPL(ieee802154_max_payload);
319
