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 (sockunion_family(&vc->local.nbma) == AF_UNSPEC)
 252                 return 0;
 253
 254         p->prio = establish > 1;
 255         p->requested = 1;
 256         vici_request_vc(nifp->ipsec_profile, &vc->local.nbma, &vc->remote.nbma, p->prio);
 257         THREAD_TIMER_ON(master, p->t_fallback, nhrp_peer_request_timeout, p,
 258                         (nifp->ipsec_fallback_profile && !p->prio) ? 15 : 30);
 259
 260         return 0;
 261 }
 262
 263 void nhrp_peer_notify_add(struct nhrp_peer *p, struct notifier_block *n, notifier_fn_t fn)
 264 {
 265         notifier_add(n, &p->notifier_list, fn);
 266 }
 267
 268 void nhrp_peer_notify_del(struct nhrp_peer *p, struct notifier_block *n)
 269 {
 270         notifier_del(n);
 271         nhrp_peer_check_delete(p);
 272 }
 273
 274 void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb)
 275 {
 276         char buf[2][256];
 277
 278         nhrp_packet_debug(zb, "Send");
 279
 280         if (!p->online)
 281                 return;
 282
 283         debugf(NHRP_DEBUG_KERNEL, "PACKET: Send %s -> %s",
 284                 sockunion2str(&p->vc->local.nbma, buf[0], sizeof buf[0]),
 285                 sockunion2str(&p->vc->remote.nbma, buf[1], sizeof buf[1]));
 286
 287         os_sendmsg(zb->head, zbuf_used(zb),
 288                 p->ifp->ifindex,
 289                 sockunion_get_addr(&p->vc->remote.nbma),
 290                 sockunion_get_addrlen(&p->vc->remote.nbma));
 291         zbuf_reset(zb);
 292 }
 293
 294 static void nhrp_handle_resolution_req(struct nhrp_packet_parser *p)
 295 {
 296         struct zbuf *zb, payload;
 297         struct nhrp_packet_header *hdr;
 298         struct nhrp_cie_header *cie;
 299         struct nhrp_extension_header *ext;
 300         struct nhrp_interface *nifp;
 301         struct nhrp_peer *peer;
 302
 303         if (!(p->if_ad->flags & NHRP_IFF_SHORTCUT)) {
 304                 debugf(NHRP_DEBUG_COMMON, "Shortcuts disabled");
 305                 /* FIXME: Send error indication? */
 306                 return;
 307         }
 308
 309         if (p->if_ad->network_id &&
 310             p->route_type == NHRP_ROUTE_OFF_NBMA &&
 311             p->route_prefix.prefixlen < 8) {
 312                 debugf(NHRP_DEBUG_COMMON, "Shortcut to more generic than /8 dropped");
 313                 return;
 314         }
 315
 316         debugf(NHRP_DEBUG_COMMON, "Parsing and replying to Resolution Req");
 317
 318         if (nhrp_route_address(p->ifp, &p->src_proto, NULL, &peer) != NHRP_ROUTE_NBMA_NEXTHOP)
 319                 return;
 320
 321 #if 0
 322         /* FIXME: Update requestors binding if CIE specifies holding time */
 323         nhrp_cache_update_binding(
 324                         NHRP_CACHE_CACHED, &p->src_proto,
 325                         nhrp_peer_get(p->ifp, &p->src_nbma),
 326                         htons(cie->holding_time));
 327 #endif
 328
 329         nifp = peer->ifp->info;
 330
 331         /* Create reply */
 332         zb = zbuf_alloc(1500);
 333         hdr = nhrp_packet_push(zb, NHRP_PACKET_RESOLUTION_REPLY, &p->src_nbma, &p->src_proto, &p->dst_proto);
 334
 335         /* Copied information from request */
 336         hdr->flags = p->hdr->flags & htons(NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER|NHRP_FLAG_RESOLUTION_SOURCE_STABLE);
 337         hdr->flags |= htons(NHRP_FLAG_RESOLUTION_DESTINATION_STABLE | NHRP_FLAG_RESOLUTION_AUTHORATIVE);
 338         hdr->u.request_id = p->hdr->u.request_id;
 339
 340         /* CIE payload */
 341         cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &p->if_ad->addr);
 342         cie->holding_time = htons(p->if_ad->holdtime);
 343         cie->mtu = htons(p->if_ad->mtu);
 344         if (p->if_ad->network_id && p->route_type == NHRP_ROUTE_OFF_NBMA)
 345                 cie->prefix_length = p->route_prefix.prefixlen;
 346         else
 347                 cie->prefix_length = 8 * sockunion_get_addrlen(&p->if_ad->addr);
 348
 349         /* Handle extensions */
 350         while ((ext = nhrp_ext_pull(&p->extensions, &payload)) != NULL) {
 351                 switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
 352                 case NHRP_EXTENSION_NAT_ADDRESS:
 353                         if (sockunion_family(&nifp->nat_nbma) == AF_UNSPEC)
 354                                 break;
 355                         ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
 356                         if (!ext) goto err;
 357                         cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nat_nbma, &p->if_ad->addr);
 358                         if (!cie) goto err;
 359                         nhrp_ext_complete(zb, ext);
 360                         break;
 361                 default:
 362                         if (nhrp_ext_reply(zb, hdr, p->ifp, ext, &payload) < 0)
 363                                 goto err;
 364                         break;
 365                 }
 366         }
 367
 368         nhrp_packet_complete(zb, hdr);
 369         nhrp_peer_send(peer, zb);
 370 err:
 371         nhrp_peer_unref(peer);
 372         zbuf_free(zb);
 373 }
 374
 375 static void nhrp_handle_registration_request(struct nhrp_packet_parser *p)
 376 {
 377         struct interface *ifp = p->ifp;
 378         struct zbuf *zb, payload;
 379         struct nhrp_packet_header *hdr;
 380         struct nhrp_cie_header *cie;
 381         struct nhrp_extension_header *ext;
 382         struct nhrp_cache *c;
 383         union sockunion cie_nbma, cie_proto, *proto_addr, *nbma_addr, *nbma_natoa;
 384         int holdtime, natted = 0;
 385         size_t paylen;
 386         void *pay;
 387
 388         debugf(NHRP_DEBUG_COMMON, "Parsing and replying to Registration Req");
 389
 390         if (!sockunion_same(&p->src_nbma, &p->peer->vc->remote.nbma))
 391                 natted = 1;
 392
 393         /* Create reply */
 394         zb = zbuf_alloc(1500);
 395         hdr = nhrp_packet_push(zb, NHRP_PACKET_REGISTRATION_REPLY,
 396                 &p->src_nbma, &p->src_proto, &p->if_ad->addr);
 397
 398         /* Copied information from request */
 399         hdr->flags = p->hdr->flags & htons(NHRP_FLAG_REGISTRATION_UNIQUE | NHRP_FLAG_REGISTRATION_NAT);
 400         hdr->u.request_id = p->hdr->u.request_id;
 401
 402         /* Copy payload CIEs */
 403         paylen = zbuf_used(&p->payload);
 404         pay = zbuf_pushn(zb, paylen);
 405         if (!pay) goto err;
 406         memcpy(pay, zbuf_pulln(&p->payload, paylen), paylen);
 407         zbuf_init(&payload, pay, paylen, paylen);
 408
 409         while ((cie = nhrp_cie_pull(&payload, hdr, &cie_nbma, &cie_proto)) != NULL) {
 410                 if (cie->prefix_length != 0xff && !(p->hdr->flags & htons(NHRP_FLAG_REGISTRATION_UNIQUE))) {
 411                         cie->code = NHRP_CODE_BINDING_NON_UNIQUE;
 412                         continue;
 413                 }
 414
 415                 /* We currently support only unique prefix registrations */
 416                 if (cie->prefix_length != 0xff) {
 417                         cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 418                         continue;
 419                 }
 420
 421                 proto_addr = (sockunion_family(&cie_proto) == AF_UNSPEC) ? &p->src_proto : &cie_proto;
 422                 nbma_addr = (sockunion_family(&cie_nbma) == AF_UNSPEC) ? &p->src_nbma : &cie_nbma;
 423                 nbma_natoa = NULL;
 424                 if (natted) {
 425                         nbma_natoa = nbma_addr;
 426                         nbma_addr = &p->peer->vc->remote.nbma;
 427                 }
 428
 429                 holdtime = htons(cie->holding_time);
 430                 if (!holdtime) holdtime = p->if_ad->holdtime;
 431
 432                 c = nhrp_cache_get(ifp, proto_addr, 1);
 433                 if (!c) {
 434                         cie->code = NHRP_CODE_INSUFFICIENT_RESOURCES;
 435                         continue;
 436                 }
 437
 438                 if (!nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC, holdtime, nhrp_peer_ref(p->peer), htons(cie->mtu), nbma_natoa)) {
 439                         cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
 440                         continue;
 441                 }
 442
 443                 cie->code = NHRP_CODE_SUCCESS;
 444         }
 445
 446         /* Handle extensions */
 447         while ((ext = nhrp_ext_pull(&p->extensions, &payload)) != NULL) {
 448                 switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
 449                 case NHRP_EXTENSION_NAT_ADDRESS:
 450                         ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
 451                         if (!ext) goto err;
 452                         zbuf_copy(zb, &payload, zbuf_used(&payload));
 453                         if (natted) {
 454                                 nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
 455                                         &p->peer->vc->remote.nbma,
 456                                         &p->src_proto);
 457                         }
 458                         nhrp_ext_complete(zb, ext);
 459                         break;
 460                 default:
 461                         if (nhrp_ext_reply(zb, hdr, ifp, ext, &payload) < 0)
 462                                 goto err;
 463                         break;
 464                 }
 465         }
 466
 467         nhrp_packet_complete(zb, hdr);
 468         nhrp_peer_send(p->peer, zb);
 469 err:
 470         zbuf_free(zb);
 471 }
 472
 473 static int parse_ether_packet(struct zbuf *zb, uint16_t protocol_type, union sockunion *src, union sockunion *dst)
 474 {
 475         switch (protocol_type) {
 476         case ETH_P_IP: {
 477                         struct iphdr *iph = zbuf_pull(zb, struct iphdr);
 478                         if (iph) {
 479                                 if (src) sockunion_set(src, AF_INET, (uint8_t*) &iph->saddr, sizeof(iph->saddr));
 480                                 if (dst) sockunion_set(dst, AF_INET, (uint8_t*) &iph->daddr, sizeof(iph->daddr));
 481                         }
 482                 }
 483                 break;
 484         case ETH_P_IPV6: {
 485                         struct ipv6hdr *iph = zbuf_pull(zb, struct ipv6hdr);
 486                         if (iph) {
 487                                 if (src) sockunion_set(src, AF_INET6, (uint8_t*) &iph->saddr, sizeof(iph->saddr));
 488                                 if (dst) sockunion_set(dst, AF_INET6, (uint8_t*) &iph->daddr, sizeof(iph->daddr));
 489                         }
 490                 }
 491                 break;
 492         default:
 493                 return 0;
 494         }
 495         return 1;
 496 }
 497
 498 void nhrp_peer_send_indication(struct interface *ifp, uint16_t protocol_type, struct zbuf *pkt)
 499 {
 500         union sockunion dst;
 501         struct zbuf *zb, payload;
 502         struct nhrp_interface *nifp = ifp->info;
 503         struct nhrp_afi_data *if_ad;
 504         struct nhrp_packet_header *hdr;
 505         struct nhrp_peer *p;
 506         char buf[2][SU_ADDRSTRLEN];
 507
 508         if (!nifp->enabled) return;
 509
 510         payload = *pkt;
 511         if (!parse_ether_packet(&payload, protocol_type, &dst, NULL))
 512                 return;
 513
 514         if (nhrp_route_address(ifp, &dst, NULL, &p) != NHRP_ROUTE_NBMA_NEXTHOP)
 515                 return;
 516
 517         if_ad = &nifp->afi[family2afi(sockunion_family(&dst))];
 518         if (!(if_ad->flags & NHRP_IFF_REDIRECT)) {
 519                 debugf(NHRP_DEBUG_COMMON, "Send Traffic Indication to %s about packet to %s ignored",
 520                         sockunion2str(&p->vc->remote.nbma, buf[0], sizeof buf[0]),
 521                         sockunion2str(&dst, buf[1], sizeof buf[1]));
 522                 return;
 523         }
 524
 525         debugf(NHRP_DEBUG_COMMON, "Send Traffic Indication to %s (online=%d) about packet to %s",
 526                 sockunion2str(&p->vc->remote.nbma, buf[0], sizeof buf[0]),
 527                 p->online,
 528                 sockunion2str(&dst, buf[1], sizeof buf[1]));
 529
 530         /* Create reply */
 531         zb = zbuf_alloc(1500);
 532         hdr = nhrp_packet_push(zb, NHRP_PACKET_TRAFFIC_INDICATION, &nifp->nbma, &if_ad->addr, &dst);
 533         hdr->hop_count = 0;
 534
 535         /* Payload is the packet causing indication */
 536         zbuf_copy(zb, pkt, zbuf_used(pkt));
 537         nhrp_packet_complete(zb, hdr);
 538         nhrp_peer_send(p, zb);
 539         nhrp_peer_unref(p);
 540         zbuf_free(zb);
 541 }
 542
 543 static void nhrp_handle_error_ind(struct nhrp_packet_parser *pp)
 544 {
 545         struct zbuf origmsg = pp->payload;
 546         struct nhrp_packet_header *hdr;
 547         struct nhrp_reqid *reqid;
 548         union sockunion src_nbma, src_proto, dst_proto;
 549         char buf[2][SU_ADDRSTRLEN];
 550
 551         hdr = nhrp_packet_pull(&origmsg, &src_nbma, &src_proto, &dst_proto);
 552         if (!hdr) return;
 553
 554         debugf(NHRP_DEBUG_COMMON, "Error Indication from %s about packet to %s ignored",
 555                 sockunion2str(&pp->src_proto, buf[0], sizeof buf[0]),
 556                 sockunion2str(&dst_proto, buf[1], sizeof buf[1]));
 557
 558         reqid = nhrp_reqid_lookup(&nhrp_packet_reqid, htonl(hdr->u.request_id));
 559         if (reqid)
 560                 reqid->cb(reqid, pp);
 561 }
 562
 563 static void nhrp_handle_traffic_ind(struct nhrp_packet_parser *p)
 564 {
 565         union sockunion dst;
 566         char buf[2][SU_ADDRSTRLEN];
 567
 568         if (!parse_ether_packet(&p->payload, htons(p->hdr->protocol_type), NULL, &dst))
 569                 return;
 570
 571         debugf(NHRP_DEBUG_COMMON, "Traffic Indication from %s about packet to %s: %s",
 572                 sockunion2str(&p->src_proto, buf[0], sizeof buf[0]),
 573                 sockunion2str(&dst, buf[1], sizeof buf[1]),
 574                 (p->if_ad->flags & NHRP_IFF_SHORTCUT) ? "trying shortcut" : "ignored");
 575
 576         if (p->if_ad->flags & NHRP_IFF_SHORTCUT)
 577                 nhrp_shortcut_initiate(&dst);
 578 }
 579
 580 enum packet_type_t {
 581         PACKET_UNKNOWN = 0,
 582         PACKET_REQUEST,
 583         PACKET_REPLY,
 584         PACKET_INDICATION,
 585 };
 586
 587 static struct {
 588         enum packet_type_t type;
 589         const char *name;
 590         void (*handler)(struct nhrp_packet_parser *);
 591 } packet_types[] = {
 592         [NHRP_PACKET_RESOLUTION_REQUEST] = {
 593                 .type = PACKET_REQUEST,
 594                 .name = "Resolution-Request",
 595                 .handler = nhrp_handle_resolution_req,
 596         },
 597         [NHRP_PACKET_RESOLUTION_REPLY] = {
 598                 .type = PACKET_REPLY,
 599                 .name = "Resolution-Reply",
 600         },
 601         [NHRP_PACKET_REGISTRATION_REQUEST] = {
 602                 .type = PACKET_REQUEST,
 603                 .name = "Registration-Request",
 604                 .handler = nhrp_handle_registration_request,
 605         },
 606         [NHRP_PACKET_REGISTRATION_REPLY] = {
 607                 .type = PACKET_REPLY,
 608                 .name = "Registration-Reply",
 609         },
 610         [NHRP_PACKET_PURGE_REQUEST] = {
 611                 .type = PACKET_REQUEST,
 612                 .name = "Purge-Request",
 613         },
 614         [NHRP_PACKET_PURGE_REPLY] = {
 615                 .type = PACKET_REPLY,
 616                 .name = "Purge-Reply",
 617         },
 618         [NHRP_PACKET_ERROR_INDICATION] = {
 619                 .type = PACKET_INDICATION,
 620                 .name = "Error-Indication",
 621                 .handler = nhrp_handle_error_ind,
 622         },
 623         [NHRP_PACKET_TRAFFIC_INDICATION] = {
 624                 .type = PACKET_INDICATION,
 625                 .name = "Traffic-Indication",
 626                 .handler = nhrp_handle_traffic_ind,
 627         }
 628 };
 629
 630 static void nhrp_peer_forward(struct nhrp_peer *p, struct nhrp_packet_parser *pp)
 631 {
 632         struct zbuf *zb, extpl;
 633         struct nhrp_packet_header *hdr;
 634         struct nhrp_extension_header *ext, *dst;
 635         struct nhrp_cie_header *cie;
 636         struct nhrp_interface *nifp = pp->ifp->info;
 637         struct nhrp_afi_data *if_ad = pp->if_ad;
 638         union sockunion cie_nbma, cie_protocol;
 639         uint16_t type, len;
 640
 641         if (pp->hdr->hop_count == 0)
 642                 return;
 643
 644         /* Create forward packet - copy header */
 645         zb = zbuf_alloc(1500);
 646         hdr = nhrp_packet_push(zb, pp->hdr->type, &pp->src_nbma, &pp->src_proto, &pp->dst_proto);
 647         hdr->flags = pp->hdr->flags;
 648         hdr->hop_count = pp->hdr->hop_count - 1;
 649         hdr->u.request_id = pp->hdr->u.request_id;
 650
 651         /* Copy payload */
 652         zbuf_copy(zb, &pp->payload, zbuf_used(&pp->payload));
 653
 654         /* Copy extensions */
 655         while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
 656                 type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
 657                 len = htons(ext->length);
 658
 659                 if (type == NHRP_EXTENSION_END)
 660                         break;
 661
 662                 dst = nhrp_ext_push(zb, hdr, htons(ext->type));
 663                 if (!dst) goto err;
 664
 665                 switch (type) {
 666                 case NHRP_EXTENSION_FORWARD_TRANSIT_NHS:
 667                 case NHRP_EXTENSION_REVERSE_TRANSIT_NHS:
 668                         zbuf_put(zb, extpl.head, len);
 669                         if ((type == NHRP_EXTENSION_REVERSE_TRANSIT_NHS) ==
 670                             (packet_types[hdr->type].type == PACKET_REPLY)) {
 671                                 /* Check NHS list for forwarding loop */
 672                                 while ((cie = nhrp_cie_pull(&extpl, pp->hdr, &cie_nbma, &cie_protocol)) != NULL) {
 673                                         if (sockunion_same(&p->vc->remote.nbma, &cie_nbma))
 674                                                 goto err;
 675                                 }
 676                                 /* Append our selves to the list */
 677                                 cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &if_ad->addr);
 678                                 if (!cie) goto err;
 679                                 cie->holding_time = htons(if_ad->holdtime);
 680                         }
 681                         break;
 682                 default:
 683                         if (htons(ext->type) & NHRP_EXTENSION_FLAG_COMPULSORY)
 684                                 /* FIXME: RFC says to just copy, but not
 685                                  * append our selves to the transit NHS list */
 686                                 goto err;
 687                 case NHRP_EXTENSION_RESPONDER_ADDRESS:
 688                         /* Supported compulsory extensions, and any
 689                          * non-compulsory that is not explicitly handled,
 690                          * should be just copied. */
 691                         zbuf_copy(zb, &extpl, len);
 692                         break;
 693                 }
 694                 nhrp_ext_complete(zb, dst);
 695         }
 696
 697         nhrp_packet_complete(zb, hdr);
 698         nhrp_peer_send(p, zb);
 699         zbuf_free(zb);
 700         return;
 701 err:
 702         nhrp_packet_debug(pp->pkt, "FWD-FAIL");
 703         zbuf_free(zb);
 704 }
 705
 706 static void nhrp_packet_debug(struct zbuf *zb, const char *dir)
 707 {
 708         char buf[2][SU_ADDRSTRLEN];
 709         union sockunion src_nbma, src_proto, dst_proto;
 710         struct nhrp_packet_header *hdr;
 711         struct zbuf zhdr;
 712         int reply;
 713
 714         if (likely(!(debug_flags & NHRP_DEBUG_COMMON)))
 715                 return;
 716
 717         zbuf_init(&zhdr, zb->buf, zb->tail-zb->buf, zb->tail-zb->buf);
 718         hdr = nhrp_packet_pull(&zhdr, &src_nbma, &src_proto, &dst_proto);
 719
 720         sockunion2str(&src_proto, buf[0], sizeof buf[0]);
 721         sockunion2str(&dst_proto, buf[1], sizeof buf[1]);
 722
 723         reply = packet_types[hdr->type].type == PACKET_REPLY;
 724         debugf(NHRP_DEBUG_COMMON, "%s %s(%d) %s -> %s",
 725                 dir,
 726                 packet_types[hdr->type].name ? : "Unknown",
 727                 hdr->type,
 728                 reply ? buf[1] : buf[0],
 729                 reply ? buf[0] : buf[1]);
 730 }
 731
 732 struct nhrp_route_info {
 733         int local;
 734         struct interface *ifp;
 735         struct nhrp_vc *vc;
 736 };
 737
 738 void nhrp_peer_recv(struct nhrp_peer *p, struct zbuf *zb)
 739 {
 740         char buf[2][SU_ADDRSTRLEN];
 741         struct nhrp_packet_header *hdr;
 742         struct nhrp_vc *vc = p->vc;
 743         struct interface *ifp = p->ifp;
 744         struct nhrp_interface *nifp = ifp->info;
 745         struct nhrp_packet_parser pp;
 746         struct nhrp_peer *peer = NULL;
 747         struct nhrp_reqid *reqid;
 748         const char *info = NULL;
 749         union sockunion *target_addr;
 750         unsigned paylen, extoff, extlen, realsize;
 751         afi_t afi;
 752
 753         debugf(NHRP_DEBUG_KERNEL, "PACKET: Recv %s -> %s",
 754                 sockunion2str(&vc->remote.nbma, buf[0], sizeof buf[0]),
 755                 sockunion2str(&vc->local.nbma, buf[1], sizeof buf[1]));
 756
 757         if (!p->online) {
 758                 info = "peer not online";
 759                 goto drop;
 760         }
 761
 762         if (nhrp_packet_calculate_checksum(zb->head, zbuf_used(zb)) != 0) {
 763                 info = "bad checksum";
 764                 goto drop;
 765         }
 766
 767         realsize = zbuf_used(zb);
 768         hdr = nhrp_packet_pull(zb, &pp.src_nbma, &pp.src_proto, &pp.dst_proto);
 769         if (!hdr) {
 770                 info = "corrupt header";
 771                 goto drop;
 772         }
 773
 774         pp.ifp = ifp;
 775         pp.pkt = zb;
 776         pp.hdr = hdr;
 777         pp.peer = p;
 778
 779         afi = htons(hdr->afnum);
 780         if (hdr->type > ZEBRA_NUM_OF(packet_types) ||
 781             hdr->version != NHRP_VERSION_RFC2332 ||
 782             afi >= AFI_MAX ||
 783             packet_types[hdr->type].type == PACKET_UNKNOWN ||
 784             htons(hdr->packet_size) > realsize) {
 785                 zlog_info("From %s: error: packet type %d, version %d, AFI %d, size %d (real size %d)",
 786                            sockunion2str(&vc->remote.nbma, buf[0], sizeof buf[0]),
 787                            (int) hdr->type, (int) hdr->version, (int) afi,
 788                            (int) htons(hdr->packet_size),
 789                            (int) realsize);
 790                 goto drop;
 791         }
 792         pp.if_ad = &((struct nhrp_interface *)ifp->info)->afi[afi];
 793
 794         extoff = htons(hdr->extension_offset);
 795         if (extoff) {
 796                 if (extoff >= realsize) {
 797                         info = "extoff larger than packet";
 798                         goto drop;
 799                 }
 800                 paylen = extoff - (zb->head - zb->buf);
 801         } else {
 802                 paylen = zbuf_used(zb);
 803         }
 804         zbuf_init(&pp.payload, zbuf_pulln(zb, paylen), paylen, paylen);
 805         extlen = zbuf_used(zb);
 806         zbuf_init(&pp.extensions, zbuf_pulln(zb, extlen), extlen, extlen);
 807
 808         if (!nifp->afi[afi].network_id) {
 809                 info = "nhrp not enabled";
 810                 goto drop;
 811         }
 812
 813         nhrp_packet_debug(zb, "Recv");
 814
 815         /* FIXME: Check authentication here. This extension needs to be
 816          * pre-handled. */
 817
 818         /* Figure out if this is local */
 819         target_addr = (packet_types[hdr->type].type == PACKET_REPLY) ? &pp.src_proto : &pp.dst_proto;
 820
 821         if (sockunion_same(&pp.src_proto, &pp.dst_proto))
 822                 pp.route_type = NHRP_ROUTE_LOCAL;
 823         else
 824                 pp.route_type = nhrp_route_address(pp.ifp, target_addr, &pp.route_prefix, &peer);
 825
 826         switch (pp.route_type) {
 827         case NHRP_ROUTE_LOCAL:
 828                 nhrp_packet_debug(zb, "!LOCAL");
 829                 if (packet_types[hdr->type].type == PACKET_REPLY) {
 830                         reqid = nhrp_reqid_lookup(&nhrp_packet_reqid, htonl(hdr->u.request_id));
 831                         if (reqid) {
 832                                 reqid->cb(reqid, &pp);
 833                                 break;
 834                         } else {
 835                                 nhrp_packet_debug(zb, "!UNKNOWN-REQID");
 836                                 /* FIXME: send error-indication */
 837                         }
 838                 }
 839         case NHRP_ROUTE_OFF_NBMA:
 840                 if (packet_types[hdr->type].handler) {
 841                         packet_types[hdr->type].handler(&pp);
 842                         break;
 843                 }
 844                 break;
 845         case NHRP_ROUTE_NBMA_NEXTHOP:
 846                 nhrp_peer_forward(peer, &pp);
 847                 break;
 848         case NHRP_ROUTE_BLACKHOLE:
 849                 break;
 850         }
 851
 852 drop:
 853         if (info) {
 854                 zlog_info("From %s: error: %s",
 855                           sockunion2str(&vc->remote.nbma, buf[0], sizeof buf[0]),
 856                           info);
 857         }
 858         if (peer) nhrp_peer_unref(peer);
 859         zbuf_free(zb);
 860 }