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#include "../comedilib.h"
91#include "../comedidev.h"
92#include <linux/string.h>
93
94
95#define MAX_CHANS 256
96
97#define MODULE_NAME "comedi_bond"
98#ifdef MODULE_LICENSE
99MODULE_LICENSE("GPL");
100#endif
101#ifndef STR
102# define STR1(x) #x
103# define STR(x) STR1(x)
104#endif
105
106static int debug;
107module_param(debug, int, 0644);
108MODULE_PARM_DESC(debug, "If true, print extra cryptic debugging output useful"
109 "only to developers.");
110
111#define LOG_MSG(x...) printk(KERN_INFO MODULE_NAME": "x)
112#define DEBUG(x...) \
113 do { \
114 if (debug) \
115 printk(KERN_DEBUG MODULE_NAME": DEBUG: "x); \
116 } while (0)
117#define WARNING(x...) printk(KERN_WARNING MODULE_NAME ": WARNING: "x)
118#define ERROR(x...) printk(KERN_ERR MODULE_NAME ": INTERNAL ERROR: "x)
119MODULE_AUTHOR("Calin A. Culianu");
120MODULE_DESCRIPTION(MODULE_NAME "A driver for COMEDI to bond multiple COMEDI "
121 "devices together as one. In the words of John Lennon: "
122 "'And the world will live as one...'");
123
124
125
126
127
128
129struct BondingBoard {
130 const char *name;
131};
132
133static const struct BondingBoard bondingBoards[] = {
134 {
135 .name = MODULE_NAME,
136 },
137};
138
139
140
141
142#define thisboard ((const struct BondingBoard *)dev->board_ptr)
143
144struct BondedDevice {
145 void *dev;
146 unsigned minor;
147 unsigned subdev;
148 unsigned subdev_type;
149 unsigned nchans;
150 unsigned chanid_offset;
151
152
153};
154
155
156
157
158struct Private {
159# define MAX_BOARD_NAME 256
160 char name[MAX_BOARD_NAME];
161 struct BondedDevice **devs;
162 unsigned ndevs;
163 struct BondedDevice *chanIdDevMap[MAX_CHANS];
164 unsigned nchans;
165};
166
167
168
169
170
171#define devpriv ((struct Private *)dev->private)
172
173
174
175
176
177
178
179static int bonding_attach(struct comedi_device *dev,
180 struct comedi_devconfig *it);
181static int bonding_detach(struct comedi_device *dev);
182
183static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it);
184static void doDevUnconfig(struct comedi_device *dev);
185
186
187static void *Realloc(const void *ptr, size_t len, size_t old_len);
188
189static struct comedi_driver driver_bonding = {
190 .driver_name = MODULE_NAME,
191 .module = THIS_MODULE,
192 .attach = bonding_attach,
193 .detach = bonding_detach,
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212 .board_name = &bondingBoards[0].name,
213 .offset = sizeof(struct BondingBoard),
214 .num_names = ARRAY_SIZE(bondingBoards),
215};
216
217static int bonding_dio_insn_bits(struct comedi_device *dev,
218 struct comedi_subdevice *s,
219 struct comedi_insn *insn, unsigned int *data);
220static int bonding_dio_insn_config(struct comedi_device *dev,
221 struct comedi_subdevice *s,
222 struct comedi_insn *insn,
223 unsigned int *data);
224
225
226
227
228
229
230
231static int bonding_attach(struct comedi_device *dev,
232 struct comedi_devconfig *it)
233{
234 struct comedi_subdevice *s;
235
236 LOG_MSG("comedi%d\n", dev->minor);
237
238
239
240
241
242 if (alloc_private(dev, sizeof(struct Private)) < 0)
243 return -ENOMEM;
244
245
246
247
248 if (!doDevConfig(dev, it))
249 return -EINVAL;
250
251
252
253
254
255 dev->board_name = devpriv->name;
256
257
258
259
260
261 if (alloc_subdevices(dev, 1) < 0)
262 return -ENOMEM;
263
264 s = dev->subdevices + 0;
265 s->type = COMEDI_SUBD_DIO;
266 s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
267 s->n_chan = devpriv->nchans;
268 s->maxdata = 1;
269 s->range_table = &range_digital;
270 s->insn_bits = bonding_dio_insn_bits;
271 s->insn_config = bonding_dio_insn_config;
272
273 LOG_MSG("attached with %u DIO channels coming from %u different "
274 "subdevices all bonded together. "
275 "John Lennon would be proud!\n",
276 devpriv->nchans, devpriv->ndevs);
277
278 return 1;
279}
280
281
282
283
284
285
286
287
288
289static int bonding_detach(struct comedi_device *dev)
290{
291 LOG_MSG("comedi%d: remove\n", dev->minor);
292 doDevUnconfig(dev);
293 return 0;
294}
295
296
297
298
299
300
301static int bonding_dio_insn_bits(struct comedi_device *dev,
302 struct comedi_subdevice *s,
303 struct comedi_insn *insn, unsigned int *data)
304{
305#define LSAMPL_BITS (sizeof(unsigned int)*8)
306 unsigned nchans = LSAMPL_BITS, num_done = 0, i;
307 if (insn->n != 2)
308 return -EINVAL;
309
310 if (devpriv->nchans < nchans)
311 nchans = devpriv->nchans;
312
313
314
315 for (i = 0; num_done < nchans && i < devpriv->ndevs; ++i) {
316 struct BondedDevice *bdev = devpriv->devs[i];
317
318
319
320
321 unsigned int subdevMask = ((1 << bdev->nchans) - 1);
322 unsigned int writeMask, dataBits;
323
324
325 if (bdev->nchans >= LSAMPL_BITS)
326 subdevMask = (unsigned int)(-1);
327
328 writeMask = (data[0] >> num_done) & subdevMask;
329 dataBits = (data[1] >> num_done) & subdevMask;
330
331
332 if (comedi_dio_bitfield(bdev->dev, bdev->subdev, writeMask,
333 &dataBits) != 2)
334 return -EINVAL;
335
336
337 data[1] &= ~(subdevMask << num_done);
338
339 data[1] |= (dataBits & subdevMask) << num_done;
340
341 s->state = data[1];
342
343 num_done += bdev->nchans;
344 }
345
346 return insn->n;
347}
348
349static int bonding_dio_insn_config(struct comedi_device *dev,
350 struct comedi_subdevice *s,
351 struct comedi_insn *insn, unsigned int *data)
352{
353 int chan = CR_CHAN(insn->chanspec), ret, io_bits = s->io_bits;
354 unsigned int io;
355 struct BondedDevice *bdev;
356
357 if (chan < 0 || chan >= devpriv->nchans)
358 return -EINVAL;
359 bdev = devpriv->chanIdDevMap[chan];
360
361
362
363
364
365 switch (data[0]) {
366 case INSN_CONFIG_DIO_OUTPUT:
367 io = COMEDI_OUTPUT;
368 io_bits |= 1 << chan;
369 break;
370 case INSN_CONFIG_DIO_INPUT:
371 io = COMEDI_INPUT;
372 io_bits &= ~(1 << chan);
373 break;
374 case INSN_CONFIG_DIO_QUERY:
375 data[1] =
376 (io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
377 return insn->n;
378 break;
379 default:
380 return -EINVAL;
381 break;
382 }
383
384 chan -= bdev->chanid_offset;
385 ret = comedi_dio_config(bdev->dev, bdev->subdev, chan, io);
386 if (ret != 1)
387 return -EINVAL;
388
389
390 s->io_bits = io_bits;
391 return insn->n;
392}
393
394static void *Realloc(const void *oldmem, size_t newlen, size_t oldlen)
395{
396 void *newmem = kmalloc(newlen, GFP_KERNEL);
397
398 if (newmem && oldmem)
399 memcpy(newmem, oldmem, min(oldlen, newlen));
400 kfree(oldmem);
401 return newmem;
402}
403
404static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
405{
406 int i;
407 void *devs_opened[COMEDI_NUM_BOARD_MINORS];
408
409 memset(devs_opened, 0, sizeof(devs_opened));
410 devpriv->name[0] = 0;;
411
412
413 for (i = 0; i < COMEDI_NDEVCONFOPTS && (!i || it->options[i]); ++i) {
414 char file[] = "/dev/comediXXXXXX";
415 int minor = it->options[i];
416 void *d;
417 int sdev = -1, nchans, tmp;
418 struct BondedDevice *bdev = NULL;
419
420 if (minor < 0 || minor > COMEDI_NUM_BOARD_MINORS) {
421 ERROR("Minor %d is invalid!\n", minor);
422 return 0;
423 }
424 if (minor == dev->minor) {
425 ERROR("Cannot bond this driver to itself!\n");
426 return 0;
427 }
428 if (devs_opened[minor]) {
429 ERROR("Minor %d specified more than once!\n", minor);
430 return 0;
431 }
432
433 snprintf(file, sizeof(file), "/dev/comedi%u", minor);
434 file[sizeof(file) - 1] = 0;
435
436 d = devs_opened[minor] = comedi_open(file);
437
438 if (!d) {
439 ERROR("Minor %u could not be opened\n", minor);
440 return 0;
441 }
442
443
444 while ((sdev = comedi_find_subdevice_by_type(d, COMEDI_SUBD_DIO,
445 sdev + 1)) > -1) {
446 nchans = comedi_get_n_channels(d, sdev);
447 if (nchans <= 0) {
448 ERROR("comedi_get_n_channels() returned %d "
449 "on minor %u subdev %d!\n",
450 nchans, minor, sdev);
451 return 0;
452 }
453 bdev = kmalloc(sizeof(*bdev), GFP_KERNEL);
454 if (!bdev) {
455 ERROR("Out of memory.\n");
456 return 0;
457 }
458 bdev->dev = d;
459 bdev->minor = minor;
460 bdev->subdev = sdev;
461 bdev->subdev_type = COMEDI_SUBD_DIO;
462 bdev->nchans = nchans;
463 bdev->chanid_offset = devpriv->nchans;
464
465
466 while (nchans--)
467 devpriv->chanIdDevMap[devpriv->nchans++] = bdev;
468
469
470
471
472
473 tmp = devpriv->ndevs * sizeof(bdev);
474 devpriv->devs =
475 Realloc(devpriv->devs,
476 ++devpriv->ndevs * sizeof(bdev), tmp);
477 if (!devpriv->devs) {
478 ERROR("Could not allocate memory. "
479 "Out of memory?");
480 return 0;
481 }
482
483 devpriv->devs[devpriv->ndevs - 1] = bdev;
484 {
485
486 char buf[20];
487 int left =
488 MAX_BOARD_NAME - strlen(devpriv->name) - 1;
489 snprintf(buf, sizeof(buf), "%d:%d ", dev->minor,
490 bdev->subdev);
491 buf[sizeof(buf) - 1] = 0;
492 strncat(devpriv->name, buf, left);
493 }
494
495 }
496 }
497
498 if (!devpriv->nchans) {
499 ERROR("No channels found!\n");
500 return 0;
501 }
502
503 return 1;
504}
505
506static void doDevUnconfig(struct comedi_device *dev)
507{
508 unsigned long devs_closed = 0;
509
510 if (devpriv) {
511 while (devpriv->ndevs-- && devpriv->devs) {
512 struct BondedDevice *bdev;
513
514 bdev = devpriv->devs[devpriv->ndevs];
515 if (!bdev)
516 continue;
517 if (!(devs_closed & (0x1 << bdev->minor))) {
518 comedi_close(bdev->dev);
519 devs_closed |= (0x1 << bdev->minor);
520 }
521 kfree(bdev);
522 }
523 kfree(devpriv->devs);
524 devpriv->devs = NULL;
525 kfree(devpriv);
526 dev->private = NULL;
527 }
528}
529
530static int __init init(void)
531{
532 return comedi_driver_register(&driver_bonding);
533}
534
535static void __exit cleanup(void)
536{
537 comedi_driver_unregister(&driver_bonding);
538}
539
540module_init(init);
541module_exit(cleanup);
542