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