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