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
);