nhrpd/nhrp_peer.c

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