1 // SPDX-License-Identifier: GPL-2.0-only
3 * Copyright (C) 2014 Fraunhofer ITWM
6 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
9 #include <linux/ieee802154.h>
11 #include <net/mac802154.h>
12 #include <net/ieee802154_netdev.h>
15 ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr,
20 if (addr->mode == IEEE802154_ADDR_NONE)
24 memcpy(buf + pos, &addr->pan_id, 2);
29 case IEEE802154_ADDR_SHORT:
30 memcpy(buf + pos, &addr->short_addr, 2);
34 case IEEE802154_ADDR_LONG:
35 memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN);
36 pos += IEEE802154_ADDR_LEN;
47 ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr)
52 memcpy(buf + 1, &hdr->frame_counter, 4);
54 switch (hdr->key_id_mode) {
55 case IEEE802154_SCF_KEY_IMPLICIT:
58 case IEEE802154_SCF_KEY_INDEX:
61 case IEEE802154_SCF_KEY_SHORT_INDEX:
62 memcpy(buf + pos, &hdr->short_src, 4);
66 case IEEE802154_SCF_KEY_HW_INDEX:
67 memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN);
68 pos += IEEE802154_ADDR_LEN;
72 buf[pos++] = hdr->key_id;
78 ieee802154_hdr_push(struct sk_buff *skb, struct ieee802154_hdr *hdr)
80 u8 buf[IEEE802154_MAX_HEADER_LEN];
83 struct ieee802154_hdr_fc *fc = &hdr->fc;
85 buf[pos++] = hdr->seq;
87 fc->dest_addr_mode = hdr->dest.mode;
89 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
94 fc->source_addr_mode = hdr->source.mode;
96 if (hdr->source.pan_id == hdr->dest.pan_id &&
97 hdr->dest.mode != IEEE802154_ADDR_NONE)
100 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan);
105 if (fc->security_enabled) {
108 rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
117 memcpy(skb_push(skb, pos), buf, pos);
121 EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
123 int ieee802154_mac_cmd_push(struct sk_buff *skb, void *f,
124 const void *pl, unsigned int pl_len)
126 struct ieee802154_mac_cmd_frame *frame = f;
127 struct ieee802154_mac_cmd_pl *mac_pl = &frame->mac_pl;
128 struct ieee802154_hdr *mhr = &frame->mhr;
131 skb_reserve(skb, sizeof(*mhr));
132 ret = ieee802154_hdr_push(skb, mhr);
136 skb_reset_mac_header(skb);
139 skb_put_data(skb, mac_pl, sizeof(*mac_pl));
140 skb_put_data(skb, pl, pl_len);
144 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_push);
146 int ieee802154_beacon_push(struct sk_buff *skb,
147 struct ieee802154_beacon_frame *beacon)
149 struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl;
150 struct ieee802154_hdr *mhr = &beacon->mhr;
153 skb_reserve(skb, sizeof(*mhr));
154 ret = ieee802154_hdr_push(skb, mhr);
158 skb_reset_mac_header(skb);
161 skb_put_data(skb, mac_pl, sizeof(*mac_pl));
163 if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count)
168 EXPORT_SYMBOL_GPL(ieee802154_beacon_push);
171 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
172 struct ieee802154_addr *addr)
178 if (mode == IEEE802154_ADDR_NONE)
182 memcpy(&addr->pan_id, buf + pos, 2);
186 if (mode == IEEE802154_ADDR_SHORT) {
187 memcpy(&addr->short_addr, buf + pos, 2);
190 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
191 return pos + IEEE802154_ADDR_LEN;
195 static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
197 int pan_len = omit_pan ? 0 : 2;
200 case IEEE802154_ADDR_NONE: return 0;
201 case IEEE802154_ADDR_SHORT: return 2 + pan_len;
202 case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
203 default: return -EINVAL;
208 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
213 memcpy(&hdr->frame_counter, buf + 1, 4);
215 switch (hdr->key_id_mode) {
216 case IEEE802154_SCF_KEY_IMPLICIT:
219 case IEEE802154_SCF_KEY_INDEX:
222 case IEEE802154_SCF_KEY_SHORT_INDEX:
223 memcpy(&hdr->short_src, buf + pos, 4);
227 case IEEE802154_SCF_KEY_HW_INDEX:
228 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
229 pos += IEEE802154_ADDR_LEN;
233 hdr->key_id = buf[pos++];
238 static int ieee802154_sechdr_lengths[4] = {
239 [IEEE802154_SCF_KEY_IMPLICIT] = 5,
240 [IEEE802154_SCF_KEY_INDEX] = 6,
241 [IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
242 [IEEE802154_SCF_KEY_HW_INDEX] = 14,
245 static int ieee802154_hdr_sechdr_len(u8 sc)
247 return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
250 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
254 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
255 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
258 if (slen < 0 || dlen < 0)
261 return 3 + dlen + slen + hdr->fc.security_enabled;
265 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
269 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
271 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
272 hdr->fc.intra_pan, &hdr->source);
274 if (hdr->fc.intra_pan)
275 hdr->source.pan_id = hdr->dest.pan_id;
281 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
285 if (!pskb_may_pull(skb, 3))
288 memcpy(hdr, skb->data, 3);
290 rc = ieee802154_hdr_minlen(hdr);
291 if (rc < 0 || !pskb_may_pull(skb, rc))
294 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
296 if (hdr->fc.security_enabled) {
297 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
299 if (!pskb_may_pull(skb, want))
302 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
308 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
310 int ieee802154_mac_cmd_pl_pull(struct sk_buff *skb,
311 struct ieee802154_mac_cmd_pl *mac_pl)
313 if (!pskb_may_pull(skb, sizeof(*mac_pl)))
316 memcpy(mac_pl, skb->data, sizeof(*mac_pl));
317 skb_pull(skb, sizeof(*mac_pl));
321 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_pl_pull);
324 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
326 const u8 *buf = skb_mac_header(skb);
329 if (buf + 3 > skb_tail_pointer(skb))
334 rc = ieee802154_hdr_minlen(hdr);
335 if (rc < 0 || buf + rc > skb_tail_pointer(skb))
338 pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
341 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
344 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
346 const u8 *buf = skb_mac_header(skb);
349 pos = ieee802154_hdr_peek_addrs(skb, hdr);
353 if (hdr->fc.security_enabled) {
354 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
355 int want = pos + ieee802154_sechdr_lengths[key_id_mode];
357 if (buf + want > skb_tail_pointer(skb))
360 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
365 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
367 int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
369 int hlen = ieee802154_hdr_minlen(hdr);
371 if (hdr->fc.security_enabled) {
372 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
373 hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
376 return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
378 EXPORT_SYMBOL_GPL(ieee802154_max_payload);