1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19#include "main.h"
20
21#include <linux/byteorder/generic.h>
22#include <linux/etherdevice.h>
23#include <linux/gfp.h>
24#include <linux/if_ether.h>
25#include <linux/kernel.h>
26#include <linux/kref.h>
27#include <linux/list.h>
28#include <linux/lockdep.h>
29#include <linux/netdevice.h>
30#include <linux/pkt_sched.h>
31#include <linux/rculist.h>
32#include <linux/rcupdate.h>
33#include <linux/skbuff.h>
34#include <linux/slab.h>
35#include <linux/spinlock.h>
36#include <linux/stddef.h>
37#include <linux/string.h>
38#include <linux/types.h>
39#include <uapi/linux/batadv_packet.h>
40
41#include "originator.h"
42#include "send.h"
43#include "tvlv.h"
44
45
46
47
48
49
50static void batadv_tvlv_handler_release(struct kref *ref)
51{
52 struct batadv_tvlv_handler *tvlv_handler;
53
54 tvlv_handler = container_of(ref, struct batadv_tvlv_handler, refcount);
55 kfree_rcu(tvlv_handler, rcu);
56}
57
58
59
60
61
62
63static void batadv_tvlv_handler_put(struct batadv_tvlv_handler *tvlv_handler)
64{
65 kref_put(&tvlv_handler->refcount, batadv_tvlv_handler_release);
66}
67
68
69
70
71
72
73
74
75
76
77static struct batadv_tvlv_handler *
78batadv_tvlv_handler_get(struct batadv_priv *bat_priv, u8 type, u8 version)
79{
80 struct batadv_tvlv_handler *tvlv_handler_tmp, *tvlv_handler = NULL;
81
82 rcu_read_lock();
83 hlist_for_each_entry_rcu(tvlv_handler_tmp,
84 &bat_priv->tvlv.handler_list, list) {
85 if (tvlv_handler_tmp->type != type)
86 continue;
87
88 if (tvlv_handler_tmp->version != version)
89 continue;
90
91 if (!kref_get_unless_zero(&tvlv_handler_tmp->refcount))
92 continue;
93
94 tvlv_handler = tvlv_handler_tmp;
95 break;
96 }
97 rcu_read_unlock();
98
99 return tvlv_handler;
100}
101
102
103
104
105
106static void batadv_tvlv_container_release(struct kref *ref)
107{
108 struct batadv_tvlv_container *tvlv;
109
110 tvlv = container_of(ref, struct batadv_tvlv_container, refcount);
111 kfree(tvlv);
112}
113
114
115
116
117
118
119static void batadv_tvlv_container_put(struct batadv_tvlv_container *tvlv)
120{
121 kref_put(&tvlv->refcount, batadv_tvlv_container_release);
122}
123
124
125
126
127
128
129
130
131
132
133
134
135
136static struct batadv_tvlv_container *
137batadv_tvlv_container_get(struct batadv_priv *bat_priv, u8 type, u8 version)
138{
139 struct batadv_tvlv_container *tvlv_tmp, *tvlv = NULL;
140
141 lockdep_assert_held(&bat_priv->tvlv.container_list_lock);
142
143 hlist_for_each_entry(tvlv_tmp, &bat_priv->tvlv.container_list, list) {
144 if (tvlv_tmp->tvlv_hdr.type != type)
145 continue;
146
147 if (tvlv_tmp->tvlv_hdr.version != version)
148 continue;
149
150 kref_get(&tvlv_tmp->refcount);
151 tvlv = tvlv_tmp;
152 break;
153 }
154
155 return tvlv;
156}
157
158
159
160
161
162
163
164
165
166
167
168static u16 batadv_tvlv_container_list_size(struct batadv_priv *bat_priv)
169{
170 struct batadv_tvlv_container *tvlv;
171 u16 tvlv_len = 0;
172
173 lockdep_assert_held(&bat_priv->tvlv.container_list_lock);
174
175 hlist_for_each_entry(tvlv, &bat_priv->tvlv.container_list, list) {
176 tvlv_len += sizeof(struct batadv_tvlv_hdr);
177 tvlv_len += ntohs(tvlv->tvlv_hdr.len);
178 }
179
180 return tvlv_len;
181}
182
183
184
185
186
187
188
189
190
191
192static void batadv_tvlv_container_remove(struct batadv_priv *bat_priv,
193 struct batadv_tvlv_container *tvlv)
194{
195 lockdep_assert_held(&bat_priv->tvlv.container_list_lock);
196
197 if (!tvlv)
198 return;
199
200 hlist_del(&tvlv->list);
201
202
203 batadv_tvlv_container_put(tvlv);
204 batadv_tvlv_container_put(tvlv);
205}
206
207
208
209
210
211
212
213
214void batadv_tvlv_container_unregister(struct batadv_priv *bat_priv,
215 u8 type, u8 version)
216{
217 struct batadv_tvlv_container *tvlv;
218
219 spin_lock_bh(&bat_priv->tvlv.container_list_lock);
220 tvlv = batadv_tvlv_container_get(bat_priv, type, version);
221 batadv_tvlv_container_remove(bat_priv, tvlv);
222 spin_unlock_bh(&bat_priv->tvlv.container_list_lock);
223}
224
225
226
227
228
229
230
231
232
233
234
235
236
237void batadv_tvlv_container_register(struct batadv_priv *bat_priv,
238 u8 type, u8 version,
239 void *tvlv_value, u16 tvlv_value_len)
240{
241 struct batadv_tvlv_container *tvlv_old, *tvlv_new;
242
243 if (!tvlv_value)
244 tvlv_value_len = 0;
245
246 tvlv_new = kzalloc(sizeof(*tvlv_new) + tvlv_value_len, GFP_ATOMIC);
247 if (!tvlv_new)
248 return;
249
250 tvlv_new->tvlv_hdr.version = version;
251 tvlv_new->tvlv_hdr.type = type;
252 tvlv_new->tvlv_hdr.len = htons(tvlv_value_len);
253
254 memcpy(tvlv_new + 1, tvlv_value, ntohs(tvlv_new->tvlv_hdr.len));
255 INIT_HLIST_NODE(&tvlv_new->list);
256 kref_init(&tvlv_new->refcount);
257
258 spin_lock_bh(&bat_priv->tvlv.container_list_lock);
259 tvlv_old = batadv_tvlv_container_get(bat_priv, type, version);
260 batadv_tvlv_container_remove(bat_priv, tvlv_old);
261
262 kref_get(&tvlv_new->refcount);
263 hlist_add_head(&tvlv_new->list, &bat_priv->tvlv.container_list);
264 spin_unlock_bh(&bat_priv->tvlv.container_list_lock);
265
266
267 batadv_tvlv_container_put(tvlv_new);
268}
269
270
271
272
273
274
275
276
277
278
279
280
281
282static bool batadv_tvlv_realloc_packet_buff(unsigned char **packet_buff,
283 int *packet_buff_len,
284 int min_packet_len,
285 int additional_packet_len)
286{
287 unsigned char *new_buff;
288
289 new_buff = kmalloc(min_packet_len + additional_packet_len, GFP_ATOMIC);
290
291
292 if (!new_buff)
293 return false;
294
295 memcpy(new_buff, *packet_buff, min_packet_len);
296 kfree(*packet_buff);
297 *packet_buff = new_buff;
298 *packet_buff_len = min_packet_len + additional_packet_len;
299
300 return true;
301}
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317u16 batadv_tvlv_container_ogm_append(struct batadv_priv *bat_priv,
318 unsigned char **packet_buff,
319 int *packet_buff_len, int packet_min_len)
320{
321 struct batadv_tvlv_container *tvlv;
322 struct batadv_tvlv_hdr *tvlv_hdr;
323 u16 tvlv_value_len;
324 void *tvlv_value;
325 bool ret;
326
327 spin_lock_bh(&bat_priv->tvlv.container_list_lock);
328 tvlv_value_len = batadv_tvlv_container_list_size(bat_priv);
329
330 ret = batadv_tvlv_realloc_packet_buff(packet_buff, packet_buff_len,
331 packet_min_len, tvlv_value_len);
332
333 if (!ret)
334 goto end;
335
336 if (!tvlv_value_len)
337 goto end;
338
339 tvlv_value = (*packet_buff) + packet_min_len;
340
341 hlist_for_each_entry(tvlv, &bat_priv->tvlv.container_list, list) {
342 tvlv_hdr = tvlv_value;
343 tvlv_hdr->type = tvlv->tvlv_hdr.type;
344 tvlv_hdr->version = tvlv->tvlv_hdr.version;
345 tvlv_hdr->len = tvlv->tvlv_hdr.len;
346 tvlv_value = tvlv_hdr + 1;
347 memcpy(tvlv_value, tvlv + 1, ntohs(tvlv->tvlv_hdr.len));
348 tvlv_value = (u8 *)tvlv_value + ntohs(tvlv->tvlv_hdr.len);
349 }
350
351end:
352 spin_unlock_bh(&bat_priv->tvlv.container_list_lock);
353 return tvlv_value_len;
354}
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371static int batadv_tvlv_call_handler(struct batadv_priv *bat_priv,
372 struct batadv_tvlv_handler *tvlv_handler,
373 bool ogm_source,
374 struct batadv_orig_node *orig_node,
375 u8 *src, u8 *dst,
376 void *tvlv_value, u16 tvlv_value_len)
377{
378 if (!tvlv_handler)
379 return NET_RX_SUCCESS;
380
381 if (ogm_source) {
382 if (!tvlv_handler->ogm_handler)
383 return NET_RX_SUCCESS;
384
385 if (!orig_node)
386 return NET_RX_SUCCESS;
387
388 tvlv_handler->ogm_handler(bat_priv, orig_node,
389 BATADV_NO_FLAGS,
390 tvlv_value, tvlv_value_len);
391 tvlv_handler->flags |= BATADV_TVLV_HANDLER_OGM_CALLED;
392 } else {
393 if (!src)
394 return NET_RX_SUCCESS;
395
396 if (!dst)
397 return NET_RX_SUCCESS;
398
399 if (!tvlv_handler->unicast_handler)
400 return NET_RX_SUCCESS;
401
402 return tvlv_handler->unicast_handler(bat_priv, src,
403 dst, tvlv_value,
404 tvlv_value_len);
405 }
406
407 return NET_RX_SUCCESS;
408}
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424int batadv_tvlv_containers_process(struct batadv_priv *bat_priv,
425 bool ogm_source,
426 struct batadv_orig_node *orig_node,
427 u8 *src, u8 *dst,
428 void *tvlv_value, u16 tvlv_value_len)
429{
430 struct batadv_tvlv_handler *tvlv_handler;
431 struct batadv_tvlv_hdr *tvlv_hdr;
432 u16 tvlv_value_cont_len;
433 u8 cifnotfound = BATADV_TVLV_HANDLER_OGM_CIFNOTFND;
434 int ret = NET_RX_SUCCESS;
435
436 while (tvlv_value_len >= sizeof(*tvlv_hdr)) {
437 tvlv_hdr = tvlv_value;
438 tvlv_value_cont_len = ntohs(tvlv_hdr->len);
439 tvlv_value = tvlv_hdr + 1;
440 tvlv_value_len -= sizeof(*tvlv_hdr);
441
442 if (tvlv_value_cont_len > tvlv_value_len)
443 break;
444
445 tvlv_handler = batadv_tvlv_handler_get(bat_priv,
446 tvlv_hdr->type,
447 tvlv_hdr->version);
448
449 ret |= batadv_tvlv_call_handler(bat_priv, tvlv_handler,
450 ogm_source, orig_node,
451 src, dst, tvlv_value,
452 tvlv_value_cont_len);
453 if (tvlv_handler)
454 batadv_tvlv_handler_put(tvlv_handler);
455 tvlv_value = (u8 *)tvlv_value + tvlv_value_cont_len;
456 tvlv_value_len -= tvlv_value_cont_len;
457 }
458
459 if (!ogm_source)
460 return ret;
461
462 rcu_read_lock();
463 hlist_for_each_entry_rcu(tvlv_handler,
464 &bat_priv->tvlv.handler_list, list) {
465 if ((tvlv_handler->flags & BATADV_TVLV_HANDLER_OGM_CIFNOTFND) &&
466 !(tvlv_handler->flags & BATADV_TVLV_HANDLER_OGM_CALLED))
467 tvlv_handler->ogm_handler(bat_priv, orig_node,
468 cifnotfound, NULL, 0);
469
470 tvlv_handler->flags &= ~BATADV_TVLV_HANDLER_OGM_CALLED;
471 }
472 rcu_read_unlock();
473
474 return NET_RX_SUCCESS;
475}
476
477
478
479
480
481
482
483
484void batadv_tvlv_ogm_receive(struct batadv_priv *bat_priv,
485 struct batadv_ogm_packet *batadv_ogm_packet,
486 struct batadv_orig_node *orig_node)
487{
488 void *tvlv_value;
489 u16 tvlv_value_len;
490
491 if (!batadv_ogm_packet)
492 return;
493
494 tvlv_value_len = ntohs(batadv_ogm_packet->tvlv_len);
495 if (!tvlv_value_len)
496 return;
497
498 tvlv_value = batadv_ogm_packet + 1;
499
500 batadv_tvlv_containers_process(bat_priv, true, orig_node, NULL, NULL,
501 tvlv_value, tvlv_value_len);
502}
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518void batadv_tvlv_handler_register(struct batadv_priv *bat_priv,
519 void (*optr)(struct batadv_priv *bat_priv,
520 struct batadv_orig_node *orig,
521 u8 flags,
522 void *tvlv_value,
523 u16 tvlv_value_len),
524 int (*uptr)(struct batadv_priv *bat_priv,
525 u8 *src, u8 *dst,
526 void *tvlv_value,
527 u16 tvlv_value_len),
528 u8 type, u8 version, u8 flags)
529{
530 struct batadv_tvlv_handler *tvlv_handler;
531
532 tvlv_handler = batadv_tvlv_handler_get(bat_priv, type, version);
533 if (tvlv_handler) {
534 batadv_tvlv_handler_put(tvlv_handler);
535 return;
536 }
537
538 tvlv_handler = kzalloc(sizeof(*tvlv_handler), GFP_ATOMIC);
539 if (!tvlv_handler)
540 return;
541
542 tvlv_handler->ogm_handler = optr;
543 tvlv_handler->unicast_handler = uptr;
544 tvlv_handler->type = type;
545 tvlv_handler->version = version;
546 tvlv_handler->flags = flags;
547 kref_init(&tvlv_handler->refcount);
548 INIT_HLIST_NODE(&tvlv_handler->list);
549
550 spin_lock_bh(&bat_priv->tvlv.handler_list_lock);
551 kref_get(&tvlv_handler->refcount);
552 hlist_add_head_rcu(&tvlv_handler->list, &bat_priv->tvlv.handler_list);
553 spin_unlock_bh(&bat_priv->tvlv.handler_list_lock);
554
555
556 batadv_tvlv_handler_put(tvlv_handler);
557}
558
559
560
561
562
563
564
565
566void batadv_tvlv_handler_unregister(struct batadv_priv *bat_priv,
567 u8 type, u8 version)
568{
569 struct batadv_tvlv_handler *tvlv_handler;
570
571 tvlv_handler = batadv_tvlv_handler_get(bat_priv, type, version);
572 if (!tvlv_handler)
573 return;
574
575 batadv_tvlv_handler_put(tvlv_handler);
576 spin_lock_bh(&bat_priv->tvlv.handler_list_lock);
577 hlist_del_rcu(&tvlv_handler->list);
578 spin_unlock_bh(&bat_priv->tvlv.handler_list_lock);
579 batadv_tvlv_handler_put(tvlv_handler);
580}
581
582
583
584
585
586
587
588
589
590
591
592
593void batadv_tvlv_unicast_send(struct batadv_priv *bat_priv, u8 *src,
594 u8 *dst, u8 type, u8 version,
595 void *tvlv_value, u16 tvlv_value_len)
596{
597 struct batadv_unicast_tvlv_packet *unicast_tvlv_packet;
598 struct batadv_tvlv_hdr *tvlv_hdr;
599 struct batadv_orig_node *orig_node;
600 struct sk_buff *skb;
601 unsigned char *tvlv_buff;
602 unsigned int tvlv_len;
603 ssize_t hdr_len = sizeof(*unicast_tvlv_packet);
604
605 orig_node = batadv_orig_hash_find(bat_priv, dst);
606 if (!orig_node)
607 return;
608
609 tvlv_len = sizeof(*tvlv_hdr) + tvlv_value_len;
610
611 skb = netdev_alloc_skb_ip_align(NULL, ETH_HLEN + hdr_len + tvlv_len);
612 if (!skb)
613 goto out;
614
615 skb->priority = TC_PRIO_CONTROL;
616 skb_reserve(skb, ETH_HLEN);
617 tvlv_buff = skb_put(skb, sizeof(*unicast_tvlv_packet) + tvlv_len);
618 unicast_tvlv_packet = (struct batadv_unicast_tvlv_packet *)tvlv_buff;
619 unicast_tvlv_packet->packet_type = BATADV_UNICAST_TVLV;
620 unicast_tvlv_packet->version = BATADV_COMPAT_VERSION;
621 unicast_tvlv_packet->ttl = BATADV_TTL;
622 unicast_tvlv_packet->reserved = 0;
623 unicast_tvlv_packet->tvlv_len = htons(tvlv_len);
624 unicast_tvlv_packet->align = 0;
625 ether_addr_copy(unicast_tvlv_packet->src, src);
626 ether_addr_copy(unicast_tvlv_packet->dst, dst);
627
628 tvlv_buff = (unsigned char *)(unicast_tvlv_packet + 1);
629 tvlv_hdr = (struct batadv_tvlv_hdr *)tvlv_buff;
630 tvlv_hdr->version = version;
631 tvlv_hdr->type = type;
632 tvlv_hdr->len = htons(tvlv_value_len);
633 tvlv_buff += sizeof(*tvlv_hdr);
634 memcpy(tvlv_buff, tvlv_value, tvlv_value_len);
635
636 batadv_send_skb_to_orig(skb, orig_node, NULL);
637out:
638 batadv_orig_node_put(orig_node);
639}
640