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
 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);