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
);
80 u16 pan_id
, short_addr
;
82 pan_id
= le16_to_cpu(priv
->pan_id
);
83 short_addr
= le16_to_cpu(priv
->short_addr
);
84 if (pan_id
== IEEE802154_PANID_BROADCAST
||
85 short_addr
== IEEE802154_ADDR_BROADCAST
) {
90 sa
->family
= AF_IEEE802154
;
91 sa
->addr
.addr_type
= IEEE802154_ADDR_SHORT
;
92 sa
->addr
.pan_id
= pan_id
;
93 sa
->addr
.short_addr
= short_addr
;
100 "Using DEBUGing ioctl SIOCSIFADDR isn't recommened!\n");
101 if (sa
->family
!= AF_IEEE802154
||
102 sa
->addr
.addr_type
!= IEEE802154_ADDR_SHORT
||
103 sa
->addr
.pan_id
== IEEE802154_PANID_BROADCAST
||
104 sa
->addr
.short_addr
== IEEE802154_ADDR_BROADCAST
||
105 sa
->addr
.short_addr
== IEEE802154_ADDR_UNDEF
) {
110 priv
->pan_id
= cpu_to_le16(sa
->addr
.pan_id
);
111 priv
->short_addr
= cpu_to_le16(sa
->addr
.short_addr
);
117 spin_unlock_bh(&priv
->mib_lock
);
121 static int mac802154_wpan_mac_addr(struct net_device
*dev
, void *p
)
123 struct sockaddr
*addr
= p
;
125 if (netif_running(dev
))
128 /* FIXME: validate addr */
129 memcpy(dev
->dev_addr
, addr
->sa_data
, dev
->addr_len
);
130 mac802154_dev_set_ieee_addr(dev
);
134 static int mac802154_header_create(struct sk_buff
*skb
,
135 struct net_device
*dev
,
141 const struct ieee802154_addr_sa
*saddr
= _saddr
;
142 const struct ieee802154_addr_sa
*daddr
= _daddr
;
143 struct ieee802154_addr_sa dev_addr
;
144 struct mac802154_sub_if_data
*priv
= netdev_priv(dev
);
146 u8 head
[MAC802154_FRAME_HARD_HEADER_LEN
];
152 head
[pos
++] = mac_cb(skb
)->seq
; /* DSN/BSN */
153 fc
= mac_cb_type(skb
);
154 if (mac_cb_is_ackreq(skb
))
155 fc
|= IEEE802154_FC_ACK_REQ
;
158 spin_lock_bh(&priv
->mib_lock
);
160 if (priv
->short_addr
== cpu_to_le16(IEEE802154_ADDR_BROADCAST
) ||
161 priv
->short_addr
== cpu_to_le16(IEEE802154_ADDR_UNDEF
) ||
162 priv
->pan_id
== cpu_to_le16(IEEE802154_PANID_BROADCAST
)) {
163 dev_addr
.addr_type
= IEEE802154_ADDR_LONG
;
164 memcpy(dev_addr
.hwaddr
, dev
->dev_addr
,
165 IEEE802154_ADDR_LEN
);
167 dev_addr
.addr_type
= IEEE802154_ADDR_SHORT
;
168 dev_addr
.short_addr
= le16_to_cpu(priv
->short_addr
);
171 dev_addr
.pan_id
= le16_to_cpu(priv
->pan_id
);
174 spin_unlock_bh(&priv
->mib_lock
);
177 if (daddr
->addr_type
!= IEEE802154_ADDR_NONE
) {
178 fc
|= (daddr
->addr_type
<< IEEE802154_FC_DAMODE_SHIFT
);
180 head
[pos
++] = daddr
->pan_id
& 0xff;
181 head
[pos
++] = daddr
->pan_id
>> 8;
183 if (daddr
->addr_type
== IEEE802154_ADDR_SHORT
) {
184 head
[pos
++] = daddr
->short_addr
& 0xff;
185 head
[pos
++] = daddr
->short_addr
>> 8;
187 mac802154_haddr_copy_swap(head
+ pos
, daddr
->hwaddr
);
188 pos
+= IEEE802154_ADDR_LEN
;
192 if (saddr
->addr_type
!= IEEE802154_ADDR_NONE
) {
193 fc
|= (saddr
->addr_type
<< IEEE802154_FC_SAMODE_SHIFT
);
195 if ((saddr
->pan_id
== daddr
->pan_id
) &&
196 (saddr
->pan_id
!= IEEE802154_PANID_BROADCAST
)) {
197 /* PANID compression/intra PAN */
198 fc
|= IEEE802154_FC_INTRA_PAN
;
200 head
[pos
++] = saddr
->pan_id
& 0xff;
201 head
[pos
++] = saddr
->pan_id
>> 8;
204 if (saddr
->addr_type
== IEEE802154_ADDR_SHORT
) {
205 head
[pos
++] = saddr
->short_addr
& 0xff;
206 head
[pos
++] = saddr
->short_addr
>> 8;
208 mac802154_haddr_copy_swap(head
+ pos
, saddr
->hwaddr
);
209 pos
+= IEEE802154_ADDR_LEN
;
216 memcpy(skb_push(skb
, pos
), head
, pos
);
217 skb_reset_mac_header(skb
);
224 mac802154_header_parse(const struct sk_buff
*skb
, unsigned char *haddr
)
226 const u8
*hdr
= skb_mac_header(skb
);
227 const u8
*tail
= skb_tail_pointer(skb
);
228 struct ieee802154_addr_sa
*addr
= (struct ieee802154_addr_sa
*)haddr
;
235 fc
= hdr
[0] | (hdr
[1] << 8);
239 da_type
= IEEE802154_FC_DAMODE(fc
);
240 addr
->addr_type
= IEEE802154_FC_SAMODE(fc
);
243 case IEEE802154_ADDR_NONE
:
244 if (fc
& IEEE802154_FC_INTRA_PAN
)
247 case IEEE802154_ADDR_LONG
:
248 if (fc
& IEEE802154_FC_INTRA_PAN
) {
251 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
255 if (hdr
+ IEEE802154_ADDR_LEN
> tail
)
258 hdr
+= IEEE802154_ADDR_LEN
;
260 case IEEE802154_ADDR_SHORT
:
261 if (fc
& IEEE802154_FC_INTRA_PAN
) {
264 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
278 switch (addr
->addr_type
) {
279 case IEEE802154_ADDR_NONE
:
281 case IEEE802154_ADDR_LONG
:
282 if (!(fc
& IEEE802154_FC_INTRA_PAN
)) {
285 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
289 if (hdr
+ IEEE802154_ADDR_LEN
> tail
)
292 mac802154_haddr_copy_swap(addr
->hwaddr
, hdr
);
293 hdr
+= IEEE802154_ADDR_LEN
;
295 case IEEE802154_ADDR_SHORT
:
296 if (!(fc
& IEEE802154_FC_INTRA_PAN
)) {
299 addr
->pan_id
= hdr
[0] | (hdr
[1] << 8);
306 addr
->short_addr
= hdr
[0] | (hdr
[1] << 8);
313 return sizeof(struct ieee802154_addr_sa
);
316 pr_debug("malformed packet\n");
321 mac802154_wpan_xmit(struct sk_buff
*skb
, struct net_device
*dev
)
323 struct mac802154_sub_if_data
*priv
;
326 priv
= netdev_priv(dev
);
328 spin_lock_bh(&priv
->mib_lock
);
331 spin_unlock_bh(&priv
->mib_lock
);
333 if (chan
== MAC802154_CHAN_NONE
||
334 page
>= WPAN_NUM_PAGES
||
335 chan
>= WPAN_NUM_CHANNELS
) {
340 skb
->skb_iif
= dev
->ifindex
;
341 dev
->stats
.tx_packets
++;
342 dev
->stats
.tx_bytes
+= skb
->len
;
344 return mac802154_tx(priv
->hw
, skb
, page
, chan
);
347 static struct header_ops mac802154_header_ops
= {
348 .create
= mac802154_header_create
,
349 .parse
= mac802154_header_parse
,
352 static const struct net_device_ops mac802154_wpan_ops
= {
353 .ndo_open
= mac802154_slave_open
,
354 .ndo_stop
= mac802154_slave_close
,
355 .ndo_start_xmit
= mac802154_wpan_xmit
,
356 .ndo_do_ioctl
= mac802154_wpan_ioctl
,
357 .ndo_set_mac_address
= mac802154_wpan_mac_addr
,
360 void mac802154_wpan_setup(struct net_device
*dev
)
362 struct mac802154_sub_if_data
*priv
;
364 dev
->addr_len
= IEEE802154_ADDR_LEN
;
365 memset(dev
->broadcast
, 0xff, IEEE802154_ADDR_LEN
);
367 dev
->hard_header_len
= MAC802154_FRAME_HARD_HEADER_LEN
;
368 dev
->header_ops
= &mac802154_header_ops
;
369 dev
->needed_tailroom
= 2; /* FCS */
370 dev
->mtu
= IEEE802154_MTU
;
371 dev
->tx_queue_len
= 300;
372 dev
->type
= ARPHRD_IEEE802154
;
373 dev
->flags
= IFF_NOARP
| IFF_BROADCAST
;
374 dev
->watchdog_timeo
= 0;
376 dev
->destructor
= free_netdev
;
377 dev
->netdev_ops
= &mac802154_wpan_ops
;
378 dev
->ml_priv
= &mac802154_mlme_wpan
;
380 priv
= netdev_priv(dev
);
381 priv
->type
= IEEE802154_DEV_WPAN
;
383 priv
->chan
= MAC802154_CHAN_NONE
;
386 spin_lock_init(&priv
->mib_lock
);
388 get_random_bytes(&priv
->bsn
, 1);
389 get_random_bytes(&priv
->dsn
, 1);
391 priv
->pan_id
= cpu_to_le16(IEEE802154_PANID_BROADCAST
);
392 priv
->short_addr
= cpu_to_le16(IEEE802154_ADDR_BROADCAST
);
395 static int mac802154_process_data(struct net_device
*dev
, struct sk_buff
*skb
)
397 return netif_rx_ni(skb
);
401 mac802154_subif_frame(struct mac802154_sub_if_data
*sdata
, struct sk_buff
*skb
)
405 pr_debug("getting packet via slave interface %s\n", sdata
->dev
->name
);
407 spin_lock_bh(&sdata
->mib_lock
);
409 span
= le16_to_cpu(sdata
->pan_id
);
410 sshort
= le16_to_cpu(sdata
->short_addr
);
412 switch (mac_cb(skb
)->da
.addr_type
) {
413 case IEEE802154_ADDR_NONE
:
414 if (mac_cb(skb
)->sa
.addr_type
!= IEEE802154_ADDR_NONE
)
415 /* FIXME: check if we are PAN coordinator */
416 skb
->pkt_type
= PACKET_OTHERHOST
;
418 /* ACK comes with both addresses empty */
419 skb
->pkt_type
= PACKET_HOST
;
421 case IEEE802154_ADDR_LONG
:
422 if (mac_cb(skb
)->da
.pan_id
!= span
&&
423 mac_cb(skb
)->da
.pan_id
!= IEEE802154_PANID_BROADCAST
)
424 skb
->pkt_type
= PACKET_OTHERHOST
;
425 else if (!memcmp(mac_cb(skb
)->da
.hwaddr
, sdata
->dev
->dev_addr
,
426 IEEE802154_ADDR_LEN
))
427 skb
->pkt_type
= PACKET_HOST
;
429 skb
->pkt_type
= PACKET_OTHERHOST
;
431 case IEEE802154_ADDR_SHORT
:
432 if (mac_cb(skb
)->da
.pan_id
!= span
&&
433 mac_cb(skb
)->da
.pan_id
!= IEEE802154_PANID_BROADCAST
)
434 skb
->pkt_type
= PACKET_OTHERHOST
;
435 else if (mac_cb(skb
)->da
.short_addr
== sshort
)
436 skb
->pkt_type
= PACKET_HOST
;
437 else if (mac_cb(skb
)->da
.short_addr
==
438 IEEE802154_ADDR_BROADCAST
)
439 skb
->pkt_type
= PACKET_BROADCAST
;
441 skb
->pkt_type
= PACKET_OTHERHOST
;
447 spin_unlock_bh(&sdata
->mib_lock
);
449 skb
->dev
= sdata
->dev
;
451 sdata
->dev
->stats
.rx_packets
++;
452 sdata
->dev
->stats
.rx_bytes
+= skb
->len
;
454 switch (mac_cb_type(skb
)) {
455 case IEEE802154_FC_TYPE_DATA
:
456 return mac802154_process_data(sdata
->dev
, skb
);
458 pr_warn("ieee802154: bad frame received (type = %d)\n",
465 static int mac802154_parse_frame_start(struct sk_buff
*skb
)
467 u8
*head
= skb
->data
;
470 if (mac802154_fetch_skb_u16(skb
, &fc
) ||
471 mac802154_fetch_skb_u8(skb
, &(mac_cb(skb
)->seq
)))
474 pr_debug("fc: %04x dsn: %02x\n", fc
, head
[2]);
476 mac_cb(skb
)->flags
= IEEE802154_FC_TYPE(fc
);
477 mac_cb(skb
)->sa
.addr_type
= IEEE802154_FC_SAMODE(fc
);
478 mac_cb(skb
)->da
.addr_type
= IEEE802154_FC_DAMODE(fc
);
480 if (fc
& IEEE802154_FC_INTRA_PAN
)
481 mac_cb(skb
)->flags
|= MAC_CB_FLAG_INTRAPAN
;
483 if (mac_cb(skb
)->da
.addr_type
!= IEEE802154_ADDR_NONE
) {
484 if (mac802154_fetch_skb_u16(skb
, &(mac_cb(skb
)->da
.pan_id
)))
487 /* source PAN id compression */
488 if (mac_cb_is_intrapan(skb
))
489 mac_cb(skb
)->sa
.pan_id
= mac_cb(skb
)->da
.pan_id
;
491 pr_debug("dest PAN addr: %04x\n", mac_cb(skb
)->da
.pan_id
);
493 if (mac_cb(skb
)->da
.addr_type
== IEEE802154_ADDR_SHORT
) {
494 u16
*da
= &(mac_cb(skb
)->da
.short_addr
);
496 if (mac802154_fetch_skb_u16(skb
, da
))
499 pr_debug("destination address is short: %04x\n",
500 mac_cb(skb
)->da
.short_addr
);
502 if (!pskb_may_pull(skb
, IEEE802154_ADDR_LEN
))
505 mac802154_haddr_copy_swap(mac_cb(skb
)->da
.hwaddr
,
507 skb_pull(skb
, IEEE802154_ADDR_LEN
);
509 pr_debug("destination address is hardware\n");
513 if (mac_cb(skb
)->sa
.addr_type
!= IEEE802154_ADDR_NONE
) {
514 /* non PAN-compression, fetch source address id */
515 if (!(mac_cb_is_intrapan(skb
))) {
516 u16
*sa_pan
= &(mac_cb(skb
)->sa
.pan_id
);
518 if (mac802154_fetch_skb_u16(skb
, sa_pan
))
522 pr_debug("source PAN addr: %04x\n", mac_cb(skb
)->da
.pan_id
);
524 if (mac_cb(skb
)->sa
.addr_type
== IEEE802154_ADDR_SHORT
) {
525 u16
*sa
= &(mac_cb(skb
)->sa
.short_addr
);
527 if (mac802154_fetch_skb_u16(skb
, sa
))
530 pr_debug("source address is short: %04x\n",
531 mac_cb(skb
)->sa
.short_addr
);
533 if (!pskb_may_pull(skb
, IEEE802154_ADDR_LEN
))
536 mac802154_haddr_copy_swap(mac_cb(skb
)->sa
.hwaddr
,
538 skb_pull(skb
, IEEE802154_ADDR_LEN
);
540 pr_debug("source address is hardware\n");
549 void mac802154_wpans_rx(struct mac802154_priv
*priv
, struct sk_buff
*skb
)
552 struct sk_buff
*sskb
;
553 struct mac802154_sub_if_data
*sdata
;
555 ret
= mac802154_parse_frame_start(skb
);
557 pr_debug("got invalid frame\n");
562 list_for_each_entry_rcu(sdata
, &priv
->slaves
, list
) {
563 if (sdata
->type
!= IEEE802154_DEV_WPAN
)
566 sskb
= skb_clone(skb
, GFP_ATOMIC
);
568 mac802154_subif_frame(sdata
, sskb
);