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