nhrp_packet.c 7.9 KB

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