summaryrefslogtreecommitdiff
path: root/serial.c
blob: f6270ba7c467368ab499a3ed566d960ff4809477 (plain)
1
2
3
4
5
6
7
8
9
10
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
42
43
44
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
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
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
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
#include <zup.h>

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

#include <fcntl.h>
#include <unistd.h>


// Hamming(7,4) encoding
static unsigned char
encode(unsigned char x)
{
    unsigned char y = 0;
    const unsigned char c[4] = {0x61, 0x52, 0x34, 0x78};

    for (int i = 0; i < 4; ++i)
        y ^= ((x >> i) & 1) * c[i];

    return y;
}

// Hamming(7,4) decoding
static unsigned char
decode(unsigned char x)
{
    unsigned char p = 0;
    const unsigned char r[7] = {6, 5, 3, 7, 1, 2, 4};

    for (int i = 0; i < 7; ++i)
        p ^= ((x >> i) & 1) * r[i];

    // Attempt correcting simple error
    if (p) {
        size_t n = 0;
        for (size_t i = 0; i < n; ++i) {
            if (r[i] == x)
                n = i;
        }
        x ^= (1 << n);
    }

    return x & 0x0F;
}

static void
encode_buf(size_t slen, const void *_src, size_t dlen, void *_dest)
{
    const uint8_t *src = _src;
    uint8_t *dest = _dest;

    for (size_t i = 0; i < slen && i * 2 < dlen; ++i) {
        dest[2 * i] = encode(src[i] & 0x0F);
        dest[2 * i + 1] = encode((src[i] >> 4) & 0x0F);
    }
}

static void
decode_buf(size_t slen, const void *_src, size_t dlen, void *_dest)
{
    const uint8_t *src = _src;
    uint8_t *dest = _dest;

    for (size_t i = 0; i < dlen && i * 2 < slen; ++i) {
        dest[i] = decode(src[2 * i]);
        dest[i] |= (decode(src[2 * i + 1]) << 4);
    }
}

// Ensure count bytes are read unless read times out MAX_ATTEMPTS times
static int
e_read(int fd, void *buf, size_t count)
{
    ssize_t ret;
    ssize_t nread = 0;
    uint8_t *p = buf;

    for (int n = 0; n < MAX_ATTEMPTS && nread < count; ++n) {
        while ((ret = read(fd, &p[nread], count - nread)) != 0) {
            if (ret < 0)
                goto error;
            else
                nread += ret;
        }
    }

    return (count == nread) ? 0 : ERR_TIMEOUT;
error:
    perror("e_read failed");
    return -1;
}

// Ensure count bytes are written
static int
e_write(int fd, const void *buf, size_t count)
{
    ssize_t ret = 0;
    ssize_t nwrite = 0;
    const uint8_t *p = buf;

    while (nwrite < count) {
        ret = write(fd, &p[nwrite], count - nwrite);
        if (ret < 0)
            goto error;
        nwrite += ret;
    }

    return 0;
error:
    perror("e_write failed!");
    return ERR_WRITE;
}

static int
read_ack(int fd)
{
    int err;
    uint8_t ack;
    uint8_t buf[2];
    if ((err = e_read(fd, buf, sizeof(buf)))) {
        msg(1, "read_ack failed\n");
        return err;
    }
    decode_buf(sizeof(buf), buf, sizeof(ack), &ack);
    msg(1, "read_ack success\n");
    return ack;
}

static int
write_ack(int fd, uint8_t ack)
{
    int err;
    uint8_t buf[2];
    encode_buf(sizeof(ack), &ack, sizeof(buf), buf);
    if ((err = e_write(fd, buf, sizeof(buf)))) {
        msg(1, "write_ack failed\n");
        return err;
    }
    tcdrain(fd);
    msg(1, "write_ack success\n");
    return 0;
}

static int
write_header(int fd, const struct header *header)
{
    int err;
    uint8_t buf[2 * sizeof(*header)];
    encode_buf(sizeof(*header), header, sizeof(buf), buf);

    msg(1, "write_header\n");

    for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i) {
        msg(1, "  attempt [%d/%d]\n", i + 1, MAX_TRANS_ATTEMPTS);

        if ((err = e_write(fd, buf, sizeof(buf))))
            goto error;
        tcdrain(fd);

        err = read_ack(fd);

        if (err == ACK) {
            msg(1, "  write_header success\n");
            return 0;
        }

        if (err != NACK)
            goto error;
    }

    err = ERR_NACK;

error:
    msg(1, "  write_header failed\n");
    return err;
}

static int
read_buf(int fd, size_t len, void *_buf)
{
    int err;
    uint8_t *buf = _buf;
    uint8_t ebuf[2 * MAX_PACKET_SIZE];

    uint16_t checksum = 0;
    uint8_t echecksum[4];

    msg(1, "read_buf\n");

    for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i) {
        msg(1, "  attempt [%d/%d]\n", i + 1, MAX_TRANS_ATTEMPTS);

        if ((err = e_read(fd, echecksum, sizeof(echecksum)))
            || (err = e_read(fd, ebuf, 2 * len)))
            goto error;

        decode_buf(sizeof(echecksum), echecksum, sizeof(checksum), &checksum);
        decode_buf(2 * len, ebuf, len, buf);

        if (checksum == crc16(buf, len)) {
            write_ack(fd, ACK);
            msg(1, "  read_buf success\n");
            return 0;
        } else {
            write_ack(fd, NACK);
        }
    }

    err = ERR_NACK;

error:
    msg(1, "  read_buf failed\n");
    return err;
}

static int
write_buf(int fd, size_t len, const void *_buf)
{
    int err;
    const uint8_t *buf = _buf;
    uint8_t ebuf[2 * MAX_PACKET_SIZE];

    uint16_t checksum = crc16(buf, len);
    uint8_t echecksum[4];

    encode_buf(sizeof(checksum), &checksum, LEN(echecksum), echecksum);
    encode_buf(len, buf, sizeof(ebuf), ebuf);

    msg(1, "write_buf\n");

    int ack;
    for (int i = 0; i < MAX_TRANS_ATTEMPTS; ++i) {
        msg(1, "  attempt [%d/%d]\n", i + 1, MAX_TRANS_ATTEMPTS);

        if ((err = e_write(fd, echecksum, LEN(echecksum)))
            || (err = e_write(fd, ebuf, 2 * len)))
            goto error;

        tcdrain(fd);

        err = read_ack(fd);

        if (err == ACK) {
            msg(1, "  write_buf success\n");
            return 0;
        }

        if (err != NACK)
            goto error;
    }

error:
    msg(1, "  write_buf failed\n");
    return ack < 0 ? ack : ERR_NACK;
}

// Sends a boot command
// returns 0 on success
int
z_boot(int fd, uint8_t bank, uint16_t address)
{
    struct header header = {
        .type = CMD_BOOT,
        .bank = bank,
        .address = address,
        .length = 0,
        .checksum = 0
    };

    header.checksum = crc16(&header, sizeof(header));

    int err;
    tcflush(fd, TCIOFLUSH);
    if ((err = write_header(fd, &header)))
        tcflush(fd, TCIOFLUSH);

    return err;
}

// Sends a single CMD_READ header and reads its response
// Does no splitting
// returns 0 on success
int
z_read(int fd, uint8_t bank, uint16_t address, uint16_t length, uint8_t *buf)
{
    assert(length <= MAX_PACKET_SIZE);

    struct header header = {
        .type = CMD_READ,
        .bank = bank,
        .address = address,
        .length = length,
        .checksum = 0
    };

    header.checksum = crc16(&header, sizeof(header));

    int err;
    tcflush(fd, TCIOFLUSH);
    if ((err = write_header(fd, &header))
        || (err = read_buf(fd, length, buf))) {
        tcflush(fd, TCIOFLUSH);
        return err;
    }

    return 0;
}

// Sends a single CMD_WRITE header and packet
// Does no splitting
// returns 0 on success
int
z_write(int fd, uint8_t bank, uint16_t address, uint16_t length,
        const uint8_t *buf)
{
    assert(length <= MAX_PACKET_SIZE);

    struct header header = {
        .type = CMD_WRITE,
        .bank = bank,
        .address = address,
        .length = length,
        .checksum = 0
    };

    header.checksum = crc16(&header, sizeof(header));

    int err;
    tcflush(fd, TCIOFLUSH);
    if ((err = write_header(fd, &header))
        || (err = write_buf(fd, length, buf))) {
        tcflush(fd, TCIOFLUSH);
        return err;
    }

    return 0;
}

// Sends a single CMD_ECHO header, packet and reads the response
// Does no splitting
// returns 0 on success
int
z_echo(int fd, uint16_t length, uint8_t *buf)
{
    assert(length <= MAX_PACKET_SIZE);

    struct header header = {
        .type = CMD_ECHO,
        .bank = 0,
        .address = 0,
        .length = length,
        .checksum = 0
    };

    header.checksum = crc16(&header, sizeof(header));

    int err = 0;
    tcflush(fd, TCIOFLUSH);
    if ((err = write_header(fd, &header))
        || (err = write_buf(fd, length, buf))
        || (err = read_buf(fd, length, buf))) {
        tcflush(fd, TCIOFLUSH);
        return err;
    }

    return 0;
}

int
open_tty(const char *port, int baud)
{
    int fd = open(port, O_RDWR);

    if (fd < 0) {
        fprintf(stderr, "Error: File '%s' could not be opened\n", port);
        exit(EXIT_FAILURE);
    }

    struct termios term;
    tcgetattr(fd, &term);

    cfmakeraw(&term);
    // read with 1s timeout
    term.c_cc[VTIME] = 5;
    term.c_cc[VMIN] = 0;
    cfsetspeed(&term, baud);

    tcsetattr(fd, TCSANOW, &term);
    tcflush(fd, TCIOFLUSH);

    return fd;
}