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