batman-adv/multicast_forw.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (C) B.A.T.M.A.N. contributors:
   3  *
   4  * Linus Lüssing
   5  */
   6
   7 #include "multicast.h"
   8 #include "main.h"
   9
  10 #include <linux/bug.h>
  11 #include <linux/build_bug.h>
  12 #include <linux/byteorder/generic.h>
  13 #include <linux/compiler.h>
  14 #include <linux/errno.h>
  15 #include <linux/etherdevice.h>
  16 #include <linux/gfp.h>
  17 #include <linux/if_ether.h>
  18 #include <linux/if_vlan.h>
  19 #include <linux/ipv6.h>
  20 #include <linux/limits.h>
  21 #include <linux/netdevice.h>
  22 #include <linux/rculist.h>
  23 #include <linux/rcupdate.h>
  24 #include <linux/skbuff.h>
  25 #include <linux/stddef.h>
  26 #include <linux/string.h>
  27 #include <linux/types.h>
  28 #include <uapi/linux/batadv_packet.h>
  29
  30 #include "bridge_loop_avoidance.h"
  31 #include "originator.h"
  32 #include "send.h"
  33 #include "translation-table.h"
  34
  35 #define batadv_mcast_forw_tracker_for_each_dest(dest, num_dests) \
  36         for (; num_dests; num_dests--, (dest) += ETH_ALEN)
  37
  38 #define batadv_mcast_forw_tracker_for_each_dest2(dest1, dest2, num_dests) \
  39         for (; num_dests; num_dests--, (dest1) += ETH_ALEN, (dest2) += ETH_ALEN)
  40
  41 /**
  42  * batadv_mcast_forw_skb_push() - skb_push and memorize amount of pushed bytes
  43  * @skb: the skb to push onto
  44  * @size: the amount of bytes to push
  45  * @len: stores the total amount of bytes pushed
  46  *
  47  * Performs an skb_push() onto the given skb and adds the amount of pushed bytes
  48  * to the given len pointer.
  49  *
  50  * Return: the return value of the skb_push() call.
  51  */
  52 static void *batadv_mcast_forw_skb_push(struct sk_buff *skb, size_t size,
  53                                         unsigned short *len)
  54 {
  55         *len += size;
  56         return skb_push(skb, size);
  57 }
  58
  59 /**
  60  * batadv_mcast_forw_push_padding() - push 2 padding bytes to skb's front
  61  * @skb: the skb to push onto
  62  * @tvlv_len: stores the amount of currently pushed TVLV bytes
  63  *
  64  * Pushes two padding bytes to the front of the given skb.
  65  *
  66  * Return: On success a pointer to the first byte of the two pushed padding
  67  * bytes within the skb. NULL otherwise.
  68  */
  69 static char *
  70 batadv_mcast_forw_push_padding(struct sk_buff *skb, unsigned short *tvlv_len)
  71 {
  72         const int pad_len = 2;
  73         char *padding;
  74
  75         if (skb_headroom(skb) < pad_len)
  76                 return NULL;
  77
  78         padding = batadv_mcast_forw_skb_push(skb, pad_len, tvlv_len);
  79         memset(padding, 0, pad_len);
  80
  81         return padding;
  82 }
  83
  84 /**
  85  * batadv_mcast_forw_push_est_padding() - push padding bytes if necessary
  86  * @skb: the skb to potentially push the padding onto
  87  * @count: the (estimated) number of originators the multicast packet needs to
  88  *  be sent to
  89  * @tvlv_len: stores the amount of currently pushed TVLV bytes
  90  *
  91  * If the number of destination entries is even then this adds two
  92  * padding bytes to the end of the tracker TVLV.
  93  *
  94  * Return: true on success or if no padding is needed, false otherwise.
  95  */
  96 static bool
  97 batadv_mcast_forw_push_est_padding(struct sk_buff *skb, int count,
  98                                    unsigned short *tvlv_len)
  99 {
 100         if (!(count % 2) && !batadv_mcast_forw_push_padding(skb, tvlv_len))
 101                 return false;
 102
 103         return true;
 104 }
 105
 106 /**
 107  * batadv_mcast_forw_orig_entry() - get orig_node from an hlist node
 108  * @node: the hlist node to get the orig_node from
 109  * @entry_offset: the offset of the hlist node within the orig_node struct
 110  *
 111  * Return: The orig_node containing the hlist node on success, NULL on error.
 112  */
 113 static struct batadv_orig_node *
 114 batadv_mcast_forw_orig_entry(struct hlist_node *node,
 115                              size_t entry_offset)
 116 {
 117         /* sanity check */
 118         switch (entry_offset) {
 119         case offsetof(struct batadv_orig_node, mcast_want_all_ipv4_node):
 120         case offsetof(struct batadv_orig_node, mcast_want_all_ipv6_node):
 121         case offsetof(struct batadv_orig_node, mcast_want_all_rtr4_node):
 122         case offsetof(struct batadv_orig_node, mcast_want_all_rtr6_node):
 123                 break;
 124         default:
 125                 WARN_ON(1);
 126                 return NULL;
 127         }
 128
 129         return (struct batadv_orig_node *)((void *)node - entry_offset);
 130 }
 131
 132 /**
 133  * batadv_mcast_forw_push_dest() - push an originator MAC address onto an skb
 134  * @bat_priv: the bat priv with all the soft interface information
 135  * @skb: the skb to push the destination address onto
 136  * @vid: the vlan identifier
 137  * @orig_node: the originator node to get the MAC address from
 138  * @num_dests: a pointer to store the number of pushed addresses in
 139  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 140  *
 141  * If the orig_node is a BLA backbone gateway, if there is not enough skb
 142  * headroom available or if num_dests is already at its maximum (65535) then
 143  * neither the skb nor num_dests is changed. Otherwise the originator's MAC
 144  * address is pushed onto the given skb and num_dests incremented by one.
 145  *
 146  * Return: true if the orig_node is a backbone gateway or if an orig address
 147  *  was pushed successfully, false otherwise.
 148  */
 149 static bool batadv_mcast_forw_push_dest(struct batadv_priv *bat_priv,
 150                                         struct sk_buff *skb, unsigned short vid,
 151                                         struct batadv_orig_node *orig_node,
 152                                         unsigned short *num_dests,
 153                                         unsigned short *tvlv_len)
 154 {
 155         BUILD_BUG_ON(sizeof_field(struct batadv_tvlv_mcast_tracker, num_dests)
 156                      != sizeof(__be16));
 157
 158         /* Avoid sending to other BLA gateways - they already got the frame from
 159          * the LAN side we share with them.
 160          * TODO: Refactor to take BLA into account earlier in mode check.
 161          */
 162         if (batadv_bla_is_backbone_gw_orig(bat_priv, orig_node->orig, vid))
 163                 return true;
 164
 165         if (skb_headroom(skb) < ETH_ALEN || *num_dests == U16_MAX)
 166                 return false;
 167
 168         batadv_mcast_forw_skb_push(skb, ETH_ALEN, tvlv_len);
 169         ether_addr_copy(skb->data, orig_node->orig);
 170         (*num_dests)++;
 171
 172         return true;
 173 }
 174
 175 /**
 176  * batadv_mcast_forw_push_dests_list() - push originators from list onto an skb
 177  * @bat_priv: the bat priv with all the soft interface information
 178  * @skb: the skb to push the destination addresses onto
 179  * @vid: the vlan identifier
 180  * @head: the list to gather originators from
 181  * @entry_offset: offset of an hlist node in an orig_node structure
 182  * @num_dests: a pointer to store the number of pushed addresses in
 183  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 184  *
 185  * Push the MAC addresses of all originators in the given list onto the given
 186  * skb.
 187  *
 188  * Return: true on success, false otherwise.
 189  */
 190 static int batadv_mcast_forw_push_dests_list(struct batadv_priv *bat_priv,
 191                                              struct sk_buff *skb,
 192                                              unsigned short vid,
 193                                              struct hlist_head *head,
 194                                              size_t entry_offset,
 195                                              unsigned short *num_dests,
 196                                              unsigned short *tvlv_len)
 197 {
 198         struct hlist_node *node;
 199         struct batadv_orig_node *orig_node;
 200
 201         rcu_read_lock();
 202         __hlist_for_each_rcu(node, head) {
 203                 orig_node = batadv_mcast_forw_orig_entry(node, entry_offset);
 204                 if (!orig_node ||
 205                     !batadv_mcast_forw_push_dest(bat_priv, skb, vid, orig_node,
 206                                                  num_dests, tvlv_len)) {
 207                         rcu_read_unlock();
 208                         return false;
 209                 }
 210         }
 211         rcu_read_unlock();
 212
 213         return true;
 214 }
 215
 216 /**
 217  * batadv_mcast_forw_push_tt() - push originators with interest through TT
 218  * @bat_priv: the bat priv with all the soft interface information
 219  * @skb: the skb to push the destination addresses onto
 220  * @vid: the vlan identifier
 221  * @num_dests: a pointer to store the number of pushed addresses in
 222  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 223  *
 224  * Push the MAC addresses of all originators which have indicated interest in
 225  * this multicast packet through the translation table onto the given skb.
 226  *
 227  * Return: true on success, false otherwise.
 228  */
 229 static bool
 230 batadv_mcast_forw_push_tt(struct batadv_priv *bat_priv, struct sk_buff *skb,
 231                           unsigned short vid, unsigned short *num_dests,
 232                           unsigned short *tvlv_len)
 233 {
 234         struct batadv_tt_orig_list_entry *orig_entry;
 235
 236         struct batadv_tt_global_entry *tt_global;
 237         const u8 *addr = eth_hdr(skb)->h_dest;
 238
 239         /* ok */
 240         int ret = true;
 241
 242         tt_global = batadv_tt_global_hash_find(bat_priv, addr, vid);
 243         if (!tt_global)
 244                 goto out;
 245
 246         rcu_read_lock();
 247         hlist_for_each_entry_rcu(orig_entry, &tt_global->orig_list, list) {
 248                 if (!batadv_mcast_forw_push_dest(bat_priv, skb, vid,
 249                                                  orig_entry->orig_node,
 250                                                  num_dests, tvlv_len)) {
 251                         ret = false;
 252                         break;
 253                 }
 254         }
 255         rcu_read_unlock();
 256
 257         batadv_tt_global_entry_put(tt_global);
 258
 259 out:
 260         return ret;
 261 }
 262
 263 /**
 264  * batadv_mcast_forw_push_want_all() - push originators with want-all flag
 265  * @bat_priv: the bat priv with all the soft interface information
 266  * @skb: the skb to push the destination addresses onto
 267  * @vid: the vlan identifier
 268  * @num_dests: a pointer to store the number of pushed addresses in
 269  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 270  *
 271  * Push the MAC addresses of all originators which have indicated interest in
 272  * this multicast packet through the want-all flag onto the given skb.
 273  *
 274  * Return: true on success, false otherwise.
 275  */
 276 static bool batadv_mcast_forw_push_want_all(struct batadv_priv *bat_priv,
 277                                             struct sk_buff *skb,
 278                                             unsigned short vid,
 279                                             unsigned short *num_dests,
 280                                             unsigned short *tvlv_len)
 281 {
 282         struct hlist_head *head = NULL;
 283         size_t offset;
 284         int ret;
 285
 286         switch (eth_hdr(skb)->h_proto) {
 287         case htons(ETH_P_IP):
 288                 head = &bat_priv->mcast.want_all_ipv4_list;
 289                 offset = offsetof(struct batadv_orig_node,
 290                                   mcast_want_all_ipv4_node);
 291                 break;
 292         case htons(ETH_P_IPV6):
 293                 head = &bat_priv->mcast.want_all_ipv6_list;
 294                 offset = offsetof(struct batadv_orig_node,
 295                                   mcast_want_all_ipv6_node);
 296                 break;
 297         default:
 298                 return false;
 299         }
 300
 301         ret = batadv_mcast_forw_push_dests_list(bat_priv, skb, vid, head,
 302                                                 offset, num_dests, tvlv_len);
 303         if (!ret)
 304                 return false;
 305
 306         return true;
 307 }
 308
 309 /**
 310  * batadv_mcast_forw_push_want_rtr() - push originators with want-router flag
 311  * @bat_priv: the bat priv with all the soft interface information
 312  * @skb: the skb to push the destination addresses onto
 313  * @vid: the vlan identifier
 314  * @num_dests: a pointer to store the number of pushed addresses in
 315  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 316  *
 317  * Push the MAC addresses of all originators which have indicated interest in
 318  * this multicast packet through the want-all-rtr flag onto the given skb.
 319  *
 320  * Return: true on success, false otherwise.
 321  */
 322 static bool batadv_mcast_forw_push_want_rtr(struct batadv_priv *bat_priv,
 323                                             struct sk_buff *skb,
 324                                             unsigned short vid,
 325                                             unsigned short *num_dests,
 326                                             unsigned short *tvlv_len)
 327 {
 328         struct hlist_head *head = NULL;
 329         size_t offset;
 330         int ret;
 331
 332         switch (eth_hdr(skb)->h_proto) {
 333         case htons(ETH_P_IP):
 334                 head = &bat_priv->mcast.want_all_rtr4_list;
 335                 offset = offsetof(struct batadv_orig_node,
 336                                   mcast_want_all_rtr4_node);
 337                 break;
 338         case htons(ETH_P_IPV6):
 339                 head = &bat_priv->mcast.want_all_rtr6_list;
 340                 offset = offsetof(struct batadv_orig_node,
 341                                   mcast_want_all_rtr6_node);
 342                 break;
 343         default:
 344                 return false;
 345         }
 346
 347         ret = batadv_mcast_forw_push_dests_list(bat_priv, skb, vid, head,
 348                                                 offset, num_dests, tvlv_len);
 349         if (!ret)
 350                 return false;
 351
 352         return true;
 353 }
 354
 355 /**
 356  * batadv_mcast_forw_scrape() - remove bytes within skb data
 357  * @skb: the skb to remove bytes from
 358  * @offset: the offset from the skb data from which to scrape
 359  * @len: the amount of bytes to scrape starting from the offset
 360  *
 361  * Scrapes/removes len bytes from the given skb at the given offset from the
 362  * skb data.
 363  *
 364  * Caller needs to ensure that the region from the skb data's start up
 365  * to/including the to be removed bytes are linearized.
 366  */
 367 static void batadv_mcast_forw_scrape(struct sk_buff *skb,
 368                                      unsigned short offset,
 369                                      unsigned short len)
 370 {
 371         char *to, *from;
 372
 373         SKB_LINEAR_ASSERT(skb);
 374
 375         to = skb_pull(skb, len);
 376         from = to - len;
 377
 378         memmove(to, from, offset);
 379 }
 380
 381 /**
 382  * batadv_mcast_forw_push_scrape_padding() - remove TVLV padding
 383  * @skb: the skb to potentially adjust the TVLV's padding on
 384  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 385  *
 386  * Remove two padding bytes from the end of the multicast tracker TVLV,
 387  * from before the payload data.
 388  *
 389  * Caller needs to ensure that the TVLV bytes are linearized.
 390  */
 391 static void batadv_mcast_forw_push_scrape_padding(struct sk_buff *skb,
 392                                                   unsigned short *tvlv_len)
 393 {
 394         const int pad_len = 2;
 395
 396         batadv_mcast_forw_scrape(skb, *tvlv_len - pad_len, pad_len);
 397         *tvlv_len -= pad_len;
 398 }
 399
 400 /**
 401  * batadv_mcast_forw_push_insert_padding() - insert TVLV padding
 402  * @skb: the skb to potentially adjust the TVLV's padding on
 403  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 404  *
 405  * Inserts two padding bytes at the end of the multicast tracker TVLV,
 406  * before the payload data in the given skb.
 407  *
 408  * Return: true on success, false otherwise.
 409  */
 410 static bool batadv_mcast_forw_push_insert_padding(struct sk_buff *skb,
 411                                                   unsigned short *tvlv_len)
 412 {
 413         unsigned short offset = *tvlv_len;
 414         char *to, *from = skb->data;
 415
 416         to = batadv_mcast_forw_push_padding(skb, tvlv_len);
 417         if (!to)
 418                 return false;
 419
 420         memmove(to, from, offset);
 421         memset(to + offset, 0, *tvlv_len - offset);
 422         return true;
 423 }
 424
 425 /**
 426  * batadv_mcast_forw_push_adjust_padding() - adjust padding if necessary
 427  * @skb: the skb to potentially adjust the TVLV's padding on
 428  * @count: the estimated number of originators the multicast packet needs to
 429  *  be sent to
 430  * @num_dests_pushed: the number of originators that were actually added to the
 431  *  multicast packet's tracker TVLV
 432  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 433  *
 434  * Adjusts the padding in the multicast packet's tracker TVLV depending on the
 435  * initially estimated amount of destinations versus the amount of destinations
 436  * that were actually added to the tracker TVLV.
 437  *
 438  * If the initial estimate was correct or at least the oddness was the same then
 439  * no padding adjustment is performed.
 440  * If the initially estimated number was even, so padding was initially added,
 441  * but it turned out to be odd then padding is removed.
 442  * If the initially estimated number was odd, so no padding was initially added,
 443  * but it turned out to be even then padding is added.
 444  *
 445  * Return: true if no padding adjustment is needed or the adjustment was
 446  * successful, false otherwise.
 447  */
 448 static bool
 449 batadv_mcast_forw_push_adjust_padding(struct sk_buff *skb, int *count,
 450                                       unsigned short num_dests_pushed,
 451                                       unsigned short *tvlv_len)
 452 {
 453         int ret = true;
 454
 455         if (likely((num_dests_pushed % 2) == (*count % 2)))
 456                 goto out;
 457
 458         /**
 459          * estimated even number of destinations, but turned out to be odd
 460          * -> remove padding
 461          */
 462         if (!(*count % 2) && (num_dests_pushed % 2))
 463                 batadv_mcast_forw_push_scrape_padding(skb, tvlv_len);
 464         /**
 465          * estimated odd number of destinations, but turned out to be even
 466          * -> add padding
 467          */
 468         else if ((*count % 2) && (!(num_dests_pushed % 2)))
 469                 ret = batadv_mcast_forw_push_insert_padding(skb, tvlv_len);
 470
 471 out:
 472         *count = num_dests_pushed;
 473         return ret;
 474 }
 475
 476 /**
 477  * batadv_mcast_forw_push_dests() - push originator addresses onto an skb
 478  * @bat_priv: the bat priv with all the soft interface information
 479  * @skb: the skb to push the destination addresses onto
 480  * @vid: the vlan identifier
 481  * @is_routable: indicates whether the destination is routable
 482  * @count: the number of originators the multicast packet needs to be sent to
 483  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 484  *
 485  * Push the MAC addresses of all originators which have indicated interest in
 486  * this multicast packet onto the given skb.
 487  *
 488  * Return: -ENOMEM if there is not enough skb headroom available. Otherwise, on
 489  * success 0.
 490  */
 491 static int
 492 batadv_mcast_forw_push_dests(struct batadv_priv *bat_priv, struct sk_buff *skb,
 493                              unsigned short vid, int is_routable, int *count,
 494                              unsigned short *tvlv_len)
 495 {
 496         unsigned short num_dests = 0;
 497
 498         if (!batadv_mcast_forw_push_est_padding(skb, *count, tvlv_len))
 499                 goto err;
 500
 501         if (!batadv_mcast_forw_push_tt(bat_priv, skb, vid, &num_dests,
 502                                        tvlv_len))
 503                 goto err;
 504
 505         if (!batadv_mcast_forw_push_want_all(bat_priv, skb, vid, &num_dests,
 506                                              tvlv_len))
 507                 goto err;
 508
 509         if (is_routable &&
 510             !batadv_mcast_forw_push_want_rtr(bat_priv, skb, vid, &num_dests,
 511                                              tvlv_len))
 512                 goto err;
 513
 514         if (!batadv_mcast_forw_push_adjust_padding(skb, count, num_dests,
 515                                                    tvlv_len))
 516                 goto err;
 517
 518         return 0;
 519 err:
 520         return -ENOMEM;
 521 }
 522
 523 /**
 524  * batadv_mcast_forw_push_tracker() - push a multicast tracker TVLV header
 525  * @skb: the skb to push the tracker TVLV onto
 526  * @num_dests: the number of destination addresses to set in the header
 527  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 528  *
 529  * Pushes a multicast tracker TVLV header onto the given skb, including the
 530  * generic TVLV header but excluding the destination MAC addresses.
 531  *
 532  * The provided num_dests value is taken into consideration to set the
 533  * num_dests field in the tracker header and to set the appropriate TVLV length
 534  * value fields.
 535  *
 536  * Return: -ENOMEM if there is not enough skb headroom available. Otherwise, on
 537  * success 0.
 538  */
 539 static int batadv_mcast_forw_push_tracker(struct sk_buff *skb, int num_dests,
 540                                           unsigned short *tvlv_len)
 541 {
 542         struct batadv_tvlv_mcast_tracker *mcast_tracker;
 543         struct batadv_tvlv_hdr *tvlv_hdr;
 544         unsigned int tvlv_value_len;
 545
 546         if (skb_headroom(skb) < sizeof(*mcast_tracker) + sizeof(*tvlv_hdr))
 547                 return -ENOMEM;
 548
 549         tvlv_value_len = sizeof(*mcast_tracker) + *tvlv_len;
 550         if (tvlv_value_len + sizeof(*tvlv_hdr) > U16_MAX)
 551                 return -ENOMEM;
 552
 553         batadv_mcast_forw_skb_push(skb, sizeof(*mcast_tracker), tvlv_len);
 554         mcast_tracker = (struct batadv_tvlv_mcast_tracker *)skb->data;
 555         mcast_tracker->num_dests = htons(num_dests);
 556
 557         skb_reset_network_header(skb);
 558
 559         batadv_mcast_forw_skb_push(skb, sizeof(*tvlv_hdr), tvlv_len);
 560         tvlv_hdr = (struct batadv_tvlv_hdr *)skb->data;
 561         tvlv_hdr->type = BATADV_TVLV_MCAST_TRACKER;
 562         tvlv_hdr->version = 1;
 563         tvlv_hdr->len = htons(tvlv_value_len);
 564
 565         return 0;
 566 }
 567
 568 /**
 569  * batadv_mcast_forw_push_tvlvs() - push a multicast tracker TVLV onto an skb
 570  * @bat_priv: the bat priv with all the soft interface information
 571  * @skb: the skb to push the tracker TVLV onto
 572  * @vid: the vlan identifier
 573  * @is_routable: indicates whether the destination is routable
 574  * @count: the number of originators the multicast packet needs to be sent to
 575  * @tvlv_len: stores the amount of currently pushed TVLV bytes
 576  *
 577  * Pushes a multicast tracker TVLV onto the given skb, including the collected
 578  * destination MAC addresses and the generic TVLV header.
 579  *
 580  * Return: -ENOMEM if there is not enough skb headroom available. Otherwise, on
 581  * success 0.
 582  */
 583 static int
 584 batadv_mcast_forw_push_tvlvs(struct batadv_priv *bat_priv, struct sk_buff *skb,
 585                              unsigned short vid, int is_routable, int count,
 586                              unsigned short *tvlv_len)
 587 {
 588         int ret;
 589
 590         ret = batadv_mcast_forw_push_dests(bat_priv, skb, vid, is_routable,
 591                                            &count, tvlv_len);
 592         if (ret < 0)
 593                 return ret;
 594
 595         ret = batadv_mcast_forw_push_tracker(skb, count, tvlv_len);
 596         if (ret < 0)
 597                 return ret;
 598
 599         return 0;
 600 }
 601
 602 /**
 603  * batadv_mcast_forw_push_hdr() - push a multicast packet header onto an skb
 604  * @skb: the skb to push the header onto
 605  * @tvlv_len: the total TVLV length value to set in the header
 606  *
 607  * Pushes a batman-adv multicast packet header onto the given skb and sets
 608  * the provided total TVLV length value in it.
 609  *
 610  * Caller needs to ensure enough skb headroom is available.
 611  *
 612  * Return: -ENOMEM if there is not enough skb headroom available. Otherwise, on
 613  * success 0.
 614  */
 615 static int
 616 batadv_mcast_forw_push_hdr(struct sk_buff *skb, unsigned short tvlv_len)
 617 {
 618         struct batadv_mcast_packet *mcast_packet;
 619
 620         if (skb_headroom(skb) < sizeof(*mcast_packet))
 621                 return -ENOMEM;
 622
 623         skb_push(skb, sizeof(*mcast_packet));
 624
 625         mcast_packet = (struct batadv_mcast_packet *)skb->data;
 626         mcast_packet->version = BATADV_COMPAT_VERSION;
 627         mcast_packet->ttl = BATADV_TTL;
 628         mcast_packet->packet_type = BATADV_MCAST;
 629         mcast_packet->reserved = 0;
 630         mcast_packet->tvlv_len = htons(tvlv_len);
 631
 632         return 0;
 633 }
 634
 635 /**
 636  * batadv_mcast_forw_scrub_dests() - scrub destinations in a tracker TVLV
 637  * @bat_priv: the bat priv with all the soft interface information
 638  * @comp_neigh: next hop neighbor to scrub+collect destinations for
 639  * @dest: start MAC entry in original skb's tracker TVLV
 640  * @next_dest: start MAC entry in to be sent skb's tracker TVLV
 641  * @num_dests: number of remaining destination MAC entries to iterate over
 642  *
 643  * This sorts destination entries into either the original batman-adv
 644  * multicast packet or the skb (copy) that is going to be sent to comp_neigh
 645  * next.
 646  *
 647  * In preparation for the next, to be (unicast) transmitted batman-adv multicast
 648  * packet skb to be sent to the given neighbor node, tries to collect all
 649  * originator MAC addresses that have the given neighbor node as their next hop
 650  * in the to be transmitted skb (copy), which next_dest points into. That is we
 651  * zero all destination entries in next_dest which do not have comp_neigh as
 652  * their next hop. And zero all destination entries in the original skb that
 653  * would have comp_neigh as their next hop (to avoid redundant transmissions and
 654  * duplicated payload later).
 655  */
 656 static void
 657 batadv_mcast_forw_scrub_dests(struct batadv_priv *bat_priv,
 658                               struct batadv_neigh_node *comp_neigh, u8 *dest,
 659                               u8 *next_dest, u16 num_dests)
 660 {
 661         struct batadv_neigh_node *next_neigh;
 662
 663         /* skip first entry, this is what we are comparing with */
 664         eth_zero_addr(dest);
 665         dest += ETH_ALEN;
 666         next_dest += ETH_ALEN;
 667         num_dests--;
 668
 669         batadv_mcast_forw_tracker_for_each_dest2(dest, next_dest, num_dests) {
 670                 if (is_zero_ether_addr(next_dest))
 671                         continue;
 672
 673                 /* sanity check, we expect unicast destinations */
 674                 if (is_multicast_ether_addr(next_dest)) {
 675                         eth_zero_addr(dest);
 676                         eth_zero_addr(next_dest);
 677                         continue;
 678                 }
 679
 680                 next_neigh = batadv_orig_to_router(bat_priv, next_dest, NULL);
 681                 if (!next_neigh) {
 682                         eth_zero_addr(next_dest);
 683                         continue;
 684                 }
 685
 686                 if (!batadv_compare_eth(next_neigh->addr, comp_neigh->addr)) {
 687                         eth_zero_addr(next_dest);
 688                         batadv_neigh_node_put(next_neigh);
 689                         continue;
 690                 }
 691
 692                 /* found an entry for our next packet to transmit, so remove it
 693                  * from the original packet
 694                  */
 695                 eth_zero_addr(dest);
 696                 batadv_neigh_node_put(next_neigh);
 697         }
 698 }
 699
 700 /**
 701  * batadv_mcast_forw_shrink_fill() - swap slot with next non-zero destination
 702  * @slot: the to be filled zero-MAC destination entry in a tracker TVLV
 703  * @num_dests_slot: remaining entries in tracker TVLV from/including slot
 704  *
 705  * Searches for the next non-zero-MAC destination entry in a tracker TVLV after
 706  * the given slot pointer. And if found, swaps it with the zero-MAC destination
 707  * entry which the slot points to.
 708  *
 709  * Return: true if slot was swapped/filled successfully, false otherwise.
 710  */
 711 static bool batadv_mcast_forw_shrink_fill(u8 *slot, u16 num_dests_slot)
 712 {
 713         u16 num_dests_filler;
 714         u8 *filler;
 715
 716         /* sanity check, should not happen */
 717         if (!num_dests_slot)
 718                 return false;
 719
 720         num_dests_filler = num_dests_slot - 1;
 721         filler = slot + ETH_ALEN;
 722
 723         /* find a candidate to fill the empty slot */
 724         batadv_mcast_forw_tracker_for_each_dest(filler, num_dests_filler) {
 725                 if (is_zero_ether_addr(filler))
 726                         continue;
 727
 728                 ether_addr_copy(slot, filler);
 729                 eth_zero_addr(filler);
 730                 return true;
 731         }
 732
 733         return false;
 734 }
 735
 736 /**
 737  * batadv_mcast_forw_shrink_pack_dests() - pack destinations of a tracker TVLV
 738  * @skb: the batman-adv multicast packet to compact destinations in
 739  *
 740  * Compacts the originator destination MAC addresses in the multicast tracker
 741  * TVLV of the given multicast packet. This is done by moving all non-zero
 742  * MAC addresses in direction of the skb head and all zero MAC addresses in skb
 743  * tail direction, within the multicast tracker TVLV.
 744  *
 745  * Return: The number of consecutive zero MAC address destinations which are
 746  * now at the end of the multicast tracker TVLV.
 747  */
 748 static int batadv_mcast_forw_shrink_pack_dests(struct sk_buff *skb)
 749 {
 750         struct batadv_tvlv_mcast_tracker *mcast_tracker;
 751         unsigned char *skb_net_hdr;
 752         u16 num_dests_slot;
 753         u8 *slot;
 754
 755         skb_net_hdr = skb_network_header(skb);
 756         mcast_tracker = (struct batadv_tvlv_mcast_tracker *)skb_net_hdr;
 757         num_dests_slot = ntohs(mcast_tracker->num_dests);
 758
 759         slot = (u8 *)mcast_tracker + sizeof(*mcast_tracker);
 760
 761         batadv_mcast_forw_tracker_for_each_dest(slot, num_dests_slot) {
 762                 /* find an empty slot */
 763                 if (!is_zero_ether_addr(slot))
 764                         continue;
 765
 766                 if (!batadv_mcast_forw_shrink_fill(slot, num_dests_slot))
 767                         /* could not find a filler, so we successfully packed
 768                          * and can stop - and must not reduce num_dests_slot!
 769                          */
 770                         break;
 771         }
 772
 773         /* num_dests_slot is now the amount of reduced, zeroed
 774          * destinations at the end of the tracker TVLV
 775          */
 776         return num_dests_slot;
 777 }
 778
 779 /**
 780  * batadv_mcast_forw_shrink_align_offset() - get new alignment offset
 781  * @num_dests_old: the old, to be updated amount of destination nodes
 782  * @num_dests_reduce: the number of destinations that were removed
 783  *
 784  * Calculates the amount of potential extra alignment offset that is needed to
 785  * adjust the TVLV padding after the change in destination nodes.
 786  *
 787  * Return:
 788  *      0: If no change to padding is needed.
 789  *      2: If padding needs to be removed.
 790  *      -2: If padding needs to be added.
 791  */
 792 static short
 793 batadv_mcast_forw_shrink_align_offset(unsigned int num_dests_old,
 794                                       unsigned int num_dests_reduce)
 795 {
 796         /* even amount of removed destinations -> no alignment change */
 797         if (!(num_dests_reduce % 2))
 798                 return 0;
 799
 800         /* even to odd amount of destinations -> remove padding */
 801         if (!(num_dests_old % 2))
 802                 return 2;
 803
 804         /* odd to even amount of destinations -> add padding */
 805         return -2;
 806 }
 807
 808 /**
 809  * batadv_mcast_forw_shrink_update_headers() - update shrunk mc packet headers
 810  * @skb: the batman-adv multicast packet to update headers of
 811  * @num_dests_reduce: the number of destinations that were removed
 812  *
 813  * This updates any fields of a batman-adv multicast packet that are affected
 814  * by the reduced number of destinations in the multicast tracket TVLV. In
 815  * particular this updates:
 816  *
 817  * The num_dest field of the multicast tracker TVLV.
 818  * The TVLV length field of the according generic TVLV header.
 819  * The batman-adv multicast packet's total TVLV length field.
 820  *
 821  * Return: The offset in skb's tail direction at which the new batman-adv
 822  * multicast packet header needs to start.
 823  */
 824 static unsigned int
 825 batadv_mcast_forw_shrink_update_headers(struct sk_buff *skb,
 826                                         unsigned int num_dests_reduce)
 827 {
 828         struct batadv_tvlv_mcast_tracker *mcast_tracker;
 829         struct batadv_mcast_packet *mcast_packet;
 830         struct batadv_tvlv_hdr *tvlv_hdr;
 831         unsigned char *skb_net_hdr;
 832         unsigned int offset;
 833         short align_offset;
 834         u16 num_dests;
 835
 836         skb_net_hdr = skb_network_header(skb);
 837         mcast_tracker = (struct batadv_tvlv_mcast_tracker *)skb_net_hdr;
 838         num_dests = ntohs(mcast_tracker->num_dests);
 839
 840         align_offset = batadv_mcast_forw_shrink_align_offset(num_dests,
 841                                                              num_dests_reduce);
 842         offset = ETH_ALEN * num_dests_reduce + align_offset;
 843         num_dests -= num_dests_reduce;
 844
 845         /* update tracker header */
 846         mcast_tracker->num_dests = htons(num_dests);
 847
 848         /* update tracker's tvlv header's length field */
 849         tvlv_hdr = (struct batadv_tvlv_hdr *)(skb_network_header(skb) -
 850                                               sizeof(*tvlv_hdr));
 851         tvlv_hdr->len = htons(ntohs(tvlv_hdr->len) - offset);
 852
 853         /* update multicast packet header's tvlv length field */
 854         mcast_packet = (struct batadv_mcast_packet *)skb->data;
 855         mcast_packet->tvlv_len = htons(ntohs(mcast_packet->tvlv_len) - offset);
 856
 857         return offset;
 858 }
 859
 860 /**
 861  * batadv_mcast_forw_shrink_move_headers() - move multicast headers by offset
 862  * @skb: the batman-adv multicast packet to move headers for
 863  * @offset: a non-negative offset to move headers by, towards the skb tail
 864  *
 865  * Moves the batman-adv multicast packet header, its multicast tracker TVLV and
 866  * any TVLVs in between by the given offset in direction towards the tail.
 867  */
 868 static void
 869 batadv_mcast_forw_shrink_move_headers(struct sk_buff *skb, unsigned int offset)
 870 {
 871         struct batadv_tvlv_mcast_tracker *mcast_tracker;
 872         unsigned char *skb_net_hdr;
 873         unsigned int len;
 874         u16 num_dests;
 875
 876         skb_net_hdr = skb_network_header(skb);
 877         mcast_tracker = (struct batadv_tvlv_mcast_tracker *)skb_net_hdr;
 878         num_dests = ntohs(mcast_tracker->num_dests);
 879         len = skb_network_offset(skb) + sizeof(*mcast_tracker);
 880         len += num_dests * ETH_ALEN;
 881
 882         batadv_mcast_forw_scrape(skb, len, offset);
 883 }
 884
 885 /**
 886  * batadv_mcast_forw_shrink_tracker() - remove zero addresses in a tracker tvlv
 887  * @skb: the batman-adv multicast packet to (potentially) shrink
 888  *
 889  * Removes all destinations with a zero MAC addresses (00:00:00:00:00:00) from
 890  * the given batman-adv multicast packet's tracker TVLV and updates headers
 891  * accordingly to maintain a valid batman-adv multicast packet.
 892  */
 893 static void batadv_mcast_forw_shrink_tracker(struct sk_buff *skb)
 894 {
 895         unsigned int offset;
 896         u16 dests_reduced;
 897
 898         dests_reduced = batadv_mcast_forw_shrink_pack_dests(skb);
 899         if (!dests_reduced)
 900                 return;
 901
 902         offset = batadv_mcast_forw_shrink_update_headers(skb, dests_reduced);
 903         batadv_mcast_forw_shrink_move_headers(skb, offset);
 904 }
 905
 906 /**
 907  * batadv_mcast_forw_packet() - forward a batman-adv multicast packet
 908  * @bat_priv: the bat priv with all the soft interface information
 909  * @skb: the received or locally generated batman-adv multicast packet
 910  * @local_xmit: indicates that the packet was locally generated and not received
 911  *
 912  * Parses the tracker TVLV of a batman-adv multicast packet and forwards the
 913  * packet as indicated in this TVLV.
 914  *
 915  * Caller needs to set the skb network header to the start of the multicast
 916  * tracker TVLV (excluding the generic TVLV header) and the skb transport header
 917  * to the next byte after this multicast tracker TVLV.
 918  *
 919  * Caller needs to free the skb.
 920  *
 921  * Return: NET_RX_SUCCESS or NET_RX_DROP on success or a negative error
 922  * code on failure. NET_RX_SUCCESS if the received packet is supposed to be
 923  * decapsulated and forwarded to the own soft interface, NET_RX_DROP otherwise.
 924  */
 925 static int batadv_mcast_forw_packet(struct batadv_priv *bat_priv,
 926                                     struct sk_buff *skb, bool local_xmit)
 927 {
 928         struct batadv_tvlv_mcast_tracker *mcast_tracker;
 929         struct batadv_neigh_node *neigh_node;
 930         unsigned long offset, num_dests_off;
 931         struct sk_buff *nexthop_skb;
 932         unsigned char *skb_net_hdr;
 933         bool local_recv = false;
 934         unsigned int tvlv_len;
 935         bool xmitted = false;
 936         u8 *dest, *next_dest;
 937         u16 num_dests;
 938         int ret;
 939
 940         /* (at least) TVLV part needs to be linearized */
 941         SKB_LINEAR_ASSERT(skb);
 942
 943         /* check if num_dests is within skb length */
 944         num_dests_off = offsetof(struct batadv_tvlv_mcast_tracker, num_dests);
 945         if (num_dests_off > skb_network_header_len(skb))
 946                 return -EINVAL;
 947
 948         skb_net_hdr = skb_network_header(skb);
 949         mcast_tracker = (struct batadv_tvlv_mcast_tracker *)skb_net_hdr;
 950         num_dests = ntohs(mcast_tracker->num_dests);
 951
 952         dest = (u8 *)mcast_tracker + sizeof(*mcast_tracker);
 953
 954         /* check if full tracker tvlv is within skb length */
 955         tvlv_len = sizeof(*mcast_tracker) + ETH_ALEN * num_dests;
 956         if (tvlv_len > skb_network_header_len(skb))
 957                 return -EINVAL;
 958
 959         /* invalidate checksum: */
 960         skb->ip_summed = CHECKSUM_NONE;
 961
 962         batadv_mcast_forw_tracker_for_each_dest(dest, num_dests) {
 963                 if (is_zero_ether_addr(dest))
 964                         continue;
 965
 966                 /* only unicast originator addresses supported */
 967                 if (is_multicast_ether_addr(dest)) {
 968                         eth_zero_addr(dest);
 969                         continue;
 970                 }
 971
 972                 if (batadv_is_my_mac(bat_priv, dest)) {
 973                         eth_zero_addr(dest);
 974                         local_recv = true;
 975                         continue;
 976                 }
 977
 978                 neigh_node = batadv_orig_to_router(bat_priv, dest, NULL);
 979                 if (!neigh_node) {
 980                         eth_zero_addr(dest);
 981                         continue;
 982                 }
 983
 984                 nexthop_skb = skb_copy(skb, GFP_ATOMIC);
 985                 if (!nexthop_skb) {
 986                         batadv_neigh_node_put(neigh_node);
 987                         return -ENOMEM;
 988                 }
 989
 990                 offset = dest - skb->data;
 991                 next_dest = nexthop_skb->data + offset;
 992
 993                 batadv_mcast_forw_scrub_dests(bat_priv, neigh_node, dest,
 994                                               next_dest, num_dests);
 995                 batadv_mcast_forw_shrink_tracker(nexthop_skb);
 996
 997                 batadv_inc_counter(bat_priv, BATADV_CNT_MCAST_TX);
 998                 batadv_add_counter(bat_priv, BATADV_CNT_MCAST_TX_BYTES,
 999                                    nexthop_skb->len + ETH_HLEN);
1000                 xmitted = true;
1001                 ret = batadv_send_unicast_skb(nexthop_skb, neigh_node);
1002
1003                 batadv_neigh_node_put(neigh_node);
1004
1005                 if (ret < 0)
1006                         return ret;
1007         }
1008
1009         if (xmitted) {
1010                 if (local_xmit) {
1011                         batadv_inc_counter(bat_priv, BATADV_CNT_MCAST_TX_LOCAL);
1012                         batadv_add_counter(bat_priv,
1013                                            BATADV_CNT_MCAST_TX_LOCAL_BYTES,
1014                                            skb->len -
1015                                            skb_transport_offset(skb));
1016                 } else {
1017                         batadv_inc_counter(bat_priv, BATADV_CNT_MCAST_FWD);
1018                         batadv_add_counter(bat_priv, BATADV_CNT_MCAST_FWD_BYTES,
1019                                            skb->len + ETH_HLEN);
1020                 }
1021         }
1022
1023         if (local_recv)
1024                 return NET_RX_SUCCESS;
1025         else
1026                 return NET_RX_DROP;
1027 }
1028
1029 /**
1030  * batadv_mcast_forw_tracker_tvlv_handler() - handle an mcast tracker tvlv
1031  * @bat_priv: the bat priv with all the soft interface information
1032  * @skb: the received batman-adv multicast packet
1033  *
1034  * Parses the tracker TVLV of an incoming batman-adv multicast packet and
1035  * forwards the packet as indicated in this TVLV.
1036  *
1037  * Caller needs to set the skb network header to the start of the multicast
1038  * tracker TVLV (excluding the generic TVLV header) and the skb transport header
1039  * to the next byte after this multicast tracker TVLV.
1040  *
1041  * Caller needs to free the skb.
1042  *
1043  * Return: NET_RX_SUCCESS or NET_RX_DROP on success or a negative error
1044  * code on failure. NET_RX_SUCCESS if the received packet is supposed to be
1045  * decapsulated and forwarded to the own soft interface, NET_RX_DROP otherwise.
1046  */
1047 int batadv_mcast_forw_tracker_tvlv_handler(struct batadv_priv *bat_priv,
1048                                            struct sk_buff *skb)
1049 {
1050         return batadv_mcast_forw_packet(bat_priv, skb, false);
1051 }
1052
1053 /**
1054  * batadv_mcast_forw_packet_hdrlen() - multicast packet header length
1055  * @num_dests: number of destination nodes
1056  *
1057  * Calculates the total batman-adv multicast packet header length for a given
1058  * number of destination nodes (excluding the outer ethernet frame).
1059  *
1060  * Return: The calculated total batman-adv multicast packet header length.
1061  */
1062 unsigned int batadv_mcast_forw_packet_hdrlen(unsigned int num_dests)
1063 {
1064         /**
1065          * If the number of destination entries is even then we need to add
1066          * two byte padding to the tracker TVLV.
1067          */
1068         int padding = (!(num_dests % 2)) ? 2 : 0;
1069
1070         return padding + num_dests * ETH_ALEN +
1071                sizeof(struct batadv_tvlv_mcast_tracker) +
1072                sizeof(struct batadv_tvlv_hdr) +
1073                sizeof(struct batadv_mcast_packet);
1074 }
1075
1076 /**
1077  * batadv_mcast_forw_expand_head() - expand headroom for an mcast packet
1078  * @bat_priv: the bat priv with all the soft interface information
1079  * @skb: the multicast packet to send
1080  *
1081  * Tries to expand an skb's headroom so that its head to tail is 1298
1082  * bytes (minimum IPv6 MTU + vlan ethernet header size) large.
1083  *
1084  * Return: -EINVAL if the given skb's length is too large or -ENOMEM on memory
1085  * allocation failure. Otherwise, on success, zero is returned.
1086  */
1087 static int batadv_mcast_forw_expand_head(struct batadv_priv *bat_priv,
1088                                          struct sk_buff *skb)
1089 {
1090         int hdr_size = VLAN_ETH_HLEN + IPV6_MIN_MTU - skb->len;
1091
1092          /* TODO: Could be tightened to actual number of destination nodes?
1093           * But it's tricky, number of destinations might have increased since
1094           * we last checked.
1095           */
1096         if (hdr_size < 0) {
1097                 /* batadv_mcast_forw_mode_check_count() should ensure we do not
1098                  * end up here
1099                  */
1100                 WARN_ON(1);
1101                 return -EINVAL;
1102         }
1103
1104         if (skb_headroom(skb) < hdr_size &&
1105             pskb_expand_head(skb, hdr_size, 0, GFP_ATOMIC) < 0)
1106                 return -ENOMEM;
1107
1108         return 0;
1109 }
1110
1111 /**
1112  * batadv_mcast_forw_push() - encapsulate skb in a batman-adv multicast packet
1113  * @bat_priv: the bat priv with all the soft interface information
1114  * @skb: the multicast packet to encapsulate and send
1115  * @vid: the vlan identifier
1116  * @is_routable: indicates whether the destination is routable
1117  * @count: the number of originators the multicast packet needs to be sent to
1118  *
1119  * Encapsulates the given multicast packet in a batman-adv multicast packet.
1120  * A multicast tracker TVLV with destination originator addresses for any node
1121  * that signaled interest in it, that is either via the translation table or the
1122  * according want-all flags, is attached accordingly.
1123  *
1124  * Return: true on success, false otherwise.
1125  */
1126 bool batadv_mcast_forw_push(struct batadv_priv *bat_priv, struct sk_buff *skb,
1127                             unsigned short vid, int is_routable, int count)
1128 {
1129         unsigned short tvlv_len = 0;
1130         int ret;
1131
1132         if (batadv_mcast_forw_expand_head(bat_priv, skb) < 0)
1133                 goto err;
1134
1135         skb_reset_transport_header(skb);
1136
1137         ret = batadv_mcast_forw_push_tvlvs(bat_priv, skb, vid, is_routable,
1138                                            count, &tvlv_len);
1139         if (ret < 0)
1140                 goto err;
1141
1142         ret = batadv_mcast_forw_push_hdr(skb, tvlv_len);
1143         if (ret < 0)
1144                 goto err;
1145
1146         return true;
1147
1148 err:
1149         if (tvlv_len)
1150                 skb_pull(skb, tvlv_len);
1151
1152         return false;
1153 }
1154
1155 /**
1156  * batadv_mcast_forw_mcsend() - send a self prepared batman-adv multicast packet
1157  * @bat_priv: the bat priv with all the soft interface information
1158  * @skb: the multicast packet to encapsulate and send
1159  *
1160  * Transmits a batman-adv multicast packet that was locally prepared and
1161  * consumes/frees it.
1162  *
1163  * Return: NET_XMIT_DROP on memory allocation failure. NET_XMIT_SUCCESS
1164  * otherwise.
1165  */
1166 int batadv_mcast_forw_mcsend(struct batadv_priv *bat_priv,
1167                              struct sk_buff *skb)
1168 {
1169         int ret = batadv_mcast_forw_packet(bat_priv, skb, true);
1170
1171         if (ret < 0) {
1172                 kfree_skb(skb);
1173                 return NET_XMIT_DROP;
1174         }
1175
1176         consume_skb(skb);
1177         return NET_XMIT_SUCCESS;
1178 }