gluon-radv-filterd.c 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580
  1. /*
  2. Copyright (c) 2016 Jan-Philipp Litza <janphilipp@litza.de>
  3. All rights reserved.
  4. Redistribution and use in source and binary forms, with or without
  5. modification, are permitted provided that the following conditions are met:
  6. 1. Redistributions of source code must retain the above copyright notice,
  7. this list of conditions and the following disclaimer.
  8. 2. Redistributions in binary form must reproduce the above copyright notice,
  9. this list of conditions and the following disclaimer in the documentation
  10. and/or other materials provided with the distribution.
  11. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  12. AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  13. IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  14. DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  15. FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  16. DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  17. SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  18. CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  19. OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  20. OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  21. */
  22. #define _GNU_SOURCE
  23. #include <errno.h>
  24. #include <signal.h>
  25. #include <stdarg.h>
  26. #include <stdbool.h>
  27. #include <stddef.h>
  28. #include <stdio.h>
  29. #include <stdint.h>
  30. #include <stdlib.h>
  31. #include <string.h>
  32. #include <time.h>
  33. #include <unistd.h>
  34. #include <sys/socket.h>
  35. #include <sys/select.h>
  36. #include <sys/types.h>
  37. #include <sys/wait.h>
  38. #include <net/if.h>
  39. #include <linux/filter.h>
  40. #include <linux/if_packet.h>
  41. #include <linux/limits.h>
  42. #include <netinet/icmp6.h>
  43. #include <netinet/in.h>
  44. #include <netinet/ip6.h>
  45. #include "mac.h"
  46. // Recheck TQs after this time even if no RA was received
  47. #define MAX_INTERVAL 60
  48. // Recheck TQs at most this often, even if new RAs were received (they won't
  49. // become the preferred routers until the TQs have been rechecked)
  50. // Also, the first update will take at least this long
  51. #define MIN_INTERVAL 15
  52. // max execution time of a single ebtables call in nanoseconds
  53. #define EBTABLES_TIMEOUT 5e8 // 500ms
  54. // TQ value assigned to local routers
  55. #define LOCAL_TQ 512
  56. #define BUFSIZE 1500
  57. #define DEBUGFS "/sys/kernel/debug/batman_adv/%s/"
  58. #define ORIGINATORS DEBUGFS "originators"
  59. #define TRANSTABLE_GLOBAL DEBUGFS "transtable_global"
  60. #define TRANSTABLE_LOCAL DEBUGFS "transtable_local"
  61. #ifdef DEBUG
  62. #define CHECK(stmt) \
  63. if(!(stmt)) { \
  64. fprintf(stderr, "check failed: " #stmt "\n"); \
  65. goto check_failed; \
  66. }
  67. #define DEBUG_MSG(msg, ...) fprintf(stderr, msg "\n", ##__VA_ARGS__)
  68. #else
  69. #define CHECK(stmt) if(!(stmt)) goto check_failed;
  70. #define DEBUG_MSG(msg, ...) do {} while(0)
  71. #endif
  72. #ifndef ARRAY_SIZE
  73. #define ARRAY_SIZE(A) (sizeof(A)/sizeof(A[0]))
  74. #endif
  75. struct list_item {
  76. struct list *next;
  77. };
  78. #define foreach(item, list) \
  79. for(item = list; item != NULL; item = item->next)
  80. struct router {
  81. struct router *next;
  82. macaddr_t src;
  83. time_t eol;
  84. macaddr_t originator;
  85. uint16_t tq;
  86. };
  87. struct global {
  88. int sock;
  89. struct router *routers;
  90. const char *mesh_iface;
  91. const char *chain;
  92. uint16_t max_tq;
  93. uint16_t hysteresis_thresh;
  94. struct router *best_router;
  95. } G = {
  96. .mesh_iface = "bat0",
  97. };
  98. static void error(int status, int errnum, char *message, ...) {
  99. va_list ap;
  100. va_start(ap, message);
  101. fflush(stdout);
  102. vfprintf(stderr, message, ap);
  103. if (errnum)
  104. fprintf(stderr, ": %s", strerror(errnum));
  105. fprintf(stderr, "\n");
  106. if (status)
  107. exit(status);
  108. }
  109. static void cleanup() {
  110. struct router *router;
  111. close(G.sock);
  112. while (G.routers != NULL) {
  113. router = G.routers;
  114. G.routers = router->next;
  115. free(router);
  116. }
  117. }
  118. static void usage(const char *msg) {
  119. if (msg != NULL && *msg != '\0') {
  120. fprintf(stderr, "ERROR: %s\n\n", msg);
  121. }
  122. fprintf(stderr,
  123. "Usage: %s [-m <mesh_iface>] [-t <thresh>] -c <chain> -i <iface>\n\n"
  124. " -m <mesh_iface> B.A.T.M.A.N. advanced mesh interface used to get metric\n"
  125. " information (\"TQ\") for the available gateways. Default: bat0\n"
  126. " -t <thresh> Minimum TQ difference required to switch the gateway.\n"
  127. " Default: 0\n"
  128. " -c <chain> ebtables chain that should be managed by the daemon. The\n"
  129. " chain already has to exist on program invocation and should\n"
  130. " have a DROP policy. It will be flushed by the program!\n"
  131. " -i <iface> Interface to listen on for router advertisements. Should be\n"
  132. " <mesh_iface> or a bridge on top of it, as no metric\n"
  133. " information will be available for hosts on other interfaces.\n\n",
  134. program_invocation_short_name);
  135. cleanup();
  136. if (msg == NULL)
  137. exit(EXIT_SUCCESS);
  138. else
  139. exit(EXIT_FAILURE);
  140. }
  141. #define exit_errmsg(message, ...) { \
  142. fprintf(stderr, message "\n", ##__VA_ARGS__); \
  143. cleanup(); \
  144. exit(1); \
  145. }
  146. static inline void exit_errno(const char *message) {
  147. cleanup();
  148. error(1, errno, "error: %s", message);
  149. }
  150. static inline void warn_errno(const char *message) {
  151. error(0, errno, "warning: %s", message);
  152. }
  153. static int init_packet_socket(unsigned int ifindex) {
  154. struct sock_filter radv_filter_code[] = {
  155. // check that this is an ICMPv6 packet
  156. BPF_STMT(BPF_LD|BPF_B|BPF_ABS, offsetof(struct ip6_hdr, ip6_nxt)),
  157. BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, IPPROTO_ICMPV6, 0, 7),
  158. // check that this is a router advertisement
  159. BPF_STMT(BPF_LD|BPF_B|BPF_ABS, sizeof(struct ip6_hdr) + offsetof(struct icmp6_hdr, icmp6_type)),
  160. BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, ND_ROUTER_ADVERT, 0, 5),
  161. // check that the code field in the ICMPv6 header is 0
  162. BPF_STMT(BPF_LD|BPF_B|BPF_ABS, sizeof(struct ip6_hdr) + offsetof(struct nd_router_advert, nd_ra_code)),
  163. BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, 0, 0, 3),
  164. // check that this is a default route (lifetime > 0)
  165. BPF_STMT(BPF_LD|BPF_B|BPF_ABS, sizeof(struct ip6_hdr) + offsetof(struct nd_router_advert, nd_ra_router_lifetime)),
  166. BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, 0, 1, 0),
  167. // return true
  168. BPF_STMT(BPF_RET|BPF_K, 0xffffffff),
  169. // return false
  170. BPF_STMT(BPF_RET|BPF_K, 0),
  171. };
  172. struct sock_fprog radv_filter = {
  173. .len = ARRAY_SIZE(radv_filter_code),
  174. .filter = radv_filter_code,
  175. };
  176. int sock = socket(AF_PACKET, SOCK_DGRAM|SOCK_CLOEXEC, htons(ETH_P_IPV6));
  177. if (sock < 0)
  178. exit_errno("can't open packet socket");
  179. int ret = setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &radv_filter, sizeof(radv_filter));
  180. if (ret < 0)
  181. exit_errno("can't attach socket filter");
  182. struct sockaddr_ll bind_iface = {
  183. .sll_family = AF_PACKET,
  184. .sll_protocol = htons(ETH_P_IPV6),
  185. .sll_ifindex = ifindex,
  186. };
  187. bind(sock, (struct sockaddr *)&bind_iface, sizeof(bind_iface));
  188. return sock;
  189. }
  190. static void parse_cmdline(int argc, char *argv[]) {
  191. int c;
  192. unsigned int ifindex;
  193. unsigned long int threshold;
  194. char *endptr;
  195. while ((c = getopt(argc, argv, "c:hi:m:t:")) != -1) {
  196. switch (c) {
  197. case 'i':
  198. if (G.sock != 0)
  199. usage("-i given more than once");
  200. ifindex = if_nametoindex(optarg);
  201. if (ifindex == 0)
  202. exit_errmsg("Unknown interface: %s", optarg);
  203. G.sock = init_packet_socket(ifindex);
  204. break;
  205. case 'm':
  206. G.mesh_iface = optarg;
  207. break;
  208. case 'c':
  209. G.chain = optarg;
  210. break;
  211. case 't':
  212. threshold = strtoul(optarg, &endptr, 10);
  213. if (*endptr != '\0')
  214. exit_errmsg("Threshold must be a number: %s", optarg);
  215. if (threshold >= LOCAL_TQ)
  216. exit_errmsg("Threshold too large: %ld (max is %d)", threshold, LOCAL_TQ);
  217. G.hysteresis_thresh = (uint16_t) threshold;
  218. break;
  219. case 'h':
  220. usage(NULL);
  221. break;
  222. default:
  223. usage("");
  224. break;
  225. }
  226. }
  227. }
  228. static void handle_ra(int sock) {
  229. struct sockaddr_ll src;
  230. unsigned int addr_size = sizeof(src);
  231. size_t len;
  232. struct {
  233. struct ip6_hdr ip6;
  234. struct nd_router_advert ra;
  235. } pkt;
  236. len = recvfrom(sock, &pkt, sizeof(pkt), 0, (struct sockaddr *)&src, &addr_size);
  237. // BPF already checked that this is an ICMPv6 RA of a default router
  238. CHECK(len >= sizeof(pkt));
  239. CHECK(ntohs(pkt.ip6.ip6_plen) + sizeof(struct ip6_hdr) >= sizeof(pkt));
  240. DEBUG_MSG("received valid RA from " F_MAC, F_MAC_VAR(src.sll_addr));
  241. // update list of known routers
  242. struct router *router;
  243. foreach(router, G.routers) {
  244. if (!memcmp(router->src, src.sll_addr, sizeof(macaddr_t))) {
  245. break;
  246. }
  247. }
  248. if (!router) {
  249. router = malloc(sizeof(struct router));
  250. memcpy(router->src, src.sll_addr, 8);
  251. router->next = G.routers;
  252. G.routers = router;
  253. }
  254. router->eol = time(NULL) + pkt.ra.nd_ra_router_lifetime;
  255. check_failed:
  256. return;
  257. }
  258. static void expire_routers() {
  259. struct router **prev_ptr = &G.routers;
  260. struct router *router;
  261. time_t now = time(NULL);
  262. foreach(router, G.routers) {
  263. if (router->eol < now) {
  264. DEBUG_MSG("router " F_MAC " expired", F_MAC_VAR(router->src));
  265. *prev_ptr = router->next;
  266. if (G.best_router == router)
  267. G.best_router = NULL;
  268. free(router);
  269. } else {
  270. prev_ptr = &router->next;
  271. }
  272. }
  273. }
  274. static void update_tqs() {
  275. FILE *f;
  276. struct router *router;
  277. char path[PATH_MAX];
  278. char *line = NULL;
  279. size_t len = 0;
  280. uint8_t tq;
  281. bool update_originators = false;
  282. int i;
  283. macaddr_t mac_a, mac_b;
  284. // reset TQs
  285. foreach(router, G.routers) {
  286. router->tq = 0;
  287. for (i = 0; i < 6; i++)
  288. if (router->originator[i] != 0)
  289. break;
  290. if (i >= 6)
  291. update_originators = true;
  292. }
  293. // TODO: Currently, we iterate over the whole list of routers all the
  294. // time. Maybe it would be a good idea to sort routers that already
  295. // have the current piece of information to the back. That way, we
  296. // could abort as soon as we hit the first router with the current
  297. // information filled in.
  298. if (update_originators) {
  299. // translate all router's MAC addresses to originators simultaneously
  300. snprintf(path, PATH_MAX, TRANSTABLE_GLOBAL, G.mesh_iface);
  301. f = fopen(path, "r");
  302. // skip header
  303. while (fgetc(f) != '\n');
  304. while (fgetc(f) != '\n');
  305. while (fscanf(f, " %*[*+] " F_MAC "%*[0-9 -] (%*3u) via " F_MAC " %*[^]]]\n",
  306. F_MAC_VAR(&mac_a), F_MAC_VAR(&mac_b)) == 12) {
  307. foreach(router, G.routers) {
  308. if (!memcmp(router->src, mac_a, sizeof(macaddr_t))) {
  309. DEBUG_MSG("Found originator for " F_MAC ", it's " F_MAC, F_MAC_VAR(router->src), F_MAC_VAR(mac_b));
  310. memcpy(router->originator, mac_b, sizeof(macaddr_t));
  311. break; // foreach
  312. }
  313. }
  314. }
  315. if (!feof(f)) {
  316. getline(&line, &len, f);
  317. fprintf(stderr, "Parsing transtable_global aborted at this line: %s\n", line);
  318. }
  319. fclose(f);
  320. }
  321. // look up TQs of originators
  322. G.max_tq = 0;
  323. snprintf(path, PATH_MAX, ORIGINATORS, G.mesh_iface);
  324. f = fopen(path, "r");
  325. // skip header
  326. while (fgetc(f) != '\n');
  327. while (fgetc(f) != '\n');
  328. while (fscanf(f, F_MAC " %*fs (%hhu) %*[^\n]\n",
  329. F_MAC_VAR(&mac_a), &tq) == 7) {
  330. foreach(router, G.routers) {
  331. if (!memcmp(router->originator, mac_a, sizeof(macaddr_t))) {
  332. DEBUG_MSG("Found TQ for router " F_MAC " (originator " F_MAC "), it's %d", F_MAC_VAR(router->src), F_MAC_VAR(router->originator), tq);
  333. router->tq = tq;
  334. if (tq > G.max_tq)
  335. G.max_tq = tq;
  336. break; // foreach
  337. }
  338. }
  339. }
  340. if (!feof(f)) {
  341. getline(&line, &len, f);
  342. fprintf(stderr, "Parsing originators aborted at this line: %s\n", line);
  343. }
  344. fclose(f);
  345. // if all routers have a TQ value, we don't need to check translocal
  346. foreach(router, G.routers) {
  347. if (router->tq == 0)
  348. break;
  349. }
  350. if (router != NULL) {
  351. // rate local routers (if present) the highest
  352. snprintf(path, PATH_MAX, TRANSTABLE_LOCAL, G.mesh_iface);
  353. f = fopen(path, "r");
  354. // skip header
  355. while (fgetc(f) != '\n');
  356. while (fgetc(f) != '\n');
  357. while (fscanf(f, " * " F_MAC " [%*5s] %*f", F_MAC_VAR(&mac_a)) == 6) {
  358. foreach(router, G.routers) {
  359. if (!memcmp(router->src, mac_a, sizeof(macaddr_t))) {
  360. DEBUG_MSG("Found router " F_MAC " in transtable_local, assigning TQ %d", F_MAC_VAR(router->src), LOCAL_TQ);
  361. router->tq = LOCAL_TQ;
  362. G.max_tq = LOCAL_TQ;
  363. break; // foreach
  364. }
  365. }
  366. }
  367. if (!feof(f)) {
  368. getline(&line, &len, f);
  369. fprintf(stderr, "Parsing transtable_local aborted at this line: %s\n", line);
  370. }
  371. fclose(f);
  372. }
  373. foreach(router, G.routers) {
  374. if (router->tq == 0) {
  375. for (i = 0; i < 6; i++)
  376. if (router->originator[i] != 0)
  377. break;
  378. if (i >= 6)
  379. fprintf(stderr,
  380. "Unable to find router " F_MAC " in transtable_{global,local}\n",
  381. F_MAC_VAR(router->src));
  382. else
  383. fprintf(stderr,
  384. "Unable to find TQ for originator " F_MAC " (router " F_MAC ")\n",
  385. F_MAC_VAR(router->originator),
  386. F_MAC_VAR(router->src));
  387. }
  388. }
  389. free(line);
  390. }
  391. static int fork_execvp_timeout(struct timespec *timeout, const char *file, const char *const argv[]) {
  392. int ret;
  393. pid_t child;
  394. siginfo_t info;
  395. sigset_t signals;
  396. sigemptyset(&signals);
  397. sigaddset(&signals, SIGCHLD);
  398. child = fork();
  399. if (!child) {
  400. // casting discards const, but should be safe
  401. // (see http://stackoverflow.com/q/36925388)
  402. execvp(file, (char**) argv);
  403. error(1, errno, "can't execvp(\"%s\", ...)", file);
  404. }
  405. sigprocmask(SIG_BLOCK, &signals, NULL);
  406. ret = sigtimedwait(&signals, &info, timeout);
  407. sigprocmask(SIG_UNBLOCK, &signals, NULL);
  408. if (ret == SIGCHLD) {
  409. if (info.si_pid != child) {
  410. cleanup();
  411. error(1, 0,
  412. "BUG: We received a SIGCHLD from a child we didn't spawn (expected PID %d, got %d)",
  413. child, info.si_pid);
  414. }
  415. waitpid(child, NULL, 0);
  416. return info.si_status;
  417. }
  418. if (ret < 0 && errno == EAGAIN)
  419. error(0, 0, "warning: child %d took too long, killing", child);
  420. else if (ret < 0)
  421. warn_errno("sigtimedwait failed, killing child");
  422. else
  423. error(1, 0,
  424. "BUG: sigtimedwait() returned some other signal than SIGCHLD: %d",
  425. ret);
  426. kill(child, SIGKILL);
  427. kill(child, SIGCONT);
  428. waitpid(child, NULL, 0);
  429. return -1;
  430. }
  431. static void update_ebtables() {
  432. struct timespec timeout = {
  433. .tv_nsec = EBTABLES_TIMEOUT,
  434. };
  435. char mac[18];
  436. struct router *router;
  437. if (G.best_router && G.best_router->tq >= G.max_tq - G.hysteresis_thresh) {
  438. DEBUG_MSG(F_MAC " is still good enough with TQ=%d (max_tq=%d), not executing ebtables",
  439. F_MAC_VAR(G.best_router->src),
  440. G.best_router->tq,
  441. G.max_tq);
  442. return;
  443. }
  444. foreach(router, G.routers) {
  445. if (router->tq == G.max_tq) {
  446. snprintf(mac, sizeof(mac), F_MAC, F_MAC_VAR(router->src));
  447. break;
  448. }
  449. }
  450. if (G.best_router)
  451. fprintf(stderr, "Switching from " F_MAC " (TQ=%d) to %s (TQ=%d)\n",
  452. F_MAC_VAR(G.best_router->src),
  453. G.best_router->tq,
  454. mac,
  455. G.max_tq);
  456. else
  457. fprintf(stderr, "Switching to %s (TQ=%d)\n",
  458. mac,
  459. G.max_tq);
  460. G.best_router = router;
  461. if (fork_execvp_timeout(&timeout, "ebtables", (const char *[])
  462. { "ebtables", "-F", G.chain, NULL }))
  463. error(0, 0, "warning: flushing ebtables chain %s failed, not adding a new rule", G.chain);
  464. else if (fork_execvp_timeout(&timeout, "ebtables", (const char *[])
  465. { "ebtables", "-A", G.chain, "-s", mac, "-j", "ACCEPT", NULL }))
  466. error(0, 0, "warning: adding new rule to ebtables chain %s failed", G.chain);
  467. }
  468. int main(int argc, char *argv[]) {
  469. int retval;
  470. fd_set rfds;
  471. struct timeval tv;
  472. time_t last_update = time(NULL);
  473. parse_cmdline(argc, argv);
  474. if (G.sock == 0)
  475. usage("No interface set!");
  476. if (G.chain == NULL)
  477. usage("No chain set!");
  478. while (1) {
  479. FD_ZERO(&rfds);
  480. FD_SET(G.sock, &rfds);
  481. tv.tv_sec = MAX_INTERVAL;
  482. tv.tv_usec = 0;
  483. retval = select(G.sock + 1, &rfds, NULL, NULL, &tv);
  484. if (retval < 0)
  485. exit_errno("select() failed");
  486. else if (retval) {
  487. if (FD_ISSET(G.sock, &rfds)) {
  488. handle_ra(G.sock);
  489. }
  490. }
  491. else
  492. DEBUG_MSG("select() timeout expired");
  493. if (G.routers != NULL && last_update <= time(NULL) - MIN_INTERVAL) {
  494. expire_routers();
  495. // all routers could have expired, check again
  496. if (G.routers != NULL) {
  497. update_tqs();
  498. update_ebtables();
  499. last_update = time(NULL);
  500. }
  501. }
  502. }
  503. cleanup();
  504. return 0;
  505. }