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