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

    sock->opts |= PATTY_AX25_SOCK_PTY;

    return 0;

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

error_open:
    return -1;
}

patty_ax25_sock *patty_ax25_sock_new(enum patty_ax25_sock_opts opts) {
    patty_ax25_sock *sock;

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

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

    if ((sock->buf = malloc(PATTY_AX25_SOCK_BUFSZ)) == NULL) {
        goto error_malloc_buf;
    }

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

    sock->bufsz    = PATTY_AX25_SOCK_BUFSZ;
    sock->status   = PATTY_AX25_SOCK_CLOSED;
    sock->mode     = PATTY_AX25_SOCK_DM;
    sock->opts     = opts;
    sock->n_maxlen = PATTY_AX25_FRAME_DEFAULT_MAXLEN;
    sock->n_window = PATTY_AX25_FRAME_DEFAULT_WINDOW;

    return sock;

error_bind_pty:
    free(sock->buf);

error_malloc_buf:
    free(sock);

error_malloc_sock:
    return NULL;
}

void patty_ax25_sock_reset(patty_ax25_sock *sock) {
    sock->opts  &= ~PATTY_AX25_SOCK_PTY;
    sock->status =  PATTY_AX25_SOCK_CLOSED;
    sock->mode   =  PATTY_AX25_SOCK_DM;
}

void patty_ax25_sock_destroy(patty_ax25_sock *sock) {
    if (sock->fd) {
        close(sock->fd);
    }

    free(sock->buf);
    free(sock);
}

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

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

static size_t copy_addr_to_tx_buf(patty_ax25_sock *sock) {
    void *buf = sock->iface->tx_buf;

    unsigned int i;

    size_t offset = 0;

    memcpy((uint8_t *)buf + offset, &sock->remote, sizeof(patty_ax25_addr));
    offset += sizeof(patty_ax25_addr);
    ((uint8_t *)buf)[offset-1] &= ~1;

    memcpy((uint8_t *)buf + offset, &sock->local,  sizeof(patty_ax25_addr));
    offset += sizeof(patty_ax25_addr);
    ((uint8_t *)buf)[offset-1] &= ~1;

    for (i=0; i<sock->hops; i++) {
        memcpy((uint8_t *)buf + offset,
               &sock->repeaters[i],
               sizeof(patty_ax25_addr));

        offset += sizeof(patty_ax25_addr);

        ((uint8_t *)buf)[offset-1] &= ~1;
    }

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

    return offset;
}

static inline int toobig(patty_ax25_sock *sock,
                         uint16_t control,
                         size_t infolen) {
    return PATTY_AX25_FRAME_SIZE(sock->mode == PATTY_AX25_SOCK_SABME?
                                               PATTY_AX25_FRAME_EXTENDED:
                                               PATTY_AX25_FRAME_NORMAL,
                                 sock->hops,
                                 control,
                                 infolen) > sock->iface->tx_bufsz? 1: 0;
}

ssize_t patty_ax25_sock_send(patty_ax25_sock *sock,
                             uint16_t control,
                             uint8_t proto,
                             void *info,
                             size_t infolen) {
    size_t offset;

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

    offset = copy_addr_to_tx_buf(sock);

    if (sock->mode == PATTY_AX25_SOCK_SABME) {
        ((uint8_t *)sock->iface->tx_buf)[offset++] = (control & 0xff00) >> 8;
        ((uint8_t *)sock->iface->tx_buf)[offset++] =  control & 0x00ff;

        sock->seq_send = (sock->seq_send + 1) & 0x07;
    } else {
        ((uint8_t *)sock->iface->tx_buf)[offset++] = control;

        sock->seq_send = (sock->seq_send + 1) & 0x7f;
    }

    if (PATTY_AX25_CONTROL_INFO(control)) {
        ((uint8_t *)sock->iface->tx_buf)[offset++] = proto;

        memcpy((uint8_t *)sock->iface->tx_buf + offset, info, infolen);
        offset += infolen;
    }

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

error_toobig:
    return -1;
}

ssize_t patty_ax25_sock_send_sabm(patty_ax25_sock *sock, int poll) {
    return patty_ax25_sock_send(sock,
                                PATTY_AX25_FRAME_U_SABM
                              | PATTY_AX25_FRAME_U_POLL,
                                PATTY_AX25_PROTO_NONE,
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_send_sabme(patty_ax25_sock *sock, int poll) {
    return patty_ax25_sock_send(sock,
                                PATTY_AX25_FRAME_U_SABME
                              | PATTY_AX25_FRAME_U_POLL,
                                PATTY_AX25_PROTO_NONE,
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_send_disc(patty_ax25_sock *sock, int poll) {
    return patty_ax25_sock_send(sock,
                                PATTY_AX25_FRAME_U_DISC
                              | PATTY_AX25_FRAME_U_POLL,
                                PATTY_AX25_PROTO_NONE,
                                NULL,
                                0);
}

ssize_t patty_ax25_sock_write(patty_ax25_sock *sock,
                              uint8_t proto,
                              void *buf,
                              size_t len) {
    uint16_t control = 0x0000;

    switch (sock->mode) {
        case PATTY_AX25_SOCK_DM:
            errno = EBADF;

            goto error_invalid_mode;

        case PATTY_AX25_SOCK_SABM:
            control = PATTY_AX25_SOCK_CONTROL_SABM(sock, 1);

            break;

        case PATTY_AX25_SOCK_SABME:
            control = PATTY_AX25_SOCK_CONTROL_SABME(sock, 1);

            break;
    }

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

    return len;

error_send:
error_invalid_mode:
    return -1;
}