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);
216 mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
218 const u8 *hdr = skb_mac_header(skb);
219 const u8 *tail = skb_tail_pointer(skb);
220 struct ieee802154_addr *addr = (struct ieee802154_addr *)haddr;
227 fc = hdr[0] | (hdr[1] << 8);
231 da_type = IEEE802154_FC_DAMODE(fc);
232 addr->addr_type = IEEE802154_FC_SAMODE(fc);
235 case IEEE802154_ADDR_NONE:
236 if (fc & IEEE802154_FC_INTRA_PAN)
239 case IEEE802154_ADDR_LONG:
240 if (fc & IEEE802154_FC_INTRA_PAN) {
243 addr->pan_id = hdr[0] | (hdr[1] << 8);
247 if (hdr + IEEE802154_ADDR_LEN > tail)
250 hdr += IEEE802154_ADDR_LEN;
252 case IEEE802154_ADDR_SHORT:
253 if (fc & IEEE802154_FC_INTRA_PAN) {
256 addr->pan_id = hdr[0] | (hdr[1] << 8);
270 switch (addr->addr_type) {
271 case IEEE802154_ADDR_NONE:
273 case IEEE802154_ADDR_LONG:
274 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
277 addr->pan_id = hdr[0] | (hdr[1] << 8);
281 if (hdr + IEEE802154_ADDR_LEN > tail)
284 mac802154_haddr_copy_swap(addr->hwaddr, hdr);
285 hdr += IEEE802154_ADDR_LEN;
287 case IEEE802154_ADDR_SHORT:
288 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
291 addr->pan_id = hdr[0] | (hdr[1] << 8);
298 addr->short_addr = hdr[0] | (hdr[1] << 8);
305 return sizeof(struct ieee802154_addr);
308 pr_debug("malformed packet\n");
313 mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
315 struct mac802154_sub_if_data *priv;
318 priv = netdev_priv(dev);
320 spin_lock_bh(&priv->mib_lock);
323 spin_unlock_bh(&priv->mib_lock);
325 if (chan == MAC802154_CHAN_NONE ||
326 page >= WPAN_NUM_PAGES ||
327 chan >= WPAN_NUM_CHANNELS) {
332 skb->skb_iif = dev->ifindex;
333 dev->stats.tx_packets++;
334 dev->stats.tx_bytes += skb->len;
336 return mac802154_tx(priv->hw, skb, page, chan);
339 static struct header_ops mac802154_header_ops = {
340 .create = mac802154_header_create,
341 .parse = mac802154_header_parse,
344 static const struct net_device_ops mac802154_wpan_ops = {
345 .ndo_open = mac802154_slave_open,
346 .ndo_stop = mac802154_slave_close,
347 .ndo_start_xmit = mac802154_wpan_xmit,
348 .ndo_do_ioctl = mac802154_wpan_ioctl,
349 .ndo_set_mac_address = mac802154_wpan_mac_addr,
352 void mac802154_wpan_setup(struct net_device *dev)
354 struct mac802154_sub_if_data *priv;
356 dev->addr_len = IEEE802154_ADDR_LEN;
357 memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
359 dev->hard_header_len = MAC802154_FRAME_HARD_HEADER_LEN;
360 dev->header_ops = &mac802154_header_ops;
361 dev->needed_tailroom = 2; /* FCS */
362 dev->mtu = IEEE802154_MTU;
363 dev->tx_queue_len = 300;
364 dev->type = ARPHRD_IEEE802154;
365 dev->flags = IFF_NOARP | IFF_BROADCAST;
366 dev->watchdog_timeo = 0;
368 dev->destructor = free_netdev;
369 dev->netdev_ops = &mac802154_wpan_ops;
370 dev->ml_priv = &mac802154_mlme_wpan;
372 priv = netdev_priv(dev);
373 priv->type = IEEE802154_DEV_WPAN;
375 priv->chan = MAC802154_CHAN_NONE;
378 spin_lock_init(&priv->mib_lock);
380 get_random_bytes(&priv->bsn, 1);
381 get_random_bytes(&priv->dsn, 1);
383 priv->pan_id = IEEE802154_PANID_BROADCAST;
384 priv->short_addr = IEEE802154_ADDR_BROADCAST;
387 static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
389 return netif_rx_ni(skb);
393 mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
395 pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
397 spin_lock_bh(&sdata->mib_lock);
399 switch (mac_cb(skb)->da.addr_type) {
400 case IEEE802154_ADDR_NONE:
401 if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE)
402 /* FIXME: check if we are PAN coordinator */
403 skb->pkt_type = PACKET_OTHERHOST;
405 /* ACK comes with both addresses empty */
406 skb->pkt_type = PACKET_HOST;
408 case IEEE802154_ADDR_LONG:
409 if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
410 mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
411 skb->pkt_type = PACKET_OTHERHOST;
412 else if (!memcmp(mac_cb(skb)->da.hwaddr, sdata->dev->dev_addr,
413 IEEE802154_ADDR_LEN))
414 skb->pkt_type = PACKET_HOST;
416 skb->pkt_type = PACKET_OTHERHOST;
418 case IEEE802154_ADDR_SHORT:
419 if (mac_cb(skb)->da.pan_id != sdata->pan_id &&
420 mac_cb(skb)->da.pan_id != IEEE802154_PANID_BROADCAST)
421 skb->pkt_type = PACKET_OTHERHOST;
422 else if (mac_cb(skb)->da.short_addr == sdata->short_addr)
423 skb->pkt_type = PACKET_HOST;
424 else if (mac_cb(skb)->da.short_addr ==
425 IEEE802154_ADDR_BROADCAST)
426 skb->pkt_type = PACKET_BROADCAST;
428 skb->pkt_type = PACKET_OTHERHOST;
434 spin_unlock_bh(&sdata->mib_lock);
436 skb->dev = sdata->dev;
438 sdata->dev->stats.rx_packets++;
439 sdata->dev->stats.rx_bytes += skb->len;
441 switch (mac_cb_type(skb)) {
442 case IEEE802154_FC_TYPE_DATA:
443 return mac802154_process_data(sdata->dev, skb);
445 pr_warning("ieee802154: bad frame received (type = %d)\n",
452 static int mac802154_parse_frame_start(struct sk_buff *skb)
454 u8 *head = skb->data;
457 if (mac802154_fetch_skb_u16(skb, &fc) ||
458 mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
461 pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
463 mac_cb(skb)->flags = IEEE802154_FC_TYPE(fc);
464 mac_cb(skb)->sa.addr_type = IEEE802154_FC_SAMODE(fc);
465 mac_cb(skb)->da.addr_type = IEEE802154_FC_DAMODE(fc);
467 if (fc & IEEE802154_FC_INTRA_PAN)
468 mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;
470 if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
471 if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
474 /* source PAN id compression */
475 if (mac_cb_is_intrapan(skb))
476 mac_cb(skb)->sa.pan_id = mac_cb(skb)->da.pan_id;
478 pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
480 if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
481 u16 *da = &(mac_cb(skb)->da.short_addr);
483 if (mac802154_fetch_skb_u16(skb, da))
486 pr_debug("destination address is short: %04x\n",
487 mac_cb(skb)->da.short_addr);
489 if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
492 mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
494 skb_pull(skb, IEEE802154_ADDR_LEN);
496 pr_debug("destination address is hardware\n");
500 if (mac_cb(skb)->sa.addr_type != IEEE802154_ADDR_NONE) {
501 /* non PAN-compression, fetch source address id */
502 if (!(mac_cb_is_intrapan(skb))) {
503 u16 *sa_pan = &(mac_cb(skb)->sa.pan_id);
505 if (mac802154_fetch_skb_u16(skb, sa_pan))
509 pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
511 if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
512 u16 *sa = &(mac_cb(skb)->sa.short_addr);
514 if (mac802154_fetch_skb_u16(skb, sa))
517 pr_debug("source address is short: %04x\n",
518 mac_cb(skb)->sa.short_addr);
520 if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
523 mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
525 skb_pull(skb, IEEE802154_ADDR_LEN);
527 pr_debug("source address is hardware\n");
536 void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
539 struct sk_buff *sskb;
540 struct mac802154_sub_if_data *sdata;
542 ret = mac802154_parse_frame_start(skb);
544 pr_debug("got invalid frame\n");
549 list_for_each_entry_rcu(sdata, &priv->slaves, list) {
550 if (sdata->type != IEEE802154_DEV_WPAN)
553 sskb = skb_clone(skb, GFP_ATOMIC);
555 mac802154_subif_frame(sdata, sskb);