ellsi.c 15 KB
Newer Older
1
2
3
4
5
6
7
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>

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

10
#include "osiStdInt.h"
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
#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;
42
43
44
} __attribute__((packed)) ellsiHeader;

STATIC_ASSERT(sizeof(ellsiHeader)==13*4);
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

#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];
88
} __attribute__((packed)) ellsiExtRegistration;
89
90
91
92
93
94
95
96
97

/* 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;
98
99
100
} __attribute__((packed)) ellsiCanTelegram;

STATIC_ASSERT(sizeof(ellsiCanTelegram)==6*4);
101
102
103
104
105
106
107
108
109
110
111
112

#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];
113
} __attribute__((packed)) ellsiLastState;
114
115
116
117
118

/* 2.6.8.1 ELLSI_IOCTL_CAN_ID_ADD/DELETE */
typedef struct {
    uint32_t    rangeStart;
    uint32_t    rangeEnd;
119
} __attribute__((packed)) ellsiCanIdRange;
120
121
122
123
124
125
126
127
128
129
130

/* 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;
131
} __attribute__((packed)) ellsiCanIfStatus;
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183

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];
184
    } __attribute__((packed)) msg;
185
186
187
188
189
190
191
192
193
194
195
    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;
196
197
    } __attribute__((packed)) msg;
    STATIC_ASSERT(sizeof(msg)==19*4);
198
199
200
201
202
203
204
205
206
207
208
209
210
    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;
211
    memcpy(msg.payload.data, frame->data, CAN_MAX_DLC);
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
    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) {
229
230
            errbuf_t buf;
            fprintf(stderr, "ellsi_read: recv failed (%s)\n", ERRNO_STR(buf));
231
232
233
            return -1;
        }
        if (state->msg.header.magic != htonl(ELLSI_MAGIC)) {
234
            fprintf(stderr, "ellsi_read: bad magic\n");
235
236
237
238
239
240
241
242
243
244
245
246
247
248
            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)) {
249
250
                errbuf_t buf;
                fprintf(stderr, "ellsi_read: reply to heartbeat failed (%s)\n", ERRNO_STR(buf));
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
            }
            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) {
268
        fprintf(stderr, "ellsi_read: ignored confirmation (TXDONE)\n");
269
270
271
272
273
274
        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;
275
    memcpy(frame->data, this->data, CAN_MAX_DLC);
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
    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;
294
    } __attribute__((packed)) ack;
295
296
297
298
299
    int r;

    memset(&ack, 0, sizeof(ack));
    r = recv(sock, &ack, sizeof(ack), MSG_WAITALL);
    if (r < 0) {
300
        fprintf(stderr, "ellsi_check: recv failed\n"); return r;
301
    }
302
    debug("ellsi_check: got back magic %#x cmd %#x subcmd %#x payloadLen %d\n",
303
304
305
306
307
308
309
310
311
312
313
314
315
        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)
    {
316
        debug("ellsi_check: last cmd %#x subcmd %#x state %#x\n",
317
318
319
320
            ntohl(ack.payload.lastCommand),
            ntohl(ack.payload.lastSubcommand),
            ntohl(ack.payload.lastState));
    } else {
321
        return -1;
322
    }
323
    return ack.payload.lastState;
324
325
326
327
328
}

static int ellsi_register(ellsi_state_t *state) {
    struct msg {
        ellsiHeader header;
329
    } __attribute__((packed)) msg;
330
331
332
333
334
335
336
337
338
339
    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) {
340
341
342
        errbuf_t buf;
        fprintf(stderr, "ellsi_register: write failed (%s)\n", ERRNO_STR(buf));
        return r;
343
344
345
346
347
348
349
350
    }
    return ellsi_check(state->sock);
}

static int ellsi_set_baudrate(ellsi_state_t *state, int baudrate) {
    struct msg {
        ellsiHeader header;
        uint32_t payload;
351
    } __attribute__((packed)) msg;
352
    int r;
353
    uint32_t code = -1;
354
355
356
357

    memset(&msg, 0, sizeof(msg));
    msg.header.magic = htonl(ELLSI_MAGIC);
    if (baudrate) {
358
        code = ellsi_from_baudrate(baudrate);
359
360
361
362
363
364
        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 {
365
            fprintf(stderr, "ignoring invalid baudrate %d\n", baudrate);
366
367
        }
    }
368
369
370
371
    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) {
372
373
374
        errbuf_t buf;
        fprintf(stderr, "ellsi_set_baudrate: write failed (%s)\n", ERRNO_STR(buf));
        return r;
375
376
377
378
    }
    msg.header.command = htonl(ELLSI_CMD_CTRL);
    msg.header.subcommand = htonl(ELLSI_IOCTL_GET_LAST_STATE);
    msg.header.payloadLen = htonl(sizeof(msg.payload));
379
380
    r = write(state->sock, &msg, sizeof(msg));
    if (r < 0) {
381
382
383
        errbuf_t buf;
        fprintf(stderr, "ellsi_set_baudrate: write failed (%s)\n", ERRNO_STR(buf));
        return r;
384
385
386
387
388
389
390
391
    }
    return ellsi_check(state->sock);
}

static int ellsi_add_can_ids(ellsi_state_t *state) {
    struct msg {
        ellsiHeader header;
        ellsiCanIdRange payload;
392
    } __attribute__((packed)) msg;
393
394
395
396
397
398
399
400
401
402
403
404
405
    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) {
406
407
408
        errbuf_t buf;
        fprintf(stderr, "ellsi_add_can_ids: write failed (%s)\n", ERRNO_STR(buf));
        return r;
409
    }
410
411
412
413
414
    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) {
415
416
417
        errbuf_t buf;
        fprintf(stderr, "ellsi_add_can_ids: write failed (%s)\n", ERRNO_STR(buf));
        return r;
418
    }
419
420
421
422
423
424
    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);
425
    scan_methods_t *methods;
426
427
428
429
430
431
432
433
434
435
436
437
438
439

    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;
440
    if (aToIPAddr(ip_addr, ip_port, &state->addr)<0) {
441
442
443
444
445
446
447
448
449
450
451
        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 */
452
    methods = new(scan_methods_t);
453
454
455
456
457
458
459
460
461
462
    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 */
463
464
465
466
467
    if (ellsi_register(state)) {
        fprintf(stderr, "ellsiConfigPort: error in ellsi_register\n");
        free(state);
        return -1;
    }
468
    /* configure baudrate */
469
470
471
472
473
    if (ellsi_set_baudrate(state, baudrate)) {
        fprintf(stderr, "ellsiConfigPort: error in ellsi_set_baudrate\n");
        free(state);
        return -1;
    }
474
    /* enable all CAN ids */
475
476
477
478
479
    if (ellsi_add_can_ids(state)) {
        fprintf(stderr, "ellsiConfigPort: error in ellsi_add_can_ids\n");
        free(state);
        return -1;
    }
480
481
482
    /* install driver and initialize sci port */
    return scan_config_port(can_port, state, methods);
}