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