nhrpd/nhrp_packet.c

   1 /* NHRP packet handling functions
   2  * Copyright (c) 2014-2015 Timo Teräs
   3  *
   4  * This file is free software: you may copy, redistribute and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  */
   9
  10 #ifdef HAVE_CONFIG_H
  11 #include "config.h"
  12 #endif
  13
  14 #include <netinet/if_ether.h>
  15 #include "nhrpd.h"
  16 #include "zbuf.h"
  17 #include "thread.h"
  18 #include "hash.h"
  19
  20 #include "nhrp_protocol.h"
  21 #include "os.h"
  22
  23 struct nhrp_reqid_pool nhrp_packet_reqid;
  24
  25 static uint16_t family2proto(int family)
  26 {
  27         switch (family) {
  28         case AF_INET:
  29                 return ETH_P_IP;
  30         case AF_INET6:
  31                 return ETH_P_IPV6;
  32         }
  33         return 0;
  34 }
  35
  36 static int proto2family(uint16_t proto)
  37 {
  38         switch (proto) {
  39         case ETH_P_IP:
  40                 return AF_INET;
  41         case ETH_P_IPV6:
  42                 return AF_INET6;
  43         }
  44         return AF_UNSPEC;
  45 }
  46
  47 struct nhrp_packet_header *nhrp_packet_push(struct zbuf *zb, uint8_t type,
  48                                             const union sockunion *src_nbma,
  49                                             const union sockunion *src_proto,
  50                                             const union sockunion *dst_proto)
  51 {
  52         struct nhrp_packet_header *hdr;
  53
  54         hdr = zbuf_push(zb, struct nhrp_packet_header);
  55         if (!hdr)
  56                 return NULL;
  57
  58         *hdr = (struct nhrp_packet_header){
  59                 .afnum = htons(family2afi(sockunion_family(src_nbma))),
  60                 .protocol_type =
  61                         htons(family2proto(sockunion_family(src_proto))),
  62                 .version = NHRP_VERSION_RFC2332,
  63                 .type = type,
  64                 .hop_count = 64,
  65                 .src_nbma_address_len = sockunion_get_addrlen(src_nbma),
  66                 .src_protocol_address_len = sockunion_get_addrlen(src_proto),
  67                 .dst_protocol_address_len = sockunion_get_addrlen(dst_proto),
  68         };
  69
  70         zbuf_put(zb, sockunion_get_addr(src_nbma), hdr->src_nbma_address_len);
  71         zbuf_put(zb, sockunion_get_addr(src_proto),
  72                  hdr->src_protocol_address_len);
  73         zbuf_put(zb, sockunion_get_addr(dst_proto),
  74                  hdr->dst_protocol_address_len);
  75
  76         return hdr;
  77 }
  78
  79 struct nhrp_packet_header *nhrp_packet_pull(struct zbuf *zb,
  80                                             union sockunion *src_nbma,
  81                                             union sockunion *src_proto,
  82                                             union sockunion *dst_proto)
  83 {
  84         struct nhrp_packet_header *hdr;
  85
  86         hdr = zbuf_pull(zb, struct nhrp_packet_header);
  87         if (!hdr)
  88                 return NULL;
  89
  90         sockunion_set(src_nbma, afi2family(htons(hdr->afnum)),
  91                       zbuf_pulln(zb,
  92                                  hdr->src_nbma_address_len
  93                                          + hdr->src_nbma_subaddress_len),
  94                       hdr->src_nbma_address_len + hdr->src_nbma_subaddress_len);
  95         sockunion_set(src_proto, proto2family(htons(hdr->protocol_type)),
  96                       zbuf_pulln(zb, hdr->src_protocol_address_len),
  97                       hdr->src_protocol_address_len);
  98         sockunion_set(dst_proto, proto2family(htons(hdr->protocol_type)),
  99                       zbuf_pulln(zb, hdr->dst_protocol_address_len),
 100                       hdr->dst_protocol_address_len);
 101
 102         return hdr;
 103 }
 104
 105 uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu, uint16_t len)
 106 {
 107         const uint16_t *pdu16 = (const uint16_t *)pdu;
 108         uint32_t csum = 0;
 109         int i;
 110
 111         for (i = 0; i < len / 2; i++)
 112                 csum += pdu16[i];
 113         if (len & 1)
 114                 csum += htons(pdu[len - 1]);
 115
 116         while (csum & 0xffff0000)
 117                 csum = (csum & 0xffff) + (csum >> 16);
 118
 119         return (~csum) & 0xffff;
 120 }
 121
 122 void nhrp_packet_complete(struct zbuf *zb, struct nhrp_packet_header *hdr)
 123 {
 124         unsigned short size;
 125
 126         if (hdr->extension_offset)
 127                 nhrp_ext_push(zb, hdr,
 128                               NHRP_EXTENSION_END
 129                                       | NHRP_EXTENSION_FLAG_COMPULSORY);
 130
 131         size = zb->tail - (uint8_t *)hdr;
 132         hdr->packet_size = htons(size);
 133         hdr->checksum = 0;
 134         hdr->checksum = nhrp_packet_calculate_checksum((uint8_t *)hdr, size);
 135 }
 136
 137 struct nhrp_cie_header *nhrp_cie_push(struct zbuf *zb, uint8_t code,
 138                                       const union sockunion *nbma,
 139                                       const union sockunion *proto)
 140 {
 141         struct nhrp_cie_header *cie;
 142
 143         cie = zbuf_push(zb, struct nhrp_cie_header);
 144         *cie = (struct nhrp_cie_header){
 145                 .code = code,
 146         };
 147         if (nbma) {
 148                 cie->nbma_address_len = sockunion_get_addrlen(nbma);
 149                 zbuf_put(zb, sockunion_get_addr(nbma), cie->nbma_address_len);
 150         }
 151         if (proto) {
 152                 cie->protocol_address_len = sockunion_get_addrlen(proto);
 153                 zbuf_put(zb, sockunion_get_addr(proto),
 154                          cie->protocol_address_len);
 155         }
 156
 157         return cie;
 158 }
 159
 160 struct nhrp_cie_header *nhrp_cie_pull(struct zbuf *zb,
 161                                       struct nhrp_packet_header *hdr,
 162                                       union sockunion *nbma,
 163                                       union sockunion *proto)
 164 {
 165         struct nhrp_cie_header *cie;
 166
 167         cie = zbuf_pull(zb, struct nhrp_cie_header);
 168         if (!cie)
 169                 return NULL;
 170
 171         if (cie->nbma_address_len + cie->nbma_subaddress_len > 0) {
 172                 sockunion_set(nbma, afi2family(htons(hdr->afnum)),
 173                               zbuf_pulln(zb,
 174                                          cie->nbma_address_len
 175                                                  + cie->nbma_subaddress_len),
 176                               cie->nbma_address_len + cie->nbma_subaddress_len);
 177         } else {
 178                 sockunion_family(nbma) = AF_UNSPEC;
 179         }
 180
 181         if (cie->protocol_address_len) {
 182                 sockunion_set(proto, proto2family(htons(hdr->protocol_type)),
 183                               zbuf_pulln(zb, cie->protocol_address_len),
 184                               cie->protocol_address_len);
 185         } else {
 186                 sockunion_family(proto) = AF_UNSPEC;
 187         }
 188
 189         return cie;
 190 }
 191
 192 struct nhrp_extension_header *
 193 nhrp_ext_push(struct zbuf *zb, struct nhrp_packet_header *hdr, uint16_t type)
 194 {
 195         struct nhrp_extension_header *ext;
 196         ext = zbuf_push(zb, struct nhrp_extension_header);
 197         if (!ext)
 198                 return NULL;
 199
 200         if (!hdr->extension_offset)
 201                 hdr->extension_offset =
 202                         htons(zb->tail - (uint8_t *)hdr
 203                               - sizeof(struct nhrp_extension_header));
 204
 205         *ext = (struct nhrp_extension_header){
 206                 .type = htons(type), .length = 0,
 207         };
 208         return ext;
 209 }
 210
 211 void nhrp_ext_complete(struct zbuf *zb, struct nhrp_extension_header *ext)
 212 {
 213         ext->length = htons(zb->tail - (uint8_t *)ext
 214                             - sizeof(struct nhrp_extension_header));
 215 }
 216
 217 struct nhrp_extension_header *nhrp_ext_pull(struct zbuf *zb,
 218                                             struct zbuf *payload)
 219 {
 220         struct nhrp_extension_header *ext;
 221         uint16_t plen;
 222
 223         ext = zbuf_pull(zb, struct nhrp_extension_header);
 224         if (!ext)
 225                 return NULL;
 226
 227         plen = htons(ext->length);
 228         zbuf_init(payload, zbuf_pulln(zb, plen), plen, plen);
 229         return ext;
 230 }
 231
 232 void nhrp_ext_request(struct zbuf *zb, struct nhrp_packet_header *hdr,
 233                       struct interface *ifp)
 234 {
 235         /* Place holders for standard extensions */
 236         nhrp_ext_push(zb, hdr,
 237                       NHRP_EXTENSION_FORWARD_TRANSIT_NHS
 238                               | NHRP_EXTENSION_FLAG_COMPULSORY);
 239         nhrp_ext_push(zb, hdr,
 240                       NHRP_EXTENSION_REVERSE_TRANSIT_NHS
 241                               | NHRP_EXTENSION_FLAG_COMPULSORY);
 242         nhrp_ext_push(zb, hdr,
 243                       NHRP_EXTENSION_RESPONDER_ADDRESS
 244                               | NHRP_EXTENSION_FLAG_COMPULSORY);
 245 }
 246
 247 int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr,
 248                    struct interface *ifp, struct nhrp_extension_header *ext,
 249                    struct zbuf *extpayload)
 250 {
 251         struct nhrp_interface *nifp = ifp->info;
 252         struct nhrp_afi_data *ad = &nifp->afi[htons(hdr->afnum)];
 253         struct nhrp_extension_header *dst;
 254         struct nhrp_cie_header *cie;
 255         uint16_t type;
 256
 257         type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
 258         if (type == NHRP_EXTENSION_END)
 259                 return 0;
 260
 261         dst = nhrp_ext_push(zb, hdr, htons(ext->type));
 262         if (!dst)
 263                 goto err;
 264
 265         switch (type) {
 266         case NHRP_EXTENSION_RESPONDER_ADDRESS:
 267                 cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma,
 268                                     &ad->addr);
 269                 if (!cie)
 270                         goto err;
 271                 cie->holding_time = htons(ad->holdtime);
 272                 break;
 273         default:
 274                 if (type & NHRP_EXTENSION_FLAG_COMPULSORY)
 275                         goto err;
 276         /* fallthru */
 277         case NHRP_EXTENSION_FORWARD_TRANSIT_NHS:
 278         case NHRP_EXTENSION_REVERSE_TRANSIT_NHS:
 279                 /* Supported compulsory extensions, and any
 280                  * non-compulsory that is not explicitly handled,
 281                  * should be just copied. */
 282                 zbuf_copy(zb, extpayload, zbuf_used(extpayload));
 283                 break;
 284         }
 285         nhrp_ext_complete(zb, dst);
 286         return 0;
 287 err:
 288         zbuf_set_werror(zb);
 289         return -1;
 290 }
 291
 292 static int nhrp_packet_recvraw(struct thread *t)
 293 {
 294         int fd = THREAD_FD(t), ifindex;
 295         struct zbuf *zb;
 296         struct interface *ifp;
 297         struct nhrp_peer *p;
 298         union sockunion remote_nbma;
 299         uint8_t addr[64];
 300         size_t len, addrlen;
 301
 302         thread_add_read(master, nhrp_packet_recvraw, 0, fd, NULL);
 303
 304         zb = zbuf_alloc(1500);
 305         if (!zb)
 306                 return 0;
 307
 308         len = zbuf_size(zb);
 309         addrlen = sizeof(addr);
 310         if (os_recvmsg(zb->buf, &len, &ifindex, addr, &addrlen) < 0)
 311                 goto err;
 312
 313         zb->head = zb->buf;
 314         zb->tail = zb->buf + len;
 315
 316         switch (addrlen) {
 317         case 4:
 318                 sockunion_set(&remote_nbma, AF_INET, addr, addrlen);
 319                 break;
 320         default:
 321                 goto err;
 322         }
 323
 324         ifp = if_lookup_by_index(ifindex, VRF_DEFAULT);
 325         if (!ifp)
 326                 goto err;
 327
 328         p = nhrp_peer_get(ifp, &remote_nbma);
 329         if (!p)
 330                 goto err;
 331
 332         nhrp_peer_recv(p, zb);
 333         nhrp_peer_unref(p);
 334         return 0;
 335
 336 err:
 337         zbuf_free(zb);
 338         return 0;
 339 }
 340
 341 int nhrp_packet_init(void)
 342 {
 343         thread_add_read(master, nhrp_packet_recvraw, 0, os_socket(), NULL);
 344         return 0;
 345 }