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