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
15static const struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX + 1];
16static DEFINE_SPINLOCK(lowpan_nhc_lock);
17
18static 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
41int 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
58int 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
95out:
96	spin_unlock_bh(&lowpan_nhc_lock);
97
98	return ret;
99}
100
101int 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
141int 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;
153out:
154	spin_unlock_bh(&lowpan_nhc_lock);
155	return ret;
156}
157EXPORT_SYMBOL(lowpan_nhc_add);
158
159void 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}
169EXPORT_SYMBOL(lowpan_nhc_del);
170