1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
|
/* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2
* as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/if_arp.h>
#include <net/6lowpan.h>
#include <net/mac802154.h>
#include <net/ieee802154_netdev.h>
#include "6lowpan_i.h"
#define LOWPAN_DISPATCH_FIRST 0xc0
#define LOWPAN_DISPATCH_FRAG_MASK 0xf8
#define LOWPAN_DISPATCH_NALP 0x00
#define LOWPAN_DISPATCH_ESC 0x7f
#define LOWPAN_DISPATCH_HC1 0x42
#define LOWPAN_DISPATCH_DFF 0x43
#define LOWPAN_DISPATCH_BC0 0x50
#define LOWPAN_DISPATCH_MESH 0x80
static int lowpan_give_skb_to_device(struct sk_buff *skb)
{
skb->protocol = htons(ETH_P_IPV6);
return netif_rx(skb);
}
static int lowpan_rx_handlers_result(struct sk_buff *skb, lowpan_rx_result res)
{
switch (res) {
case RX_CONTINUE:
/* nobody cared about this packet */
net_warn_ratelimited("%s: received unknown dispatch\n",
__func__);
/* fall-through */
case RX_DROP_UNUSABLE:
kfree_skb(skb);
/* fall-through */
case RX_DROP:
return NET_RX_DROP;
case RX_QUEUED:
return lowpan_give_skb_to_device(skb);
default:
break;
}
return NET_RX_DROP;
}
static inline bool lowpan_is_frag1(u8 dispatch)
{
return (dispatch & LOWPAN_DISPATCH_FRAG_MASK) == LOWPAN_DISPATCH_FRAG1;
}
static inline bool lowpan_is_fragn(u8 dispatch)
{
return (dispatch & LOWPAN_DISPATCH_FRAG_MASK) == LOWPAN_DISPATCH_FRAGN;
}
static lowpan_rx_result lowpan_rx_h_frag(struct sk_buff *skb)
{
int ret;
if (!(lowpan_is_frag1(*skb_network_header(skb)) ||
lowpan_is_fragn(*skb_network_header(skb))))
return RX_CONTINUE;
ret = lowpan_frag_rcv(skb, *skb_network_header(skb) &
LOWPAN_DISPATCH_FRAG_MASK);
if (ret == 1)
return RX_QUEUED;
/* Packet is freed by lowpan_frag_rcv on error or put into the frag
* bucket.
*/
return RX_DROP;
}
int lowpan_iphc_decompress(struct sk_buff *skb)
{
struct ieee802154_addr_sa sa, da;
struct ieee802154_hdr hdr;
u8 iphc0, iphc1;
void *sap, *dap;
if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
return -EINVAL;
raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
if (lowpan_fetch_skb_u8(skb, &iphc0) ||
lowpan_fetch_skb_u8(skb, &iphc1))
return -EINVAL;
ieee802154_addr_to_sa(&sa, &hdr.source);
ieee802154_addr_to_sa(&da, &hdr.dest);
if (sa.addr_type == IEEE802154_ADDR_SHORT)
sap = &sa.short_addr;
else
sap = &sa.hwaddr;
if (da.addr_type == IEEE802154_ADDR_SHORT)
dap = &da.short_addr;
else
dap = &da.hwaddr;
return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
IEEE802154_ADDR_LEN, dap, da.addr_type,
IEEE802154_ADDR_LEN, iphc0, iphc1);
}
static lowpan_rx_result lowpan_rx_h_iphc(struct sk_buff *skb)
{
int ret;
if (!lowpan_is_iphc(*skb_network_header(skb)))
return RX_CONTINUE;
/* Setting datagram_offset to zero indicates non frag handling
* while doing lowpan_header_decompress.
*/
lowpan_802154_cb(skb)->d_size = 0;
ret = lowpan_iphc_decompress(skb);
if (ret < 0)
return RX_DROP_UNUSABLE;
return RX_QUEUED;
}
lowpan_rx_result lowpan_rx_h_ipv6(struct sk_buff *skb)
{
if (!lowpan_is_ipv6(*skb_network_header(skb)))
return RX_CONTINUE;
/* Pull off the 1-byte of 6lowpan header. */
skb_pull(skb, 1);
return RX_QUEUED;
}
static inline bool lowpan_is_esc(u8 dispatch)
{
return dispatch == LOWPAN_DISPATCH_ESC;
}
static lowpan_rx_result lowpan_rx_h_esc(struct sk_buff *skb)
{
if (!lowpan_is_esc(*skb_network_header(skb)))
return RX_CONTINUE;
net_warn_ratelimited("%s: %s\n", skb->dev->name,
"6LoWPAN ESC not supported\n");
return RX_DROP_UNUSABLE;
}
static inline bool lowpan_is_hc1(u8 dispatch)
{
return dispatch == LOWPAN_DISPATCH_HC1;
}
static lowpan_rx_result lowpan_rx_h_hc1(struct sk_buff *skb)
{
if (!lowpan_is_hc1(*skb_network_header(skb)))
return RX_CONTINUE;
net_warn_ratelimited("%s: %s\n", skb->dev->name,
"6LoWPAN HC1 not supported\n");
return RX_DROP_UNUSABLE;
}
static inline bool lowpan_is_dff(u8 dispatch)
{
return dispatch == LOWPAN_DISPATCH_DFF;
}
static lowpan_rx_result lowpan_rx_h_dff(struct sk_buff *skb)
{
if (!lowpan_is_dff(*skb_network_header(skb)))
return RX_CONTINUE;
net_warn_ratelimited("%s: %s\n", skb->dev->name,
"6LoWPAN DFF not supported\n");
return RX_DROP_UNUSABLE;
}
static inline bool lowpan_is_bc0(u8 dispatch)
{
return dispatch == LOWPAN_DISPATCH_BC0;
}
static lowpan_rx_result lowpan_rx_h_bc0(struct sk_buff *skb)
{
if (!lowpan_is_bc0(*skb_network_header(skb)))
return RX_CONTINUE;
net_warn_ratelimited("%s: %s\n", skb->dev->name,
"6LoWPAN BC0 not supported\n");
return RX_DROP_UNUSABLE;
}
static inline bool lowpan_is_mesh(u8 dispatch)
{
return (dispatch & LOWPAN_DISPATCH_FIRST) == LOWPAN_DISPATCH_MESH;
}
static lowpan_rx_result lowpan_rx_h_mesh(struct sk_buff *skb)
{
if (!lowpan_is_mesh(*skb_network_header(skb)))
return RX_CONTINUE;
net_warn_ratelimited("%s: %s\n", skb->dev->name,
"6LoWPAN MESH not supported\n");
return RX_DROP_UNUSABLE;
}
static int lowpan_invoke_rx_handlers(struct sk_buff *skb)
{
lowpan_rx_result res;
#define CALL_RXH(rxh) \
do { \
res = rxh(skb); \
if (res != RX_CONTINUE) \
goto rxh_next; \
} while (0)
/* likely at first */
CALL_RXH(lowpan_rx_h_iphc);
CALL_RXH(lowpan_rx_h_frag);
CALL_RXH(lowpan_rx_h_ipv6);
CALL_RXH(lowpan_rx_h_esc);
CALL_RXH(lowpan_rx_h_hc1);
CALL_RXH(lowpan_rx_h_dff);
CALL_RXH(lowpan_rx_h_bc0);
CALL_RXH(lowpan_rx_h_mesh);
rxh_next:
return lowpan_rx_handlers_result(skb, res);
#undef CALL_RXH
}
static inline bool lowpan_is_nalp(u8 dispatch)
{
return (dispatch & LOWPAN_DISPATCH_FIRST) == LOWPAN_DISPATCH_NALP;
}
/* Lookup for reserved dispatch values at:
* https://www.iana.org/assignments/_6lowpan-parameters/_6lowpan-parameters.xhtml#_6lowpan-parameters-1
*
* Last Updated: 2015-01-22
*/
static inline bool lowpan_is_reserved(u8 dispatch)
{
return ((dispatch >= 0x44 && dispatch <= 0x4F) ||
(dispatch >= 0x51 && dispatch <= 0x5F) ||
(dispatch >= 0xc8 && dispatch <= 0xdf) ||
(dispatch >= 0xe8 && dispatch <= 0xff));
}
/* lowpan_rx_h_check checks on generic 6LoWPAN requirements
* in MAC and 6LoWPAN header.
*
* Don't manipulate the skb here, it could be shared buffer.
*/
static inline bool lowpan_rx_h_check(struct sk_buff *skb)
{
__le16 fc = ieee802154_get_fc_from_skb(skb);
/* check on ieee802154 conform 6LoWPAN header */
if (!ieee802154_is_data(fc) ||
!ieee802154_is_intra_pan(fc))
return false;
/* check if we can dereference the dispatch */
if (unlikely(!skb->len))
return false;
if (lowpan_is_nalp(*skb_network_header(skb)) ||
lowpan_is_reserved(*skb_network_header(skb)))
return false;
return true;
}
static int lowpan_rcv(struct sk_buff *skb, struct net_device *wdev,
struct packet_type *pt, struct net_device *orig_wdev)
{
struct net_device *ldev;
if (wdev->type != ARPHRD_IEEE802154 ||
skb->pkt_type == PACKET_OTHERHOST ||
!lowpan_rx_h_check(skb))
return NET_RX_DROP;
ldev = wdev->ieee802154_ptr->lowpan_dev;
if (!ldev || !netif_running(ldev))
return NET_RX_DROP;
/* Replacing skb->dev and followed rx handlers will manipulate skb. */
skb = skb_share_check(skb, GFP_ATOMIC);
if (!skb)
return NET_RX_DROP;
skb->dev = ldev;
/* When receive frag1 it's likely that we manipulate the buffer.
* When recevie iphc we manipulate the data buffer. So we need
* to unshare the buffer.
*/
if (lowpan_is_frag1(*skb_network_header(skb)) ||
lowpan_is_iphc(*skb_network_header(skb))) {
skb = skb_unshare(skb, GFP_ATOMIC);
if (!skb)
return NET_RX_DROP;
}
return lowpan_invoke_rx_handlers(skb);
}
static struct packet_type lowpan_packet_type = {
.type = htons(ETH_P_IEEE802154),
.func = lowpan_rcv,
};
void lowpan_rx_init(void)
{
dev_add_pack(&lowpan_packet_type);
}
void lowpan_rx_exit(void)
{
dev_remove_pack(&lowpan_packet_type);
}
|