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