pimd/pim_igmp_mtrace.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Multicast traceroute for FRRouting
   4  * Copyright (C) 2017  Mladen Sablic
   5  */
   6
   7 /* based on draft-ietf-idmr-traceroute-ipm-07 */
   8
   9 #include <zebra.h>
  10
  11 #include "pimd.h"
  12 #include "pim_instance.h"
  13 #include "pim_util.h"
  14 #include "pim_sock.h"
  15 #include "pim_rp.h"
  16 #include "pim_oil.h"
  17 #include "pim_ifchannel.h"
  18 #include "pim_macro.h"
  19 #include "pim_igmp_mtrace.h"
  20
  21 static struct in_addr mtrace_primary_address(struct interface *ifp)
  22 {
  23         struct connected *ifc;
  24         struct listnode *node;
  25         struct in_addr any;
  26         struct pim_interface *pim_ifp;
  27
  28         if (ifp->info) {
  29                 pim_ifp = ifp->info;
  30                 return pim_ifp->primary_address;
  31         }
  32
  33         any.s_addr = INADDR_ANY;
  34
  35         for (ALL_LIST_ELEMENTS_RO(ifp->connected, node, ifc)) {
  36                 struct prefix *p = ifc->address;
  37
  38                 if (p->family != AF_INET)
  39                         continue;
  40
  41                 if (!CHECK_FLAG(ifc->flags, ZEBRA_IFA_SECONDARY))
  42                         return p->u.prefix4;
  43                 /* in case no primary found, return a secondary */
  44                 any = p->u.prefix4;
  45         }
  46         return any;
  47 }
  48
  49 static bool mtrace_fwd_info_weak(struct pim_instance *pim,
  50                                     struct igmp_mtrace *mtracep,
  51                                     struct igmp_mtrace_rsp *rspp,
  52                                     struct interface **ifpp)
  53 {
  54         struct pim_nexthop nexthop;
  55         struct interface *ifp_in;
  56         struct in_addr nh_addr;
  57
  58         nh_addr.s_addr = INADDR_ANY;
  59
  60         memset(&nexthop, 0, sizeof(nexthop));
  61
  62         if (!pim_nexthop_lookup(pim, &nexthop, mtracep->src_addr, 1)) {
  63                 if (PIM_DEBUG_MTRACE)
  64                         zlog_debug("mtrace not found neighbor");
  65                 return false;
  66         }
  67
  68         if (PIM_DEBUG_MTRACE)
  69                 zlog_debug("mtrace pim_nexthop_lookup OK");
  70
  71         if (PIM_DEBUG_MTRACE)
  72                 zlog_debug("mtrace next_hop=%pPAs", &nexthop.mrib_nexthop_addr);
  73
  74         nh_addr = nexthop.mrib_nexthop_addr;
  75
  76         ifp_in = nexthop.interface;
  77
  78         /* return interface for forwarding mtrace packets */
  79         *ifpp = ifp_in;
  80
  81         /* 6.2.2. 4. Fill in the Incoming Interface Address... */
  82         rspp->incoming = mtrace_primary_address(ifp_in);
  83         rspp->prev_hop = nh_addr;
  84         rspp->in_count = htonl(MTRACE_UNKNOWN_COUNT);
  85         rspp->total = htonl(MTRACE_UNKNOWN_COUNT);
  86         rspp->rtg_proto = MTRACE_RTG_PROTO_PIM;
  87         return true;
  88 }
  89
  90 static bool mtrace_fwd_info(struct pim_instance *pim,
  91                             struct igmp_mtrace *mtracep,
  92                             struct igmp_mtrace_rsp *rspp,
  93                             struct interface **ifpp)
  94 {
  95         pim_sgaddr sg;
  96         struct pim_upstream *up;
  97         struct interface *ifp_in;
  98         struct in_addr nh_addr;
  99         uint32_t total;
 100
 101         memset(&sg, 0, sizeof(sg));
 102         sg.src = mtracep->src_addr;
 103         sg.grp = mtracep->grp_addr;
 104
 105         up = pim_upstream_find(pim, &sg);
 106
 107         if (!up) {
 108                 sg.src = PIMADDR_ANY;
 109                 up = pim_upstream_find(pim, &sg);
 110         }
 111
 112         if (!up)
 113                 return false;
 114
 115         if (!up->rpf.source_nexthop.interface) {
 116                 if (PIM_DEBUG_TRACE)
 117                         zlog_debug("%s: up %s RPF is not present", __func__,
 118                                    up->sg_str);
 119                 return false;
 120         }
 121
 122         ifp_in = up->rpf.source_nexthop.interface;
 123         nh_addr = up->rpf.source_nexthop.mrib_nexthop_addr;
 124         total = htonl(MTRACE_UNKNOWN_COUNT);
 125
 126         if (PIM_DEBUG_MTRACE)
 127                 zlog_debug("fwd_info: upstream next hop=%pI4", &nh_addr);
 128
 129         if (up->channel_oil)
 130                 total = up->channel_oil->cc.pktcnt;
 131
 132         /* return interface for forwarding mtrace packets */
 133         *ifpp = ifp_in;
 134
 135         /* 6.2.2. 4. Fill in the Incoming Interface Address... */
 136         rspp->incoming = mtrace_primary_address(ifp_in);
 137         rspp->prev_hop = nh_addr;
 138         rspp->in_count = htonl(MTRACE_UNKNOWN_COUNT);
 139         rspp->total = total;
 140         rspp->rtg_proto = MTRACE_RTG_PROTO_PIM;
 141
 142         /* 6.2.2. 4. Fill in ... S, and Src Mask */
 143         if (!pim_addr_is_any(sg.src)) {
 144                 rspp->s = 1;
 145                 rspp->src_mask = MTRACE_SRC_MASK_SOURCE;
 146         } else {
 147                 rspp->s = 0;
 148                 rspp->src_mask = MTRACE_SRC_MASK_GROUP;
 149         }
 150
 151         return true;
 152 }
 153
 154 static void mtrace_rsp_set_fwd_code(struct igmp_mtrace_rsp *mtrace_rspp,
 155                                     enum mtrace_fwd_code fwd_code)
 156 {
 157         if (mtrace_rspp->fwd_code == MTRACE_FWD_CODE_NO_ERROR)
 158                 mtrace_rspp->fwd_code = fwd_code;
 159 }
 160
 161 static void mtrace_rsp_init(struct igmp_mtrace_rsp *mtrace_rspp)
 162 {
 163         mtrace_rspp->arrival = 0;
 164         mtrace_rspp->incoming.s_addr = INADDR_ANY;
 165         mtrace_rspp->outgoing.s_addr = INADDR_ANY;
 166         mtrace_rspp->prev_hop.s_addr = INADDR_ANY;
 167         mtrace_rspp->in_count = htonl(MTRACE_UNKNOWN_COUNT);
 168         mtrace_rspp->out_count = htonl(MTRACE_UNKNOWN_COUNT);
 169         mtrace_rspp->total = htonl(MTRACE_UNKNOWN_COUNT);
 170         mtrace_rspp->rtg_proto = 0;
 171         mtrace_rspp->fwd_ttl = 0;
 172         mtrace_rspp->mbz = 0;
 173         mtrace_rspp->s = 0;
 174         mtrace_rspp->src_mask = 0;
 175         mtrace_rspp->fwd_code = MTRACE_FWD_CODE_NO_ERROR;
 176 }
 177
 178 static void mtrace_rsp_debug(uint32_t qry_id, int rsp,
 179                              struct igmp_mtrace_rsp *mrspp)
 180 {
 181         struct in_addr incoming = mrspp->incoming;
 182         struct in_addr outgoing = mrspp->outgoing;
 183         struct in_addr prev_hop = mrspp->prev_hop;
 184
 185         zlog_debug(
 186                 "Rx mt(%d) qid=%ud arr=%x in=%pI4 out=%pI4 prev=%pI4 proto=%d fwd=%d",
 187                 rsp, ntohl(qry_id), mrspp->arrival, &incoming, &outgoing,
 188                 &prev_hop, mrspp->rtg_proto, mrspp->fwd_code);
 189 }
 190
 191 static void mtrace_debug(struct pim_interface *pim_ifp,
 192                          struct igmp_mtrace *mtracep, int mtrace_len)
 193 {
 194         struct in_addr ga, sa, da, ra;
 195
 196         ga = mtracep->grp_addr;
 197         sa = mtracep->src_addr;
 198         da = mtracep->dst_addr;
 199         ra = mtracep->rsp_addr;
 200
 201         zlog_debug(
 202                 "Rx mtrace packet incoming on %pI4: hops=%d type=%d size=%d, grp=%pI4, src=%pI4, dst=%pI4 rsp=%pI4 ttl=%d qid=%ud",
 203                 &pim_ifp->primary_address, mtracep->hops, mtracep->type,
 204                 mtrace_len, &ga, &sa, &da, &ra, mtracep->rsp_ttl,
 205                 ntohl(mtracep->qry_id));
 206         if (mtrace_len > (int)sizeof(struct igmp_mtrace)) {
 207
 208                 int i;
 209
 210                 int responses = mtrace_len - sizeof(struct igmp_mtrace);
 211
 212                 if ((responses % sizeof(struct igmp_mtrace_rsp)) != 0)
 213                         if (PIM_DEBUG_MTRACE)
 214                                 zlog_debug(
 215                                         "Mtrace response block of wrong length");
 216
 217                 responses = responses / sizeof(struct igmp_mtrace_rsp);
 218
 219                 for (i = 0; i < responses; i++)
 220                         mtrace_rsp_debug(mtracep->qry_id, i, &mtracep->rsp[i]);
 221         }
 222 }
 223
 224 /* 5.1 Query Arrival Time */
 225 static uint32_t query_arrival_time(void)
 226 {
 227         struct timeval tv;
 228         uint32_t qat;
 229
 230         if (gettimeofday(&tv, NULL) < 0) {
 231                 if (PIM_DEBUG_MTRACE)
 232                         zlog_debug("Query arrival time lookup failed: errno=%d: %s",
 233                                    errno, safe_strerror(errno));
 234                 return 0;
 235         }
 236         /* not sure second offset correct, as I get different value */
 237         qat = ((tv.tv_sec + 32384) << 16) + ((tv.tv_usec << 10) / 15625);
 238
 239         return qat;
 240 }
 241
 242 static int mtrace_send_packet(struct interface *ifp,
 243                               struct igmp_mtrace *mtracep,
 244                               size_t mtrace_buf_len, struct in_addr dst_addr,
 245                               struct in_addr group_addr)
 246 {
 247         struct sockaddr_in to;
 248         socklen_t tolen;
 249         ssize_t sent;
 250         int ret;
 251         int fd;
 252         uint8_t ttl;
 253
 254         memset(&to, 0, sizeof(to));
 255         to.sin_family = AF_INET;
 256         to.sin_addr = dst_addr;
 257         tolen = sizeof(to);
 258
 259         if (PIM_DEBUG_MTRACE) {
 260                 struct in_addr if_addr;
 261                 struct in_addr rsp_addr = mtracep->rsp_addr;
 262
 263                 if_addr = mtrace_primary_address(ifp);
 264                 zlog_debug("Sending mtrace packet to %pI4 on %pI4", &rsp_addr,
 265                            &if_addr);
 266         }
 267
 268         fd = pim_socket_raw(IPPROTO_IGMP);
 269
 270         if (fd < 0)
 271                 return -1;
 272
 273         ret = pim_socket_bind(fd, ifp);
 274
 275         if (ret < 0) {
 276                 ret = -1;
 277                 goto close_fd;
 278         }
 279
 280         if (IPV4_CLASS_DE(ntohl(dst_addr.s_addr))) {
 281                 if (IPV4_MC_LINKLOCAL(ntohl(dst_addr.s_addr))) {
 282                         ttl = 1;
 283                 } else {
 284                         if (mtracep->type == PIM_IGMP_MTRACE_RESPONSE)
 285                                 ttl = mtracep->rsp_ttl;
 286                         else
 287                                 ttl = 64;
 288                 }
 289                 ret = setsockopt(fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl,
 290                                  sizeof(ttl));
 291
 292                 if (ret < 0) {
 293                         if (PIM_DEBUG_MTRACE)
 294                                 zlog_debug("Failed to set socket multicast TTL");
 295                         ret = -1;
 296                         goto close_fd;
 297                 }
 298         }
 299
 300         sent = sendto(fd, (char *)mtracep, mtrace_buf_len, MSG_DONTWAIT,
 301                       (struct sockaddr *)&to, tolen);
 302
 303         if (sent != (ssize_t)mtrace_buf_len) {
 304                 char dst_str[INET_ADDRSTRLEN];
 305                 char group_str[INET_ADDRSTRLEN];
 306
 307                 pim_inet4_dump("<dst?>", dst_addr, dst_str, sizeof(dst_str));
 308                 pim_inet4_dump("<group?>", group_addr, group_str,
 309                                sizeof(group_str));
 310                 if (sent < 0) {
 311                         if (PIM_DEBUG_MTRACE)
 312                                 zlog_debug(
 313                                         "Send mtrace request failed for %s on%s: group=%s msg_size=%zd: errno=%d:  %s",
 314                                         dst_str, ifp->name, group_str,
 315                                         mtrace_buf_len, errno,
 316                                         safe_strerror(errno));
 317                 } else {
 318                         if (PIM_DEBUG_MTRACE)
 319                                 zlog_debug(
 320                                         "Send mtrace request failed for %s on %s: group=%s msg_size=%zd: sent=%zd",
 321                                         dst_str, ifp->name, group_str,
 322                                         mtrace_buf_len, sent);
 323                 }
 324                 ret = -1;
 325                 goto close_fd;
 326         }
 327         ret = 0;
 328 close_fd:
 329         close(fd);
 330         return ret;
 331 }
 332
 333 static int mtrace_un_forward_packet(struct pim_instance *pim, struct ip *ip_hdr,
 334                                     struct interface *interface)
 335 {
 336         struct pim_nexthop nexthop;
 337         struct sockaddr_in to;
 338         struct interface *if_out;
 339         socklen_t tolen;
 340         int ret;
 341         int fd;
 342         int sent;
 343         uint16_t checksum;
 344
 345         checksum = ip_hdr->ip_sum;
 346
 347         ip_hdr->ip_sum = 0;
 348
 349         if (checksum != in_cksum(ip_hdr, ip_hdr->ip_hl * 4))
 350                 return -1;
 351
 352         if (ip_hdr->ip_ttl-- <= 1)
 353                 return -1;
 354
 355         if (interface == NULL) {
 356                 memset(&nexthop, 0, sizeof(nexthop));
 357                 if (!pim_nexthop_lookup(pim, &nexthop, ip_hdr->ip_dst, 0)) {
 358                         if (PIM_DEBUG_MTRACE)
 359                                 zlog_debug(
 360                                         "Dropping mtrace packet, no route to destination");
 361                         return -1;
 362                 }
 363
 364                 if_out = nexthop.interface;
 365         } else {
 366                 if_out = interface;
 367         }
 368
 369         ip_hdr->ip_sum = in_cksum(ip_hdr, ip_hdr->ip_hl * 4);
 370
 371         fd = pim_socket_raw(IPPROTO_RAW);
 372
 373         if (fd < 0)
 374                 return -1;
 375
 376         pim_socket_ip_hdr(fd);
 377
 378         ret = pim_socket_bind(fd, if_out);
 379
 380         if (ret < 0) {
 381                 close(fd);
 382                 return -1;
 383         }
 384
 385         memset(&to, 0, sizeof(to));
 386         to.sin_family = AF_INET;
 387         to.sin_addr = ip_hdr->ip_dst;
 388         tolen = sizeof(to);
 389
 390         sent = sendto(fd, ip_hdr, ntohs(ip_hdr->ip_len), 0,
 391                       (struct sockaddr *)&to, tolen);
 392
 393         close(fd);
 394
 395         if (sent < 0) {
 396                 if (PIM_DEBUG_MTRACE)
 397                         zlog_debug(
 398                                 "Failed to forward mtrace packet: sendto errno=%d, %s",
 399                                 errno, safe_strerror(errno));
 400                 return -1;
 401         }
 402
 403         if (PIM_DEBUG_MTRACE) {
 404                 zlog_debug("Fwd mtrace packet len=%u to %pI4 ttl=%u",
 405                            ntohs(ip_hdr->ip_len), &ip_hdr->ip_dst,
 406                            ip_hdr->ip_ttl);
 407         }
 408
 409         return 0;
 410 }
 411
 412 static int mtrace_mc_forward_packet(struct pim_instance *pim, struct ip *ip_hdr)
 413 {
 414         pim_sgaddr sg;
 415         struct channel_oil *c_oil;
 416         struct listnode *chnode;
 417         struct listnode *chnextnode;
 418         struct pim_ifchannel *ch = NULL;
 419         int ret = -1;
 420
 421         memset(&sg, 0, sizeof(sg));
 422         sg.grp = ip_hdr->ip_dst;
 423
 424         c_oil = pim_find_channel_oil(pim, &sg);
 425
 426         if (c_oil == NULL) {
 427                 if (PIM_DEBUG_MTRACE) {
 428                         zlog_debug(
 429                                 "Dropping mtrace multicast packet len=%u to %pI4 ttl=%u",
 430                                 ntohs(ip_hdr->ip_len),
 431                                 &ip_hdr->ip_dst, ip_hdr->ip_ttl);
 432                 }
 433                 return -1;
 434         }
 435         if (c_oil->up == NULL)
 436                 return -1;
 437         if (c_oil->up->ifchannels == NULL)
 438                 return -1;
 439         for (ALL_LIST_ELEMENTS(c_oil->up->ifchannels, chnode, chnextnode, ch)) {
 440                 if (pim_macro_chisin_oiflist(ch)) {
 441                         int r;
 442
 443                         r = mtrace_un_forward_packet(pim, ip_hdr,
 444                                                      ch->interface);
 445                         if (r == 0)
 446                                 ret = 0;
 447                 }
 448         }
 449         return ret;
 450 }
 451
 452
 453 static int mtrace_forward_packet(struct pim_instance *pim, struct ip *ip_hdr)
 454 {
 455         if (IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr)))
 456                 return mtrace_mc_forward_packet(pim, ip_hdr);
 457         else
 458                 return mtrace_un_forward_packet(pim, ip_hdr, NULL);
 459 }
 460
 461 static int mtrace_send_mc_response(struct pim_instance *pim,
 462                                    struct igmp_mtrace *mtracep,
 463                                    size_t mtrace_len)
 464 {
 465         pim_sgaddr sg;
 466         struct channel_oil *c_oil;
 467         struct listnode *chnode;
 468         struct listnode *chnextnode;
 469         struct pim_ifchannel *ch = NULL;
 470         int ret = -1;
 471
 472         memset(&sg, 0, sizeof(sg));
 473         sg.grp = mtracep->rsp_addr;
 474
 475         c_oil = pim_find_channel_oil(pim, &sg);
 476
 477         if (c_oil == NULL) {
 478                 if (PIM_DEBUG_MTRACE) {
 479                         struct in_addr rsp_addr = mtracep->rsp_addr;
 480
 481                         zlog_debug(
 482                                 "Dropping mtrace multicast response packet len=%u to %pI4",
 483                                 (unsigned int)mtrace_len, &rsp_addr);
 484                 }
 485                 return -1;
 486         }
 487         if (c_oil->up == NULL)
 488                 return -1;
 489         if (c_oil->up->ifchannels == NULL)
 490                 return -1;
 491         for (ALL_LIST_ELEMENTS(c_oil->up->ifchannels, chnode, chnextnode, ch)) {
 492                 if (pim_macro_chisin_oiflist(ch)) {
 493                         int r;
 494
 495                         r = mtrace_send_packet(ch->interface, mtracep,
 496                                                mtrace_len, mtracep->rsp_addr,
 497                                                mtracep->grp_addr);
 498                         if (r == 0)
 499                                 ret = 0;
 500                 }
 501         }
 502         return ret;
 503 }
 504
 505 /* 6.5 Sending Traceroute Responses */
 506 static int mtrace_send_response(struct pim_instance *pim,
 507                                 struct igmp_mtrace *mtracep, size_t mtrace_len)
 508 {
 509         struct pim_nexthop nexthop;
 510
 511         mtracep->type = PIM_IGMP_MTRACE_RESPONSE;
 512
 513         mtracep->checksum = 0;
 514         mtracep->checksum = in_cksum((char *)mtracep, mtrace_len);
 515
 516         if (IPV4_CLASS_DE(ntohl(mtracep->rsp_addr.s_addr))) {
 517                 struct pim_rpf *p_rpf;
 518
 519                 if (pim_rp_i_am_rp(pim, mtracep->rsp_addr))
 520                         return mtrace_send_mc_response(pim, mtracep,
 521                                                        mtrace_len);
 522
 523                 p_rpf = pim_rp_g(pim, mtracep->rsp_addr);
 524
 525                 if (p_rpf == NULL) {
 526                         if (PIM_DEBUG_MTRACE) {
 527                                 struct in_addr rsp_addr = mtracep->rsp_addr;
 528
 529                                 zlog_debug("mtrace no RP for %pI4", &rsp_addr);
 530                         }
 531                         return -1;
 532                 }
 533                 nexthop = p_rpf->source_nexthop;
 534                 if (PIM_DEBUG_MTRACE)
 535                         zlog_debug("mtrace response to RP");
 536         } else {
 537                 memset(&nexthop, 0, sizeof(nexthop));
 538                 /* TODO: should use unicast rib lookup */
 539                 if (!pim_nexthop_lookup(pim, &nexthop, mtracep->rsp_addr, 1)) {
 540                         if (PIM_DEBUG_MTRACE)
 541                                 zlog_debug(
 542                                         "Dropped response qid=%ud, no route to response address",
 543                                         mtracep->qry_id);
 544                         return -1;
 545                 }
 546         }
 547
 548         return mtrace_send_packet(nexthop.interface, mtracep, mtrace_len,
 549                                   mtracep->rsp_addr, mtracep->grp_addr);
 550 }
 551
 552 int igmp_mtrace_recv_qry_req(struct gm_sock *igmp, struct ip *ip_hdr,
 553                              struct in_addr from, const char *from_str,
 554                              char *igmp_msg, int igmp_msg_len)
 555 {
 556         static uint32_t qry_id, qry_src;
 557         char mtrace_buf[MTRACE_HDR_SIZE + MTRACE_MAX_HOPS * MTRACE_RSP_SIZE];
 558         struct interface *ifp;
 559         struct interface *out_ifp = NULL;
 560         struct pim_interface *pim_ifp;
 561         struct pim_instance *pim;
 562         struct igmp_mtrace *mtracep;
 563         struct igmp_mtrace_rsp *rspp;
 564         struct in_addr nh_addr;
 565         enum mtrace_fwd_code fwd_code = MTRACE_FWD_CODE_NO_ERROR;
 566         size_t r_len;
 567         int last_rsp_ind = 0;
 568         size_t mtrace_len;
 569         uint16_t recv_checksum;
 570         uint16_t checksum;
 571         bool reached_source;
 572         bool fwd_info;
 573
 574         ifp = igmp->interface;
 575         pim_ifp = ifp->info;
 576         pim = pim_ifp->pim;
 577
 578         /*
 579          * 6. Router Behaviour
 580          * Check if mtrace packet is addressed elsewhere and forward,
 581          * if applicable
 582          */
 583         if (!IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr)))
 584                 if (!if_address_is_local(&ip_hdr->ip_dst, AF_INET,
 585                                          pim->vrf->vrf_id))
 586                         return mtrace_forward_packet(pim, ip_hdr);
 587
 588         if (igmp_msg_len < (int)sizeof(struct igmp_mtrace)) {
 589                 if (PIM_DEBUG_MTRACE)
 590                         zlog_debug(
 591                                 "Recv mtrace packet from %s on %s: too short, len=%d, min=%zu",
 592                                 from_str, ifp->name, igmp_msg_len,
 593                                 sizeof(struct igmp_mtrace));
 594                 return -1;
 595         }
 596
 597         mtracep = (struct igmp_mtrace *)igmp_msg;
 598
 599         recv_checksum = mtracep->checksum;
 600
 601         mtracep->checksum = 0;
 602
 603         checksum = in_cksum(igmp_msg, igmp_msg_len);
 604
 605         if (recv_checksum != checksum) {
 606                 if (PIM_DEBUG_MTRACE)
 607                         zlog_debug(
 608                                 "Recv mtrace packet from %s on %s: checksum mismatch: received=%x computed=%x",
 609                                 from_str, ifp->name, recv_checksum, checksum);
 610                 return -1;
 611         }
 612
 613         /* Collecting IGMP Rx stats */
 614         igmp->igmp_stats.mtrace_req++;
 615
 616         if (PIM_DEBUG_MTRACE)
 617                 mtrace_debug(pim_ifp, mtracep, igmp_msg_len);
 618
 619         /* subtract header from message length */
 620         r_len = igmp_msg_len - sizeof(struct igmp_mtrace);
 621
 622         /* Classify mtrace packet, check if it is a query */
 623         if (!r_len) {
 624                 if (PIM_DEBUG_MTRACE)
 625                         zlog_debug("Received IGMP multicast traceroute query");
 626
 627                 /* 6.1.1  Packet verification */
 628                 if (!pim_if_connected_to_source(ifp, mtracep->dst_addr)) {
 629                         if (IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr))) {
 630                                 if (PIM_DEBUG_MTRACE)
 631                                         zlog_debug(
 632                                                 "Dropping multicast query on wrong interface");
 633                                 return -1;
 634                         }
 635                         /* Unicast query on wrong interface */
 636                         fwd_code = MTRACE_FWD_CODE_WRONG_IF;
 637                         if (PIM_DEBUG_MTRACE)
 638                                 zlog_debug("Multicast query on wrong interface");
 639                 }
 640                 if (qry_id == mtracep->qry_id && qry_src == from.s_addr) {
 641                         if (PIM_DEBUG_MTRACE)
 642                                 zlog_debug(
 643                                         "Dropping multicast query with duplicate source and id");
 644                         return -1;
 645                 }
 646                 qry_id = mtracep->qry_id;
 647                 qry_src = from.s_addr;
 648         }
 649         /* if response fields length is equal to a whole number of responses */
 650         else if ((r_len % sizeof(struct igmp_mtrace_rsp)) == 0) {
 651                 r_len = igmp_msg_len - sizeof(struct igmp_mtrace);
 652
 653                 if (r_len != 0)
 654                         last_rsp_ind = r_len / sizeof(struct igmp_mtrace_rsp);
 655                 if (last_rsp_ind > MTRACE_MAX_HOPS) {
 656                         if (PIM_DEBUG_MTRACE)
 657                                 zlog_debug("Mtrace request of excessive size");
 658                         return -1;
 659                 }
 660         } else {
 661                 if (PIM_DEBUG_MTRACE)
 662                         zlog_debug(
 663                                 "Recv mtrace packet from %s on %s: invalid length %d",
 664                                 from_str, ifp->name, igmp_msg_len);
 665                 return -1;
 666         }
 667
 668         /* 6.2.1 Packet Verification - drop not link-local multicast */
 669         if (IPV4_CLASS_DE(ntohl(ip_hdr->ip_dst.s_addr))
 670             && !IPV4_MC_LINKLOCAL(ntohl(ip_hdr->ip_dst.s_addr))) {
 671                 if (PIM_DEBUG_MTRACE)
 672                         zlog_debug(
 673                                 "Recv mtrace packet from %s on %s: not link-local multicast %pI4",
 674                                 from_str, ifp->name, &ip_hdr->ip_dst);
 675                 return -1;
 676         }
 677
 678         /* 6.2.2. Normal Processing */
 679
 680         /* 6.2.2. 1. If there is room in the current buffer? */
 681
 682         if (last_rsp_ind == MTRACE_MAX_HOPS) {
 683                 /* ...there was no room... */
 684                 mtracep->rsp[MTRACE_MAX_HOPS - 1].fwd_code =
 685                         MTRACE_FWD_CODE_NO_SPACE;
 686                 return mtrace_send_response(pim_ifp->pim, mtracep,
 687                                             igmp_msg_len);
 688         }
 689
 690         /* ...insert new response block... */
 691
 692         /* calculate new mtrace lenght with extra response */
 693         mtrace_len = igmp_msg_len + sizeof(struct igmp_mtrace_rsp);
 694
 695         /* copy received query/request */
 696         memcpy(mtrace_buf, igmp_msg, igmp_msg_len);
 697
 698         /* repoint mtracep pointer to copy */
 699         mtracep = (struct igmp_mtrace *)mtrace_buf;
 700
 701         /* pointer for extra response field to be filled in */
 702         rspp = &mtracep->rsp[last_rsp_ind];
 703
 704         /* initialize extra response field */
 705         mtrace_rsp_init(rspp);
 706
 707         /* carry over any error noted when receiving the query */
 708         rspp->fwd_code = fwd_code;
 709
 710         /* ...and fill in Query Arrival Time... */
 711         rspp->arrival = htonl(query_arrival_time());
 712         rspp->outgoing = pim_ifp->primary_address;
 713         rspp->out_count = htonl(MTRACE_UNKNOWN_COUNT);
 714         rspp->fwd_ttl = 1;
 715
 716         /* 6.2.2. 2. Attempt to determine the forwarding information... */
 717
 718         if (mtracep->grp_addr.s_addr != INADDR_ANY)
 719                 fwd_info = mtrace_fwd_info(pim, mtracep, rspp, &out_ifp);
 720         else
 721                 fwd_info = mtrace_fwd_info_weak(pim, mtracep, rspp, &out_ifp);
 722
 723         /* 6.2.2 3. If no forwarding information... */
 724         if (!fwd_info) {
 725                 if (PIM_DEBUG_MTRACE)
 726                         zlog_debug("mtrace not found multicast state");
 727                 mtrace_rsp_set_fwd_code(rspp, MTRACE_FWD_CODE_NO_ROUTE);
 728                 /* 6.2.2. 3. forward the packet to requester */
 729                 return mtrace_send_response(pim, mtracep, mtrace_len);
 730         }
 731
 732         nh_addr = rspp->prev_hop;
 733
 734         reached_source = false;
 735
 736         if (nh_addr.s_addr == INADDR_ANY) {
 737                 /* no pim? i.e. 7.5.3. No Previous Hop */
 738                 if (!out_ifp->info) {
 739                         if (PIM_DEBUG_MTRACE)
 740                                 zlog_debug("mtrace not found incoming if w/ pim");
 741                         mtrace_rsp_set_fwd_code(rspp,
 742                                                 MTRACE_FWD_CODE_NO_MULTICAST);
 743                         return mtrace_send_response(pim, mtracep, mtrace_len);
 744                 }
 745                 /* reached source? i.e. 7.5.1 Arriving at source */
 746                 if (pim_if_connected_to_source(out_ifp, mtracep->src_addr)) {
 747                         reached_source = true;
 748                         rspp->prev_hop = mtracep->src_addr;
 749                 }
 750                 /*
 751                  * 6.4 Forwarding Traceroute Requests:
 752                  * Previous-hop router not known,
 753                  * packet is sent to an appropriate multicast address
 754                  */
 755                 (void)inet_aton(MCAST_ALL_ROUTERS, &nh_addr);
 756         }
 757
 758         /* 6.2.2 8. If this router is the Rendez-vous Point */
 759         if (mtracep->grp_addr.s_addr != INADDR_ANY &&
 760             pim_rp_i_am_rp(pim, mtracep->grp_addr)) {
 761                 mtrace_rsp_set_fwd_code(rspp, MTRACE_FWD_CODE_REACHED_RP);
 762                 /* 7.7.1. PIM-SM ...RP has not performed source-specific join */
 763                 if (rspp->src_mask == MTRACE_SRC_MASK_GROUP)
 764                         return mtrace_send_response(pim, mtracep, mtrace_len);
 765         }
 766
 767         /*
 768          * 6.4 Forwarding Traceroute Requests: the number of response
 769          * blocks exceeds number of responses, so forward to the requester.
 770          */
 771         if (mtracep->hops <= (last_rsp_ind + 1))
 772                 return mtrace_send_response(pim, mtracep, mtrace_len);
 773
 774         /* 7.5.1. Arriving at source: terminate trace */
 775         if (reached_source)
 776                 return mtrace_send_response(pim, mtracep, mtrace_len);
 777
 778         mtracep->checksum = 0;
 779
 780         mtracep->checksum = in_cksum(mtrace_buf, mtrace_len);
 781
 782         /* 6.4 Forwarding Traceroute Requests: response blocks less than req. */
 783         return mtrace_send_packet(out_ifp, mtracep, mtrace_len, nh_addr,
 784                                   mtracep->grp_addr);
 785 }
 786
 787 /* 6.3. Traceroute responses */
 788 int igmp_mtrace_recv_response(struct gm_sock *igmp, struct ip *ip_hdr,
 789                               struct in_addr from, const char *from_str,
 790                               char *igmp_msg, int igmp_msg_len)
 791 {
 792         static uint32_t qry_id, rsp_dst;
 793         struct interface *ifp;
 794         struct pim_interface *pim_ifp;
 795         struct pim_instance *pim;
 796         struct igmp_mtrace *mtracep;
 797         uint16_t recv_checksum;
 798         uint16_t checksum;
 799
 800         ifp = igmp->interface;
 801         pim_ifp = ifp->info;
 802         pim = pim_ifp->pim;
 803
 804         if (igmp_msg_len < (int)sizeof(struct igmp_mtrace)) {
 805                 if (PIM_DEBUG_MTRACE)
 806                         zlog_debug(
 807                                 "Recv mtrace packet from %s on %s: too short, len=%d, min=%zu",
 808                                 from_str, ifp->name, igmp_msg_len,
 809                                 sizeof(struct igmp_mtrace));
 810                 return -1;
 811         }
 812
 813         mtracep = (struct igmp_mtrace *)igmp_msg;
 814
 815         recv_checksum = mtracep->checksum;
 816
 817         mtracep->checksum = 0;
 818
 819         checksum = in_cksum(igmp_msg, igmp_msg_len);
 820
 821         if (recv_checksum != checksum) {
 822                 if (PIM_DEBUG_MTRACE)
 823                         zlog_debug(
 824                                 "Recv mtrace response from %s on %s: checksum mismatch: received=%x computed=%x",
 825                                 from_str, ifp->name, recv_checksum, checksum);
 826                 return -1;
 827         }
 828
 829         mtracep->checksum = checksum;
 830
 831         /* Collecting IGMP Rx stats */
 832         igmp->igmp_stats.mtrace_rsp++;
 833
 834         if (PIM_DEBUG_MTRACE)
 835                 mtrace_debug(pim_ifp, mtracep, igmp_msg_len);
 836
 837         /* Drop duplicate packets */
 838         if (qry_id == mtracep->qry_id && rsp_dst == ip_hdr->ip_dst.s_addr) {
 839                 if (PIM_DEBUG_MTRACE)
 840                         zlog_debug("duplicate mtrace response packet dropped");
 841                 return -1;
 842         }
 843
 844         qry_id = mtracep->qry_id;
 845         rsp_dst = ip_hdr->ip_dst.s_addr;
 846
 847         return mtrace_forward_packet(pim, ip_hdr);
 848 }