test_routed2.c

   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 <sys/epoll.h>
  30
  31 #include "list.h"
  32 #include "libcor.h"
  33
  34
  35 #define MAX_ADDRLEN 128
  36
  37
  38 struct route{
  39         struct node *dst;
  40 };
  41
  42 struct route_list{
  43         struct route *routes;
  44         __u16 rows_alloc;
  45         __u16 numroutes;
  46 };
  47
  48 struct service_list{
  49         __be16 *ports;
  50         __u16 rows_alloc;
  51         __u16 numports;
  52 };
  53
  54 #define ROUTE_MAXHOPS 7
  55
  56 struct node{
  57         char *addr;
  58
  59         struct route_list routes;
  60
  61         struct service_list services;
  62
  63         __u16 addrlen;
  64         __u16 minhops;
  65         __u8 neighs_queried;
  66
  67         struct list_head node_list;
  68
  69         __u32 route[ROUTE_MAXHOPS + 1];
  70         __u32 hopcount;
  71 };
  72
  73 #define SEARCHQUERY_NEIGHSNOTQUERIED 1
  74 #define SEARCHQUERY_NODEBYADDRESS 2
  75 struct search_query{
  76         int type;
  77         union {
  78                 struct {
  79                         __u16 addrlen;
  80                         char *addr;
  81                 }nodebyaddress;
  82         }query;
  83 };
  84
  85
  86 char *localaddr;
  87 __u32 localaddrlen;
  88
  89 struct route_list localneighs;
  90 struct service_list localservices;
  91
  92 struct list_head node_list;
  93
  94 int discover_finished = 0;
  95 int epoll_fd = 0;
  96
  97 int rdsock_fd = 0;
  98
  99 #define EPOLL_EVENTS 16
 100
 101 #define EPOLLDATA_DISCOVERNETWORK 1
 102 #define EPOLLDATA_RDSCONNECT 2
 103 #define EPOLLDATA_RDSOCKCMD 3
 104
 105 struct nonblock_resumeinfo_connect_to_host_send{
 106         int fd;
 107         struct node *node;
 108
 109         __u8 state;
 110
 111         struct route_list *routes;
 112         int u;
 113 };
 114
 115 struct nonblock_resumeinfo_connect_to_host_recv{
 116         int fd;
 117         struct node *node;
 118
 119         __u8 state;
 120
 121         int u;
 122 };
 123
 124 struct nonblock_resumeinfo{
 125         int fd;
 126
 127         __u8 type;
 128
 129         union{
 130                 struct{
 131                         struct node *node;
 132
 133                         __u8 state;
 134
 135                         struct route_list *list;
 136                         struct service_list *service;
 137
 138                         struct nonblock_resumeinfo_connect_to_host_send connect_send;
 139                         struct nonblock_resumeinfo_connect_to_host_recv connect_recv;
 140
 141                         struct libcor_nonblock_resumeinfo lnr;
 142                 }discover_network;
 143
 144                 struct{
 145                         __u8 state;
 146
 147                         __u64 cookie;
 148                         struct node *node;
 149
 150                         struct nonblock_resumeinfo_connect_to_host_send connect_send;
 151                         struct nonblock_resumeinfo_connect_to_host_recv connect_resp;
 152
 153                         struct libcor_nonblock_resumeinfo lnr;
 154                 }rds_connect;
 155
 156                 struct{
 157                         struct libcor_nonblock_resumeinfo lnr;
 158                 }rdsock_cmd;
 159         }data;
 160 };
 161
 162 #define offsetof(type, member)  __builtin_offsetof (type, member)
 163
 164 #define container_of(ptr, type, member) ({                              \
 165                 const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
 166                 (type *)( (char *)__mptr - offsetof(type,member) );})
 167
 168
 169 static void set_nonblock(int fd, int value)
 170 {
 171         int flags;
 172
 173         flags = fcntl(fd, F_GETFL, 0);
 174         if (flags == -1) {
 175                 perror("set_nonblock F_GETFL");
 176                 exit(1);
 177         }
 178
 179         flags = (flags & (~(O_NONBLOCK)));
 180         if (value)
 181                 flags = flags | O_NONBLOCK;
 182
 183         if (fcntl(fd, F_SETFL, flags) == -1) {
 184                 perror("set_nonblock F_SETFL");
 185                 exit(1);
 186         }
 187 }
 188
 189 static int addr_eq(char *addr1, __u32 addrlen1,
 190                 char *addr2, __u32 addrlen2)
 191 {
 192         __u16 cmplen = addrlen1;
 193         __u32 u;
 194
 195         if (addrlen2 < cmplen)
 196                 cmplen = addrlen2;
 197
 198         if (cmplen > MAX_ADDRLEN)
 199                 return 0;
 200
 201         if (memcmp(addr1, addr2, cmplen) != 0)
 202                 return 0;
 203
 204         for (u=cmplen;u<addrlen1;u++) {
 205                 if (addr1[u] != 0) {
 206                         return 0;
 207                 }
 208         }
 209         for (u=cmplen;u<addrlen2;u++) {
 210                 if (addr2[u] != 0) {
 211                         return 0;
 212                 }
 213         }
 214         return 1;
 215 }
 216
 217 static int node_matches_searchquery(struct node *n, struct search_query *q)
 218 {
 219         if (q->type == SEARCHQUERY_NEIGHSNOTQUERIED) {
 220                 return n->neighs_queried == 0;
 221         } else if (q->type == SEARCHQUERY_NODEBYADDRESS) {
 222                 return addr_eq(n->addr, n->addrlen,
 223                                 q->query.nodebyaddress.addr,
 224                                 q->query.nodebyaddress.addrlen);
 225         } else {
 226                 assert(0);
 227                 return 0;
 228         }
 229 }
 230
 231 static struct node * try_find_neigh(struct search_query *q)
 232 {
 233         struct list_head *curr = node_list.next;
 234
 235         while (curr != &node_list) {
 236                 struct node *currnode = container_of(curr, struct node,
 237                                 node_list);
 238
 239                 if (node_matches_searchquery(currnode, q)) {
 240                         return currnode;
 241                 }
 242
 243                 curr = curr->next;
 244         }
 245
 246         return 0;
 247 }
 248
 249 static struct node * try_find_neigh_byaddr(__u16 addrlen, char *addr)
 250 {
 251         struct search_query q;
 252         bzero(&q, sizeof(struct search_query));
 253         q.type = SEARCHQUERY_NODEBYADDRESS;
 254         q.query.nodebyaddress.addrlen = addrlen;
 255         q.query.nodebyaddress.addr = addr;
 256         return try_find_neigh(&q);
 257 }
 258
 259 static int check_rc(int rc, char *errormsg)
 260 {
 261         if (rc != RC_OK && rc != RC_WOULDBLOCK)
 262                 printf("%s\n", errormsg);
 263         return (rc != RC_OK);
 264 }
 265
 266 static int connect_to_host_recv(int fd, struct libcor_nonblock_resumeinfo *lnr,
 267                 struct nonblock_resumeinfo_connect_to_host_recv *rcr,
 268                 struct node *node)
 269 {
 270         if (rcr->state == 0) {
 271                 rcr->fd = fd;
 272                 rcr->node = node;
 273
 274                 rcr->state = 1;
 275                 rcr->u=0;
 276         }
 277
 278         assert(rcr->fd = fd);
 279         assert(rcr->node = node);
 280
 281         assert(node->hopcount != 0);
 282
 283         for (;rcr->u < node->hopcount;rcr->u++) {
 284                 int rc = read_resp_nonblock(fd, lnr);
 285                 if (rc != RC_OK)
 286                         return rc;
 287         }
 288         bzero(rcr, sizeof(struct nonblock_resumeinfo_connect_to_host_recv));
 289         return RC_OK;
 290 }
 291
 292 static int connect_to_host_send(int fd,
 293                 struct libcor_nonblock_resumeinfo *lnr,
 294                 struct nonblock_resumeinfo_connect_to_host_send *cth,
 295                 struct node *node)
 296 {
 297         if (cth->state == 0) {
 298                 cth->fd = fd;
 299                 cth->node = node;
 300
 301                 cth->routes = &localneighs;
 302
 303                 cth->state = 1;
 304                 cth->u=0;
 305         }
 306
 307         assert(cth->fd = fd);
 308         assert(cth->node = node);
 309
 310         assert(node->hopcount != 0);
 311
 312         for (;cth->u < node->hopcount;) {
 313                 struct node *n = cth->routes->routes[node->route[cth->u]].dst;
 314                 int rc = send_connect_neigh_nonblock(fd, lnr, n->addrlen,
 315                                 n->addr);
 316                 cth->u++;
 317                 if (rc != RC_OK)
 318                         return rc;
 319                 cth->routes = &(n->routes);
 320         }
 321         bzero(cth, sizeof(struct nonblock_resumeinfo_connect_to_host_send));
 322         return RC_OK;
 323 }
 324
 325 void add_neigh(void *ptr, __u32 addrlen, char *addr)
 326 {
 327         struct route_list *list = (struct route_list *) ptr;
 328         struct node *node;
 329
 330         if (addrlen > MAX_ADDRLEN)
 331                 return;
 332
 333         if (list->numroutes >= list->rows_alloc)
 334                 return;
 335
 336         assert(list->routes[list->numroutes].dst == 0);
 337
 338         node = try_find_neigh_byaddr(addrlen, addr);
 339
 340         if (node == 0) {
 341                 node = calloc(1, sizeof(struct node));
 342
 343                 node->addr = malloc(((int) addrlen));
 344                 node->addrlen = addrlen;
 345                 memcpy(node->addr, addr, addrlen);
 346
 347                 list_add_tail(&(node->node_list), &node_list);
 348         }
 349
 350         list->routes[list->numroutes].dst = node;
 351         list->numroutes++;
 352 }
 353
 354 void init_neighlist(void *ptr, __u32 numneighs)
 355 {
 356         struct route_list *list = (struct route_list *) ptr;
 357         if (numneighs > 16)
 358                 numneighs = 16;
 359         #warning todo limit
 360         list->rows_alloc = (__u16) numneighs;
 361         list->routes = calloc(numneighs, sizeof(struct route));
 362 }
 363
 364 void add_service(void *ptr, __be16 port)
 365 {
 366         struct service_list *list = (struct service_list *) ptr;
 367
 368         printf("add service %u\n", ntohs(port));
 369
 370         if (list->numports >= list->rows_alloc)
 371                 return;
 372
 373         list->ports[list->numports] = port;
 374         list->numports++;
 375 }
 376
 377 void init_servicelist(void *ptr, __u32 numservices)
 378 {
 379         struct service_list *list = (struct service_list *) ptr;
 380
 381         printf("init servicelist %u\n", numservices);
 382
 383         if (numservices > 16)
 384                 numservices = 16;
 385         list->rows_alloc = (__u16) numservices;
 386         list->ports = calloc(numservices, 2);
 387 }
 388
 389 static void print_hex(char *buf, int len)
 390 {
 391         int u;
 392         for(u=0;u<len;u++) {
 393                 printf("%hhx ", buf[u]);
 394         }
 395 }
 396
 397 void neigh_printaddr(__u16 addrlen, char *addr)
 398 {
 399         printf("addrlen = %d addr: ", (int) addrlen);
 400         print_hex(addr, addrlen);
 401         printf("\n");
 402 }
 403
 404
 405 static void __recalc_routes(struct node *node)
 406 {
 407         struct list_head *curr;
 408
 409         curr = node_list.next;
 410         while (curr != &node_list) {
 411                 struct node *currnode = container_of(curr, struct node,
 412                                 node_list);
 413
 414                 if (currnode->hopcount < node->hopcount)
 415                         break;
 416
 417                 curr = curr->next;
 418         }
 419
 420         list_del(&(node->node_list));
 421         list_add_tail(&(node->node_list), curr);
 422 }
 423
 424 static void _recalc_routes(struct node *node)
 425 {
 426         struct route_list *routes = (node == 0 ?
 427                         &localneighs : &(node->routes));
 428
 429         __u32 u;
 430
 431         for (u=0;u<routes->numroutes;u++) {
 432                 struct node *target = routes->routes[u].dst;
 433
 434                 __u32 hopcount = (node == 0 ? 0 : node->hopcount) + 1;
 435
 436                 if (target->hopcount != 0 && target->hopcount <= hopcount)
 437                         continue;
 438
 439                 if (node == 0) {
 440                         bzero(&(target->route[0]), sizeof(target->route));
 441                 } else {
 442                         memcpy(&(target->route[0]), &(node->route[0]),
 443                                         sizeof(target->route));
 444                 }
 445
 446                 target->hopcount = hopcount;
 447                 target->route[hopcount-1] = u;
 448
 449                 __recalc_routes(target);
 450         }
 451 }
 452
 453 static void recalc_routes(void)
 454 {
 455         struct list_head oldnodes;
 456         struct list_head *curr;
 457
 458         list_add(&oldnodes, &node_list);
 459         list_del(&node_list);
 460         init_list_head(&node_list);
 461
 462
 463         curr = oldnodes.next;
 464         while (curr != &oldnodes) {
 465                 struct node *currnode = container_of(curr, struct node,
 466                                 node_list);
 467
 468                 bzero(&(currnode->route[0]), sizeof(currnode->route));
 469                 currnode->hopcount = 0;
 470
 471                 curr = curr->next;
 472         }
 473
 474
 475         _recalc_routes(0);
 476
 477         curr = node_list.next;
 478         while (curr != &node_list) {
 479                 struct node *currnode = container_of(curr, struct node,
 480                                 node_list);
 481
 482                 _recalc_routes(currnode);
 483
 484                 curr = curr->next;
 485         }
 486
 487
 488         curr = oldnodes.next;
 489         while (curr != &oldnodes) {
 490                 struct node *currnode = container_of(curr, struct node,
 491                                 node_list);
 492
 493                 curr = curr->next;
 494
 495                 list_del(&(currnode->node_list));
 496
 497                 if (currnode->addr != 0) {
 498                         free(currnode->addr);
 499                         currnode->addr = 0;
 500                 }
 501
 502                 free(currnode);
 503         }
 504 }
 505
 506 void load_neigh_list(int fd, struct nonblock_resumeinfo *nr, struct node *node)
 507 {
 508         struct libcor_nonblock_resumeinfo *lnr;
 509         int rc;
 510
 511         assert((fd == 0 && nr == 0) || (fd != 0 && nr != 0));
 512
 513         if (fd == 0 && nr == 0) {
 514                 struct epoll_event epe;
 515
 516                 fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
 517                 if(fd < 0) {
 518                         perror("socket");
 519                         goto early_out;
 520                 }
 521
 522                 rc = connect(fd, 0, 0);
 523                 if(rc < 0) {
 524                         perror("connect");
 525                         goto out;
 526                 }
 527
 528                 set_nonblock(fd, 1);
 529
 530                 nr = (struct nonblock_resumeinfo *)
 531                                 malloc(sizeof(struct nonblock_resumeinfo));
 532                 bzero(nr, sizeof(struct nonblock_resumeinfo));
 533                 nr->fd = fd;
 534                 nr->type = EPOLLDATA_DISCOVERNETWORK;
 535                 nr->data.discover_network.node = node;
 536
 537                 epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR |
 538                                 EPOLLET);
 539                 epe.data.ptr = nr;
 540                 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &epe);
 541         }
 542
 543         lnr = &(nr->data.discover_network.lnr);
 544
 545         assert(nr->type == EPOLLDATA_DISCOVERNETWORK);
 546         assert(nr->data.discover_network.node == node);
 547
 548         if (nr->data.discover_network.state == 1) {
 549                 goto state_1;
 550         } else if (nr->data.discover_network.state == 2) {
 551                 goto state_2;
 552         } else if (nr->data.discover_network.state == 3) {
 553                 goto state_3;
 554         } else if (nr->data.discover_network.state == 4) {
 555                 goto state_4;
 556         } else if (nr->data.discover_network.state == 5) {
 557                 goto state_5;
 558         } else if (nr->data.discover_network.state == 6) {
 559                 goto state_6;
 560         } else if (nr->data.discover_network.state == 7) {
 561                 goto state_7;
 562         } else if (nr->data.discover_network.state == 8) {
 563                 goto state_8;
 564         } else if (nr->data.discover_network.state != 0) {
 565                 assert(0);
 566         }
 567
 568         if (node == 0) {
 569                 nr->data.discover_network.list = &localneighs;
 570                 nr->data.discover_network.service = &localservices;
 571         } else {
 572                 neigh_printaddr(node->addrlen,
 573                                 node->addr);
 574                 nr->data.discover_network.list = &(node->routes);
 575                 nr->data.discover_network.service = &(node->services);
 576
 577                 nr->data.discover_network.state = 1;
 578 state_1:
 579                 rc = connect_to_host_send(fd, lnr,
 580                                 &(nr->data.discover_network.connect_send),
 581                                 node);
 582
 583                 if (check_rc(rc, "load_neigh_list: connect_to_host_send error"))
 584                         goto out;
 585         }
 586
 587         rc = send_list_neigh_nonblock(fd, lnr);
 588         nr->data.discover_network.state = 2;
 589         if (check_rc(rc, "load_neigh_list: send_list_neigh error"))
 590                 goto out;
 591
 592 state_2:
 593         rc = send_list_services_nonblock(fd, lnr);
 594         nr->data.discover_network.state = 3;
 595         if (check_rc(rc, "load_neigh_list: send_list_services error"))
 596                 goto out;
 597
 598 state_3:
 599         if (node != 0) {
 600                 nr->data.discover_network.state = 4;
 601 state_4:
 602                 rc = connect_to_host_recv(fd, lnr,
 603                                 &(nr->data.discover_network.connect_recv),
 604                                 node);
 605                 if (check_rc(rc, "load_neigh_list: connect_to_host_recv error"))
 606                         goto out;
 607         }
 608
 609         nr->data.discover_network.state = 5;
 610 state_5:
 611         rc = read_resp_nonblock(fd, lnr);
 612         if (check_rc(rc, "load_neigh_list: read_resp error"))
 613                 goto out;
 614
 615         nr->data.discover_network.state = 6;
 616 state_6:
 617         rc = read_neigh_list_nonblock(fd, lnr, nr->data.discover_network.list,
 618                         init_neighlist, add_neigh);
 619         if (check_rc(rc, "load_neigh_list: read_neigh_list error"))
 620                 goto out;
 621
 622         nr->data.discover_network.state = 7;
 623 state_7:
 624         rc = read_resp_nonblock(fd, lnr);
 625         if (check_rc(rc, "load_neigh_list: read_resp error"))
 626                 goto out;
 627
 628         nr->data.discover_network.state = 8;
 629 state_8:
 630         rc = read_service_list_nonblock(fd, lnr,
 631                         nr->data.discover_network.service,
 632                         init_servicelist, add_service);
 633         if (check_rc(rc, "load_neigh_list: read_service_list error"))
 634                 goto out;
 635
 636         #warning todo rollback node->routes and node->services on error
 637         recalc_routes();
 638 out:
 639         //printf("load_neigh_list state %d\n", nr->data.discover_network.state);
 640         if (rc != RC_WOULDBLOCK) {
 641                 printf("load_neigh_list rc %d\n", rc);
 642                 close(fd);
 643                 if (nr != 0)
 644                         free(nr);
 645         }
 646
 647 early_out:
 648         return;
 649 }
 650
 651 static void discover_network(int fd, struct nonblock_resumeinfo *nr)
 652 {
 653         struct search_query q;
 654         bzero(&q, sizeof(struct search_query));
 655         q.type = SEARCHQUERY_NEIGHSNOTQUERIED;
 656
 657         assert((fd == 0 && nr == 0) || (fd != 0 && nr != 0));
 658
 659         if (fd == 0 && nr == 0)
 660                 load_neigh_list(fd, nr, 0);
 661
 662         while (1) {
 663                 struct node *node;
 664                 if (nr == 0) {
 665                         node = try_find_neigh(&q);
 666                         if (node == 0)
 667                                 break;
 668                 } else {
 669                         assert(nr->type == EPOLLDATA_DISCOVERNETWORK);
 670                         node = nr->data.discover_network.node;
 671                 }
 672
 673                 load_neigh_list(fd, nr, node);
 674                 node->neighs_queried = 1;
 675                 fd = 0;
 676                 nr = 0;
 677         }
 678         discover_finished = 1;
 679 }
 680
 681 struct rdscmd_connect_data{
 682         int fd;
 683         struct nonblock_resumeinfo *nr;
 684 };
 685
 686 //cookie+addr are not passed on nonblocking resume
 687 static int rdscmd_void_connect(void *ptr, __u64 cookie,
 688                 struct cor_sockaddr *addr)
 689 {
 690         int fd, rc;
 691         struct rdscmd_connect_data *rcd = ((struct rdscmd_connect_data *) ptr);
 692
 693         struct libcor_nonblock_resumeinfo *lnr;
 694
 695         if (rcd->nr == 0) {
 696                 struct epoll_event epe;
 697                 struct node *node;
 698
 699                 if (addr->sin_family != AF_COR) {
 700                         printf("rds_connect %llu not af_cor\n", cookie);
 701                         goto out_noclose;
 702                 }
 703
 704                 printf("rds_connect cookie: %llu, addr: ", cookie);
 705                 print_hex(&(addr->addr[0]), sizeof(addr->addr));
 706                 printf(" port: %u\n", ntohs(addr->port));
 707
 708
 709                 if ((node = try_find_neigh_byaddr(sizeof(addr->addr),
 710                                 &(addr->addr[0]))) == 0) {
 711                         printf("connect_to_host host not found\n");
 712                         goto out_noclose;
 713                 }
 714
 715
 716                 fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
 717                 if(fd < 0) {
 718                         perror("socket");
 719                         goto out_noclose;
 720                 }
 721
 722                 rc = connect(fd, 0, 0);
 723                 if(rc < 0) {
 724                         perror("connect");
 725                         goto out;
 726                 }
 727
 728                 set_nonblock(fd, 1);
 729
 730                 rcd->nr = (struct nonblock_resumeinfo *)
 731                                 malloc(sizeof(struct nonblock_resumeinfo));
 732                 bzero(rcd->nr, sizeof(struct nonblock_resumeinfo));
 733                 rcd->nr->fd = fd;
 734                 rcd->nr->type = EPOLLDATA_RDSCONNECT;
 735                 rcd->nr->data.rds_connect.node = node;
 736
 737                 epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR |
 738                                 EPOLLET);
 739                 epe.data.ptr = rcd->nr;
 740                 epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &epe);
 741         }
 742
 743         lnr = &(rcd->nr->data.rds_connect.lnr);
 744
 745         assert(rcd->nr->type == EPOLLDATA_RDSCONNECT);
 746
 747         if (rcd->nr->data.rds_connect.state == 1) {
 748                 goto state_1;
 749         } else if (rcd->nr->data.rds_connect.state == 2) {
 750                 goto state_2;
 751         } else if (rcd->nr->data.rds_connect.state == 3) {
 752                 goto state_3;
 753         } else if (rcd->nr->data.rds_connect.state != 0) {
 754                 assert(0);
 755         }
 756
 757         rc = connect_to_host_send(fd,  lnr,
 758                                 &(rcd->nr->data.rds_connect.connect_send),
 759                                 rcd->nr->data.rds_connect.node);
 760         rcd->nr->data.rds_connect.state = 1;
 761         if (rc != RC_OK) {
 762                 printf("connect_to_host_send error\n");
 763                 goto out;
 764         }
 765
 766 state_1:
 767         rc = send_connect_port_nonblock(fd, lnr, addr->port);
 768         rcd->nr->data.rds_connect.state = 2;
 769         if (rc != RC_OK) {
 770                 printf("send_connect_port error\n");
 771                 goto out;
 772         }
 773
 774
 775 state_2:
 776         rc = connect_to_host_recv(fd, lnr,
 777                                 &(rcd->nr->data.rds_connect.connect_resp),
 778                                 rcd->nr->data.rds_connect.node);
 779         if (rc != RC_OK) {
 780                 printf("connect_to_host_recv error\n");
 781                 goto out;
 782         }
 783
 784         rcd->nr->data.rds_connect.state = 3;
 785 state_3:
 786         rc = read_resp_nonblock(fd, lnr);
 787         if (rc != RC_OK) {
 788                 printf("read_resp error\n");
 789                 goto out;
 790         }
 791
 792         rc = pass_socket(fd, cookie);
 793         if (rc != RC_OK) {
 794                 printf("pass_socket error\n");
 795                 goto out;
 796         }
 797
 798         if (0) {
 799 out:
 800                 close(fd);
 801                 #warning todo rdsock resumeinfo
 802 out_noclose:
 803                 printf("send_rdsock_connecterror\n");
 804                 rc = send_rdsock_connecterror(rdsock_fd, cookie,
 805                                 CONNECTERROR_NETUNREACH);
 806                 if (rc != RC_OK) {
 807                         printf("send_rdsock_connecterror returned %d\n", rc);
 808                         return 1;
 809                 }
 810         }
 811
 812         return 0;
 813 }
 814
 815 static int proc_rdsock_cmd(int fd, struct rdsock_cmd *cmd)
 816 {
 817         if (cmd->cmd == CRD_KTU_CONNECT) {
 818                 struct rdscmd_connect_data data;
 819                 return parse_rdsock_connect(&data, cmd, rdscmd_void_connect);
 820         } else {
 821                 printf("error in proc_rdsock_cmd: unknown cmd: %u\n", cmd->cmd);
 822                 assert(0);
 823                 exit(1);
 824                 return RC_CONNBROKEN;
 825         }
 826 }
 827
 828 static void nonblock_resume(struct nonblock_resumeinfo *nr)
 829 {
 830         #warning todo call rdscmd_void_connect
 831         if (nr->type == EPOLLDATA_DISCOVERNETWORK) {
 832                 int rc = resume_send_ifneeded(nr->fd,
 833                                 &(nr->data.discover_network.lnr));
 834
 835                 //printf("load_neigh_list state = %d rc = %d\n", nr->data.discover_network.state, rc);
 836
 837                 if (rc != RC_OK)
 838                         return;
 839
 840                 discover_network(nr->fd, nr);
 841         } else {
 842                 assert(0);
 843                 exit(1);
 844         }
 845 }
 846
 847 static int parse_hexchar(char c)
 848 {
 849         if (c == '0')
 850                 return 0;
 851         else if (c == '1')
 852                 return 1;
 853         else if (c == '2')
 854                 return 2;
 855         else if (c == '3')
 856                 return 3;
 857         else if (c == '4')
 858                 return 4;
 859         else if (c == '5')
 860                 return 5;
 861         else if (c == '6')
 862                 return 6;
 863         else if (c == '7')
 864                 return 7;
 865         else if (c == '8')
 866                 return 8;
 867         else if (c == '9')
 868                 return 9;
 869         else if (c == 'A' || c == 'a')
 870                 return 10;
 871         else if (c == 'B' || c == 'b')
 872                 return 11;
 873         else if (c == 'C' || c == 'c')
 874                 return 12;
 875         else if (c == 'D' || c == 'd')
 876                 return 13;
 877         else if (c == 'E' || c == 'e')
 878                 return 14;
 879         else if (c == 'F' || c == 'f')
 880                 return 15;
 881         else
 882                 return -1;
 883 }
 884
 885 static int parse_hex(char *target, char *src, int srclen)
 886 {
 887         int u;
 888
 889         assert(src != 0);
 890         assert(target != 0);
 891         assert(srclen > 0);
 892         assert((srclen%2) == 0);
 893
 894         for(u=0;u<(srclen/2);u++) {
 895                 int high = parse_hexchar(src[u*2]);
 896                 int low = parse_hexchar(src[u*2+1]);
 897
 898                 if (high == -1 || low == -1)
 899                         return -1;
 900
 901                  target[u] = (char) ((high << 4) + low);
 902         }
 903
 904         return 0;
 905 }
 906
 907 int parse_addr(char *argv_addr)
 908 {
 909         int rc;
 910         int len = strlen(argv_addr);
 911         char *noaddr = "noaddr";
 912
 913         if (strncmp(argv_addr, noaddr, len) == 0) {
 914                 localaddr = 0;
 915                 localaddrlen = 0;
 916                 return;
 917         }
 918
 919         if ((len < 2) || (len%2) != 0)
 920                 goto parseerror;
 921
 922         localaddr = malloc(len/2);
 923         localaddrlen = len/2;
 924
 925         rc = parse_hex(localaddr, argv_addr, len);
 926         if (rc != 0)
 927                 goto parseerror;
 928
 929         return 0;
 930
 931 parseerror:
 932         fprintf(stderr, "error: argv_addr is not a valid address (contains "
 933                         "non-hex characters or number of hex characters is not "
 934                         "a multiple of 2)\n");
 935         return -1;
 936 }
 937
 938 int main(int argc, char *argv[])
 939 {
 940         int rc;
 941         struct epoll_event rds_epe;
 942         struct nonblock_resumeinfo rds_nr;
 943
 944         discover_finished = 0;
 945         epoll_fd = 0;
 946
 947         if (argc <= 1)
 948                 goto usage;
 949
 950         rc = parse_addr(argv[1]);
 951         if (rc != 0)
 952                 goto usage;
 953
 954         init_list_head(&node_list);
 955
 956
 957         epoll_fd = epoll_create(1);
 958         if (epoll_fd <= 0) {
 959                 perror("epoll_create");
 960                 goto out;
 961         }
 962
 963
 964         rdsock_fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RDEAMON);
 965         if (rdsock_fd < 0) {
 966                 perror("socket");
 967                 goto out;
 968         }
 969
 970         rc = connect(rdsock_fd, 0, 0);
 971         if (rc < 0) {
 972                 perror("connect");
 973                 goto out;
 974         }
 975
 976
 977         bzero(&rds_nr, sizeof(struct nonblock_resumeinfo));
 978         rds_nr.fd = rdsock_fd;
 979         rds_nr.type = EPOLLDATA_RDSOCKCMD;
 980         rds_epe.events = (EPOLLIN | EPOLLRDHUP | EPOLLERR);
 981         rds_epe.data.ptr = &rds_nr;
 982
 983
 984         send_rdsock_up(rdsock_fd, localaddr, localaddrlen);
 985
 986         sleep(30);
 987
 988         discover_network(0, 0);
 989
 990         while (1) {
 991                 int discover_finished_old = discover_finished;
 992                 int u;
 993                 struct epoll_event events[EPOLL_EVENTS];
 994
 995                 int rdycnt = epoll_wait(epoll_fd, events, EPOLL_EVENTS, -1);
 996
 997                 for (u=0;u<rdycnt;u++) {
 998                         struct nonblock_resumeinfo *nr = events[u].data.ptr;
 999                         nonblock_resume(nr);
1000                 }
1001
1002                 if (discover_finished_old == 0 && discover_finished != 0) {
1003                         epoll_ctl(epoll_fd, EPOLL_CTL_ADD, rdsock_fd, &rds_epe);
1004                 }
1005         }
1006
1007
1008         while (1) {
1009                 struct rdsock_cmd cmd;
1010
1011                 rc = read_rdsock_cmd(rdsock_fd, &cmd);
1012                 if (rc != RC_OK) {
1013                         printf("read_rdsock_cmd rc = %d\n", rc);
1014                         break;
1015                 }
1016
1017                 rc = proc_rdsock_cmd(rdsock_fd, &cmd);
1018                 free_rdsockcmd_data(&cmd);
1019                 if (rc != RC_OK) {
1020                         printf("proc_rdsock_cmd rc = %d\n", rc);
1021                         break;
1022                 }
1023         }
1024
1025
1026 usage:
1027         fprintf(stderr, "usage test_routed addr\n");
1028 out:
1029         return 1;
1030 }