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