pimd/pim_igmp_mtrace.c

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