ellsi.c

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

#include "errlog.h"
#include "osiSock.h"
#include "epicsAssert.h"

#include "osiStdInt.h"
#include "can_frame.h"
#include "scan_driver.h"
#include "new.h"

#define NDEBUG

#ifdef NDEBUG
static int nothing(const char *fmt, ...) { return 0; }
#define debug nothing
#else
#define debug printf
#endif

/* The default port for the ELLSI UDP server is 2209. */
/* All ELLSI-telegram data has to be given (or is given) in network byte order. */

/* The data always consists of a header plus trailing payload data. The payload
data itself consists of the data according to a single command or to
n-CAN-telegrams. */

/* 2.5 Header */
typedef struct {
    uint32_t    magic;
    uint32_t    sequence;
    uint32_t    command;
    uint32_t    payloadLen;
    uint32_t    subcommand;
    union {
        int32_t i[8];
        int8_t  c[32];
    } reserved;
} __attribute__((packed)) ellsiHeader;

STATIC_ASSERT(sizeof(ellsiHeader)==13*4);

#define ELLSI_MAGIC             0x454c5349

/* 2.6.1 Numerical values of commands */
#define ELLSI_CMD_NOP                   0
#define ELLSI_CMD_CAN_TELEGRAM          1
#define ELLSI_CMD_HEARTBEAT             2
#define ELLSI_CMD_CTRL                  3
#define ELLSI_CMD_REGISTER              4
#define ELLSI_CMD_REGISTERX             5
#define ELLSI_CMD_UNREGISTER            6

/* 2.6.2 Numerical values of sub-commands (if command is ELLSI_CMD_CTRL) */
#define ELLSI_IOCTL_NOP                 0
#define ELLSI_SUBCMD_NONE               0
#define ELLSI_IOCTL_CAN_ID_ADD          1
#define ELLSI_IOCTL_CAN_ID_DELETE       2
#define ELLSI_IOCTL_CAN_SET_BAUDRATE    3
#define ELLSI_IOCTL_CAN_GET_BAUDRATE    4
#define ELLSI_IOCTL_GET_LAST_STATE      5
#define ELLSI_IOCTL_SET_SJA1000_ACMR    6
#define ELLSI_IOCTL_CAN_STATUS          7
#define ELLSI_IOCTL_BUS_STATISTIC       8
#define ELLSI_IOCTL_GET_TIMESTAMP       9
#define ELLSI_IOCTL_GET_TIMESTAMP_FREQ  10
#define ELLSI_IOCTL_GET_SERIAL          11
#define ELLSI_SUBCMD_TXDONE             128
#define ELLSI_SUBCMD_AUTOACK            256

/* Payloads */

/* 2.6.5 ELLSI_CMD_REGISTERX */
typedef struct {
    uint32_t    heartBeatIntervall;
    uint32_t    clientDeadMultiplier;
    uint32_t    canTxQueueSize;
    uint32_t    canRxQueueSize;
    uint32_t    socketSendMaxNTelegrams;
    uint32_t    socketSendIntervall;
    uint16_t    flags;
    uint8_t     clientProtocolVersion;
    uint8_t     netNumber;
    uint32_t    reserved[7];
} __attribute__((packed)) ellsiExtRegistration;

/* 2.6.6 ELLSI_CMD_CAN_TELEGRAM */
typedef struct {
    uint32_t    id;
    uint8_t     len;
    uint8_t     msg_lost;
    uint8_t     reserved[2];
    uint8_t     data[8];
    uint64_t    timestamp;
} __attribute__((packed)) ellsiCanTelegram;

STATIC_ASSERT(sizeof(ellsiCanTelegram)==6*4);

#define CAN_TELEGRAM_LEN_MASK       0x0f
#define CAN_TELEGRAM_LEN_RTR        0x10
#define CAN_TELEGRAM_LEN_TXDONE     0x20

/* 2.6.8.5 ELLSI_IOCTL_GET_LAST_STATE */
typedef struct {
    uint32_t    lastCommand;
    uint32_t    lastSubcommand;
    int32_t     lastState;
    uint32_t    lastRxSeq;
    uint32_t    reserved[4];
} __attribute__((packed)) ellsiLastState;

/* 2.6.8.1 ELLSI_IOCTL_CAN_ID_ADD/DELETE */
typedef struct {
    uint32_t    rangeStart;
    uint32_t    rangeEnd;
} __attribute__((packed)) ellsiCanIdRange;

/* 2.6.8.6 ELLSI_IOCTL_CAN_STATUS */
typedef struct
{
    uint16_t    hardware;
    uint16_t    firmware;
    uint16_t    driver;
    uint16_t    dll;
    uint32_t    boardstatus;
    uint8_t     boardid[14];
    uint16_t    features;
} __attribute__((packed)) ellsiCanIfStatus;

static uint32_t ellsi_from_baudrate(int rate) {
    switch (rate) {
    case 1000: return 0;
    case  666: return 1;
    case  500: return 2;
    case  333: return 3;
    case  250: return 4;
    case  166: return 5;
    case  125: return 6;
    case  100: return 7;
    case   66: return 8;
    case   50: return 9;
    case   33: return 10;
    case   20: return 11;
    case   12: return 12;
    case   10: return 13;
    default:   return -1;
    }
}

#if 0
int ellsi_to_baudrate(uint32_t code) {
    switch (code) {
    case  0: return 1000;
    case  1: return  666;
    case  2: return  500;
    case  3: return  333;
    case  4: return  250;
    case  5: return  166;
    case  6: return  125;
    case  7: return  100;
    case  8: return   66;
    case  9: return   50;
    case 10: return   33;
    case 11: return   20;
    case 12: return   12;
    case 13: return   10;
    default:   return  0;
}
#endif

/*
 * The 50 ellsiCanTelegrams below are the default maximum number of
 * CAN frames that the server puts into a single message according
 * to the docs.
 */
typedef struct ellsi_state ellsi_state_t;
struct ellsi_state {
    struct msg {
        ellsiHeader header;
        ellsiCanTelegram payload[50];
    } __attribute__((packed)) msg;
    ellsiCanTelegram    *next;
    unsigned            unread; /* number of as yet unread telegrams in packet */
    int                 sock;   /* socket to communicate over */
    struct sockaddr_in  addr;   /* socket address info */
};

static ssize_t ellsi_write(void *pvt, const struct can_frame *frame) {
    ellsi_state_t *state = (ellsi_state_t *)pvt;
    struct msg {
        ellsiHeader header;
        ellsiCanTelegram payload;
    } __attribute__((packed)) msg;
    STATIC_ASSERT(sizeof(msg)==19*4);
    int r;

    memset(&msg, 0, sizeof(msg));
    msg.header.magic = htonl(ELLSI_MAGIC);
    msg.header.command = htonl(ELLSI_CMD_CAN_TELEGRAM);
#if 0
    msg.header.subcommand = htonl(ELLSI_SUBCMD_TXDONE);
#endif
    msg.header.payloadLen = htonl(sizeof(msg.payload));
    msg.payload.id = htonl(frame->can_id & CAN_SFF_MASK);
    msg.payload.len = frame->can_dlc & CAN_TELEGRAM_LEN_MASK;
    if (frame->can_id & CAN_RTR_FLAG)
        msg.payload.len |= CAN_TELEGRAM_LEN_RTR;
    memcpy(msg.payload.data, frame->data, CAN_MAX_DLC);
    r = write(state->sock, &msg, sizeof(msg));
    debug("ellsi_write: sent %d\n", r);
    return r;
}

static ssize_t ellsi_read(void *pvt, struct can_frame *frame) {
    ellsi_state_t *state = (ellsi_state_t *)pvt;
    int nbytes;
    ellsiCanTelegram *this;

    while (state->unread == 0) {
        nbytes = recv(state->sock, &state->msg, sizeof(state->msg), MSG_WAITALL);
        debug("ellsi_read: got cmd %d subcmd %d more bytes %d\n",
            ntohl(state->msg.header.command),
            ntohl(state->msg.header.subcommand),
            ntohl(state->msg.header.payloadLen));
        if (nbytes < 0) {
            errbuf_t buf;
            fprintf(stderr, "ellsi_read: recv failed (%s)\n", ERRNO_STR(buf));
            return -1;
        }
        if (state->msg.header.magic != htonl(ELLSI_MAGIC)) {
            fprintf(stderr, "ellsi_read: bad magic\n");
            return -1;
        }
        switch (ntohl(state->msg.header.command)) {
        case ELLSI_CMD_CAN_TELEGRAM:
            assert(nbytes == sizeof(state->msg.header) + ntohl(state->msg.header.payloadLen));
            state->unread = ntohl(state->msg.header.payloadLen) / sizeof(ellsiCanTelegram);
            assert(state->unread <= 50);
            state->next = &state->msg.payload[0];
            break;
        case ELLSI_CMD_HEARTBEAT:
            /* immediately send it back so server knows we are still there */
            assert(state->msg.header.payloadLen == 0);
            nbytes = write(state->sock, &state->msg.header, sizeof(state->msg.header));
            if (nbytes < sizeof(state->msg.header)) {
                errbuf_t buf;
                fprintf(stderr, "ellsi_read: reply to heartbeat failed (%s)\n", ERRNO_STR(buf));
            }
            break;
#if 0
        case ELLSI_CMD_NOP:
        case ELLSI_CMD_CTRL:
        case ELLSI_CMD_REGISTER:
        case ELLSI_CMD_REGISTERX:
        case ELLSI_CMD_UNREGISTER:
#endif
        default:
            /* ignore messages we don't care about */
            break;
        }
    }
    state->unread--;
    this = state->next++;
    if (this->len & CAN_TELEGRAM_LEN_TXDONE) {
        fprintf(stderr, "ellsi_read: ignored confirmation (TXDONE)\n");
        return ellsi_read(pvt, frame);
    }
    frame->can_id = ntohl(this->id) & CAN_SFF_MASK;
    if (this->len & CAN_TELEGRAM_LEN_RTR)
        frame->can_id |= CAN_RTR_FLAG;
    frame->can_dlc = this->len & CAN_TELEGRAM_LEN_MASK;
    memcpy(frame->data, this->data, CAN_MAX_DLC);
    assert(this - state->msg.payload <= 50);
    return 0;
}

static void ellsi_dump(void *pvt) {
    ellsi_state_t *state = (ellsi_state_t *)pvt;
    char buf[100];

    ipAddrToDottedIP(&state->addr, buf, 100);
    printf("  driver ellsi\n");
    printf("  socket %d\n", state->sock);
    printf("  ip_addr:ip_port %s\n", buf);
}

static int ellsi_check(int sock) {
    struct ack {
        ellsiHeader header;
        ellsiLastState payload;
    } __attribute__((packed)) ack;
    int r;

    memset(&ack, 0, sizeof(ack));
    r = recv(sock, &ack, sizeof(ack), MSG_WAITALL);
    if (r < 0) {
        fprintf(stderr, "ellsi_check: recv failed\n"); return r;
    }
    debug("ellsi_check: got back magic %#x cmd %#x subcmd %#x payloadLen %d\n",
        ntohl(ack.header.magic),
        ntohl(ack.header.command),
        ntohl(ack.header.subcommand),
        ntohl(ack.header.payloadLen)
    );
#if 0
    assert(r == sizeof(ack));
    assert(ntohl(ack.header.payloadLen)==sizeof(ack.payload));
#endif
    if (ntohl(ack.header.magic) == ELLSI_MAGIC
        && ntohl(ack.header.command) == ELLSI_CMD_CTRL
        && ntohl(ack.header.subcommand) == ELLSI_IOCTL_GET_LAST_STATE)
    {
        debug("ellsi_check: last cmd %#x subcmd %#x state %#x\n",
            ntohl(ack.payload.lastCommand),
            ntohl(ack.payload.lastSubcommand),
            ntohl(ack.payload.lastState));
    } else {
        return -1;
    }
    return ack.payload.lastState;
}

static int ellsi_register(ellsi_state_t *state) {
    struct msg {
        ellsiHeader header;
    } __attribute__((packed)) msg;
    int r;

    memset(&msg, 0, sizeof(msg));
    msg.header.magic = htonl(ELLSI_MAGIC);
    msg.header.command = htonl(ELLSI_CMD_REGISTER);
    msg.header.subcommand = htonl(ELLSI_SUBCMD_AUTOACK);
    debug("ellsi_register: send magic %#x, cmd %#x subcmd %#x\n",
        ELLSI_MAGIC, ELLSI_CMD_REGISTER, ELLSI_SUBCMD_AUTOACK);
    r = write(state->sock, &msg, sizeof(msg));
    if (r < 0) {
        errbuf_t buf;
        fprintf(stderr, "ellsi_register: write failed (%s)\n", ERRNO_STR(buf));
        return r;
    }
    return ellsi_check(state->sock);
}

static int ellsi_set_baudrate(ellsi_state_t *state, int baudrate) {
    struct msg {
        ellsiHeader header;
        uint32_t payload;
    } __attribute__((packed)) msg;
    int r;
    uint32_t code = -1;

    memset(&msg, 0, sizeof(msg));
    msg.header.magic = htonl(ELLSI_MAGIC);
    if (baudrate) {
        code = ellsi_from_baudrate(baudrate);
        if (code != -1) {
            msg.header.command = htonl(ELLSI_CMD_CTRL);
            msg.header.subcommand = htonl(ELLSI_IOCTL_CAN_SET_BAUDRATE);
            msg.header.payloadLen = htonl(sizeof(msg.payload));
            msg.payload = htonl(code);
        } else {
            fprintf(stderr, "ignoring invalid baudrate %d\n", baudrate);
        }
    }
    debug("ellsi_set_baudrate: send magic %#x cmd %#x subcmd %#x baudrate_code %#x\n",
        ELLSI_MAGIC, ELLSI_CMD_CTRL, ELLSI_IOCTL_CAN_SET_BAUDRATE, code);
    r = write(state->sock, &msg, sizeof(msg));
    if (r < 0) {
        errbuf_t buf;
        fprintf(stderr, "ellsi_set_baudrate: write failed (%s)\n", ERRNO_STR(buf));
        return r;
    }
    msg.header.command = htonl(ELLSI_CMD_CTRL);
    msg.header.subcommand = htonl(ELLSI_IOCTL_GET_LAST_STATE);
    msg.header.payloadLen = htonl(sizeof(msg.payload));
    r = write(state->sock, &msg, sizeof(msg));
    if (r < 0) {
        errbuf_t buf;
        fprintf(stderr, "ellsi_set_baudrate: write failed (%s)\n", ERRNO_STR(buf));
        return r;
    }
    return ellsi_check(state->sock);
}

static int ellsi_add_can_ids(ellsi_state_t *state) {
    struct msg {
        ellsiHeader header;
        ellsiCanIdRange payload;
    } __attribute__((packed)) msg;
    int r;

    memset(&msg, 0, sizeof(msg));
    msg.header.magic = htonl(ELLSI_MAGIC);
    msg.header.command = htonl(ELLSI_CMD_CTRL);
    msg.header.subcommand = htonl(ELLSI_IOCTL_CAN_ID_ADD);
    msg.header.payloadLen = htonl(sizeof(msg.payload));
    msg.payload.rangeStart = 0;
    msg.payload.rangeEnd = htonl(CAN_SFF_MASK);
    debug("ellsi_add_can_ids: send magic %#x, cmd %#x subcmd %#x\n",
        ELLSI_MAGIC, ELLSI_CMD_CTRL, ELLSI_IOCTL_CAN_ID_ADD);
    r = write(state->sock, &msg, sizeof(msg));
    if (r < 0) {
        errbuf_t buf;
        fprintf(stderr, "ellsi_add_can_ids: write failed (%s)\n", ERRNO_STR(buf));
        return r;
    }
    msg.header.command = htonl(ELLSI_CMD_CTRL);
    msg.header.subcommand = htonl(ELLSI_IOCTL_GET_LAST_STATE);
    msg.header.payloadLen = htonl(sizeof(msg.payload));
    r = write(state->sock, &msg, sizeof(msg));
    if (r < 0) {
        errbuf_t buf;
        fprintf(stderr, "ellsi_add_can_ids: write failed (%s)\n", ERRNO_STR(buf));
        return r;
    }
    return ellsi_check(state->sock);
}

int ellsiConfigPort(int can_port, const char *ip_addr, int ip_port, int baudrate) {
    errbuf_t buf;
    ellsi_state_t *state = new(ellsi_state_t);
    scan_methods_t *methods;

    if (!state) {
        fprintf(stderr, "ellsiConfigPort: out of memory\n");
        return -1;
    }
    /* success, create socket */
    state->sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (state->sock < 0) {
        fprintf(stderr, "ellsiConfigPort: error creating socket (%s)\n", ERRNO_STR(buf));
        free(state);
        return -1;
    }
    /* success, prepare addr with parameters ip_addr and ip_port */
    state->addr.sin_family  = AF_INET;
    if (aToIPAddr(ip_addr, ip_port, &state->addr)<0) {
        fprintf(stderr, "ellsiConfigPort: error converting ip address (%s)\n", ERRNO_STR(buf));
        free(state);
        return -1;
    }
    /* success, connect socket */
    if (connect(state->sock, (struct sockaddr *)&state->addr, sizeof(struct sockaddr_in)) < 0) {
        fprintf(stderr, "ellsiConfigPort: error in socket connect (%s)\n", ERRNO_STR(buf));
        free(state);
        return -1;
    }
    /* success, allocate methods */
    methods = new(scan_methods_t);
    if (!methods) {
        fprintf(stderr, "ellsiConfigPort: out of memory\n");
        free(state);
        return -1;
    }
    /* success, fill methods struct */
    methods->write = ellsi_write;
    methods->read = ellsi_read;
    methods->dump = ellsi_dump;
    /* initialize protocol */
    if (ellsi_register(state)) {
        fprintf(stderr, "ellsiConfigPort: error in ellsi_register\n");
        free(state);
        return -1;
    }
    /* configure baudrate */
    if (ellsi_set_baudrate(state, baudrate)) {
        fprintf(stderr, "ellsiConfigPort: error in ellsi_set_baudrate\n");
        free(state);
        return -1;
    }
    /* enable all CAN ids */
    if (ellsi_add_can_ids(state)) {
        fprintf(stderr, "ellsiConfigPort: error in ellsi_add_can_ids\n");
        free(state);
        return -1;
    }
    /* install driver and initialize sci port */
    return scan_config_port(can_port, state, methods);
}