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