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_slot_size(size_t len) {
    return sizeof(size_t)
         + sizeof(patty_ax25_addr) * 2
         + sizeof(patty_ax25_addr) * PATTY_AX25_MAX_HOPS
         + len;
}

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

static inline void *tx_slot(void *slots, size_t i, size_t len) {
    return (void *)((uint8_t *)slots + (i * tx_slot_size(len)));
}

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));

    sock->proto       = proto;
    sock->type        = type;
    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_RX_MAXLEN;
    sock->n_window_tx = PATTY_AX25_SOCK_DEFAULT_WINDOW;
    sock->n_window_rx = PATTY_AX25_SOCK_RX_WINDOW;

    if ((sock->buf = malloc(PATTY_AX25_FRAME_OVERHEAD + sock->n_maxlen_rx)) == NULL) {
        goto error_malloc_buf;
    }

    if ((sock->tx_slots = malloc(tx_slots_size(sock))) == NULL) {
        goto error_malloc_tx_slots;
    }

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

    return sock;

error_bind_pty:
    free(sock->tx_slots);

error_malloc_tx_slots:
    free(sock->buf);

error_malloc_buf:
    free(sock);

error_malloc_sock:
    return NULL;
}

void patty_ax25_sock_reset(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_RX_MAXLEN;
    sock->n_retry         = PATTY_AX25_SOCK_DEFAULT_RETRY;
    sock->n_window_tx     = PATTY_AX25_SOCK_RX_WINDOW;
    sock->timer_ack       = 0;
    sock->timer_response  = 0;
    sock->timer_keepalive = 0;
    sock->flags           = 0;
    sock->seq_send        = 0;
    sock->seq_recv        = 0;
}

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->buf);
    free(sock->tx_slots);
    free(sock);
}

int patty_ax25_sock_mode_set(patty_ax25_sock *sock,
                             enum patty_ax25_sock_mode mode) {
    if (sock->mode == mode) {
        return 0;
    }

    sock->mode = mode;

    return 0;
}

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;
        }
    }

    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_SABME) {
        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_SABME) {
        sock->seq_recv = (sock->seq_recv + 1) & 0x07;
    } else {
        sock->seq_recv = (sock->seq_recv + 1) & 0x7f;
    }
}

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

    uint8_t flags_remote = 0x00,
            flags_local  = 0x00;

    unsigned int i;

    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;
}

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

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;

    void *slot = tx_slot(sock->tx_slots,
                         sock->seq_send,
                         sock->n_maxlen_tx);

    uint8_t *buf = (uint8_t *)slot + sizeof(size_t);

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

    offset = addr_copy_to_buf(buf, sock, cr);

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

    if (info) {
        buf[offset++] = (uint8_t)sock->proto;

        memcpy(buf + offset, info, infolen);
        offset += infolen;
    }

    *((size_t *)slot) = offset;

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

error_toobig:
    return -1;
}

ssize_t patty_ax25_sock_resend(patty_ax25_sock *sock,
                               unsigned int seq) {
    void *slot = tx_slot(sock->tx_slots, seq, sock->n_maxlen_tx);
    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) {
    return patty_ax25_sock_send(sock,
                                cr,
                                control_s(sock, PATTY_AX25_FRAME_RR, 1),
                                NULL,
                                0);
}

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

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

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_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_RESPONSE, control, buf, len) < 0) {
        goto error_send;
    }

    patty_ax25_sock_seq_send_incr(sock);

    return len;

error_send:
error_invalid_mode:
    return -1;
}