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 | |
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, len: 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 | |
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(lock: &lowpan_nhc_lock); |
48 | |
49 | nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; |
50 | if (!(nhc && nhc->compress)) |
51 | ret = -ENOENT; |
52 | |
53 | spin_unlock_bh(lock: &lowpan_nhc_lock); |
54 | |
55 | return ret; |
56 | } |
57 | |
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(lock: &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, offset: 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, len: nhc->nexthdrlen); |
94 | |
95 | out: |
96 | spin_unlock_bh(lock: &lowpan_nhc_lock); |
97 | |
98 | return ret; |
99 | } |
100 | |
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(lock: &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(lock: &lowpan_nhc_lock); |
117 | return ret; |
118 | } |
119 | } else { |
120 | spin_unlock_bh(lock: &lowpan_nhc_lock); |
121 | netdev_warn(dev, format: "received nhc id for %s which is not implemented.\n" , |
122 | nhc->name); |
123 | return -ENOTSUPP; |
124 | } |
125 | } else { |
126 | spin_unlock_bh(lock: &lowpan_nhc_lock); |
127 | netdev_warn(dev, format: "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(caller: __func__, msg: "raw transport header dump" , |
134 | buf: skb_transport_header(skb), len: nhc->nexthdrlen); |
135 | |
136 | spin_unlock_bh(lock: &lowpan_nhc_lock); |
137 | |
138 | return 0; |
139 | } |
140 | |
141 | int lowpan_nhc_add(const struct lowpan_nhc *nhc) |
142 | { |
143 | int ret = 0; |
144 | |
145 | spin_lock_bh(lock: &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(lock: &lowpan_nhc_lock); |
155 | return ret; |
156 | } |
157 | EXPORT_SYMBOL(lowpan_nhc_add); |
158 | |
159 | void lowpan_nhc_del(const struct lowpan_nhc *nhc) |
160 | { |
161 | spin_lock_bh(lock: &lowpan_nhc_lock); |
162 | |
163 | lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL; |
164 | |
165 | spin_unlock_bh(lock: &lowpan_nhc_lock); |
166 | |
167 | synchronize_net(); |
168 | } |
169 | EXPORT_SYMBOL(lowpan_nhc_del); |
170 | |