patty/src/sock.c

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

#include <patty/ax25.h>

patty_ax25_sock *patty_ax25_sock_new(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->status   = PATTY_AX25_SOCK_CLOSED;
    sock->mode     = PATTY_AX25_SOCK_DM;
    sock->type     = type;
    sock->n_maxlen = PATTY_AX25_FRAME_DEFAULT_MAXLEN;
    sock->n_window = PATTY_AX25_FRAME_DEFAULT_WINDOW;

    return sock;

error_malloc_sock:
    return NULL;
}

void patty_ax25_sock_destroy(patty_ax25_sock *sock) {
    free(sock);
}

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 = 0;
    ssize_t len;

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

    if ((len = copy_addr_to_tx_buf(sock)) < 0) {
        goto error_toobig;
    } else {
        offset += len;
    }

    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;
    } else {
        ((uint8_t *)sock->iface->tx_buf)[offset++] = control;
    }

    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 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_write(patty_ax25_sock *sock,
                              uint8_t proto,
                              void *buf,
                              size_t len) {
    uint16_t control = sock->mode == PATTY_AX25_SOCK_SABME?
        PATTY_AX25_SOCK_CONTROL_SABME(sock, 1):
        PATTY_AX25_SOCK_CONTROL_SABM(sock, 1);

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

    return len;

error_send:
    return -1;
}