net/6lowpan/nhc.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *      6LoWPAN next header compression
   4  *
   5  *      Authors:
   6  *      Alexander Aring         <aar@pengutronix.de>
   7  */
   8
   9 #include <linux/netdevice.h>
  10
  11 #include <net/ipv6.h>
  12
  13 #include "nhc.h"
  14
  15 static const struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX + 1];
  16 static DEFINE_SPINLOCK(lowpan_nhc_lock);
  17
  18 static const struct lowpan_nhc *lowpan_nhc_by_nhcid(struct sk_buff *skb)
  19 {
  20         const struct lowpan_nhc *nhc;
  21         int i;
  22         u8 id;
  23
  24         if (!pskb_may_pull(skb, 1))
  25                 return NULL;
  26
  27         id = *skb->data;
  28
  29         for (i = 0; i < NEXTHDR_MAX + 1; i++) {
  30                 nhc = lowpan_nexthdr_nhcs[i];
  31                 if (!nhc)
  32                         continue;
  33
  34                 if ((id & nhc->idmask) == nhc->id)
  35                         return nhc;
  36         }
  37
  38         return NULL;
  39 }
  40
  41 int lowpan_nhc_check_compression(struct sk_buff *skb,
  42                                  const struct ipv6hdr *hdr, u8 **hc_ptr)
  43 {
  44         const struct lowpan_nhc *nhc;
  45         int ret = 0;
  46
  47         spin_lock_bh(&lowpan_nhc_lock);
  48
  49         nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
  50         if (!(nhc && nhc->compress))
  51                 ret = -ENOENT;
  52
  53         spin_unlock_bh(&lowpan_nhc_lock);
  54
  55         return ret;
  56 }
  57
  58 int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr,
  59                               u8 **hc_ptr)
  60 {
  61         int ret;
  62         const struct lowpan_nhc *nhc;
  63
  64         spin_lock_bh(&lowpan_nhc_lock);
  65
  66         nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
  67         /* check if the nhc module was removed in unlocked part.
  68          * TODO: this is a workaround we should prevent unloading
  69          * of nhc modules while unlocked part, this will always drop
  70          * the lowpan packet but it's very unlikely.
  71          *
  72          * Solution isn't easy because we need to decide at
  73          * lowpan_nhc_check_compression if we do a compression or not.
  74          * Because the inline data which is added to skb, we can't move this
  75          * handling.
  76          */
  77         if (unlikely(!nhc || !nhc->compress)) {
  78                 ret = -EINVAL;
  79                 goto out;
  80         }
  81
  82         /* In the case of RAW sockets the transport header is not set by
  83          * the ip6 stack so we must set it ourselves
  84          */
  85         if (skb->transport_header == skb->network_header)
  86                 skb_set_transport_header(skb, sizeof(struct ipv6hdr));
  87
  88         ret = nhc->compress(skb, hc_ptr);
  89         if (ret < 0)
  90                 goto out;
  91
  92         /* skip the transport header */
  93         skb_pull(skb, nhc->nexthdrlen);
  94
  95 out:
  96         spin_unlock_bh(&lowpan_nhc_lock);
  97
  98         return ret;
  99 }
 100
 101 int lowpan_nhc_do_uncompression(struct sk_buff *skb,
 102                                 const struct net_device *dev,
 103                                 struct ipv6hdr *hdr)
 104 {
 105         const struct lowpan_nhc *nhc;
 106         int ret;
 107
 108         spin_lock_bh(&lowpan_nhc_lock);
 109
 110         nhc = lowpan_nhc_by_nhcid(skb);
 111         if (nhc) {
 112                 if (nhc->uncompress) {
 113                         ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) +
 114                                               nhc->nexthdrlen);
 115                         if (ret < 0) {
 116                                 spin_unlock_bh(&lowpan_nhc_lock);
 117                                 return ret;
 118                         }
 119                 } else {
 120                         spin_unlock_bh(&lowpan_nhc_lock);
 121                         netdev_warn(dev, "received nhc id for %s which is not implemented.\n",
 122                                     nhc->name);
 123                         return -ENOTSUPP;
 124                 }
 125         } else {
 126                 spin_unlock_bh(&lowpan_nhc_lock);
 127                 netdev_warn(dev, "received unknown nhc id which was not found.\n");
 128                 return -ENOENT;
 129         }
 130
 131         hdr->nexthdr = nhc->nexthdr;
 132         skb_reset_transport_header(skb);
 133         raw_dump_table(__func__, "raw transport header dump",
 134                        skb_transport_header(skb), nhc->nexthdrlen);
 135
 136         spin_unlock_bh(&lowpan_nhc_lock);
 137
 138         return 0;
 139 }
 140
 141 int lowpan_nhc_add(const struct lowpan_nhc *nhc)
 142 {
 143         int ret = 0;
 144
 145         spin_lock_bh(&lowpan_nhc_lock);
 146
 147         if (lowpan_nexthdr_nhcs[nhc->nexthdr]) {
 148                 ret = -EEXIST;
 149                 goto out;
 150         }
 151
 152         lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc;
 153 out:
 154         spin_unlock_bh(&lowpan_nhc_lock);
 155         return ret;
 156 }
 157 EXPORT_SYMBOL(lowpan_nhc_add);
 158
 159 void lowpan_nhc_del(const struct lowpan_nhc *nhc)
 160 {
 161         spin_lock_bh(&lowpan_nhc_lock);
 162
 163         lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL;
 164
 165         spin_unlock_bh(&lowpan_nhc_lock);
 166
 167         synchronize_net();
 168 }
 169 EXPORT_SYMBOL(lowpan_nhc_del);