nhrpd/nhrp_peer.c

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