GNU Linux-libre 4.19.211-gnu1
[releases.git] / 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 + 1];
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 = rb_entry(*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 = rb_entry(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 {
100         struct lowpan_nhc *nhc;
101         int ret = 0;
102
103         spin_lock_bh(&lowpan_nhc_lock);
104
105         nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
106         if (!(nhc && nhc->compress))
107                 ret = -ENOENT;
108
109         spin_unlock_bh(&lowpan_nhc_lock);
110
111         return ret;
112 }
113
114 int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr,
115                               u8 **hc_ptr)
116 {
117         int ret;
118         struct lowpan_nhc *nhc;
119
120         spin_lock_bh(&lowpan_nhc_lock);
121
122         nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
123         /* check if the nhc module was removed in unlocked part.
124          * TODO: this is a workaround we should prevent unloading
125          * of nhc modules while unlocked part, this will always drop
126          * the lowpan packet but it's very unlikely.
127          *
128          * Solution isn't easy because we need to decide at
129          * lowpan_nhc_check_compression if we do a compression or not.
130          * Because the inline data which is added to skb, we can't move this
131          * handling.
132          */
133         if (unlikely(!nhc || !nhc->compress)) {
134                 ret = -EINVAL;
135                 goto out;
136         }
137
138         /* In the case of RAW sockets the transport header is not set by
139          * the ip6 stack so we must set it ourselves
140          */
141         if (skb->transport_header == skb->network_header)
142                 skb_set_transport_header(skb, sizeof(struct ipv6hdr));
143
144         ret = nhc->compress(skb, hc_ptr);
145         if (ret < 0)
146                 goto out;
147
148         /* skip the transport header */
149         skb_pull(skb, nhc->nexthdrlen);
150
151 out:
152         spin_unlock_bh(&lowpan_nhc_lock);
153
154         return ret;
155 }
156
157 int lowpan_nhc_do_uncompression(struct sk_buff *skb,
158                                 const struct net_device *dev,
159                                 struct ipv6hdr *hdr)
160 {
161         struct lowpan_nhc *nhc;
162         int ret;
163
164         spin_lock_bh(&lowpan_nhc_lock);
165
166         nhc = lowpan_nhc_by_nhcid(skb);
167         if (nhc) {
168                 if (nhc->uncompress) {
169                         ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) +
170                                               nhc->nexthdrlen);
171                         if (ret < 0) {
172                                 spin_unlock_bh(&lowpan_nhc_lock);
173                                 return ret;
174                         }
175                 } else {
176                         spin_unlock_bh(&lowpan_nhc_lock);
177                         netdev_warn(dev, "received nhc id for %s which is not implemented.\n",
178                                     nhc->name);
179                         return -ENOTSUPP;
180                 }
181         } else {
182                 spin_unlock_bh(&lowpan_nhc_lock);
183                 netdev_warn(dev, "received unknown nhc id which was not found.\n");
184                 return -ENOENT;
185         }
186
187         hdr->nexthdr = nhc->nexthdr;
188         skb_reset_transport_header(skb);
189         raw_dump_table(__func__, "raw transport header dump",
190                        skb_transport_header(skb), nhc->nexthdrlen);
191
192         spin_unlock_bh(&lowpan_nhc_lock);
193
194         return 0;
195 }
196
197 int lowpan_nhc_add(struct lowpan_nhc *nhc)
198 {
199         int ret;
200
201         if (!nhc->idlen || !nhc->idsetup)
202                 return -EINVAL;
203
204         WARN_ONCE(nhc->idlen > LOWPAN_NHC_MAX_ID_LEN,
205                   "LOWPAN_NHC_MAX_ID_LEN should be updated to %zd.\n",
206                   nhc->idlen);
207
208         nhc->idsetup(nhc);
209
210         spin_lock_bh(&lowpan_nhc_lock);
211
212         if (lowpan_nexthdr_nhcs[nhc->nexthdr]) {
213                 ret = -EEXIST;
214                 goto out;
215         }
216
217         ret = lowpan_nhc_insert(nhc);
218         if (ret < 0)
219                 goto out;
220
221         lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc;
222 out:
223         spin_unlock_bh(&lowpan_nhc_lock);
224         return ret;
225 }
226 EXPORT_SYMBOL(lowpan_nhc_add);
227
228 void lowpan_nhc_del(struct lowpan_nhc *nhc)
229 {
230         spin_lock_bh(&lowpan_nhc_lock);
231
232         lowpan_nhc_remove(nhc);
233         lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL;
234
235         spin_unlock_bh(&lowpan_nhc_lock);
236
237         synchronize_net();
238 }
239 EXPORT_SYMBOL(lowpan_nhc_del);