2 * Copyright (C) 2014 Fraunhofer ITWM
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.
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.
14 * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
17 #include <net/mac802154.h>
18 #include <net/ieee802154.h>
19 #include <net/ieee802154_netdev.h>
22 ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr,
27 if (addr->mode == IEEE802154_ADDR_NONE)
31 memcpy(buf + pos, &addr->pan_id, 2);
36 case IEEE802154_ADDR_SHORT:
37 memcpy(buf + pos, &addr->short_addr, 2);
41 case IEEE802154_ADDR_LONG:
42 memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN);
43 pos += IEEE802154_ADDR_LEN;
54 ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr)
59 memcpy(buf + 1, &hdr->frame_counter, 4);
61 switch (hdr->key_id_mode) {
62 case IEEE802154_SCF_KEY_IMPLICIT:
65 case IEEE802154_SCF_KEY_INDEX:
68 case IEEE802154_SCF_KEY_SHORT_INDEX:
69 memcpy(buf + pos, &hdr->short_src, 4);
73 case IEEE802154_SCF_KEY_HW_INDEX:
74 memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN);
75 pos += IEEE802154_ADDR_LEN;
79 buf[pos++] = hdr->key_id;
85 ieee802154_hdr_push(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
87 u8 buf[MAC802154_FRAME_HARD_HEADER_LEN];
90 struct ieee802154_hdr_fc fc = hdr->fc;
92 buf[pos++] = hdr->seq;
94 fc.dest_addr_mode = hdr->dest.mode;
96 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
101 fc.source_addr_mode = hdr->source.mode;
103 if (hdr->source.pan_id == hdr->dest.pan_id &&
104 hdr->dest.mode != IEEE802154_ADDR_NONE)
107 rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc.intra_pan);
112 if (fc.security_enabled) {
115 rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
124 memcpy(skb_push(skb, pos), buf, pos);
128 EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
131 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
132 struct ieee802154_addr *addr)
138 if (mode == IEEE802154_ADDR_NONE)
142 memcpy(&addr->pan_id, buf + pos, 2);
146 if (mode == IEEE802154_ADDR_SHORT) {
147 memcpy(&addr->short_addr, buf + pos, 2);
150 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
151 return pos + IEEE802154_ADDR_LEN;
155 static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
157 int pan_len = omit_pan ? 0 : 2;
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;
168 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
173 memcpy(&hdr->frame_counter, buf + 1, 4);
175 switch (hdr->key_id_mode) {
176 case IEEE802154_SCF_KEY_IMPLICIT:
179 case IEEE802154_SCF_KEY_INDEX:
182 case IEEE802154_SCF_KEY_SHORT_INDEX:
183 memcpy(&hdr->short_src, buf + pos, 4);
187 case IEEE802154_SCF_KEY_HW_INDEX:
188 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
189 pos += IEEE802154_ADDR_LEN;
193 hdr->key_id = buf[pos++];
198 static int ieee802154_hdr_sechdr_len(u8 sc)
200 switch (IEEE802154_SCF_KEY_ID_MODE(sc)) {
201 case IEEE802154_SCF_KEY_IMPLICIT: return 5;
202 case IEEE802154_SCF_KEY_INDEX: return 6;
203 case IEEE802154_SCF_KEY_SHORT_INDEX: return 10;
204 case IEEE802154_SCF_KEY_HW_INDEX: return 14;
205 default: return -EINVAL;
209 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
213 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
214 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
217 if (slen < 0 || dlen < 0)
220 return 3 + dlen + slen + hdr->fc.security_enabled;
224 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
228 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
230 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
231 hdr->fc.intra_pan, &hdr->source);
233 if (hdr->fc.intra_pan)
234 hdr->source.pan_id = hdr->dest.pan_id;
240 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
244 if (!pskb_may_pull(skb, 3))
247 memcpy(hdr, skb->data, 3);
249 rc = ieee802154_hdr_minlen(hdr);
250 if (rc < 0 || !pskb_may_pull(skb, rc))
253 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
255 if (hdr->fc.security_enabled) {
256 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
258 if (!pskb_may_pull(skb, want))
261 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
267 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
270 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
272 const u8 *buf = skb_mac_header(skb);
275 if (buf + 3 > skb_tail_pointer(skb))
280 rc = ieee802154_hdr_minlen(hdr);
281 if (rc < 0 || buf + rc > skb_tail_pointer(skb))
284 pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
287 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);