pimd/pim_igmp_mtrace.c

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