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 struct rb_root rb_root = RB_ROOT;
  16 static struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX + 1];
  17 static DEFINE_SPINLOCK(lowpan_nhc_lock);
  18
  19 static int lowpan_nhc_insert(struct lowpan_nhc *nhc)
  20 {
  21         struct rb_node **new = &rb_root.rb_node, *parent = NULL;
  22
  23         /* Figure out where to put new node */
  24         while (*new) {
  25                 struct lowpan_nhc *this = rb_entry(*new, struct lowpan_nhc,
  26                                                    node);
  27                 int result, len_dif, len;
  28
  29                 len_dif = nhc->idlen - this->idlen;
  30
  31                 if (nhc->idlen < this->idlen)
  32                         len = nhc->idlen;
  33                 else
  34                         len = this->idlen;
  35
  36                 result = memcmp(nhc->id, this->id, len);
  37                 if (!result)
  38                         result = len_dif;
  39
  40                 parent = *new;
  41                 if (result < 0)
  42                         new = &((*new)->rb_left);
  43                 else if (result > 0)
  44                         new = &((*new)->rb_right);
  45                 else
  46                         return -EEXIST;
  47         }
  48
  49         /* Add new node and rebalance tree. */
  50         rb_link_node(&nhc->node, parent, new);
  51         rb_insert_color(&nhc->node, &rb_root);
  52
  53         return 0;
  54 }
  55
  56 static void lowpan_nhc_remove(struct lowpan_nhc *nhc)
  57 {
  58         rb_erase(&nhc->node, &rb_root);
  59 }
  60
  61 static struct lowpan_nhc *lowpan_nhc_by_nhcid(const struct sk_buff *skb)
  62 {
  63         struct rb_node *node = rb_root.rb_node;
  64         const u8 *nhcid_skb_ptr = skb->data;
  65
  66         while (node) {
  67                 struct lowpan_nhc *nhc = rb_entry(node, struct lowpan_nhc,
  68                                                   node);
  69                 u8 nhcid_skb_ptr_masked[LOWPAN_NHC_MAX_ID_LEN];
  70                 int result, i;
  71
  72                 if (nhcid_skb_ptr + nhc->idlen > skb->data + skb->len)
  73                         return NULL;
  74
  75                 /* copy and mask afterwards the nhid value from skb */
  76                 memcpy(nhcid_skb_ptr_masked, nhcid_skb_ptr, nhc->idlen);
  77                 for (i = 0; i < nhc->idlen; i++)
  78                         nhcid_skb_ptr_masked[i] &= nhc->idmask[i];
  79
  80                 result = memcmp(nhcid_skb_ptr_masked, nhc->id, nhc->idlen);
  81                 if (result < 0)
  82                         node = node->rb_left;
  83                 else if (result > 0)
  84                         node = node->rb_right;
  85                 else
  86                         return nhc;
  87         }
  88
  89         return NULL;
  90 }
  91
  92 int lowpan_nhc_check_compression(struct sk_buff *skb,
  93                                  const struct ipv6hdr *hdr, u8 **hc_ptr)
  94 {
  95         struct lowpan_nhc *nhc;
  96         int ret = 0;
  97
  98         spin_lock_bh(&lowpan_nhc_lock);
  99
 100         nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
 101         if (!(nhc && nhc->compress))
 102                 ret = -ENOENT;
 103
 104         spin_unlock_bh(&lowpan_nhc_lock);
 105
 106         return ret;
 107 }
 108
 109 int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr,
 110                               u8 **hc_ptr)
 111 {
 112         int ret;
 113         struct lowpan_nhc *nhc;
 114
 115         spin_lock_bh(&lowpan_nhc_lock);
 116
 117         nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
 118         /* check if the nhc module was removed in unlocked part.
 119          * TODO: this is a workaround we should prevent unloading
 120          * of nhc modules while unlocked part, this will always drop
 121          * the lowpan packet but it's very unlikely.
 122          *
 123          * Solution isn't easy because we need to decide at
 124          * lowpan_nhc_check_compression if we do a compression or not.
 125          * Because the inline data which is added to skb, we can't move this
 126          * handling.
 127          */
 128         if (unlikely(!nhc || !nhc->compress)) {
 129                 ret = -EINVAL;
 130                 goto out;
 131         }
 132
 133         /* In the case of RAW sockets the transport header is not set by
 134          * the ip6 stack so we must set it ourselves
 135          */
 136         if (skb->transport_header == skb->network_header)
 137                 skb_set_transport_header(skb, sizeof(struct ipv6hdr));
 138
 139         ret = nhc->compress(skb, hc_ptr);
 140         if (ret < 0)
 141                 goto out;
 142
 143         /* skip the transport header */
 144         skb_pull(skb, nhc->nexthdrlen);
 145
 146 out:
 147         spin_unlock_bh(&lowpan_nhc_lock);
 148
 149         return ret;
 150 }
 151
 152 int lowpan_nhc_do_uncompression(struct sk_buff *skb,
 153                                 const struct net_device *dev,
 154                                 struct ipv6hdr *hdr)
 155 {
 156         struct lowpan_nhc *nhc;
 157         int ret;
 158
 159         spin_lock_bh(&lowpan_nhc_lock);
 160
 161         nhc = lowpan_nhc_by_nhcid(skb);
 162         if (nhc) {
 163                 if (nhc->uncompress) {
 164                         ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) +
 165                                               nhc->nexthdrlen);
 166                         if (ret < 0) {
 167                                 spin_unlock_bh(&lowpan_nhc_lock);
 168                                 return ret;
 169                         }
 170                 } else {
 171                         spin_unlock_bh(&lowpan_nhc_lock);
 172                         netdev_warn(dev, "received nhc id for %s which is not implemented.\n",
 173                                     nhc->name);
 174                         return -ENOTSUPP;
 175                 }
 176         } else {
 177                 spin_unlock_bh(&lowpan_nhc_lock);
 178                 netdev_warn(dev, "received unknown nhc id which was not found.\n");
 179                 return -ENOENT;
 180         }
 181
 182         hdr->nexthdr = nhc->nexthdr;
 183         skb_reset_transport_header(skb);
 184         raw_dump_table(__func__, "raw transport header dump",
 185                        skb_transport_header(skb), nhc->nexthdrlen);
 186
 187         spin_unlock_bh(&lowpan_nhc_lock);
 188
 189         return 0;
 190 }
 191
 192 int lowpan_nhc_add(struct lowpan_nhc *nhc)
 193 {
 194         int ret;
 195
 196         if (!nhc->idlen || !nhc->idsetup)
 197                 return -EINVAL;
 198
 199         WARN_ONCE(nhc->idlen > LOWPAN_NHC_MAX_ID_LEN,
 200                   "LOWPAN_NHC_MAX_ID_LEN should be updated to %zd.\n",
 201                   nhc->idlen);
 202
 203         nhc->idsetup(nhc);
 204
 205         spin_lock_bh(&lowpan_nhc_lock);
 206
 207         if (lowpan_nexthdr_nhcs[nhc->nexthdr]) {
 208                 ret = -EEXIST;
 209                 goto out;
 210         }
 211
 212         ret = lowpan_nhc_insert(nhc);
 213         if (ret < 0)
 214                 goto out;
 215
 216         lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc;
 217 out:
 218         spin_unlock_bh(&lowpan_nhc_lock);
 219         return ret;
 220 }
 221 EXPORT_SYMBOL(lowpan_nhc_add);
 222
 223 void lowpan_nhc_del(struct lowpan_nhc *nhc)
 224 {
 225         spin_lock_bh(&lowpan_nhc_lock);
 226
 227         lowpan_nhc_remove(nhc);
 228         lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL;
 229
 230         spin_unlock_bh(&lowpan_nhc_lock);
 231
 232         synchronize_net();
 233 }
 234 EXPORT_SYMBOL(lowpan_nhc_del);