2 * Copyright 2007-2012 Siemens AG
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.
13 * You should have received a copy of the GNU General Public License along
14 * with this program; if not, write to the Free Software Foundation, Inc.,
15 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
18 * Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
19 * Sergey Lapin <slapin@ossfans.org>
20 * Maxim Gorbachyov <maxim.gorbachev@siemens.com>
21 * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
24 #include <linux/netdevice.h>
25 #include <linux/module.h>
26 #include <linux/if_arp.h>
28 #include <net/rtnetlink.h>
29 #include <linux/nl802154.h>
30 #include <net/af_ieee802154.h>
31 #include <net/mac802154.h>
32 #include <net/ieee802154_netdev.h>
33 #include <net/ieee802154.h>
34 #include <net/wpan-phy.h>
36 #include "mac802154.h"
38 static inline int mac802154_fetch_skb_u8(struct sk_buff
*skb
, u8
*val
)
40 if (unlikely(!pskb_may_pull(skb
, 1)))
49 static inline int mac802154_fetch_skb_u16(struct sk_buff
*skb
, u16
*val
)
51 if (unlikely(!pskb_may_pull(skb
, 2)))
54 *val
= skb
->data
[0] | (skb
->data
[1] << 8);
60 static inline void mac802154_haddr_copy_swap(u8
*dest
, const u8
*src
)
63 for (i
= 0; i
< IEEE802154_ADDR_LEN
; i
++)
64 dest
[IEEE802154_ADDR_LEN
- i
- 1] = src
[i
];
68 mac802154_wpan_ioctl(struct net_device
*dev
, struct ifreq
*ifr
, int cmd
)
70 struct mac802154_sub_if_data
*priv
= netdev_priv(dev
);
71 struct sockaddr_ieee802154
*sa
=
72 (struct sockaddr_ieee802154
*)&ifr
->ifr_addr
;
73 int err
= -ENOIOCTLCMD
;
75 spin_lock_bh(&priv
->mib_lock
);
79 if (priv
->pan_id
== IEEE802154_PANID_BROADCAST
||
80 priv
->short_addr
== IEEE802154_ADDR_BROADCAST
) {
85 sa
->family
= AF_IEEE802154
;
86 sa
->addr
.addr_type
= IEEE802154_ADDR_SHORT
;
87 sa
->addr
.pan_id
= priv
->pan_id
;
88 sa
->addr
.short_addr
= priv
->short_addr
;
94 "Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
95 if (sa
->family
!= AF_IEEE802154
||
96 sa
->addr
.addr_type
!= IEEE802154_ADDR_SHORT
||
97 sa
->addr
.pan_id
== IEEE802154_PANID_BROADCAST
||
98 sa
->addr
.short_addr
== IEEE802154_ADDR_BROADCAST
||
99 sa
->addr
.short_addr
== IEEE802154_ADDR_UNDEF
) {
104 priv
->pan_id
= sa
->addr
.pan_id
;
105 priv
->short_addr
= sa
->addr
.short_addr
;
111 spin_unlock_bh(&priv
->mib_lock
);
115 static int mac802154_wpan_mac_addr(struct net_device
*dev
, void *p
)
117 struct sockaddr
*addr
= p
;
119 if (netif_running(dev
))
122 /* FIXME: validate addr */
123 memcpy(dev
->dev_addr
, addr
->sa_data
, dev
->addr_len
);
124 mac802154_dev_set_ieee_addr(dev
);
128 static int mac802154_header_create(struct sk_buff
*skb
,
129 struct net_device
*dev
,
135 const struct ieee802154_addr
*saddr
= _saddr
;
136 const struct ieee802154_addr
*daddr
= _daddr
;
137 struct ieee802154_addr dev_addr
;
138 struct mac802154_sub_if_data
*priv
= netdev_priv(dev
);
140 u8 head
[MAC802154_FRAME_HARD_HEADER_LEN
];
146 head
[pos
++] = mac_cb(skb
)->seq
; /* DSN/BSN */
147 fc
= mac_cb_type(skb
);
148 if (mac_cb_is_ackreq(skb
))
149 fc
|= IEEE802154_FC_ACK_REQ
;
152 spin_lock_bh(&priv
->mib_lock
);
154 if (priv
->short_addr
== IEEE802154_ADDR_BROADCAST
||
155 priv
->short_addr
== IEEE802154_ADDR_UNDEF
||
156 priv
->pan_id
== IEEE802154_PANID_BROADCAST
) {
157 dev_addr
.addr_type
= IEEE802154_ADDR_LONG
;
158 memcpy(dev_addr
.hwaddr
, dev
->dev_addr
,
159 IEEE802154_ADDR_LEN
);
161 dev_addr
.addr_type
= IEEE802154_ADDR_SHORT
;
162 dev_addr
.short_addr
= priv
->short_addr
;
165 dev_addr
.pan_id
= priv
->pan_id
;
168 spin_unlock_bh(&priv
->mib_lock
);
171 if (daddr
->addr_type
!= IEEE802154_ADDR_NONE
) {
172 fc
|= (daddr
->addr_type
<< IEEE802154_FC_DAMODE_SHIFT
);
174 head
[pos
++] = daddr
->pan_id
& 0xff;
175 head
[pos
++] = daddr
->pan_id
>> 8;
177 if (daddr
->addr_type
== IEEE802154_ADDR_SHORT
) {
178 head
[pos
++] = daddr
->short_addr
& 0xff;
179 head
[pos
++] = daddr
->short_addr
>> 8;
181 mac802154_haddr_copy_swap(head
+ pos
, daddr
->hwaddr
);
182 pos
+= IEEE802154_ADDR_LEN
;
186 if (saddr
->addr_type
!= IEEE802154_ADDR_NONE
) {
187 fc
|= (saddr
->addr_type
<< IEEE802154_FC_SAMODE_SHIFT
);
189 if ((saddr
->pan_id
== daddr
->pan_id
) &&
190 (saddr
->pan_id
!= IEEE802154_PANID_BROADCAST
)) {
191 /* PANID compression/intra PAN */
192 fc
|= IEEE802154_FC_INTRA_PAN
;
194 head
[pos
++] = saddr
->pan_id
& 0xff;
195 head
[pos
++] = saddr
->pan_id
>> 8;
198 if (saddr
->addr_type
== IEEE802154_ADDR_SHORT
) {
199 head
[pos
++] = saddr
->short_addr
& 0xff;
200 head
[pos
++] = saddr
->short_addr
>> 8;
202 mac802154_haddr_copy_swap(head
+ pos
, saddr
->hwaddr
);
203 pos
+= IEEE802154_ADDR_LEN
;
210 memcpy(skb_push(skb
, pos
), head
, pos
);
211 skb_reset_mac_header(skb
);
218 mac802154_header_parse(const struct sk_buff
*skb
, unsigned char *haddr
)
220 const u8
*hdr
= skb_mac_header(skb
);
221 const u8
*tail
= skb_tail_pointer(skb
);
222 struct ieee802154_addr
*addr
= (struct ieee802154_addr
*)haddr
;
229 fc
= hdr
[0] | (hdr
[1] << 8);
233 da_type
= IEEE802154_FC_DAMODE(fc
);
234 addr
->addr_type
= IEEE802154_FC_SAMODE(fc
);
237 case IEEE802154_ADDR_NONE
:
238 if (fc
& IEEE802154_FC_INTRA_PAN
)
241 case IEEE802154_ADDR_LONG
:
242 if (fc
& IEEE802154_FC_INTRA_PAN
) {
245 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
249 if (hdr
+ IEEE802154_ADDR_LEN
> tail
)
252 hdr
+= IEEE802154_ADDR_LEN
;
254 case IEEE802154_ADDR_SHORT
:
255 if (fc
& IEEE802154_FC_INTRA_PAN
) {
258 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
272 switch (addr
->addr_type
) {
273 case IEEE802154_ADDR_NONE
:
275 case IEEE802154_ADDR_LONG
:
276 if (!(fc
& IEEE802154_FC_INTRA_PAN
)) {
279 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
283 if (hdr
+ IEEE802154_ADDR_LEN
> tail
)
286 mac802154_haddr_copy_swap(addr
->hwaddr
, hdr
);
287 hdr
+= IEEE802154_ADDR_LEN
;
289 case IEEE802154_ADDR_SHORT
:
290 if (!(fc
& IEEE802154_FC_INTRA_PAN
)) {
293 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
300 addr
->short_addr
= hdr
[0] | (hdr
[1] << 8);
307 return sizeof(struct ieee802154_addr
);
310 pr_debug("malformed packet\n");
315 mac802154_wpan_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
317 struct mac802154_sub_if_data
*priv
;
320 priv
= netdev_priv(dev
);
322 spin_lock_bh(&priv
->mib_lock
);
325 spin_unlock_bh(&priv
->mib_lock
);
327 if (chan
== MAC802154_CHAN_NONE
||
328 page
>= WPAN_NUM_PAGES
||
329 chan
>= WPAN_NUM_CHANNELS
) {
334 skb
->skb_iif
= dev
->ifindex
;
335 dev
->stats
.tx_packets
++;
336 dev
->stats
.tx_bytes
+= skb
->len
;
338 return mac802154_tx(priv
->hw
, skb
, page
, chan
);
341 static struct header_ops mac802154_header_ops
= {
342 .create
= mac802154_header_create
,
343 .parse
= mac802154_header_parse
,
346 static const struct net_device_ops mac802154_wpan_ops
= {
347 .ndo_open
= mac802154_slave_open
,
348 .ndo_stop
= mac802154_slave_close
,
349 .ndo_start_xmit
= mac802154_wpan_xmit
,
350 .ndo_do_ioctl
= mac802154_wpan_ioctl
,
351 .ndo_set_mac_address
= mac802154_wpan_mac_addr
,
354 void mac802154_wpan_setup(struct net_device
*dev
)
356 struct mac802154_sub_if_data
*priv
;
358 dev
->addr_len
= IEEE802154_ADDR_LEN
;
359 memset(dev
->broadcast
, 0xff, IEEE802154_ADDR_LEN
);
361 dev
->hard_header_len
= MAC802154_FRAME_HARD_HEADER_LEN
;
362 dev
->header_ops
= &mac802154_header_ops
;
363 dev
->needed_tailroom
= 2; /* FCS */
364 dev
->mtu
= IEEE802154_MTU
;
365 dev
->tx_queue_len
= 300;
366 dev
->type
= ARPHRD_IEEE802154
;
367 dev
->flags
= IFF_NOARP
| IFF_BROADCAST
;
368 dev
->watchdog_timeo
= 0;
370 dev
->destructor
= free_netdev
;
371 dev
->netdev_ops
= &mac802154_wpan_ops
;
372 dev
->ml_priv
= &mac802154_mlme_wpan
;
374 priv
= netdev_priv(dev
);
375 priv
->type
= IEEE802154_DEV_WPAN
;
377 priv
->chan
= MAC802154_CHAN_NONE
;
380 spin_lock_init(&priv
->mib_lock
);
382 get_random_bytes(&priv
->bsn
, 1);
383 get_random_bytes(&priv
->dsn
, 1);
385 priv
->pan_id
= IEEE802154_PANID_BROADCAST
;
386 priv
->short_addr
= IEEE802154_ADDR_BROADCAST
;
389 static int mac802154_process_data(struct net_device
*dev
, struct sk_buff
*skb
)
391 return netif_rx_ni(skb
);
395 mac802154_subif_frame(struct mac802154_sub_if_data
*sdata
, struct sk_buff
*skb
)
397 pr_debug("getting packet via slave interface %s\n", sdata
->dev
->name
);
399 spin_lock_bh(&sdata
->mib_lock
);
401 switch (mac_cb(skb
)->da
.addr_type
) {
402 case IEEE802154_ADDR_NONE
:
403 if (mac_cb(skb
)->sa
.addr_type
!= IEEE802154_ADDR_NONE
)
404 /* FIXME: check if we are PAN coordinator */
405 skb
->pkt_type
= PACKET_OTHERHOST
;
407 /* ACK comes with both addresses empty */
408 skb
->pkt_type
= PACKET_HOST
;
410 case IEEE802154_ADDR_LONG
:
411 if (mac_cb(skb
)->da
.pan_id
!= sdata
->pan_id
&&
412 mac_cb(skb
)->da
.pan_id
!= IEEE802154_PANID_BROADCAST
)
413 skb
->pkt_type
= PACKET_OTHERHOST
;
414 else if (!memcmp(mac_cb(skb
)->da
.hwaddr
, sdata
->dev
->dev_addr
,
415 IEEE802154_ADDR_LEN
))
416 skb
->pkt_type
= PACKET_HOST
;
418 skb
->pkt_type
= PACKET_OTHERHOST
;
420 case IEEE802154_ADDR_SHORT
:
421 if (mac_cb(skb
)->da
.pan_id
!= sdata
->pan_id
&&
422 mac_cb(skb
)->da
.pan_id
!= IEEE802154_PANID_BROADCAST
)
423 skb
->pkt_type
= PACKET_OTHERHOST
;
424 else if (mac_cb(skb
)->da
.short_addr
== sdata
->short_addr
)
425 skb
->pkt_type
= PACKET_HOST
;
426 else if (mac_cb(skb
)->da
.short_addr
==
427 IEEE802154_ADDR_BROADCAST
)
428 skb
->pkt_type
= PACKET_BROADCAST
;
430 skb
->pkt_type
= PACKET_OTHERHOST
;
436 spin_unlock_bh(&sdata
->mib_lock
);
438 skb
->dev
= sdata
->dev
;
440 sdata
->dev
->stats
.rx_packets
++;
441 sdata
->dev
->stats
.rx_bytes
+= skb
->len
;
443 switch (mac_cb_type(skb
)) {
444 case IEEE802154_FC_TYPE_DATA
:
445 return mac802154_process_data(sdata
->dev
, skb
);
447 pr_warning("ieee802154: bad frame received (type = %d)\n",
454 static int mac802154_parse_frame_start(struct sk_buff
*skb
)
456 u8
*head
= skb
->data
;
459 if (mac802154_fetch_skb_u16(skb
, &fc
) ||
460 mac802154_fetch_skb_u8(skb
, &(mac_cb(skb
)->seq
)))
463 pr_debug("fc: %04x dsn: %02x\n", fc
, head
[2]);
465 mac_cb(skb
)->flags
= IEEE802154_FC_TYPE(fc
);
466 mac_cb(skb
)->sa
.addr_type
= IEEE802154_FC_SAMODE(fc
);
467 mac_cb(skb
)->da
.addr_type
= IEEE802154_FC_DAMODE(fc
);
469 if (fc
& IEEE802154_FC_INTRA_PAN
)
470 mac_cb(skb
)->flags
|= MAC_CB_FLAG_INTRAPAN
;
472 if (mac_cb(skb
)->da
.addr_type
!= IEEE802154_ADDR_NONE
) {
473 if (mac802154_fetch_skb_u16(skb
, &(mac_cb(skb
)->da
.pan_id
)))
476 /* source PAN id compression */
477 if (mac_cb_is_intrapan(skb
))
478 mac_cb(skb
)->sa
.pan_id
= mac_cb(skb
)->da
.pan_id
;
480 pr_debug("dest PAN addr: %04x\n", mac_cb(skb
)->da
.pan_id
);
482 if (mac_cb(skb
)->da
.addr_type
== IEEE802154_ADDR_SHORT
) {
483 u16
*da
= &(mac_cb(skb
)->da
.short_addr
);
485 if (mac802154_fetch_skb_u16(skb
, da
))
488 pr_debug("destination address is short: %04x\n",
489 mac_cb(skb
)->da
.short_addr
);
491 if (!pskb_may_pull(skb
, IEEE802154_ADDR_LEN
))
494 mac802154_haddr_copy_swap(mac_cb(skb
)->da
.hwaddr
,
496 skb_pull(skb
, IEEE802154_ADDR_LEN
);
498 pr_debug("destination address is hardware\n");
502 if (mac_cb(skb
)->sa
.addr_type
!= IEEE802154_ADDR_NONE
) {
503 /* non PAN-compression, fetch source address id */
504 if (!(mac_cb_is_intrapan(skb
))) {
505 u16
*sa_pan
= &(mac_cb(skb
)->sa
.pan_id
);
507 if (mac802154_fetch_skb_u16(skb
, sa_pan
))
511 pr_debug("source PAN addr: %04x\n", mac_cb(skb
)->da
.pan_id
);
513 if (mac_cb(skb
)->sa
.addr_type
== IEEE802154_ADDR_SHORT
) {
514 u16
*sa
= &(mac_cb(skb
)->sa
.short_addr
);
516 if (mac802154_fetch_skb_u16(skb
, sa
))
519 pr_debug("source address is short: %04x\n",
520 mac_cb(skb
)->sa
.short_addr
);
522 if (!pskb_may_pull(skb
, IEEE802154_ADDR_LEN
))
525 mac802154_haddr_copy_swap(mac_cb(skb
)->sa
.hwaddr
,
527 skb_pull(skb
, IEEE802154_ADDR_LEN
);
529 pr_debug("source address is hardware\n");
538 void mac802154_wpans_rx(struct mac802154_priv
*priv
, struct sk_buff
*skb
)
541 struct sk_buff
*sskb
;
542 struct mac802154_sub_if_data
*sdata
;
544 ret
= mac802154_parse_frame_start(skb
);
546 pr_debug("got invalid frame\n");
551 list_for_each_entry_rcu(sdata
, &priv
->slaves
, list
) {
552 if (sdata
->type
!= IEEE802154_DEV_WPAN
)
555 sskb
= skb_clone(skb
, GFP_ATOMIC
);
557 mac802154_subif_frame(sdata
, sskb
);