patty/src/server.c

#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <errno.h>

#include <patty/ax25.h>

typedef int (*patty_ax25_server_call)(patty_ax25_server *, int);

struct _patty_ax25_server {
    char path[PATTY_AX25_SOCK_PATH_SIZE];

    int fd,     /* fd of UNIX domain socket */
        fd_max;

    struct timeval timeout;

    fd_set fds_watch,   /* fds to monitor with select() */
           fds_socks,   /* fds belonging to socks */
           fds_clients, /* fds belonging to clients */
           fds_r,       /* fds select()ed for reading */
           fds_w;       /* fds select()ed for writing */

    patty_list *ifaces;
    patty_ax25_route_table *routes;

    patty_dict *socks_by_fd,
               *socks_pending_accept,
               *socks_pending_connect,
               *socks_established;

    patty_dict *clients,
               *clients_by_sock;
};

patty_ax25_server *patty_ax25_server_new(const char *path) {
    patty_ax25_server *server;

    if ((server = malloc(sizeof(*server))) == NULL) {
        goto error_malloc_server;
    }

    memset(server, '\0', sizeof(*server));

    strncpy(server->path, path, PATTY_AX25_SOCK_PATH_SIZE);

    if ((server->ifaces = patty_list_new()) == NULL) {
        goto error_list_new_ifaces;
    }

    if ((server->routes = patty_ax25_route_table_new()) == NULL) {
        goto error_route_table_new;
    }

    if ((server->socks_by_fd = patty_dict_new()) == NULL) {
        goto error_dict_new_socks_by_fd;
    }

    if ((server->socks_pending_accept = patty_dict_new()) == NULL) {
        goto error_dict_new_socks_pending_accept;
    }

    if ((server->socks_pending_connect = patty_dict_new()) == NULL) {
        goto error_dict_new_socks_pending_connect;
    }

    if ((server->socks_established = patty_dict_new()) == NULL) {
        goto error_dict_new_socks_established;
    }

    if ((server->clients = patty_dict_new()) == NULL) {
        goto error_dict_new_clients;
    }

    if ((server->clients_by_sock = patty_dict_new()) == NULL) {
        goto error_dict_new_clients_by_sock;
    }

    return server;

error_dict_new_clients_by_sock:
    patty_dict_destroy(server->clients);

error_dict_new_clients:
    patty_dict_destroy(server->socks_established);

error_dict_new_socks_established:
    patty_dict_destroy(server->socks_pending_connect);

error_dict_new_socks_pending_connect:
    patty_dict_destroy(server->socks_pending_accept);

error_dict_new_socks_pending_accept:
    patty_dict_destroy(server->socks_by_fd);

error_dict_new_socks_by_fd:
    patty_ax25_route_table_destroy(server->routes);

error_route_table_new:
    patty_list_destroy(server->ifaces);

error_list_new_ifaces:
    free(server);

error_malloc_server:
    return NULL;
}

static int destroy_if(patty_ax25_if *iface, void *ctx) {
    patty_ax25_if_destroy(iface);

    return 0;
}

static patty_ax25_sock *sock_by_fd(patty_dict *dict,
                                   int fd) {
    return patty_dict_get_with_hash(dict, (uint32_t)fd);
}

static patty_ax25_sock *sock_by_addr(patty_dict *dict,
                                     patty_ax25_addr *addr) {
    patty_dict_slot *slot;

    uint32_t hash;

    patty_hash_init(&hash);
    patty_ax25_addr_hash(&hash, addr);
    patty_hash_end(&hash);

    if ((slot = patty_dict_slot_find(dict, hash)) == NULL) {
        errno = EEXIST;

        goto error_dict_slot_find;
    }

    return (patty_ax25_sock *)slot->value;

error_dict_slot_find:
    return NULL;
}

static patty_ax25_sock *sock_by_addrpair(patty_dict *dict,
                                         patty_ax25_addr *local,
                                         patty_ax25_addr *remote) {
    patty_dict_slot *slot;

    uint32_t hash;
    
    patty_hash_init(&hash);
    patty_ax25_addr_hash(&hash, local);
    patty_ax25_addr_hash(&hash, remote);
    patty_hash_end(&hash);

    if ((slot = patty_dict_slot_find(dict, hash)) == NULL) {
        errno = EEXIST;

        goto error_dict_slot_find;
    }

    return (patty_ax25_sock *)slot->value;

error_dict_slot_find:
    return NULL;
}

static int sock_save_by_fd(patty_dict *dict, patty_ax25_sock *sock) {
    if (patty_dict_set_with_hash(dict,
                                 NULL + sock->fd,
                                 sizeof(sock->fd),
                                 sock,
                                 (uint32_t)sock->fd) == NULL) {
        goto error_dict_set;
    }

    return sock->fd;

error_dict_set:
    return -1;
}

static int sock_save_by_addr(patty_dict *dict,
                             patty_ax25_sock *sock,
                             patty_ax25_addr *addr) {
    uint32_t hash;

    patty_hash_init(&hash);
    patty_ax25_addr_hash(&hash, addr);
    patty_hash_end(&hash);

    if (patty_dict_set_with_hash(dict,
                                 addr,
                                 sizeof(*addr),
                                 sock,
                                 hash) == NULL) {
        goto error_dict_set_with_hash;
    }
    
    return 0;

error_dict_set_with_hash:
    return -1;
}

static int sock_save_by_addrpair(patty_dict *dict,
                                 patty_ax25_sock *sock,
                                 patty_ax25_addr *local,
                                 patty_ax25_addr *remote) {
    uint32_t hash;

    patty_hash_init(&hash);
    patty_ax25_addr_hash(&hash, local);
    patty_ax25_addr_hash(&hash, remote);
    patty_hash_end(&hash);

    if (patty_dict_set_with_hash(dict,
                                sock,
                                sizeof(*sock),
                                sock,
                                hash) == NULL) {
        goto error_dict_set_with_hash;
    }
    
    return 0;

error_dict_set_with_hash:
    return -1;
}

static int sock_delete(patty_ax25_server *server,
                       patty_ax25_sock *sock) {
    if (sock->status == PATTY_AX25_SOCK_ESTABLISHED) {
        uint32_t hash;

        patty_hash_init(&hash);
        patty_ax25_addr_hash(&hash, &sock->local);
        patty_ax25_addr_hash(&hash, &sock->remote);
        patty_hash_end(&hash);

        if (patty_dict_delete_with_hash(server->socks_established, hash) < 0) {
            goto error_dict_delete_established;
        }
    }

    if (sock->status == PATTY_AX25_SOCK_PENDING_CONNECT) {
        uint32_t hash;

        patty_hash_init(&hash);
        patty_ax25_addr_hash(&hash, &sock->local);
        patty_hash_end(&hash);

        if (patty_dict_delete_with_hash(server->socks_pending_connect, hash) < 0) {
            goto error_dict_delete_pending_connect;
        }
    }

    if (sock->status == PATTY_AX25_SOCK_PENDING_ACCEPT) {
        uint32_t hash;

        patty_hash_init(&hash);
        patty_ax25_addr_hash(&hash, &sock->local);
        patty_hash_end(&hash);

        if (patty_dict_delete_with_hash(server->socks_pending_accept, hash) < 0) {
            goto error_dict_delete_pending_accept;
        }
    }

    if (patty_dict_delete_with_hash(server->socks_by_fd, sock->fd) < 0) {
        goto error_dict_delete_by_fd;
    }

    free(sock);

    return 0;

error_dict_delete_by_fd:
error_dict_delete_pending_accept:
error_dict_delete_pending_connect:
error_dict_delete_established:
    return -1;
}

static int sock_delete_by_addr(patty_dict *dict,
                               patty_ax25_addr *addr) {
    uint32_t hash;

    patty_hash_init(&hash);
    patty_ax25_addr_hash(&hash, addr);
    patty_hash_end(&hash);

    return patty_dict_delete_with_hash(dict, hash);
}

static int sock_delete_by_addrpair(patty_dict *dict,
                                   patty_ax25_addr *local,
                                   patty_ax25_addr *remote) {
    uint32_t hash;

    patty_hash_init(&hash);
    patty_ax25_addr_hash(&hash, local);
    patty_ax25_addr_hash(&hash, remote);
    patty_hash_end(&hash);

    return patty_dict_delete_with_hash(dict, hash);
}

static inline void watch_fd(patty_ax25_server *server, int fd) {
    FD_SET(fd, &server->fds_watch);

    if (server->fd_max <= fd) {
        server->fd_max  = fd + 1;
    }
}

static inline void clear_fd(patty_ax25_server *server, int fd) {
    FD_CLR(fd, &server->fds_watch);

    if (server->fd_max == fd + 1) {
        server->fd_max--;
    }
}

static inline void watch_sock(patty_ax25_server *server, patty_ax25_sock *sock) {
    watch_fd(server, sock->fd);

    FD_SET(sock->fd, &server->fds_socks);
}

static inline void clear_sock(patty_ax25_server *server, patty_ax25_sock *sock) {
    clear_fd(server, sock->fd);

    FD_CLR(sock->fd, &server->fds_socks);
}

static inline void watch_client(patty_ax25_server *server, int fd) {
    watch_fd(server, fd);

    FD_SET(fd, &server->fds_clients);
}

static inline void clear_client(patty_ax25_server *server, int fd) {
    clear_fd(server, fd);

    FD_CLR(fd, &server->fds_clients);
}

int patty_ax25_server_add_if(patty_ax25_server *server,
                             patty_ax25_if *iface) {
    int fd;

    if ((fd = patty_kiss_tnc_fd(iface->tnc)) >= 0) {
        watch_fd(server, fd);
    }

    if (patty_list_append(server->ifaces, iface) == NULL) {
        goto error_list_append;
    }

    return 0;

error_list_append:
    return -1;
}

int patty_ax25_server_delete_if(patty_ax25_server *server,
                                patty_ax25_if *iface) {
    patty_list_item *item = server->ifaces->first;

    int fd, i = 0;

    while (item) {
        if (item->value == iface) {
            if (patty_list_splice(server->ifaces, i) == NULL) {
                goto error_list_splice;
            }

            return 0;
        }

        item = item->next;
        i++;
    }

    if ((fd = patty_kiss_tnc_fd(iface->tnc)) >= 0) {
        watch_fd(server, fd);

        if (patty_dict_delete(server->socks_by_fd, NULL + fd, sizeof(fd)) < 0) {
            goto error_dict_delete;
        }
    }

    return 0;

error_dict_delete:
error_list_splice:
    return -1;
}

patty_ax25_if *patty_ax25_server_get_if(patty_ax25_server *server,
                                        const char *name) {
    patty_list_iterator *iter;
    patty_ax25_if *iface;

    if ((iter = patty_list_start(server->ifaces)) == NULL) {
        goto error_list_start;
    }

    while ((iface = patty_list_next(iter)) != NULL) {
        if (strncmp(iface->name, name, sizeof(iface->name)) == 0) {
            patty_list_finish(iter);

            return iface;
        }
    }

    patty_list_finish(iter);

error_list_start:
    return NULL;
}

int patty_ax25_server_each_if(patty_ax25_server *server,
                              int (*callback)(patty_ax25_if *, void *),
                              void *ctx) {
    patty_list_iterator *iter;
    patty_ax25_if *iface;

    if ((iter = patty_list_start(server->ifaces)) == NULL) {
        goto error_list_start;
    }

    while ((iface = patty_list_next(iter)) != NULL) {
        if (callback(iface, ctx) < 0) {
            goto error_callback;
        }
    }

    patty_list_finish(iter);

    return 0;

error_callback:
    patty_list_finish(iter);

error_list_start:
    return -1;
}

int patty_ax25_server_add_route(patty_ax25_server *server,
                                patty_ax25_route *route) {
    return patty_ax25_route_table_add(server->routes, route);
}

static int server_socket(patty_ax25_server *server,
                         int client) {
    patty_ax25_call_socket_request request;
    patty_ax25_call_socket_response response;

    patty_ax25_sock *sock;

    if (read(client, &request, sizeof(request)) < 0) {
        goto error_read;
    }

    if ((sock = patty_ax25_sock_new(request.type)) == NULL) {
        goto error_sock_new;
    }

    if ((sock->fd = open("/dev/null", O_RDWR)) < 0) {
        goto error_open;
    }

    response.ret = sock->fd;
    response.eno = 0;

    if (sock_save_by_fd(server->socks_by_fd, sock) < 0) {
        response.ret = -1;
        response.eno = errno;

        goto error_sock_save_by_fd;
    }

error_sock_save_by_fd:
    if (write(client, &response, sizeof(response)) < 0) {
        goto error_write;
    }

    return 0;

error_write:
    close(sock->fd);

error_open:
    patty_ax25_sock_destroy(sock);

error_sock_new:
error_read:
    return -1;
}

static int server_bind(patty_ax25_server *server,
                       int client) {
    patty_ax25_call_bind_request request;
    patty_ax25_call_bind_response response;

    patty_ax25_sock *sock;

    if (read(client, &request, sizeof(request)) < 0) {
        goto error_io;
    }

    if ((sock = sock_by_fd(server->socks_by_fd, request.socket)) == NULL) {
        response.ret = -1;
        response.eno = EBADF;

        goto error_sock_by_fd;
    }

    switch (sock->status) {
        case PATTY_AX25_SOCK_LISTENING:
        case PATTY_AX25_SOCK_ESTABLISHED:
            response.ret = -1;
            response.eno = EINVAL;

            goto error_invalid_status;

        default:
            break;
    }

    memcpy(&sock->local, &request.peer, sizeof(request.peer));

    response.ret = 0;
    response.eno = 0;

error_invalid_status:
error_sock_by_fd:
    if (write(client, &response, sizeof(response)) < 0) {
        goto error_io;
    }

    return 0;

error_io:
    return -1;
}

static int server_listen(patty_ax25_server *server,
                         int client) {
    patty_ax25_call_listen_request request;
    patty_ax25_call_listen_response response;

    patty_ax25_sock *sock;

    if (read(client, &request, sizeof(request)) < 0) {
        goto error_io;
    }

    if ((sock = sock_by_fd(server->socks_by_fd, request.socket)) == NULL) {
        response.ret = -1;
        response.eno = EBADF;

        goto error_sock_by_fd;
    }

    if (sock->local.callsign[0] == '\0') {
        response.eno = EINVAL;

        goto error_invalid_fd;
    }

    sock->status = PATTY_AX25_SOCK_LISTENING;

    response.ret = 0;
    response.eno = 0;

error_invalid_fd:
error_sock_by_fd:
    if (write(client, &response, sizeof(response)) < 0) {
        goto error_io;
    }

    return 0;

error_io:
    return -1;
}

static int server_accept_pty(patty_ax25_server *server,
                             patty_ax25_call_accept_response *response,
                             patty_ax25_sock *sock) {
    if ((sock->fd = open("/dev/ptmx", O_RDWR)) < 0) {
        goto error_open;
    }

    if (grantpt(sock->fd) < 0) {
        goto error_grantpt;
    }

    if (unlockpt(sock->fd) < 0) {
        goto error_unlockpt;
    }

    if (ptsname_r(sock->fd, response->path, PATTY_AX25_SOCK_PATH_SIZE) < 0) {
        goto error_ptsname_r;
    }

    sock->type |= PATTY_AX25_SOCK_PTY;

    response->ret = sock->fd;
    response->eno = 0;

    return 0;

error_ptsname_r:
error_unlockpt:
error_grantpt:
    close(sock->fd);

error_open:
    return -1;
}

static int server_accept_unix(patty_ax25_server *server,
                              patty_ax25_call_accept_response *response,
                              patty_ax25_sock *sock) {
    struct sockaddr_un addr;
    struct stat st;

    if ((sock->fd = socket(PF_UNIX, SOCK_STREAM, 0)) < 0) {
        goto error_socket;
    }

    if (snprintf(response->path,
                 PATTY_AX25_SOCK_PATH_SIZE,
                 PATTY_AX25_SERVER_CLIENT_PATH_FORMAT,
                 sock->fd) < 0) {
        goto error_snprintf;
    }

    if (stat(response->path, &st) >= 0) {
        unlink(response->path);
    }

    memset(&addr, '\0', sizeof(addr));
    addr.sun_family = AF_UNIX;
    strncpy(addr.sun_path, response->path, sizeof(addr.sun_path));

    if (bind(sock->fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        goto error_bind;
    }

    sock->type |= PATTY_AX25_SOCK_UNIX;

    response->ret = sock->fd;
    response->eno = 0;

    return 0;

error_bind:
error_snprintf:
    close(sock->fd);

error_socket:
    return -1;
}

static int server_accept(patty_ax25_server *server,
                         int client) {
    patty_ax25_call_accept_request request;
    patty_ax25_call_accept_response response;

    patty_ax25_sock *local,
                    *remote;

    if (read(client, &request, sizeof(request)) < 0) {
        goto error_io;
    }

    if ((local = sock_by_fd(server->socks_by_fd, request.socket)) == NULL) {
        response.ret = -1;
        response.eno = EBADF;

        goto error_sock_by_fd;
    }

    if ((remote = patty_ax25_sock_new(local->type)) == NULL) {
        goto error_sock_new_remote;
    }

    memcpy(&remote->local, &local->local, sizeof(remote->local));

    if (local->type & PATTY_AX25_SOCK_PTY) {
        if (server_accept_pty(server, &response, remote) < 0) {
            goto error_server_accept_pty;
        }
    } else {
        if (server_accept_unix(server, &response, remote) < 0) {
            goto error_server_accept_unix;
        }
    }

    remote->status = PATTY_AX25_SOCK_PENDING_ACCEPT;

    if (sock_save_by_addr(server->socks_pending_accept,
                          remote,
                          &local->local) < 0) {
        goto error_save_by_addr;
    }

    if (sock_save_by_fd(server->socks_by_fd, remote) < 0) {
        goto error_save_by_fd;
    }

    return 0;

error_sock_by_fd:
    if (write(client, &response, sizeof(response)) < 0) {
        goto error_io;
    }

    return 0;

error_save_by_fd:
error_save_by_addr:
error_server_accept_unix:
error_server_accept_pty:
    patty_ax25_sock_destroy(remote);

error_sock_new_remote:
error_io:
    return -1;
}

static int server_connect(patty_ax25_server *server,
                          int client) {
    patty_ax25_call_connect_request request;
    patty_ax25_call_connect_response response;

    patty_ax25_sock *sock;
    patty_ax25_route *route;
    patty_ax25_if *iface;

    if (read(client, &request, sizeof(request)) < 0) {
        goto error_io;
    }

    if ((sock = sock_by_fd(server->socks_by_fd, request.socket)) == NULL) {
        response.ret = -1;
        response.eno = EBADF;

        goto error_sock_by_fd;
    }

    switch (sock->status) {
        case PATTY_AX25_SOCK_LISTENING:
            response.ret = -1;
            response.eno = EINVAL;

            goto error_invalid_status;

            break;

        case PATTY_AX25_SOCK_ESTABLISHED:
            response.ret = -1;
            response.eno = EISCONN;

            goto error_invalid_status;

            break;

        default:
            break;
    }

    if (sock->local.callsign[0]) {
        /*
         * If there is an address already bound to this socket, locate the
         * appropriate route.
         */
        route = patty_ax25_route_table_find(server->routes, &sock->local);
    } else {
        /*
         * Otherwise, locate the default route.
         */
        route = patty_ax25_route_table_default(server->routes);
    }

    /*
     * If no route could be found, then assume the network is down or not
     * configured.
     */
    if (route == NULL) {
        response.ret = -1;
        response.eno = ENETDOWN;

        goto error_network_down;
    }

    iface = sock->iface = route->iface;

    /*
     * If there is no local address bound to this sock, then bind the
     * address of the default route interface.
     */
    if (sock->local.callsign[0] == '\0') {
        memcpy(&sock->local, &iface->addr, sizeof(sock->local));
    }

    /*
     * Bind the requested remote address to the socket.
     */
    memcpy(&sock->remote, &request.peer, sizeof(request.peer));

    sock->status = PATTY_AX25_SOCK_PENDING_CONNECT;

    if (sock_save_by_addrpair(server->socks_pending_connect,
                              sock,
                              &sock->local,
                              &sock->remote) < 0) {
        goto error_sock_save_by_addrpair;
    }

    /*
     * TODO: Send SABM(E) frame; set timer, and await response from peer.
     * We will need to know what client to send a response to when either the
     * timeout has been exceeded or the peer has sent a UA frame to
     * acknowledge and establish the connection.
     */
    if (patty_ax25_sock_send_sabm(sock, 1) < 0) {
        response.ret = -1;
        response.eno = EIO;

        goto error_sock_send_sabm;
    }

     return 0;

error_sock_send_sabm:
error_network_down:
error_invalid_status:
error_sock_by_fd:
    if (write(client, &response, sizeof(response)) < 0) {
        goto error_io;
    }

    return 0;

error_sock_save_by_addrpair:
error_io:
    return -1;
}

static int server_close(patty_ax25_server *server,
                        int client) {
    patty_ax25_call_close_request request;
    patty_ax25_call_close_response response;

    patty_ax25_sock *sock;

    if (read(client, &request, sizeof(request)) < 0) {
        goto error_io;
    }

    if ((sock = sock_by_fd(server->socks_by_fd, request.socket)) == NULL) {
        response.ret = -1;
        response.eno = EBADF;

        goto error_sock_by_fd;
    }

    if (sock_delete(server, sock) < 0) {
        response.ret = -1;
        response.eno = EBADF;

        goto error_sock_delete;
    }

    if (close(request.socket) < 0) {
        response.ret = -1;
        response.eno = errno;

        goto error_close;
    }

    response.ret = 0;
    response.eno = 0;

error_close:
error_sock_delete:
error_sock_by_fd:
    if (write(client, &response, sizeof(response)) < 0) {
        goto error_io;
    }

    return 0;

error_io:
    return -1;
}

static patty_ax25_server_call server_calls[PATTY_AX25_CALL_COUNT] = {
    NULL,
    server_socket,
    server_bind,
    server_listen,
    server_accept,
    server_connect,
    server_close,
    NULL,
    NULL
};

static int listen_unix(patty_ax25_server *server, const char *path) {
    struct sockaddr_un addr;
    struct stat st;

    if (server->fd) {
        errno = EBUSY;

        goto error_listening;
    }

    if (stat(path, &st) >= 0) {
        unlink(path);
    }

    if ((server->fd = socket(PF_UNIX, SOCK_STREAM, 0)) < 0) {
        goto error_socket;
    }

    memset(&addr, '\0', sizeof(addr));
    addr.sun_family = AF_UNIX;
    strncpy(addr.sun_path, path, sizeof(addr.sun_path));

    if (bind(server->fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        goto error_bind;
    }

    if (listen(server->fd, 0) < 0) {
        goto error_listen;
    }

    watch_fd(server, server->fd);

    return 0;

error_listen:
error_bind:
    close(server->fd);

error_socket:
error_listening:
    return -1;
}

static int accept_client(patty_ax25_server *server) {
    int fd;
    struct sockaddr addr;
    socklen_t addrlen = sizeof(addr);

    memset(&addr, '\0', addrlen);

    if (!FD_ISSET(server->fd, &server->fds_r)) {
        goto done;
    }

    if ((fd = accept(server->fd, &addr, &addrlen)) < 0) {
        goto error_accept;
    }

    if (patty_dict_set_with_hash(server->clients,
                                 NULL + fd,
                                 sizeof(fd),
                                 NULL + fd,
                                 (uint32_t)fd) == NULL) {
        goto error_dict_set;
    }

    watch_client(server, fd);

done:
    return 0;

error_dict_set:
    close(fd);

error_accept:
    return -1;
}

static int handle_client(void *key,
                         size_t keysz,
                         void *value,
                         void *ctx) {
    patty_ax25_server *server = ctx;
    int client = (int)(key - NULL);

    ssize_t readlen;
    enum patty_ax25_call call;

    if (!FD_ISSET(client, &server->fds_r)) {
        goto done;
    }

    if ((readlen = read(client, &call, sizeof(call))) < 0) {
        goto error_io;
    } else if (readlen == 0) {
        clear_client(server, client);

        if (patty_dict_delete_with_hash(server->clients,
                                        (uint32_t)(key - NULL)) < 0) {
            goto error_dict_delete;
        }

        if (close(client) < 0) {
            goto error_io;
        }

        goto done;
    }

    if (call <= PATTY_AX25_CALL_NONE || call >= PATTY_AX25_CALL_COUNT) {
        goto error_io;
    }

    if (server_calls[call] == NULL) {
        errno = ENOSYS;

        goto error_not_implemented;
    }
        
    return server_calls[call](server, client);

done:
    return 0;

error_not_implemented:
error_dict_delete:
error_io:
    return -1;
}

static int handle_clients(patty_ax25_server *server) {
    return patty_dict_each(server->clients, handle_client, server);
}

static void save_reply_addr(patty_ax25_sock *sock,
                            patty_ax25_frame *frame) {
    unsigned int i,
                 hops = frame->hops > PATTY_AX25_MAX_HOPS?
                                      PATTY_AX25_MAX_HOPS: sock->hops;

    memcpy(&sock->remote, &frame->src, sizeof(patty_ax25_addr));

    for (i=0; i<hops; i++) {
        memcpy(&sock->repeaters[i],
               &frame->repeaters[hops-1-i],
               sizeof(patty_ax25_addr));
    }

    sock->hops = hops;
}

static int reply_to(patty_ax25_frame *frame,
                    patty_ax25_if *iface,
                    enum patty_ax25_frame_format format,
                    uint16_t control,
                    uint8_t proto,
                    void *info,
                    size_t infolen) {
    ssize_t len;

    if ((len = patty_ax25_frame_encode_reply_to(frame,
                                                iface->tx_buf,
                                                PATTY_AX25_FRAME_NORMAL,
                                                PATTY_AX25_FRAME_U_DISC,
                                                PATTY_AX25_PROTO_NONE,
                                                NULL,
                                                0,
                                                iface->tx_bufsz)) < 0) {
        goto error_toobig;
    }

    return patty_ax25_if_send(iface, iface->tx_buf, len);

error_toobig:
    return -1;
}

static int reply_dm(patty_ax25_frame *frame,
                    patty_ax25_if *iface,
                    enum patty_ax25_frame_u_flags flags) {
    return reply_to(frame,
                    iface,
                    PATTY_AX25_FRAME_NORMAL,
                    PATTY_AX25_FRAME_U_DM | flags,
                    PATTY_AX25_PROTO_NONE,
                    NULL,
                    0);
}

static int reply_ua(patty_ax25_frame *frame,
                    patty_ax25_if *iface,
                    enum patty_ax25_frame_u_flags flags) {
    return reply_to(frame,
                    iface,
                    PATTY_AX25_FRAME_NORMAL,
                    PATTY_AX25_FRAME_U_UA | flags,
                    PATTY_AX25_PROTO_NONE,
                    NULL,
                    0);
}

static int handle_frame(patty_ax25_server *server,
                        patty_ax25_if *iface,
                        void *buf,
                        size_t len) {
    patty_ax25_frame frame;

    if (patty_ax25_frame_decode(&frame,
                                PATTY_AX25_FRAME_NORMAL,
                                buf,
                                len) < 0) {
        goto error_io;
    }

    if (PATTY_AX25_CONTROL_UNNUMBERED_SABM(frame.control)) {
        patty_ax25_sock *sock;

        if ((sock = sock_by_addr(server->socks_pending_accept,
                                 &frame.dest)) == NULL) {
            if (reply_dm(&frame, iface, PATTY_AX25_FRAME_U_FINAL) < 0) {
                goto error_io;
            }
        }

        sock->status = PATTY_AX25_SOCK_ESTABLISHED;
        sock->mode   = PATTY_AX25_SOCK_SABM;

        save_reply_addr(sock, &frame);

        if (sock_save_by_addrpair(server->socks_established,
                                  sock,
                                  &sock->local,
                                  &sock->remote) < 0) {
            goto error_io;
        }

        if (sock_delete_by_addr(server->socks_pending_accept,
                                &frame.dest) < 0) {
            goto error_io;
        }
    }

    return 0;

error_io:
    return -1;
}

static int handle_iface(patty_ax25_server *server, patty_ax25_if *iface) {
    int fd = patty_kiss_tnc_fd(iface->tnc);

    void *buf;
    ssize_t readlen;

    if (!FD_ISSET(fd, &server->fds_r)) {
        goto done;
    }

    if ((readlen = patty_ax25_if_recv(iface, &buf)) < 0) {
        goto error_io;
    } else if (readlen == 0) {
        clear_fd(server, fd);

        goto done;
    }

    if (handle_frame(server, iface, buf, readlen) < 0) {
        goto error_io;
    }

done:
    return 0;

error_io:
    return -1;
}

static int handle_ifaces(patty_ax25_server *server) {
    patty_list_iterator *iter;
    patty_ax25_if *iface;

    if ((iter = patty_list_start(server->ifaces)) == NULL) {
        goto error_list_start;
    }

    while ((iface = patty_list_next(iter)) != NULL) {
        if (handle_iface(server, iface) < 0) {
            goto error_io;
        }
    }

    patty_list_finish(iter);

    return 0;

error_io:
    patty_list_finish(iter);

error_list_start:
    return -1;
}

static int handle_sock(void *key,
                       size_t keysz,
                       void *value,
                       void *ctx) {
    patty_ax25_server *server = ctx;
    patty_ax25_sock   *sock   = value;
    patty_ax25_if     *iface  = sock->iface;

    int fd     = (int)(key - NULL),
        fd_tnc = patty_kiss_tnc_fd(iface->tnc);

    ssize_t len;

    if (!FD_ISSET(fd, &server->fds_r) || !FD_ISSET(fd_tnc, &server->fds_w)) {
        goto done;
    }

    if ((len = read(fd, iface->tx_buf, iface->tx_bufsz)) < 0) {
        goto error_io;
    } else if (len == 0) {
        clear_sock(server, sock);

        if (patty_dict_delete(server->socks_established,
                              NULL + fd,
                              sizeof(fd)) < 0) {
            goto error_dict_delete;
        }

        goto done;
    }

    if ((len = patty_ax25_if_send(iface, iface->tx_buf, len)) < 0) {
        goto error_io;
    }

    /*
     * TODO: Finish this
     */

done:
    return 0;

error_dict_delete:
error_io:
    return -1;
}

static int handle_socks(patty_ax25_server *server) {
    return patty_dict_each(server->socks_established,
                           handle_sock,
                           server);
}

int patty_ax25_server_run(patty_ax25_server *server) {
    if (listen_unix(server, server->path) < 0) {
        goto error_listen_unix;
    }

    while (1) {
        int nready;

        memcpy(&server->fds_r, &server->fds_watch, sizeof(server->fds_r));

        if ((nready = select( server->fd_max,
                             &server->fds_r,
                             NULL,
                             NULL,
                             NULL)) < 0) {
            goto error_io;
        }

        if (accept_client(server) < 0) {
            goto error_io;
        }

        if (handle_clients(server) < 0) {
            goto error_io;
        }

        if (handle_ifaces(server) < 0) {
            goto error_io;
        }

        if (handle_socks(server) < 0) {
            goto error_io;
        }
    }

    close(server->fd);

    return 0;

error_io:
    close(server->fd);

error_listen_unix:
    return -1;
}

void patty_ax25_server_destroy(patty_ax25_server *server) {
    patty_dict_destroy(server->clients_by_sock);
    patty_dict_destroy(server->clients);
    patty_dict_destroy(server->socks_established);
    patty_dict_destroy(server->socks_pending_connect);
    patty_dict_destroy(server->socks_pending_accept);
    patty_dict_destroy(server->socks_by_fd);

    patty_ax25_server_each_if(server, destroy_if, NULL);
    patty_ax25_route_table_destroy(server->routes);
    patty_list_destroy(server->ifaces);

    free(server);
}