test_routed.c

   1 /**
   2  *      Connection oriented routing user space utils
   3  *      Copyright (C) 2009-2011
   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
  29 #include "libcor.h"
  30
  31
  32 #define MAX_ADDRLEN 128
  33
  34
  35 struct route{
  36         struct node *dst;
  37 };
  38
  39 struct route_list{
  40         struct route *routes;
  41         __u16 rows_alloc;
  42         __u16 numroutes;
  43 };
  44
  45 struct service_list{
  46         __be16 *ports;
  47         __u16 rows_alloc;
  48         __u16 numports;
  49 };
  50
  51 struct node{
  52         char *addr;
  53
  54         struct route_list routes;
  55
  56         struct service_list services;
  57
  58         __u16 addrlen;
  59         __u16 minhops;
  60         __u8 neighs_queried;
  61 };
  62
  63 #define ROUTE_MAXHOPS 7
  64
  65 struct e2e_route{
  66         struct node *result;
  67         int routes[ROUTE_MAXHOPS + 1];
  68         int pos;
  69 };
  70
  71 #define SEARCHQUERY_NEIGHSNOTQUERIED 1
  72 #define SEARCHQUERY_NODEBYADDRESS 2
  73 struct search_query{
  74         int type;
  75         union {
  76                 struct {
  77                         __u16 addrlen;
  78                         char *addr;
  79                 }nodebyaddress;
  80         }query;
  81 };
  82
  83
  84 char *localaddr;
  85 __u32 localaddrlen;
  86
  87 struct route_list localneighs;
  88 struct service_list localservices;
  89
  90
  91 static int node_matches_searchquery(struct node *n, struct search_query *q)
  92 {
  93         if (q->type == SEARCHQUERY_NEIGHSNOTQUERIED) {
  94                 return n->neighs_queried == 0;
  95         } else if (q->type == SEARCHQUERY_NODEBYADDRESS) {
  96                 /* if (n->addrlen != q->query.nodebyaddress.addrlen)
  97                         return 0;
  98                 if (memcmp(n->addr, q->query.nodebyaddress.addr,
  99                                 n->addrlen) != 0)
 100                         return 0;
 101                 return 1; */
 102
 103                 __u16 cmplen = n->addrlen;
 104                 __u32 u;
 105
 106                 if (q->query.nodebyaddress.addrlen < cmplen)
 107                         cmplen = q->query.nodebyaddress.addrlen;
 108
 109                 if (memcmp(n->addr, q->query.nodebyaddress.addr, cmplen) != 0)
 110                         return 0;
 111
 112                 for (u=cmplen;u<n->addrlen;u++) {
 113                         if (n->addr[u] != 0) {
 114                                 return 0;
 115                         }
 116                 }
 117                 for (u=cmplen;u<q->query.nodebyaddress.addrlen;u++) {
 118                         if (q->query.nodebyaddress.addr[u] != 0) {
 119                                 return 0;
 120                         }
 121                 }
 122                 return 1;
 123         } else {
 124                 assert(0);
 125                 return 0;
 126         }
 127 }
 128
 129 static int try_find_neigh(struct e2e_route *route, struct search_query *q,
 130                 struct route_list *curr)
 131 {
 132         int *pos = &(route->routes[route->pos]);
 133
 134         for (*pos=0;*pos<curr->numroutes;(*pos)++) {
 135                 struct node *n = curr->routes[*pos].dst;
 136                 if (node_matches_searchquery(n, q)) {
 137                         route->result = n;
 138                         return 1;
 139                 }
 140         }
 141
 142         if (route->pos >= ROUTE_MAXHOPS)
 143                 return 0;
 144
 145         route->pos++;
 146
 147         for (*pos=0;*pos<curr->numroutes;(*pos)++) {
 148                 int rc = try_find_neigh(route, q,
 149                                 &(curr->routes[*pos].dst->routes));
 150                 if (rc)
 151                         return rc;
 152         }
 153
 154         route->pos--;
 155
 156         return 0;
 157 }
 158
 159 static int try_find_neigh_byaddr(struct e2e_route *route, __u16 addrlen,
 160                 char *addr)
 161 {
 162         struct search_query q;
 163         bzero(&q, sizeof(struct search_query));
 164         q.type = SEARCHQUERY_NODEBYADDRESS;
 165         q.query.nodebyaddress.addrlen = addrlen;
 166         q.query.nodebyaddress.addr = addr;
 167
 168         return try_find_neigh(route, &q, &localneighs);
 169 }
 170
 171 static int connect_to_host_recv(int fd, struct e2e_route *route)
 172 {
 173         int u;
 174         printf("1\n");
 175         for (u=0;route != 0 && u<=route->pos;u++) {
 176                 int rc = read_resp(fd);
 177                 if (rc != RC_OK)
 178                         return RC_CONNBROKEN;
 179         }
 180         return RC_OK;
 181 }
 182
 183 static int connect_to_host_send(int fd, struct e2e_route *route)
 184 {
 185         struct route_list *routes = &localneighs;
 186         int u;
 187         for (u=0;u<=route->pos;u++) {
 188                 struct node *n = routes->routes[route->routes[u]].dst;
 189                 int rc = send_connect_neigh(fd,  n->addrlen, n->addr);
 190                 if (rc != RC_OK)
 191                         return RC_CONNBROKEN;
 192                 routes = &(n->routes);
 193         }
 194
 195         return RC_OK;
 196 }
 197
 198 static int connect_to_host(int fd, __u16 addrlen, char *addr)
 199 {
 200         int rc;
 201         struct e2e_route route;
 202         bzero(&route, sizeof(struct e2e_route));
 203
 204         if (try_find_neigh_byaddr(&route, addrlen, addr) == 0)
 205                 return RC_CONNBROKEN;
 206
 207         rc = connect_to_host_send(fd, &route);
 208         if (rc != RC_OK)
 209                 goto out;
 210
 211         rc = connect_to_host_recv(fd, &route);
 212
 213 out:
 214         return rc;
 215 }
 216
 217 void add_neigh(void *ptr, __u32 addrlen, char *addr)
 218 {
 219         struct route_list *list = (struct route_list *) ptr;
 220         struct node *node;
 221         struct e2e_route route;
 222
 223         if (addrlen > MAX_ADDRLEN)
 224                 return;
 225
 226         if (list->numroutes >= list->rows_alloc)
 227                 return;
 228
 229         assert(list->routes[list->numroutes].dst == 0);
 230
 231         bzero(&route, sizeof(struct e2e_route));
 232         if (try_find_neigh_byaddr(&route, addrlen, addr) == 0) {
 233                 node = calloc(1, sizeof(struct node));
 234
 235                 node->addr = malloc(((int) addrlen));
 236                 node->addrlen = addrlen;
 237                 memcpy(node->addr, addr, addrlen);
 238         } else {
 239                 node = route.result;
 240         }
 241
 242         list->routes[list->numroutes].dst = node;
 243         list->numroutes++;
 244 }
 245
 246 void init_neighlist(void *ptr, __u32 numneighs)
 247 {
 248         struct route_list *list = (struct route_list *) ptr;
 249         if (numneighs > 16)
 250                 numneighs = 16;
 251         #warning todo limit
 252         list->rows_alloc = (__u16) numneighs;
 253         list->routes = calloc(numneighs, sizeof(struct route));
 254 }
 255
 256 void add_service(void *ptr, __be16 port)
 257 {
 258         struct service_list *list = (struct service_list *) ptr;
 259
 260         printf("add service %u\n", ntohs(port));
 261
 262         if (list->numports >= list->rows_alloc)
 263                 return;
 264
 265         list->ports[list->numports] = port;
 266         list->numports++;
 267 }
 268
 269 void init_servicelist(void *ptr, __u32 numservices)
 270 {
 271         struct service_list *list = (struct service_list *) ptr;
 272
 273         printf("init servicelist %u\n", numservices);
 274
 275         if (numservices > 16)
 276                 numservices = 16;
 277         list->rows_alloc = (__u16) numservices;
 278         list->ports = calloc(numservices, 2);
 279 }
 280
 281 static void print_hex(char *buf, int len)
 282 {
 283         int u;
 284         for(u=0;u<len;u++) {
 285                 printf("%hhx ", buf[u]);
 286         }
 287 }
 288
 289 void neigh_printaddr(__u16 addrlen, char *addr)
 290 {
 291         printf("addrlen = %d addr: ", (int) addrlen);
 292         print_hex(addr, addrlen);
 293         printf("\n");
 294 }
 295
 296 void load_neigh_list(struct e2e_route *route)
 297 {
 298         struct route_list *list;
 299         struct service_list *service;
 300         int fd, rc;
 301         int u;
 302
 303         fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
 304         if(fd < 0) {
 305                 perror("socket");
 306                 goto early_out;
 307         }
 308
 309         rc = connect(fd, 0, 0);
 310         if(rc < 0) {
 311                 perror("connect");
 312                 goto out;
 313         }
 314
 315         if (route == 0) {
 316                 list = &localneighs;
 317                 service = &localservices;
 318         } else {
 319                 neigh_printaddr(route->result->addrlen,
 320                                 route->result->addr);
 321                 list = &(route->result->routes);
 322                 service = &(route->result->services);
 323                 rc = connect_to_host_send(fd, route);
 324
 325                 if (rc != RC_OK) {
 326                         printf("load_neigh_list: connect_to_host_send error\n");
 327                         goto out;
 328                 }
 329         }
 330
 331         rc = send_list_neigh(fd);
 332         if (rc != RC_OK) {
 333                 printf("load_neigh_list: send_list_neigh error\n");
 334                 goto out;
 335         }
 336
 337         rc = send_list_services(fd);
 338         if (rc != RC_OK) {
 339                 printf("load_neigh_list: send_list_services error\n");
 340                 goto out;
 341         }
 342
 343         if (route != 0) {
 344                 rc = connect_to_host_recv(fd, route);
 345                 if (rc != RC_OK) {
 346                         printf("load_neigh_list: connect_to_host_recv error\n");
 347                         goto out;
 348                 }
 349         }
 350
 351         printf("2\n");
 352         rc = read_resp(fd);
 353         if (rc != RC_OK)
 354                 printf("load_neigh_list: read_resp error\n");
 355
 356         rc = read_neigh_list(fd, list, init_neighlist, add_neigh);
 357         if (rc != RC_OK)
 358                 printf("load_neigh_list: read_neigh_list error\n");
 359
 360         printf("3\n");
 361         rc = read_resp(fd);
 362         if (rc != RC_OK)
 363                 printf("load_neigh_list: read_resp error\n");
 364
 365         rc = read_service_list(fd, service, init_servicelist, add_service);
 366         if (rc != RC_OK)
 367                 printf("load_neigh_list: read_service_list error\n");
 368 out:
 369         printf("load_neigh_list rc %d\n", rc);
 370         close(fd);
 371
 372 early_out:
 373         return;
 374 }
 375
 376 static void discover_network(void)
 377 {
 378         struct search_query q;
 379         bzero(&q, sizeof(struct search_query));
 380         q.type = SEARCHQUERY_NEIGHSNOTQUERIED;
 381         load_neigh_list(0);
 382         while (1) {
 383                 struct e2e_route nextroute;
 384                 bzero(&nextroute, sizeof(struct e2e_route));
 385                 int rc = try_find_neigh(&nextroute, &q, &localneighs);
 386                 if (rc == 0)
 387                         break;
 388                 load_neigh_list(&nextroute);
 389                 nextroute.result->neighs_queried = 1;
 390         }
 391 }
 392
 393 struct rdscmd_connect_data{
 394         int rds_fd;
 395 };
 396
 397 static int rdscmd_void_connect(void *ptr, __u64 cookie,
 398                 struct cor_sockaddr *addr)
 399 {
 400         int fd, rc;
 401
 402         int rds_fd = ((struct rdscmd_connect_data *) ptr)->rds_fd;
 403
 404         printf("rds_connect %llu\n", cookie);
 405
 406         if (addr->sin_family != AF_COR)
 407                 goto out_noclose;
 408
 409         printf("rds_connect %llu, %u\n", cookie, ntohs(addr->port));
 410
 411         fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
 412         if(fd < 0) {
 413                 perror("socket");
 414                 goto out_noclose;
 415         }
 416
 417         rc = connect(fd, 0, 0);
 418         if(rc < 0) {
 419                 perror("connect");
 420                 goto out;
 421         }
 422
 423
 424         printf("rds_connect addr: ");
 425         print_hex(&(addr->addr[0]), sizeof(addr->addr));
 426         printf(" port: %u\n", ntohs(addr->port));
 427
 428         rc = connect_to_host(fd, sizeof(addr->addr), &(addr->addr[0]));
 429
 430         if (rc != RC_OK) {
 431                 printf("connect_to_host error\n");
 432                 goto out;
 433         }
 434
 435
 436         rc = send_connect_port(fd, addr->port);
 437         if (rc != RC_OK) {
 438                 printf("send_connect_port error\n");
 439                 goto out;
 440         }
 441
 442         printf("4\n");
 443         rc = read_resp(fd);
 444         if (rc != RC_OK) {
 445                 printf("read_resp error\n");
 446                 goto out;
 447         }
 448
 449         rc = pass_socket(fd, cookie);
 450         if (rc != RC_OK) {
 451                 printf("pass_socket error\n");
 452                 goto out;
 453         }
 454
 455         if (0) {
 456 out:
 457                 close(fd);
 458 out_noclose:
 459                 printf("send_rdsock_connecterror\n");
 460                 rc = send_rdsock_connecterror(rds_fd, cookie,
 461                                 CONNECTERROR_NETUNREACH);
 462                 if (rc != RC_OK) {
 463                         printf("send_rdsock_connecterror returned %d\n", rc);
 464                         return 1;
 465                 }
 466         }
 467
 468         return 0;
 469 }
 470
 471 static int proc_rdsock_cmd(int fd, struct rdsock_cmd *cmd)
 472 {
 473         if (cmd->cmd == CRD_KTU_CONNECT) {
 474                 struct rdscmd_connect_data data;
 475                 data.rds_fd = fd;
 476                 return parse_rdsock_connect(&data, cmd, rdscmd_void_connect);
 477         } else {
 478                 printf("error in proc_rdsock_cmd: unknown cmd: %u\n", cmd->cmd);
 479                 assert(0);
 480                 exit(1);
 481                 return RC_CONNBROKEN;
 482         }
 483 }
 484
 485 static int parse_hexchar(char c)
 486 {
 487         if (c == '0')
 488                 return 0;
 489         else if (c == '1')
 490                 return 1;
 491         else if (c == '2')
 492                 return 2;
 493         else if (c == '3')
 494                 return 3;
 495         else if (c == '4')
 496                 return 4;
 497         else if (c == '5')
 498                 return 5;
 499         else if (c == '6')
 500                 return 6;
 501         else if (c == '7')
 502                 return 7;
 503         else if (c == '8')
 504                 return 8;
 505         else if (c == '9')
 506                 return 9;
 507         else if (c == 'A' || c == 'a')
 508                 return 10;
 509         else if (c == 'B' || c == 'b')
 510                 return 11;
 511         else if (c == 'C' || c == 'c')
 512                 return 12;
 513         else if (c == 'D' || c == 'd')
 514                 return 13;
 515         else if (c == 'E' || c == 'e')
 516                 return 14;
 517         else if (c == 'F' || c == 'f')
 518                 return 15;
 519         else
 520                 return -1;
 521 }
 522
 523 static int parse_hex(char *target, char *src, int srclen)
 524 {
 525         int u;
 526
 527         assert(src != 0);
 528         assert(target != 0);
 529         assert(srclen > 0);
 530         assert((srclen%2) == 0);
 531
 532         for(u=0;u<(srclen/2);u++) {
 533                 int high = parse_hexchar(src[u*2]);
 534                 int low = parse_hexchar(src[u*2+1]);
 535
 536                 if (high == -1 || low == -1)
 537                         return -1;
 538
 539                  target[u] = (char) ((high << 4) + low);
 540         }
 541
 542         return 0;
 543 }
 544
 545 int parse_addr(char *argv_addr)
 546 {
 547         int rc;
 548         int len = strlen(argv_addr);
 549         char *noaddr = "noaddr";
 550
 551         if (strncmp(argv_addr, noaddr, len) == 0) {
 552                 localaddr = 0;
 553                 localaddrlen = 0;
 554                 return;
 555         }
 556
 557         if ((len < 2) || (len%2) != 0)
 558                 goto parseerror;
 559
 560         localaddr = malloc(len/2);
 561         localaddrlen = len/2;
 562
 563         rc = parse_hex(localaddr, argv_addr, len);
 564         if (rc != 0)
 565                 goto parseerror;
 566
 567         return 0;
 568
 569 parseerror:
 570         fprintf(stderr, "error: argv_addr is not a valid address (contains "
 571                         "non-hex characters or number of hex characters is not "
 572                         "a multiple of 2)\n");
 573         return -1;
 574 }
 575
 576 int main(int argc, char *argv[])
 577 {
 578         int rc;
 579         int rdsock;
 580
 581         if (argc <= 1)
 582                 goto usage;
 583
 584         rc = parse_addr(argv[1]);
 585         if (rc != 0)
 586                 goto usage;
 587
 588         rdsock = socket(PF_COR, SOCK_RAW, PROTO_COR_RDEAMON);
 589         if(rdsock < 0) {
 590                 perror("socket");
 591                 goto out;
 592         }
 593
 594         rc = connect(rdsock, 0, 0);
 595         if(rc < 0) {
 596                 perror("connect");
 597                 goto out;
 598         }
 599
 600         send_rdsock_version(rdsock, 0);
 601         send_rdsock_up(rdsock, localaddr, localaddrlen);
 602
 603         sleep(30);
 604
 605         discover_network();
 606
 607         while(1) {
 608                 struct rdsock_cmd cmd;
 609
 610                 rc = read_rdsock_cmd(rdsock, &cmd);
 611                 if (rc != RC_OK) {
 612                         printf("read_rdsock_cmd rc = %d\n", rc);
 613                         break;
 614                 }
 615
 616                 rc = proc_rdsock_cmd(rdsock, &cmd);
 617                 free_rdsockcmd_data(&cmd);
 618                 if (rc != RC_OK) {
 619                         printf("proc_rdsock_cmd rc = %d\n", rc);
 620                         break;
 621                 }
 622         }
 623
 624 out:
 625         return 0;
 626
 627 usage:
 628         fprintf(stderr, "usage test_routed addr\n");
 629         return 1;
 630 }