~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

TOMOYO Linux Cross Reference
Linux/net/mac802154/wpan.c

Version: ~ [ linux-5.11 ] ~ [ linux-5.10.17 ] ~ [ linux-5.9.16 ] ~ [ linux-5.8.18 ] ~ [ linux-5.7.19 ] ~ [ linux-5.6.19 ] ~ [ linux-5.5.19 ] ~ [ linux-5.4.99 ] ~ [ linux-5.3.18 ] ~ [ linux-5.2.21 ] ~ [ linux-5.1.21 ] ~ [ linux-5.0.21 ] ~ [ linux-4.20.17 ] ~ [ linux-4.19.176 ] ~ [ linux-4.18.20 ] ~ [ linux-4.17.19 ] ~ [ linux-4.16.18 ] ~ [ linux-4.15.18 ] ~ [ linux-4.14.221 ] ~ [ linux-4.13.16 ] ~ [ linux-4.12.14 ] ~ [ linux-4.11.12 ] ~ [ linux-4.10.17 ] ~ [ linux-4.9.257 ] ~ [ linux-4.8.17 ] ~ [ linux-4.7.10 ] ~ [ linux-4.6.7 ] ~ [ linux-4.5.7 ] ~ [ linux-4.4.257 ] ~ [ linux-4.3.6 ] ~ [ linux-4.2.8 ] ~ [ linux-4.1.52 ] ~ [ linux-4.0.9 ] ~ [ linux-3.19.8 ] ~ [ linux-3.18.140 ] ~ [ linux-3.17.8 ] ~ [ linux-3.16.85 ] ~ [ linux-3.15.10 ] ~ [ linux-3.14.79 ] ~ [ linux-3.13.11 ] ~ [ linux-3.12.74 ] ~ [ linux-3.11.10 ] ~ [ linux-3.10.108 ] ~ [ linux-2.6.32.71 ] ~ [ linux-2.6.0 ] ~ [ linux-2.4.37.11 ] ~ [ unix-v6-master ] ~ [ ccs-tools-1.8.5 ] ~ [ policy-sample ] ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  1 /*
  2  * Copyright 2007-2012 Siemens AG
  3  *
  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.
  7  *
  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.
 12  *
 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.
 16  *
 17  * Written by:
 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>
 22  */
 23 
 24 #include <linux/netdevice.h>
 25 #include <linux/module.h>
 26 #include <linux/if_arp.h>
 27 
 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>
 35 
 36 #include "mac802154.h"
 37 
 38 static inline int mac802154_fetch_skb_u8(struct sk_buff *skb, u8 *val)
 39 {
 40         if (unlikely(!pskb_may_pull(skb, 1)))
 41                 return -EINVAL;
 42 
 43         *val = skb->data[0];
 44         skb_pull(skb, 1);
 45 
 46         return 0;
 47 }
 48 
 49 static inline int mac802154_fetch_skb_u16(struct sk_buff *skb, u16 *val)
 50 {
 51         if (unlikely(!pskb_may_pull(skb, 2)))
 52                 return -EINVAL;
 53 
 54         *val = skb->data[0] | (skb->data[1] << 8);
 55         skb_pull(skb, 2);
 56 
 57         return 0;
 58 }
 59 
 60 static inline void mac802154_haddr_copy_swap(u8 *dest, const u8 *src)
 61 {
 62         int i;
 63         for (i = 0; i < IEEE802154_ADDR_LEN; i++)
 64                 dest[IEEE802154_ADDR_LEN - i - 1] = src[i];
 65 }
 66 
 67 static int
 68 mac802154_wpan_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 69 {
 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;
 74 
 75         spin_lock_bh(&priv->mib_lock);
 76 
 77         switch (cmd) {
 78         case SIOCGIFADDR:
 79                 if (priv->pan_id == IEEE802154_PANID_BROADCAST ||
 80                     priv->short_addr == IEEE802154_ADDR_BROADCAST) {
 81                         err = -EADDRNOTAVAIL;
 82                         break;
 83                 }
 84 
 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;
 89 
 90                 err = 0;
 91                 break;
 92         case SIOCSIFADDR:
 93                 dev_warn(&dev->dev,
 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) {
100                         err = -EINVAL;
101                         break;
102                 }
103 
104                 priv->pan_id = sa->addr.pan_id;
105                 priv->short_addr = sa->addr.short_addr;
106 
107                 err = 0;
108                 break;
109         }
110 
111         spin_unlock_bh(&priv->mib_lock);
112         return err;
113 }
114 
115 static int mac802154_wpan_mac_addr(struct net_device *dev, void *p)
116 {
117         struct sockaddr *addr = p;
118 
119         if (netif_running(dev))
120                 return -EBUSY;
121 
122         /* FIXME: validate addr */
123         memcpy(dev->dev_addr, addr->sa_data, dev->addr_len);
124         mac802154_dev_set_ieee_addr(dev);
125         return 0;
126 }
127 
128 static int mac802154_header_create(struct sk_buff *skb,
129                                    struct net_device *dev,
130                                    unsigned short type,
131                                    const void *_daddr,
132                                    const void *_saddr,
133                                    unsigned len)
134 {
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);
139         int pos = 2;
140         u8 head[MAC802154_FRAME_HARD_HEADER_LEN];
141         u16 fc;
142 
143         if (!daddr)
144                 return -EINVAL;
145 
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;
150 
151         if (!saddr) {
152                 spin_lock_bh(&priv->mib_lock);
153 
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);
160                 } else {
161                         dev_addr.addr_type = IEEE802154_ADDR_SHORT;
162                         dev_addr.short_addr = priv->short_addr;
163                 }
164 
165                 dev_addr.pan_id = priv->pan_id;
166                 saddr = &dev_addr;
167 
168                 spin_unlock_bh(&priv->mib_lock);
169         }
170 
171         if (daddr->addr_type != IEEE802154_ADDR_NONE) {
172                 fc |= (daddr->addr_type << IEEE802154_FC_DAMODE_SHIFT);
173 
174                 head[pos++] = daddr->pan_id & 0xff;
175                 head[pos++] = daddr->pan_id >> 8;
176 
177                 if (daddr->addr_type == IEEE802154_ADDR_SHORT) {
178                         head[pos++] = daddr->short_addr & 0xff;
179                         head[pos++] = daddr->short_addr >> 8;
180                 } else {
181                         mac802154_haddr_copy_swap(head + pos, daddr->hwaddr);
182                         pos += IEEE802154_ADDR_LEN;
183                 }
184         }
185 
186         if (saddr->addr_type != IEEE802154_ADDR_NONE) {
187                 fc |= (saddr->addr_type << IEEE802154_FC_SAMODE_SHIFT);
188 
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;
193                 } else {
194                         head[pos++] = saddr->pan_id & 0xff;
195                         head[pos++] = saddr->pan_id >> 8;
196                 }
197 
198                 if (saddr->addr_type == IEEE802154_ADDR_SHORT) {
199                         head[pos++] = saddr->short_addr & 0xff;
200                         head[pos++] = saddr->short_addr >> 8;
201                 } else {
202                         mac802154_haddr_copy_swap(head + pos, saddr->hwaddr);
203                         pos += IEEE802154_ADDR_LEN;
204                 }
205         }
206 
207         head[0] = fc;
208         head[1] = fc >> 8;
209 
210         memcpy(skb_push(skb, pos), head, pos);
211 
212         return pos;
213 }
214 
215 static int
216 mac802154_header_parse(const struct sk_buff *skb, unsigned char *haddr)
217 {
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;
221         u16 fc;
222         int da_type;
223 
224         if (hdr + 3 > tail)
225                 goto malformed;
226 
227         fc = hdr[0] | (hdr[1] << 8);
228 
229         hdr += 3;
230 
231         da_type = IEEE802154_FC_DAMODE(fc);
232         addr->addr_type = IEEE802154_FC_SAMODE(fc);
233 
234         switch (da_type) {
235         case IEEE802154_ADDR_NONE:
236                 if (fc & IEEE802154_FC_INTRA_PAN)
237                         goto malformed;
238                 break;
239         case IEEE802154_ADDR_LONG:
240                 if (fc & IEEE802154_FC_INTRA_PAN) {
241                         if (hdr + 2 > tail)
242                                 goto malformed;
243                         addr->pan_id = hdr[0] | (hdr[1] << 8);
244                         hdr += 2;
245                 }
246 
247                 if (hdr + IEEE802154_ADDR_LEN > tail)
248                         goto malformed;
249 
250                 hdr += IEEE802154_ADDR_LEN;
251                 break;
252         case IEEE802154_ADDR_SHORT:
253                 if (fc & IEEE802154_FC_INTRA_PAN) {
254                         if (hdr + 2 > tail)
255                                 goto malformed;
256                         addr->pan_id = hdr[0] | (hdr[1] << 8);
257                         hdr += 2;
258                 }
259 
260                 if (hdr + 2 > tail)
261                         goto malformed;
262 
263                 hdr += 2;
264                 break;
265         default:
266                 goto malformed;
267 
268         }
269 
270         switch (addr->addr_type) {
271         case IEEE802154_ADDR_NONE:
272                 break;
273         case IEEE802154_ADDR_LONG:
274                 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
275                         if (hdr + 2 > tail)
276                                 goto malformed;
277                         addr->pan_id = hdr[0] | (hdr[1] << 8);
278                         hdr += 2;
279                 }
280 
281                 if (hdr + IEEE802154_ADDR_LEN > tail)
282                         goto malformed;
283 
284                 mac802154_haddr_copy_swap(addr->hwaddr, hdr);
285                 hdr += IEEE802154_ADDR_LEN;
286                 break;
287         case IEEE802154_ADDR_SHORT:
288                 if (!(fc & IEEE802154_FC_INTRA_PAN)) {
289                         if (hdr + 2 > tail)
290                                 goto malformed;
291                         addr->pan_id = hdr[0] | (hdr[1] << 8);
292                         hdr += 2;
293                 }
294 
295                 if (hdr + 2 > tail)
296                         goto malformed;
297 
298                 addr->short_addr = hdr[0] | (hdr[1] << 8);
299                 hdr += 2;
300                 break;
301         default:
302                 goto malformed;
303         }
304 
305         return sizeof(struct ieee802154_addr);
306 
307 malformed:
308         pr_debug("malformed packet\n");
309         return 0;
310 }
311 
312 static netdev_tx_t
313 mac802154_wpan_xmit(struct sk_buff *skb, struct net_device *dev)
314 {
315         struct mac802154_sub_if_data *priv;
316         u8 chan, page;
317 
318         priv = netdev_priv(dev);
319 
320         spin_lock_bh(&priv->mib_lock);
321         chan = priv->chan;
322         page = priv->page;
323         spin_unlock_bh(&priv->mib_lock);
324 
325         if (chan == MAC802154_CHAN_NONE ||
326             page >= WPAN_NUM_PAGES ||
327             chan >= WPAN_NUM_CHANNELS) {
328                 kfree_skb(skb);
329                 return NETDEV_TX_OK;
330         }
331 
332         skb->skb_iif = dev->ifindex;
333         dev->stats.tx_packets++;
334         dev->stats.tx_bytes += skb->len;
335 
336         return mac802154_tx(priv->hw, skb, page, chan);
337 }
338 
339 static struct header_ops mac802154_header_ops = {
340         .create         = mac802154_header_create,
341         .parse          = mac802154_header_parse,
342 };
343 
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,
350 };
351 
352 void mac802154_wpan_setup(struct net_device *dev)
353 {
354         struct mac802154_sub_if_data *priv;
355 
356         dev->addr_len           = IEEE802154_ADDR_LEN;
357         memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
358 
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;
367 
368         dev->destructor         = free_netdev;
369         dev->netdev_ops         = &mac802154_wpan_ops;
370         dev->ml_priv            = &mac802154_mlme_wpan;
371 
372         priv = netdev_priv(dev);
373         priv->type = IEEE802154_DEV_WPAN;
374 
375         priv->chan = MAC802154_CHAN_NONE;
376         priv->page = 0;
377 
378         spin_lock_init(&priv->mib_lock);
379 
380         get_random_bytes(&priv->bsn, 1);
381         get_random_bytes(&priv->dsn, 1);
382 
383         priv->pan_id = IEEE802154_PANID_BROADCAST;
384         priv->short_addr = IEEE802154_ADDR_BROADCAST;
385 }
386 
387 static int mac802154_process_data(struct net_device *dev, struct sk_buff *skb)
388 {
389         return netif_rx_ni(skb);
390 }
391 
392 static int
393 mac802154_subif_frame(struct mac802154_sub_if_data *sdata, struct sk_buff *skb)
394 {
395         pr_debug("getting packet via slave interface %s\n", sdata->dev->name);
396 
397         spin_lock_bh(&sdata->mib_lock);
398 
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;
404                 else
405                         /* ACK comes with both addresses empty */
406                         skb->pkt_type = PACKET_HOST;
407                 break;
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;
415                 else
416                         skb->pkt_type = PACKET_OTHERHOST;
417                 break;
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;
427                 else
428                         skb->pkt_type = PACKET_OTHERHOST;
429                 break;
430         default:
431                 break;
432         }
433 
434         spin_unlock_bh(&sdata->mib_lock);
435 
436         skb->dev = sdata->dev;
437 
438         sdata->dev->stats.rx_packets++;
439         sdata->dev->stats.rx_bytes += skb->len;
440 
441         switch (mac_cb_type(skb)) {
442         case IEEE802154_FC_TYPE_DATA:
443                 return mac802154_process_data(sdata->dev, skb);
444         default:
445                 pr_warning("ieee802154: bad frame received (type = %d)\n",
446                            mac_cb_type(skb));
447                 kfree_skb(skb);
448                 return NET_RX_DROP;
449         }
450 }
451 
452 static int mac802154_parse_frame_start(struct sk_buff *skb)
453 {
454         u8 *head = skb->data;
455         u16 fc;
456 
457         if (mac802154_fetch_skb_u16(skb, &fc) ||
458             mac802154_fetch_skb_u8(skb, &(mac_cb(skb)->seq)))
459                 goto err;
460 
461         pr_debug("fc: %04x dsn: %02x\n", fc, head[2]);
462 
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);
466 
467         if (fc & IEEE802154_FC_INTRA_PAN)
468                 mac_cb(skb)->flags |= MAC_CB_FLAG_INTRAPAN;
469 
470         if (mac_cb(skb)->da.addr_type != IEEE802154_ADDR_NONE) {
471                 if (mac802154_fetch_skb_u16(skb, &(mac_cb(skb)->da.pan_id)))
472                         goto err;
473 
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;
477 
478                 pr_debug("dest PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
479 
480                 if (mac_cb(skb)->da.addr_type == IEEE802154_ADDR_SHORT) {
481                         u16 *da = &(mac_cb(skb)->da.short_addr);
482 
483                         if (mac802154_fetch_skb_u16(skb, da))
484                                 goto err;
485 
486                         pr_debug("destination address is short: %04x\n",
487                                  mac_cb(skb)->da.short_addr);
488                 } else {
489                         if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
490                                 goto err;
491 
492                         mac802154_haddr_copy_swap(mac_cb(skb)->da.hwaddr,
493                                                   skb->data);
494                         skb_pull(skb, IEEE802154_ADDR_LEN);
495 
496                         pr_debug("destination address is hardware\n");
497                 }
498         }
499 
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);
504 
505                         if (mac802154_fetch_skb_u16(skb, sa_pan))
506                                 goto err;
507                 }
508 
509                 pr_debug("source PAN addr: %04x\n", mac_cb(skb)->da.pan_id);
510 
511                 if (mac_cb(skb)->sa.addr_type == IEEE802154_ADDR_SHORT) {
512                         u16 *sa = &(mac_cb(skb)->sa.short_addr);
513 
514                         if (mac802154_fetch_skb_u16(skb, sa))
515                                 goto err;
516 
517                         pr_debug("source address is short: %04x\n",
518                                  mac_cb(skb)->sa.short_addr);
519                 } else {
520                         if (!pskb_may_pull(skb, IEEE802154_ADDR_LEN))
521                                 goto err;
522 
523                         mac802154_haddr_copy_swap(mac_cb(skb)->sa.hwaddr,
524                                                   skb->data);
525                         skb_pull(skb, IEEE802154_ADDR_LEN);
526 
527                         pr_debug("source address is hardware\n");
528                 }
529         }
530 
531         return 0;
532 err:
533         return -EINVAL;
534 }
535 
536 void mac802154_wpans_rx(struct mac802154_priv *priv, struct sk_buff *skb)
537 {
538         int ret;
539         struct sk_buff *sskb;
540         struct mac802154_sub_if_data *sdata;
541 
542         ret = mac802154_parse_frame_start(skb);
543         if (ret) {
544                 pr_debug("got invalid frame\n");
545                 return;
546         }
547 
548         rcu_read_lock();
549         list_for_each_entry_rcu(sdata, &priv->slaves, list) {
550                 if (sdata->type != IEEE802154_DEV_WPAN)
551                         continue;
552 
553                 sskb = skb_clone(skb, GFP_ATOMIC);
554                 if (sskb)
555                         mac802154_subif_frame(sdata, sskb);
556         }
557         rcu_read_unlock();
558 }
559 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~

kernel.org | git.kernel.org | LWN.net | Project Home | Wiki (Japanese) | Wiki (English) | SVN repository | Mail admin

Linux® is a registered trademark of Linus Torvalds in the United States and other countries.
TOMOYO® is a registered trademark of NTT DATA CORPORATION.

osdn.jp