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