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 
lowpan_nhc_by_nhcid(struct sk_buff * skb)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 
lowpan_nhc_check_compression(struct sk_buff * skb,const struct ipv6hdr * hdr,u8 ** hc_ptr)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 
lowpan_nhc_do_compression(struct sk_buff * skb,const struct ipv6hdr * hdr,u8 ** hc_ptr)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 
lowpan_nhc_do_uncompression(struct sk_buff * skb,const struct net_device * dev,struct ipv6hdr * hdr)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 
lowpan_nhc_add(const struct lowpan_nhc * nhc)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 
lowpan_nhc_del(const struct lowpan_nhc * nhc)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);
170