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