Project

General

Profile

Repository access links

This URL has Read-Only access
Download (9 KB) Statistics
| Branch: | Tag: | Revision:

mikrokopter-genom3 / codels / tty.c @ c71ca2b5

1
/*
2
 * Copyright (c) 2015-2017 LAAS/CNRS
3
 * All rights reserved.
4
 *
5
 * Redistribution and use  in source  and binary  forms,  with or without
6
 * modification, are permitted provided that the following conditions are
7
 * met:
8
 *
9
 *   1. Redistributions of  source  code must retain the  above copyright
10
 *      notice and this list of conditions.
11
 *   2. Redistributions in binary form must reproduce the above copyright
12
 *      notice and  this list of  conditions in the  documentation and/or
13
 *      other materials provided with the distribution.
14
 *
15
 *                                      Anthony Mallet on Mon Feb 16 2015
16
 */
17
#include <sys/stat.h>
18
#include <sys/types.h>
19
#include <sys/uio.h>
20

    
21
#include <err.h>
22
#include <errno.h>
23
#include <fcntl.h>
24
#include <poll.h>
25
#include <stdarg.h>
26
#include <stdio.h>
27
#include <termios.h>
28
#include <unistd.h>
29

    
30
#ifdef HAVE_LOW_LATENCY_IOCTL
31
# include <sys/ioctl.h>
32
# include <linux/serial.h> /* for TIOCGSERIAL and ASYNC_LOW_LATENCY */
33
#endif
34

    
35
#ifdef __linux__
36
# include <libudev.h>
37
#endif
38

    
39
#include "codels.h"
40

    
41

    
42
/* --- mk_open_tty --------------------------------------------------------- */
43

    
44
static const char *	usb_serial_to_tty(const char *serial);
45

    
46
/* Open a serial port, configure to the given baud rate */
47
int
48
mk_open_tty(const char *device, uint32_t speed)
49
{
50
  const char *path;
51
  struct termios t;
52
  speed_t baud;
53
  int fd;
54

    
55
  /* select baud rate */
56
#ifndef B57600
57
# define B57600 57600U
58
#endif
59
#ifndef B115200
60
# define B115200 115200U
61
#endif
62
#ifndef B500000
63
# define B500000 500000U
64
#endif
65
#ifndef B2000000
66
# define B2000000 2000000U
67
#endif
68
  switch(speed) {
69
    case 57600:		baud = B57600; break;
70
    case 115200:	baud = B115200; break;
71
    case 500000:	baud = B500000; break;
72
    case 2000000:	baud = B2000000; break;
73

    
74
    default: errno = EINVAL; return -1;
75
  }
76

    
77
  /* try to match a serial id first */
78
  path = usb_serial_to_tty(device);
79
  if (path) device = path;
80

    
81
  /* open non-blocking */
82
  fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
83
  if (fd < 0) return fd;
84
  if (!isatty(fd)) {
85
    errno = ENOTTY;
86
    return -1;
87
  }
88

    
89
  /* configure line discipline */
90
  if (tcgetattr(fd, &t)) return -1;
91

    
92
  t.c_iflag = IGNBRK;
93
  t.c_oflag = 0;
94
  t.c_lflag = 0;
95
  t.c_cflag = CS8 | CREAD | CLOCAL;
96
  t.c_cc[VMIN] = 0;
97
  t.c_cc[VTIME] = 0;
98

    
99
  if (cfsetospeed(&t, baud)) return -1;
100
  if (cfsetispeed(&t, baud)) return -1;
101

    
102
  if (tcsetattr(fd, TCSANOW, &t)) return -1;
103

    
104
  /* discard any pending data */
105
  tcflush(fd, TCIOFLUSH);
106

    
107
#ifdef HAVE_LOW_LATENCY_IOCTL
108
  /* Linux: enable low latency mode
109
   * see /sys/bus/usb-serial/devices/.../latency_timer
110
   */
111
  {
112
    struct serial_struct s;
113
    int e;
114

    
115
    if (ioctl(fd, TIOCGSERIAL, &s)) {
116
      e = errno;
117
      close(fd);
118
      return e;
119
    }
120

    
121
    if (!(s.flags & ASYNC_LOW_LATENCY)) {
122
      s.flags |= ASYNC_LOW_LATENCY;
123
      if (ioctl(fd, TIOCSSERIAL, &s)) {
124
        e = errno;
125
        close(fd);
126
        return e;
127
      }
128
    }
129
  }
130
#endif
131

    
132
  return fd;
133
}
134

    
135

    
136
/* --- usb_serial_to_tty --------------------------------------------------- */
137

    
138
/* Return a tty device matching the "serial" string */
139

    
140
static const char *
141
usb_serial_to_tty(const char *serial)
142
{
143
#ifdef __linux__
144
  struct udev *udev;
145
  struct udev_enumerate *scan = NULL;
146
  struct udev_list_entry *ttys, *tty;
147
  struct udev_device *dev = NULL, *usb;
148
  const char *path = NULL;
149

    
150
  udev = udev_new();
151
  if (!udev) return NULL;
152

    
153
  /* iterate over tty devices */
154
  scan = udev_enumerate_new(udev);
155
  if (udev_enumerate_add_match_subsystem(scan, "tty")) goto done;
156
  if (udev_enumerate_scan_devices(scan)) goto done;
157

    
158
  ttys = udev_enumerate_get_list_entry(scan);
159
  udev_list_entry_foreach(tty, ttys) {
160
    const char *sysfs, *userial;
161

    
162
    /* get sysfs entry for the device and create a corresponding udev_device */
163
    if (dev) udev_device_unref(dev);
164
    sysfs = udev_list_entry_get_name(tty);
165
    dev = udev_device_new_from_syspath(udev, sysfs);
166

    
167
    /* get the USB device, it any */
168
    usb = udev_device_get_parent_with_subsystem_devtype(
169
      dev, "usb", "usb_device");
170
    if (!usb) continue;
171

    
172
    userial = udev_device_get_sysattr_value(usb, "serial");
173
    if (!userial || strcmp(userial, serial)) continue;
174

    
175
    /* got a match, return the tty path */
176
    path = strdup(udev_device_get_devnode(dev)); /* this will leak ... */
177
    break;
178
  }
179
  if (dev) udev_device_unref(dev);
180

    
181
done:
182
  if (scan) udev_enumerate_unref(scan);
183
  if (udev) udev_unref(udev);
184
  return path;
185

    
186
#else
187
  return NULL; /* if needed, implement this for other OSes */
188
#endif
189
}
190

    
191

    
192
/* --- mk_wait_msg --------------------------------------------------------- */
193

    
194
int
195
mk_wait_msg(const struct mk_channel_s *channels, int n)
196
{
197
  struct pollfd pfds[n];
198
  int i, s;
199

    
200
  for(i = 0; i < n; i++) {
201
    pfds[i].fd = channels[i].fd;
202
    pfds[i].events = POLLIN;
203
  }
204

    
205
  s = poll(pfds, n, 500/*ms*/);
206

    
207
  for(i = 0; i < n; i++) {
208
    if (pfds[i].revents & POLLHUP) {
209
      close(pfds[i].fd);
210

    
211
      /* cheating with const. Oh well... */
212
      ((struct mk_channel_s *)channels)[i].fd = -1;
213
      warnx("disconnected from %s", channels[i].path);
214
    }
215
  }
216

    
217
  return s;
218
}
219

    
220

    
221
/* --- mk_recv_msg --------------------------------------------------------- */
222

    
223
/* returns: 0: timeout/incomplete, -1: error, 1: complete msg */
224

    
225
int
226
mk_recv_msg(struct mk_channel_s *chan, bool block)
227
{
228
  struct iovec iov[2];
229
  ssize_t s;
230
  uint8_t c;
231

    
232
  if (chan->fd < 0) return -1;
233

    
234
  do {
235
    /* feed the ring  buffer */
236
    iov[0].iov_base = chan->buf + chan->w;
237
    iov[1].iov_base = chan->buf;
238

    
239
    if (chan->r > chan->w) {
240
      iov[0].iov_len = chan->r - chan->w - 1;
241
      iov[1].iov_len = 0;
242
    } else if (chan->r > 0) {
243
      iov[0].iov_len = sizeof(chan->buf) - chan->w;
244
      iov[1].iov_len = chan->r - 1;
245
    } else {
246
      iov[0].iov_len = sizeof(chan->buf) - chan->w - 1;
247
      iov[1].iov_len = 0;
248
    }
249

    
250
    if (iov[0].iov_len || iov[1].iov_len) {
251
      do {
252
        s = readv(chan->fd, iov, 2);
253
      } while(s < 0 && errno == EINTR);
254

    
255
      if (s < 0)
256
        return -1;
257
      else if (s == 0 && chan->start && block) {
258
        struct pollfd fd = { .fd = chan->fd, .events = POLLIN };
259

    
260
        s = poll(&fd, 1, 500/*ms*/);
261
        if (fd.revents & POLLHUP) return -1;
262
      } else if (s == 0 && chan->r == chan->w)
263
        return 0;
264
      else
265
        chan->w = (chan->w + s) % sizeof(chan->buf);
266
    }
267

    
268
    while(chan->r != chan->w) {
269
      c = chan->buf[chan->r];
270
      chan->r = (chan->r + 1) % sizeof(chan->buf);
271

    
272
      switch(c) {
273
        case '^':
274
        chan->start = true;
275
        chan->escape = false;
276
        chan->len = 0;
277
        break;
278

    
279
        case '$':
280
          if (!chan->start) break;
281

    
282
          chan->start = false;
283
          return 1;
284

    
285
        case '!':
286
          chan->start = false;
287
          break;
288

    
289
        case '\\':
290
          chan->escape = true;
291
          break;
292

    
293
        default:
294
          if (!chan->start) break;
295
          if (chan->len >= sizeof(chan->msg)) {
296
            chan->start = false; break;
297
          }
298

    
299
          if (chan->escape) {
300
            c = ~c; chan->escape = false;
301
          }
302
          chan->msg[chan->len++] = c;
303
          break;
304
      }
305
    }
306
  } while(1);
307

    
308
  return 0;
309
}
310

    
311

    
312
/* --- mk_send_msg --------------------------------------------------------- */
313

    
314
static void	mk_encode(char x, char **buf);
315

    
316
int
317
mk_send_msg(const struct mk_channel_s *chan, const char *fmt, ...)
318
{
319
  va_list ap;
320
  ssize_t s;
321
  char buf[64], *w, *r;
322
  char c;
323

    
324
  if (chan->fd < 0) return -1;
325

    
326
  w = buf;
327

    
328
  va_start(ap, fmt);
329
  *w++ = '^';
330
  while((c = *fmt++)) {
331
    while ((unsigned)(w - buf) > sizeof(buf)-8 /* 8 = worst case (4 bytes
332
                                                * escaped) */) {
333
      do {
334
        s = write(chan->fd, buf, w - buf);
335
      } while (s < 0 && errno == EINTR);
336
      if (s < 0) return -1;
337

    
338
      if (s > 0 && s < w - buf) memmove(buf, buf + s, w - buf - s);
339
      w -= s;
340
    }
341

    
342
    switch(c) {
343
      case '%': {
344
        switch(*fmt++) {
345
          case '1':
346
            mk_encode(va_arg(ap, int/*promotion*/), &w);
347
            break;
348

    
349
          case '2': {
350
            uint16_t x = va_arg(ap, int/*promotion*/);
351
            mk_encode((x >> 8) & 0xff, &w);
352
            mk_encode(x & 0xff, &w);
353
            break;
354
          }
355
          case '@': {
356
            uint16_t *x = va_arg(ap, uint16_t *);
357
            size_t l = va_arg(ap, size_t);
358
            while (l--) {
359
              mk_encode((*x >> 8) & 0xff, &w);
360
              mk_encode(*x & 0xff, &w);
361
              x++;
362
            }
363
            break;
364
          }
365

    
366
          case '4': {
367
            uint32_t x = va_arg(ap, uint32_t);
368
            mk_encode((x >> 24) & 0xff, &w);
369
            mk_encode((x >> 16) & 0xff, &w);
370
            mk_encode((x >> 8) & 0xff, &w);
371
            mk_encode(x & 0xff, &w);
372
            break;
373
          }
374
        }
375
        break;
376
      }
377

    
378
      default:
379
        mk_encode(c, &w);
380
    }
381
  }
382
  *w++ = '$';
383
  va_end(ap);
384

    
385
  r = buf;
386
  while (w > r) {
387
    do {
388
      s = write(chan->fd, r, w - r);
389
    } while (s < 0 && errno == EINTR);
390
    if (s < 0) return -1;
391

    
392
    r += s;
393
  }
394

    
395
  return 0;
396
}
397

    
398
static void
399
mk_encode(char x, char **buf)
400
{
401
  switch (x) {
402
    case '^': case '$': case '\\': case '!':
403
      *(*buf)++ = '\\';
404
      x = ~x;
405
  }
406
  *(*buf)++ = x;
407
}
(8-8/8)