patty/src/sock.c

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

#include <patty/ax25.h>

static int bind_pty(patty_ax25_sock *sock) {
    struct termios t;

    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, sock->path, sizeof(sock->path)) < 0) {
        goto error_ptsname_r;
    }

    if (tcgetattr(sock->fd, &t) < 0) {
        goto error_tcgetattr;
    }

    cfmakeraw(&t);

    if (tcsetattr(sock->fd, TCSANOW, &t) < 0) {
        goto error_tcsetattr;
    }

    return 0;

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

error_open:
    return -1;
}

static inline size_t tx_bufsz(patty_ax25_sock *sock) {
    return PATTY_AX25_FRAME_OVERHEAD + sock->n_maxlen_tx;
}

static inline size_t rx_bufsz(patty_ax25_sock *sock) {
    return PATTY_AX25_FRAME_OVERHEAD + sock->n_maxlen_rx;
}

static inline size_t tx_slot_size(patty_ax25_sock *sock) {
    return sizeof(size_t) + PATTY_AX25_FRAME_OVERHEAD + sock->n_maxlen_tx;
}

static inline size_t tx_slots_size(patty_ax25_sock *sock) {
    return sock->n_window_tx * tx_slot_size(sock);
}

static inline void *tx_slot(patty_ax25_sock *sock, size_t seq) {
    size_t win = sock->n_window_tx,
             i = seq % win;

    return (uint8_t *)sock->tx_slots + (i * tx_slot_size(sock));
}

static int init_bufs(patty_ax25_sock *sock) {
    if ((sock->tx_buf = realloc(sock->tx_buf, tx_bufsz(sock))) == NULL) {
        goto error_realloc_tx_buf;
    }

    if ((sock->rx_buf = realloc(sock->rx_buf, rx_bufsz(sock))) == NULL) {
        goto error_realloc_rx_buf;
    }

    if ((sock->tx_slots = realloc(sock->tx_slots, tx_slots_size(sock))) == NULL) {
        goto error_realloc_tx_slots;
    }

    return 0;

error_realloc_tx_slots:
    free(sock->rx_buf);
    sock->rx_buf = NULL;

error_realloc_rx_buf:
    free(sock->tx_buf);
    sock->tx_buf = NULL;

error_realloc_tx_buf:
    return -1;
}

void patty_ax25_sock_init(patty_ax25_sock *sock) {
    sock->status      = PATTY_AX25_SOCK_CLOSED;
    sock->mode        = PATTY_AX25_SOCK_DM;
    sock->n_maxlen_tx = PATTY_AX25_SOCK_DEFAULT_MAXLEN;
    sock->n_maxlen_rx = PATTY_AX25_SOCK_DEFAULT_MAXLEN;
    sock->n_window_tx = PATTY_AX25_SOCK_DEFAULT_WINDOW;
    sock->n_window_rx = PATTY_AX25_SOCK_DEFAULT_WINDOW;
    sock->n_ack       = PATTY_AX25_SOCK_DEFAULT_ACK;
    sock->n_retry     = PATTY_AX25_SOCK_DEFAULT_RETRY;
    sock->seq_send    = 0;
    sock->seq_recv    = 0;

    timerclear(&sock->timer_ack);
    timerclear(&sock->timer_response);
    timerclear(&sock->timer_keepalive);
}

patty_ax25_sock *patty_ax25_sock_new(enum patty_ax25_proto proto,
                                     enum patty_ax25_sock_type type) {
    patty_ax25_sock *sock;

    if ((sock = malloc(sizeof(*sock))) == NULL) {
        goto error_malloc_sock;
    }

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

    patty_ax25_sock_init(sock);

    sock->proto         = proto;
    sock->type          = type;
    sock->version       = PATTY_AX25_2_0;
    sock->flags_classes = PATTY_AX25_SOCK_DEFAULT_CLASSES;
    sock->flags_hdlc    = PATTY_AX25_SOCK_DEFAULT_HDLC;

    if (init_bufs(sock) < 0) {
        goto error_init_bufs;
    }

    if (bind_pty(sock) < 0) {
        goto error_bind_pty;
    }

    return sock;

error_bind_pty:
error_init_bufs:
    if (sock->tx_slots) free(sock->tx_slots);
    if (sock->rx_buf)   free(sock->rx_buf);
    if (sock->tx_buf)   free(sock->tx_buf);

    free(sock);

error_malloc_sock:
    return NULL;
}

void patty_ax25_sock_destroy(patty_ax25_sock *sock) {
    if (sock->fd) {
        if (sock->iface) {
            (void)patty_ax25_if_promisc_delete(sock->iface, sock->fd);
        }

        close(sock->fd);
    }

    free(sock->tx_slots);
    free(sock->rx_buf);
    free(sock->tx_buf);
    free(sock);
}

/*
 * AX.25 v2.2 Specification, Section 6.5 "Resetting Procedure"
 */
int patty_ax25_sock_reset(patty_ax25_sock *sock) {
    sock->seq_send = 0;
    sock->seq_recv = 0;

    return 0;
}

int patty_ax25_sock_upgrade(patty_ax25_sock *sock,
                            enum patty_ax25_version version) {
    if (sock->version >= version) {
        return 0;
    }

    switch (version) {
        case PATTY_AX25_OLD:
        case PATTY_AX25_2_0:
            break;

        case PATTY_AX25_2_2:
            sock->flags_hdlc  = PATTY_AX25_SOCK_2_2_DEFAULT_HDLC;
            sock->n_maxlen_tx = PATTY_AX25_SOCK_2_2_DEFAULT_MAXLEN;
            sock->n_maxlen_rx = PATTY_AX25_SOCK_2_2_DEFAULT_MAXLEN;
            sock->n_window_tx = PATTY_AX25_SOCK_2_2_DEFAULT_WINDOW;
            sock->n_window_rx = PATTY_AX25_SOCK_2_2_DEFAULT_WINDOW;
    }

    sock->version = version;

    return init_bufs(sock);
}

int patty_ax25_sock_params_set(patty_ax25_sock *sock,
                               patty_ax25_params *params) {
    if (params->flags & PATTY_AX25_PARAM_CLASSES) {
        if (!(params->classes & PATTY_AX25_PARAM_CLASSES_ABM)) {
            goto error_notsup;
        }

        if (!(params->classes & PATTY_AX25_PARAM_CLASSES_HALF_DUPLEX)) {
            goto error_notsup;
        }

        if (params->classes & PATTY_AX25_PARAM_CLASSES_FULL_DUPLEX) {
            goto error_notsup;
        }
    }

    if (params->flags & PATTY_AX25_PARAM_HDLC) {
        if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_REJ)) {
            sock->flags_hdlc &= ~PATTY_AX25_PARAM_HDLC_REJ;
        }

        if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_SREJ)) {
            sock->flags_hdlc &= ~PATTY_AX25_PARAM_HDLC_SREJ;
        }

        if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_XADDR)) {
            goto error_invalid;
        }

        if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_MODULO_8)) {
            if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_MODULO_128)) {
                goto error_invalid;
            }
        } else {
            if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_MODULO_128)) {
                goto error_invalid;
            }

            params->hdlc &= ~PATTY_AX25_PARAM_HDLC_MODULO_8;
            params->hdlc |=  PATTY_AX25_PARAM_HDLC_MODULO_128;
        }

        if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_TEST)) {
            params->hdlc &= ~PATTY_AX25_PARAM_HDLC_TEST;
        }

        if (!(params->hdlc & PATTY_AX25_PARAM_HDLC_SYNC_TX)) {
            goto error_invalid;
        }
    }

    if (params->flags & PATTY_AX25_PARAM_INFO_RX) {
        if (sock->n_maxlen_tx > params->info_rx) {
            sock->n_maxlen_tx = params->info_rx;
        }
    }

    if (params->flags & PATTY_AX25_PARAM_WINDOW_RX) {
        if (sock->n_window_tx > params->window_rx) {
            sock->n_window_tx = params->window_rx;
        }
    }

    if (params->flags & PATTY_AX25_PARAM_ACK) {
        if (sock->n_ack < params->ack) {
            sock->n_ack = params->ack;
        }
    }

    if (params->flags & PATTY_AX25_PARAM_RETRY) {
        if (sock->n_retry < params->retry) {
            sock->n_retry = params->retry;
        }
    }

    return init_bufs(sock);

error_notsup:
    errno = ENOTSUP;

    return -1;

error_invalid:
    errno = EINVAL;

    return -1;
}

char *patty_ax25_sock_pty(patty_ax25_sock *sock) {
    return sock->path;
}

int patty_ax25_sock_bind_if(patty_ax25_sock *sock,
                            patty_ax25_if *iface) {
    sock->iface = iface;

    if (sock->type == PATTY_AX25_SOCK_RAW) {
        if (patty_ax25_if_promisc_add(iface, sock->fd) < 0) {
            goto error_if_promisc_add;
        }
    }

    sock->flags_classes |= iface->flags_classes;

    return 0;

error_if_promisc_add:
    return -1;
}

void patty_ax25_sock_seq_send_incr(patty_ax25_sock *sock) {
    if (sock->mode == PATTY_AX25_SOCK_SABM) {
        sock->seq_send = (sock->seq_send + 1) & 0x07;
    } else {
        sock->seq_send = (sock->seq_send + 1) & 0x7f;
    }
}

void patty_ax25_sock_seq_recv_incr(patty_ax25_sock *sock) {
    if (sock->mode == PATTY_AX25_SOCK_SABM) {
        sock->seq_recv = (sock->seq_recv + 1) & 0x07;
    } else {
        sock->seq_recv = (sock->seq_recv + 1) & 0x7f;
    }
}

static inline size_t maxsz(patty_ax25_sock *sock,
                           size_t infolen) {
    return infolen + PATTY_AX25_FRAME_OVERHEAD;
}

static inline int toobig(patty_ax25_sock *sock,
                         size_t infolen) {
    return infolen > PATTY_AX25_FRAME_OVERHEAD + sock->n_maxlen_tx;
}

static ssize_t encode_address(patty_ax25_sock *sock,
                              enum patty_ax25_frame_cr cr,
                              void *dest,
                              size_t len) {
    uint8_t *buf = (uint8_t *)dest;
    size_t offset = 0;

    uint8_t flags_remote = 0x00,
            flags_local  = 0x00;

    unsigned int i;

    if ((2 + sock->hops) * sizeof(patty_ax25_addr) > len) {
        errno = EOVERFLOW;

        goto error_toobig;
    }

    switch (cr) {
        case PATTY_AX25_FRAME_COMMAND:  flags_remote = 0x80; break;
        case PATTY_AX25_FRAME_RESPONSE: flags_local  = 0x80; break;
        case PATTY_AX25_FRAME_OLD: break;
    }

    offset += patty_ax25_addr_copy(buf + offset, &sock->remote, flags_remote);
    offset += patty_ax25_addr_copy(buf + offset, &sock->local,  flags_local);

    for (i=0; i<sock->hops; i++) {
        offset += patty_ax25_addr_copy(buf + offset, &sock->repeaters[i], 0);
    }

    ((uint8_t *)buf)[offset-1] |= 1;

    return offset;

error_toobig:
    return -1;
}

ssize_t patty_ax25_sock_send(patty_ax25_sock *sock,
                             enum patty_ax25_frame_cr cr,
                             uint16_t control,
                             void *info,
                             size_t infolen) {
     size_t offset = 0;
    ssize_t encoded;

    uint8_t *buf = PATTY_AX25_FRAME_CONTROL_U(control)?
        sock->tx_buf: tx_slot(sock, sock->seq_send);

    if (sock->iface == NULL) {
        errno = ENETDOWN;

        goto error_noif;
    }

    if (sock->remote.callsign[0] == '\0') {
        errno = EBADF;

        goto error_nopeer;
    }

    if (toobig(sock, infolen)) {
        goto error_toobig;
    }

    if ((encoded = encode_address(sock, cr, buf, tx_bufsz(sock))) < 0) {
        goto error_encode_address;
    } else {
        offset += encoded;
    }

    buf[offset++] = control & 0x00ff;

    if (sock->mode == PATTY_AX25_SOCK_SABME && !PATTY_AX25_FRAME_CONTROL_U(control)) {
        buf[offset++] = (control & 0xff00) >> 8;
    }

    if (info && infolen) {
        if (PATTY_AX25_FRAME_CONTROL_I(control)) {
            buf[offset++] = (uint8_t)sock->proto;
        }

        memcpy(buf + offset, info, infolen);

        offset += infolen;
    }

    return patty_ax25_if_send(sock->iface, buf, offset);

error_encode_address:
error_toobig:
error_nopeer:
error_noif:
    return -1;
}

ssize_t patty_ax25_sock_resend(patty_ax25_sock *sock,
                               unsigned int seq) {
    void *slot = tx_slot(sock, seq);
    void *buf  = (uint8_t *)slot + sizeof(size_t);

    size_t len = *((size_t *)slot);

    return patty_ax25_if_send(sock->iface, buf, len);
}

static uint16_t control_i(patty_ax25_sock *sock, int flag) {
    switch (sock->mode) {
        case PATTY_AX25_SOCK_SABM:
            return ((sock->seq_recv & 0x07) << 5)
                 | ((sock->seq_send & 0x07))
                 | (flag << 4);

        case PATTY_AX25_SOCK_SABME:
            return ((sock->seq_recv & 0x7f) << 9)
                 | ((sock->seq_send & 0x7f))
                 | (flag << 8);

        default:
            return 0;
    }
}

static uint16_t control_s(patty_ax25_sock *sock,
                          enum patty_ax25_frame_type type,
                          int flag) {
    switch (sock->mode) {
        case PATTY_AX25_SOCK_SABM:
            return ((sock->seq_recv & 0x07) << 5)
                 | (type & 0x0f)
                 | (flag << 4);

        case PATTY_AX25_SOCK_SABME:
            return ((sock->seq_recv & 0x7f) << 9)
                 | (type & 0x0f)
                 | (flag << 8);

        default:
            return 0;
    }
}

static uint16_t control_u(enum patty_ax25_frame_type type,
                          int pf) {
    return (type & PATTY_AX25_FRAME_U_MASK)
         | (pf << 4);
}

ssize_t patty_ax25_sock_send_rr(patty_ax25_sock *sock,
                                enum patty_ax25_frame_cr cr,
                                int pf) {
    return patty_ax25_sock_send(sock,
                                cr,
                                control_s(sock, PATTY_AX25_FRAME_RR, 1),
                                NULL,
                                pf);
}

ssize_t patty_ax25_sock_send_rnr(patty_ax25_sock *sock,
                                 enum patty_ax25_frame_cr cr,
                                 int pf) {
    return patty_ax25_sock_send(sock,
                                cr,
                                control_s(sock, PATTY_AX25_FRAME_RNR, 1),
                                NULL,
                                pf);
}

ssize_t patty_ax25_sock_send_rej(patty_ax25_sock *sock,
                                 enum patty_ax25_frame_cr cr,
                                 int pf) {
    return patty_ax25_sock_send(sock,
                                cr,
                                control_s(sock, PATTY_AX25_FRAME_REJ, 1),
                                NULL,
                                pf);
}

ssize_t patty_ax25_sock_send_srej(patty_ax25_sock *sock,
                                  enum patty_ax25_frame_cr cr) {
    return patty_ax25_sock_send(sock,
                                cr,
                                control_s(sock, PATTY_AX25_FRAME_SREJ, 1),
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_send_sabm(patty_ax25_sock *sock, int pf) {
    return patty_ax25_sock_send(sock,
                                PATTY_AX25_FRAME_COMMAND,
                                control_u(PATTY_AX25_FRAME_SABM, pf),
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_send_sabme(patty_ax25_sock *sock, int pf) {
    return patty_ax25_sock_send(sock,
                                PATTY_AX25_FRAME_COMMAND,
                                control_u(PATTY_AX25_FRAME_SABME, pf),
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_send_disc(patty_ax25_sock *sock, int pf) {
    return patty_ax25_sock_send(sock,
                                PATTY_AX25_FRAME_COMMAND,
                                control_u(PATTY_AX25_FRAME_DISC, pf),
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_send_xid(patty_ax25_sock *sock,
                                 enum patty_ax25_frame_cr cr) {
    patty_ax25_params params;
    char buf[256];

    ssize_t encoded;

    if (sock->iface == NULL) {
        errno = ENETDOWN;

        goto error_noif;
    }

    params.flags = (1 << PATTY_AX25_PARAM_CLASSES)
                 | (1 << PATTY_AX25_PARAM_HDLC)
                 | (1 << PATTY_AX25_PARAM_INFO_RX)
                 | (1 << PATTY_AX25_PARAM_WINDOW_RX)
                 | (1 << PATTY_AX25_PARAM_ACK)
                 | (1 << PATTY_AX25_PARAM_RETRY);

    params.classes   = sock->flags_classes;
    params.hdlc      = sock->flags_hdlc;
    params.info_rx   = sock->n_maxlen_rx;
    params.window_rx = sock->n_window_rx;
    params.ack       = sock->n_ack;
    params.retry     = sock->n_retry;

    if ((encoded = patty_ax25_frame_encode_xid(&params, buf, sizeof(buf))) < 0) {
        goto error_frame_encode_xid;
    }

    return patty_ax25_sock_send(sock,
                                cr,
                                control_u(PATTY_AX25_FRAME_XID, 0),
                                buf,
                                encoded);

error_frame_encode_xid:
error_noif:
    return -1;
}

ssize_t patty_ax25_sock_write(patty_ax25_sock *sock,
                              void *buf,
                              size_t len) {
    uint16_t control;

    if (sock->type == PATTY_AX25_SOCK_RAW) {
        return patty_ax25_if_send(sock->iface, buf, len);
    }

    if (sock->mode == PATTY_AX25_SOCK_DM) {
        errno = EBADF;

        goto error_invalid_mode;
    }

    control = control_i(sock, 0);

    if (patty_ax25_sock_send(sock, PATTY_AX25_FRAME_COMMAND, control, buf, len) < 0) {
        goto error_send;
    }

    patty_ax25_sock_seq_send_incr(sock);

    return len;

error_send:
error_invalid_mode:
    return -1;
}