pimd/mtracebis.c

   1 /*
   2  * Multicast Traceroute for FRRouting
   3  * Copyright (C) 2018  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 #ifdef __linux__
  21
  22 #include "pim_igmp_mtrace.h"
  23
  24 #include "checksum.h"
  25 #include "mtracebis_routeget.h"
  26
  27 #include <sys/select.h>
  28 #include <netinet/in.h>
  29 #include <arpa/inet.h>
  30 #include <unistd.h>
  31 #include <sys/types.h>
  32 #include <sys/time.h>
  33 #include <stdio.h>
  34 #include <stdlib.h>
  35 #include <string.h>
  36 #include <time.h>
  37 #include <net/if.h>
  38 #include <unistd.h>
  39 #include <getopt.h>
  40
  41 #define MTRACEBIS_VERSION "0.1"
  42 #define MTRACE_TIMEOUT (5)
  43
  44 #define IP_HDR_LEN (sizeof(struct ip))
  45 #define IP_RA_LEN (4)
  46 #define MTRACE_BUF_LEN (MTRACE_HDR_SIZE + (MTRACE_MAX_HOPS * MTRACE_RSP_SIZE))
  47 #define IP_AND_MTRACE_BUF_LEN (IP_HDR_LEN + IP_RA_LEN + MTRACE_BUF_LEN)
  48
  49 static const char *progname;
  50 static void usage(void)
  51 {
  52         fprintf(stderr, "Usage : %s <multicast source>\n", progname);
  53 }
  54 static void version(void)
  55 {
  56         fprintf(stderr, "%s %s\n", progname, MTRACEBIS_VERSION);
  57 }
  58
  59 static int send_query(int fd, struct in_addr to_addr,
  60                       struct igmp_mtrace *mtrace)
  61 {
  62         struct sockaddr_in to;
  63         socklen_t tolen;
  64         int sent;
  65
  66         memset(&to, 0, sizeof(to));
  67         to.sin_family = AF_INET;
  68         to.sin_addr = to_addr;
  69         tolen = sizeof(to);
  70
  71         sent = sendto(fd, (char *)mtrace, sizeof(*mtrace), MSG_DONTWAIT,
  72                       (struct sockaddr *)&to, tolen);
  73
  74         if (sent < 1)
  75                 return -1;
  76         return 0;
  77 }
  78
  79 static void print_query(struct igmp_mtrace *mtrace)
  80 {
  81         char src_str[INET_ADDRSTRLEN];
  82         char dst_str[INET_ADDRSTRLEN];
  83         char grp_str[INET_ADDRSTRLEN];
  84
  85         printf("* Mtrace from %s to %s via group %s\n",
  86                inet_ntop(AF_INET, &mtrace->src_addr, src_str, sizeof(src_str)),
  87                inet_ntop(AF_INET, &mtrace->dst_addr, dst_str, sizeof(dst_str)),
  88                inet_ntop(AF_INET, &mtrace->grp_addr, grp_str, sizeof(grp_str)));
  89 }
  90
  91 static int recv_response(int fd, long msec, int *hops)
  92 {
  93         int recvd;
  94         char mtrace_buf[IP_AND_MTRACE_BUF_LEN];
  95         struct ip *ip;
  96         struct igmp_mtrace *mtrace;
  97         int mtrace_len;
  98         int responses;
  99         int i;
 100         u_short sum;
 101
 102         recvd = recvfrom(fd, mtrace_buf, IP_AND_MTRACE_BUF_LEN, 0, NULL, 0);
 103
 104         if (recvd < 1) {
 105                 fprintf(stderr, "recvfrom error: %s\n", strerror(errno));
 106                 return -1;
 107         }
 108
 109         if (recvd < (int)sizeof(struct ip)) {
 110                 fprintf(stderr, "no ip header\n");
 111                 return -1;
 112         }
 113
 114         ip = (struct ip *)mtrace_buf;
 115
 116         if (ip->ip_v != 4) {
 117                 fprintf(stderr, "IP not version 4\n");
 118                 return -1;
 119         }
 120
 121         sum = ip->ip_sum;
 122         ip->ip_sum = 0;
 123
 124         if (sum != in_cksum(ip, ip->ip_hl * 4))
 125                 return -1;
 126
 127         mtrace = (struct igmp_mtrace *)(mtrace_buf + (4 * ip->ip_hl));
 128
 129         mtrace_len = ntohs(ip->ip_len) - ip->ip_hl * 4;
 130
 131         if (mtrace_len < (int)MTRACE_HDR_SIZE)
 132                 return -1;
 133
 134         sum = mtrace->checksum;
 135         mtrace->checksum = 0;
 136         if (sum != in_cksum(mtrace, mtrace_len)) {
 137                 fprintf(stderr, "mtrace checksum wrong\n");
 138                 return -1;
 139         }
 140
 141         if (mtrace->type != PIM_IGMP_MTRACE_RESPONSE)
 142                 return -1;
 143
 144
 145         responses = mtrace_len - sizeof(struct igmp_mtrace);
 146         responses /= sizeof(struct igmp_mtrace_rsp);
 147
 148         printf("%ld ms received responses from %d hops.\n", msec, responses);
 149
 150         if (hops)
 151                 *hops = responses;
 152
 153         for (i = 0; i < responses; i++) {
 154                 struct igmp_mtrace_rsp *rsp = &mtrace->rsp[i];
 155
 156                 if (rsp->fwd_code != 0)
 157                         printf("-%d fwd. code 0x%2x.\n", i, rsp->fwd_code);
 158         }
 159
 160         return 0;
 161 }
 162
 163 static int wait_for_response(int fd, int *hops)
 164 {
 165         fd_set readfds;
 166         struct timeval timeout;
 167         int ret = -1;
 168         long msec, rmsec, tmsec;
 169
 170         FD_ZERO(&readfds);
 171         FD_SET(fd, &readfds);
 172
 173         memset(&timeout, 0, sizeof(timeout));
 174
 175         timeout.tv_sec = MTRACE_TIMEOUT;
 176
 177         tmsec = timeout.tv_sec * 1000 + timeout.tv_usec / 1000;
 178         do {
 179                 ret = select(fd + 1, &readfds, NULL, NULL, &timeout);
 180                 if (ret <= 0)
 181                         return ret;
 182                 rmsec = timeout.tv_sec * 1000 + timeout.tv_usec / 1000;
 183                 msec = tmsec - rmsec;
 184         } while (recv_response(fd, msec, hops) != 0);
 185
 186         return ret;
 187 }
 188
 189 int main(int argc, char *const argv[])
 190 {
 191         struct in_addr mc_source;
 192         struct in_addr iface_addr;
 193         struct in_addr gw_addr;
 194         struct in_addr mtrace_addr;
 195         struct igmp_mtrace mtrace;
 196         int hops = 255;
 197         int rhops;
 198         int maxhops = 255;
 199         int perhop = 3;
 200         int ifindex;
 201         int unicast = 1;
 202         int ttl = 64;
 203         int fd = -1;
 204         int ret = -1;
 205         int c;
 206         int i, j;
 207         char ifname[IF_NAMESIZE];
 208         char ip_str[INET_ADDRSTRLEN];
 209
 210         mtrace_addr.s_addr = inet_addr("224.0.1.32");
 211
 212         uid_t uid = getuid();
 213
 214         if (uid != 0) {
 215                 printf("must run as root\n");
 216                 exit(EXIT_FAILURE);
 217         }
 218
 219         if (argc <= 0)
 220                 progname = "mtracebis";
 221         else
 222                 progname = argv[0];
 223
 224         if (argc != 2) {
 225                 usage();
 226                 exit(EXIT_FAILURE);
 227         }
 228
 229         while (1) {
 230                 static struct option long_options[] = {
 231                         {"help", no_argument, 0, 'h'},
 232                         {"version", no_argument, 0, 'v'},
 233                         {0, 0, 0, 0}};
 234                 int option_index = 0;
 235
 236                 c = getopt_long(argc, argv, "vh", long_options, &option_index);
 237
 238                 if (c == -1)
 239                         break;
 240
 241                 switch (c) {
 242                 case 'h':
 243                         usage();
 244                         exit(0);
 245                 case 'v':
 246                         version();
 247                         exit(0);
 248                 default:
 249                         usage();
 250                         exit(EXIT_FAILURE);
 251                 }
 252         }
 253         if (inet_pton(AF_INET, argv[1], &mc_source) != 1) {
 254                 usage();
 255                 fprintf(stderr, "%s: %s not a valid IPv4 address\n", argv[0],
 256                         argv[1]);
 257                 exit(EXIT_FAILURE);
 258         }
 259
 260         ifindex = routeget(mc_source, &iface_addr, &gw_addr);
 261         if (ifindex < 0) {
 262                 fprintf(stderr, "%s: failed to get route to source %s\n",
 263                         argv[0], argv[1]);
 264                 exit(EXIT_FAILURE);
 265         }
 266
 267         if (if_indextoname(ifindex, ifname) == NULL) {
 268                 fprintf(stderr, "%s: if_indextoname error: %s\n", argv[0],
 269                         strerror(errno));
 270                 exit(EXIT_FAILURE);
 271         }
 272
 273         /* zero mtrace struct */
 274         memset((char *)&mtrace, 0, sizeof(mtrace));
 275
 276         /* set up query */
 277         mtrace.type = PIM_IGMP_MTRACE_QUERY_REQUEST;
 278         mtrace.hops = hops;
 279         mtrace.checksum = 0;
 280         mtrace.grp_addr.s_addr = 0;
 281         mtrace.src_addr = mc_source;
 282         mtrace.dst_addr = iface_addr;
 283         mtrace.rsp_addr = unicast ? iface_addr : mtrace_addr;
 284         mtrace.rsp_ttl = ttl;
 285         mtrace.qry_id = 0xffffff & time(NULL);
 286
 287         mtrace.checksum = in_cksum(&mtrace, sizeof(mtrace));
 288
 289         fd = socket(AF_INET, SOCK_RAW, IPPROTO_IGMP);
 290
 291         if (fd < 1) {
 292                 fprintf(stderr, "%s: socket error: %s\n", argv[0],
 293                         strerror(errno));
 294                 exit(EXIT_FAILURE);
 295         }
 296
 297         ret = setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, ifname,
 298                          strlen(ifname));
 299
 300         if (ret < 0) {
 301                 fprintf(stderr, "%s: setsockopt error: %s\n", argv[0],
 302                         strerror(errno));
 303                 ret = EXIT_FAILURE;
 304                 goto close_fd;
 305         }
 306
 307         print_query(&mtrace);
 308         if (send_query(fd, gw_addr, &mtrace) < 0) {
 309                 fprintf(stderr, "%s: sendto error: %s\n", argv[0],
 310                         strerror(errno));
 311                 ret = EXIT_FAILURE;
 312                 goto close_fd;
 313         }
 314         printf("Querying full reverse path...\n");
 315         ret = wait_for_response(fd, NULL);
 316         if (ret > 0) {
 317                 ret = 0;
 318                 goto close_fd;
 319         }
 320         if (ret < 0) {
 321                 fprintf(stderr, "%s: select error: %s\n", argv[0],
 322                         strerror(errno));
 323                 ret = EXIT_FAILURE;
 324                 goto close_fd;
 325         }
 326         printf(" * ");
 327         printf("switching to hop-by-hop:\n");
 328         printf("%3d  ? (%s)\n", 0,
 329                inet_ntop(AF_INET, &mtrace.dst_addr, ip_str, sizeof(ip_str)));
 330         for (i = 1; i < maxhops; i++) {
 331                 printf("%3d ", -i);
 332                 mtrace.hops = i;
 333                 for (j = 0; j < perhop; j++) {
 334                         mtrace.qry_id++;
 335                         mtrace.checksum = 0;
 336                         mtrace.checksum = in_cksum(&mtrace, sizeof(mtrace));
 337                         if (send_query(fd, gw_addr, &mtrace) < 0) {
 338                                 fprintf(stderr, "%s: sendto error: %s\n",
 339                                         argv[0], strerror(errno));
 340                                 ret = EXIT_FAILURE;
 341                                 goto close_fd;
 342                         }
 343                         ret = wait_for_response(fd, &rhops);
 344                         if (ret > 0) {
 345                                 if (i > rhops) {
 346                                         ret = 0;
 347                                         goto close_fd;
 348                                 }
 349                                 break;
 350                         }
 351                         printf(" *");
 352                 }
 353                 if (ret <= 0)
 354                         printf("\n");
 355         }
 356         ret = 0;
 357 close_fd:
 358         close(fd);
 359         exit(ret);
 360 }
 361
 362 #else /* __linux__ */
 363
 364 #include <stdio.h>
 365 #include <stdlib.h>
 366
 367 int main(int argc, char *argv[])
 368 {
 369         printf("%s implemented only for GNU/Linux\n", argv[0]);
 370         exit(0);
 371 }
 372
 373 #endif /* __linux__ */