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