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