#include <stdlib.h>
#include <string.h>

#ifdef ECP_DEBUG
#include <stdio.h>
#endif

#include "core.h"
#include "ext.h"

#ifdef ECP_WITH_VCONN
#include "vconn/vconn.h"
#endif

#include "cr.h"
#include "tr.h"
#include "tm.h"

#ifdef ECP_WITH_HTABLE
#include "ht.h"
#endif

int ecp_dhkey_gen(ECPDHKey *key) {
    int rv;

    rv = ecp_ecdh_mkpair(&key->public, &key->private);
    if (rv) return rv;

    key->valid = 1;
    return ECP_OK;
}

int ecp_ctx_init(ECPContext *ctx, ecp_err_handler_t handle_err, ecp_conn_new_t conn_new, ecp_conn_free_t conn_free, ecp_key_checker_t key_checker) {
    int rv;

    memset(ctx, 0, sizeof(ECPContext));
    ctx->handle_err = handle_err;
    ctx->conn_new = conn_new;
    ctx->conn_free = conn_free;
    ctx->key_checker = key_checker;

    rv = ecp_tr_init(ctx);
    if (rv) return rv;

    rv = ecp_tm_init(ctx);
    if (rv) return rv;

    return ECP_OK;
}

int ecp_ctx_set_handler(ECPContext *ctx, unsigned char ctype, ECPConnHandler *handler) {
    unsigned char _ctype = ctype & ECP_CTYPE_MASK;

    if (ctype & ECP_CTYPE_FLAG_SYS) {
        if (_ctype >= ECP_MAX_CTYPE_SYS) return ECP_ERR_CTYPE;
        ctx->handler_sys[_ctype] = handler;
    } else {
        if (_ctype >= ECP_MAX_CTYPE) return ECP_ERR_CTYPE;
        ctx->handler[_ctype] = handler;
    }
    return ECP_OK;
}

ECPConnHandler *ecp_ctx_get_handler(ECPContext *ctx, unsigned char ctype) {
    unsigned char _ctype = ctype & ECP_CTYPE_MASK;

    if (ctype & ECP_CTYPE_FLAG_SYS) {
        if (_ctype >= ECP_MAX_CTYPE_SYS) return NULL;
        return ctx->handler_sys[_ctype];
    } else {
        if (_ctype >= ECP_MAX_CTYPE) return NULL;
        return ctx->handler[_ctype];
    }
    return NULL;
}

int ecp_addr_init(ecp_tr_addr_t *addr, void *addr_s) {
    int rv = ECP_OK;

    memset(addr, 0, sizeof(ecp_tr_addr_t));
    if (addr_s) {
        rv = ecp_tr_addr_set(addr, addr_s);
    }

    return rv;
}

void ecp_node_init(ECPNode *node, ecp_ecdh_public_t *public, ecp_tr_addr_t *addr) {
    memset(node, 0, sizeof(ECPNode));

    if (public) {
        ECPDHPub *key = &node->key_perma;

        memcpy(&key->public, public, sizeof(key->public));
        key->valid = 1;
    }

    if (addr) {
        node->addr = *addr;
    }
}

void ecp_node_set_pub(ECPNode *node, ecp_ecdh_public_t *public) {
    ECPDHPub *key = &node->key_perma;

    memcpy(&key->public, public, sizeof(key->public));
    key->valid = 1;
}

int ecp_node_set_addr(ECPNode *node, void *addr) {
    int rv;

    rv = ecp_tr_addr_set(&node->addr, addr);
    if (rv) return ECP_ERR_NET_ADDR;

    return ECP_OK;
}

static int conn_table_create(ECPConnTable *conn_table) {
    int rv;

    memset(conn_table, 0, sizeof(ECPConnTable));

#ifdef ECP_WITH_PTHREAD
    rv = pthread_mutex_init(&conn_table->mutex, NULL);
    if (rv) {
        return ECP_ERR;
    }

    rv = pthread_mutex_init(&conn_table->mutex_gc, NULL);
    if (rv) {
        pthread_mutex_destroy(&conn_table->mutex);
        return ECP_ERR;
    }
#endif

    rv = ECP_OK;

#ifdef ECP_WITH_HTABLE
    conn_table->keys = ecp_ht_create_keys();
    if (conn_table->keys == NULL) rv = ECP_ERR_ALLOC;

    if (!rv) {
        conn_table->keys_gc = ecp_ht_create_keys();
        if (conn_table->keys_gc == NULL) rv = ECP_ERR_ALLOC;
    }

    if (!rv) {
        conn_table->addrs = ecp_ht_create_addrs();
        if (conn_table->addrs == NULL) rv = ECP_ERR_ALLOC;
    }

    if (rv) {
#ifdef ECP_WITH_PTHREAD
        pthread_mutex_destroy(&conn_table->mutex_gc);
        pthread_mutex_destroy(&conn_table->mutex);
#endif
        if (conn_table->addrs) ecp_ht_destroy(conn_table->addrs);
        if (conn_table->keys_gc) ecp_ht_destroy(conn_table->keys_gc);
        if (conn_table->keys) ecp_ht_destroy(conn_table->keys);
    }
#endif

    return rv;
}

static void conn_table_destroy(ECPConnTable *conn_table) {
#ifdef ECP_WITH_PTHREAD
    pthread_mutex_destroy(&conn_table->mutex_gc);
    pthread_mutex_destroy(&conn_table->mutex);
#endif
#ifdef ECP_WITH_HTABLE
    ecp_ht_destroy(conn_table->addrs);
    ecp_ht_destroy(conn_table->keys_gc);
    ecp_ht_destroy(conn_table->keys);
#endif
}

static int conn_table_insert(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;

#ifdef ECP_WITH_HTABLE
    int i, rv = ECP_OK;

    if (ecp_conn_is_outb(conn)) {
        if (ecp_conn_is_root(conn) && !_ecp_conn_is_open(conn)) {
            rv = ecp_ht_insert(sock->conn_table.addrs, &conn->remote.addr, conn);
            if (rv) return rv;
        }

        for (i=0; i<ECP_MAX_CONN_KEY; i++) {
            if (conn->key[i].valid) {
                rv = ecp_ht_insert_uniq(sock->conn_table.keys, &conn->key[i].public, conn);
                if (rv) {
                    int j;

                    for (j=0; j<i; j++) {
                        if (conn->key[j].valid) {
                            ecp_ht_remove(sock->conn_table.keys, &conn->key[j].public);
                        }
                    }
                    if (ecp_conn_is_root(conn) && !_ecp_conn_is_open(conn)) {
                        ecp_ht_remove_kv(sock->conn_table.addrs, &conn->remote.addr, conn);
                    }
                    return rv;
                }
            }
        }
    } else {
        for (i=0; i<ECP_MAX_NODE_KEY; i++) {
            if (conn->rkey[i].valid) {
                rv = ecp_ht_insert_uniq(sock->conn_table.keys, &conn->rkey[i].public, conn);
                if (rv) {
                    int j;

                    for (j=0; j<i; j++) {
                        if (conn->rkey[j].valid) {
                            ecp_ht_remove(sock->conn_table.keys, &conn->rkey[j].public);
                        }
                    }
                    return rv;
                }
            }
        }
    }

#else   /* ECP_WITH_HTABLE */

    if (sock->conn_table.size == ECP_MAX_SOCK_CONN) return ECP_ERR_FULL;
    sock->conn_table.arr[sock->conn_table.size] = conn;
    sock->conn_table.size++;

#endif  /* ECP_WITH_HTABLE */

    return ECP_OK;
}

static void conn_table_remove(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;
    int i;

#ifdef ECP_WITH_HTABLE

    if (ecp_conn_is_outb(conn)) {
        for (i=0; i<ECP_MAX_CONN_KEY; i++) {
            if (conn->key[i].valid) {
                ecp_ht_remove(sock->conn_table.keys, &conn->key[i].public);
            }
        }
        if (ecp_conn_is_root(conn) && !_ecp_conn_is_open(conn)) {
            ecp_ht_remove_kv(sock->conn_table.addrs, &conn->remote.addr, conn);
        }
    } else {
        for (i=0; i<ECP_MAX_NODE_KEY; i++) {
            if (conn->rkey[i].valid) {
                ecp_ht_remove(sock->conn_table.keys, &conn->rkey[i].public);
            }
        }
    }

#else   /* ECP_WITH_HTABLE */

    for (i=0; i<sock->conn_table.size; i++) {
        if (conn == sock->conn_table.arr[i]) {
            while (i < (sock->conn_table.size-1)) {
                sock->conn_table.arr[i] = sock->conn_table.arr[i+1];
                i++;
            }
            sock->conn_table.arr[i] = NULL;
            sock->conn_table.size--;
            return;
        }
    }

#endif  /* ECP_WITH_HTABLE */
}

static void conn_table_remove_addr(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;

#ifdef ECP_WITH_HTABLE
    ecp_ht_remove_kv(sock->conn_table.addrs, &conn->remote.addr, conn);
#endif
}

static ECPConnection *conn_table_search_pub(ECPSocket *sock, unsigned char c_idx, ecp_ecdh_public_t *c_public) {
#ifdef ECP_WITH_HTABLE
    return ecp_ht_search(sock->conn_table.keys, c_public);
#else   /* ECP_WITH_HTABLE */
    ECPConnection *conn = NULL;
    int i;

    for (i=0; i<sock->conn_table.size; i++) {
        conn = sock->conn_table.arr[i];
        if (ecp_conn_is_outb(conn)) {
            if (c_idx >= ECP_MAX_CONN_KEY) continue;

            if (conn->key[c_idx].valid && (memcmp(c_public, &conn->key[c_idx].public, sizeof(conn->key[c_idx].public)) == 0)) {
                return conn;
            }
        } else {
            unsigned char _c_idx;

            if (c_idx & ~ECP_ECDH_IDX_MASK) continue;

            _c_idx = c_idx % ECP_MAX_NODE_KEY;
            if (conn->rkey[_c_idx].valid && (memcmp(c_public, &conn->rkey[_c_idx].public, sizeof(conn->key[c_idx].public) == 0)) {
                return conn;
            }
        }
    }

    return NULL;
#endif  /* ECP_WITH_HTABLE */
}

static ECPConnection *conn_table_search_addr(ECPSocket *sock, ecp_tr_addr_t *addr) {
#ifdef ECP_WITH_HTABLE
    return ecp_ht_search(sock->conn_table.addrs, addr);
#else   /* ECP_WITH_HTABLE */
    ECPConnection *conn = NULL;
    int i;

    for (i=0; i<sock->conn_table.size; i++) {
        conn = sock->conn_table.arr[i];
        if (ecp_conn_is_root(conn) && ecp_conn_is_outb(conn) && ecp_tr_addr_eq(&conn->remote.addr, addr)) {
            return conn;
        }
    }

    return NULL;
#endif  /* ECP_WITH_HTABLE */
}

static ECPConnection *conn_table_search_addr_next(ECPSocket *sock, ecp_tr_addr_t *addr, ECPConnection *_conn) {
#ifdef ECP_WITH_HTABLE
    return ecp_ht_search_next(sock->conn_table.addrs, addr, _conn);
#else   /* ECP_WITH_HTABLE */
    ECPConnection *conn = NULL;
    int i, f;

    f = 0;
    for (i=0; i<sock->conn_table.size; i++) {
        conn = sock->conn_table.arr[i];
        if (f) {
            if (ecp_conn_is_root(conn) && ecp_conn_is_outb(conn) && ecp_tr_addr_eq(&conn->remote.addr, addr)) {
                return conn;
            }
        } else if (conn == _conn) {
            f = 1;
        }
    }

    return NULL;
#endif  /* ECP_WITH_HTABLE */
}

static ECPConnection *conn_table_search(ECPSocket *sock, unsigned char c_idx, ecp_ecdh_public_t *c_public, ecp_tr_addr_t *addr, ECPConnection *parent) {
#ifdef ECP_WITH_VCONN
    if ((c_public == NULL) && parent) {
        if ((parent->type == ECP_CTYPE_VCONN) && ecp_conn_is_outb(parent)) {
            ECPVConnOutb *_parent = (ECPVConnOutb *)parent;

            return _parent->next;
        }
        return NULL;
    }
#endif

    if (c_public) {
        return conn_table_search_pub(sock, c_idx, c_public);
    } else if (addr) {
        return conn_table_search_addr(sock, addr);
    }

    return NULL;
}

static void conn_table_expire(ECPSocket *sock, ecp_conn_expired_t conn_expired) {
    ECPConnection *conn;
    ECPConnection *to_remove[ECP_MAX_EXPIRED];
    int i, remove_cnt, expired;
    ecp_sts_t now = ecp_tm_get_s();

#ifdef ECP_WITH_HTABLE
    struct hashtable_itr itr;
    void *remove_next;
    int rv = ECP_OK;


    remove_next = NULL;
    do {
        ecp_ht_table_t *keys_gc = sock->conn_table.keys_gc;
        remove_cnt = 0;

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&sock->conn_table.mutex_gc);
#endif

        if (ecp_ht_count(keys_gc) > 0) {
            ecp_ht_itr_create(&itr, keys_gc);
            if (remove_next) {
                ecp_ht_itr_search(&itr, remove_next);
                remove_next = NULL;
            }
            do {
                conn = ecp_ht_itr_value(&itr);

#ifdef ECP_WITH_PTHREAD
                pthread_mutex_lock(&conn->mutex);
#endif

                expired = conn_expired(conn, now);

#ifdef ECP_WITH_PTHREAD
                pthread_mutex_unlock(&conn->mutex);
#endif

                if (expired) {
                    rv = ecp_ht_itr_remove(&itr);

                    to_remove[remove_cnt] = conn;
                    remove_cnt++;
                    if (remove_cnt == ECP_MAX_EXPIRED) {
                        if (!rv) {
                            remove_next = ecp_ht_itr_key(&itr);
                        } else {
                            remove_next = NULL;
                        }
                        break;
                    }
                } else {
                    rv = ecp_ht_itr_advance(&itr);
                }
            } while (rv == ECP_OK);
        }

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&sock->conn_table.mutex_gc);
#endif

        for (i=0; i<remove_cnt; i++) {
            ecp_conn_remove(to_remove[i]);
            ecp_conn_refcount_dec(to_remove[i]);
        }
    } while (remove_next);

#else   /* ECP_WITH_HTABLE */

    do {
        remove_cnt = 0;

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&sock->conn_table.mutex);
#endif

        for (i=0; i<sock->conn_table.size; i++) {
            conn = sock->conn_table.arr[i];
#ifdef ECP_WITH_PTHREAD
            pthread_mutex_lock(&conn->mutex);
#endif

            expired = 0;
            if (_ecp_conn_in_gct(conn)) expired = conn_expired(conn, now);
            if (expired) _ecp_conn_pull_gct(conn);

#ifdef ECP_WITH_PTHREAD
            pthread_mutex_unlock(&conn->mutex);
#endif

            if (expired) {
                to_remove[remove_cnt] = conn;
                remove_cnt++;
                if (remove_cnt == ECP_MAX_EXPIRED) break;
            }
        }

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

        for (i=0; i<remove_cnt; i++) {
            ecp_conn_remove(to_remove[i]);
            ecp_conn_refcount_dec(to_remove[i]);
        }

    } while (remove_cnt < ECP_MAX_EXPIRED);

#endif  /* ECP_WITH_HTABLE */
}

int ecp_sock_init(ECPSocket *sock, ECPContext *ctx, ECPDHKey *key) {
    int rv;

    memset(sock, 0, sizeof(ECPSocket));
    sock->ctx = ctx;
    sock->key_curr = 0;
    if (key) sock->key_perma = *key;

    rv = ecp_dhkey_gen(&sock->key[sock->key_curr]);
    if (rv) return rv;

    arc4random_buf(&sock->ckey, sizeof(sock->ckey));

    return ECP_OK;
}

int ecp_sock_create(ECPSocket *sock, ECPContext *ctx, ECPDHKey *key) {
    int rv;

    rv = ecp_sock_init(sock, ctx, key);
    if (rv) return rv;

    rv = conn_table_create(&sock->conn_table);
    if (rv) return rv;

    rv = ecp_timer_create(&sock->timer);
    if (rv) {
        conn_table_destroy(&sock->conn_table);
        return rv;
    }

#ifdef ECP_WITH_PTHREAD
    rv = pthread_mutex_init(&sock->mutex, NULL);
    if (rv) {
        ecp_timer_destroy(&sock->timer);
        conn_table_destroy(&sock->conn_table);
        return ECP_ERR;
    }
#endif

    arc4random_buf(&sock->nonce_out, sizeof(sock->nonce_out));
    return ECP_OK;
}

void ecp_sock_destroy(ECPSocket *sock) {
    ecp_timer_destroy(&sock->timer);
    conn_table_destroy(&sock->conn_table);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_destroy(&sock->mutex);
#endif
}

int ecp_sock_open(ECPSocket *sock, ecp_tr_addr_t *myaddr) {
    return ecp_tr_open(sock, myaddr);
}

void ecp_sock_close(ECPSocket *sock) {
    ecp_sock_destroy(sock);
    ecp_tr_close(sock);
}

void ecp_sock_ckey_new(ECPSocket *sock) {
#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->mutex);
#endif

    arc4random_buf(&sock->ckey, sizeof(sock->ckey));

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->mutex);
#endif
}

int ecp_sock_dhkey_new(ECPSocket *sock) {
    ECPDHKey new_key;
    int rv;

    rv = ecp_dhkey_gen(&new_key);
    if (rv) return rv;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->mutex);
#endif

    sock->key_curr = (sock->key_curr + 1) % ECP_MAX_SOCK_KEY;
    sock->key[sock->key_curr] = new_key;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->mutex);
#endif

    return ECP_OK;
}

int ecp_sock_dhkey_get(ECPSocket *sock, unsigned char idx, ECPDHKey *key) {
    int rv = ECP_OK;

    if (idx == ECP_ECDH_IDX_PERMA) {
        *key = sock->key_perma;
    } else {

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&sock->mutex);
#endif

        if (idx < ECP_MAX_SOCK_KEY) {
            *key = sock->key[idx];
        } else {
            rv = ECP_ERR_ECDH_IDX;
        }

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&sock->mutex);
#endif

    }

    if (!rv && !key->valid) rv = ECP_ERR_ECDH_IDX;
    return rv;
}

int ecp_sock_dhkey_get_pub(ECPSocket *sock, unsigned char *idx, ecp_ecdh_public_t *public) {
    ECPDHKey *key;
    unsigned char _idx;
    int rv = ECP_OK;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->mutex);
#endif

    _idx = sock->key_curr;
    if (_idx == ECP_ECDH_IDX_INV) rv = ECP_ERR_ECDH_IDX;

    if (!rv) {
        key = &sock->key[_idx];
        if (!key->valid) rv = ECP_ERR_ECDH_IDX;
    }
    if (!rv) memcpy(public, &key->public, sizeof(key->public));

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->mutex);
#endif

    if (rv) return rv;

    if (idx) *idx = _idx;
    return ECP_OK;
}

void ecp_sock_expire(ECPSocket *sock, ecp_conn_expired_t conn_expired) {
    conn_table_expire(sock, conn_expired);
}

void ecp_atag_gen(ECPSocket *sock, unsigned char *public_buf, unsigned char *atag, ecp_nonce_t *nonce) {
    unsigned char msg[ECP_SIZE_ECDH_PUB+ECP_SIZE_NONCE];
    ecp_hmac_key_t key;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->mutex);
#endif

    memcpy(&key, &sock->ckey, sizeof(sock->ckey));
    *nonce = sock->nonce_out;
    sock->nonce_out++;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->mutex);
#endif

    memcpy(msg, public_buf, ECP_SIZE_ECDH_PUB);
    ecp_nonce2buf(msg+ECP_SIZE_ECDH_PUB, nonce);

    ecp_hmac(atag, &key, msg, sizeof(msg));
}

int ecp_cookie_verify(ECPSocket *sock, unsigned char *cookie, unsigned char *public_buf) {
    unsigned char msg[ECP_SIZE_ECDH_PUB+ECP_SIZE_NONCE];
    unsigned char atag[ECP_SIZE_ATAG];
    ecp_hmac_key_t key;
    ecp_nonce_t nonce_sock, nonce_cookie, nonce_offset;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->mutex);
#endif

    memcpy(&key, &sock->ckey, sizeof(sock->ckey));
    nonce_sock = sock->nonce_out - 1;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->mutex);
#endif

    ecp_buf2nonce(&nonce_cookie, cookie);
    nonce_offset = nonce_sock - nonce_cookie;
    if (nonce_offset > ECP_MAX_NONCE_FWD) return ECP_ERR_COOKIE;

    memcpy(msg, public_buf, ECP_SIZE_ECDH_PUB);
    memcpy(msg+ECP_SIZE_ECDH_PUB, cookie, ECP_SIZE_NONCE);
    ecp_hmac(atag, &key, msg, sizeof(msg));

    if (memcmp(cookie+ECP_SIZE_NONCE, atag, ECP_SIZE_ATAG) == 0) return ECP_OK;
    return ECP_ERR_COOKIE;
}

static ECPDHKey *conn_dhkey_get(ECPConnection *conn, unsigned char idx) {
    ECPDHKey *key = NULL;

    if (idx < ECP_MAX_CONN_KEY) {
        key = &conn->key[idx];
    }

    if (key && key->valid) return key;
    return NULL;
}

static int conn_dhkey_set(ECPConnection *conn, unsigned char idx, ECPDHKey *key) {
    ECPSocket *sock = conn->sock;

    if (idx >= ECP_MAX_CONN_KEY) return ECP_ERR_ECDH_IDX;

#ifdef ECP_WITH_HTABLE
    if (ecp_conn_is_outb(conn) && _ecp_conn_is_reg(conn) && conn->key[idx].valid) {
        ecp_ht_remove(sock->conn_table.keys, &conn->key[idx].public);
    }
#endif

    if (conn->key[idx].valid) {
        unsigned char _idx = idx % ECP_MAX_NODE_KEY;
        int i;

        for (i=0; i<ECP_MAX_NODE_KEY; i++) {
            conn->shkey[i][_idx].valid = 0;
        }
    }

    conn->key[idx] = *key;

#ifdef ECP_WITH_HTABLE
    if (ecp_conn_is_outb(conn) && _ecp_conn_is_reg(conn)) {
        int rv;

        rv = ecp_ht_insert_uniq(sock->conn_table.keys, &conn->key[idx].public, conn);
        if (rv) return rv;
    }
#endif

    return ECP_OK;
}

static int conn_dhkey_new(ECPConnection *conn, ECPDHKey *key) {
    unsigned char idx;
    int rv;

    idx = conn->key_curr;
    if (idx == ECP_ECDH_IDX_INV) return ECP_ERR_ECDH_IDX;

    idx = (idx + 1) % ECP_MAX_CONN_KEY;
    rv = conn_dhkey_set(conn, idx, key);
    if (rv) return rv;
    conn->key_next = idx;

    return ECP_OK;
}

static void conn_dhkey_set_curr(ECPConnection *conn) {
    if (conn->key_next != ECP_ECDH_IDX_INV) {
        conn->key_curr = conn->key_next;
        conn->key_next = ECP_ECDH_IDX_INV;
    }
}

static int conn_dhkey_get_pub(ECPConnection *conn, unsigned char *idx, ecp_ecdh_public_t *public) {
    if (ecp_conn_is_inb(conn)) {
        ECPSocket *sock = conn->sock;
        int rv;

        rv = ecp_sock_dhkey_get_pub(sock, idx, public);
        if (rv) return rv;

        if (*idx != conn->key_curr) {
            unsigned char _idx = *idx % ECP_MAX_NODE_KEY;
            int i;

            for (i=0; i<ECP_MAX_NODE_KEY; i++) {
                conn->shkey[_idx][i].valid = 0;
            }
        }
    } else {
        if (conn->key_next != ECP_ECDH_IDX_INV) {
            *idx = conn->key_next;
        } else {
            *idx = conn->key_curr;
        }
        if ((*idx == ECP_ECDH_IDX_INV) || !conn->key[*idx].valid) return ECP_ERR_ECDH_IDX;

        memcpy(public, &conn->key[*idx].public, sizeof(conn->key[*idx].public));
    }

    return ECP_OK;
}

static int conn_dhkey_set_pub(ECPConnection *conn, unsigned char idx, ecp_ecdh_public_t *public) {
    unsigned char _idx;
    ECPDHPub *key;
    ECPSocket *sock = conn->sock;
    int i;
    ecp_sts_t now = ecp_tm_get_s();

    if (idx & ~ECP_ECDH_IDX_MASK) return ECP_ERR_ECDH_IDX;

    _idx = idx % ECP_MAX_NODE_KEY;
    key = &conn->rkey[_idx];
    if (key->valid && (memcmp(public, &key->public, sizeof(key->public)) == 0)) return ECP_ERR_DUP;

    if (_ecp_conn_is_open(conn) && (now - conn->keyx_ts < ECP_MIN_KEYX_DT)) return ECP_ERR;

#ifdef ECP_WITH_HTABLE
    if (ecp_conn_is_inb(conn) && _ecp_conn_is_reg(conn) && (key->valid)) {
        ecp_ht_remove(sock->conn_table.keys, &key->public);
    }
#endif

    memcpy(&key->public, public, sizeof(key->public));
    key->valid = 1;
    conn->keyx_ts = now;

#ifdef ECP_WITH_HTABLE
    if (ecp_conn_is_inb(conn) && _ecp_conn_is_reg(conn)) {
        int rv;

        rv = ecp_ht_insert_uniq(sock->conn_table.keys, &key->public, conn);
        if (rv) return rv;
    }
#endif

    if (ecp_conn_is_outb(conn)) {
        conn->rkey_curr = idx;
        for (i=0; i<ECP_MAX_NODE_KEY; i++) {
            conn->shkey[_idx][i].valid = 0;
        }
    } else {
        for (i=0; i<ECP_MAX_NODE_KEY; i++) {
            conn->shkey[i][_idx].valid = 0;
        }
    }

    return ECP_OK;
}

static ECPDHPub *conn_dhkey_get_remote(ECPConnection *conn, unsigned char idx) {
    ECPDHPub *key = NULL;

    if (idx == ECP_ECDH_IDX_PERMA) {
        key = &conn->remote.key_perma;
    } else if ((idx & ECP_ECDH_IDX_MASK) == idx) {
        key = &conn->rkey[idx % ECP_MAX_NODE_KEY];
    }

    if (key && key->valid) return key;
    return NULL;
}

static int conn_shkey_get(ECPConnection *conn, unsigned char s_idx, unsigned char c_idx, ecp_aead_key_t *shkey) {
    ECPDHPub *pub;
    ECPDHKey *priv;
    int rv;

    if (ecp_conn_is_outb(conn) && (s_idx == ECP_ECDH_IDX_PERMA)) {
        pub = conn_dhkey_get_remote(conn, s_idx);
        priv = conn_dhkey_get(conn, c_idx);

        if ((pub == NULL) || (priv == NULL)) return ECP_ERR_ECDH_IDX;
        ecp_ecdh_shkey(shkey, &pub->public, &priv->private);
    } else {
        ECPDHShkey *_shkey;

        if (s_idx & ~ECP_ECDH_IDX_MASK) return ECP_ERR_ECDH_IDX;
        if (c_idx & ~ECP_ECDH_IDX_MASK) return ECP_ERR_ECDH_IDX;

        _shkey = &conn->shkey[s_idx % ECP_MAX_NODE_KEY][c_idx % ECP_MAX_NODE_KEY];
        if (!_shkey->valid) {
            if (ecp_conn_is_inb(conn)) {
                ECPSocket *sock = conn->sock;
                ECPDHKey priv;

                pub = conn_dhkey_get_remote(conn, c_idx);
                if (pub == NULL) return ECP_ERR_ECDH_IDX;

                rv = ecp_sock_dhkey_get(sock, s_idx, &priv);
                if (rv) return rv;

                ecp_ecdh_shkey(&_shkey->key, &pub->public, &priv.private);
                conn->key_curr = s_idx;
                conn->rkey_curr = c_idx;
            } else {
                pub = conn_dhkey_get_remote(conn, s_idx);
                priv = conn_dhkey_get(conn, c_idx);

                if ((pub == NULL) || (priv == NULL)) return ECP_ERR_ECDH_IDX;
                ecp_ecdh_shkey(&_shkey->key, &pub->public, &priv->private);
            }
            _shkey->valid = 1;
        }
        memcpy(shkey, &_shkey->key, sizeof(_shkey->key));
    }

    return ECP_OK;
}

static int conn_shkey_set(ECPConnection *conn, unsigned char s_idx, unsigned char c_idx, ecp_aead_key_t *shkey) {
    ECPDHShkey *_shkey;

    if (s_idx & ~ECP_ECDH_IDX_MASK) return ECP_ERR_ECDH_IDX;
    if (c_idx & ~ECP_ECDH_IDX_MASK) return ECP_ERR_ECDH_IDX;

    _shkey = &conn->shkey[s_idx % ECP_MAX_NODE_KEY][c_idx % ECP_MAX_NODE_KEY];
    memcpy(_shkey->key, shkey, sizeof(_shkey->key));
    _shkey->valid = 1;

    return ECP_OK;
}

ECPConnection *ecp_conn_new_inb(ECPSocket *sock, ECPConnection *parent, unsigned char ctype) {
    ECPContext *ctx = sock->ctx;

    if (ctx->conn_new) return ctx->conn_new(sock, parent, ctype);
    return NULL;
}

void ecp_conn_init(ECPConnection *conn, ECPSocket *sock, unsigned char ctype) {
    int i;

    memset(conn, 0, sizeof(ECPConnection));

    conn->sock = sock;
    conn->type = ctype;
    conn->key_curr = ECP_ECDH_IDX_INV;
    conn->key_next = ECP_ECDH_IDX_INV;
    conn->rkey_curr = ECP_ECDH_IDX_INV;
    arc4random_buf(&conn->nonce_out, sizeof(conn->nonce_out));
    conn->access_ts = 0;
    conn->keyx_ts = 0;
}

void ecp_conn_set_flags(ECPConnection *conn, unsigned char flags) {
    conn->flags_im |= flags;
}

void ecp_conn_clr_flags(ECPConnection *conn, unsigned char flags) {
    conn->flags_im &= ~flags;
}

int ecp_conn_test_flags(ECPConnection *conn, unsigned char flags) {
    return !!(conn->flags_im & flags);
}

int _ecp_conn_set_uflags(ECPConnection *conn, unsigned char flags) {
    if (flags & ~ECP_CONN_FLAG_UMASK) return ECP_ERR;
    conn->flags |= flags;
    return ECP_OK;
}

int _ecp_conn_clr_uflags(ECPConnection *conn, unsigned char flags) {
    if (flags & ~ECP_CONN_FLAG_UMASK) return ECP_ERR;
    conn->flags &= ~flags;
    return ECP_OK;
}

int _ecp_conn_test_uflags(ECPConnection *conn, unsigned char flags) {
    return !!(conn->flags & flags);
}

int ecp_conn_set_uflags(ECPConnection *conn, unsigned char flags) {
    int rv;

    pthread_mutex_lock(&conn->mutex);
    rv = _ecp_conn_set_uflags(conn, flags);
    pthread_mutex_unlock(&conn->mutex);

    return rv;
}

int ecp_conn_clr_uflags(ECPConnection *conn, unsigned char flags) {
    int rv;

    pthread_mutex_lock(&conn->mutex);
    rv = _ecp_conn_clr_uflags(conn, flags);
    pthread_mutex_unlock(&conn->mutex);

    return rv;
}

int ecp_conn_test_uflags(ECPConnection *conn, unsigned char flags) {
    int rv;

    pthread_mutex_lock(&conn->mutex);
    rv = _ecp_conn_test_uflags(conn, flags);
    pthread_mutex_unlock(&conn->mutex);

    return rv;
}

int ecp_conn_is_reg(ECPConnection *conn) {
    int rv;

    pthread_mutex_lock(&conn->mutex);
    rv = _ecp_conn_is_reg(conn);
    pthread_mutex_unlock(&conn->mutex);

    return rv;
}

int ecp_conn_is_open(ECPConnection *conn) {
    int rv;

    pthread_mutex_lock(&conn->mutex);
    rv = _ecp_conn_is_open(conn);
    pthread_mutex_unlock(&conn->mutex);

    return rv;
}

void ecp_conn_set_remote_key(ECPConnection *conn, ECPDHPub *key) {
    conn->remote.key_perma = *key;
}

void ecp_conn_set_remote_addr(ECPConnection *conn, ecp_tr_addr_t *addr) {
    conn->remote.addr = *addr;
}

int ecp_conn_create(ECPConnection *conn, ECPConnection *parent) {
    int rv;
#ifdef ECP_WITH_VCONN
    unsigned short pcount;

    pcount = parent ? parent->pcount + 1 : 0;
    if (pcount > ECP_MAX_PARENT) return ECP_ERR_MAX_PARENT;
    if (parent) {
        rv = ecp_conn_refcount_inc(parent);
        if (rv) return rv;
        conn->parent = parent;
        conn->pcount = pcount;
    }
#endif

#ifdef ECP_WITH_PTHREAD
    rv = pthread_mutex_init(&conn->mutex, NULL);
    if (rv) {
#ifdef ECP_WITH_VCONN
        if (conn->parent) ecp_conn_refcount_dec(conn->parent);
#endif

        return ECP_ERR;
    }
#endif

    rv = ecp_ext_conn_create(conn);
    if (rv) {
#ifdef ECP_WITH_VCONN
        if (conn->parent) ecp_conn_refcount_dec(conn->parent);
#endif
#ifdef ECP_WITH_PTHREAD
        pthread_mutex_destroy(&conn->mutex);
#endif

        return ECP_ERR;
    }
    if (!ecp_conn_is_gc(conn)) conn->refcount++;

    return ECP_OK;
}

int ecp_conn_create_inb(ECPConnection *conn, ECPConnection *parent, unsigned char s_idx, unsigned char c_idx, ecp_ecdh_public_t *public, ECPDHPub *rkey_perma, ecp_aead_key_t *shkey) {
    ECPSocket *sock = conn->sock;
    int rv;

    if (ecp_conn_has_vbox(conn) && ((rkey_perma == NULL) || !rkey_perma->valid)) return ECP_ERR_VBOX;

    ecp_conn_set_inb(conn);

    rv = conn_dhkey_set_pub(conn, c_idx, public);
    if (rv) return rv;

    rv = conn_shkey_set(conn, s_idx, c_idx, shkey);
    if (rv) return rv;

    conn->refcount = 1;
    conn->key_curr = s_idx;
    conn->rkey_curr = c_idx;
    if (rkey_perma && rkey_perma->valid) {
        conn->remote.key_perma = *rkey_perma;
    } else {
        ECPDHPub *key_perma = &conn->remote.key_perma;
        memcpy(&key_perma->public, public, sizeof(key_perma->public));
    }

    rv = ecp_conn_create(conn, parent);
    return rv;
}

int ecp_conn_create_outb(ECPConnection *conn, ECPConnection *parent, ECPNode *node) {
    ECPDHKey key;
    int rv;

    ecp_conn_set_outb(conn);

    conn->refcount = 1;
    if (node) conn->remote = *node;

    rv = ecp_dhkey_gen(&key);
    if (rv) return rv;

    conn->key_curr = 0;
    conn->key[conn->key_curr] = key;

    rv = ecp_conn_create(conn, parent);
    return rv;
}

void ecp_conn_destroy(ECPConnection *conn) {
#ifdef ECP_WITH_VCONN
    if (conn->parent) ecp_conn_refcount_dec(conn->parent);
#endif

    ecp_ext_conn_destroy(conn);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_destroy(&conn->mutex);
#endif

    ecp_conn_free(conn);
}

void ecp_conn_free(ECPConnection *conn) {
    ECPContext *ctx = conn->sock->ctx;

    if (ctx->conn_free) ctx->conn_free(conn);
}

int ecp_conn_insert(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;
    int rv;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex);
    pthread_mutex_lock(&conn->mutex);
#endif

    _ecp_conn_set_reg(conn);
    rv = conn_table_insert(conn);
    if (rv) _ecp_conn_clr_reg(conn);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
    pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

    return rv;
}

int ecp_conn_insert_gc(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;
    int rv = ECP_OK;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex_gc);
#endif

    rv = ecp_conn_refcount_inc(conn);
    if (!rv) {
#ifdef ECP_WITH_HTABLE

        rv = ecp_ht_insert(sock->conn_table.keys_gc, &conn->remote.key_perma.public, conn);
        if (rv) ecp_conn_refcount_dec(conn);

#else   /* ECP_WITH_HTABLE */

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&conn->mutex);
#endif

        _ecp_conn_push_gct(conn);

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&conn->mutex);
#endif

#endif  /* ECP_WITH_HTABLE */
    }

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->conn_table.mutex_gc);
#endif

    return rv;
}

int _ecp_conn_remove(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;
    int rv = ECP_OK;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex);
    pthread_mutex_lock(&conn->mutex);
#endif

    if (!_ecp_conn_is_reg(conn)) rv = ECP_ERR_CLOSED;
    if (!rv && _ecp_conn_is_open(conn)) rv = ECP_ERR_BUSY;
    if (!rv) {
        conn_table_remove(conn);
        _ecp_conn_clr_reg(conn);
    }

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
    pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

    return rv;
}

void ecp_conn_remove(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex);
    pthread_mutex_lock(&conn->mutex);
#endif

    if (_ecp_conn_is_reg(conn)) {
        conn_table_remove(conn);
        _ecp_conn_clr_reg(conn);
    }

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
    pthread_mutex_unlock(&sock->conn_table.mutex);
#endif
}

void ecp_conn_remove_addr(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;

    if (!ecp_conn_is_outb(conn)) return;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex);
#endif

    conn_table_remove_addr(conn);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->conn_table.mutex);
#endif
}

void ecp_conn_remove_gc(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;
    ECPConnection *_conn = NULL;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex_gc);
#endif

#ifdef ECP_WITH_HTABLE
    _conn = ecp_ht_remove_kv(sock->conn_table.keys_gc, &conn->remote.key_perma.public, conn);
#else   /* ECP_WITH_HTABLE */

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    if (_ecp_conn_in_gct(conn)) {
        _ecp_conn_pull_gct(conn);
        _conn = conn;
    }

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

#endif  /* ECP_WITH_HTABLE */

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&sock->conn_table.mutex_gc);
#endif

    if (_conn) {
        ecp_conn_refcount_dec(conn);
    }
}

int _ecp_conn_open(ECPConnection *conn, ECPConnection *parent, ECPNode *node, int retry) {
    int rv;
    ssize_t _rv;

    rv = ecp_conn_create_outb(conn, parent, node);
    if (rv) {
        ecp_conn_free(conn);
        return rv;
    }

    rv = ecp_conn_insert(conn);
    if (rv) {
        ecp_conn_destroy(conn);
        return rv;
    }

    _rv = ecp_send_init_req(conn, retry);
    if (_rv < 0) {
        ecp_timer_remove(conn);
        ecp_conn_remove(conn);
        if (!ecp_conn_is_gc(conn)) ecp_conn_refcount_dec(conn);
        rv = _rv;
    }

    ecp_conn_refcount_dec(conn);

    return rv;
}

int ecp_conn_open(ECPConnection *conn, ECPNode *node) {
    int rv;

    rv = _ecp_conn_open(conn, NULL, node, 1);
    return rv;
}

int ecp_conn_try_open(ECPConnection *conn, ECPNode *node) {
    int rv;

    rv = _ecp_conn_open(conn, NULL, node, 0);
    return rv;
}

void _ecp_conn_close(ECPConnection *conn) {
    if (_ecp_conn_is_open(conn)) {
        ecp_close_handler_t handler;

        _ecp_conn_clr_open(conn);
        handler = ecp_get_close_handler(conn);
        if (handler) handler(conn);
        ecp_ext_conn_close(conn);
    }
    ecp_conn_destroy(conn);
}

void ecp_conn_close(ECPConnection *conn) {
    ecp_timer_remove(conn);
    ecp_conn_remove(conn);
    if (ecp_conn_is_gc(conn)) {
        ecp_conn_remove_gc(conn);
    } else {
        ecp_conn_refcount_dec(conn);
    }
}

int _ecp_conn_is_zombie(ECPConnection *conn, ecp_sts_t now, ecp_sts_t to) {
    if (ECP_STS_LT(conn->access_ts, now) && (now - conn->access_ts > to)) return 1;
    return 0;
}

int ecp_conn_is_zombie(ECPConnection *conn, ecp_sts_t now, ecp_sts_t to) {
    int z = 0;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    z = _ecp_conn_is_zombie(conn, now, to);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    return z;
}

void ecp_conn_lock(ECPConnection *conn) {
#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif
}

void ecp_conn_unlock(ECPConnection *conn) {
#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif
}

int ecp_conn_refcount_inc(ECPConnection *conn) {
    int is_reg;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    is_reg = _ecp_conn_is_reg(conn);
    if (is_reg) conn->refcount++;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (!is_reg) return ECP_ERR_CLOSED;
    return ECP_OK;
}

void ecp_conn_refcount_dec(ECPConnection *conn) {
    int destroy = 0;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    conn->refcount--;
    if ((conn->refcount == 0) && !_ecp_conn_is_reg(conn)) destroy = 1;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (destroy) {
        _ecp_conn_close(conn);
    }
}

int ecp_conn_dhkey_new(ECPConnection *conn) {
    ECPSocket *sock = conn->sock;
    ECPDHKey new_key;
    int rv;

    rv = ecp_dhkey_gen(&new_key);
    if (rv) return rv;

#ifdef ECP_WITH_PTHREAD
    if (ecp_conn_is_outb(conn)) pthread_mutex_lock(&sock->conn_table.mutex);
    pthread_mutex_lock(&conn->mutex);
#endif

    rv = conn_dhkey_new(conn, &new_key);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
    if (ecp_conn_is_outb(conn)) pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

    return rv;
}

int ecp_conn_dhkey_get(ECPConnection *conn, unsigned char idx, ECPDHKey *key) {
    ECPDHKey *_key;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    if (idx == ECP_ECDH_IDX_INV) idx = conn->key_curr;
    _key = conn_dhkey_get(conn, idx);
    if (_key) *key = *_key;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (_key == NULL) return ECP_ERR_ECDH_IDX;
    return ECP_OK;
}

void ecp_conn_dhkey_set_curr(ECPConnection *conn) {
#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    conn_dhkey_set_curr(conn);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif
}

int ecp_conn_dhkey_get_pub(ECPConnection *conn, unsigned char *idx, ecp_ecdh_public_t *public) {
    int rv = ECP_OK;
    unsigned char _idx;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    rv = conn_dhkey_get_pub(conn, &_idx, public);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (rv) return rv;

    if (idx) *idx = _idx;
    return ECP_OK;
}

int ecp_conn_dhkey_set_pub(ECPConnection *conn, unsigned char idx, ecp_ecdh_public_t *public) {
    ECPSocket *sock = conn->sock;
    int rv;

#ifdef ECP_WITH_PTHREAD
    if (ecp_conn_is_inb(conn)) pthread_mutex_lock(&sock->conn_table.mutex);
    pthread_mutex_lock(&conn->mutex);
#endif

    rv = conn_dhkey_set_pub(conn, idx, public);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
    if (ecp_conn_is_inb(conn)) pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

    if (rv == ECP_ERR_DUP) rv = ECP_OK;

    return rv;
}

int ecp_conn_dhkey_get_remote(ECPConnection *conn, unsigned char idx, ECPDHPub *key) {
    ECPDHPub *_key;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    if (idx == ECP_ECDH_IDX_INV) idx = conn->rkey_curr;
    _key = conn_dhkey_get_remote(conn, idx);
    if (_key) *key = *_key;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (_key == NULL) return ECP_ERR_ECDH_IDX;
    return ECP_OK;
}

void ecp_conn_handler_init(ECPConnHandler *handler, ecp_msg_handler_t handle_msg, ecp_open_handler_t handle_open, ecp_close_handler_t handle_close, ecp_send_open_t send_open) {
    memset(handler, 0, sizeof(ECPConnHandler));
    handler->handle_msg = handle_msg;
    handler->handle_open = handle_open;
    handler->handle_close = handle_close;
    handler->send_open = send_open;
}

ecp_msg_handler_t ecp_get_msg_handler(ECPConnection *conn) {
    ECPContext *ctx = conn->sock->ctx;
    unsigned char ctype;

    ctype = conn->type & ECP_CTYPE_MASK;
    if (ecp_conn_is_sys(conn)) {
        if (ctype >= ECP_MAX_CTYPE_SYS) return NULL;
        return ctx->handler_sys[ctype] ? ctx->handler_sys[ctype]->handle_msg : NULL;
    } else {
        if (ctype >= ECP_MAX_CTYPE) return NULL;
        return ctx->handler[ctype] ? ctx->handler[ctype]->handle_msg : NULL;
    }
}

ecp_open_handler_t ecp_get_open_handler(ECPConnection *conn) {
    ECPContext *ctx = conn->sock->ctx;
    unsigned char ctype;

    ctype = conn->type & ECP_CTYPE_MASK;
    if (ecp_conn_is_sys(conn)) {
        if (ctype >= ECP_MAX_CTYPE_SYS) return NULL;
        return ctx->handler_sys[ctype] ? ctx->handler_sys[ctype]->handle_open : NULL;
    } else {
        if (ctype >= ECP_MAX_CTYPE) return NULL;
        return ctx->handler[ctype] ? ctx->handler[ctype]->handle_open : NULL;
    }
}

ecp_close_handler_t ecp_get_close_handler(ECPConnection *conn) {
    ECPContext *ctx = conn->sock->ctx;
    unsigned char ctype;

    ctype = conn->type & ECP_CTYPE_MASK;
    if (ecp_conn_is_sys(conn)) {
        if (ctype >= ECP_MAX_CTYPE_SYS) return NULL;
        return ctx->handler_sys[ctype] ? ctx->handler_sys[ctype]->handle_close : NULL;
    } else {
        if (ctype >= ECP_MAX_CTYPE) return NULL;
        return ctx->handler[ctype] ? ctx->handler[ctype]->handle_close : NULL;
    }
}

ecp_send_open_t ecp_get_send_open_f(ECPConnection *conn) {
    ECPContext *ctx = conn->sock->ctx;
    unsigned char ctype;

    ctype = conn->type & ECP_CTYPE_MASK;
    if (ecp_conn_is_sys(conn)) {
        if (ctype >= ECP_MAX_CTYPE_SYS) return NULL;
        return ctx->handler_sys[ctype] ? ctx->handler_sys[ctype]->send_open : NULL;
    } else {
        if (ctype >= ECP_MAX_CTYPE) return NULL;
        return ctx->handler[ctype] ? ctx->handler[ctype]->send_open : NULL;
    }
}

void ecp_err_handle(ECPConnection *conn, unsigned char mtype, int err) {
    ECPContext *ctx = conn->sock->ctx;
    int rv;

    rv = ecp_ext_err_handle(conn, mtype, err);
    if (rv != ECP_PASS) return;

    if (err == ECP_ERR_CLOSED) return;
    if (ctx->handle_err) ctx->handle_err(conn, mtype, err);
}

static ssize_t _send_ireq(ECPConnection *conn, ECPTimerItem *ti) {
    ECPBuffer packet;
    ECPBuffer payload;
    unsigned char pkt_buf[ECP_SIZE_PKT_BUF_IREQ(ECP_SIZE_ZPAD_PLD, ECP_MTYPE_INIT_REQ, conn)];
    unsigned char pld_buf[ECP_SIZE_PLD_BUF_IREQ(ECP_SIZE_ZPAD_PLD, ECP_MTYPE_INIT_REQ, conn)];
    unsigned char *msg;
    ssize_t rv;

    packet.buffer = pkt_buf;
    packet.size = sizeof(pkt_buf);
    payload.buffer = pld_buf;
    payload.size = sizeof(pld_buf);

    ecp_pld_set_type(payload.buffer, payload.size, ECP_MTYPE_INIT_REQ);
    msg = ecp_pld_get_msg(payload.buffer, payload.size);
    memset(msg, 0, ECP_SIZE_ZPAD_PLD);

    rv = _ecp_pld_send(conn, &packet, ECP_ECDH_IDX_PERMA, ECP_ECDH_IDX_INV, ECP_SIZE_ZPAD_HDR, NULL, NULL, &payload, ECP_SIZE_PLD(ECP_SIZE_ZPAD_PLD, ECP_MTYPE_INIT_REQ), 0, ti);
    return rv;
}

static ssize_t _retry_ireq(ECPConnection *conn, ECPTimerItem *ti) {
    ECPSocket *sock = conn->sock;
    ECPDHKey key;
    int is_open, refcount_ok;
    int rv;

    rv = ecp_dhkey_gen(&key);
    if (rv) return rv;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex);
    pthread_mutex_lock(&conn->mutex);
#endif

    rv = ECP_OK;
    refcount_ok = 0;

    is_open = _ecp_conn_is_open(conn);
    if (!is_open) {
        /* caller (timer) holds one reference */
        refcount_ok = 1;
        if (!ecp_conn_is_gc(conn)) refcount_ok++;

        refcount_ok = (conn->refcount == refcount_ok);
        if (refcount_ok) {
            rv = conn_dhkey_set(conn, conn->key_curr, &key);
        }
    }

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
    pthread_mutex_unlock(&sock->conn_table.mutex);
#endif
    if (rv) return rv;

    if (is_open) {
        /* already opened, that's OK */
        return 0;
    }

    if (refcount_ok) {
        return _send_ireq(conn, ti);
    } else {
        /* check later */
        rv = ecp_timer_push(ti);
        return rv;
    }
}

ssize_t ecp_send_init_req(ECPConnection *conn, int retry) {
    ssize_t rv;

    if (retry) {
        ECPTimerItem ti;

        ecp_timer_item_init(&ti, conn, ECP_MTYPE_OPEN_REP, _retry_ireq, ECP_SEND_TRIES-1, ECP_SEND_TIMEOUT);

        rv = _send_ireq(conn, &ti);
    } else {
        rv = _send_ireq(conn, NULL);
    }

    return rv;
}

ssize_t ecp_handle_init_req(ECPSocket *sock, ECPConnection *parent, ecp_tr_addr_t *addr, unsigned char c_idx, unsigned char *public_buf, unsigned char *msg, size_t msg_size, ecp_aead_key_t *shkey, ECP2Buffer *bufs) {
    unsigned char _public_buf[ECP_SIZE_ECDH_PUB];
    ssize_t rv;
    int i;

    if (msg_size < ECP_SIZE_ZPAD_PLD) return ECP_ERR_SIZE;
    for (i=0; i<ECP_SIZE_ZPAD_PLD; i++) {
        if (msg[i] != 0) return ECP_ERR;
    }

    memcpy(_public_buf, public_buf, sizeof(_public_buf));
    ecp_tr_release(bufs->packet, 1);
    rv = ecp_send_init_rep(sock, parent, addr, c_idx, _public_buf, shkey);
    if (rv < 0) return rv;

    return ECP_SIZE_ZPAD_PLD;
}

ssize_t ecp_send_init_rep(ECPSocket *sock, ECPConnection *parent, ecp_tr_addr_t *addr, unsigned char c_idx, unsigned char *public_buf, ecp_aead_key_t *shkey) {
    ECPBuffer packet;
    ECPBuffer payload;
    ECPPktMeta pkt_meta;
    unsigned char pkt_buf[ECP_SIZE_PKT_BUF_IREP(1+ECP_SIZE_ECDH_PUB+ECP_SIZE_ATAG, ECP_MTYPE_INIT_REP, parent)];
    unsigned char pld_buf[ECP_SIZE_PLD_BUF_IREP(1+ECP_SIZE_ECDH_PUB+ECP_SIZE_ATAG, ECP_MTYPE_INIT_REP, parent)];
    unsigned char atag[ECP_SIZE_ATAG];
    unsigned char *msg;
    ecp_nonce_t nonce;
    ssize_t rv;
    int _rv;

    packet.buffer = pkt_buf;
    packet.size = sizeof(pkt_buf);
    payload.buffer = pld_buf;
    payload.size = sizeof(pld_buf);

    ecp_pld_set_type(payload.buffer, payload.size, ECP_MTYPE_INIT_REP);
    msg = ecp_pld_get_msg(payload.buffer, payload.size);

    ecp_atag_gen(sock, public_buf, atag, &nonce);

    _rv = ecp_sock_dhkey_get_pub(sock, msg, (ecp_ecdh_public_t *)(msg+1));
    if (_rv) return _rv;
    memcpy(msg+1+ECP_SIZE_ECDH_PUB, atag, ECP_SIZE_ATAG);

    pkt_meta.zpad = 0;
    pkt_meta.cookie = NULL;
    pkt_meta.shkey = shkey;
    pkt_meta.nonce = &nonce;
    pkt_meta.ntype = ECP_NTYPE_INB;
    pkt_meta.public = (ecp_ecdh_public_t *)public_buf;
    pkt_meta.s_idx = ECP_ECDH_IDX_PERMA;
    pkt_meta.c_idx = c_idx;

    rv = ecp_pld_send_irep(sock, parent, addr, &packet, &pkt_meta, &payload, ECP_SIZE_PLD(1+ECP_SIZE_ECDH_PUB+ECP_SIZE_ATAG, ECP_MTYPE_INIT_REP), 0);
    return rv;
}

ssize_t ecp_handle_init_rep(ECPConnection *conn, unsigned char *msg, size_t msg_size, unsigned char *nonce_buf, ECP2Buffer *bufs) {
    ecp_send_open_t send_open_f;
    unsigned char cookie[ECP_SIZE_COOKIE];
    unsigned char *atag;
    ssize_t rv;
    int _rv;

    if (msg_size < 1+ECP_SIZE_ECDH_PUB+ECP_SIZE_ATAG) return ECP_ERR_SIZE;

    _rv = ecp_conn_dhkey_set_pub(conn, msg[0], (ecp_ecdh_public_t *)(msg+1));
    if (_rv) return _rv;

    atag = msg+1+ECP_SIZE_ECDH_PUB;
    send_open_f = ecp_get_send_open_f(conn);
    if (send_open_f == NULL) send_open_f = ecp_send_open_req;
    memcpy(cookie, nonce_buf, ECP_SIZE_NONCE);
    memcpy(cookie+ECP_SIZE_NONCE, atag, ECP_SIZE_ATAG);

    ecp_tr_release(bufs->packet, 1);
    rv = send_open_f(conn, cookie);
    if (rv < 0) return rv;

    return 1+ECP_SIZE_ECDH_PUB+ECP_SIZE_ATAG;
}

ssize_t ecp_write_open_req(ECPConnection *conn, ECPBuffer *payload) {
    unsigned char *msg;
    unsigned char vbox;
    ssize_t rv;
    int _rv;

    if (payload->size < ECP_SIZE_PLD(2, ECP_MTYPE_OPEN_REQ)) return ECP_ERR_SIZE;

    ecp_pld_set_type(payload->buffer, payload->size, ECP_MTYPE_OPEN_REQ);
    msg = ecp_pld_get_msg(payload->buffer, payload->size);
    *msg = conn->type;
    msg++;

    vbox = ecp_conn_has_vbox(conn);
    *msg = vbox;
    msg++;

    rv = 0;
    if (vbox) {
        ECPSocket *sock = conn->sock;
        ECPDHKey key_perma;
        ECPDHPub rkey_perma;
        ECPDHPub rkey_curr;
        ecp_ecdh_public_t public;
        ecp_aead_key_t vbox_shkey;
        ecp_nonce_t vbox_nonce;
        unsigned char vbox_buf[2*ECP_SIZE_ECDH_PUB];

        if (payload->size < ECP_SIZE_PLD(2+ECP_SIZE_VBOX, ECP_MTYPE_OPEN_REQ)) return ECP_ERR_SIZE;

        _rv = ecp_sock_dhkey_get(sock, ECP_ECDH_IDX_PERMA, &key_perma);
        if (!_rv) _rv = ecp_conn_dhkey_get_remote(conn, ECP_ECDH_IDX_PERMA, &rkey_perma);
        if (!_rv) _rv = ecp_conn_dhkey_get_remote(conn, ECP_ECDH_IDX_INV, &rkey_curr);
        if (!_rv) _rv = ecp_conn_dhkey_get_pub(conn, NULL, &public);
        if (_rv) return _rv;

        memcpy(vbox_buf, &public, ECP_SIZE_ECDH_PUB);
        memcpy(vbox_buf+ECP_SIZE_ECDH_PUB, &rkey_curr.public, ECP_SIZE_ECDH_PUB);
        memcpy(msg, &key_perma.public, ECP_SIZE_ECDH_PUB);
        msg += ECP_SIZE_ECDH_PUB;

        arc4random_buf(&vbox_nonce, sizeof(vbox_nonce));
        ecp_nonce2buf(msg, &vbox_nonce);
        msg += ECP_SIZE_NONCE;

        ecp_ecdh_shkey(&vbox_shkey, &rkey_perma.public, &key_perma.private);
        rv = ecp_aead_enc(msg, payload->size - (msg - payload->buffer), vbox_buf, sizeof(vbox_buf), &vbox_shkey, &vbox_nonce, ECP_NTYPE_VBOX);
        if (rv < 0) return rv;
        rv += ECP_SIZE_ECDH_PUB + ECP_SIZE_NONCE;
    }

    return ECP_SIZE_PLD(2+rv, ECP_MTYPE_OPEN_REQ);
}

ssize_t ecp_send_open_req(ECPConnection *conn, unsigned char *cookie) {
    ECPBuffer packet;
    ECPBuffer payload;
    unsigned char pkt_buf[ECP_SIZE_PKT_BUF_OREQ(2+ECP_SIZE_VBOX, ECP_MTYPE_OPEN_REQ, conn)];
    unsigned char pld_buf[ECP_SIZE_PLD_BUF_OREQ(2+ECP_SIZE_VBOX, ECP_MTYPE_OPEN_REQ, conn)];
    ssize_t rv;

    packet.buffer = pkt_buf;
    packet.size = sizeof(pkt_buf);
    payload.buffer = pld_buf;
    payload.size = sizeof(pld_buf);

    rv = ecp_write_open_req(conn, &payload);
    if (rv < 0) return rv;

    rv = ecp_pld_send_wcookie(conn, &packet, &payload, rv, 0, cookie);
    return rv;
}

ssize_t ecp_handle_open_req(ECPSocket *sock, ECPConnection *parent, unsigned char s_idx, unsigned char c_idx, unsigned char *public_buf, unsigned char *msg, size_t msg_size, ecp_aead_key_t *shkey, ECPConnection **_conn) {
    ECPConnection *conn;
    ECPDHPub rkey_perma;
    unsigned char ctype;
    unsigned char vbox;
    ssize_t rv;
    int _rv;

    if (msg_size < 2) return ECP_ERR_SIZE;
    ctype = *msg;
    msg++;

    vbox = *msg;
    msg++;

    msg_size -= 2;
    rkey_perma.valid = 0;
    if (vbox) {
        ECPDHKey key_perma;
        ECPDHKey key_curr;
        ecp_aead_key_t vbox_shkey;
        ecp_nonce_t vbox_nonce;
        unsigned char vbox_buf[2*ECP_SIZE_ECDH_PUB];

        if (msg_size < ECP_SIZE_VBOX) return ECP_ERR_SIZE;

        _rv = ecp_sock_dhkey_get(sock, ECP_ECDH_IDX_PERMA, &key_perma);
        if (!_rv) _rv = ecp_sock_dhkey_get(sock, s_idx, &key_curr);
        if (_rv) return _rv;

        memcpy(&rkey_perma.public, msg, ECP_SIZE_ECDH_PUB);
        msg+= ECP_SIZE_ECDH_PUB;
        msg_size -= ECP_SIZE_ECDH_PUB;

        ecp_buf2nonce(&vbox_nonce, msg);
        msg+= ECP_SIZE_NONCE;
        msg_size -= ECP_SIZE_NONCE;

        ecp_ecdh_shkey(&vbox_shkey, &rkey_perma.public, &key_perma.private);
        rv = ecp_aead_dec(vbox_buf, sizeof(vbox_buf), msg, msg_size, &vbox_shkey, &vbox_nonce, ECP_NTYPE_VBOX);
        if (rv < 0) return ECP_ERR_VBOX;

        if (memcmp(vbox_buf, public_buf, ECP_SIZE_ECDH_PUB) != 0) return ECP_ERR_VBOX;
        if (memcmp(vbox_buf+ECP_SIZE_ECDH_PUB, &key_curr.public, ECP_SIZE_ECDH_PUB) != 0) return ECP_ERR_VBOX;
        rkey_perma.valid = 1;
    }

    if (sock->ctx->key_checker) {
        _rv = sock->ctx->key_checker(sock, parent, ctype, rkey_perma.valid ? &rkey_perma.public : NULL);
        if (!_rv) return ECP_ERR_VBOX;
    }

    conn = ecp_conn_new_inb(sock, parent, ctype);
    if (conn == NULL) return ECP_ERR_ALLOC;

    _rv = ecp_conn_create_inb(conn, parent, s_idx, c_idx, (ecp_ecdh_public_t *)public_buf, rkey_perma.valid ? &rkey_perma : NULL, shkey);
    if (_rv) {
        ecp_conn_free(conn);
        return _rv;
    }

    _rv = ecp_conn_insert(conn);
    if (_rv) {
        ecp_conn_destroy(conn);
        return _rv;
    }
    *_conn = conn;

    /* handle_open will be called from ecp_unpack */
    return 2+(vbox ? ECP_SIZE_VBOX : 0);
}

ssize_t ecp_send_open_rep(ECPConnection *conn) {
    ECPBuffer packet;
    ECPBuffer payload;
    unsigned char pkt_buf[ECP_SIZE_PKT_BUF(0, ECP_MTYPE_OPEN_REP, conn)];
    unsigned char pld_buf[ECP_SIZE_PLD_BUF(0, ECP_MTYPE_OPEN_REP, conn)];
    ssize_t rv;

    packet.buffer = pkt_buf;
    packet.size = sizeof(pkt_buf);
    payload.buffer = pld_buf;
    payload.size = sizeof(pld_buf);

    ecp_pld_set_type(payload.buffer, payload.size, ECP_MTYPE_OPEN_REP);
    rv = ecp_pld_send(conn, &packet, &payload, ECP_SIZE_PLD(0, ECP_MTYPE_OPEN_REP), 0);
    return rv;
}

ssize_t ecp_handle_open(ECPConnection *conn, unsigned char mtype, unsigned char *msg, size_t msg_size, ECP2Buffer *bufs) {
    ecp_open_handler_t handler;
    int is_reg, is_open, is_gc;
    int _rv = ECP_OK;
    ssize_t rv;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    is_reg = _ecp_conn_is_reg(conn);
    is_open = _ecp_conn_is_open(conn);
    if (!is_reg) _rv = ECP_ERR_CLOSED;
    if (!_rv && is_open) _rv = ECP_ERR;

    /* set open if registered, avoids race condition with timeout handler */
    if (!_rv) _ecp_conn_set_open(conn);

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (_rv) return _rv;

    if (mtype == ECP_MTYPE_OPEN_REQ) {
        if (ecp_conn_is_outb(conn)) {
            _rv = ECP_ERR;
            goto handle_open_fin;
        }

        rv = 2;
        if (msg_size < rv) {
            _rv = ECP_ERR_SIZE;
            goto handle_open_fin;
        }

        if (msg[1]) rv += ECP_SIZE_VBOX;
        if (msg_size < rv) {
            _rv = ECP_ERR_SIZE;
            goto handle_open_fin;
        }
    } else {
        if (ecp_conn_is_inb(conn)) {
            _rv = ECP_ERR;
            goto handle_open_fin;
        }
        rv = 0;
    }

    is_gc = ecp_conn_is_gc(conn);
    if (is_gc) {
        _rv = ecp_conn_insert_gc(conn);
        if (_rv) goto handle_open_fin;
    }

    _rv = ecp_ext_conn_open(conn);
    if (_rv) {
        if (is_gc) ecp_conn_remove_gc(conn);
        goto handle_open_fin;
    }

    handler = ecp_get_open_handler(conn);
    if (handler) {
        _rv = handler(conn, bufs);
        if (_rv) {
            if (is_gc) ecp_conn_remove_gc(conn);
            ecp_ext_conn_close(conn);
            goto handle_open_fin;
        }
    }

    if (ecp_conn_is_inb(conn)) {
        ecp_tr_release(bufs->packet, 1);
        ecp_send_open_rep(conn);
    } else if (ecp_conn_is_root(conn)) {
        ecp_conn_remove_addr(conn);
    }

handle_open_fin:
    if (_rv) {
#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&conn->mutex);
#endif

        _ecp_conn_clr_open(conn);

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&conn->mutex);
#endif

        return _rv;
    }

    return rv;
}

static ssize_t _send_kxreq(ECPConnection *conn, ECPTimerItem *ti) {
    ECPBuffer packet;
    ECPBuffer payload;
    unsigned char pkt_buf[ECP_SIZE_PKT_BUF(1+ECP_SIZE_ECDH_PUB, ECP_MTYPE_KEYX_REQ, conn)];
    unsigned char pld_buf[ECP_SIZE_PLD_BUF(1+ECP_SIZE_ECDH_PUB, ECP_MTYPE_KEYX_REQ, conn)];
    unsigned char *msg;
    ssize_t rv;
    int _rv;

    packet.buffer = pkt_buf;
    packet.size = sizeof(pkt_buf);
    payload.buffer = pld_buf;
    payload.size = sizeof(pld_buf);

    ecp_pld_set_type(payload.buffer, payload.size, ECP_MTYPE_KEYX_REQ);
    msg = ecp_pld_get_msg(payload.buffer, payload.size);

    _rv = ecp_conn_dhkey_get_pub(conn, msg, (ecp_ecdh_public_t *)(msg+1));
    if (_rv) return _rv;

    rv = ecp_pld_send_wtimer(conn, &packet, &payload, ECP_SIZE_PLD(1+ECP_SIZE_ECDH_PUB, ECP_MTYPE_KEYX_REQ), 0, ti);
    return rv;
}

ssize_t ecp_send_keyx_req(ECPConnection *conn, int retry) {
    ssize_t rv;

    if (retry) {
        rv = ecp_timer_send(conn, _send_kxreq, ECP_MTYPE_KEYX_REP, ECP_SEND_TRIES, ECP_SEND_TIMEOUT);
    } else {
        rv = _send_kxreq(conn, NULL);
    }

    return rv;
}

ssize_t ecp_send_keyx_rep(ECPConnection *conn) {
    ECPBuffer packet;
    ECPBuffer payload;
    unsigned char pkt_buf[ECP_SIZE_PKT_BUF(1+ECP_SIZE_ECDH_PUB, ECP_MTYPE_KEYX_REP, conn)];
    unsigned char pld_buf[ECP_SIZE_PLD_BUF(1+ECP_SIZE_ECDH_PUB, ECP_MTYPE_KEYX_REP, conn)];
    unsigned char *msg;
    ssize_t rv;
    int _rv;

    packet.buffer = pkt_buf;
    packet.size = sizeof(pkt_buf);
    payload.buffer = pld_buf;
    payload.size = sizeof(pld_buf);

    ecp_pld_set_type(payload.buffer, payload.size, ECP_MTYPE_KEYX_REP);
    msg = ecp_pld_get_msg(payload.buffer, payload.size);

    _rv = ecp_conn_dhkey_get_pub(conn, msg, (ecp_ecdh_public_t *)(msg+1));
    if (_rv) return _rv;

    rv = ecp_pld_send(conn, &packet, &payload, ECP_SIZE_PLD(1+ECP_SIZE_ECDH_PUB, ECP_MTYPE_KEYX_REP), 0);
    return rv;
}

ssize_t ecp_handle_keyx(ECPConnection *conn, unsigned char mtype, unsigned char *msg, size_t msg_size, ECP2Buffer *bufs) {
    ssize_t rv;
    int _rv;

    if (msg_size < 1+ECP_SIZE_ECDH_PUB) return ECP_ERR_SIZE;

    _rv = ecp_conn_dhkey_set_pub(conn, msg[0], (ecp_ecdh_public_t *)(msg+1));
    if (_rv) return _rv;

    if (mtype == ECP_MTYPE_KEYX_REP) {
        if (ecp_conn_is_inb(conn)) return ECP_ERR;

        ecp_conn_dhkey_set_curr(conn);
    } else {
        if (ecp_conn_is_outb(conn)) return ECP_ERR;

        ecp_tr_release(bufs->packet, 1);
        rv = ecp_send_keyx_rep(conn);
        if (rv < 0) return rv;
    }

    return 1+ECP_SIZE_ECDH_PUB;
}

ssize_t ecp_msg_handle(ECPConnection *conn, ecp_seq_t seq, unsigned char mtype, unsigned char *msg, size_t msg_size, ECP2Buffer *bufs) {
    if (mtype & ECP_MTYPE_FLAG_SYS) {
        switch (mtype) {
            case ECP_MTYPE_OPEN_REQ:
            case ECP_MTYPE_OPEN_REP:
                return ecp_handle_open(conn, mtype, msg, msg_size, bufs);

            case ECP_MTYPE_KEYX_REQ:
            case ECP_MTYPE_KEYX_REP:
                return ecp_handle_keyx(conn, mtype, msg, msg_size, bufs);

            default:
                return ecp_ext_msg_handle(conn, seq, mtype, msg, msg_size, bufs);
        }
    } else {
        ecp_msg_handler_t handler;

        handler = ecp_get_msg_handler(conn);
        if (handler) {
            return handler(conn, seq, mtype, msg, msg_size, bufs);
        } else {
            return ECP_ERR_HANDLER;
        }
    }
}

ssize_t ecp_pld_handle_one(ECPConnection *conn, ecp_seq_t seq, unsigned char *payload, size_t pld_size, ECP2Buffer *bufs) {
    unsigned char mtype;
    unsigned char *msg;
    size_t hdr_size, msg_size;
    ssize_t rv;
    int _rv;

    _rv = ecp_pld_get_type(payload, pld_size, &mtype);
    if (_rv) return _rv;

    msg = ecp_pld_get_msg(payload, pld_size);
    if (msg == NULL) return ECP_ERR;
    hdr_size = msg - payload;
    msg_size = pld_size - hdr_size;

    rv = ecp_msg_handle(conn, seq, mtype, msg, msg_size, bufs);
    ecp_timer_pop(conn, mtype);
    if (rv < 0) {
        ecp_err_handle(conn, mtype, rv);
        return rv;
    }

    rv += hdr_size;

    if (rv > pld_size) return ECP_ERR_SIZE;
    return rv;
}

ssize_t ecp_pld_handle(ECPConnection *conn, ecp_seq_t seq, unsigned char *payload, size_t _pld_size, ECP2Buffer *bufs) {
    size_t pld_size = _pld_size;
    ssize_t rv;

    rv = ecp_ext_pld_handle_one(conn, seq, payload, pld_size, bufs);
    if (rv < 0) return rv;

    payload += rv;
    pld_size -= rv;

    while (pld_size) {
        rv = ecp_pld_handle_one(conn, seq, payload, pld_size, bufs);
        if (rv < 0) return rv;

        payload += rv;
        pld_size -= rv;
    }

    return _pld_size;
}

ssize_t ecp_unpack(ECPSocket *sock, ECPConnection *parent, ecp_tr_addr_t *addr, ECP2Buffer *bufs, size_t _pkt_size, ECPConnection **_conn, unsigned char **_payload, ecp_seq_t *_seq) {
    ECPConnection *conn = NULL;
    unsigned char idx;
    unsigned char s_idx;
    unsigned char c_idx;
    unsigned char *payload;
    unsigned char *packet;
    ssize_t pld_size;
    size_t pkt_size = _pkt_size;
    ecp_aead_key_t shkey;
    unsigned char *public_buf;
    unsigned char *nonce_buf;
    unsigned char is_inb = 0;
    unsigned char is_open = 0;
    unsigned char is_open_msg = 0;
    ecp_nonce_t nonce_pkt, nonce_conn, nonce_in;
    ecp_ack_t nonce_map;
    ssize_t rv;
    int _rv = ECP_OK;

    if (pkt_size < ECP_MIN_PKT) return ECP_ERR_SIZE;

    *_conn = NULL;
    *_payload = NULL;
    *_seq = 0;

    packet = bufs->packet->buffer;
    idx = packet[ECP_SIZE_PROTO];
    s_idx = (idx & 0xF0) >> 4;
    c_idx = (idx & 0x0F);
    if ((s_idx == ECP_ECDH_IDX_PERMA) && (c_idx == ECP_ECDH_IDX_NOKEY)) {
        public_buf = NULL;
        c_idx = 0;
    } else {
        public_buf = packet+ECP_SIZE_PROTO+1;
    }

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&sock->conn_table.mutex);
#endif

    conn = conn_table_search(sock, c_idx, (ecp_ecdh_public_t *)public_buf, addr, parent);

#ifdef ECP_WITH_PTHREAD
    if (conn) pthread_mutex_lock(&conn->mutex);
    pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

    if (conn) {
        is_inb = ecp_conn_is_inb(conn);
        is_open = _ecp_conn_is_open(conn);

        if (public_buf) {
            nonce_buf = public_buf+ECP_SIZE_ECDH_PUB;
            packet += ECP_SIZE_PKT_HDR;
            pkt_size -= ECP_SIZE_PKT_HDR;

            if (is_open) {
                nonce_conn = conn->nonce_in;
                nonce_map = conn->nonce_map;
            }
        } else if (!is_open && !is_inb) {
            /* init reply message */
            nonce_buf = packet+ECP_SIZE_PROTO+1;
            packet += ECP_SIZE_PKT_HDR_IREP;
            pkt_size -= ECP_SIZE_PKT_HDR_IREP;
        } else {
            _rv = ECP_ERR;
        }
        if (!_rv) _rv = conn_shkey_get(conn, s_idx, c_idx, &shkey);
        if (!_rv) conn->refcount++;

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&conn->mutex);
#endif

        if (_rv) return _rv;
    } else if (public_buf) {
        ECPDHKey key;

        is_inb = 1;

        if (s_idx == ECP_ECDH_IDX_PERMA) {
            /* init request message */
            unsigned char *zpad_buf;
            int i;

            if (pkt_size < (ECP_MIN_PKT+ECP_SIZE_ZPAD_HDR+ECP_SIZE_ZPAD_PLD)) return ECP_ERR_SIZE;

            zpad_buf = public_buf+ECP_SIZE_ECDH_PUB;
            nonce_buf = zpad_buf+ECP_SIZE_ZPAD_HDR;
            packet += ECP_SIZE_PKT_HDR_IREQ;
            pkt_size -= ECP_SIZE_PKT_HDR_IREQ;

            for (i=0; i<ECP_SIZE_ZPAD_HDR; i++) {
                if (zpad_buf[i] != 0) return ECP_ERR;
            }
            _rv = ecp_sock_dhkey_get(sock, s_idx, &key);
            if (_rv) return _rv;

            ecp_ecdh_shkey(&shkey, (ecp_ecdh_public_t *)public_buf, &key.private);
        } else if (pkt_size >= (ECP_MIN_PKT+ECP_SIZE_COOKIE)) {
            /* open request message */
            unsigned char *cookie_buf;

            cookie_buf = public_buf+ECP_SIZE_ECDH_PUB;
            nonce_buf = cookie_buf+ECP_SIZE_COOKIE;
            packet += ECP_SIZE_PKT_HDR_OREQ;
            pkt_size -= ECP_SIZE_PKT_HDR_OREQ;

            _rv = ecp_cookie_verify(sock, cookie_buf, public_buf);
            if (_rv) return _rv;

            _rv = ecp_sock_dhkey_get(sock, s_idx, &key);
            if (_rv) return _rv;

            ecp_ecdh_shkey(&shkey, (ecp_ecdh_public_t *)public_buf, &key.private);
        } else {
            return ECP_ERR;
        }
    } else {
        return ECP_ERR;
    }

    ecp_buf2nonce(&nonce_pkt, nonce_buf);
    if (is_open) {
        if (ECP_NONCE_LTE(nonce_pkt, nonce_conn)) {
            ecp_nonce_t nonce_offset = nonce_conn - nonce_pkt;

            if (nonce_offset < ECP_SIZE_ACKB) {
                ecp_ack_t nonce_mask = ((ecp_ack_t)1 << nonce_offset);

                if (nonce_mask & nonce_map) _rv = ECP_ERR_SEQ;
                if (!_rv) nonce_in = nonce_conn;
            } else {
                _rv = ECP_ERR_SEQ;
            }
        } else {
            ecp_nonce_t nonce_offset = nonce_pkt - nonce_conn;

            if (nonce_offset < ECP_MAX_SEQ_FWD) {
                if (nonce_offset < ECP_SIZE_ACKB) {
                    nonce_map = nonce_map << nonce_offset;
                } else {
                    nonce_map = 0;
                }
                nonce_map |= 1;
                nonce_in = nonce_pkt;
            } else {
                _rv = ECP_ERR_SEQ;
            }
        }
        if (_rv) {
            rv = _rv;
            goto unpack_fin;
        }
    }

    payload = bufs->payload->buffer;
    rv = ecp_aead_dec(payload, bufs->payload->size, packet, pkt_size, &shkey, &nonce_pkt, is_inb ? ECP_NTYPE_INB : ECP_NTYPE_OUTB);

    /* init reply collision */
    if ((rv == ECP_ERR_DECRYPT) && (public_buf == NULL) && conn) {
        ECPConnection *_conn = conn;

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&sock->conn_table.mutex);
#endif

        conn = conn_table_search_addr(sock, addr);

        while (conn && (rv == ECP_ERR_DECRYPT)) {
            if (conn != _conn) {
#ifdef ECP_WITH_PTHREAD
                pthread_mutex_lock(&conn->mutex);
#endif

                _rv = conn_shkey_get(conn, s_idx, c_idx, &shkey);

#ifdef ECP_WITH_PTHREAD
                pthread_mutex_unlock(&conn->mutex);
#endif

                if (_rv) rv = _rv;
                if (rv == ECP_ERR_DECRYPT) rv = ecp_aead_dec(payload, bufs->payload->size, packet, pkt_size, &shkey, &nonce_pkt, ECP_NTYPE_OUTB);
            }
            if (rv == ECP_ERR_DECRYPT) conn = conn_table_search_addr_next(sock, addr, conn);
        }

        if (rv < 0) conn = NULL;
        if (conn) {
            _rv = ecp_conn_refcount_inc(conn);
            if (_rv) {
                rv = _rv;
                conn = NULL;
            }
        }

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&sock->conn_table.mutex);
#endif

        ecp_conn_refcount_dec(_conn);
    }
    if (rv < 0) goto unpack_fin;

    pld_size = rv;
    if (pld_size < ECP_MIN_PLD) {
        rv = ECP_ERR_SIZE;
        goto unpack_fin;
    }

    if (!(conn && is_open)) {
        unsigned char mtype;
        unsigned char *msg;
        size_t hdr_size, msg_size;

        _rv = ecp_pld_get_type(payload, pld_size, &mtype);
        if (_rv) {
            rv = _rv;
            goto unpack_fin;
        }

        msg = ecp_pld_get_msg(payload, pld_size);
        if (msg == NULL) {
            rv = ECP_ERR;
            goto unpack_fin;
        }
        hdr_size = msg - payload;
        msg_size = pld_size - hdr_size;

        if (conn == NULL) {
            switch (mtype) {
                case ECP_MTYPE_INIT_REQ: {
                    rv = ecp_handle_init_req(sock, parent, addr, c_idx, public_buf, msg, msg_size, &shkey, bufs);
                    if (rv < 0) goto unpack_fin;

                    payload += (hdr_size + rv);
                    pld_size -= (hdr_size + rv);
                    break;
                }

                case ECP_MTYPE_OPEN_REQ: {
                    rv = ecp_handle_open_req(sock, parent, s_idx, c_idx, public_buf, msg, msg_size, &shkey, &conn);
                    if (rv < 0) goto unpack_fin;

                    /* pass to open handler */
                    nonce_in = nonce_pkt;
                    nonce_map = ECP_ACK_FULL;
                    is_open_msg = 1;
                    break;
                }

                default:
                    rv = ECP_ERR_MTYPE;
                    goto unpack_fin;
            }
        } else if (!is_open) {
            switch (mtype) {
                case ECP_MTYPE_INIT_REP: {
                    rv = ecp_handle_init_rep(conn, msg, msg_size, nonce_buf, bufs);
                    if (rv < 0) goto unpack_fin;

                    payload += (hdr_size + rv);
                    pld_size -= (hdr_size + rv);
                    break;
                }

                case ECP_MTYPE_OPEN_REP: {
                    /* pass to open handler */
                    nonce_in = nonce_pkt;
                    nonce_map = ECP_ACK_FULL;
                    is_open_msg = 1;
                    break;
                }

                default:
                    rv = ECP_ERR_MTYPE;
                    goto unpack_fin;
            }
        }
    }

    if (is_open || is_open_msg) {
        ecp_seq_t seq = (ecp_seq_t)nonce_pkt;

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_lock(&conn->mutex);
#endif

        conn->nonce_in = nonce_in;
        conn->nonce_map = nonce_map;
        conn->access_ts = ecp_tm_get_s();
        if (is_inb && addr) {
            conn->remote.addr = *addr;
        }

#ifdef ECP_WITH_PTHREAD
        pthread_mutex_unlock(&conn->mutex);
#endif

        if (is_open_msg) {
            rv = ecp_pld_handle_one(conn, seq, payload, pld_size, bufs);
            if (rv < 0) {
                ecp_conn_remove(conn);
                if (!ecp_conn_is_gc(conn)) ecp_conn_refcount_dec(conn);
                goto unpack_fin;
            }

            payload += rv;
            pld_size -= rv;
        }

        *_conn = conn;
        *_payload = payload;
        *_seq = seq;
    }

    rv = _pkt_size - pld_size;

unpack_fin:
    if (conn && (*_conn == NULL)) ecp_conn_refcount_dec(conn);
    return rv;
}

ssize_t ecp_pkt_handle(ECPSocket *sock, ECPConnection *parent, ecp_tr_addr_t *addr, ECP2Buffer *bufs, size_t pkt_size) {
    ECPConnection *conn;
    unsigned char *payload;
    ecp_seq_t seq;
    size_t pld_size;
    ssize_t rv;

    rv = ecp_unpack(sock, parent, addr, bufs, pkt_size, &conn, &payload, &seq);
    if (rv < 0) return rv;

    pld_size = pkt_size - rv;
    if (conn == NULL) {
        if (pld_size) return ECP_ERR;
        return pkt_size;
    }

    if (pld_size) {
        rv = ecp_ext_pld_handle(conn, seq, payload, pld_size, bufs);
        if (rv < 0) goto pkt_handle_fin;

        payload += rv;
        pld_size -= rv;
    }

    if (pld_size) {
        rv = ecp_pld_handle(conn, seq, payload, pld_size, bufs);
        if (rv < 0) goto pkt_handle_fin;

        payload += rv;
        pld_size -= rv;
    }

    rv = pkt_size - pld_size;

pkt_handle_fin:
    ecp_conn_refcount_dec(conn);
    return rv;
}

ssize_t ecp_pkt_send(ECPSocket *sock, ECPBuffer *packet, size_t pkt_size, unsigned char flags, ECPTimerItem *ti, ecp_tr_addr_t *addr) {
    ssize_t rv;

    if (ti) {
        int _rv;

        _rv = ecp_timer_push(ti);
        if (_rv) return _rv;
    }

    rv = ecp_tr_send(sock, packet, pkt_size, addr, flags);
    if (rv < 0) return rv;

    return rv;
}

void ecp_nonce2buf(unsigned char *b, ecp_nonce_t *n) {
    b[0] = *n >> 56;
    b[1] = *n >> 48;
    b[2] = *n >> 40;
    b[3] = *n >> 32;
    b[4] = *n >> 24;
    b[5] = *n >> 16;
    b[6] = *n >> 8;
    b[7] = *n;
}

void ecp_buf2nonce(ecp_nonce_t *n, unsigned char *b) {
    *n  = (ecp_nonce_t)b[0] << 56;
    *n |= (ecp_nonce_t)b[1] << 48;
    *n |= (ecp_nonce_t)b[2] << 40;
    *n |= (ecp_nonce_t)b[3] << 32;
    *n |= (ecp_nonce_t)b[4] << 24;
    *n |= (ecp_nonce_t)b[5] << 16;
    *n |= (ecp_nonce_t)b[6] << 8;
    *n |= (ecp_nonce_t)b[7];
}

int ecp_pkt_get_seq(unsigned char *pkt, size_t pkt_size, ecp_seq_t *s) {
    *s = 0;

    if (pkt_size < ECP_MIN_PKT) return ECP_ERR_SIZE;

    pkt += ECP_SIZE_PROTO+1+ECP_SIZE_ECDH_PUB;
    *s |= (ecp_seq_t)pkt[4] << 24;
    *s |= (ecp_seq_t)pkt[5] << 16;
    *s |= (ecp_seq_t)pkt[6] << 8;
    *s |= (ecp_seq_t)pkt[7];

    return ECP_OK;
}

static ssize_t _pack(ECPBuffer *packet, ECPPktMeta *pkt_meta, ECPBuffer *payload, size_t pld_size) {
    ssize_t rv;
    unsigned char s_idx, c_idx;
    unsigned char *pkt_buf;
    size_t pkt_size;
    size_t hdr_size;

    pkt_size = ECP_MIN_PKT;
    if (pkt_meta->public == NULL) pkt_size -= ECP_SIZE_ECDH_PUB;
    if (pkt_meta->zpad) pkt_size += pkt_meta->zpad;
    if (pkt_meta->cookie) pkt_size += ECP_SIZE_COOKIE;

    if (packet->size < pkt_size) return ECP_ERR_SIZE;

    pkt_buf = packet->buffer;
    pkt_size = packet->size;

    /* PROTO */
    pkt_buf[0] = 0;
    pkt_buf[1] = 0;
    pkt_buf[ECP_SIZE_PROTO] = (pkt_meta->s_idx << 4) | pkt_meta->c_idx;
    pkt_buf += 3;

    if (pkt_meta->public) {
        memcpy(pkt_buf, pkt_meta->public, ECP_SIZE_ECDH_PUB);
        pkt_buf += ECP_SIZE_ECDH_PUB;
    }
    if (pkt_meta->zpad) {
        memset(pkt_buf, 0, pkt_meta->zpad);
        pkt_buf += pkt_meta->zpad;
    }
    if (pkt_meta->cookie) {
        memcpy(pkt_buf, pkt_meta->cookie, ECP_SIZE_COOKIE);
        pkt_buf += ECP_SIZE_COOKIE;
    }
    ecp_nonce2buf(pkt_buf, pkt_meta->nonce);
    pkt_buf += ECP_SIZE_NONCE;

    hdr_size = pkt_buf - packet->buffer;
    rv = ecp_aead_enc(pkt_buf, packet->size-hdr_size, payload->buffer, pld_size, pkt_meta->shkey, pkt_meta->nonce, pkt_meta->ntype);
    if (rv < 0) return rv;

    return rv+hdr_size;
}

static ssize_t _pack_conn(ECPConnection *conn, ECPBuffer *packet, unsigned char s_idx, unsigned char c_idx, uint16_t zpad, unsigned char *cookie, ecp_nonce_t *_nonce, ECPBuffer *payload, size_t pld_size, ecp_tr_addr_t *addr) {
    ECPPktMeta pkt_meta;
    ecp_ecdh_public_t public;
    ecp_aead_key_t shkey;
    ecp_nonce_t nonce;
    int rv = ECP_OK;

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_lock(&conn->mutex);
#endif

    if (s_idx == ECP_ECDH_IDX_INV) {
        if (ecp_conn_is_inb(conn)) {
            s_idx = conn->key_curr;
        } else {
            s_idx = conn->rkey_curr;
        }
    }
    if (c_idx == ECP_ECDH_IDX_INV) {
        if (ecp_conn_is_outb(conn)) {
            c_idx = conn->key_curr;
        } else {
            c_idx = conn->rkey_curr;
        }
    }
    rv = conn_shkey_get(conn, s_idx, c_idx, &shkey);
    if (rv) goto pack_conn_fin;

    if (ecp_conn_is_outb(conn)) {
        ECPDHKey *key = conn_dhkey_get(conn, c_idx);

        if (key) {
            memcpy(&public, &key->public, sizeof(public));
        } else {
            rv = ECP_ERR_ECDH_IDX;
        }
    } else {
        ECPDHPub *key = conn_dhkey_get_remote(conn, c_idx);

        if (key) {
            memcpy(&public, &key->public, sizeof(public));
        } else {
            rv = ECP_ERR_ECDH_IDX;
        }
        memcpy(&public, &key->public, sizeof(public));
    }
    if (rv) goto pack_conn_fin;

    if (_nonce) {
        nonce = *_nonce;
    } else {
        nonce = conn->nonce_out;
        conn->nonce_out++;
    }
    if (ecp_conn_is_root(conn) && addr) *addr = conn->remote.addr;

pack_conn_fin:

#ifdef ECP_WITH_PTHREAD
    pthread_mutex_unlock(&conn->mutex);
#endif

    if (rv) return rv;

    pkt_meta.zpad = zpad;
    pkt_meta.cookie = cookie;
    pkt_meta.public = &public;
    pkt_meta.shkey = &shkey;
    pkt_meta.nonce = &nonce;
    pkt_meta.ntype = (ecp_conn_is_inb(conn) ? ECP_NTYPE_INB : ECP_NTYPE_OUTB);
    pkt_meta.s_idx = s_idx;
    pkt_meta.c_idx = c_idx;
    return _pack(packet, &pkt_meta, payload, pld_size);
}

ssize_t ecp_pack_irep(ECPConnection *parent, ECPBuffer *packet, ECPPktMeta *pkt_meta, ECPBuffer *payload, size_t pld_size, ecp_tr_addr_t *addr) {
    ssize_t rv;

    if (parent == NULL) {
        pkt_meta->public = NULL;
        pkt_meta->c_idx = ECP_ECDH_IDX_NOKEY;
    }

    rv = _pack(packet, pkt_meta, payload, pld_size);
    if (rv < 0) return rv;

#ifdef ECP_WITH_VCONN
    if (parent) {
        rv = ecp_vconn_pack_parent(parent, payload, packet, rv, addr);
    }
#endif

    return rv;
}

ssize_t ecp_pack_conn(ECPConnection *conn, ECPBuffer *packet, unsigned char s_idx, unsigned char c_idx, uint16_t zpad, unsigned char *cookie, ecp_nonce_t *nonce, ECPBuffer *payload, size_t pld_size, ecp_tr_addr_t *addr) {
    ssize_t rv;

    rv = _pack_conn(conn, packet, s_idx, c_idx, zpad, cookie, nonce, payload, pld_size, addr);
    if (rv < 0) return rv;

#ifdef ECP_WITH_VCONN
    if (conn->parent) {
        rv = ecp_vconn_pack_parent(conn->parent, payload, packet, rv, addr);
    }
#endif

    return rv;
}

int ecp_pld_get_type(unsigned char *pld, size_t pld_size, unsigned char *mtype) {
    if (pld_size < ECP_SIZE_MTYPE) return ECP_ERR_SIZE;

    *mtype = pld[0];
    return ECP_OK;
}

int ecp_pld_set_type(unsigned char *pld, size_t pld_size, unsigned char mtype) {
    if (pld_size < ECP_SIZE_MTYPE) return ECP_ERR_SIZE;

    pld[0] = mtype;
    return ECP_OK;
}

int ecp_pld_get_frag(unsigned char *pld, size_t pld_size, unsigned char *frag_cnt, unsigned char *frag_tot, uint16_t *frag_size) {
    if (pld_size < ECP_SIZE_MTYPE) return ECP_ERR_SIZE;

    if (!(pld[0] & ECP_MTYPE_FLAG_FRAG)) return ECP_ERR;
    if (pld_size < (ECP_SIZE_MTYPE + ECP_SIZE_MT_FLAG(pld[0]))) return ECP_ERR_SIZE;
    if (pld[2] == 0) return ECP_ERR;

    *frag_cnt = pld[1];
    *frag_tot = pld[2];
    *frag_size = \
        ((uint16_t)pld[3] << 8) | \
        ((uint16_t)pld[4]);

    return ECP_OK;
}

int ecp_pld_set_frag(unsigned char *pld, size_t pld_size, unsigned char frag_cnt, unsigned char frag_tot, uint16_t frag_size) {
    if (pld_size < ECP_SIZE_MTYPE) return ECP_ERR_SIZE;

    if (!(pld[0] & ECP_MTYPE_FLAG_FRAG)) return ECP_ERR;
    if (pld_size < (ECP_SIZE_MTYPE + ECP_SIZE_MT_FLAG(pld[0]))) return ECP_ERR_SIZE;

    pld[1] = frag_cnt;
    pld[2] = frag_tot;
    pld[3] = frag_size >> 8;
    pld[4] = frag_size;

    return ECP_OK;
}

int ecp_pld_get_pts(unsigned char *pld, size_t pld_size, ecp_pts_t *pts) {
    size_t offset;

    if (pld_size < ECP_SIZE_MTYPE) return ECP_ERR_SIZE;

    if (!(pld[0] & ECP_MTYPE_FLAG_PTS)) return ECP_ERR;
    if (pld_size < (ECP_SIZE_MTYPE + ECP_SIZE_MT_FLAG(pld[0]))) return ECP_ERR_SIZE;

    offset = ECP_SIZE_MTYPE + ECP_SIZE_MT_FRAG(pld[0]);
    *pts = \
        ((ecp_pts_t)pld[offset]     << 24) | \
        ((ecp_pts_t)pld[offset + 1] << 16) | \
        ((ecp_pts_t)pld[offset + 2] << 8)  | \
        ((ecp_pts_t)pld[offset + 3]);

    return ECP_OK;
}

int ecp_pld_set_pts(unsigned char *pld, size_t pld_size, ecp_pts_t pts) {
    size_t offset;

    if (pld_size < ECP_SIZE_MTYPE) return ECP_ERR_SIZE;

    if (!(pld[0] & ECP_MTYPE_FLAG_PTS)) return ECP_ERR;
    if (pld_size < (ECP_SIZE_MTYPE + ECP_SIZE_MT_FLAG(pld[0]))) return ECP_ERR_SIZE;

    offset = ECP_SIZE_MTYPE + ECP_SIZE_MT_FRAG(pld[0]);
    pld[offset]     = pts >> 24;
    pld[offset + 1] = pts >> 16;
    pld[offset + 2] = pts >> 8;
    pld[offset + 3] = pts;

    return ECP_OK;
}

unsigned char *ecp_pld_get_msg(unsigned char *pld, size_t pld_size) {
    size_t offset;

    if (pld_size < ECP_SIZE_MTYPE) return NULL;
    if (pld_size < (ECP_SIZE_MTYPE + ECP_SIZE_MT_FLAG(pld[0]))) return NULL;

    return pld + ECP_SIZE_MTYPE + ECP_SIZE_MT_FLAG(pld[0]);
}

ssize_t _ecp_pld_send(ECPConnection *conn, ECPBuffer *packet, unsigned char s_idx, unsigned char c_idx, uint16_t zpad, unsigned char *cookie, ecp_nonce_t *n, ECPBuffer *payload, size_t pld_size, unsigned char flags, ECPTimerItem *ti) {
    ecp_tr_addr_t addr;
    size_t pkt_size;
    ssize_t rv;

    rv = ecp_pack_conn(conn, packet, s_idx, c_idx, zpad, cookie, n, payload, pld_size, &addr);
    if (rv < 0) return rv;

    pkt_size = rv;
    rv = ecp_ext_pld_send(conn, payload, pld_size, packet, pkt_size, flags, ti, &addr);
    if (rv) return rv;

    rv = ecp_pkt_send(conn->sock, packet, pkt_size, flags, ti, &addr);
    if (rv < 0) return rv;

    return pld_size;
}

ssize_t ecp_pld_send(ECPConnection *conn, ECPBuffer *packet, ECPBuffer *payload, size_t pld_size, unsigned char flags) {
    return _ecp_pld_send(conn, packet, ECP_ECDH_IDX_INV, ECP_ECDH_IDX_INV, 0, NULL, NULL, payload, pld_size, flags, NULL);
}

ssize_t ecp_pld_send_wtimer(ECPConnection *conn, ECPBuffer *packet, ECPBuffer *payload, size_t pld_size, unsigned char flags, ECPTimerItem *ti) {
    return _ecp_pld_send(conn, packet, ECP_ECDH_IDX_INV, ECP_ECDH_IDX_INV, 0, NULL, NULL, payload, pld_size, flags, ti);
}

ssize_t ecp_pld_send_wcookie(ECPConnection *conn, ECPBuffer *packet, ECPBuffer *payload, size_t pld_size, unsigned char flags, unsigned char *cookie) {
    return _ecp_pld_send(conn, packet, ECP_ECDH_IDX_INV, ECP_ECDH_IDX_INV, 0, cookie, NULL, payload, pld_size, flags, NULL);
}

ssize_t ecp_pld_send_wnonce(ECPConnection *conn, ECPBuffer *packet, ECPBuffer *payload, size_t pld_size, unsigned char flags, ecp_nonce_t *nonce) {
    return _ecp_pld_send(conn, packet, ECP_ECDH_IDX_INV, ECP_ECDH_IDX_INV, 0, NULL, nonce, payload, pld_size, flags, NULL);
}

ssize_t ecp_pld_send_irep(ECPSocket *sock, ECPConnection *parent, ecp_tr_addr_t *addr, ECPBuffer *packet, ECPPktMeta *pkt_meta, ECPBuffer *payload, size_t pld_size, unsigned char flags) {
    ecp_tr_addr_t _addr;
    ssize_t rv;

    rv = ecp_pack_irep(parent, packet, pkt_meta, payload, pld_size, addr ? NULL : &_addr);
    if (rv < 0) return rv;

    rv = ecp_pkt_send(sock, packet, rv, flags, NULL, addr ? addr : &_addr);
    if (rv < 0) return rv;

    return pld_size;
}

ssize_t ecp_msg_send(ECPConnection *conn, unsigned char mtype, unsigned char *msg, size_t msg_size) {
    ECPBuffer packet;
    ECPBuffer payload;
    unsigned char pkt_buf[ECP_MAX_PKT];
    unsigned char pld_buf[ECP_MAX_PLD];
    unsigned char *msg_buf;
    ssize_t rv;

    packet.buffer = pkt_buf;
    packet.size = ECP_MAX_PKT;
    payload.buffer = pld_buf;
    payload.size = ECP_MAX_PLD;

    rv = ecp_ext_msg_send(conn, mtype, msg, msg_size, &packet, &payload);
    if (rv) return rv;

    if (ECP_SIZE_PKT_BUF(msg_size, mtype, conn) <= ECP_MAX_PKT) {
        ecp_pld_set_type(pld_buf, ECP_MAX_PLD, mtype);
        msg_buf = ecp_pld_get_msg(pld_buf, ECP_MAX_PLD);

        memcpy(msg_buf, msg, msg_size);
        rv = ecp_pld_send(conn, &packet, &payload, ECP_SIZE_PLD(msg_size, mtype), 0);
        if (rv < 0) return rv;
    } else {
        return ECP_ERR_SIZE;
    }

    return msg_size;
}

static int recv_p(ECPSocket *sock, ecp_tr_addr_t *addr, ECPBuffer *packet, size_t size) {
    ECP2Buffer bufs;
    ECPBuffer payload;
    unsigned char pld_buf[ECP_MAX_PLD];
    ssize_t rv;

    bufs.packet = packet;
    bufs.payload = &payload;

    payload.buffer = pld_buf;
    payload.size = ECP_MAX_PLD;

    rv = ecp_pkt_handle(sock, NULL, addr, &bufs, size);
    if (rv < 0) return rv;

    return ECP_OK;
}

int ecp_receiver(ECPSocket *sock) {
    ecp_tr_addr_t addr;
    ECPBuffer packet;
    unsigned char pkt_buf[ECP_MAX_PKT];
    ecp_sts_t next = 0;
    ssize_t rv;
    int _rv;

    sock->running = 1;
    while(sock->running) {
        packet.buffer = pkt_buf;
        packet.size = ECP_MAX_PKT;

        rv = ecp_tr_recv(sock, &packet, &addr, next ? next : ECP_POLL_TIMEOUT);
        if (rv > 0) {
            _rv = recv_p(sock, &addr, &packet, rv);

#ifdef ECP_DEBUG
            if (_rv) printf("RECEIVE ERR:%d\n", _rv);
#endif

        }
        next = ecp_timer_exe(sock);
    }

    return ECP_OK;
}

#ifdef ECP_WITH_PTHREAD
static void *_ecp_receiver(void *arg) {
    ecp_receiver((ECPSocket *)arg);
    return NULL;
}

int ecp_start_receiver(ECPSocket *sock) {
    int rv;

    rv = pthread_create(&sock->rcvr_thd, NULL, _ecp_receiver, sock);
    if (rv) return ECP_ERR;
    return ECP_OK;
}

int ecp_stop_receiver(ECPSocket *sock) {
    int rv;

    sock->running = 0;
    rv = pthread_join(sock->rcvr_thd, NULL);
    if (rv) return ECP_ERR;
    return ECP_OK;
}
#endif