capabilities cmd

[corutils.git] / test_routed2.c

/**
 *      Connection oriented routing user space utils
 *      Copyright (C) 2009-2014
 *      Authors:
 *              Michael Blizek
 *
 *      This program is free software; you can redistribute it and/or
 *      modify it under the terms of the GNU General Public License
 *      as published by the Free Software Foundation; either version 2
 *      of the License, or (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 *      02110-1301, USA.
 */

#include <stdio.h>
#include <string.h>
#include <sys/socket.h>
#include <assert.h>
#include <unistd.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include <errno.h>
#include <sys/epoll.h>

#include "list.h"
#include "libcor.h"


#define U32_MAX ((__u32) ((1LL << 32) - 1))


#define MAX_ADDRLEN 128

#define ASSERT_ERR()    \
        do {                                    \
                assert(0);                      \
                exit(1);                        \
                while (1) {                     \
                }                               \
        } while(0)                              \


#define ASSERT(a)       \
        do {                                    \
                if (!(a)) { ASSERT_ERR(); }     \
        } while(0)                              \


struct route{
        struct node *dst;
};

struct route_list{
        struct route *routes;
        __u16 rows_alloc;
        __u16 numroutes;
};

struct service_list{
        __be16 *ports;
        __u16 rows_alloc;
        __u16 numports;
};

#define ROUTE_MAXHOPS 7

struct node{
        char *addr;

        struct route_list routes;

        struct service_list services;

        __u32 addrlen;
        __u16 minhops;
        __u8 neighs_queried;

        struct list_head node_list;

        __u32 route[ROUTE_MAXHOPS + 1];
        __u32 hopcount;
};

#define SEARCHQUERY_NEIGHSNOTQUERIED 1
#define SEARCHQUERY_NODEBYADDRESS 2
#define SEARCHQUERY_NODEBYSERVICEPORT 3
struct search_query{
        int type;
        union {
                struct {
                        __u32 addrlen;
                        char *addr;
                }nodebyaddress;

                __be16 serviceport;
        }query;
};


char *localaddr;
__u32 localaddrlen;

__u8 export_servicelist_enabled = 0;

struct route_list localneighs;
struct service_list localservices;

struct list_head node_list;

int discover_finished = 0;
int epoll_fd = 0;

int rdsock_fd = 0;

#define EPOLL_EVENTS 16

#define EPOLLDATA_DISCOVERNETWORK 1
#define EPOLLDATA_RDSCONNECT 2
#define EPOLLDATA_RDSOCKCMD 3
#define EPOLLDATA_FWD_TO_RSERVICE 4
#define EPOLLDATA_FORWARD 5

struct nonblock_resumeinfo_connect_to_host_send{
        int fd;
        struct node *node;

        __u8 state;

        struct route_list *routes;
        int u;
};

struct nonblock_resumeinfo_connect_to_host_recv{
        int fd;
        struct node *node;

        __u8 state;

        int u;
};

#define FORWARD_BUF_SIZE 4096

struct nonblock_resumeinfo{
        int fd;

        __u8 type;

        union{
                struct{
                        struct node *node;

                        __u8 state;

                        struct route_list *list;
                        struct service_list *service;

                        struct nonblock_resumeinfo_connect_to_host_send connect_send;
                        struct nonblock_resumeinfo_connect_to_host_recv connect_recv;

                        struct libcor_nonblock_resumeinfo lnr;
                }discover_network;

                struct{
                        __u8 state;

                        __u64 cookie;
                        struct node *node;
                        __be16 port;

                        struct nonblock_resumeinfo_connect_to_host_send connect_send;
                        struct nonblock_resumeinfo_connect_to_host_recv connect_resp;

                        struct libcor_nonblock_resumeinfo lnr;
                }rds_connect;

                struct{
                        struct libcor_nonblock_resumeinfo lnr;
                }rdsock_cmd;

                struct{
                        __be16 targetport;
                }fwd_to_rservice;

                struct{
                        /**
                         * buf gets filled by reversedir and written into
                         * this fd
                         */
                        char *buf;
                        __u32 buffill;
                        __u32 bufwritten;

                        __u8 is_connected;

                        struct nonblock_resumeinfo *reversedir;
                }forward;
        }data;
};

struct fwd_to_rservice_item{
        struct list_head allfwds;

        int bindfd;

        __be16 targetport;
};



#define offsetof(type, member)  __builtin_offsetof (type, member)

#define container_of(ptr, type, member) ({                              \
                const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
                (type *)( (char *)__mptr - offsetof(type,member) );})


static void set_nonblock(int fd, int value)
{
        int flags;

        flags = fcntl(fd, F_GETFL, 0);
        if (flags == -1) {
                perror("set_nonblock F_GETFL");
                exit(1);
        }

        flags = (flags & (~(O_NONBLOCK)));
        if (value)
                flags = flags | O_NONBLOCK;

        if (fcntl(fd, F_SETFL, flags) == -1) {
                perror("set_nonblock F_SETFL");
                exit(1);
        }
}

static int addr_eq(char *addr1, __u32 addrlen1,
                char *addr2, __u32 addrlen2)
{
        while (addrlen1>0) {
                if (addr1[addrlen1-1] == 0) {
                        addrlen1--;
                } else {
                        break;
                }
        }

        while (addrlen2>0) {
                if (addr2[addrlen2-1] == 0) {
                        addrlen2--;
                } else {
                        break;
                }
        }

        if (addrlen1 != addrlen2)
                return 0;

        if (addrlen1 > MAX_ADDRLEN)
                return 0;

        if (addrlen1 != 0 && memcmp(addr1, addr2, addrlen1) != 0)
                return 0;

        return 1;
}

static int service_list_contains(struct service_list *services, __be16 port)
{
        __u32 u;

        for (u=0;u<services->numports;u++) {
                if (services->ports[u] == port) {
                        return 1;
                }
        }

        return 0;
}

static int node_matches_searchquery(struct node *n, struct search_query *q)
{
        if (q->type == SEARCHQUERY_NEIGHSNOTQUERIED) {
                return n->neighs_queried == 0 &&
                                addr_eq(n->addr, n->addrlen, localaddr,
                                localaddrlen) == 0;
        } else if (q->type == SEARCHQUERY_NODEBYADDRESS) {
                return addr_eq(n->addr, n->addrlen,
                                q->query.nodebyaddress.addr,
                                q->query.nodebyaddress.addrlen);
        } else if (q->type == SEARCHQUERY_NODEBYSERVICEPORT) {
                return service_list_contains(&(n->services),
                                q->query.serviceport);
        } else {
                ASSERT(0);
                return 0;
        }
}

static struct node * try_find_neigh(struct search_query *q)
{
        struct list_head *curr = node_list.next;

        while (curr != &node_list) {
                struct node *currnode = container_of(curr, struct node,
                                node_list);

                if (node_matches_searchquery(currnode, q)) {
                        return currnode;
                }

                curr = curr->next;
        }

        return 0;
}

static struct node * try_find_neigh_byaddr(__u16 addrlen, char *addr)
{
        struct search_query q;
        bzero(&q, sizeof(struct search_query));
        q.type = SEARCHQUERY_NODEBYADDRESS;
        q.query.nodebyaddress.addrlen = addrlen;
        q.query.nodebyaddress.addr = addr;
        return try_find_neigh(&q);
}

static struct node * try_find_neigh_byservice(__be16 serviceport)
{
        struct search_query q;
        bzero(&q, sizeof(struct search_query));
        q.type = SEARCHQUERY_NODEBYSERVICEPORT;
        q.query.serviceport = serviceport;
        return try_find_neigh(&q);
}


static int check_rc(int rc, char *errormsg)
{
        if (rc != RC_OK && rc != RC_WOULDBLOCK)
                printf("%s\n", errormsg);
        return (rc != RC_OK);
}

static int connect_to_host_recv(int fd, struct libcor_nonblock_resumeinfo *lnr,
                struct nonblock_resumeinfo_connect_to_host_recv *rcr,
                struct node *node)
{
        if (rcr->state == 0) {
                rcr->fd = fd;
                rcr->node = node;

                rcr->state = 1;
                rcr->u=0;
        }

        ASSERT(rcr->fd = fd);
        ASSERT(rcr->node = node);

        ASSERT(node->hopcount != 0);

        for (;rcr->u < node->hopcount;rcr->u++) {
                int rc = read_resp_nonblock(fd, lnr);
                if (rc != RC_OK)
                        return rc;
        }
        bzero(rcr, sizeof(struct nonblock_resumeinfo_connect_to_host_recv));
        return RC_OK;
}

static int connect_to_host_send(int fd,
                struct libcor_nonblock_resumeinfo *lnr,
                struct nonblock_resumeinfo_connect_to_host_send *cth,
                struct node *node)
{
        if (cth->state == 0) {
                cth->fd = fd;
                cth->node = node;

                cth->routes = &localneighs;

                cth->state = 1;
                cth->u=0;
        }

        ASSERT(cth->fd = fd);
        ASSERT(cth->node = node);

        ASSERT(node->hopcount != 0);

        for (;cth->u < node->hopcount;) {
                struct node *n = cth->routes->routes[node->route[cth->u]].dst;
                int rc = send_connect_neigh_nonblock(fd, lnr, n->addrlen,
                                n->addr);
                cth->u++;
                if (rc != RC_OK)
                        return rc;
                cth->routes = &(n->routes);
        }
        bzero(cth, sizeof(struct nonblock_resumeinfo_connect_to_host_send));
        return RC_OK;
}

static void add_neigh(void *ptr, __u32 addrlen, char *addr)
{
        struct route_list *list = (struct route_list *) ptr;
        struct node *node;

        if (addrlen > MAX_ADDRLEN)
                return;

        if (addr_eq(addr, addrlen, 0, 0))
                return;

        if (list->numroutes >= list->rows_alloc)
                return;

        ASSERT(list->routes[list->numroutes].dst == 0);

        node = try_find_neigh_byaddr(addrlen, addr);

        if (node == 0) {
                node = calloc(1, sizeof(struct node));

                node->addr = malloc(((int) addrlen));
                node->addrlen = addrlen;
                memcpy(node->addr, addr, addrlen);

                list_add_tail(&(node->node_list), &node_list);
        }

        list->routes[list->numroutes].dst = node;
        list->numroutes++;
}

static void init_neighlist(void *ptr, __u32 numneighs)
{
        struct route_list *list = (struct route_list *) ptr;
        if (numneighs > 16)
                numneighs = 16;
        #warning todo limit
        list->rows_alloc = (__u16) numneighs;
        list->routes = calloc(numneighs, sizeof(struct route));
}

static void add_service(void *ptr, __be16 port)
{
        struct service_list *list = (struct service_list *) ptr;

        printf("add service %u\n", ntohs(port));

        if (list->numports >= list->rows_alloc)
                return;

        list->ports[list->numports] = port;
        list->numports++;
}

static void init_servicelist(void *ptr, __u32 numservices)
{
        struct service_list *list = (struct service_list *) ptr;

        printf("init servicelist %u\n", numservices);

        if (numservices > 16)
                numservices = 16;
        list->rows_alloc = (__u16) numservices;
        list->numports = 0;
        list->ports = calloc(numservices, 2);
}

static void print_hex(char *buf, __u32 len)
{
        __u32 u;

        ASSERT(len != U32_MAX);

        for(u=0;u<len;u++) {
                printf("%hhx ", buf[u]);
        }
}

static void neigh_printaddr(__u16 addrlen, char *addr)
{
        printf("addrlen = %d addr: ", (int) addrlen);
        print_hex(addr, addrlen);
        printf("\n");
}


static int __export_servicelist(int fd, char *data, __u32 len)
{
        __u32 totalwritten = 0;

        ASSERT(len != U32_MAX);

        while (totalwritten < len) {
                int written = write(fd, data + totalwritten, len -totalwritten);

                if (written <= 0) {
                        if (errno == EINTR)
                                continue;

                        perror("export servicelist");
                        return 1;
                }

                totalwritten += written;
        }

        return 0;
}

static int _export_servicelist(int fd, char *addr, __u32 addrlen, __u32 cost,
                struct service_list *services)
{
        __u32 u;
        char buf[24];
        int rc;

        for (u=0;u<services->numports;u++) {
                __u32 v;

                ASSERT(addrlen != U32_MAX);

                for(v=0;v<addrlen;v++) {
                        if (((__u8) addr[v]) < 16)
                                rc = snprintf(&(buf[0]), 20, "0%hhx", addr[v]);
                        else
                                rc = snprintf(&(buf[0]), 20, "%hhx", addr[v]);
                        ASSERT(rc > 0);
                        ASSERT(rc < 20);

                        rc = __export_servicelist(fd, &(buf[0]), rc);
                        if (rc != 0)
                                return 1;
                }

                rc =  snprintf(&(buf[0]), 20, ":%hu:%u\n",
                                ntohs(services->ports[u]), cost);
                ASSERT(rc > 0);
                ASSERT(rc < 20);

                rc = __export_servicelist(fd, &(buf[0]), rc);
                if (rc != 0)
                        return 1;
        }

        return 0;
}

static void export_servicelist(void)
{
        char *servicelist_txtfile = "/var/lib/cor/services/txt";
        char *servicelist_tmpfile = "/var/lib/cor/services/tmp";

        int fd = 0;
        struct list_head *curr;

        if (export_servicelist_enabled == 0)
                return;

        fd = open(servicelist_tmpfile, O_WRONLY | O_CREAT | O_TRUNC, 0644);
        if (fd <= 0) {
                perror("export servicelist");
                return;
        }

        if (_export_servicelist(fd, 0, 0, 0, &localservices) != 0)
                goto err;

        curr = node_list.next;
        while (curr != &node_list) {
                struct node *currnode = container_of(curr, struct node,
                                node_list);

                curr = curr->next;

                if (addr_eq(currnode->addr, currnode->addrlen, localaddr,
                                localaddrlen) != 0)
                        continue;

                if (_export_servicelist(fd, currnode->addr, currnode->addrlen,
                                currnode->hopcount, &(currnode->services)) != 0)
                        goto err;
        }

        fsync(fd);
        close(fd);

        if (rename(servicelist_tmpfile, servicelist_txtfile) != 0) {
                perror("export servicelist");
                goto err2;
        }

        if (0) {
err:
                close(fd);
err2:
                unlink(servicelist_tmpfile);
        }
}

static void __recalc_routes(struct node *node)
{
        struct list_head *curr;

        curr = node_list.next;
        while (curr != &node_list) {
                struct node *currnode = container_of(curr, struct node,
                                node_list);

                if (currnode->hopcount < node->hopcount)
                        break;

                curr = curr->next;
        }

        list_del(&(node->node_list));
        list_add_tail(&(node->node_list), curr);
}

static void _recalc_routes(struct node *node)
{
        struct route_list *routes = (node == 0 ?
                        &localneighs : &(node->routes));

        __u32 u;

        for (u=0;u<routes->numroutes;u++) {
                struct node *target = routes->routes[u].dst;

                __u32 hopcount = (node == 0 ? 0 : node->hopcount) + 1;

                if (target->hopcount != 0 && target->hopcount <= hopcount)
                        continue;

                if (node == 0) {
                        bzero(&(target->route[0]), sizeof(target->route));
                } else {
                        memcpy(&(target->route[0]), &(node->route[0]),
                                        sizeof(target->route));
                }

                target->hopcount = hopcount;
                target->route[hopcount-1] = u;

                __recalc_routes(target);
        }
}

static void recalc_routes(void)
{
        struct list_head oldnodes;
        struct list_head *curr;

        list_add(&oldnodes, &node_list);
        list_del(&node_list);
        init_list_head(&node_list);


        curr = oldnodes.next;
        while (curr != &oldnodes) {
                struct node *currnode = container_of(curr, struct node,
                                node_list);

                bzero(&(currnode->route[0]), sizeof(currnode->route));
                currnode->hopcount = 0;

                curr = curr->next;
        }


        _recalc_routes(0);

        curr = node_list.next;
        while (curr != &node_list) {
                struct node *currnode = container_of(curr, struct node,
                                node_list);

                _recalc_routes(currnode);

                curr = curr->next;
        }


        curr = oldnodes.next;
        while (curr != &oldnodes) {
                struct node *currnode = container_of(curr, struct node,
                                node_list);

                curr = curr->next;

                list_del(&(currnode->node_list));

                if (currnode->addr != 0) {
                        free(currnode->addr);
                        currnode->addr = 0;
                }

                free(currnode);
        }

        export_servicelist();
}

static int load_neigh_list(int fd, struct nonblock_resumeinfo *nr,
                struct node *node)
{
        struct libcor_nonblock_resumeinfo *lnr;
        int rc = RC_CONNBROKEN;

        ASSERT((fd == 0 && nr == 0) || (fd != 0 && nr != 0));

        if (fd == 0 && nr == 0) {
                struct epoll_event epe;

                fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
                if(fd < 0) {
                        perror("socket");
                        goto early_out;
                }

                rc = connect(fd, 0, 0);
                if(rc < 0) {
                        perror("connect");
                        goto out;
                }

                set_nonblock(fd, 1);

                nr = (struct nonblock_resumeinfo *)
                                malloc(sizeof(struct nonblock_resumeinfo));
                bzero(nr, sizeof(struct nonblock_resumeinfo));
                nr->fd = fd;
                nr->type = EPOLLDATA_DISCOVERNETWORK;
                nr->data.discover_network.node = node;

                bzero(&epe, sizeof(struct epoll_event));
                epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR |
                                EPOLLET);
                epe.data.ptr = nr;
                epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &epe);
        }

        lnr = &(nr->data.discover_network.lnr);

        ASSERT(nr->type == EPOLLDATA_DISCOVERNETWORK);
        ASSERT(nr->data.discover_network.node == node);

        if (nr->data.discover_network.state == 1) {
                goto state_1;
        } else if (nr->data.discover_network.state == 2) {
                goto state_2;
        } else if (nr->data.discover_network.state == 3) {
                goto state_3;
        } else if (nr->data.discover_network.state == 4) {
                goto state_4;
        } else if (nr->data.discover_network.state == 5) {
                goto state_5;
        } else if (nr->data.discover_network.state == 6) {
                goto state_6;
        } else if (nr->data.discover_network.state == 7) {
                goto state_7;
        } else if (nr->data.discover_network.state == 8) {
                goto state_8;
        } else if (nr->data.discover_network.state != 0) {
                ASSERT(0);
        }

        if (node == 0) {
                nr->data.discover_network.list = &localneighs;
                nr->data.discover_network.service = &localservices;
        } else {
                neigh_printaddr(node->addrlen,
                                node->addr);
                nr->data.discover_network.list = &(node->routes);
                nr->data.discover_network.service = &(node->services);

                nr->data.discover_network.state = 1;
state_1:
                rc = connect_to_host_send(fd, lnr,
                                &(nr->data.discover_network.connect_send),
                                node);

                if (check_rc(rc, "load_neigh_list: connect_to_host_send error"))
                        goto out;
        }

        rc = send_list_neigh_nonblock(fd, lnr);
        nr->data.discover_network.state = 2;
        if (check_rc(rc, "load_neigh_list: send_list_neigh error"))
                goto out;

state_2:
        rc = send_list_services_nonblock(fd, lnr);
        nr->data.discover_network.state = 3;
        if (check_rc(rc, "load_neigh_list: send_list_services error"))
                goto out;

state_3:
        if (node != 0) {
                nr->data.discover_network.state = 4;
state_4:
                rc = connect_to_host_recv(fd, lnr,
                                &(nr->data.discover_network.connect_recv),
                                node);
                if (check_rc(rc, "load_neigh_list: connect_to_host_recv error"))
                        goto out;
        }

        nr->data.discover_network.state = 5;
state_5:
        rc = read_resp_nonblock(fd, lnr);
        if (check_rc(rc, "load_neigh_list: read_resp error"))
                goto out;

        nr->data.discover_network.state = 6;
state_6:
        rc = read_neigh_list_nonblock(fd, lnr, nr->data.discover_network.list,
                        init_neighlist, add_neigh);
        if (check_rc(rc, "load_neigh_list: read_neigh_list error"))
                goto out;

        nr->data.discover_network.state = 7;
state_7:
        rc = read_resp_nonblock(fd, lnr);
        if (check_rc(rc, "load_neigh_list: read_resp error"))
                goto out;

        nr->data.discover_network.state = 8;
state_8:
        rc = read_service_list_nonblock(fd, lnr,
                        nr->data.discover_network.service,
                        init_servicelist, add_service);
        if (check_rc(rc, "load_neigh_list: read_service_list error"))
                goto out;

        #warning todo rollback node->routes and node->services on error
        recalc_routes();
out:
        //printf("load_neigh_list state %d\n", nr->data.discover_network.state);
        if (rc != RC_WOULDBLOCK) {
                //printf("load_neigh_list rc %d\n", rc);
                close(fd);
                if (nr != 0)
                        free(nr);
        }

early_out:
        return rc;
}

static void discover_network(int fd, struct nonblock_resumeinfo *nr)
{
        int rc;
        struct search_query q;
        bzero(&q, sizeof(struct search_query));
        q.type = SEARCHQUERY_NEIGHSNOTQUERIED;

        ASSERT((fd == 0 && nr == 0) || (fd != 0 && nr != 0));

        #warning todo catch RC_CONNBROKEN
        if (fd == 0 && nr == 0) {
                rc = load_neigh_list(fd, nr, 0);
                if (rc == RC_WOULDBLOCK)
                        return;
        }

        while (1) {
                struct node *node;
                if (nr == 0) {
                        node = try_find_neigh(&q);
                        if (node == 0)
                                break;
                } else {
                        ASSERT(nr->type == EPOLLDATA_DISCOVERNETWORK);
                        node = nr->data.discover_network.node;
                }

                rc = load_neigh_list(fd, nr, node);
                if (rc == RC_WOULDBLOCK)
                        return;
                node->neighs_queried = 1;
                fd = 0;
                nr = 0;
        }
        discover_finished = 1;
}

static void send_unreach_error(__u64 cookie)
{
        int rc;
        // printf("send_rdsock_connecterror\n");
        set_nonblock(rdsock_fd, 0);
        rc = send_rdsock_connecterror(rdsock_fd, cookie,
                        CONNECTERROR_NETUNREACH);
        set_nonblock(rdsock_fd, 1);
        ASSERT(rc == RC_OK);
}

//cookie+addr are not passed on nonblocking resume
static void _rdscmd_void_connect(struct nonblock_resumeinfo *nr)
{
        int rc = RC_OK;

        struct libcor_nonblock_resumeinfo *lnr = &(nr->data.rds_connect.lnr);

        ASSERT(nr->type == EPOLLDATA_RDSCONNECT);

        if (nr->data.rds_connect.state == 1) {
                goto state_1;
        } else if (nr->data.rds_connect.state == 2) {
                goto state_2;
        } else if (nr->data.rds_connect.state == 3) {
                goto state_3;
        } else if (nr->data.rds_connect.state != 0) {
                ASSERT(0);
        }

        if (nr->data.rds_connect.node == 0)
                rc = RC_OK;
        else
                rc = connect_to_host_send(nr->fd,  lnr,
                                &(nr->data.rds_connect.connect_send),
                                nr->data.rds_connect.node);
        nr->data.rds_connect.state = 1;
        if (check_rc(rc, "_rdscmd_void_connect: connect_to_host_send error"))
                goto out;

state_1:
        rc = send_connect_port_nonblock(nr->fd, lnr, nr->data.rds_connect.port);
        nr->data.rds_connect.state = 2;
        if (check_rc(rc, "_rdscmd_void_connect: send_connect_port error"))
                goto out;

state_2:
        if (nr->data.rds_connect.node == 0)
                rc = RC_OK;
        else
                rc = connect_to_host_recv(nr->fd, lnr,
                                &(nr->data.rds_connect.connect_resp),
                                nr->data.rds_connect.node);
        if (check_rc(rc, "_rdscmd_void_connect: connect_to_host_recv error"))
                goto out;

        nr->data.rds_connect.state = 3;
state_3:
        rc = read_resp_nonblock(nr->fd, lnr);
        if (check_rc(rc, "_rdscmd_void_connect: read_resp error"))
                goto out;

        rc = pass_socket(nr->fd, nr->data.rds_connect.cookie);
        if (rc != RC_OK) {
                printf("pass_socket error\n");
                goto err;
        }

        return;

out:
        if (rc == RC_WOULDBLOCK)
                return;

err:
        close(nr->fd);
        send_unreach_error(nr->data.rds_connect.cookie);
}

static int rdscmd_void_connect(void *ptr, __u64 cookie,
                struct cor_sockaddr *addr)
{
        int fd, rc;
        struct nonblock_resumeinfo *nr;

        struct epoll_event epe;
        struct node *node;

        if (addr->sin_family != AF_COR) {
                printf("rds_connect %llu not af_cor\n", cookie);
                goto out_noclose;
        }

        printf("rds_connect cookie: %llu, addr: ", cookie);
        print_hex(&(addr->addr[0]), sizeof(addr->addr));
        printf(" port: %u\n", ntohs(addr->port));

        if (addr_eq(addr->addr, sizeof(addr->addr), 0, 0) != 0 ||
                        addr_eq(addr->addr, sizeof(addr->addr), localaddr,
                        localaddrlen) != 0) {
                node = 0;
        } else if ((node = try_find_neigh_byaddr(sizeof(addr->addr),
                        &(addr->addr[0]))) == 0) {
                printf("connect_to_host host not found\n");
                goto out_noclose;
        }


        fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RAW);
        if(fd < 0) {
                perror("socket");
                goto out_noclose;
        }

        rc = connect(fd, 0, 0);
        if(rc < 0) {
                perror("connect");
                goto out;
        }

        set_nonblock(fd, 1);

        nr = (struct nonblock_resumeinfo *)
                        malloc(sizeof(struct nonblock_resumeinfo));
        bzero(nr, sizeof(struct nonblock_resumeinfo));
        nr->fd = fd;
        nr->type = EPOLLDATA_RDSCONNECT;
        nr->data.rds_connect.cookie = cookie;
        nr->data.rds_connect.node = node;
        nr->data.rds_connect.port = addr->port;

        bzero(&epe, sizeof(struct epoll_event));
        epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR |
                        EPOLLET);
        epe.data.ptr = nr;
        epoll_ctl(epoll_fd, EPOLL_CTL_ADD, fd, &epe);

        _rdscmd_void_connect(nr);

        if (0) {
out:
                close(fd);
out_noclose:
                send_unreach_error(cookie);
        }
        return 0;
}

static int proc_rdsock_cmd(int fd, struct rdsock_cmd *cmd)
{
        if (cmd->cmd == CRD_KTU_CONNECT) {
                return parse_rdsock_connect(0, cmd, rdscmd_void_connect);
        } else {
                printf("error in proc_rdsock_cmd: unknown cmd: %u\n", cmd->cmd);
                ASSERT(0);
                return 1;
        }
}

static struct nonblock_resumeinfo *_init_fwd(int fd)
{
        struct nonblock_resumeinfo *nr =
                        malloc(sizeof(struct nonblock_resumeinfo));

        struct epoll_event epe;

        bzero(nr, sizeof(struct nonblock_resumeinfo));
        nr->fd = fd;
        nr->type = EPOLLDATA_FORWARD;
        nr->data.forward.buf = malloc(FORWARD_BUF_SIZE);
        bzero(&epe, sizeof(struct epoll_event));
        epe.events = (EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLERR | EPOLLET);
        epe.data.ptr = nr;

        epoll_ctl(epoll_fd, EPOLL_CTL_ADD, nr->fd, &epe);

        return nr;
}

static void init_fwd(int fd0, int fd1)
{
        struct nonblock_resumeinfo *dir0 = _init_fwd(fd0);
        struct nonblock_resumeinfo *dir1 = _init_fwd(fd1);

        dir0->data.forward.reversedir = dir1;
        dir1->data.forward.reversedir = dir0;
}

static void fwd_to_rservice_accept(int listenerfd, __be16 targetport)
{
        while (1) {
                int localfd;
                struct node *node;
                struct cor_sockaddr addr;
                int remotefd;

                localfd = accept(listenerfd, 0, 0);
                if (localfd < 0)
                        return;

                set_nonblock(localfd, 1);

                if (service_list_contains(&(localservices), targetport) != 0) {
                        node = 0;
                } else if ((node = try_find_neigh_byservice(targetport)) == 0) {
                        printf("fwd_to_rservice_accept no host with service found\n");
                        goto failed;
                }

                bzero(&addr, sizeof(struct cor_sockaddr));
                addr.sin_family = AF_COR;
                addr.port = targetport;

                if (node != 0) {
                        if (node->addrlen > MAX_ADDRLEN)
                                goto failed;
                        memcpy(&(addr.addr[0]), node->addr, node->addrlen);
                }

                remotefd = socket(PF_COR, SOCK_STREAM, 0);
                if (remotefd == -1) {
                        perror("socket");
                        goto failed;
                }

                set_nonblock(remotefd, 1);

                if (connect(remotefd, (struct sockaddr *) &addr,
                                sizeof(struct cor_sockaddr)) != 0) {
                        perror("connect");
                        goto failed;
                }

                init_fwd(localfd, remotefd);

                if (0) {
failed:
                        close(localfd);
                }

        }
}

#define RC_FORWARD_OK 1
#define RC_FORWARD_WOULDBLOCK 2
#define RC_FORWARD_ERROR 3

static int forward_write(struct nonblock_resumeinfo *nr)
{
        __u32 buffill = nr->data.forward.buffill;
        int more = 0;

        ASSERT(buffill <= FORWARD_BUF_SIZE);
        ASSERT(nr->data.forward.bufwritten <= buffill);

        if (buffill == FORWARD_BUF_SIZE) {
                buffill -= 1;
                more = 1;
        }

        while (nr->data.forward.bufwritten < buffill) {
                int rc = send(nr->fd, nr->data.forward.buf +
                                nr->data.forward.bufwritten,
                                buffill - nr->data.forward.bufwritten,
                                more == 0 ? 0 : MSG_MORE);

                if (rc < 0 && (errno == EAGAIN ||
                                errno == EWOULDBLOCK)) {
                        return RC_FORWARD_WOULDBLOCK;
                } else if (rc <= 0) {
                        if (errno == EINTR)
                                continue;

                        perror("forward_write");
                        return RC_FORWARD_ERROR;
                } else {
                        nr->data.forward.bufwritten += rc;
                        ASSERT(nr->data.forward.bufwritten <= buffill);
                }
        }

        return RC_FORWARD_OK;
}

static int forward_read(struct nonblock_resumeinfo *nr)
{
        int readfd = nr->data.forward.reversedir->fd;
        int rc;

        ASSERT(nr->data.forward.bufwritten <= nr->data.forward.buffill);

        if (nr->data.forward.bufwritten == nr->data.forward.buffill) {
                nr->data.forward.buffill = 0;
                nr->data.forward.bufwritten = 0;
        } else if (nr->data.forward.bufwritten * 3 > nr->data.forward.buffill) {
                memmove(nr->data.forward.buf, nr->data.forward.buf +
                                nr->data.forward.bufwritten,
                                nr->data.forward.buffill -
                                nr->data.forward.bufwritten);
                nr->data.forward.buffill -= nr->data.forward.bufwritten;
                nr->data.forward.bufwritten = 0;
        }

        if (nr->data.forward.buffill == FORWARD_BUF_SIZE)
                return RC_FORWARD_OK;

        while (nr->data.forward.buffill < FORWARD_BUF_SIZE) {
                rc = recv(readfd, nr->data.forward.buf +
                                nr->data.forward.buffill,
                                FORWARD_BUF_SIZE - nr->data.forward.buffill, 0);

                if (rc == 0) {
                        #warning todo eof
                        return RC_FORWARD_OK;
                } else if (rc < 0) {
                        if (errno == EAGAIN || errno == EWOULDBLOCK) {
                                return RC_FORWARD_WOULDBLOCK;
                        } else if (errno == EINTR) {
                                continue;
                        } else {
                                perror("forward_read");
                                return RC_FORWARD_ERROR;
                        }
                } else {
                        nr->data.forward.buffill += rc;
                        ASSERT(nr->data.forward.buffill <= FORWARD_BUF_SIZE);
                }
        }

        return RC_FORWARD_OK;
}

static void _forward(struct nonblock_resumeinfo *nr)
{
        while (1) {
                int rc;

                rc = forward_read(nr);
                if (rc == RC_FORWARD_ERROR)
                        goto err;
                if (nr->data.forward.bufwritten == nr->data.forward.buffill)
                        break;

                rc = forward_write(nr);
                if (rc == RC_FORWARD_ERROR)
                        goto err;
                else if (rc == RC_FORWARD_WOULDBLOCK)
                        return;
        }

        #warning todo call EPOLL_CTL_DEL + free where needed (some places calling close do not)
        if (0) {
err:
                epoll_ctl(epoll_fd, EPOLL_CTL_DEL,
                                nr->data.forward.reversedir->fd, 0);
                close(nr->data.forward.reversedir->fd);
                epoll_ctl(epoll_fd, EPOLL_CTL_DEL, nr->fd, 0);
                close(nr->fd);

                #warning todo free after all events are processed
                /* free(nr->data.forward.reversedir);
                free(nr); */
        }

}

static void forward(struct nonblock_resumeinfo *nr, __u32 eventflags)
{
        ASSERT(nr->type == EPOLLDATA_FORWARD);
        ASSERT(nr->data.forward.reversedir->type == EPOLLDATA_FORWARD);

        if ((eventflags & EPOLLOUT) != 0) {
                nr->data.forward.is_connected = 1;
        }

        if (nr->data.forward.is_connected == 0 ||
                        nr->data.forward.reversedir->data.forward.is_connected
                        == 0)
                return;

        _forward(nr);
        _forward(nr->data.forward.reversedir);
}

static void nonblock_resume(struct nonblock_resumeinfo *nr, __u32 eventflags)
{
        if (nr->type == EPOLLDATA_DISCOVERNETWORK) {
                int rc = resume_send_ifneeded(nr->fd,
                                &(nr->data.discover_network.lnr));

                //printf("load_neigh_list state = %d rc = %d\n", nr->data.discover_network.state, rc);

                if (rc != RC_OK)
                        return;

                discover_network(nr->fd, nr);
        } else if (nr->type == EPOLLDATA_RDSCONNECT) {
                int rc = resume_send_ifneeded(nr->fd,
                                &(nr->data.discover_network.lnr));

                if (rc != RC_OK)
                        return;

                _rdscmd_void_connect(nr);
        } else if (nr->type == EPOLLDATA_RDSOCKCMD) {
                struct rdsock_cmd cmd;

                int rc = resume_send_ifneeded(nr->fd,
                                &(nr->data.rdsock_cmd.lnr));

                if (rc != RC_OK)
                        return;

                rc = read_rdsock_cmd_nonblock(rdsock_fd,
                                &(nr->data.rdsock_cmd.lnr), &cmd);
                ASSERT(rc != RC_CONNBROKEN);
                if (rc == RC_WOULDBLOCK)
                        return;

                rc = proc_rdsock_cmd(rdsock_fd, &cmd);
                free_rdsockcmd_data(&cmd);
                ASSERT(rc == 0);
        } else if (nr->type == EPOLLDATA_FWD_TO_RSERVICE) {
                fwd_to_rservice_accept(nr->fd,
                                nr->data.fwd_to_rservice.targetport);
        } else if (nr->type == EPOLLDATA_FORWARD) {
                forward(nr, eventflags);
        } else {
                ASSERT(0);
        }
}

static int rdsock_negotiate_version(void)
{
        struct rdsock_cmd cmd;
        __u32 versionmin = 1000;
        __u32 versionmax = 0;
        int rc;

        rc = read_rdsock_cmd(rdsock_fd, &cmd);
        if (rc != RC_OK) {
                printf("read_rdsock_cmd rc = %d\n", rc);
                return 1;
        }

        if (cmd.cmd != CRD_KTU_SUPPORTEDVERSIONS) {
                printf("rhsock supportedversions not sent\n", rc);
                return 1;
        }

        rc = parse_rdsock_supported_versions(&cmd, &versionmin, &versionmax);
        if (rc != 0) {
                printf("parse_rdsock_supported_versions rc = %d\n", rc);
                return 1;
        }

        /* printf("rdsock_negotiate_version versionmin %u versionmax %u\n",
                        versionmin, versionmax); */

        if (versionmin != 0) {
                printf("rdsock_negotiate_version versionmin of kernel is %u, "
                                "but needs to be 0\n"
                                "You probably need to upgrade corutils or "
                                "downgrade the kernel\n", versionmin);
                return 1;
        }

        rc = send_rdsock_version(rdsock_fd, 0);
        ASSERT(rc == RC_OK);

        return 0;
}

static void epoll_add_fwd_to_rservice(struct list_head *rservice_fwds)
{
        while (list_empty(rservice_fwds) == 0) {
                struct fwd_to_rservice_item *item = container_of(
                                rservice_fwds->next,
                                struct fwd_to_rservice_item,
                                allfwds);

                struct nonblock_resumeinfo *nr;

                struct epoll_event epe;

                set_nonblock(item->bindfd, 1);

                nr = malloc(sizeof(struct nonblock_resumeinfo));
                bzero(nr, sizeof(struct nonblock_resumeinfo));
                nr->fd = item->bindfd;
                nr->type = EPOLLDATA_FWD_TO_RSERVICE;
                nr->data.fwd_to_rservice.targetport = item->targetport;
                bzero(&epe, sizeof(struct epoll_event));
                epe.events = (EPOLLIN | EPOLLERR);
                epe.data.ptr = nr;

                epoll_ctl(epoll_fd, EPOLL_CTL_ADD, item->bindfd, &epe);

                list_del(&(item->allfwds));
                free(item);
        }
}

static int parse_hexchar(char c)
{
        if (c == '0')
                return 0;
        else if (c == '1')
                return 1;
        else if (c == '2')
                return 2;
        else if (c == '3')
                return 3;
        else if (c == '4')
                return 4;
        else if (c == '5')
                return 5;
        else if (c == '6')
                return 6;
        else if (c == '7')
                return 7;
        else if (c == '8')
                return 8;
        else if (c == '9')
                return 9;
        else if (c == 'A' || c == 'a')
                return 10;
        else if (c == 'B' || c == 'b')
                return 11;
        else if (c == 'C' || c == 'c')
                return 12;
        else if (c == 'D' || c == 'd')
                return 13;
        else if (c == 'E' || c == 'e')
                return 14;
        else if (c == 'F' || c == 'f')
                return 15;
        else
                return -1;
}

static int parse_hex(char *target, char *src, int srclen)
{
        int u;

        ASSERT(src != 0);
        ASSERT(target != 0);
        ASSERT(srclen > 0);
        ASSERT((srclen%2) == 0);

        for(u=0;u<(srclen/2);u++) {
                int high = parse_hexchar(src[u*2]);
                int low = parse_hexchar(src[u*2+1]);

                if (high == -1 || low == -1)
                        return -1;

                 target[u] = (char) ((high << 4) + low);
        }

        return 0;
}

static int parse_addr(char *argv_addr)
{
        int rc;
        int len = strlen(argv_addr);
        char *noaddr = "noaddr";

        if (strncmp(argv_addr, noaddr, len) == 0) {
                localaddr = 0;
                localaddrlen = 0;
                return;
        }

        if ((len < 2) || (len%2) != 0)
                goto parseerror;

        localaddr = malloc(len/2);
        localaddrlen = len/2;

        rc = parse_hex(localaddr, argv_addr, len);
        if (rc != 0)
                goto parseerror;

        return 0;

parseerror:
        fprintf(stderr, "error: argv_addr is not a valid address (contains "
                        "non-hex characters or number of hex characters is not "
                        "a multiple of 2)\n");
        return -1;
}

static int parse_port(char *arg, __u32 len, __u16 *port)
{
        char *tmpbuf;
        char *endptr = 0;

        long int tmpport = 0;

        tmpbuf = malloc(len+1);
        memcpy(tmpbuf, arg, len);
        tmpbuf[len] = 0;

        tmpport = strtol(tmpbuf, &endptr, 10);
        if (tmpbuf[0] == 0 || endptr == 0 || endptr != tmpbuf + len) {
                free(tmpbuf);
                tmpbuf = 0;
                return 1;
        }
        free(tmpbuf);
        tmpbuf = 0;

        if (tmpport <= 0 || tmpport >= 65536)
                return 1;

        *port = (__u16) tmpport;
        return 0;
}

static int parse_inetaddr(char *addr, __u32 len, int *af,
                struct sockaddr **saddr, socklen_t *saddrlen)
{
        __u32 idx = len - 1;

        __u16 port;

        char *host;

        struct in_addr inaddr;
        struct in6_addr in6addr;

        while (idx > 0) {
                if (addr[idx] == ':') {
                        break;
                }
                idx--;
        }

        if (idx <= 0 || idx >= len - 1)
                return 1;

        if (parse_port(addr + idx + 1, len - idx - 1, &port) != 0)
                return 1;

        host = malloc(idx + 1);
        memcpy(host, addr, idx);
        host[idx] = 0;

        if (inet_pton(AF_INET, host, &inaddr) == 1) {
                struct sockaddr_in *ret;

                free(host);
                host = 0;

                ret = malloc(sizeof(struct sockaddr_in));
                bzero(ret, sizeof(struct sockaddr_in));

                ret->sin_family = AF_INET;
                ret->sin_port = htons(port);
                memcpy(&(ret->sin_addr), &inaddr, sizeof(inaddr));

                *af = AF_INET;
                *saddr = (struct sockaddr *) ret;
                *saddrlen = sizeof(struct sockaddr_in);

                return 0;
        } else if (inet_pton(AF_INET6, host, &in6addr) == 1) {
                free(host);
                host = 0;

                printf("sorry IPv6 support is not implemented yet\n");

                return 1;
        } else {
                free(host);
                host = 0;
                return 1;
        }
}

static int parse_fwd_to_rservice(char *argv_fwd,
                struct list_head *rservice_fwds)
{
        struct fwd_to_rservice_item *item;
        int af;
        struct sockaddr *saddr;
        socklen_t saddrlen;

        __u16 serviceport;

        __u32 idx = 0;
        __u32 len = 0;

        int optval;

        while (1) {
                if (argv_fwd[len] == 0) {
                        break;
                } else if (argv_fwd[len] == ':') {
                        idx = len;
                }
                len++;
        }

        if (idx <= 0 || idx >= len - 1)
                return 1;

        if (parse_inetaddr(argv_fwd, idx, &af, &saddr, &saddrlen) != 0)
                return 1;

        if (parse_port(argv_fwd + idx + 1, len - idx - 1, &serviceport) != 0)
                return 1;

        item = malloc(sizeof(struct fwd_to_rservice_item));
        item->bindfd = socket(af, SOCK_STREAM, 0);
        if (item->bindfd < 0) {
                perror("socket");
                goto err;
        }
        optval = 1;
        if (setsockopt(item->bindfd, SOL_SOCKET, SO_REUSEADDR, &optval,
                        sizeof(optval)) != 0) {
                perror("setsockopt");
                goto err;
        }
        if (setsockopt(item->bindfd, SOL_SOCKET, SO_REUSEADDR, &optval,
                        sizeof(optval)) != 0) {
                perror("setsockopt");
                goto err;
        }
        if (bind(item->bindfd, saddr, saddrlen) != 0) {
                perror("bind");
                goto err;
        }
        if (listen(item->bindfd, 100) != 0) {
                perror("listen");
                goto err;
        }
        item->targetport = htons(serviceport);

        list_add_tail(&(item->allfwds), rservice_fwds);

        free(saddr);

        return 0;

err:
        free(item);
        free(saddr);

        return 1;
}

static int parse_args(int argc, char *argv[], struct list_head *rservice_fwds)
{
        int rc;
        int addrfound = 0;
        __u32 argsconsumed = 0;

        if (argc > 1000000)
                goto usage;

        while (1) {
                if (argc <= argsconsumed + 1)
                        break;

                if (strcmp(argv[argsconsumed + 1], "--addr") == 0) {
                        if (argc <= argsconsumed + 2)
                                goto usage;

                        if (addrfound != 0)
                                goto usage;

                        rc = parse_addr(argv[argsconsumed + 2]);
                        if (rc != 0)
                                goto usage;

                        addrfound = 1;

                        argsconsumed += 2;
                } else if (strcmp(argv[argsconsumed + 1],
                                "--export-servicelist") == 0) {
                        export_servicelist_enabled = 1;

                        argsconsumed += 1;
                } else if (strcmp(argv[argsconsumed + 1],
                                "--fwd-to-rservice") == 0) {
                        if (argc <= argsconsumed + 2)
                                goto usage;

                        rc = parse_fwd_to_rservice(argv[argsconsumed + 2],
                                        rservice_fwds);
                        if (rc != 0)
                                goto usage;

                        argsconsumed += 2;
                } else {
                        goto usage;
                }
        }

        if (addrfound == 0)
                goto usage;

        if (0) {
usage:
                fprintf(stderr, "usage: test_routed2"
                                " [--export-servicelist]"
                                " [--fwd-to-rservice bindaddr:bindport:remoteport]"
                                " --addr addr (even number of hex digits or noaddr)\n");
                rc = 1;
        }

        return rc;
}

int main(int argc, char *argv[])
{
        int rc;
        struct list_head rservice_fwds;

        struct nonblock_resumeinfo rds_nr;

        int discover_finished_executed = 0;

        discover_finished = 0;
        epoll_fd = 0;

        init_list_head(&rservice_fwds);

        umask(0);

        rc = parse_args(argc, argv, &rservice_fwds);
        if (rc != 0)
                goto out;


        init_list_head(&node_list);

        epoll_fd = epoll_create(1);
        if (epoll_fd <= 0) {
                perror("epoll_create");
                goto out;
        }


        rdsock_fd = socket(PF_COR, SOCK_RAW, PROTO_COR_RDEAMON);
        if (rdsock_fd < 0) {
                perror("socket");
                goto out;
        }

        rc = connect(rdsock_fd, 0, 0);
        if (rc < 0) {
                perror("connect");
                goto out;
        }

        rc = rdsock_negotiate_version();
        if (rc != 0)
                goto out;

        rc = send_rdsock_up(rdsock_fd, localaddr, localaddrlen);
        ASSERT(rc == RC_OK);

        set_nonblock(rdsock_fd, 1);

        sleep(30);

        discover_network(0, 0);

        while (1) {
                int u;
                struct epoll_event events[EPOLL_EVENTS];
                int rdycnt;

                if (discover_finished_executed == 0 && discover_finished != 0) {
                        struct epoll_event rds_epe;

                        bzero(&rds_nr, sizeof(struct nonblock_resumeinfo));
                        rds_nr.fd = rdsock_fd;
                        rds_nr.type = EPOLLDATA_RDSOCKCMD;
                        bzero(&rds_epe, sizeof(struct epoll_event));
                        rds_epe.events = (EPOLLIN | EPOLLRDHUP | EPOLLERR);
                        rds_epe.data.ptr = &rds_nr;

                        epoll_ctl(epoll_fd, EPOLL_CTL_ADD, rdsock_fd, &rds_epe);

                        epoll_add_fwd_to_rservice(&rservice_fwds);

                        discover_finished_executed = 1;
                        printf("discover finished\n");
                }

                rdycnt = epoll_wait(epoll_fd, events, EPOLL_EVENTS, -1);

                for (u=0;u<rdycnt;u++) {
                        struct nonblock_resumeinfo *nr = events[u].data.ptr;
                        __u32 eventflags = events[u].events;
                        nonblock_resume(nr, eventflags);
                }
        }

out:
        return 1;
}