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