comedi.c 15.6 KB
Newer Older
Anders Blomdell's avatar
Anders Blomdell committed
1
#include <moberg_config.h>
Anders Blomdell's avatar
Anders Blomdell committed
2
3
#include <moberg_parser.h>
#include <moberg_module.h>
Anders Blomdell's avatar
Anders Blomdell committed
4
#include <moberg_device.h>
5
#include <stdio.h>
Anders Blomdell's avatar
Anders Blomdell committed
6
7
#include <stdlib.h>
#include <string.h>
Anders Blomdell's avatar
Anders Blomdell committed
8
#include <comedilib.h>
Anders Blomdell's avatar
Anders Blomdell committed
9

Anders Blomdell's avatar
Anders Blomdell committed
10
11
12
13
typedef enum moberg_parser_token_kind kind_t;
typedef struct moberg_parser_token token_t;
typedef struct moberg_parser_ident ident_t;
typedef struct moberg_parser_context context_t;
Anders Blomdell's avatar
Anders Blomdell committed
14

Anders Blomdell's avatar
Anders Blomdell committed
15
struct moberg_device_context {
Anders Blomdell's avatar
Anders Blomdell committed
16
  struct moberg *moberg;
Anders Blomdell's avatar
Anders Blomdell committed
17
18
19
  int (*dlclose)(void *dlhandle);
  void *dlhandle;
  int use_count;
20
  char *name;
Anders Blomdell's avatar
Anders Blomdell committed
21
22
23
24
  struct {
    int count;
    comedi_t *handle;
  } comedi;
25
26
27
28
29
30
  struct idstr {
    struct idstr *next;
    struct idstr *prev;
    kind_t kind;
    char *value;
  } modprobe_list, config_list;
Anders Blomdell's avatar
Anders Blomdell committed
31
32
};

Anders Blomdell's avatar
Anders Blomdell committed
33
34
35
36
struct moberg_channel_context {
  void *to_free; /* To be free'd when use_count goes to zero */
  struct moberg_device_context *device;
  int use_count;
Anders Blomdell's avatar
Anders Blomdell committed
37
38
39
40
41
42
43
44
45
  struct channel_descriptor {
    int subdevice;
    int subchannel;
    int route;
    lsampl_t maxdata;
    double min;
    double max;
    double delta;
  } descriptor;
Anders Blomdell's avatar
Anders Blomdell committed
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
};

struct moberg_channel_analog_in {
  struct moberg_channel channel;
  struct moberg_channel_context channel_context;
};

struct moberg_channel_analog_out {
  struct moberg_channel channel;
  struct moberg_channel_context channel_context;
};

struct moberg_channel_digital_in {
  struct moberg_channel channel;
  struct moberg_channel_context channel_context;
};

struct moberg_channel_digital_out {
  struct moberg_channel channel;
  struct moberg_channel_context channel_context;
};

struct moberg_channel_encoder_in {
  struct moberg_channel channel;
  struct moberg_channel_context channel_context;
};

Anders Blomdell's avatar
Anders Blomdell committed
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
static int analog_in_read(struct moberg_channel_analog_in *analog_in,
                          double *value)
{
  struct channel_descriptor descriptor = analog_in->channel_context.descriptor;
  lsampl_t data;
  comedi_data_read(analog_in->channel_context.device->comedi.handle,
                   descriptor.subdevice,
                   descriptor.subchannel,
                   0, 0, &data);
        
  *value = descriptor.min + data * descriptor.delta;
  fprintf(stderr, "Data: %d %f\n", data, *value);
  return 1;
}

static struct moberg_device_context *new_context(struct moberg *moberg,
                                                 int (*dlclose)(void *dlhandle),
                                                 void *dlhandle)
Anders Blomdell's avatar
Anders Blomdell committed
91
92
93
94
{
  struct moberg_device_context *result = malloc(sizeof(*result));
  if (result) {
    memset(result, 0, sizeof(*result));
Anders Blomdell's avatar
Anders Blomdell committed
95
    result->moberg = moberg;
Anders Blomdell's avatar
Anders Blomdell committed
96
97
    result->dlclose = dlclose;
    result->dlhandle = dlhandle;
98
99
100
101
    result->modprobe_list.next = &result->modprobe_list;
    result->modprobe_list.prev = &result->modprobe_list;
    result->config_list.next = &result->config_list;
    result->config_list.prev = &result->config_list;
Anders Blomdell's avatar
Anders Blomdell committed
102
103
104
105
  }
  return result;
}

Anders Blomdell's avatar
Anders Blomdell committed
106
static int device_up(struct moberg_device_context *device)
Anders Blomdell's avatar
Anders Blomdell committed
107
{
Anders Blomdell's avatar
Anders Blomdell committed
108
109
  device->use_count++;
  return device->use_count;
Anders Blomdell's avatar
Anders Blomdell committed
110
111
}

Anders Blomdell's avatar
Anders Blomdell committed
112
static int device_down(struct moberg_device_context *device)
Anders Blomdell's avatar
Anders Blomdell committed
113
{
Anders Blomdell's avatar
Anders Blomdell committed
114
115
116
117
118
119
  device->use_count--;
  int result = device->use_count;
  if (device->use_count <= 0) {
    moberg_deferred_action(device->moberg,
                           device->dlclose, device->dlhandle);
    free(device->name);
120
121
    struct idstr *e;
    
Anders Blomdell's avatar
Anders Blomdell committed
122
123
    e = device->modprobe_list.next;
    while (e != &device->modprobe_list) {
124
125
126
127
128
      struct idstr *next = e->next;
      free(e->value);
      free(e);
      e = next;
    }
Anders Blomdell's avatar
Anders Blomdell committed
129
130
    e = device->config_list.next;
    while (e != &device->config_list) {
131
132
133
134
135
      struct idstr *next = e->next;
      free(e->value);
      free(e);
      e = next;
    }
Anders Blomdell's avatar
Anders Blomdell committed
136
    free(device);
Anders Blomdell's avatar
Anders Blomdell committed
137
138
139
140
141
    return 0;
  }
  return result;
}

Anders Blomdell's avatar
Anders Blomdell committed
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
static int device_open(struct moberg_device_context *device)
{
  if (device->comedi.count == 0) {
    device->comedi.handle = comedi_open(device->name);
    if (device->comedi.handle == NULL) {
      goto err;
    }
  }
  device->comedi.count++;
  return 1;
err:
  fprintf(stderr, "Failed to open %s\n", device->name);
  return 0;
}

static int device_close(struct moberg_device_context *device)
{
  device->comedi.count--;
  if (device->comedi.count == 0) {
    comedi_close(device->comedi.handle);
  }

  return 1;
}

Anders Blomdell's avatar
Anders Blomdell committed
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
static int channel_up(struct moberg_channel *channel)
{
  device_up(channel->context->device);
  channel->context->use_count++;
  return channel->context->use_count;
}

static int channel_down(struct moberg_channel *channel)
{
  device_down(channel->context->device);
  channel->context->use_count--;
  if (channel->context->use_count <= 0) {
    free(channel->context->to_free);
    return 0;
  }
  return channel->context->use_count;
}

Anders Blomdell's avatar
Anders Blomdell committed
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
static int channel_open(struct moberg_channel *channel)
{
  channel_up(channel);
  if (! device_open(channel->context->device)) { goto err; }
  fprintf(stderr, "Open %s[%d][%d]\n",
          channel->context->device->name, 
          channel->context->descriptor.subdevice, 
          channel->context->descriptor.subchannel);
  lsampl_t maxdata;
  comedi_range *range;
          
  maxdata = comedi_get_maxdata(channel->context->device->comedi.handle,
                               channel->context->descriptor.subdevice,
                               channel->context->descriptor.subchannel);
  range = comedi_get_range(channel->context->device->comedi.handle,
                           channel->context->descriptor.subdevice,
                           channel->context->descriptor.subchannel,
                           0);

  if (! maxdata) {
    fprintf(stderr, "Failed to get maxdata for %s[%d][%d]\n",
            channel->context->device->name, 
            channel->context->descriptor.subdevice, 
            channel->context->descriptor.subchannel);
    goto err;
  }
  if (! range) {
    fprintf(stderr, "Failed to get range for %s[%d][%d]\n",
            channel->context->device->name, 
            channel->context->descriptor.subdevice, 
            channel->context->descriptor.subchannel);
    goto err;
  }
  channel->context->descriptor.maxdata = maxdata;
  channel->context->descriptor.min = range->min;
  channel->context->descriptor.max = range->max;
  channel->context->descriptor.delta = (range->max - range->min) / maxdata;
  return 1;
err:
  return 0;
}

static int channel_close(struct moberg_channel *channel)
{
  device_close(channel->context->device);
  channel_down(channel);
  return 1;  
}

234
235
236
237
static void init_channel(struct moberg_channel *channel,
                         void *to_free,
                         struct moberg_channel_context *context,
                         struct moberg_device_context *device,
Anders Blomdell's avatar
Anders Blomdell committed
238
                         struct channel_descriptor descriptor,
239
240
                         enum moberg_channel_kind kind,
                         union moberg_channel_action action)
Anders Blomdell's avatar
Anders Blomdell committed
241
242
243
244
{
  context->to_free = to_free;
  context->device = device;
  context->use_count = 0;
Anders Blomdell's avatar
Anders Blomdell committed
245
  context->descriptor = descriptor;
Anders Blomdell's avatar
Anders Blomdell committed
246
247
248
249
  
  channel->context = context;
  channel->up = channel_up;
  channel->down = channel_down;
Anders Blomdell's avatar
Anders Blomdell committed
250
251
  channel->open = channel_open;
  channel->close = channel_close;
Anders Blomdell's avatar
Anders Blomdell committed
252
253
254
255
  channel->kind = kind;
  channel->action = action;
};

Anders Blomdell's avatar
Anders Blomdell committed
256
257
258
259
static inline int acceptsym(context_t *c,
			   kind_t kind,
			   token_t *token)
{
Anders Blomdell's avatar
Anders Blomdell committed
260
  return moberg_parser_acceptsym(c, kind, token);
Anders Blomdell's avatar
Anders Blomdell committed
261
262
263
264
265
}
  
static inline int acceptkeyword(context_t *c,
				const char *keyword)
{
Anders Blomdell's avatar
Anders Blomdell committed
266
  return moberg_parser_acceptkeyword(c, keyword);
Anders Blomdell's avatar
Anders Blomdell committed
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

static int append_modprobe(struct moberg_device_context *device,
                           token_t token)
{
  struct idstr *modprobe = malloc(sizeof(*modprobe));
  if (! modprobe) { goto err; }
  modprobe->value = strndup(token.u.idstr.value, token.u.idstr.length);
  if (! modprobe->value) { goto free_modprobe; }
  modprobe->prev = device->modprobe_list.prev;
  modprobe->next = modprobe->prev->next;
  modprobe->prev->next = modprobe;
  modprobe->next->prev = modprobe;
  return 1;
free_modprobe:
  free(modprobe);
err:
  return 0;
}
  
static int append_config(struct moberg_device_context *device,
                           token_t token)
{
  struct idstr *config = malloc(sizeof(*config));
  if (! config) { goto err; }
  config->value = strndup(token.u.idstr.value, token.u.idstr.length);
  if (! config->value) { goto free_config; }
  config->prev = device->config_list.prev;
  config->next = config->prev->next;
  config->prev->next = config;
  config->next->prev = config;
  return 1;
free_config:
  free(config);
err:
  return 0;
}
Anders Blomdell's avatar
Anders Blomdell committed
304
  
305
306
static int parse_config(struct moberg_device_context *device,
                        struct moberg_parser_context *c)
307
{
308
  if (! acceptsym(c, tok_LBRACE, NULL)) { goto syntax_err; }
Anders Blomdell's avatar
Anders Blomdell committed
309
  for (;;) {
Anders Blomdell's avatar
Anders Blomdell committed
310
311
    if (acceptsym(c, tok_RBRACE, NULL)) {
	break;
Anders Blomdell's avatar
Anders Blomdell committed
312
    } else if (acceptkeyword(c, "device")) {
Anders Blomdell's avatar
Anders Blomdell committed
313
      token_t name;
314
      if (! acceptsym(c, tok_EQUAL, NULL)) { goto syntax_err; }
Anders Blomdell's avatar
Anders Blomdell committed
315
      if (! acceptsym(c, tok_STRING, &name)) { goto syntax_err; }
316
      if (! acceptsym(c, tok_SEMICOLON, NULL)) { goto syntax_err; }
317
      device->name = strndup(name.u.idstr.value, name.u.idstr.length);
Anders Blomdell's avatar
Anders Blomdell committed
318
    } else if (acceptkeyword(c, "config")) {
319
320
      if (! acceptsym(c, tok_EQUAL, NULL)) { goto syntax_err; }
      if (! acceptsym(c, tok_LBRACKET, NULL)) { goto syntax_err; }
Anders Blomdell's avatar
Anders Blomdell committed
321
      for (;;) {
322
        token_t config;
Anders Blomdell's avatar
Anders Blomdell committed
323
324
	if (acceptsym(c, tok_RBRACKET, NULL)) {
	  break;
325
326
327
	} else if (acceptsym(c, tok_IDENT, &config) ||
		   acceptsym(c, tok_STRING, &config)) {
          append_config(device, config);
Anders Blomdell's avatar
Anders Blomdell committed
328
	} else {
329
	  goto syntax_err;
Anders Blomdell's avatar
Anders Blomdell committed
330
	}
Anders Blomdell's avatar
Anders Blomdell committed
331
      }
332
      if (! acceptsym(c, tok_SEMICOLON, NULL)) { goto syntax_err; }
Anders Blomdell's avatar
Anders Blomdell committed
333
    } else if (acceptkeyword(c, "modprobe")) {
334
335
      if (! acceptsym(c, tok_EQUAL, NULL)) { goto syntax_err; }
      if (! acceptsym(c, tok_LBRACKET, NULL)) { goto syntax_err; }
Anders Blomdell's avatar
Anders Blomdell committed
336
      for (;;) {
337
        token_t modprobe;
Anders Blomdell's avatar
Anders Blomdell committed
338
339
	if (acceptsym(c, tok_RBRACKET, NULL)) {
	  break;
340
341
342
	} else if (acceptsym(c, tok_IDENT, &modprobe) ||
		   acceptsym(c, tok_STRING, &modprobe)) {
          append_modprobe(device, modprobe);
Anders Blomdell's avatar
Anders Blomdell committed
343
	} else {
344
	  goto syntax_err;
Anders Blomdell's avatar
Anders Blomdell committed
345
	}
Anders Blomdell's avatar
Anders Blomdell committed
346
      }
347
      if (! acceptsym(c, tok_SEMICOLON, NULL)) { goto syntax_err; }
Anders Blomdell's avatar
Anders Blomdell committed
348
    } else {
349
      goto syntax_err;
Anders Blomdell's avatar
Anders Blomdell committed
350
    }
351
  }
Anders Blomdell's avatar
Anders Blomdell committed
352
  return 1;
353
syntax_err:
Anders Blomdell's avatar
Anders Blomdell committed
354
  moberg_parser_failed(c, stderr);
Anders Blomdell's avatar
Anders Blomdell committed
355
  return 0;
356
357
}

358
359
360
361
static int parse_map(struct moberg_device_context *device,
                     struct moberg_parser_context *c,
                     enum moberg_channel_kind kind,
                     struct moberg_channel_map *map)
362
{
Anders Blomdell's avatar
Anders Blomdell committed
363
  token_t min, max, route={ .u.integer.value=0 };
364

365
366
367
368
369
370
371
372
373
374
375
  if (! acceptsym(c, tok_LBRACE, NULL)) { goto err; }
  for (;;) {
    token_t subdevice;
    if (! acceptkeyword(c, "subdevice") != 0) {  goto err; }
    if (! acceptsym(c, tok_LBRACKET, NULL)) { goto err; }
    if (! acceptsym(c, tok_INTEGER, &subdevice)) { goto err; }
    if (! acceptsym(c, tok_RBRACKET, NULL)) { goto err; }
    if (acceptkeyword(c, "route")) {
      if (! acceptsym(c, tok_INTEGER, &route)) { goto err; }
    }
    if (! acceptsym(c, tok_LBRACKET, NULL)) { goto err; }
Anders Blomdell's avatar
Anders Blomdell committed
376
    if (! acceptsym(c, tok_INTEGER, &min)) { goto err; }
377
    if (acceptsym(c, tok_COLON, NULL)) { 
Anders Blomdell's avatar
Anders Blomdell committed
378
379
380
381
382
      if (! acceptsym(c, tok_INTEGER, &max)) { goto err; }
    } else {
      max = min;
    }
    for (int i = min.u.integer.value ; i <= max.u.integer.value ; i++) {
Anders Blomdell's avatar
Anders Blomdell committed
383
384
385
386
387
388
389
390
      struct channel_descriptor descriptor = {
        .subdevice=subdevice.u.integer.value,
        .subchannel=i,
        .route=route.u.integer.value,
        .maxdata=0,
        .min=0.0,
        .max=0.0
      };
Anders Blomdell's avatar
Anders Blomdell committed
391
      switch (kind) {
Anders Blomdell's avatar
Anders Blomdell committed
392
393
394
395
396
397
398
399
400
        case chan_ANALOGIN: {
          struct moberg_channel_analog_in *channel =
            malloc(sizeof(*channel));
          
          if (channel) {
            init_channel(&channel->channel,
                         channel,
                         &channel->channel_context,
                         device,
Anders Blomdell's avatar
Anders Blomdell committed
401
                         descriptor,
Anders Blomdell's avatar
Anders Blomdell committed
402
403
404
                         kind,
                         (union moberg_channel_action) {
                           .analog_in.context=channel,
Anders Blomdell's avatar
Anders Blomdell committed
405
                           .analog_in.read=analog_in_read });
Anders Blomdell's avatar
Anders Blomdell committed
406
407
408
409
410
411
412
413
414
415
416
417
            map->map(map->device, &channel->channel);
          }
        } break;
        case chan_ANALOGOUT: {
          struct moberg_channel_analog_out *channel =
            malloc(sizeof(*channel));
          
          if (channel) {
            init_channel(&channel->channel,
                         channel,
                         &channel->channel_context,
                         device,
Anders Blomdell's avatar
Anders Blomdell committed
418
                         descriptor,
Anders Blomdell's avatar
Anders Blomdell committed
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
                         kind,
                         (union moberg_channel_action) {
                           .analog_out.context=channel,
                           .analog_out.write=NULL });
            map->map(map->device, &channel->channel);
          }
        } break;
        case chan_DIGITALIN: {
          struct moberg_channel_digital_in *channel =
            malloc(sizeof(*channel));
          
          if (channel) {
            init_channel(&channel->channel,
                         channel,
                         &channel->channel_context,
                         device,
Anders Blomdell's avatar
Anders Blomdell committed
435
                         descriptor,
Anders Blomdell's avatar
Anders Blomdell committed
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
                         kind,
                         (union moberg_channel_action) {
                           .digital_in.context=channel,
                           .digital_in.read=NULL });
            map->map(map->device, &channel->channel);
          }
        } break;
        case chan_DIGITALOUT: {
          struct moberg_channel_digital_out *channel =
            malloc(sizeof(*channel));
          
          if (channel) {
            init_channel(&channel->channel,
                         channel,
                         &channel->channel_context,
                         device,
Anders Blomdell's avatar
Anders Blomdell committed
452
                         descriptor,
Anders Blomdell's avatar
Anders Blomdell committed
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
                         kind,
                         (union moberg_channel_action) {
                           .digital_out.context=channel,
                           .digital_out.write=NULL });
            map->map(map->device, &channel->channel);
          }
        } break;
        case chan_ENCODERIN: {
          struct moberg_channel_encoder_in *channel =
            malloc(sizeof(*channel));
          
          if (channel) {
            init_channel(&channel->channel,
                         channel,
                         &channel->channel_context,
                         device,
Anders Blomdell's avatar
Anders Blomdell committed
469
                         descriptor,
Anders Blomdell's avatar
Anders Blomdell committed
470
471
472
473
474
475
476
                         kind,
                         (union moberg_channel_action) {
                           .encoder_in.context=channel,
                           .encoder_in.read=NULL });
            map->map(map->device, &channel->channel);
          }
        } break;
Anders Blomdell's avatar
Anders Blomdell committed
477
      }
478
479
480
    }
    if (! acceptsym(c, tok_RBRACKET, NULL)) { goto err; }
    if (! acceptsym(c, tok_COMMA, NULL)) { break; }
Anders Blomdell's avatar
Anders Blomdell committed
481
    
482
  }
483
  if (! acceptsym(c, tok_RBRACE, NULL)) { goto err; }
Anders Blomdell's avatar
Anders Blomdell committed
484
  return 1;
Anders Blomdell's avatar
Anders Blomdell committed
485
err:
Anders Blomdell's avatar
Anders Blomdell committed
486
  moberg_parser_failed(c, stderr);
Anders Blomdell's avatar
Anders Blomdell committed
487
488
489
  return 0;
}

490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
static int start(struct moberg_device_context *device,
                 FILE *f)
{
  for (struct idstr *e = device->modprobe_list.next ;
       e != &device->modprobe_list ;
       e = e->next) {
    fprintf(f, "modprobe %s\n", e->value);
  }
  fprintf(f, "comedi_config %s ", device->name);
  int i = 0;
  for (struct idstr *e = device->config_list.next ;
       e != &device->config_list ;
       e = e->next) {
    switch (i) {
      case 0: fprintf(f, "%s", e->value); break;
      case 1: fprintf(f, " %s", e->value); break;
      default: fprintf(f, ",%s", e->value); break;
    }
    i++;
  }
  fprintf(f, "\n");
  
  return 1;
}

static int stop(struct moberg_device_context *device,
                 FILE *f)
{
  fprintf(f, "comedi_config --remove %s\n", device->name);
  for (struct idstr *e = device->modprobe_list.prev ;
       e != &device->modprobe_list ;
       e = e->prev) {
    fprintf(f, "rmmod %s\n", e->value);
  }
  return 1;
}

Anders Blomdell's avatar
Anders Blomdell committed
527
struct moberg_device_driver moberg_device_driver = {
Anders Blomdell's avatar
Anders Blomdell committed
528
529
530
531
532
  .new=new_context,
  .up=device_up,
  .down=device_down,
  .parse_config=parse_config,
  .parse_map=parse_map,
533
534
  .start=start,
  .stop=stop
535
};