?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
forwards
[corutils.git] / test_routed2.c
blob81947c89fb5d80b4d3e10bd94e6c8c7c750f32a9
1 /**
2 * Connection oriented routing user space utils
3 * Copyright (C) 2009-2014
4 * Authors:
5 * Michael Blizek
6 *
7 * This program is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU General Public License
9 * as published by the Free Software Foundation; either version 2
10 * of the License, or (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
20 * 02110-1301, USA.
21 */
22
23 #include <stdio.h>
24 #include <string.h>
25 #include <sys/socket.h>
26 #include <assert.h>
27 #include <unistd.h>
28 #include <fcntl.h>
29 #include <arpa/inet.h>
30 #include <errno.h>
31 #include <sys/epoll.h>
32
33 #include "list.h"
34 #include "libcor.h"
35
36
37 #define U32_MAX ((__u32) ((1LL << 32) - 1))
38
39
40 #define MAX_ADDRLEN 128
41
42 #define ASSERT_ERR() \
43 do { \
44 assert(0); \
45 exit(1); \
46 while (1) { \
47 } \
48 } while(0) \
49
50
51 #define ASSERT(a) \
52 do { \
53 if (!(a)) { ASSERT_ERR(); } \
54 } while(0) \
55
56
57 struct route{
58 struct node *dst;
59 };
60
61 struct route_list{
62 struct route *routes;
63 __u16 rows_alloc;
64 __u16 numroutes;
65 };
66
67 struct service_list{
68 __be16 *ports;
69 __u16 rows_alloc;
70 __u16 numports;
71 };
72
73 #define ROUTE_MAXHOPS 7
74
75 struct node{
76 char *addr;
77
78 struct route_list routes;
79
80 struct service_list services;
81
82 __u32 addrlen;
83 __u16 minhops;
84 __u8 neighs_queried;
85
86 struct list_head node_list;
87
88 __u32 route[ROUTE_MAXHOPS + 1];
89 __u32 hopcount;
90 };
91
92 #define SEARCHQUERY_NEIGHSNOTQUERIED 1
93 #define SEARCHQUERY_NODEBYADDRESS 2
94 #define SEARCHQUERY_NODEBYSERVICEPORT 3
95 struct search_query{
96 int type;
97 union {
98 struct {
99 __u32 addrlen;
100 char *addr;
101 }nodebyaddress;
102
103 __be16 serviceport;
104 }query;
105 };
106
107
108 char *localaddr;
109 __u32 localaddrlen;
110
111 __u8 export_servicelist_enabled = 0;
112
113 struct route_list localneighs;
114 struct service_list localservices;
115
116 struct list_head node_list;
117
118 int discover_finished = 0;
119 int epoll_fd = 0;
120
121 int rdsock_fd = 0;
122
123 #define EPOLL_EVENTS 16
124
125 #define EPOLLDATA_DISCOVERNETWORK 1
126 #define EPOLLDATA_RDSCONNECT 2
127 #define EPOLLDATA_RDSOCKCMD 3
128 #define EPOLLDATA_FWD_TO_R 4
129 #define EPOLLDATA_FWD_TO_LSERVICE 5
130 #define EPOLLDATA_FORWARD 6
131
132 struct nonblock_resumeinfo_connect_to_host_send{
133 int fd;
134 struct node *node;
135
136 __u8 state;
137
138 struct route_list *routes;
139 int u;
140 };
141
142 struct nonblock_resumeinfo_connect_to_host_recv{
143 int fd;
144 struct node *node;
145
146 __u8 state;
147
148 int u;
149 };
150
151 #define FORWARD_BUF_SIZE 4096
152
153 struct nonblock_resumeinfo{
154 int fd;
155
156 __u8 type;
157
158 union{
159 struct{
160 struct node *node;
161
162 __u8 state;
163
164 struct route_list *list;
165 struct service_list *service;
166
167 struct nonblock_resumeinfo_connect_to_host_send connect_send;
168 struct nonblock_resumeinfo_connect_to_host_recv connect_recv;
169
170 struct libcor_nonblock_resumeinfo lnr;
171 }discover_network;
172
173 struct{
174 __u8 state;
175
176 __u64 cookie;
177 struct node *node;
178 __be16 port;
179
180 struct nonblock_resumeinfo_connect_to_host_send connect_send;
181 struct nonblock_resumeinfo_connect_to_host_recv connect_resp;
182
183 struct libcor_nonblock_resumeinfo lnr;
184 }rds_connect;
185
186 struct{
187 struct libcor_nonblock_resumeinfo lnr;
188 }rdsock_cmd;
189
190 struct{
191 __u8 fwd_to_host;
192 char *addr;
193 __u32 addrlen;
194 __be16 targetport;
195 }fwd_to_r;
196
197 struct{
198 int af;
199 struct sockaddr *saddr;
200 socklen_t saddrlen;
201 }fwd_to_lservice;
202
203 struct{
204 /**
205 * buf gets filled by reversedir and written into
206 * this fd
207 */
208 char *buf;
209 __u32 buffill;
210 __u32 bufwritten;
211
212 __u8 is_connected;
213
214 struct nonblock_resumeinfo *reversedir;
215 }forward;
216 }data;
217 };
218
219 struct fwd_to_r_item{
220 struct list_head allfwds;
221
222 int bindfd;
223
224 __u8 fwd_to_host;
225 char *addr;
226 __u32 addrlen;
227 __be16 targetport;
228 };
229
230 struct fwd_to_lservice_item{
231 struct list_head allfwds;
232
233 int bindfd;
234
235 int af;
236 struct sockaddr *saddr;
237 socklen_t saddrlen;
238 };
239
240
241
242 #define offsetof(type, member) __builtin_offsetof (type, member)
243
244 #define container_of(ptr, type, member) ({ \
245 const typeof( ((type *)0)->member ) *__mptr = (ptr); \
246 (type *)( (char *)__mptr - offsetof(type,member) );})
247
248
249 static void set_nonblock(int fd, int value)
250 {
251 int flags;
252
253 flags = fcntl(fd, F_GETFL, 0);
254 if (flags == -1) {
255 perror("set_nonblock F_GETFL");
256 exit(1);
257 }
258
259 flags = (flags & (~(O_NONBLOCK)));
260 if (value)
261 flags = flags | O_NONBLOCK;
262
263 if (fcntl(fd, F_SETFL, flags) == -1) {
264 perror("set_nonblock F_SETFL");
265 exit(1);
266 }
267 }
268
269 static int addr_eq(char *addr1, __u32 addrlen1,
270 char *addr2, __u32 addrlen2)
271 {
272 while (addrlen1>0) {
273 if (addr1[addrlen1-1] == 0) {
274 addrlen1--;
275 } else {
276 break;
277 }
278 }
279
280 while (addrlen2>0) {
281 if (addr2[addrlen2-1] == 0) {
282 addrlen2--;
283 } else {
284 break;
285 }
286 }
287
288 if (addrlen1 != addrlen2)
289 return 0;
290
291 if (addrlen1 > MAX_ADDRLEN)
292 return 0;
293
294 if (addrlen1 != 0 && memcmp(addr1, addr2, addrlen1) != 0)
295 return 0;
296
297 return 1;
298 }
299
300 static int service_list_contains(struct service_list *services, __be16 port)
301 {
302 __u32 u;
303
304 for (u=0;u<services->numports;u++) {
305 if (services->ports[u] == port) {
306 return 1;
307 }
308 }
309
310 return 0;
311 }
312
313 static int node_matches_searchquery(struct node *n, struct search_query *q)
314 {
315 if (q->type == SEARCHQUERY_NEIGHSNOTQUERIED) {
316 return n->neighs_queried == 0 &&
317 addr_eq(n->addr, n->addrlen, localaddr,
318 localaddrlen) == 0;
319 } else if (q->type == SEARCHQUERY_NODEBYADDRESS) {
320 return addr_eq(n->addr, n->addrlen,
321 q->query.nodebyaddress.addr,
322 q->query.nodebyaddress.addrlen);
323 } else if (q->type == SEARCHQUERY_NODEBYSERVICEPORT) {
324 return service_list_contains(&(n->services),
325 q->query.serviceport);
326 } else {
327 ASSERT(0);
328 return 0;
329 }
330 }
331
332 static struct node * try_find_neigh(struct search_query *q)
333 {
334 struct list_head *curr = node_list.next;
335
336 while (curr != &node_list) {
337 struct node *currnode = container_of(curr, struct node,
338 node_list);
339
340 if (node_matches_searchquery(currnode, q)) {
341 return currnode;
342 }
343
344 curr = curr->next;
345 }
346
347 return 0;
348 }
349
350 static struct node * try_find_neigh_byaddr(__u16 addrlen, char *addr)
351 {
352 struct search_query q;
353 bzero(&q, sizeof(struct search_query));
354 q.type = SEARCHQUERY_NODEBYADDRESS;
355 q.query.nodebyaddress.addrlen = addrlen;
356 q.query.nodebyaddress.addr = addr;
357 return try_find_neigh(&q);
358 }
359
360 static struct node * try_find_neigh_byservice(__be16 serviceport)
361 {
362 struct search_query q;
363 bzero(&q, sizeof(struct search_query));
364 q.type = SEARCHQUERY_NODEBYSERVICEPORT;
365 q.query.serviceport = serviceport;
366 return try_find_neigh(&q);
367 }
368
369
370 static int check_rc(int rc, char *errormsg)
371 {
372 if (rc != RC_OK && rc != RC_WOULDBLOCK)
373 printf("%s\n", errormsg);
374 return (rc != RC_OK);
375 }
376
377 static int connect_to_host_recv(int fd, struct libcor_nonblock_resumeinfo *lnr,
378 struct nonblock_resumeinfo_connect_to_host_recv *rcr,
379 struct node *node)
380 {
381 if (rcr->state == 0) {
382 rcr->fd = fd;
383 rcr->node = node;
384
385 rcr->state = 1;
386 rcr->u=0;
387 }
388
389 ASSERT(rcr->fd = fd);
390 ASSERT(rcr->node = node);
391
392 ASSERT(node->hopcount != 0);
393
394 for (;rcr->u < node->hopcount;rcr->u++) {
395 int rc = read_resp_nonblock(fd, lnr);
396 if (rc != RC_OK)
397 return rc;
398 }
399 bzero(rcr, sizeof(struct nonblock_resumeinfo_connect_to_host_recv));
400 return RC_OK;
401 }
402
403 static int connect_to_host_send(int fd,
404 struct libcor_nonblock_resumeinfo *lnr,
405 struct nonblock_resumeinfo_connect_to_host_send *cth,
406 struct node *node)
407 {
408 if (cth->state == 0) {
409 cth->fd = fd;
410 cth->node = node;
411
412 cth->routes = &localneighs;
413
414 cth->state = 1;
415 cth->u=0;
416 }
417
418 ASSERT(cth->fd = fd);
419 ASSERT(cth->node = node);
420
421 ASSERT(node->hopcount != 0);
422
423 for (;cth->u < node->hopcount;) {
424 struct node *n = cth->routes->routes[node->route[cth->u]].dst;
425 int rc = send_connect_neigh_nonblock(fd, lnr, n->addrlen,
426 n->addr);
427 cth->u++;
428 if (rc != RC_OK)
429 return rc;
430 cth->routes = &(n->routes);
431 }
432 bzero(cth, sizeof(struct nonblock_resumeinfo_connect_to_host_send));
433 return RC_OK;
434 }
435
436 static void add_neigh(void *ptr, __u32 addrlen, char *addr)
437 {
438 struct route_list *list = (struct route_list *) ptr;
439 struct node *node;
440
441 if (addrlen > MAX_ADDRLEN)
442 return;
443
444 if (addr_eq(addr, addrlen, 0, 0))
445 return;
446
447 if (list->numroutes >= list->rows_alloc)
448 return;
449
450 ASSERT(list->routes[list->numroutes].dst == 0);
451
452 node = try_find_neigh_byaddr(addrlen, addr);
453
454 if (node == 0) {
455 node = calloc(1, sizeof(struct node));
456
457 node->addr = malloc(((int) addrlen));
458 node->addrlen = addrlen;
459 memcpy(node->addr, addr, addrlen);
460
461 list_add_tail(&(node->node_list), &node_list);
462 }
463
464 list->routes[list->numroutes].dst = node;
465 list->numroutes++;
466 }
467
468 static void init_neighlist(void *ptr, __u32 numneighs)
469 {
470 struct route_list *list = (struct route_list *) ptr;
471 if (numneighs > 16)
472 numneighs = 16;
473 #warning todo limit
474 list->rows_alloc = (__u16) numneighs;
475 list->routes = calloc(numneighs, sizeof(struct route));
476 }
477
478 static void add_service(void *ptr, __be16 port)
479 {
480 struct service_list *list = (struct service_list *) ptr;
481
482 printf("add service %u\n", ntohs(port));
483
484 if (list->numports >= list->rows_alloc)
485 return;
486
487 list->ports[list->numports] = port;
488 list->numports++;
489 }
490
491 static void init_servicelist(void *ptr, __u32 numservices)
492 {
493 struct service_list *list = (struct service_list *) ptr;
494
495 printf("init servicelist %u\n", numservices);
496
497 if (numservices > 16)
498 numservices = 16;
499 list->rows_alloc = (__u16) numservices;
500 list->numports = 0;
501 list->ports = calloc(numservices, 2);
502 }
503
504 static void print_hex(char *buf, __u32 len)
505 {
506 __u32 u;
507
508 ASSERT(len != U32_MAX);
509
510 for(u=0;u<len;u++) {
511 printf("%hhx ", buf[u]);
512 }
513 }
514
515 static void neigh_printaddr(__u16 addrlen, char *addr)
516 {
517 printf("addrlen = %d addr: ", (int) addrlen);
518 print_hex(addr, addrlen);
519 printf("\n");
520 }
521
522
523 static int __export_servicelist(int fd, char *data, __u32 len)
524 {
525 __u32 totalwritten = 0;
526
527 ASSERT(len != U32_MAX);
528
529 while (totalwritten < len) {
530 int written = write(fd, data + totalwritten, len -totalwritten);
531
532 if (written <= 0) {
533 if (errno == EINTR)
534 continue;
535
536 perror("export servicelist");
537 return 1;
538 }
539
540 totalwritten += written;
541 }
542
543 return 0;
544 }
545
546 static int _export_servicelist(int fd, char *addr, __u32 addrlen, __u32 cost,
547 struct service_list *services)
548 {
549 __u32 u;
550 char buf[24];
551 int rc;
552
553 for (u=0;u<services->numports;u++) {
554 __u32 v;
555
556 ASSERT(addrlen != U32_MAX);
557
558 for(v=0;v<addrlen;v++) {
559 if (((__u8) addr[v]) < 16)
560 rc = snprintf(&(buf[0]), 20, "0%hhx", addr[v]);
561 else
562 rc = snprintf(&(buf[0]), 20, "%hhx", addr[v]);
563 ASSERT(rc > 0);
564 ASSERT(rc < 20);
565
566 rc = __export_servicelist(fd, &(buf[0]), rc);
567 if (rc != 0)
568 return 1;
569 }
570
571 rc = snprintf(&(buf[0]), 20, ":%hu:%u\n",
572 ntohs(services->ports[u]), cost);
573 ASSERT(rc > 0);
574 ASSERT(rc < 20);
575
576 rc = __export_servicelist(fd, &(buf[0]), rc);
577 if (rc != 0)
578 return 1;
579 }
580
581 return 0;
582 }
583
584 static void export_servicelist(void)
585 {
586 char *servicelist_txtfile = "/var/lib/cor/services/txt";
587 char *servicelist_tmpfile = "/var/lib/cor/services/tmp";
588
589 int fd = 0;
590 struct list_head *curr;
591
592 if (export_servicelist_enabled == 0)
593 return;
594
595 fd = open(servicelist_tmpfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
596 if (fd <= 0) {
597 perror("export servicelist");
598 return;
599 }
600
601 if (_export_servicelist(fd, 0, 0, 0, &localservices) != 0)
602 goto err;
603
604 curr = node_list.next;
605 while (curr != &node_list) {
606 struct node *currnode = container_of(curr, struct node,
607 node_list);
608
609 curr = curr->next;
610
611 if (addr_eq(currnode->addr, currnode->addrlen, localaddr,
612 localaddrlen) != 0)
613 continue;
614
615 if (_export_servicelist(fd, currnode->addr, currnode->addrlen,
616 currnode->hopcount, &(currnode->services)) != 0)
617 goto err;
618 }
619
620 fsync(fd);
621 close(fd);
622
623 if (rename(servicelist_tmpfile, servicelist_txtfile) != 0) {
624 perror("export servicelist");
625 goto err2;
626 }
627
628 if (0) {
629 err:
630 close(fd);
631 err2:
632 unlink(servicelist_tmpfile);
633 }
634 }
635
636 static void __recalc_routes(struct node *node)
637 {
638 struct list_head *curr;
639
640 curr = node_list.next;
641 while (curr != &node_list) {
642 struct node *currnode = container_of(curr, struct node,
643 node_list);
644
645 if (currnode->hopcount < node->hopcount)
646 break;
647
648 curr = curr->next;
649 }
650
651 list_del(&(node->node_list));
652 list_add_tail(&(node->node_list), curr);
653 }
654
655 static void _recalc_routes(struct node *node)
656 {
657 struct route_list *routes = (node == 0 ?
658 &localneighs : &(node->routes));
659
660 __u32 u;
661
662 for (u=0;u<routes->numroutes;u++) {
663 struct node *target = routes->routes[u].dst;
664
665 __u32 hopcount = (node == 0 ? 0 : node->hopcount) + 1;
666
667 if (target->hopcount != 0 && target->hopcount <= hopcount)
668 continue;
669
670 if (node == 0) {
671 bzero(&(target->route[0]), sizeof(target->route));
672 } else {
673 memcpy(&(target->route[0]), &(node->route[0]),
674 sizeof(target->route));
675 }
676
677 target->hopcount = hopcount;
678 target->route[hopcount-1] = u;
679
680 __recalc_routes(target);
681 }
682 }
683
684 static void recalc_routes(void)
685 {
686 struct list_head oldnodes;
687 struct list_head *curr;
688
689 list_add(&oldnodes, &node_list);
690 list_del(&node_list);
691 init_list_head(&node_list);
692
693
694 curr = oldnodes.next;
695 while (curr != &oldnodes) {
696 struct node *currnode = container_of(curr, struct node,
697 node_list);
698
699 bzero(&(currnode->route[0]), sizeof(currnode->route));
700 currnode->hopcount = 0;
701
702 curr = curr->next;
703 }
704
705
706 _recalc_routes(0);
707
708 curr = node_list.next;
709 while (curr != &node_list) {
710 struct node *currnode = container_of(curr, struct node,
711 node_list);
712
713 _recalc_routes(currnode);
714
715 curr = curr->next;
716 }
717
718
719 curr = oldnodes.next;
720 while (curr != &oldnodes) {
721 struct node *currnode = container_of(curr, struct node,
722 node_list);
723
724 curr = curr->next;
725
726 list_del(&(currnode->node_list));
727
728 if (currnode->addr != 0) {
729 free(currnode->addr);
730 currnode->addr = 0;
731 }
732
733 free(currnode);
734 }
735
736 export_servicelist();
737 }
738
739 static int load_neigh_list(int fd, struct nonblock_resumeinfo *nr,
740 struct node *node)
741 {
742 struct libcor_nonblock_resumeinfo *lnr;
743 int rc = RC_CONNBROKEN;
744
745 ASSERT((fd == 0 && nr == 0) || (fd != 0 && nr != 0));
746
747 if (fd == 0 && nr == 0) {
748 struct epoll_event epe;
749
750 fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
751 if(fd < 0) {
752 perror("socket");
753 goto early_out;
754 }
755
756 rc = connect(fd, 0, 0);
757 if(rc < 0) {
758 perror("connect");
759 goto out;
760 }
761
762 set_nonblock(fd, 1);
763
764 nr = (struct nonblock_resumeinfo *)
765 malloc(sizeof(struct nonblock_resumeinfo));
766 bzero(nr, sizeof(struct nonblock_resumeinfo));
767 nr->fd = fd;
768 nr->type = EPOLLDATA_DISCOVERNETWORK;
769 nr->data.discover_network.node = node;
770
771 bzero(&epe, sizeof(struct epoll_event));
772 epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR |
773 EPOLLET);
774 epe.data.ptr = nr;
775 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &epe);
776 }
777
778 lnr = &(nr->data.discover_network.lnr);
779
780 ASSERT(nr->type == EPOLLDATA_DISCOVERNETWORK);
781 ASSERT(nr->data.discover_network.node == node);
782
783 if (nr->data.discover_network.state == 1) {
784 goto state_1;
785 } else if (nr->data.discover_network.state == 2) {
786 goto state_2;
787 } else if (nr->data.discover_network.state == 3) {
788 goto state_3;
789 } else if (nr->data.discover_network.state == 4) {
790 goto state_4;
791 } else if (nr->data.discover_network.state == 5) {
792 goto state_5;
793 } else if (nr->data.discover_network.state == 6) {
794 goto state_6;
795 } else if (nr->data.discover_network.state == 7) {
796 goto state_7;
797 } else if (nr->data.discover_network.state == 8) {
798 goto state_8;
799 } else if (nr->data.discover_network.state != 0) {
800 ASSERT(0);
801 }
802
803 if (node == 0) {
804 nr->data.discover_network.list = &localneighs;
805 nr->data.discover_network.service = &localservices;
806 } else {
807 neigh_printaddr(node->addrlen,
808 node->addr);
809 nr->data.discover_network.list = &(node->routes);
810 nr->data.discover_network.service = &(node->services);
811
812 nr->data.discover_network.state = 1;
813 state_1:
814 rc = connect_to_host_send(fd, lnr,
815 &(nr->data.discover_network.connect_send),
816 node);
817
818 if (check_rc(rc, "load_neigh_list: connect_to_host_send error"))
819 goto out;
820 }
821
822 rc = send_list_neigh_nonblock(fd, lnr);
823 nr->data.discover_network.state = 2;
824 if (check_rc(rc, "load_neigh_list: send_list_neigh error"))
825 goto out;
826
827 state_2:
828 rc = send_list_services_nonblock(fd, lnr);
829 nr->data.discover_network.state = 3;
830 if (check_rc(rc, "load_neigh_list: send_list_services error"))
831 goto out;
832
833 state_3:
834 if (node != 0) {
835 nr->data.discover_network.state = 4;
836 state_4:
837 rc = connect_to_host_recv(fd, lnr,
838 &(nr->data.discover_network.connect_recv),
839 node);
840 if (check_rc(rc, "load_neigh_list: connect_to_host_recv error"))
841 goto out;
842 }
843
844 nr->data.discover_network.state = 5;
845 state_5:
846 rc = read_resp_nonblock(fd, lnr);
847 if (check_rc(rc, "load_neigh_list: read_resp error"))
848 goto out;
849
850 nr->data.discover_network.state = 6;
851 state_6:
852 rc = read_neigh_list_nonblock(fd, lnr, nr->data.discover_network.list,
853 init_neighlist, add_neigh);
854 if (check_rc(rc, "load_neigh_list: read_neigh_list error"))
855 goto out;
856
857 nr->data.discover_network.state = 7;
858 state_7:
859 rc = read_resp_nonblock(fd, lnr);
860 if (check_rc(rc, "load_neigh_list: read_resp error"))
861 goto out;
862
863 nr->data.discover_network.state = 8;
864 state_8:
865 rc = read_service_list_nonblock(fd, lnr,
866 nr->data.discover_network.service,
867 init_servicelist, add_service);
868 if (check_rc(rc, "load_neigh_list: read_service_list error"))
869 goto out;
870
871 #warning todo rollback node->routes and node->services on error
872 recalc_routes();
873 out:
874 //printf("load_neigh_list state %d\n", nr->data.discover_network.state);
875 if (rc != RC_WOULDBLOCK) {
876 //printf("load_neigh_list rc %d\n", rc);
877 close(fd);
878 if (nr != 0)
879 free(nr);
880 }
881
882 early_out:
883 return rc;
884 }
885
886 static void discover_network(int fd, struct nonblock_resumeinfo *nr)
887 {
888 int rc;
889 struct search_query q;
890 bzero(&q, sizeof(struct search_query));
891 q.type = SEARCHQUERY_NEIGHSNOTQUERIED;
892
893 ASSERT((fd == 0 && nr == 0) || (fd != 0 && nr != 0));
894
895 #warning todo catch RC_CONNBROKEN
896 if (fd == 0 && nr == 0) {
897 rc = load_neigh_list(fd, nr, 0);
898 if (rc == RC_WOULDBLOCK)
899 return;
900 }
901
902 while (1) {
903 struct node *node;
904 if (nr == 0) {
905 node = try_find_neigh(&q);
906 if (node == 0)
907 break;
908 } else {
909 ASSERT(nr->type == EPOLLDATA_DISCOVERNETWORK);
910 node = nr->data.discover_network.node;
911 }
912
913 rc = load_neigh_list(fd, nr, node);
914 if (rc == RC_WOULDBLOCK)
915 return;
916 node->neighs_queried = 1;
917 fd = 0;
918 nr = 0;
919 }
920 discover_finished = 1;
921 }
922
923 static void send_unreach_error(__u64 cookie)
924 {
925 int rc;
926 // printf("send_rdsock_connecterror\n");
927 set_nonblock(rdsock_fd, 0);
928 rc = send_rdsock_connecterror(rdsock_fd, cookie,
929 CONNECTERROR_NETUNREACH);
930 set_nonblock(rdsock_fd, 1);
931 ASSERT(rc == RC_OK);
932 }
933
934 //cookie+addr are not passed on nonblocking resume
935 static void _rdscmd_connect(struct nonblock_resumeinfo *nr)
936 {
937 int rc = RC_OK;
938
939 struct libcor_nonblock_resumeinfo *lnr = &(nr->data.rds_connect.lnr);
940
941 ASSERT(nr->type == EPOLLDATA_RDSCONNECT);
942
943 if (nr->data.rds_connect.state == 1) {
944 goto state_1;
945 } else if (nr->data.rds_connect.state == 2) {
946 goto state_2;
947 } else if (nr->data.rds_connect.state == 3) {
948 goto state_3;
949 } else if (nr->data.rds_connect.state != 0) {
950 ASSERT(0);
951 }
952
953 if (nr->data.rds_connect.node == 0)
954 rc = RC_OK;
955 else
956 rc = connect_to_host_send(nr->fd, lnr,
957 &(nr->data.rds_connect.connect_send),
958 nr->data.rds_connect.node);
959 nr->data.rds_connect.state = 1;
960 if (check_rc(rc, "_rdscmd_connect: connect_to_host_send error"))
961 goto out;
962
963 state_1:
964 rc = send_connect_port_nonblock(nr->fd, lnr, nr->data.rds_connect.port);
965 nr->data.rds_connect.state = 2;
966 if (check_rc(rc, "_rdscmd_connect: send_connect_port error"))
967 goto out;
968
969 state_2:
970 if (nr->data.rds_connect.node == 0)
971 rc = RC_OK;
972 else
973 rc = connect_to_host_recv(nr->fd, lnr,
974 &(nr->data.rds_connect.connect_resp),
975 nr->data.rds_connect.node);
976 if (check_rc(rc, "_rdscmd_void_connect: connect_to_host_recv error"))
977 goto out;
978
979 nr->data.rds_connect.state = 3;
980 state_3:
981 rc = read_resp_nonblock(nr->fd, lnr);
982 if (check_rc(rc, "_rdscmd_connect: read_resp error"))
983 goto out;
984
985 rc = pass_socket(nr->fd, nr->data.rds_connect.cookie);
986 if (rc != RC_OK) {
987 printf("pass_socket error\n");
988 goto err;
989 }
990
991 return;
992
993 out:
994 if (rc == RC_WOULDBLOCK)
995 return;
996
997 err:
998 close(nr->fd);
999 send_unreach_error(nr->data.rds_connect.cookie);
1000 }
1001
1002 static int rdscmd_connect(void *ptr, __u64 cookie,
1003 struct cor_sockaddr *addr)
1004 {
1005 int fd, rc;
1006 struct nonblock_resumeinfo *nr;
1007
1008 struct epoll_event epe;
1009 struct node *node;
1010
1011 if (addr->sin_family != AF_COR) {
1012 printf("rds_connect %llu not af_cor\n", cookie);
1013 goto out_noclose;
1014 }
1015
1016 printf("rds_connect cookie: %llu, addr: ", cookie);
1017 print_hex(&(addr->addr[0]), sizeof(addr->addr));
1018 printf(" port: %u\n", ntohs(addr->port));
1019
1020 if (addr_eq(addr->addr, sizeof(addr->addr), 0, 0) != 0 ||
1021 addr_eq(addr->addr, sizeof(addr->addr), localaddr,
1022 localaddrlen) != 0) {
1023 node = 0;
1024 } else if ((node = try_find_neigh_byaddr(sizeof(addr->addr),
1025 &(addr->addr[0]))) == 0) {
1026 printf("connect_to_host host not found\n");
1027 goto out_noclose;
1028 }
1029
1030
1031 fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
1032 if(fd < 0) {
1033 perror("socket");
1034 goto out_noclose;
1035 }
1036
1037 rc = connect(fd, 0, 0);
1038 if(rc < 0) {
1039 perror("connect");
1040 goto out;
1041 }
1042
1043 set_nonblock(fd, 1);
1044
1045 nr = (struct nonblock_resumeinfo *)
1046 malloc(sizeof(struct nonblock_resumeinfo));
1047 bzero(nr, sizeof(struct nonblock_resumeinfo));
1048 nr->fd = fd;
1049 nr->type = EPOLLDATA_RDSCONNECT;
1050 nr->data.rds_connect.cookie = cookie;
1051 nr->data.rds_connect.node = node;
1052 nr->data.rds_connect.port = addr->port;
1053
1054 bzero(&epe, sizeof(struct epoll_event));
1055 epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR |
1056 EPOLLET);
1057 epe.data.ptr = nr;
1058 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &epe);
1059
1060 _rdscmd_connect(nr);
1061
1062 if (0) {
1063 out:
1064 close(fd);
1065 out_noclose:
1066 send_unreach_error(cookie);
1067 }
1068 return 0;
1069 }
1070
1071 static int proc_rdsock_cmd(int fd, struct rdsock_cmd *cmd)
1072 {
1073 if (cmd->cmd == CRD_KTU_CONNECT) {
1074 return parse_rdsock_connect(0, cmd, rdscmd_connect);
1075 } else {
1076 printf("error in proc_rdsock_cmd: unknown cmd: %u\n", cmd->cmd);
1077 ASSERT(0);
1078 return 1;
1079 }
1080 }
1081
1082 static struct nonblock_resumeinfo *_init_fwd(int fd, __u8 is_connected)
1083 {
1084 struct nonblock_resumeinfo *nr =
1085 malloc(sizeof(struct nonblock_resumeinfo));
1086
1087 struct epoll_event epe;
1088
1089 bzero(nr, sizeof(struct nonblock_resumeinfo));
1090 nr->fd = fd;
1091 nr->type = EPOLLDATA_FORWARD;
1092 nr->data.forward.buf = malloc(FORWARD_BUF_SIZE);
1093 nr->data.forward.is_connected = is_connected;
1094 bzero(&epe, sizeof(struct epoll_event));
1095 epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR | EPOLLET);
1096 epe.data.ptr = nr;
1097
1098 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, nr->fd, &epe);
1099
1100 return nr;
1101 }
1102
1103 static void init_fwd(int fd0, __u8 fd0connected, int fd1, __u8 fd1connected)
1104 {
1105 struct nonblock_resumeinfo *dir0 = _init_fwd(fd0, fd0connected);
1106 struct nonblock_resumeinfo *dir1 = _init_fwd(fd1, fd1connected);
1107
1108 dir0->data.forward.reversedir = dir1;
1109 dir1->data.forward.reversedir = dir0;
1110 }
1111
1112 static void fwd_to_r_accept(int listenerfd, __u8 fwd_to_host, char *hostaddr,
1113 __u32 hostaddrlen, __be16 targetport)
1114 {
1115 while (1) {
1116 int localfd;
1117 struct node *node;
1118 struct cor_sockaddr addr;
1119 int remotefd;
1120 __u8 remote_fd_isconnected = 0;
1121
1122 #warning todo remote_fd_isconnected = 1 bug
1123
1124 localfd = accept(listenerfd, 0, 0);
1125 if (localfd < 0)
1126 return;
1127
1128 set_nonblock(localfd, 1);
1129
1130 if (fwd_to_host != 0) {
1131 if (addr_eq(hostaddr, hostaddrlen, 0, 0) != 0 ||
1132 addr_eq(hostaddr, hostaddrlen,
1133 localaddr, localaddrlen) != 0) {
1134 node = 0;
1135 } else if ((node = try_find_neigh_byaddr(hostaddrlen,
1136 hostaddr)) == 0) {
1137 printf("fwd_to_r_accept host not found\n");
1138 goto failed;
1139 }
1140 } else {
1141 if (service_list_contains(&(localservices), targetport)
1142 != 0) {
1143 node = 0;
1144 } else if ((node = try_find_neigh_byservice(targetport))
1145 == 0) {
1146 printf("fwd_to_r_accept no host with service found\n");
1147 goto failed;
1148 }
1149 }
1150
1151 bzero(&addr, sizeof(struct cor_sockaddr));
1152 addr.sin_family = AF_COR;
1153 addr.port = targetport;
1154
1155 if (node != 0) {
1156 if (node->addrlen > MAX_ADDRLEN)
1157 goto failed;
1158 memcpy(&(addr.addr[0]), node->addr, node->addrlen);
1159 }
1160
1161
1162 remotefd = socket(PF_COR, SOCK_STREAM, 0);
1163 if (remotefd == -1) {
1164 perror("socket");
1165 goto failed;
1166 }
1167
1168 set_nonblock(remotefd, 1);
1169
1170 if (connect(remotefd, (struct sockaddr *) &addr,
1171 sizeof(struct cor_sockaddr)) != 0) {
1172 if (errno == EINPROGRESS) {
1173 remote_fd_isconnected = 0;
1174 } else {
1175 perror("connect");
1176 goto failed;
1177 }
1178 }
1179
1180 init_fwd(localfd, 1, remotefd, remote_fd_isconnected);
1181
1182 if (0) {
1183 failed:
1184 close(localfd);
1185 }
1186 }
1187 }
1188
1189 static void fwd_to_lservice_accept(int listenerfd, int af,
1190 struct sockaddr *saddr, socklen_t saddrlen)
1191 {
1192 while (1) {
1193 int localfd;
1194 int remotefd;
1195 __u8 remote_fd_isconnected = 1;
1196
1197 localfd = accept(listenerfd, 0, 0);
1198
1199 if (localfd < 0)
1200 return;
1201
1202 set_nonblock(localfd, 1);
1203
1204 remotefd = socket(af, SOCK_STREAM, 0);
1205 if (remotefd == -1) {
1206 perror("socket");
1207 goto failed;
1208 }
1209
1210 set_nonblock(remotefd, 1);
1211
1212 if (connect(remotefd, saddr, saddrlen) != 0) {
1213 if (errno == EINPROGRESS) {
1214 remote_fd_isconnected = 0;
1215 } else {
1216 perror("connect");
1217 goto failed;
1218 }
1219 }
1220
1221 init_fwd(localfd, 1, remotefd, remote_fd_isconnected);
1222
1223 if (0) {
1224 failed:
1225 close(localfd);
1226 }
1227 }
1228 }
1229
1230
1231
1232 #define RC_FORWARD_OK 1
1233 #define RC_FORWARD_WOULDBLOCK 2
1234 #define RC_FORWARD_ERROR 3
1235
1236 static int forward_write(struct nonblock_resumeinfo *nr)
1237 {
1238 __u32 buffill = nr->data.forward.buffill;
1239 int more = 0;
1240
1241 ASSERT(buffill <= FORWARD_BUF_SIZE);
1242 ASSERT(nr->data.forward.bufwritten <= buffill);
1243
1244 if (buffill == FORWARD_BUF_SIZE) {
1245 buffill -= 1;
1246 more = 1;
1247 }
1248
1249 while (nr->data.forward.bufwritten < buffill) {
1250 int rc = send(nr->fd, nr->data.forward.buf +
1251 nr->data.forward.bufwritten,
1252 buffill - nr->data.forward.bufwritten,
1253 more == 0 ? 0 : MSG_MORE);
1254
1255 if (rc < 0 && (errno == EAGAIN ||
1256 errno == EWOULDBLOCK)) {
1257 return RC_FORWARD_WOULDBLOCK;
1258 } else if (rc <= 0) {
1259 if (errno == EINTR)
1260 continue;
1261
1262 perror("forward_write");
1263 return RC_FORWARD_ERROR;
1264 } else {
1265 nr->data.forward.bufwritten += rc;
1266 ASSERT(nr->data.forward.bufwritten <= buffill);
1267 }
1268 }
1269
1270 return RC_FORWARD_OK;
1271 }
1272
1273 static int forward_read(struct nonblock_resumeinfo *nr)
1274 {
1275 int readfd = nr->data.forward.reversedir->fd;
1276 int rc;
1277
1278 ASSERT(nr->data.forward.bufwritten <= nr->data.forward.buffill);
1279
1280 if (nr->data.forward.bufwritten == nr->data.forward.buffill) {
1281 nr->data.forward.buffill = 0;
1282 nr->data.forward.bufwritten = 0;
1283 } else if (nr->data.forward.bufwritten * 3 > nr->data.forward.buffill) {
1284 memmove(nr->data.forward.buf, nr->data.forward.buf +
1285 nr->data.forward.bufwritten,
1286 nr->data.forward.buffill -
1287 nr->data.forward.bufwritten);
1288 nr->data.forward.buffill -= nr->data.forward.bufwritten;
1289 nr->data.forward.bufwritten = 0;
1290 }
1291
1292 if (nr->data.forward.buffill == FORWARD_BUF_SIZE)
1293 return RC_FORWARD_OK;
1294
1295 while (nr->data.forward.buffill < FORWARD_BUF_SIZE) {
1296 rc = recv(readfd, nr->data.forward.buf +
1297 nr->data.forward.buffill,
1298 FORWARD_BUF_SIZE - nr->data.forward.buffill, 0);
1299
1300 if (rc == 0) {
1301 #warning todo eof
1302 return RC_FORWARD_OK;
1303 } else if (rc < 0) {
1304 if (errno == EAGAIN || errno == EWOULDBLOCK) {
1305 return RC_FORWARD_WOULDBLOCK;
1306 } else if (errno == EINTR) {
1307 continue;
1308 } else {
1309 perror("forward_read");
1310 return RC_FORWARD_ERROR;
1311 }
1312 } else {
1313 nr->data.forward.buffill += rc;
1314 ASSERT(nr->data.forward.buffill <= FORWARD_BUF_SIZE);
1315 }
1316 }
1317
1318 return RC_FORWARD_OK;
1319 }
1320
1321 static void _forward(struct nonblock_resumeinfo *nr)
1322 {
1323 while (1) {
1324 int rc;
1325
1326 rc = forward_read(nr);
1327 if (rc == RC_FORWARD_ERROR)
1328 goto err;
1329 if (nr->data.forward.bufwritten == nr->data.forward.buffill)
1330 break;
1331
1332 rc = forward_write(nr);
1333 if (rc == RC_FORWARD_ERROR)
1334 goto err;
1335 else if (rc == RC_FORWARD_WOULDBLOCK)
1336 return;
1337 }
1338
1339 #warning todo call EPOLL_CTL_DEL + free where needed (some places calling close do not)
1340 if (0) {
1341 err:
1342 epoll_ctl(epoll_fd, EPOLL_CTL_DEL,
1343 nr->data.forward.reversedir->fd, 0);
1344 close(nr->data.forward.reversedir->fd);
1345 epoll_ctl(epoll_fd, EPOLL_CTL_DEL, nr->fd, 0);
1346 close(nr->fd);
1347
1348 #warning todo free after all events are processed
1349 /* free(nr->data.forward.reversedir);
1350 free(nr); */
1351 }
1352
1353 }
1354
1355 static void forward(struct nonblock_resumeinfo *nr, __u32 eventflags)
1356 {
1357 ASSERT(nr->type == EPOLLDATA_FORWARD);
1358 ASSERT(nr->data.forward.reversedir->type == EPOLLDATA_FORWARD);
1359
1360 if ((eventflags & EPOLLOUT) != 0)
1361 nr->data.forward.is_connected = 1;
1362
1363 if (nr->data.forward.is_connected == 0 ||
1364 nr->data.forward.reversedir->data.forward.is_connected
1365 == 0)
1366 return;
1367
1368 _forward(nr);
1369 _forward(nr->data.forward.reversedir);
1370 }
1371
1372 static void nonblock_resume(struct nonblock_resumeinfo *nr, __u32 eventflags)
1373 {
1374 if (nr->type == EPOLLDATA_DISCOVERNETWORK) {
1375 int rc = resume_send_ifneeded(nr->fd,
1376 &(nr->data.discover_network.lnr));
1377
1378 //printf("load_neigh_list state = %d rc = %d\n", nr->data.discover_network.state, rc);
1379
1380 if (rc != RC_OK)
1381 return;
1382
1383 discover_network(nr->fd, nr);
1384 } else if (nr->type == EPOLLDATA_RDSCONNECT) {
1385 int rc = resume_send_ifneeded(nr->fd,
1386 &(nr->data.discover_network.lnr));
1387
1388 if (rc != RC_OK)
1389 return;
1390
1391 _rdscmd_connect(nr);
1392 } else if (nr->type == EPOLLDATA_RDSOCKCMD) {
1393 struct rdsock_cmd cmd;
1394
1395 int rc = resume_send_ifneeded(nr->fd,
1396 &(nr->data.rdsock_cmd.lnr));
1397
1398 if (rc != RC_OK)
1399 return;
1400
1401 rc = read_rdsock_cmd_nonblock(rdsock_fd,
1402 &(nr->data.rdsock_cmd.lnr), &cmd);
1403 ASSERT(rc != RC_CONNBROKEN);
1404 if (rc == RC_WOULDBLOCK)
1405 return;
1406
1407 rc = proc_rdsock_cmd(rdsock_fd, &cmd);
1408 free_rdsockcmd_data(&cmd);
1409 ASSERT(rc == 0);
1410 } else if (nr->type == EPOLLDATA_FWD_TO_R) {
1411 fwd_to_r_accept(nr->fd, nr->data.fwd_to_r.fwd_to_host,
1412 nr->data.fwd_to_r.addr,
1413 nr->data.fwd_to_r.addrlen,
1414 nr->data.fwd_to_r.targetport);
1415 } else if (nr->type == EPOLLDATA_FWD_TO_LSERVICE) {
1416 fwd_to_lservice_accept(nr->fd, nr->data.fwd_to_lservice.af,
1417 nr->data.fwd_to_lservice.saddr,
1418 nr->data.fwd_to_lservice.saddrlen);
1419 } else if (nr->type == EPOLLDATA_FORWARD) {
1420 forward(nr, eventflags);
1421 } else {
1422 ASSERT(0);
1423 }
1424 }
1425
1426 static int rdsock_negotiate_version(void)
1427 {
1428 struct rdsock_cmd cmd;
1429 __u32 versionmin = 1000;
1430 __u32 versionmax = 0;
1431 int rc;
1432
1433 rc = read_rdsock_cmd(rdsock_fd, &cmd);
1434 if (rc != RC_OK) {
1435 printf("read_rdsock_cmd rc = %d\n", rc);
1436 return 1;
1437 }
1438
1439 if (cmd.cmd != CRD_KTU_SUPPORTEDVERSIONS) {
1440 printf("rhsock supportedversions not sent\n", rc);
1441 return 1;
1442 }
1443
1444 rc = parse_rdsock_supported_versions(&cmd, &versionmin, &versionmax);
1445 if (rc != 0) {
1446 printf("parse_rdsock_supported_versions rc = %d\n", rc);
1447 return 1;
1448 }
1449
1450 /* printf("rdsock_negotiate_version versionmin %u versionmax %u\n",
1451 versionmin, versionmax); */
1452
1453 if (versionmin != 0) {
1454 printf("rdsock_negotiate_version versionmin of kernel is %u, "
1455 "but needs to be 0\n"
1456 "You probably need to upgrade corutils or "
1457 "downgrade the kernel\n", versionmin);
1458 return 1;
1459 }
1460
1461 rc = send_rdsock_version(rdsock_fd, 0);
1462 ASSERT(rc == RC_OK);
1463
1464 return 0;
1465 }
1466
1467 static void epoll_add_fwd_to_r(struct list_head *r_fwds)
1468 {
1469 while (list_empty(r_fwds) == 0) {
1470 struct fwd_to_r_item *item = container_of(r_fwds->next,
1471 struct fwd_to_r_item, allfwds);
1472
1473 struct nonblock_resumeinfo *nr;
1474
1475 struct epoll_event epe;
1476
1477 set_nonblock(item->bindfd, 1);
1478
1479 nr = malloc(sizeof(struct nonblock_resumeinfo));
1480 bzero(nr, sizeof(struct nonblock_resumeinfo));
1481 nr->fd = item->bindfd;
1482 nr->type = EPOLLDATA_FWD_TO_R;
1483 nr->data.fwd_to_r.fwd_to_host = item->fwd_to_host;
1484 nr->data.fwd_to_r.addr = item->addr;
1485 nr->data.fwd_to_r.addrlen = item->addrlen;
1486 nr->data.fwd_to_r.targetport = item->targetport;
1487 bzero(&epe, sizeof(struct epoll_event));
1488 epe.events = (EPOLLIN | EPOLLERR);
1489 epe.data.ptr = nr;
1490
1491 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, item->bindfd, &epe);
1492
1493 list_del(&(item->allfwds));
1494 free(item);
1495 }
1496 }
1497
1498 static void epoll_add_fwd_to_lservice(struct list_head *lservice_fwds)
1499 {
1500 while (list_empty(lservice_fwds) == 0) {
1501 struct fwd_to_lservice_item *item = container_of(
1502 lservice_fwds->next,
1503 struct fwd_to_lservice_item,
1504 allfwds);
1505
1506 struct nonblock_resumeinfo *nr;
1507
1508 struct epoll_event epe;
1509
1510 set_nonblock(item->bindfd, 1);
1511
1512 nr = malloc(sizeof(struct nonblock_resumeinfo));
1513 bzero(nr, sizeof(struct nonblock_resumeinfo));
1514 nr->fd = item->bindfd;
1515 nr->type = EPOLLDATA_FWD_TO_LSERVICE;
1516 nr->data.fwd_to_lservice.af = item->af;
1517 nr->data.fwd_to_lservice.saddr = item->saddr;
1518 nr->data.fwd_to_lservice.saddrlen = item->saddrlen;
1519 bzero(&epe, sizeof(struct epoll_event));
1520 epe.events = (EPOLLIN | EPOLLERR);
1521 epe.data.ptr = nr;
1522
1523 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, item->bindfd, &epe);
1524
1525 list_del(&(item->allfwds));
1526 free(item);
1527 }
1528 }
1529
1530
1531 static int parse_hexchar(char c)
1532 {
1533 if (c == '0')
1534 return 0;
1535 else if (c == '1')
1536 return 1;
1537 else if (c == '2')
1538 return 2;
1539 else if (c == '3')
1540 return 3;
1541 else if (c == '4')
1542 return 4;
1543 else if (c == '5')
1544 return 5;
1545 else if (c == '6')
1546 return 6;
1547 else if (c == '7')
1548 return 7;
1549 else if (c == '8')
1550 return 8;
1551 else if (c == '9')
1552 return 9;
1553 else if (c == 'A' || c == 'a')
1554 return 10;
1555 else if (c == 'B' || c == 'b')
1556 return 11;
1557 else if (c == 'C' || c == 'c')
1558 return 12;
1559 else if (c == 'D' || c == 'd')
1560 return 13;
1561 else if (c == 'E' || c == 'e')
1562 return 14;
1563 else if (c == 'F' || c == 'f')
1564 return 15;
1565 else
1566 return -1;
1567 }
1568
1569 static int parse_hex(char *target, char *src, int srclen)
1570 {
1571 int u;
1572
1573 ASSERT(src != 0);
1574 ASSERT(target != 0);
1575 ASSERT(srclen > 0);
1576 ASSERT((srclen%2) == 0);
1577
1578 for(u=0;u<(srclen/2);u++) {
1579 int high = parse_hexchar(src[u*2]);
1580 int low = parse_hexchar(src[u*2+1]);
1581
1582 if (high == -1 || low == -1)
1583 return -1;
1584
1585 target[u] = (char) ((high << 4) + low);
1586 }
1587
1588 return 0;
1589 }
1590
1591 static int _parse_addr(char *arg, size_t arg_len, char **addr, __u32 *addrlen,
1592 int noaddr_allowed)
1593 {
1594 char *noaddr = "noaddr";
1595
1596 if (arg_len > 1048576)
1597 return 1;
1598
1599 if (noaddr_allowed != 0 && arg_len == 6 && memcmp(arg, noaddr, 6) == 0){
1600 *addr = 0;
1601 *addrlen = 0;
1602 return 0;
1603 }
1604
1605 if ((arg_len < 2) || (arg_len%2) != 0)
1606 goto parseerror;
1607
1608 *addr = malloc(arg_len/2);
1609 *addrlen = arg_len/2;
1610
1611 if (parse_hex(*addr, arg, arg_len) != 0)
1612 goto parseerror;
1613
1614 return 0;
1615
1616 parseerror:
1617 fprintf(stderr, "error: argv_addr is not a valid address (contains "
1618 "non-hex characters or number of hex characters is not "
1619 "a multiple of 2)\n");
1620 return 1;
1621 }
1622
1623 static size_t _parse_addr_lastarglen(char *arg, size_t len)
1624 {
1625 size_t lastarglen = 0;
1626
1627 while (lastarglen < len) {
1628 if (arg[len-lastarglen-1] == ':')
1629 break;
1630 lastarglen++;
1631 }
1632 return lastarglen;
1633 }
1634
1635 static int parse_addr(char *arg, size_t arg_len, size_t *arg_addrlen,
1636 char **addr, __u32 *addrlen)
1637 {
1638 size_t lastarglen = _parse_addr_lastarglen(arg, arg_len);
1639
1640 if (lastarglen == 0 || lastarglen >= arg_len)
1641 return 1;
1642
1643 *arg_addrlen = lastarglen + 1; // +1 ... ':'
1644
1645 return _parse_addr(arg + arg_len - lastarglen, lastarglen,
1646 addr, addrlen, 0);
1647 }
1648
1649 static int _parse_port(char *arg, size_t len, __u16 *port)
1650 {
1651 char *tmpbuf;
1652 char *endptr = 0;
1653
1654 long int tmpport = 0;
1655
1656 tmpbuf = malloc(len+1);
1657 memcpy(tmpbuf, arg, len);
1658 tmpbuf[len] = 0;
1659
1660 tmpport = strtol(tmpbuf, &endptr, 10);
1661 if (tmpbuf[0] == 0 || endptr == 0 || endptr != tmpbuf + len) {
1662 free(tmpbuf);
1663 tmpbuf = 0;
1664 return 1;
1665 }
1666 free(tmpbuf);
1667 tmpbuf = 0;
1668
1669 if (tmpport <= 0 || tmpport >= 65536)
1670 return 1;
1671
1672 *port = (__u16) tmpport;
1673
1674 return 0;
1675 }
1676
1677 static int parse_port_firstarg(char *arg, size_t len, size_t *p_portlen,
1678 __u16 *port)
1679 {
1680 size_t portlen = 0;
1681
1682 while (portlen < len) {
1683 if (arg[portlen] == ':')
1684 break;
1685 portlen++;
1686 }
1687
1688 if (portlen == 0 || portlen >= len)
1689 return 1;
1690
1691 if (_parse_port(arg, portlen, port) != 0)
1692 return 1;
1693
1694 *p_portlen = portlen + 1;// +1 ... ':'
1695 return 0;
1696 }
1697
1698 static int parse_port_lastarg(char *arg, size_t len, size_t *p_portlen,
1699 __u16 *port)
1700 {
1701 size_t portlen = 0;
1702
1703 portlen = _parse_addr_lastarglen(arg, len);
1704
1705 if (portlen == 0 || portlen >= len)
1706 return 1;
1707
1708 if (_parse_port(arg + len - portlen, portlen, port) != 0)
1709 return 1;
1710
1711 *p_portlen = portlen + 1;// +1 ... ':'
1712 return 0;
1713 }
1714
1715 static int parse_inetaddr(char *addr, size_t len, int *af,
1716 struct sockaddr **saddr, socklen_t *saddrlen)
1717 {
1718 size_t portlen = 0;
1719
1720 __u16 port;
1721
1722 char *host;
1723
1724 struct in_addr inaddr;
1725 struct in6_addr in6addr;
1726
1727 if (parse_port_lastarg(addr, len, &portlen, &port) != 0)
1728 return 1;
1729
1730 if (portlen >= len)
1731 return 1;
1732
1733 host = malloc(len - portlen + 1);
1734 memcpy(host, addr, len - portlen);
1735 host[len - portlen] = 0;
1736
1737 if (inet_pton(AF_INET, host, &inaddr) == 1) {
1738 struct sockaddr_in *ret;
1739
1740 free(host);
1741 host = 0;
1742
1743 ret = malloc(sizeof(struct sockaddr_in));
1744 bzero(ret, sizeof(struct sockaddr_in));
1745
1746 ret->sin_family = AF_INET;
1747 ret->sin_port = htons(port);
1748 memcpy(&(ret->sin_addr), &inaddr, sizeof(inaddr));
1749
1750 *af = AF_INET;
1751 *saddr = (struct sockaddr *) ret;
1752 *saddrlen = sizeof(struct sockaddr_in);
1753
1754 return 0;
1755 } else if (inet_pton(AF_INET6, host, &in6addr) == 1) {
1756 free(host);
1757 host = 0;
1758
1759 printf("sorry IPv6 support is not implemented yet\n");
1760
1761 return 1;
1762 } else {
1763 free(host);
1764 host = 0;
1765 return 1;
1766 }
1767 }
1768
1769 static int parse_fwd_to_r(char *argv_fwd,
1770 struct list_head *r_fwds, __u8 fwd_to_host)
1771 {
1772 struct fwd_to_r_item *item;
1773 int af;
1774 struct sockaddr *saddr;
1775 socklen_t saddrlen;
1776
1777 size_t portlen = 0;
1778
1779 char *addr = 0;
1780 __u32 addrlen = 0;
1781 __u16 serviceport;
1782
1783 size_t len = strlen(argv_fwd);
1784
1785 int optval;
1786
1787 if (parse_port_lastarg(argv_fwd, len, &portlen, &serviceport) != 0)
1788 return 1;
1789
1790 len -= portlen;
1791
1792 if (fwd_to_host != 0) {
1793 size_t arg_addrlen = 0;
1794 if (parse_addr(argv_fwd, len, &arg_addrlen,
1795 &addr, &addrlen) != 0)
1796 return 1;
1797 len -= arg_addrlen;
1798 }
1799
1800 if (parse_inetaddr(argv_fwd, len, &af, &saddr, &saddrlen) != 0)
1801 return 1;
1802
1803 item = malloc(sizeof(struct fwd_to_r_item));
1804 item->bindfd = socket(af, SOCK_STREAM, 0);
1805 if (item->bindfd < 0) {
1806 perror("socket");
1807 goto err_noclose;
1808 }
1809 optval = 1;
1810 if (setsockopt(item->bindfd, SOL_SOCKET, SO_REUSEADDR, &optval,
1811 sizeof(optval)) != 0) {
1812 perror("setsockopt");
1813 goto err;
1814 }
1815 if (bind(item->bindfd, saddr, saddrlen) != 0) {
1816 perror("bind");
1817 goto err;
1818 }
1819 if (listen(item->bindfd, 100) != 0) {
1820 perror("listen");
1821 goto err;
1822 }
1823 item->fwd_to_host = fwd_to_host;
1824 if (fwd_to_host) {
1825 item->addr = addr;
1826 item->addrlen = addrlen;
1827 }
1828 item->targetport = htons(serviceport);
1829
1830 list_add_tail(&(item->allfwds), r_fwds);
1831
1832 free(saddr);
1833
1834 return 0;
1835
1836 err:
1837 close(item->bindfd);
1838 err_noclose:
1839 free(item);
1840 free(saddr);
1841
1842 return 1;
1843 }
1844
1845 static int parse_fwd_to_lservice(char *argv_fwd,
1846 struct list_head *lservice_fwds, __u8 publish)
1847 {
1848 size_t portlen = 0;
1849
1850 __u16 serviceport;
1851
1852 struct cor_sockaddr bind_saddr;
1853
1854 int optval;
1855
1856 struct fwd_to_lservice_item *item;
1857
1858 int af;
1859 struct sockaddr *saddr;
1860 socklen_t saddrlen;
1861
1862 size_t len = strlen(argv_fwd);
1863
1864 if (parse_port_firstarg(argv_fwd, len, &portlen, &serviceport) != 0)
1865 return 1;
1866
1867 argv_fwd += portlen;
1868 len -= portlen;
1869
1870 if (parse_inetaddr(argv_fwd, len, &af, &saddr, &saddrlen) != 0)
1871 return 1;
1872
1873 memset(&bind_saddr, 0, sizeof(struct cor_sockaddr));
1874 bind_saddr.sin_family = AF_COR;
1875 bind_saddr.port = htons(serviceport);
1876
1877 #warning todo
1878
1879 item = malloc(sizeof(struct fwd_to_lservice_item));
1880 item->bindfd = socket(PF_COR, SOCK_STREAM, 0);
1881 if (item->bindfd < 0) {
1882 perror("socket");
1883 goto err;
1884 }
1885 optval = publish == 0 ? 0 : 1;
1886 if (setsockopt(item->bindfd, SOL_COR, COR_PUBLISH_SERVICE, &optval,
1887 sizeof(optval)) != 0) {
1888 perror("setsockopt");
1889 goto err;
1890 }
1891 if (bind(item->bindfd, (struct sockaddr *) &bind_saddr,
1892 sizeof(bind_saddr)) != 0) {
1893 perror("bind");
1894 goto err;
1895 }
1896 if (listen(item->bindfd, 100) != 0) {
1897 perror("listen");
1898 goto err;
1899 }
1900
1901 item->af = af;
1902 item->saddr = saddr;
1903 item->saddrlen = saddrlen;
1904
1905 list_add_tail(&(item->allfwds), lservice_fwds);
1906
1907 return 0;
1908
1909 err:
1910 close(item->bindfd);
1911 err_noclose:
1912 free(item);
1913 free(saddr);
1914
1915 return 1;
1916 }
1917
1918 static int parse_args(int argc, char *argv[], struct list_head *r_fwds,
1919 struct list_head *lservice_fwds)
1920 {
1921 int rc;
1922 int addrfound = 0;
1923 __u32 argsconsumed = 0;
1924
1925 if (argc > 10000)
1926 goto usage;
1927
1928 while (1) {
1929 if (argc <= argsconsumed + 1)
1930 break;
1931
1932 if (strcmp(argv[argsconsumed + 1], "--addr") == 0) {
1933 if (argc <= argsconsumed + 2)
1934 goto usage;
1935
1936 if (addrfound != 0)
1937 goto usage;
1938
1939 rc = _parse_addr(argv[argsconsumed + 2],
1940 strlen(argv[argsconsumed + 2]),
1941 &localaddr, &localaddrlen, 1);
1942 if (rc != 0)
1943 goto usage;
1944
1945 addrfound = 1;
1946
1947 argsconsumed += 2;
1948 } else if (strcmp(argv[argsconsumed + 1],
1949 "--export-servicelist") == 0) {
1950 export_servicelist_enabled = 1;
1951
1952 argsconsumed += 1;
1953 } else if (strcmp(argv[argsconsumed + 1],
1954 "--fwd-to-rservice") == 0) {
1955 if (argc <= argsconsumed + 2)
1956 goto usage;
1957
1958 rc = parse_fwd_to_r(argv[argsconsumed + 2],
1959 r_fwds, 0);
1960 if (rc != 0)
1961 goto usage;
1962
1963 argsconsumed += 2;
1964 } else if (strcmp(argv[argsconsumed + 1],
1965 "--fwd-to-rhost") == 0) {
1966 if (argc <= argsconsumed + 2)
1967 goto usage;
1968
1969 rc = parse_fwd_to_r(argv[argsconsumed + 2],
1970 r_fwds, 1);
1971 if (rc != 0)
1972 goto usage;
1973
1974 argsconsumed += 2;
1975 } else if (strcmp(argv[argsconsumed + 1],
1976 "--fwd-to-lservice") == 0) {
1977 if (argc <= argsconsumed + 2)
1978 goto usage;
1979
1980 rc = parse_fwd_to_lservice(argv[argsconsumed + 2],
1981 lservice_fwds, 0);
1982 if (rc != 0)
1983 goto usage;
1984 argsconsumed += 2;
1985 } else if (strcmp(argv[argsconsumed + 1],
1986 "--fwd-to-lservice-pub") == 0) {
1987 if (argc <= argsconsumed + 2)
1988 goto usage;
1989
1990 rc = parse_fwd_to_lservice(argv[argsconsumed + 2],
1991 lservice_fwds, 1);
1992 if (rc != 0)
1993 goto usage;
1994
1995 argsconsumed += 2;
1996 } else {
1997 goto usage;
1998 }
1999 }
2000
2001 if (addrfound == 0)
2002 goto usage;
2003
2004 if (0) {
2005 usage:
2006 fprintf(stderr, "usage: test_routed2"
2007 " [--export-servicelist]"
2008 " [--fwd-to-rservice bindaddr:bindport:remoteport]"
2009 " [--fwd-to-rhost bindaddr:bindport:remotaaddr:remoteport]"
2010 " [--fwd-to-lservice bindport:localaddr:localport]"
2011 " [--fwd-to-lservice-pub bindport:localaddr:localport]"
2012 " --addr addr (even number of hex digits or noaddr)\n");
2013 rc = 1;
2014 }
2015
2016 return rc;
2017 }
2018
2019 int main(int argc, char *argv[])
2020 {
2021 int rc;
2022 struct list_head r_fwds;
2023 struct list_head lservice_fwds;
2024
2025 struct nonblock_resumeinfo rds_nr;
2026
2027 int discover_finished_executed = 0;
2028
2029 discover_finished = 0;
2030 epoll_fd = 0;
2031
2032 init_list_head(&r_fwds);
2033 init_list_head(&lservice_fwds);
2034
2035 umask(0);
2036
2037 rc = parse_args(argc, argv, &r_fwds, &lservice_fwds);
2038 if (rc != 0)
2039 goto out;
2040
2041
2042 init_list_head(&node_list);
2043
2044 epoll_fd = epoll_create(1);
2045 if (epoll_fd <= 0) {
2046 perror("epoll_create");
2047 goto out;
2048 }
2049
2050
2051 rdsock_fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RDEAMON);
2052 if (rdsock_fd < 0) {
2053 perror("socket");
2054 goto out;
2055 }
2056
2057 rc = connect(rdsock_fd, 0, 0);
2058 if (rc < 0) {
2059 perror("connect");
2060 goto out;
2061 }
2062
2063 rc = rdsock_negotiate_version();
2064 if (rc != 0)
2065 goto out;
2066
2067 rc = send_rdsock_up(rdsock_fd, localaddr, localaddrlen);
2068 ASSERT(rc == RC_OK);
2069
2070 set_nonblock(rdsock_fd, 1);
2071
2072 epoll_add_fwd_to_lservice(&lservice_fwds);
2073
2074 sleep(30);
2075
2076 discover_network(0, 0);
2077
2078 while (1) {
2079 int u;
2080 struct epoll_event events[EPOLL_EVENTS];
2081 int rdycnt;
2082
2083 if (discover_finished_executed == 0 && discover_finished != 0) {
2084 struct epoll_event rds_epe;
2085
2086 bzero(&rds_nr, sizeof(struct nonblock_resumeinfo));
2087 rds_nr.fd = rdsock_fd;
2088 rds_nr.type = EPOLLDATA_RDSOCKCMD;
2089 bzero(&rds_epe, sizeof(struct epoll_event));
2090 rds_epe.events = (EPOLLIN | EPOLLRDHUP | EPOLLERR);
2091 rds_epe.data.ptr = &rds_nr;
2092
2093 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, rdsock_fd, &rds_epe);
2094
2095 epoll_add_fwd_to_r(&r_fwds);
2096
2097 discover_finished_executed = 1;
2098 printf("discover finished\n");
2099 }
2100
2101 rdycnt = epoll_wait(epoll_fd, events, EPOLL_EVENTS, -1);
2102
2103 for (u=0;u<rdycnt;u++) {
2104 struct nonblock_resumeinfo *nr = events[u].data.ptr;
2105 __u32 eventflags = events[u].events;
2106 nonblock_resume(nr, eventflags);
2107 }
2108 }
2109
2110 out:
2111 return 1;
2112 }
connection orienter routing user space examples and tools