net/6lowpan/nhc.c

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