bfdd/bfd_packet.c

   1 /*********************************************************************
   2  * Copyright 2017 Cumulus Networks, Inc.  All rights reserved.
   3  *
   4  * This program is free software; you can redistribute it and/or modify it
   5  * under the terms of the GNU General Public License as published by the Free
   6  * Software Foundation; either version 2 of the License, or (at your option)
   7  * any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  12  * more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; see the file COPYING; if not, write to the Free Software
  16  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  17  *
  18  * bfd_packet.c: implements the BFD protocol packet handling.
  19  *
  20  * Authors
  21  * -------
  22  * Shrijeet Mukherjee [shm@cumulusnetworks.com]
  23  * Kanna Rajagopal [kanna@cumulusnetworks.com]
  24  * Radhika Mahankali [Radhika@cumulusnetworks.com]
  25  */
  26
  27 #include <zebra.h>
  28
  29 #ifdef BFD_LINUX
  30 #include <linux/if_packet.h>
  31 #endif /* BFD_LINUX */
  32
  33 #include <netinet/if_ether.h>
  34 #include <netinet/udp.h>
  35
  36 #include "lib/sockopt.h"
  37
  38 #include "bfd.h"
  39
  40
  41 /*
  42  * Prototypes
  43  */
  44 static int ptm_bfd_process_echo_pkt(int s);
  45 int _ptm_bfd_send(struct bfd_session *bs, uint16_t *port, const void *data,
  46                   size_t datalen);
  47
  48 static void bfd_sd_reschedule(int sd);
  49 ssize_t bfd_recv_ipv4(int sd, uint8_t *msgbuf, size_t msgbuflen, uint8_t *ttl,
  50                       ifindex_t *ifindex, struct sockaddr_any *local,
  51                       struct sockaddr_any *peer);
  52 ssize_t bfd_recv_ipv6(int sd, uint8_t *msgbuf, size_t msgbuflen, uint8_t *ttl,
  53                       ifindex_t *ifindex, struct sockaddr_any *local,
  54                       struct sockaddr_any *peer);
  55 int bp_udp_send(int sd, uint8_t ttl, uint8_t *data, size_t datalen,
  56                 struct sockaddr *to, socklen_t tolen);
  57 int bp_bfd_echo_in(int sd, uint8_t *ttl, uint32_t *my_discr);
  58
  59 /* socket related prototypes */
  60 static void bp_set_ipopts(int sd);
  61 static void bp_bind_ip(int sd, uint16_t port);
  62 static void bp_set_ipv6opts(int sd);
  63 static void bp_bind_ipv6(int sd, uint16_t port);
  64
  65
  66 /*
  67  * Functions
  68  */
  69 int _ptm_bfd_send(struct bfd_session *bs, uint16_t *port, const void *data,
  70                   size_t datalen)
  71 {
  72         struct sockaddr *sa;
  73         struct sockaddr_in sin;
  74         struct sockaddr_in6 sin6;
  75         socklen_t slen;
  76         ssize_t rv;
  77         int sd = -1;
  78
  79         if (BFD_CHECK_FLAG(bs->flags, BFD_SESS_FLAG_IPV6)) {
  80                 memset(&sin6, 0, sizeof(sin6));
  81                 sin6.sin6_family = AF_INET6;
  82                 memcpy(&sin6.sin6_addr, &bs->key.peer, sizeof(sin6.sin6_addr));
  83                 if (IN6_IS_ADDR_LINKLOCAL(&sin6.sin6_addr))
  84                         sin6.sin6_scope_id = bs->ifp->ifindex;
  85
  86                 sin6.sin6_port =
  87                         (port) ? *port
  88                                : (BFD_CHECK_FLAG(bs->flags, BFD_SESS_FLAG_MH))
  89                                          ? htons(BFD_DEF_MHOP_DEST_PORT)
  90                                          : htons(BFD_DEFDESTPORT);
  91
  92                 sd = bs->sock;
  93                 sa = (struct sockaddr *)&sin6;
  94                 slen = sizeof(sin6);
  95         } else {
  96                 memset(&sin, 0, sizeof(sin));
  97                 sin.sin_family = AF_INET;
  98                 memcpy(&sin.sin_addr, &bs->key.peer, sizeof(sin.sin_addr));
  99                 sin.sin_port =
 100                         (port) ? *port
 101                                : (BFD_CHECK_FLAG(bs->flags, BFD_SESS_FLAG_MH))
 102                                          ? htons(BFD_DEF_MHOP_DEST_PORT)
 103                                          : htons(BFD_DEFDESTPORT);
 104
 105                 sd = bs->sock;
 106                 sa = (struct sockaddr *)&sin;
 107                 slen = sizeof(sin);
 108         }
 109
 110 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 111         sa->sa_len = slen;
 112 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 113         rv = sendto(sd, data, datalen, 0, sa, slen);
 114         if (rv <= 0) {
 115                 log_debug("packet-send: send failure: %s", strerror(errno));
 116                 return -1;
 117         }
 118         if (rv < (ssize_t)datalen)
 119                 log_debug("packet-send: send partial", strerror(errno));
 120
 121         return 0;
 122 }
 123
 124 void ptm_bfd_echo_snd(struct bfd_session *bfd)
 125 {
 126         struct sockaddr *sa;
 127         socklen_t salen;
 128         int sd;
 129         struct bfd_echo_pkt bep;
 130         struct sockaddr_in sin;
 131         struct sockaddr_in6 sin6;
 132
 133         if (!BFD_CHECK_FLAG(bfd->flags, BFD_SESS_FLAG_ECHO_ACTIVE))
 134                 BFD_SET_FLAG(bfd->flags, BFD_SESS_FLAG_ECHO_ACTIVE);
 135
 136         memset(&bep, 0, sizeof(bep));
 137         bep.ver = BFD_ECHO_VERSION;
 138         bep.len = BFD_ECHO_PKT_LEN;
 139         bep.my_discr = htonl(bfd->discrs.my_discr);
 140
 141         if (BFD_CHECK_FLAG(bfd->flags, BFD_SESS_FLAG_IPV6)) {
 142                 sd = bglobal.bg_echov6;
 143                 memset(&sin6, 0, sizeof(sin6));
 144                 memcpy(&sin6.sin6_addr, &bfd->key.peer, sizeof(sin6.sin6_addr));
 145                 if (bfd->ifp && IN6_IS_ADDR_LINKLOCAL(&sin6.sin6_addr))
 146                         sin6.sin6_scope_id = bfd->ifp->ifindex;
 147
 148                 sin6.sin6_port = htons(BFD_DEF_ECHO_PORT);
 149 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 150                 sin6.sin6_len = sizeof(sin6);
 151 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 152
 153                 sa = (struct sockaddr *)&sin6;
 154                 salen = sizeof(sin6);
 155         } else {
 156                 sd = bglobal.bg_echo;
 157                 memset(&sin6, 0, sizeof(sin6));
 158                 memcpy(&sin.sin_addr, &bfd->key.peer, sizeof(sin.sin_addr));
 159                 sin.sin_port = htons(BFD_DEF_ECHO_PORT);
 160 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 161                 sin.sin_len = sizeof(sin);
 162 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 163
 164                 sa = (struct sockaddr *)&sin;
 165                 salen = sizeof(sin);
 166         }
 167         if (bp_udp_send(sd, BFD_TTL_VAL, (uint8_t *)&bep, sizeof(bep), sa,
 168                         salen)
 169             == -1)
 170                 return;
 171
 172         bfd->stats.tx_echo_pkt++;
 173 }
 174
 175 static int ptm_bfd_process_echo_pkt(int s)
 176 {
 177         struct bfd_session *bfd;
 178         uint32_t my_discr = 0;
 179         uint8_t ttl = 0;
 180
 181         /* Receive and parse echo packet. */
 182         if (bp_bfd_echo_in(s, &ttl, &my_discr) == -1)
 183                 return 0;
 184
 185         /* Your discriminator not zero - use it to find session */
 186         bfd = bfd_id_lookup(my_discr);
 187         if (bfd == NULL) {
 188                 log_debug("echo-packet: no matching session (id:%u)", my_discr);
 189                 return -1;
 190         }
 191
 192         if (!BFD_CHECK_FLAG(bfd->flags, BFD_SESS_FLAG_ECHO_ACTIVE)) {
 193                 log_debug("echo-packet: echo disabled [%s] (id:%u)",
 194                           bs_to_string(bfd), my_discr);
 195                 return -1;
 196         }
 197
 198         bfd->stats.rx_echo_pkt++;
 199
 200         /* Compute detect time */
 201         bfd->echo_detect_TO = bfd->remote_detect_mult * bfd->echo_xmt_TO;
 202
 203         /* Update echo receive timeout. */
 204         if (bfd->echo_detect_TO > 0)
 205                 bfd_echo_recvtimer_update(bfd);
 206
 207         return 0;
 208 }
 209
 210 void ptm_bfd_snd(struct bfd_session *bfd, int fbit)
 211 {
 212         struct bfd_pkt cp;
 213
 214         /* Set fields according to section 6.5.7 */
 215         cp.diag = bfd->local_diag;
 216         BFD_SETVER(cp.diag, BFD_VERSION);
 217         cp.flags = 0;
 218         BFD_SETSTATE(cp.flags, bfd->ses_state);
 219         BFD_SETDEMANDBIT(cp.flags, BFD_DEF_DEMAND);
 220
 221         /*
 222          * Polling and Final can't be set at the same time.
 223          *
 224          * RFC 5880, Section 6.5.
 225          */
 226         BFD_SETFBIT(cp.flags, fbit);
 227         if (fbit == 0)
 228                 BFD_SETPBIT(cp.flags, bfd->polling);
 229
 230         cp.detect_mult = bfd->detect_mult;
 231         cp.len = BFD_PKT_LEN;
 232         cp.discrs.my_discr = htonl(bfd->discrs.my_discr);
 233         cp.discrs.remote_discr = htonl(bfd->discrs.remote_discr);
 234         if (bfd->polling) {
 235                 cp.timers.desired_min_tx =
 236                         htonl(bfd->timers.desired_min_tx);
 237                 cp.timers.required_min_rx =
 238                         htonl(bfd->timers.required_min_rx);
 239         } else {
 240                 /*
 241                  * We can only announce current setting on poll, this
 242                  * avoids timing mismatch with our peer and give it
 243                  * the oportunity to learn. See `bs_final_handler` for
 244                  * more information.
 245                  */
 246                 cp.timers.desired_min_tx =
 247                         htonl(bfd->cur_timers.desired_min_tx);
 248                 cp.timers.required_min_rx =
 249                         htonl(bfd->cur_timers.required_min_rx);
 250         }
 251         cp.timers.required_min_echo = htonl(bfd->timers.required_min_echo);
 252
 253         if (_ptm_bfd_send(bfd, NULL, &cp, BFD_PKT_LEN) != 0)
 254                 return;
 255
 256         bfd->stats.tx_ctrl_pkt++;
 257 }
 258
 259 ssize_t bfd_recv_ipv4(int sd, uint8_t *msgbuf, size_t msgbuflen, uint8_t *ttl,
 260                       ifindex_t *ifindex, struct sockaddr_any *local,
 261                       struct sockaddr_any *peer)
 262 {
 263         struct cmsghdr *cm;
 264         ssize_t mlen;
 265         struct sockaddr_in msgaddr;
 266         struct msghdr msghdr;
 267         struct iovec iov[1];
 268         uint8_t cmsgbuf[255];
 269
 270         /* Prepare the recvmsg params. */
 271         iov[0].iov_base = msgbuf;
 272         iov[0].iov_len = msgbuflen;
 273
 274         memset(&msghdr, 0, sizeof(msghdr));
 275         msghdr.msg_name = &msgaddr;
 276         msghdr.msg_namelen = sizeof(msgaddr);
 277         msghdr.msg_iov = iov;
 278         msghdr.msg_iovlen = 1;
 279         msghdr.msg_control = cmsgbuf;
 280         msghdr.msg_controllen = sizeof(cmsgbuf);
 281
 282         mlen = recvmsg(sd, &msghdr, MSG_DONTWAIT);
 283         if (mlen == -1) {
 284                 if (errno != EAGAIN)
 285                         log_error("ipv4-recv: recv failed: %s",
 286                                   strerror(errno));
 287
 288                 return -1;
 289         }
 290
 291         /* Get source address */
 292         peer->sa_sin = *((struct sockaddr_in *)(msghdr.msg_name));
 293
 294         /* Get and check TTL */
 295         for (cm = CMSG_FIRSTHDR(&msghdr); cm != NULL;
 296              cm = CMSG_NXTHDR(&msghdr, cm)) {
 297                 if (cm->cmsg_level != IPPROTO_IP)
 298                         continue;
 299
 300                 switch (cm->cmsg_type) {
 301 #ifdef BFD_LINUX
 302                 case IP_TTL: {
 303                         uint32_t ttlval;
 304
 305                         memcpy(&ttlval, CMSG_DATA(cm), sizeof(ttlval));
 306                         if (ttlval > 255) {
 307                                 log_debug("ipv4-recv: invalid TTL: %u", ttlval);
 308                                 return -1;
 309                         }
 310                         *ttl = ttlval;
 311                         break;
 312                 }
 313
 314                 case IP_PKTINFO: {
 315                         struct in_pktinfo *pi =
 316                                 (struct in_pktinfo *)CMSG_DATA(cm);
 317
 318                         if (pi == NULL)
 319                                 break;
 320
 321                         local->sa_sin.sin_family = AF_INET;
 322                         local->sa_sin.sin_addr = pi->ipi_addr;
 323 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 324                         local->sa_sin.sin_len = sizeof(local->sa_sin);
 325 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 326
 327                         *ifindex = pi->ipi_ifindex;
 328                         break;
 329                 }
 330 #endif /* BFD_LINUX */
 331 #ifdef BFD_BSD
 332                 case IP_RECVTTL: {
 333                         memcpy(ttl, CMSG_DATA(cm), sizeof(*ttl));
 334                         break;
 335                 }
 336
 337                 case IP_RECVDSTADDR: {
 338                         struct in_addr ia;
 339
 340                         memcpy(&ia, CMSG_DATA(cm), sizeof(ia));
 341                         local->sa_sin.sin_family = AF_INET;
 342                         local->sa_sin.sin_addr = ia;
 343 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 344                         local->sa_sin.sin_len = sizeof(local->sa_sin);
 345 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 346                         break;
 347                 }
 348 #endif /* BFD_BSD */
 349
 350                 default:
 351                         /*
 352                          * On *BSDs we expect to land here when skipping
 353                          * the IP_RECVIF header. It will be handled by
 354                          * getsockopt_ifindex() below.
 355                          */
 356                         /* NOTHING */
 357                         break;
 358                 }
 359         }
 360
 361         /* OS agnostic way of getting interface name. */
 362         if (*ifindex == IFINDEX_INTERNAL)
 363                 *ifindex = getsockopt_ifindex(AF_INET, &msghdr);
 364
 365         return mlen;
 366 }
 367
 368 ssize_t bfd_recv_ipv6(int sd, uint8_t *msgbuf, size_t msgbuflen, uint8_t *ttl,
 369                       ifindex_t *ifindex, struct sockaddr_any *local,
 370                       struct sockaddr_any *peer)
 371 {
 372         struct cmsghdr *cm;
 373         struct in6_pktinfo *pi6 = NULL;
 374         ssize_t mlen;
 375         uint32_t ttlval;
 376         struct sockaddr_in6 msgaddr6;
 377         struct msghdr msghdr6;
 378         struct iovec iov[1];
 379         uint8_t cmsgbuf6[255];
 380
 381         /* Prepare the recvmsg params. */
 382         iov[0].iov_base = msgbuf;
 383         iov[0].iov_len = msgbuflen;
 384
 385         memset(&msghdr6, 0, sizeof(msghdr6));
 386         msghdr6.msg_name = &msgaddr6;
 387         msghdr6.msg_namelen = sizeof(msgaddr6);
 388         msghdr6.msg_iov = iov;
 389         msghdr6.msg_iovlen = 1;
 390         msghdr6.msg_control = cmsgbuf6;
 391         msghdr6.msg_controllen = sizeof(cmsgbuf6);
 392
 393         mlen = recvmsg(sd, &msghdr6, MSG_DONTWAIT);
 394         if (mlen == -1) {
 395                 if (errno != EAGAIN)
 396                         log_error("ipv6-recv: recv failed: %s",
 397                                   strerror(errno));
 398
 399                 return -1;
 400         }
 401
 402         /* Get source address */
 403         peer->sa_sin6 = *((struct sockaddr_in6 *)(msghdr6.msg_name));
 404
 405         /* Get and check TTL */
 406         for (cm = CMSG_FIRSTHDR(&msghdr6); cm != NULL;
 407              cm = CMSG_NXTHDR(&msghdr6, cm)) {
 408                 if (cm->cmsg_level != IPPROTO_IPV6)
 409                         continue;
 410
 411                 if (cm->cmsg_type == IPV6_HOPLIMIT) {
 412                         memcpy(&ttlval, CMSG_DATA(cm), sizeof(ttlval));
 413                         if (ttlval > 255) {
 414                                 log_debug("ipv6-recv: invalid TTL: %u", ttlval);
 415                                 return -1;
 416                         }
 417
 418                         *ttl = ttlval;
 419                 } else if (cm->cmsg_type == IPV6_PKTINFO) {
 420                         pi6 = (struct in6_pktinfo *)CMSG_DATA(cm);
 421                         if (pi6) {
 422                                 local->sa_sin6.sin6_family = AF_INET6;
 423                                 local->sa_sin6.sin6_addr = pi6->ipi6_addr;
 424 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 425                                 local->sa_sin6.sin6_len = sizeof(local->sa_sin6);
 426 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 427
 428                                 *ifindex = pi6->ipi6_ifindex;
 429                         }
 430                 }
 431         }
 432
 433         /* Set scope ID for link local addresses. */
 434         if (IN6_IS_ADDR_LINKLOCAL(&peer->sa_sin6.sin6_addr))
 435                 peer->sa_sin6.sin6_scope_id = *ifindex;
 436         if (IN6_IS_ADDR_LINKLOCAL(&local->sa_sin6.sin6_addr))
 437                 local->sa_sin6.sin6_scope_id = *ifindex;
 438
 439         return mlen;
 440 }
 441
 442 static void bfd_sd_reschedule(int sd)
 443 {
 444         if (sd == bglobal.bg_shop) {
 445                 THREAD_OFF(bglobal.bg_ev[0]);
 446                 thread_add_read(master, bfd_recv_cb, NULL, bglobal.bg_shop,
 447                                 &bglobal.bg_ev[0]);
 448         } else if (sd == bglobal.bg_mhop) {
 449                 THREAD_OFF(bglobal.bg_ev[1]);
 450                 thread_add_read(master, bfd_recv_cb, NULL, bglobal.bg_mhop,
 451                                 &bglobal.bg_ev[1]);
 452         } else if (sd == bglobal.bg_shop6) {
 453                 THREAD_OFF(bglobal.bg_ev[2]);
 454                 thread_add_read(master, bfd_recv_cb, NULL, bglobal.bg_shop6,
 455                                 &bglobal.bg_ev[2]);
 456         } else if (sd == bglobal.bg_mhop6) {
 457                 THREAD_OFF(bglobal.bg_ev[3]);
 458                 thread_add_read(master, bfd_recv_cb, NULL, bglobal.bg_mhop6,
 459                                 &bglobal.bg_ev[3]);
 460         } else if (sd == bglobal.bg_echo) {
 461                 THREAD_OFF(bglobal.bg_ev[4]);
 462                 thread_add_read(master, bfd_recv_cb, NULL, bglobal.bg_echo,
 463                                 &bglobal.bg_ev[4]);
 464         } else if (sd == bglobal.bg_echov6) {
 465                 THREAD_OFF(bglobal.bg_ev[5]);
 466                 thread_add_read(master, bfd_recv_cb, NULL, bglobal.bg_echov6,
 467                                 &bglobal.bg_ev[5]);
 468         }
 469 }
 470
 471 static void cp_debug(bool mhop, struct sockaddr_any *peer,
 472                      struct sockaddr_any *local, ifindex_t ifindex,
 473                      vrf_id_t vrfid, const char *fmt, ...)
 474 {
 475         char buf[512], peerstr[128], localstr[128], portstr[64], vrfstr[64];
 476         va_list vl;
 477
 478         if (peer->sa_sin.sin_family)
 479                 snprintf(peerstr, sizeof(peerstr), " peer:%s", satostr(peer));
 480         else
 481                 peerstr[0] = 0;
 482
 483         if (local->sa_sin.sin_family)
 484                 snprintf(localstr, sizeof(localstr), " local:%s",
 485                          satostr(local));
 486         else
 487                 localstr[0] = 0;
 488
 489         if (ifindex != IFINDEX_INTERNAL)
 490                 snprintf(portstr, sizeof(portstr), " port:%u", ifindex);
 491         else
 492                 portstr[0] = 0;
 493
 494         if (vrfid != VRF_DEFAULT)
 495                 snprintf(vrfstr, sizeof(vrfstr), " vrf:%u", vrfid);
 496         else
 497                 vrfstr[0] = 0;
 498
 499         va_start(vl, fmt);
 500         vsnprintf(buf, sizeof(buf), fmt, vl);
 501         va_end(vl);
 502
 503         log_debug("control-packet: %s [mhop:%s%s%s%s%s]", buf,
 504                   mhop ? "yes" : "no", peerstr, localstr, portstr, vrfstr);
 505 }
 506
 507 int bfd_recv_cb(struct thread *t)
 508 {
 509         int sd = THREAD_FD(t);
 510         struct bfd_session *bfd;
 511         struct bfd_pkt *cp;
 512         bool is_mhop;
 513         ssize_t mlen = 0;
 514         uint8_t ttl = 0;
 515         vrf_id_t vrfid = VRF_DEFAULT;
 516         ifindex_t ifindex = IFINDEX_INTERNAL;
 517         struct sockaddr_any local, peer;
 518         uint8_t msgbuf[1516];
 519
 520         /* Schedule next read. */
 521         bfd_sd_reschedule(sd);
 522
 523         /* Handle echo packets. */
 524         if (sd == bglobal.bg_echo || sd == bglobal.bg_echov6) {
 525                 ptm_bfd_process_echo_pkt(sd);
 526                 return 0;
 527         }
 528
 529         /* Sanitize input/output. */
 530         memset(&local, 0, sizeof(local));
 531         memset(&peer, 0, sizeof(peer));
 532
 533         /* Handle control packets. */
 534         is_mhop = false;
 535         if (sd == bglobal.bg_shop || sd == bglobal.bg_mhop) {
 536                 is_mhop = sd == bglobal.bg_mhop;
 537                 mlen = bfd_recv_ipv4(sd, msgbuf, sizeof(msgbuf), &ttl, &ifindex,
 538                                      &local, &peer);
 539         } else if (sd == bglobal.bg_shop6 || sd == bglobal.bg_mhop6) {
 540                 is_mhop = sd == bglobal.bg_mhop6;
 541                 mlen = bfd_recv_ipv6(sd, msgbuf, sizeof(msgbuf), &ttl, &ifindex,
 542                                      &local, &peer);
 543         }
 544
 545         /* Implement RFC 5880 6.8.6 */
 546         if (mlen < BFD_PKT_LEN) {
 547                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 548                          "too small (%ld bytes)", mlen);
 549                 return 0;
 550         }
 551
 552         /* Validate packet TTL. */
 553         if ((!is_mhop) && (ttl != BFD_TTL_VAL)) {
 554                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 555                          "invalid TTL: %d expected %d", ttl, BFD_TTL_VAL);
 556                 return 0;
 557         }
 558
 559         /*
 560          * Parse the control header for inconsistencies:
 561          * - Invalid version;
 562          * - Bad multiplier configuration;
 563          * - Short packets;
 564          * - Invalid discriminator;
 565          */
 566         cp = (struct bfd_pkt *)(msgbuf);
 567         if (BFD_GETVER(cp->diag) != BFD_VERSION) {
 568                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 569                          "bad version %d", BFD_GETVER(cp->diag));
 570                 return 0;
 571         }
 572
 573         if (cp->detect_mult == 0) {
 574                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 575                          "detect multiplier set to zero");
 576                 return 0;
 577         }
 578
 579         if ((cp->len < BFD_PKT_LEN) || (cp->len > mlen)) {
 580                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid, "too small");
 581                 return 0;
 582         }
 583
 584         if (cp->discrs.my_discr == 0) {
 585                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 586                          "'my discriminator' is zero");
 587                 return 0;
 588         }
 589
 590         /* Find the session that this packet belongs. */
 591         bfd = ptm_bfd_sess_find(cp, &peer, &local, ifindex, vrfid, is_mhop);
 592         if (bfd == NULL) {
 593                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 594                          "no session found");
 595                 return 0;
 596         }
 597
 598         bfd->stats.rx_ctrl_pkt++;
 599
 600         /*
 601          * Multi hop: validate packet TTL.
 602          * Single hop: set local address that received the packet.
 603          */
 604         if (is_mhop) {
 605                 if ((BFD_TTL_VAL - bfd->mh_ttl) > BFD_TTL_VAL) {
 606                         cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 607                                  "exceeded max hop count (expected %d, got %d)",
 608                                  bfd->mh_ttl, BFD_TTL_VAL);
 609                         return 0;
 610                 }
 611         } else if (bfd->local_address.sa_sin.sin_family == AF_UNSPEC) {
 612                 bfd->local_address = local;
 613         }
 614
 615         /*
 616          * If no interface was detected, save the interface where the
 617          * packet came in.
 618          */
 619         if (bfd->ifp == NULL)
 620                 bfd->ifp = if_lookup_by_index(ifindex, vrfid);
 621
 622         /* Log remote discriminator changes. */
 623         if ((bfd->discrs.remote_discr != 0)
 624             && (bfd->discrs.remote_discr != ntohl(cp->discrs.my_discr)))
 625                 cp_debug(is_mhop, &peer, &local, ifindex, vrfid,
 626                          "remote discriminator mismatch (expected %u, got %u)",
 627                          bfd->discrs.remote_discr, ntohl(cp->discrs.my_discr));
 628
 629         bfd->discrs.remote_discr = ntohl(cp->discrs.my_discr);
 630
 631         /* Save remote diagnostics before state switch. */
 632         bfd->remote_diag = cp->diag & BFD_DIAGMASK;
 633
 634         /* Update remote timers settings. */
 635         bfd->remote_timers.desired_min_tx = ntohl(cp->timers.desired_min_tx);
 636         bfd->remote_timers.required_min_rx = ntohl(cp->timers.required_min_rx);
 637         bfd->remote_timers.required_min_echo =
 638                 ntohl(cp->timers.required_min_echo);
 639         bfd->remote_detect_mult = cp->detect_mult;
 640
 641         /* State switch from section 6.2. */
 642         bs_state_handler(bfd, BFD_GETSTATE(cp->flags));
 643
 644         /* RFC 5880, Section 6.5: handle POLL/FINAL negotiation sequence. */
 645         if (bfd->polling && BFD_GETFBIT(cp->flags)) {
 646                 /* Disable pooling. */
 647                 bfd->polling = 0;
 648
 649                 /* Handle poll finalization. */
 650                 bs_final_handler(bfd);
 651         } else {
 652                 /* Received a packet, lets update the receive timer. */
 653                 bfd_recvtimer_update(bfd);
 654         }
 655
 656         /* Handle echo timers changes. */
 657         bs_echo_timer_handler(bfd);
 658
 659         /*
 660          * We've received a packet with the POLL bit set, we must send
 661          * a control packet back with the FINAL bit set.
 662          *
 663          * RFC 5880, Section 6.5.
 664          */
 665         if (BFD_GETPBIT(cp->flags)) {
 666                 /* We are finalizing a poll negotiation. */
 667                 bs_final_handler(bfd);
 668
 669                 /* Send the control packet with the final bit immediately. */
 670                 ptm_bfd_snd(bfd, 1);
 671         }
 672
 673         return 0;
 674 }
 675
 676 /*
 677  * bp_bfd_echo_in: proccesses an BFD echo packet. On TTL == BFD_TTL_VAL
 678  * the packet is looped back or returns the my discriminator ID along
 679  * with the TTL.
 680  *
 681  * Returns -1 on error or loopback or 0 on success.
 682  */
 683 int bp_bfd_echo_in(int sd, uint8_t *ttl, uint32_t *my_discr)
 684 {
 685         struct bfd_echo_pkt *bep;
 686         ssize_t rlen;
 687         struct sockaddr_any local, peer;
 688         ifindex_t ifindex = IFINDEX_INTERNAL;
 689         vrf_id_t vrfid = VRF_DEFAULT;
 690         uint8_t msgbuf[1516];
 691
 692         if (sd == bglobal.bg_echo)
 693                 rlen = bfd_recv_ipv4(sd, msgbuf, sizeof(msgbuf), ttl, &ifindex,
 694                                      &local, &peer);
 695         else
 696                 rlen = bfd_recv_ipv6(sd, msgbuf, sizeof(msgbuf), ttl, &ifindex,
 697                                      &local, &peer);
 698
 699         /* Short packet, better not risk reading it. */
 700         if (rlen < (ssize_t)sizeof(*bep)) {
 701                 cp_debug(false, &peer, &local, ifindex, vrfid,
 702                          "small echo packet");
 703                 return -1;
 704         }
 705
 706         /* Test for loopback. */
 707         if (*ttl == BFD_TTL_VAL) {
 708                 bp_udp_send(sd, *ttl - 1, msgbuf, rlen,
 709                             (struct sockaddr *)&peer,
 710                             (sd == bglobal.bg_echo) ? sizeof(peer.sa_sin)
 711                                                     : sizeof(peer.sa_sin6));
 712                 return -1;
 713         }
 714
 715         /* Read my discriminator from BFD Echo packet. */
 716         bep = (struct bfd_echo_pkt *)msgbuf;
 717         *my_discr = ntohl(bep->my_discr);
 718         if (*my_discr == 0) {
 719                 cp_debug(false, &peer, &local, ifindex, vrfid,
 720                          "invalid echo packet discriminator (zero)");
 721                 return -1;
 722         }
 723
 724         return 0;
 725 }
 726
 727 int bp_udp_send(int sd, uint8_t ttl, uint8_t *data, size_t datalen,
 728                 struct sockaddr *to, socklen_t tolen)
 729 {
 730         struct cmsghdr *cmsg;
 731         ssize_t wlen;
 732         int ttlval = ttl;
 733         bool is_ipv6 = to->sa_family == AF_INET6;
 734         struct msghdr msg;
 735         struct iovec iov[1];
 736         uint8_t msgctl[255];
 737
 738         /* Prepare message data. */
 739         iov[0].iov_base = data;
 740         iov[0].iov_len = datalen;
 741
 742         memset(&msg, 0, sizeof(msg));
 743         memset(msgctl, 0, sizeof(msgctl));
 744         msg.msg_name = to;
 745         msg.msg_namelen = tolen;
 746         msg.msg_iov = iov;
 747         msg.msg_iovlen = 1;
 748
 749         /* Prepare the packet TTL information. */
 750         if (ttl > 0) {
 751                 /* Use ancillary data. */
 752                 msg.msg_control = msgctl;
 753                 msg.msg_controllen = CMSG_LEN(sizeof(ttlval));
 754
 755                 /* Configure the ancillary data. */
 756                 cmsg = CMSG_FIRSTHDR(&msg);
 757                 cmsg->cmsg_len = CMSG_LEN(sizeof(ttlval));
 758                 if (is_ipv6) {
 759                         cmsg->cmsg_level = IPPROTO_IPV6;
 760                         cmsg->cmsg_type = IPV6_HOPLIMIT;
 761                 } else {
 762 #if BFD_LINUX
 763                         cmsg->cmsg_level = IPPROTO_IP;
 764                         cmsg->cmsg_type = IP_TTL;
 765 #else
 766                         /* FreeBSD does not support TTL in ancillary data. */
 767                         msg.msg_control = NULL;
 768                         msg.msg_controllen = 0;
 769
 770                         bp_set_ttl(sd, ttl);
 771 #endif /* BFD_BSD */
 772                 }
 773                 memcpy(CMSG_DATA(cmsg), &ttlval, sizeof(ttlval));
 774         }
 775
 776         /* Send echo back. */
 777         wlen = sendmsg(sd, &msg, 0);
 778         if (wlen <= 0) {
 779                 log_debug("udp-send: loopback failure: (%d) %s", errno, strerror(errno));
 780                 return -1;
 781         } else if (wlen < (ssize_t)datalen) {
 782                 log_debug("udp-send: partial send: %ld expected %ld", wlen,
 783                           datalen);
 784                 return -1;
 785         }
 786
 787         return 0;
 788 }
 789
 790
 791 /*
 792  * Sockets creation.
 793  */
 794
 795
 796 /*
 797  * IPv4 sockets
 798  */
 799 int bp_set_ttl(int sd, uint8_t value)
 800 {
 801         int ttl = value;
 802
 803         if (setsockopt(sd, IPPROTO_IP, IP_TTL, &ttl, sizeof(ttl)) == -1) {
 804                 log_warning("set-ttl: setsockopt(IP_TTL, %d): %s", value,
 805                             strerror(errno));
 806                 return -1;
 807         }
 808
 809         return 0;
 810 }
 811
 812 int bp_set_tos(int sd, uint8_t value)
 813 {
 814         int tos = value;
 815
 816         if (setsockopt(sd, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)) == -1) {
 817                 log_warning("set-tos: setsockopt(IP_TOS, %d): %s", value,
 818                             strerror(errno));
 819                 return -1;
 820         }
 821
 822         return 0;
 823 }
 824
 825 static void bp_set_ipopts(int sd)
 826 {
 827         int rcvttl = BFD_RCV_TTL_VAL;
 828
 829         if (bp_set_ttl(sd, BFD_TTL_VAL) != 0)
 830                 log_fatal("set-ipopts: TTL configuration failed");
 831
 832         if (setsockopt(sd, IPPROTO_IP, IP_RECVTTL, &rcvttl, sizeof(rcvttl))
 833             == -1)
 834                 log_fatal("set-ipopts: setsockopt(IP_RECVTTL, %d): %s", rcvttl,
 835                           strerror(errno));
 836
 837 #ifdef BFD_LINUX
 838         int pktinfo = BFD_PKT_INFO_VAL;
 839
 840         /* Figure out address and interface to do the peer matching. */
 841         if (setsockopt(sd, IPPROTO_IP, IP_PKTINFO, &pktinfo, sizeof(pktinfo))
 842             == -1)
 843                 log_fatal("set-ipopts: setsockopt(IP_PKTINFO, %d): %s", pktinfo,
 844                           strerror(errno));
 845 #endif /* BFD_LINUX */
 846 #ifdef BFD_BSD
 847         int yes = 1;
 848
 849         /* Find out our address for peer matching. */
 850         if (setsockopt(sd, IPPROTO_IP, IP_RECVDSTADDR, &yes, sizeof(yes)) == -1)
 851                 log_fatal("set-ipopts: setsockopt(IP_RECVDSTADDR, %d): %s", yes,
 852                           strerror(errno));
 853
 854         /* Find out interface where the packet came in. */
 855         if (setsockopt_ifindex(AF_INET, sd, yes) == -1)
 856                 log_fatal("set-ipopts: setsockopt_ipv4_ifindex(%d): %s", yes,
 857                           strerror(errno));
 858 #endif /* BFD_BSD */
 859 }
 860
 861 static void bp_bind_ip(int sd, uint16_t port)
 862 {
 863         struct sockaddr_in sin;
 864
 865         memset(&sin, 0, sizeof(sin));
 866         sin.sin_family = AF_INET;
 867         sin.sin_addr.s_addr = htonl(INADDR_ANY);
 868         sin.sin_port = htons(port);
 869         if (bind(sd, (struct sockaddr *)&sin, sizeof(sin)) == -1)
 870                 log_fatal("bind-ip: bind: %s", strerror(errno));
 871 }
 872
 873 int bp_udp_shop(void)
 874 {
 875         int sd;
 876
 877         sd = socket(AF_INET, SOCK_DGRAM, PF_UNSPEC);
 878         if (sd == -1)
 879                 log_fatal("udp-shop: socket: %s", strerror(errno));
 880
 881         bp_set_ipopts(sd);
 882         bp_bind_ip(sd, BFD_DEFDESTPORT);
 883
 884         return sd;
 885 }
 886
 887 int bp_udp_mhop(void)
 888 {
 889         int sd;
 890
 891         sd = socket(AF_INET, SOCK_DGRAM, PF_UNSPEC);
 892         if (sd == -1)
 893                 log_fatal("udp-mhop: socket: %s", strerror(errno));
 894
 895         bp_set_ipopts(sd);
 896         bp_bind_ip(sd, BFD_DEF_MHOP_DEST_PORT);
 897
 898         return sd;
 899 }
 900
 901 int bp_peer_socket(const struct bfd_session *bs)
 902 {
 903         int sd, pcount;
 904         struct sockaddr_in sin;
 905         static int srcPort = BFD_SRCPORTINIT;
 906
 907         sd = socket(AF_INET, SOCK_DGRAM, PF_UNSPEC);
 908         if (sd == -1) {
 909                 log_error("ipv4-new: failed to create socket: %s",
 910                           strerror(errno));
 911                 return -1;
 912         }
 913
 914         /* Set TTL to 255 for all transmitted packets */
 915         if (bp_set_ttl(sd, BFD_TTL_VAL) != 0) {
 916                 close(sd);
 917                 return -1;
 918         }
 919
 920         /* Set TOS to CS6 for all transmitted packets */
 921         if (bp_set_tos(sd, BFD_TOS_VAL) != 0) {
 922                 close(sd);
 923                 return -1;
 924         }
 925
 926         if (bs->key.ifname[0]) {
 927                 if (bp_bind_dev(sd, bs->key.ifname) != 0) {
 928                         close(sd);
 929                         return -1;
 930                 }
 931         } else if (BFD_CHECK_FLAG(bs->flags, BFD_SESS_FLAG_MH)
 932                    && bs->key.vrfname[0]) {
 933                 if (bp_bind_dev(sd, bs->key.vrfname) != 0) {
 934                         close(sd);
 935                         return -1;
 936                 }
 937         }
 938
 939         /* Find an available source port in the proper range */
 940         memset(&sin, 0, sizeof(sin));
 941         sin.sin_family = AF_INET;
 942 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
 943         sin.sin_len = sizeof(sin);
 944 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
 945         memcpy(&sin.sin_addr, &bs->key.local, sizeof(sin.sin_addr));
 946         if (BFD_CHECK_FLAG(bs->flags, BFD_SESS_FLAG_MH) == 0)
 947                 sin.sin_addr.s_addr = INADDR_ANY;
 948
 949         pcount = 0;
 950         do {
 951                 if ((++pcount) > (BFD_SRCPORTMAX - BFD_SRCPORTINIT)) {
 952                         /* Searched all ports, none available */
 953                         log_error("ipv4-new: failed to bind port: %s",
 954                                   strerror(errno));
 955                         close(sd);
 956                         return -1;
 957                 }
 958                 if (srcPort >= BFD_SRCPORTMAX)
 959                         srcPort = BFD_SRCPORTINIT;
 960                 sin.sin_port = htons(srcPort++);
 961         } while (bind(sd, (struct sockaddr *)&sin, sizeof(sin)) < 0);
 962
 963         return sd;
 964 }
 965
 966
 967 /*
 968  * IPv6 sockets
 969  */
 970
 971 int bp_peer_socketv6(const struct bfd_session *bs)
 972 {
 973         int sd, pcount;
 974         struct sockaddr_in6 sin6;
 975         static int srcPort = BFD_SRCPORTINIT;
 976
 977         sd = socket(AF_INET6, SOCK_DGRAM, PF_UNSPEC);
 978         if (sd == -1) {
 979                 log_error("ipv6-new: failed to create socket: %s",
 980                           strerror(errno));
 981                 return -1;
 982         }
 983
 984         /* Set TTL to 255 for all transmitted packets */
 985         if (bp_set_ttlv6(sd, BFD_TTL_VAL) != 0) {
 986                 close(sd);
 987                 return -1;
 988         }
 989
 990         /* Set TOS to CS6 for all transmitted packets */
 991         if (bp_set_tosv6(sd, BFD_TOS_VAL) != 0) {
 992                 close(sd);
 993                 return -1;
 994         }
 995
 996         /* Find an available source port in the proper range */
 997         memset(&sin6, 0, sizeof(sin6));
 998         sin6.sin6_family = AF_INET6;
 999 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
1000         sin6.sin6_len = sizeof(sin6);
1001 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
1002         memcpy(&sin6.sin6_addr, &bs->key.local, sizeof(sin6.sin6_addr));
1003         if (IN6_IS_ADDR_LINKLOCAL(&sin6.sin6_addr))
1004                 sin6.sin6_scope_id = bs->ifp->ifindex;
1005
1006         if (bs->key.ifname[0]) {
1007                 if (bp_bind_dev(sd, bs->key.ifname) != 0) {
1008                         close(sd);
1009                         return -1;
1010                 }
1011         } else if (BFD_CHECK_FLAG(bs->flags, BFD_SESS_FLAG_MH)
1012                    && bs->key.vrfname[0]) {
1013                 if (bp_bind_dev(sd, bs->key.vrfname) != 0) {
1014                         close(sd);
1015                         return -1;
1016                 }
1017         }
1018
1019         pcount = 0;
1020         do {
1021                 if ((++pcount) > (BFD_SRCPORTMAX - BFD_SRCPORTINIT)) {
1022                         /* Searched all ports, none available */
1023                         log_error("ipv6-new: failed to bind port: %s",
1024                                   strerror(errno));
1025                         close(sd);
1026                         return -1;
1027                 }
1028                 if (srcPort >= BFD_SRCPORTMAX)
1029                         srcPort = BFD_SRCPORTINIT;
1030                 sin6.sin6_port = htons(srcPort++);
1031         } while (bind(sd, (struct sockaddr *)&sin6, sizeof(sin6)) < 0);
1032
1033         return sd;
1034 }
1035
1036 int bp_set_ttlv6(int sd, uint8_t value)
1037 {
1038         int ttl = value;
1039
1040         if (setsockopt(sd, IPPROTO_IPV6, IPV6_UNICAST_HOPS, &ttl, sizeof(ttl))
1041             == -1) {
1042                 log_warning("set-ttlv6: setsockopt(IPV6_UNICAST_HOPS, %d): %s",
1043                             value, strerror(errno));
1044                 return -1;
1045         }
1046
1047         return 0;
1048 }
1049
1050 int bp_set_tosv6(int sd, uint8_t value)
1051 {
1052         int tos = value;
1053
1054         if (setsockopt(sd, IPPROTO_IPV6, IPV6_TCLASS, &tos, sizeof(tos))
1055             == -1) {
1056                 log_warning("set-tosv6: setsockopt(IPV6_TCLASS, %d): %s", value,
1057                             strerror(errno));
1058                 return -1;
1059         }
1060
1061         return 0;
1062 }
1063
1064 static void bp_set_ipv6opts(int sd)
1065 {
1066         int ipv6_pktinfo = BFD_IPV6_PKT_INFO_VAL;
1067         int ipv6_only = BFD_IPV6_ONLY_VAL;
1068
1069         if (bp_set_ttlv6(sd, BFD_TTL_VAL) == -1)
1070                 log_fatal("set-ipv6opts: setsockopt(IPV6_UNICAST_HOPS, %d): %s",
1071                           BFD_TTL_VAL, strerror(errno));
1072
1073         if (setsockopt_ipv6_hoplimit(sd, BFD_RCV_TTL_VAL) == -1)
1074                 log_fatal("set-ipv6opts: setsockopt(IPV6_HOPLIMIT, %d): %s",
1075                           BFD_RCV_TTL_VAL, strerror(errno));
1076
1077         if (setsockopt_ipv6_pktinfo(sd, ipv6_pktinfo) == -1)
1078                 log_fatal("set-ipv6opts: setsockopt(IPV6_PKTINFO, %d): %s",
1079                           ipv6_pktinfo, strerror(errno));
1080
1081         if (setsockopt(sd, IPPROTO_IPV6, IPV6_V6ONLY, &ipv6_only,
1082                        sizeof(ipv6_only))
1083             == -1)
1084                 log_fatal("set-ipv6opts: setsockopt(IPV6_V6ONLY, %d): %s",
1085                           ipv6_only, strerror(errno));
1086 }
1087
1088 static void bp_bind_ipv6(int sd, uint16_t port)
1089 {
1090         struct sockaddr_in6 sin6;
1091
1092         memset(&sin6, 0, sizeof(sin6));
1093         sin6.sin6_family = AF_INET6;
1094         sin6.sin6_addr = in6addr_any;
1095         sin6.sin6_port = htons(port);
1096 #ifdef HAVE_STRUCT_SOCKADDR_SA_LEN
1097         sin6.sin6_len = sizeof(sin6);
1098 #endif /* HAVE_STRUCT_SOCKADDR_SA_LEN */
1099         if (bind(sd, (struct sockaddr *)&sin6, sizeof(sin6)) == -1)
1100                 log_fatal("bind-ipv6: bind: %s", strerror(errno));
1101 }
1102
1103 int bp_udp6_shop(void)
1104 {
1105         int sd;
1106
1107         sd = socket(AF_INET6, SOCK_DGRAM, PF_UNSPEC);
1108         if (sd == -1)
1109                 log_fatal("udp6-shop: socket: %s", strerror(errno));
1110
1111         bp_set_ipv6opts(sd);
1112         bp_bind_ipv6(sd, BFD_DEFDESTPORT);
1113
1114         return sd;
1115 }
1116
1117 int bp_udp6_mhop(void)
1118 {
1119         int sd;
1120
1121         sd = socket(AF_INET6, SOCK_DGRAM, PF_UNSPEC);
1122         if (sd == -1)
1123                 log_fatal("udp6-mhop: socket: %s", strerror(errno));
1124
1125         bp_set_ipv6opts(sd);
1126         bp_bind_ipv6(sd, BFD_DEF_MHOP_DEST_PORT);
1127
1128         return sd;
1129 }
1130
1131 int bp_echo_socket(void)
1132 {
1133         int s;
1134
1135         s = socket(AF_INET, SOCK_DGRAM, 0);
1136         if (s == -1)
1137                 log_fatal("echo-socket: socket: %s", strerror(errno));
1138
1139         bp_set_ipopts(s);
1140         bp_bind_ip(s, BFD_DEF_ECHO_PORT);
1141
1142         return s;
1143 }
1144
1145 int bp_echov6_socket(void)
1146 {
1147         int s;
1148
1149         s = socket(AF_INET6, SOCK_DGRAM, 0);
1150         if (s == -1)
1151                 log_fatal("echov6-socket: socket: %s", strerror(errno));
1152
1153         bp_set_ipv6opts(s);
1154         bp_bind_ipv6(s, BFD_DEF_ECHO_PORT);
1155
1156         return s;
1157 }