nhrpd/nhrp_peer.c

   1 /* NHRP peer functions
   2  * Copyright (c) 2014-2015 Timo Teräs
   3  *
   4  * This file is free software: you may copy, redistribute and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  */
   9
  10 #ifdef HAVE_CONFIG_H
  11 #include "config.h"
  12 #endif
  13
  14 #include <netinet/if_ether.h>
  15
  16 #include "zebra.h"
  17 #include "memory.h"
  18 #include "thread.h"
  19 #include "hash.h"
  20
  21 #include "nhrpd.h"
  22 #include "nhrp_protocol.h"
  23 #include "os.h"
  24
  25 DEFINE_MTYPE_STATIC(NHRPD, NHRP_PEER, "NHRP peer entry")
  26
  27 struct ipv6hdr {
  28         uint8_t priority_version;
  29         uint8_t flow_lbl[3];
  30         uint16_t payload_len;
  31         uint8_t nexthdr;
  32         uint8_t hop_limit;
  33         struct in6_addr saddr;
  34         struct in6_addr daddr;
  35 };
  36
  37 static void nhrp_packet_debug(struct zbuf *zb, const char *dir);
  38
  39 static void nhrp_peer_check_delete(struct nhrp_peer *p)
  40 {
  41         char buf[2][256];
  42         struct nhrp_interface *nifp = p->ifp->info;
  43
  44         if (p->ref || notifier_active(&p->notifier_list))
  45                 return;
  46
  47         debugf(NHRP_DEBUG_COMMON, "Deleting peer ref:%d remote:%s local:%s",
  48                p->ref,
  49                sockunion2str(&p->vc->remote.nbma, buf[0], sizeof(buf[0])),
  50                sockunion2str(&p->vc->local.nbma, buf[1], sizeof(buf[1])));
  51
  52         THREAD_OFF(p->t_fallback);
  53         hash_release(nifp->peer_hash, p);
  54         nhrp_interface_notify_del(p->ifp, &p->ifp_notifier);
  55         nhrp_vc_notify_del(p->vc, &p->vc_notifier);
  56         XFREE(MTYPE_NHRP_PEER, p);
  57 }
  58
  59 static int nhrp_peer_notify_up(struct thread *t)
  60 {
  61         struct nhrp_peer *p = THREAD_ARG(t);
  62         struct nhrp_vc *vc = p->vc;
  63         struct interface *ifp = p->ifp;
  64         struct nhrp_interface *nifp = ifp->info;
  65
  66         p->t_fallback = NULL;
  67         if (nifp->enabled && (!nifp->ipsec_profile || vc->ipsec)) {
  68                 p->online = 1;
  69                 nhrp_peer_ref(p);
  70                 notifier_call(&p->notifier_list, NOTIFY_PEER_UP);
  71                 nhrp_peer_unref(p);
  72         }
  73
  74         return 0;
  75 }
  76
  77 static void __nhrp_peer_check(struct nhrp_peer *p)
  78 {
  79         struct nhrp_vc *vc = p->vc;
  80         struct interface *ifp = p->ifp;
  81         struct nhrp_interface *nifp = ifp->info;
  82         unsigned online;
  83
  84         online = nifp->enabled && (!nifp->ipsec_profile || vc->ipsec);
  85         if (p->online != online) {
  86                 THREAD_OFF(p->t_fallback);
  87                 if (online && notifier_active(&p->notifier_list)) {
  88                         /* If we requested the IPsec connection, delay
  89                          * the up notification a bit to allow things
  90                          * settle down. This allows IKE to install
  91                          * SPDs and SAs. */
  92                         thread_add_timer_msec(master, nhrp_peer_notify_up, p,
  93                                               50, &p->t_fallback);
  94                 } else {
  95                         nhrp_peer_ref(p);
  96                         p->online = online;
  97                         if (online) {
  98                                 notifier_call(&p->notifier_list,
  99                                               NOTIFY_PEER_UP);
 100                         } else {
 101                                 p->requested = p->fallback_requested = 0;
 102                                 notifier_call(&p->notifier_list,
 103                                               NOTIFY_PEER_DOWN);
 104                         }
 105                         nhrp_peer_unref(p);
 106                 }
 107         }
 108 }
 109
 110 static void nhrp_peer_vc_notify(struct notifier_block *n, unsigned long cmd)
 111 {
 112         struct nhrp_peer *p = container_of(n, struct nhrp_peer, vc_notifier);
 113
 114         switch (cmd) {
 115         case NOTIFY_VC_IPSEC_CHANGED:
 116                 __nhrp_peer_check(p);
 117                 break;
 118         case NOTIFY_VC_IPSEC_UPDATE_NBMA:
 119                 nhrp_peer_ref(p);
 120                 notifier_call(&p->notifier_list, NOTIFY_PEER_NBMA_CHANGING);
 121                 nhrp_peer_unref(p);
 122                 break;
 123         }
 124 }
 125
 126 static void nhrp_peer_ifp_notify(struct notifier_block *n, unsigned long cmd)
 127 {
 128         struct nhrp_peer *p = container_of(n, struct nhrp_peer, ifp_notifier);
 129         struct nhrp_interface *nifp;
 130         struct nhrp_vc *vc;
 131
 132         nhrp_peer_ref(p);
 133         switch (cmd) {
 134         case NOTIFY_INTERFACE_UP:
 135         case NOTIFY_INTERFACE_DOWN:
 136                 __nhrp_peer_check(p);
 137                 break;
 138         case NOTIFY_INTERFACE_NBMA_CHANGED:
 139                 /* Source NBMA changed, rebind to new VC */
 140                 nifp = p->ifp->info;
 141                 vc = nhrp_vc_get(&nifp->nbma, &p->vc->remote.nbma, 1);
 142                 if (vc && p->vc != vc) {
 143                         nhrp_vc_notify_del(p->vc, &p->vc_notifier);
 144                         p->vc = vc;
 145                         nhrp_vc_notify_add(p->vc, &p->vc_notifier,
 146                                            nhrp_peer_vc_notify);
 147                         __nhrp_peer_check(p);
 148                 }
 149                 /* fallthru */ /* to post config update */
 150         case NOTIFY_INTERFACE_ADDRESS_CHANGED:
 151                 notifier_call(&p->notifier_list, NOTIFY_PEER_IFCONFIG_CHANGED);
 152                 break;
 153         case NOTIFY_INTERFACE_MTU_CHANGED:
 154                 notifier_call(&p->notifier_list, NOTIFY_PEER_MTU_CHANGED);
 155                 break;
 156         }
 157         nhrp_peer_unref(p);
 158 }
 159
 160 static unsigned int nhrp_peer_key(const void *peer_data)
 161 {
 162         const struct nhrp_peer *p = peer_data;
 163         return sockunion_hash(&p->vc->remote.nbma);
 164 }
 165
 166 static bool nhrp_peer_cmp(const void *cache_data, const void *key_data)
 167 {
 168         const struct nhrp_peer *a = cache_data;
 169         const struct nhrp_peer *b = key_data;
 170
 171         return a->ifp == b->ifp && a->vc == b->vc;
 172 }
 173
 174 static void *nhrp_peer_create(void *data)
 175 {
 176         struct nhrp_peer *p, *key = data;
 177
 178         p = XMALLOC(MTYPE_NHRP_PEER, sizeof(*p));
 179
 180         *p = (struct nhrp_peer){
 181                 .ref = 0,
 182                 .ifp = key->ifp,
 183                 .vc = key->vc,
 184                 .notifier_list =
 185                 NOTIFIER_LIST_INITIALIZER(&p->notifier_list),
 186         };
 187         nhrp_vc_notify_add(p->vc, &p->vc_notifier, nhrp_peer_vc_notify);
 188         nhrp_interface_notify_add(p->ifp, &p->ifp_notifier,
 189                                   nhrp_peer_ifp_notify);
 190
 191         return p;
 192 }
 193
 194 static void do_peer_hash_free(struct hash_bucket *hb,
 195                               void *arg __attribute__((__unused__)))
 196 {
 197         struct nhrp_peer *p = hb->data;
 198         nhrp_peer_check_delete(p);
 199 }
 200
 201 void nhrp_peer_interface_del(struct interface *ifp)
 202 {
 203         struct nhrp_interface *nifp = ifp->info;
 204
 205         debugf(NHRP_DEBUG_COMMON, "Cleaning up undeleted peer entries (%lu)",
 206                nifp->peer_hash ? nifp->peer_hash->count : 0);
 207
 208         if (nifp->peer_hash) {
 209                 hash_iterate(nifp->peer_hash, do_peer_hash_free, NULL);
 210                 assert(nifp->peer_hash->count == 0);
 211                 hash_free(nifp->peer_hash);
 212         }
 213 }
 214
 215 struct nhrp_peer *nhrp_peer_get(struct interface *ifp,
 216                                 const union sockunion *remote_nbma)
 217 {
 218         struct nhrp_interface *nifp = ifp->info;
 219         struct nhrp_peer key, *p;
 220         struct nhrp_vc *vc;
 221
 222         if (!nifp->peer_hash) {
 223                 nifp->peer_hash = hash_create(nhrp_peer_key, nhrp_peer_cmp,
 224                                               "NHRP Peer Hash");
 225                 if (!nifp->peer_hash)
 226                         return NULL;
 227         }
 228
 229         vc = nhrp_vc_get(&nifp->nbma, remote_nbma, 1);
 230         if (!vc)
 231                 return NULL;
 232
 233         key.ifp = ifp;
 234         key.vc = vc;
 235
 236         p = hash_get(nifp->peer_hash, &key, nhrp_peer_create);
 237         nhrp_peer_ref(p);
 238         if (p->ref == 1)
 239                 __nhrp_peer_check(p);
 240
 241         return p;
 242 }
 243
 244 struct nhrp_peer *nhrp_peer_ref(struct nhrp_peer *p)
 245 {
 246         if (p)
 247                 p->ref++;
 248         return p;
 249 }
 250
 251 void nhrp_peer_unref(struct nhrp_peer *p)
 252 {
 253         if (p) {
 254                 p->ref--;
 255                 nhrp_peer_check_delete(p);
 256         }
 257 }
 258
 259 static int nhrp_peer_request_timeout(struct thread *t)
 260 {
 261         struct nhrp_peer *p = THREAD_ARG(t);
 262         struct nhrp_vc *vc = p->vc;
 263         struct interface *ifp = p->ifp;
 264         struct nhrp_interface *nifp = ifp->info;
 265
 266         p->t_fallback = NULL;
 267
 268         if (p->online)
 269                 return 0;
 270
 271         if (nifp->ipsec_fallback_profile && !p->prio
 272             && !p->fallback_requested) {
 273                 p->fallback_requested = 1;
 274                 vici_request_vc(nifp->ipsec_fallback_profile, &vc->local.nbma,
 275                                 &vc->remote.nbma, p->prio);
 276                 thread_add_timer(master, nhrp_peer_request_timeout, p, 30,
 277                                  &p->t_fallback);
 278         } else {
 279                 p->requested = p->fallback_requested = 0;
 280         }
 281
 282         return 0;
 283 }
 284
 285 int nhrp_peer_check(struct nhrp_peer *p, int establish)
 286 {
 287         struct nhrp_vc *vc = p->vc;
 288         struct interface *ifp = p->ifp;
 289         struct nhrp_interface *nifp = ifp->info;
 290
 291         if (p->online)
 292                 return 1;
 293         if (!establish)
 294                 return 0;
 295         if (p->requested)
 296                 return 0;
 297         if (!nifp->ipsec_profile)
 298                 return 0;
 299         if (sockunion_family(&vc->local.nbma) == AF_UNSPEC)
 300                 return 0;
 301         if (vc->ipsec)
 302                 return 1;
 303
 304         p->prio = establish > 1;
 305         p->requested = 1;
 306         vici_request_vc(nifp->ipsec_profile, &vc->local.nbma, &vc->remote.nbma,
 307                         p->prio);
 308         thread_add_timer(master, nhrp_peer_request_timeout, p,
 309                          (nifp->ipsec_fallback_profile && !p->prio) ? 15 : 30,
 310                          &p->t_fallback);
 311
 312         return 0;
 313 }
 314
 315 void nhrp_peer_notify_add(struct nhrp_peer *p, struct notifier_block *n,
 316                           notifier_fn_t fn)
 317 {
 318         notifier_add(n, &p->notifier_list, fn);
 319 }
 320
 321 void nhrp_peer_notify_del(struct nhrp_peer *p, struct notifier_block *n)
 322 {
 323         notifier_del(n);
 324         nhrp_peer_check_delete(p);
 325 }
 326
 327 void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb)
 328 {
 329         char buf[2][256];
 330
 331         nhrp_packet_debug(zb, "Send");
 332
 333         if (!p->online)
 334                 return;
 335
 336         debugf(NHRP_DEBUG_KERNEL, "PACKET: Send %s -> %s",
 337                sockunion2str(&p->vc->local.nbma, buf[0], sizeof(buf[0])),
 338                sockunion2str(&p->vc->remote.nbma, buf[1], sizeof(buf[1])));
 339
 340         os_sendmsg(zb->head, zbuf_used(zb), p->ifp->ifindex,
 341                    sockunion_get_addr(&p->vc->remote.nbma),
 342                    sockunion_get_addrlen(&p->vc->remote.nbma));
 343         zbuf_reset(zb);
 344 }
 345
 346 static void nhrp_handle_resolution_req(struct nhrp_packet_parser *pp)
 347 {
 348         struct interface *ifp = pp->ifp;
 349         struct zbuf *zb, payload;
 350         struct nhrp_packet_header *hdr;
 351         struct nhrp_cie_header *cie;
 352         struct nhrp_extension_header *ext;
 353         struct nhrp_cache *c;
 354         union sockunion cie_nbma, cie_proto, *proto_addr, *nbma_addr;
 355         int holdtime, prefix_len, hostprefix_len;
 356         struct nhrp_interface *nifp = ifp->info;
 357         struct nhrp_peer *peer;
 358         size_t paylen;
 359         char buf[SU_ADDRSTRLEN];
 360
 361         if (!(pp->if_ad->flags & NHRP_IFF_SHORTCUT)) {
 362                 debugf(NHRP_DEBUG_COMMON, "Shortcuts disabled");
 363                 /* FIXME: Send error indication? */
 364                 return;
 365         }
 366
 367         if (pp->if_ad->network_id && pp->route_type == NHRP_ROUTE_OFF_NBMA
 368             && pp->route_prefix.prefixlen < 8) {
 369                 debugf(NHRP_DEBUG_COMMON,
 370                        "Shortcut to more generic than /8 dropped");
 371                 return;
 372         }
 373
 374         debugf(NHRP_DEBUG_COMMON, "Parsing and replying to Resolution Req");
 375
 376         if (nhrp_route_address(ifp, &pp->src_proto, NULL, &peer)
 377             != NHRP_ROUTE_NBMA_NEXTHOP)
 378                 return;
 379
 380         /* Copy payload CIE */
 381         hostprefix_len = 8 * sockunion_get_addrlen(&pp->if_ad->addr);
 382         paylen = zbuf_used(&pp->payload);
 383         debugf(NHRP_DEBUG_COMMON, "shortcut res_rep: paylen %zu", paylen);
 384
 385         while ((cie = nhrp_cie_pull(&pp->payload, pp->hdr, &cie_nbma,
 386                                     &cie_proto))
 387                != NULL) {
 388                 prefix_len = cie->prefix_length;
 389                 debugf(NHRP_DEBUG_COMMON,
 390                        "shortcut res_rep: parsing CIE with prefixlen=%u",
 391                        prefix_len);
 392                 if (prefix_len == 0 || prefix_len >= hostprefix_len)
 393                         prefix_len = hostprefix_len;
 394
 395                 if (prefix_len != hostprefix_len
 396                     && !(pp->hdr->flags
 397                          & htons(NHRP_FLAG_REGISTRATION_UNIQUE))) {
 398                         cie->code = NHRP_CODE_BINDING_NON_UNIQUE;
 399                         continue;
 400                 }
 401
 402                 /* We currently support only unique prefix registrations */
 403                 if (prefix_len != hostprefix_len) {
 404                         cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 405                         continue;
 406                 }
 407
 408                 proto_addr = (sockunion_family(&cie_proto) == AF_UNSPEC)
 409                                      ? &pp->src_proto
 410                                      : &cie_proto;
 411                 nbma_addr = (sockunion_family(&cie_nbma) == AF_UNSPEC)
 412                                     ? &pp->src_nbma
 413                                     : &cie_nbma;
 414
 415                 holdtime = htons(cie->holding_time);
 416                 debugf(NHRP_DEBUG_COMMON,
 417                        "shortcut res_rep: holdtime is %u (if 0, using %u)",
 418                        holdtime, pp->if_ad->holdtime);
 419                 if (!holdtime)
 420                         holdtime = pp->if_ad->holdtime;
 421
 422                 c = nhrp_cache_get(ifp, proto_addr, 1);
 423                 if (!c) {
 424                         debugf(NHRP_DEBUG_COMMON,
 425                                "shortcut res_rep: no cache found");
 426                         cie->code = NHRP_CODE_INSUFFICIENT_RESOURCES;
 427                         continue;
 428                 }
 429                 if (nbma_addr)
 430                         sockunion2str(nbma_addr, buf, sizeof(buf));
 431
 432                 debugf(NHRP_DEBUG_COMMON,
 433                        "shortcut res_rep: updating binding for nmba addr %s",
 434                        nbma_addr ? buf : "(NULL)");
 435                 if (!nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC, holdtime,
 436                                                nhrp_peer_ref(pp->peer),
 437                                                htons(cie->mtu), nbma_addr)) {
 438                         cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 439                         continue;
 440                 }
 441
 442                 cie->code = NHRP_CODE_SUCCESS;
 443         }
 444
 445         /* Create reply */
 446         zb = zbuf_alloc(1500);
 447         hdr = nhrp_packet_push(zb, NHRP_PACKET_RESOLUTION_REPLY, &pp->src_nbma,
 448                                &pp->src_proto, &pp->dst_proto);
 449
 450         /* Copied information from request */
 451         hdr->flags = pp->hdr->flags
 452                      & htons(NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER
 453                              | NHRP_FLAG_RESOLUTION_SOURCE_STABLE);
 454         hdr->flags |= htons(NHRP_FLAG_RESOLUTION_DESTINATION_STABLE
 455                             | NHRP_FLAG_RESOLUTION_AUTHORATIVE);
 456         hdr->u.request_id = pp->hdr->u.request_id;
 457
 458         /* CIE payload for the reply packet */
 459         cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma,
 460                             &pp->if_ad->addr);
 461         cie->holding_time = htons(pp->if_ad->holdtime);
 462         cie->mtu = htons(pp->if_ad->mtu);
 463         if (pp->if_ad->network_id && pp->route_type == NHRP_ROUTE_OFF_NBMA)
 464                 cie->prefix_length = pp->route_prefix.prefixlen;
 465         else
 466                 cie->prefix_length =
 467                         8 * sockunion_get_addrlen(&pp->if_ad->addr);
 468
 469         /* Handle extensions */
 470         while ((ext = nhrp_ext_pull(&pp->extensions, &payload)) != NULL) {
 471                 switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
 472                 case NHRP_EXTENSION_NAT_ADDRESS:
 473                         if (sockunion_family(&nifp->nat_nbma) == AF_UNSPEC)
 474                                 break;
 475                         ext = nhrp_ext_push(zb, hdr,
 476                                             NHRP_EXTENSION_NAT_ADDRESS);
 477                         if (!ext)
 478                                 goto err;
 479                         cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
 480                                             &nifp->nat_nbma, &pp->if_ad->addr);
 481                         if (!cie)
 482                                 goto err;
 483                         nhrp_ext_complete(zb, ext);
 484                         break;
 485                 default:
 486                         if (nhrp_ext_reply(zb, hdr, ifp, ext, &payload) < 0)
 487                                 goto err;
 488                         break;
 489                 }
 490         }
 491
 492         nhrp_packet_complete(zb, hdr);
 493         nhrp_peer_send(peer, zb);
 494 err:
 495         nhrp_peer_unref(peer);
 496         zbuf_free(zb);
 497 }
 498
 499 static void nhrp_handle_registration_request(struct nhrp_packet_parser *p)
 500 {
 501         struct interface *ifp = p->ifp;
 502         struct zbuf *zb, payload;
 503         struct nhrp_packet_header *hdr;
 504         struct nhrp_cie_header *cie;
 505         struct nhrp_extension_header *ext;
 506         struct nhrp_cache *c;
 507         union sockunion cie_nbma, cie_proto, *proto_addr, *nbma_addr,
 508                 *nbma_natoa;
 509         int holdtime, prefix_len, hostprefix_len, natted = 0;
 510         size_t paylen;
 511         void *pay;
 512
 513         debugf(NHRP_DEBUG_COMMON, "Parsing and replying to Registration Req");
 514         hostprefix_len = 8 * sockunion_get_addrlen(&p->if_ad->addr);
 515
 516         if (!sockunion_same(&p->src_nbma, &p->peer->vc->remote.nbma))
 517                 natted = 1;
 518
 519         /* Create reply */
 520         zb = zbuf_alloc(1500);
 521         hdr = nhrp_packet_push(zb, NHRP_PACKET_REGISTRATION_REPLY, &p->src_nbma,
 522                                &p->src_proto, &p->if_ad->addr);
 523
 524         /* Copied information from request */
 525         hdr->flags = p->hdr->flags & htons(NHRP_FLAG_REGISTRATION_UNIQUE
 526                                            | NHRP_FLAG_REGISTRATION_NAT);
 527         hdr->u.request_id = p->hdr->u.request_id;
 528
 529         /* Copy payload CIEs */
 530         paylen = zbuf_used(&p->payload);
 531         pay = zbuf_pushn(zb, paylen);
 532         if (!pay)
 533                 goto err;
 534         memcpy(pay, zbuf_pulln(&p->payload, paylen), paylen);
 535         zbuf_init(&payload, pay, paylen, paylen);
 536
 537         while ((cie = nhrp_cie_pull(&payload, hdr, &cie_nbma, &cie_proto))
 538                != NULL) {
 539                 prefix_len = cie->prefix_length;
 540                 if (prefix_len == 0 || prefix_len >= hostprefix_len)
 541                         prefix_len = hostprefix_len;
 542
 543                 if (prefix_len != hostprefix_len
 544                     && !(p->hdr->flags
 545                          & htons(NHRP_FLAG_REGISTRATION_UNIQUE))) {
 546                         cie->code = NHRP_CODE_BINDING_NON_UNIQUE;
 547                         continue;
 548                 }
 549
 550                 /* We currently support only unique prefix registrations */
 551                 if (prefix_len != hostprefix_len) {
 552                         cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 553                         continue;
 554                 }
 555
 556                 proto_addr = (sockunion_family(&cie_proto) == AF_UNSPEC)
 557                                      ? &p->src_proto
 558                                      : &cie_proto;
 559                 nbma_addr = (sockunion_family(&cie_nbma) == AF_UNSPEC)
 560                                     ? &p->src_nbma
 561                                     : &cie_nbma;
 562                 nbma_natoa = NULL;
 563                 if (natted) {
 564                         nbma_natoa = nbma_addr;
 565                 }
 566
 567                 holdtime = htons(cie->holding_time);
 568                 if (!holdtime)
 569                         holdtime = p->if_ad->holdtime;
 570
 571                 c = nhrp_cache_get(ifp, proto_addr, 1);
 572                 if (!c) {
 573                         cie->code = NHRP_CODE_INSUFFICIENT_RESOURCES;
 574                         continue;
 575                 }
 576
 577                 if (!nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC, holdtime,
 578                                                nhrp_peer_ref(p->peer),
 579                                                htons(cie->mtu), nbma_natoa)) {
 580                         cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 581                         continue;
 582                 }
 583
 584                 cie->code = NHRP_CODE_SUCCESS;
 585         }
 586
 587         /* Handle extensions */
 588         while ((ext = nhrp_ext_pull(&p->extensions, &payload)) != NULL) {
 589                 switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
 590                 case NHRP_EXTENSION_NAT_ADDRESS:
 591                         ext = nhrp_ext_push(zb, hdr,
 592                                             NHRP_EXTENSION_NAT_ADDRESS);
 593                         if (!ext)
 594                                 goto err;
 595                         zbuf_copy(zb, &payload, zbuf_used(&payload));
 596                         if (natted) {
 597                                 nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
 598                                               &p->peer->vc->remote.nbma,
 599                                               &p->src_proto);
 600                         }
 601                         nhrp_ext_complete(zb, ext);
 602                         break;
 603                 default:
 604                         if (nhrp_ext_reply(zb, hdr, ifp, ext, &payload) < 0)
 605                                 goto err;
 606                         break;
 607                 }
 608         }
 609
 610         nhrp_packet_complete(zb, hdr);
 611         nhrp_peer_send(p->peer, zb);
 612 err:
 613         zbuf_free(zb);
 614 }
 615
 616 static int parse_ether_packet(struct zbuf *zb, uint16_t protocol_type,
 617                               union sockunion *src, union sockunion *dst)
 618 {
 619         switch (protocol_type) {
 620         case ETH_P_IP: {
 621                 struct iphdr *iph = zbuf_pull(zb, struct iphdr);
 622                 if (iph) {
 623                         if (src)
 624                                 sockunion_set(src, AF_INET,
 625                                               (uint8_t *)&iph->saddr,
 626                                               sizeof(iph->saddr));
 627                         if (dst)
 628                                 sockunion_set(dst, AF_INET,
 629                                               (uint8_t *)&iph->daddr,
 630                                               sizeof(iph->daddr));
 631                 }
 632         } break;
 633         case ETH_P_IPV6: {
 634                 struct ipv6hdr *iph = zbuf_pull(zb, struct ipv6hdr);
 635                 if (iph) {
 636                         if (src)
 637                                 sockunion_set(src, AF_INET6,
 638                                               (uint8_t *)&iph->saddr,
 639                                               sizeof(iph->saddr));
 640                         if (dst)
 641                                 sockunion_set(dst, AF_INET6,
 642                                               (uint8_t *)&iph->daddr,
 643                                               sizeof(iph->daddr));
 644                 }
 645         } break;
 646         default:
 647                 return 0;
 648         }
 649         return 1;
 650 }
 651
 652 void nhrp_peer_send_indication(struct interface *ifp, uint16_t protocol_type,
 653                                struct zbuf *pkt)
 654 {
 655         union sockunion dst;
 656         struct zbuf *zb, payload;
 657         struct nhrp_interface *nifp = ifp->info;
 658         struct nhrp_afi_data *if_ad;
 659         struct nhrp_packet_header *hdr;
 660         struct nhrp_peer *p;
 661         char buf[2][SU_ADDRSTRLEN];
 662
 663         if (!nifp->enabled)
 664                 return;
 665
 666         payload = *pkt;
 667         if (!parse_ether_packet(&payload, protocol_type, &dst, NULL))
 668                 return;
 669
 670         if (nhrp_route_address(ifp, &dst, NULL, &p) != NHRP_ROUTE_NBMA_NEXTHOP)
 671                 return;
 672
 673         if_ad = &nifp->afi[family2afi(sockunion_family(&dst))];
 674         if (!(if_ad->flags & NHRP_IFF_REDIRECT)) {
 675                 debugf(NHRP_DEBUG_COMMON,
 676                        "Send Traffic Indication to %s about packet to %s ignored",
 677                        sockunion2str(&p->vc->remote.nbma, buf[0],
 678                                      sizeof(buf[0])),
 679                        sockunion2str(&dst, buf[1], sizeof(buf[1])));
 680                 return;
 681         }
 682
 683         debugf(NHRP_DEBUG_COMMON,
 684                "Send Traffic Indication to %s (online=%d) about packet to %s",
 685                sockunion2str(&p->vc->remote.nbma, buf[0], sizeof(buf[0])),
 686                p->online, sockunion2str(&dst, buf[1], sizeof(buf[1])));
 687
 688         /* Create reply */
 689         zb = zbuf_alloc(1500);
 690         hdr = nhrp_packet_push(zb, NHRP_PACKET_TRAFFIC_INDICATION, &nifp->nbma,
 691                                &if_ad->addr, &dst);
 692         hdr->hop_count = 0;
 693
 694         /* Payload is the packet causing indication */
 695         zbuf_copy(zb, pkt, zbuf_used(pkt));
 696         nhrp_packet_complete(zb, hdr);
 697         nhrp_peer_send(p, zb);
 698         nhrp_peer_unref(p);
 699         zbuf_free(zb);
 700 }
 701
 702 static void nhrp_handle_error_ind(struct nhrp_packet_parser *pp)
 703 {
 704         struct zbuf origmsg = pp->payload;
 705         struct nhrp_packet_header *hdr;
 706         struct nhrp_reqid *reqid;
 707         union sockunion src_nbma, src_proto, dst_proto;
 708         char buf[2][SU_ADDRSTRLEN];
 709
 710         hdr = nhrp_packet_pull(&origmsg, &src_nbma, &src_proto, &dst_proto);
 711         if (!hdr)
 712                 return;
 713
 714         debugf(NHRP_DEBUG_COMMON,
 715                "Error Indication from %s about packet to %s ignored",
 716                sockunion2str(&pp->src_proto, buf[0], sizeof(buf[0])),
 717                sockunion2str(&dst_proto, buf[1], sizeof(buf[1])));
 718
 719         reqid = nhrp_reqid_lookup(&nhrp_packet_reqid, htonl(hdr->u.request_id));
 720         if (reqid)
 721                 reqid->cb(reqid, pp);
 722 }
 723
 724 static void nhrp_handle_traffic_ind(struct nhrp_packet_parser *p)
 725 {
 726         union sockunion dst;
 727         char buf[2][SU_ADDRSTRLEN];
 728
 729         if (!parse_ether_packet(&p->payload, htons(p->hdr->protocol_type), NULL,
 730                                 &dst))
 731                 return;
 732
 733         debugf(NHRP_DEBUG_COMMON,
 734                "Traffic Indication from %s about packet to %s: %s",
 735                sockunion2str(&p->src_proto, buf[0], sizeof(buf[0])),
 736                sockunion2str(&dst, buf[1], sizeof(buf[1])),
 737                (p->if_ad->flags & NHRP_IFF_SHORTCUT) ? "trying shortcut"
 738                                                      : "ignored");
 739
 740         if (p->if_ad->flags & NHRP_IFF_SHORTCUT)
 741                 nhrp_shortcut_initiate(&dst);
 742 }
 743
 744 enum packet_type_t {
 745         PACKET_UNKNOWN = 0,
 746         PACKET_REQUEST,
 747         PACKET_REPLY,
 748         PACKET_INDICATION,
 749 };
 750
 751 static struct {
 752         enum packet_type_t type;
 753         const char *name;
 754         void (*handler)(struct nhrp_packet_parser *);
 755 } packet_types[] = {[0] =
 756                             {
 757                                     .type = PACKET_UNKNOWN,
 758                                     .name = "UNKNOWN",
 759                             },
 760                     [NHRP_PACKET_RESOLUTION_REQUEST] =
 761                             {
 762                                     .type = PACKET_REQUEST,
 763                                     .name = "Resolution-Request",
 764                                     .handler = nhrp_handle_resolution_req,
 765                             },
 766                     [NHRP_PACKET_RESOLUTION_REPLY] =
 767                             {
 768                                     .type = PACKET_REPLY,
 769                                     .name = "Resolution-Reply",
 770                             },
 771                     [NHRP_PACKET_REGISTRATION_REQUEST] =
 772                             {
 773                                     .type = PACKET_REQUEST,
 774                                     .name = "Registration-Request",
 775                                     .handler = nhrp_handle_registration_request,
 776                             },
 777                     [NHRP_PACKET_REGISTRATION_REPLY] =
 778                             {
 779                                     .type = PACKET_REPLY,
 780                                     .name = "Registration-Reply",
 781                             },
 782                     [NHRP_PACKET_PURGE_REQUEST] =
 783                             {
 784                                     .type = PACKET_REQUEST,
 785                                     .name = "Purge-Request",
 786                             },
 787                     [NHRP_PACKET_PURGE_REPLY] =
 788                             {
 789                                     .type = PACKET_REPLY,
 790                                     .name = "Purge-Reply",
 791                             },
 792                     [NHRP_PACKET_ERROR_INDICATION] =
 793                             {
 794                                     .type = PACKET_INDICATION,
 795                                     .name = "Error-Indication",
 796                                     .handler = nhrp_handle_error_ind,
 797                             },
 798                     [NHRP_PACKET_TRAFFIC_INDICATION] = {
 799                             .type = PACKET_INDICATION,
 800                             .name = "Traffic-Indication",
 801                             .handler = nhrp_handle_traffic_ind,
 802                     }};
 803
 804 static void nhrp_peer_forward(struct nhrp_peer *p,
 805                               struct nhrp_packet_parser *pp)
 806 {
 807         struct zbuf *zb, extpl;
 808         struct nhrp_packet_header *hdr;
 809         struct nhrp_extension_header *ext, *dst;
 810         struct nhrp_cie_header *cie;
 811         struct nhrp_interface *nifp = pp->ifp->info;
 812         struct nhrp_afi_data *if_ad = pp->if_ad;
 813         union sockunion cie_nbma, cie_protocol;
 814         uint16_t type, len;
 815
 816         if (pp->hdr->hop_count == 0)
 817                 return;
 818
 819         /* Create forward packet - copy header */
 820         zb = zbuf_alloc(1500);
 821         hdr = nhrp_packet_push(zb, pp->hdr->type, &pp->src_nbma, &pp->src_proto,
 822                                &pp->dst_proto);
 823         hdr->flags = pp->hdr->flags;
 824         hdr->hop_count = pp->hdr->hop_count - 1;
 825         hdr->u.request_id = pp->hdr->u.request_id;
 826
 827         /* Copy payload */
 828         zbuf_copy(zb, &pp->payload, zbuf_used(&pp->payload));
 829
 830         /* Copy extensions */
 831         while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
 832                 type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
 833                 len = htons(ext->length);
 834
 835                 if (type == NHRP_EXTENSION_END)
 836                         break;
 837
 838                 dst = nhrp_ext_push(zb, hdr, htons(ext->type));
 839                 if (!dst)
 840                         goto err;
 841
 842                 switch (type) {
 843                 case NHRP_EXTENSION_FORWARD_TRANSIT_NHS:
 844                 case NHRP_EXTENSION_REVERSE_TRANSIT_NHS:
 845                         zbuf_put(zb, extpl.head, len);
 846                         if ((type == NHRP_EXTENSION_REVERSE_TRANSIT_NHS)
 847                             == (packet_types[hdr->type].type == PACKET_REPLY)) {
 848                                 /* Check NHS list for forwarding loop */
 849                                 while (nhrp_cie_pull(&extpl, pp->hdr,
 850                                                      &cie_nbma,
 851                                                      &cie_protocol) != NULL) {
 852                                         if (sockunion_same(&p->vc->remote.nbma,
 853                                                            &cie_nbma))
 854                                                 goto err;
 855                                 }
 856                                 /* Append our selves to the list */
 857                                 cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
 858                                                     &nifp->nbma, &if_ad->addr);
 859                                 if (!cie)
 860                                         goto err;
 861                                 cie->holding_time = htons(if_ad->holdtime);
 862                         }
 863                         break;
 864                 default:
 865                         if (htons(ext->type) & NHRP_EXTENSION_FLAG_COMPULSORY)
 866                                 /* FIXME: RFC says to just copy, but not
 867                                  * append our selves to the transit NHS list */
 868                                 goto err;
 869                 /* fallthru */
 870                 case NHRP_EXTENSION_RESPONDER_ADDRESS:
 871                         /* Supported compulsory extensions, and any
 872                          * non-compulsory that is not explicitly handled,
 873                          * should be just copied. */
 874                         zbuf_copy(zb, &extpl, len);
 875                         break;
 876                 }
 877                 nhrp_ext_complete(zb, dst);
 878         }
 879
 880         nhrp_packet_complete(zb, hdr);
 881         nhrp_peer_send(p, zb);
 882         zbuf_free(zb);
 883         return;
 884 err:
 885         nhrp_packet_debug(pp->pkt, "FWD-FAIL");
 886         zbuf_free(zb);
 887 }
 888
 889 static void nhrp_packet_debug(struct zbuf *zb, const char *dir)
 890 {
 891         char buf[2][SU_ADDRSTRLEN];
 892         union sockunion src_nbma, src_proto, dst_proto;
 893         struct nhrp_packet_header *hdr;
 894         struct zbuf zhdr;
 895         int reply;
 896
 897         if (likely(!(debug_flags & NHRP_DEBUG_COMMON)))
 898                 return;
 899
 900         zbuf_init(&zhdr, zb->buf, zb->tail - zb->buf, zb->tail - zb->buf);
 901         hdr = nhrp_packet_pull(&zhdr, &src_nbma, &src_proto, &dst_proto);
 902
 903         sockunion2str(&src_proto, buf[0], sizeof(buf[0]));
 904         sockunion2str(&dst_proto, buf[1], sizeof(buf[1]));
 905
 906         reply = packet_types[hdr->type].type == PACKET_REPLY;
 907         debugf(NHRP_DEBUG_COMMON, "%s %s(%d) %s -> %s", dir,
 908                (packet_types[hdr->type].name ? packet_types[hdr->type].name
 909                                              : "Unknown"),
 910                hdr->type, reply ? buf[1] : buf[0], reply ? buf[0] : buf[1]);
 911 }
 912
 913 static int proto2afi(uint16_t proto)
 914 {
 915         switch (proto) {
 916         case ETH_P_IP:
 917                 return AFI_IP;
 918         case ETH_P_IPV6:
 919                 return AFI_IP6;
 920         }
 921         return AF_UNSPEC;
 922 }
 923
 924 struct nhrp_route_info {
 925         int local;
 926         struct interface *ifp;
 927         struct nhrp_vc *vc;
 928 };
 929
 930 void nhrp_peer_recv(struct nhrp_peer *p, struct zbuf *zb)
 931 {
 932         char buf[2][SU_ADDRSTRLEN];
 933         struct nhrp_packet_header *hdr;
 934         struct nhrp_vc *vc = p->vc;
 935         struct interface *ifp = p->ifp;
 936         struct nhrp_interface *nifp = ifp->info;
 937         struct nhrp_packet_parser pp;
 938         struct nhrp_peer *peer = NULL;
 939         struct nhrp_reqid *reqid;
 940         const char *info = NULL;
 941         union sockunion *target_addr;
 942         unsigned paylen, extoff, extlen, realsize;
 943         afi_t nbma_afi, proto_afi;
 944
 945         debugf(NHRP_DEBUG_KERNEL, "PACKET: Recv %s -> %s",
 946                sockunion2str(&vc->remote.nbma, buf[0], sizeof(buf[0])),
 947                sockunion2str(&vc->local.nbma, buf[1], sizeof(buf[1])));
 948
 949         if (!p->online) {
 950                 info = "peer not online";
 951                 goto drop;
 952         }
 953
 954         if (nhrp_packet_calculate_checksum(zb->head, zbuf_used(zb)) != 0) {
 955                 info = "bad checksum";
 956                 goto drop;
 957         }
 958
 959         realsize = zbuf_used(zb);
 960         hdr = nhrp_packet_pull(zb, &pp.src_nbma, &pp.src_proto, &pp.dst_proto);
 961         if (!hdr) {
 962                 info = "corrupt header";
 963                 goto drop;
 964         }
 965
 966         pp.ifp = ifp;
 967         pp.pkt = zb;
 968         pp.hdr = hdr;
 969         pp.peer = p;
 970
 971         nbma_afi = htons(hdr->afnum);
 972         proto_afi = proto2afi(htons(hdr->protocol_type));
 973         if (hdr->type > NHRP_PACKET_MAX || hdr->version != NHRP_VERSION_RFC2332
 974             || nbma_afi >= AFI_MAX || proto_afi == AF_UNSPEC
 975             || packet_types[hdr->type].type == PACKET_UNKNOWN
 976             || htons(hdr->packet_size) > realsize) {
 977                 zlog_info(
 978                         "From %s: error: packet type %d, version %d, AFI %d, proto %x, size %d (real size %d)",
 979                         sockunion2str(&vc->remote.nbma, buf[0], sizeof(buf[0])),
 980                         (int)hdr->type, (int)hdr->version, (int)nbma_afi,
 981                         (int)htons(hdr->protocol_type),
 982                         (int)htons(hdr->packet_size), (int)realsize);
 983                 goto drop;
 984         }
 985         pp.if_ad = &((struct nhrp_interface *)ifp->info)->afi[proto_afi];
 986
 987         extoff = htons(hdr->extension_offset);
 988         if (extoff) {
 989                 assert(zb->head > zb->buf);
 990                 uint32_t header_offset = zb->head - zb->buf;
 991                 if (extoff >= realsize) {
 992                         info = "extoff larger than packet";
 993                         goto drop;
 994                 }
 995                 if (extoff < header_offset) {
 996                         info = "extoff smaller than header offset";
 997                         goto drop;
 998                 }
 999                 paylen = extoff - header_offset;
1000         } else {
1001                 paylen = zbuf_used(zb);
1002         }
1003         zbuf_init(&pp.payload, zbuf_pulln(zb, paylen), paylen, paylen);
1004         extlen = zbuf_used(zb);
1005         zbuf_init(&pp.extensions, zbuf_pulln(zb, extlen), extlen, extlen);
1006
1007         if (!nifp->afi[proto_afi].network_id) {
1008                 info = "nhrp not enabled";
1009                 goto drop;
1010         }
1011
1012         nhrp_packet_debug(zb, "Recv");
1013
1014         /* FIXME: Check authentication here. This extension needs to be
1015          * pre-handled. */
1016
1017         /* Figure out if this is local */
1018         target_addr = (packet_types[hdr->type].type == PACKET_REPLY)
1019                               ? &pp.src_proto
1020                               : &pp.dst_proto;
1021
1022         if (sockunion_same(&pp.src_proto, &pp.dst_proto))
1023                 pp.route_type = NHRP_ROUTE_LOCAL;
1024         else
1025                 pp.route_type = nhrp_route_address(pp.ifp, target_addr,
1026                                                    &pp.route_prefix, &peer);
1027
1028         switch (pp.route_type) {
1029         case NHRP_ROUTE_LOCAL:
1030                 nhrp_packet_debug(zb, "!LOCAL");
1031                 if (packet_types[hdr->type].type == PACKET_REPLY) {
1032                         reqid = nhrp_reqid_lookup(&nhrp_packet_reqid,
1033                                                   htonl(hdr->u.request_id));
1034                         if (reqid) {
1035                                 reqid->cb(reqid, &pp);
1036                                 break;
1037                         } else {
1038                                 nhrp_packet_debug(zb, "!UNKNOWN-REQID");
1039                                 /* FIXME: send error-indication */
1040                         }
1041                 }
1042                 /* fallthru */ /* FIXME: double check, is this correct? */
1043         case NHRP_ROUTE_OFF_NBMA:
1044                 if (packet_types[hdr->type].handler) {
1045                         packet_types[hdr->type].handler(&pp);
1046                         break;
1047                 }
1048                 break;
1049         case NHRP_ROUTE_NBMA_NEXTHOP:
1050                 nhrp_peer_forward(peer, &pp);
1051                 break;
1052         case NHRP_ROUTE_BLACKHOLE:
1053                 break;
1054         }
1055
1056 drop:
1057         if (info) {
1058                 zlog_info(
1059                         "From %s: error: %s",
1060                         sockunion2str(&vc->remote.nbma, buf[0], sizeof(buf[0])),
1061                         info);
1062         }
1063         if (peer)
1064                 nhrp_peer_unref(peer);
1065         zbuf_free(zb);
1066 }