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