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
62struct BondedDevice {
63 struct comedi_device *dev;
64 unsigned minor;
65 unsigned subdev;
66 unsigned subdev_type;
67 unsigned nchans;
68 unsigned chanid_offset;
69
70
71};
72
73
74
75
76struct comedi_bond_private {
77# define MAX_BOARD_NAME 256
78 char name[MAX_BOARD_NAME];
79 struct BondedDevice **devs;
80 unsigned ndevs;
81 struct BondedDevice *chanIdDevMap[MAX_CHANS];
82 unsigned nchans;
83};
84
85
86
87
88
89
90static int bonding_dio_insn_bits(struct comedi_device *dev,
91 struct comedi_subdevice *s,
92 struct comedi_insn *insn, unsigned int *data)
93{
94 struct comedi_bond_private *devpriv = dev->private;
95#define LSAMPL_BITS (sizeof(unsigned int)*8)
96 unsigned nchans = LSAMPL_BITS, num_done = 0, i;
97
98 if (devpriv->nchans < nchans)
99 nchans = devpriv->nchans;
100
101
102
103 for (i = 0; num_done < nchans && i < devpriv->ndevs; ++i) {
104 struct BondedDevice *bdev = devpriv->devs[i];
105
106
107
108
109 unsigned int subdevMask = ((1 << bdev->nchans) - 1);
110 unsigned int writeMask, dataBits;
111
112
113 if (bdev->nchans >= LSAMPL_BITS)
114 subdevMask = (unsigned int)(-1);
115
116 writeMask = (data[0] >> num_done) & subdevMask;
117 dataBits = (data[1] >> num_done) & subdevMask;
118
119
120 if (comedi_dio_bitfield(bdev->dev, bdev->subdev, writeMask,
121 &dataBits) != 2)
122 return -EINVAL;
123
124
125 data[1] &= ~(subdevMask << num_done);
126
127 data[1] |= (dataBits & subdevMask) << num_done;
128
129 s->state = data[1];
130
131 num_done += bdev->nchans;
132 }
133
134 return insn->n;
135}
136
137static int bonding_dio_insn_config(struct comedi_device *dev,
138 struct comedi_subdevice *s,
139 struct comedi_insn *insn, unsigned int *data)
140{
141 struct comedi_bond_private *devpriv = dev->private;
142 int chan = CR_CHAN(insn->chanspec), ret, io_bits = s->io_bits;
143 unsigned int io;
144 struct BondedDevice *bdev;
145
146 if (chan < 0 || chan >= devpriv->nchans)
147 return -EINVAL;
148 bdev = devpriv->chanIdDevMap[chan];
149
150
151
152
153
154 switch (data[0]) {
155 case INSN_CONFIG_DIO_OUTPUT:
156 io = COMEDI_OUTPUT;
157 io_bits |= 1 << chan;
158 break;
159 case INSN_CONFIG_DIO_INPUT:
160 io = COMEDI_INPUT;
161 io_bits &= ~(1 << chan);
162 break;
163 case INSN_CONFIG_DIO_QUERY:
164 data[1] =
165 (io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
166 return insn->n;
167 break;
168 default:
169 return -EINVAL;
170 break;
171 }
172
173 chan -= bdev->chanid_offset;
174 ret = comedi_dio_config(bdev->dev, bdev->subdev, chan, io);
175 if (ret != 1)
176 return -EINVAL;
177
178
179 s->io_bits = io_bits;
180 return insn->n;
181}
182
183static void *Realloc(const void *oldmem, size_t newlen, size_t oldlen)
184{
185 void *newmem = kmalloc(newlen, GFP_KERNEL);
186
187 if (newmem && oldmem)
188 memcpy(newmem, oldmem, min(oldlen, newlen));
189 kfree(oldmem);
190 return newmem;
191}
192
193static int doDevConfig(struct comedi_device *dev, struct comedi_devconfig *it)
194{
195 struct comedi_bond_private *devpriv = dev->private;
196 int i;
197 struct comedi_device *devs_opened[COMEDI_NUM_BOARD_MINORS];
198
199 memset(devs_opened, 0, sizeof(devs_opened));
200 devpriv->name[0] = 0;
201
202
203 for (i = 0; i < COMEDI_NDEVCONFOPTS && (!i || it->options[i]); ++i) {
204 char file[] = "/dev/comediXXXXXX";
205 int minor = it->options[i];
206 struct comedi_device *d;
207 int sdev = -1, nchans, tmp;
208 struct BondedDevice *bdev = NULL;
209
210 if (minor < 0 || minor >= COMEDI_NUM_BOARD_MINORS) {
211 dev_err(dev->class_dev,
212 "Minor %d is invalid!\n", minor);
213 return 0;
214 }
215 if (minor == dev->minor) {
216 dev_err(dev->class_dev,
217 "Cannot bond this driver to itself!\n");
218 return 0;
219 }
220 if (devs_opened[minor]) {
221 dev_err(dev->class_dev,
222 "Minor %d specified more than once!\n", minor);
223 return 0;
224 }
225
226 snprintf(file, sizeof(file), "/dev/comedi%u", minor);
227 file[sizeof(file) - 1] = 0;
228
229 d = devs_opened[minor] = comedi_open(file);
230
231 if (!d) {
232 dev_err(dev->class_dev,
233 "Minor %u could not be opened\n", minor);
234 return 0;
235 }
236
237
238 while ((sdev = comedi_find_subdevice_by_type(d, COMEDI_SUBD_DIO,
239 sdev + 1)) > -1) {
240 nchans = comedi_get_n_channels(d, sdev);
241 if (nchans <= 0) {
242 dev_err(dev->class_dev,
243 "comedi_get_n_channels() returned %d on minor %u subdev %d!\n",
244 nchans, minor, sdev);
245 return 0;
246 }
247 bdev = kmalloc(sizeof(*bdev), GFP_KERNEL);
248 if (!bdev) {
249 dev_err(dev->class_dev, "Out of memory\n");
250 return 0;
251 }
252 bdev->dev = d;
253 bdev->minor = minor;
254 bdev->subdev = sdev;
255 bdev->subdev_type = COMEDI_SUBD_DIO;
256 bdev->nchans = nchans;
257 bdev->chanid_offset = devpriv->nchans;
258
259
260 while (nchans--)
261 devpriv->chanIdDevMap[devpriv->nchans++] = bdev;
262
263
264
265
266
267 tmp = devpriv->ndevs * sizeof(bdev);
268 devpriv->devs =
269 Realloc(devpriv->devs,
270 ++devpriv->ndevs * sizeof(bdev), tmp);
271 if (!devpriv->devs) {
272 dev_err(dev->class_dev,
273 "Could not allocate memory. Out of memory?\n");
274 return 0;
275 }
276
277 devpriv->devs[devpriv->ndevs - 1] = bdev;
278 {
279
280 char buf[20];
281 int left =
282 MAX_BOARD_NAME - strlen(devpriv->name) - 1;
283 snprintf(buf, sizeof(buf), "%d:%d ", dev->minor,
284 bdev->subdev);
285 buf[sizeof(buf) - 1] = 0;
286 strncat(devpriv->name, buf, left);
287 }
288
289 }
290 }
291
292 if (!devpriv->nchans) {
293 dev_err(dev->class_dev, "No channels found!\n");
294 return 0;
295 }
296
297 return 1;
298}
299
300static int bonding_attach(struct comedi_device *dev,
301 struct comedi_devconfig *it)
302{
303 struct comedi_bond_private *devpriv;
304 struct comedi_subdevice *s;
305 int ret;
306
307 devpriv = kzalloc(sizeof(*devpriv), GFP_KERNEL);
308 if (!devpriv)
309 return -ENOMEM;
310 dev->private = devpriv;
311
312
313
314
315 if (!doDevConfig(dev, it))
316 return -EINVAL;
317
318 dev->board_name = devpriv->name;
319
320 ret = comedi_alloc_subdevices(dev, 1);
321 if (ret)
322 return ret;
323
324 s = &dev->subdevices[0];
325 s->type = COMEDI_SUBD_DIO;
326 s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
327 s->n_chan = devpriv->nchans;
328 s->maxdata = 1;
329 s->range_table = &range_digital;
330 s->insn_bits = bonding_dio_insn_bits;
331 s->insn_config = bonding_dio_insn_config;
332
333 dev_info(dev->class_dev,
334 "%s: %s attached, %u channels from %u devices\n",
335 dev->driver->driver_name, dev->board_name,
336 devpriv->nchans, devpriv->ndevs);
337
338 return 1;
339}
340
341static void bonding_detach(struct comedi_device *dev)
342{
343 struct comedi_bond_private *devpriv = dev->private;
344 unsigned long devs_closed = 0;
345
346 if (devpriv) {
347 while (devpriv->ndevs-- && devpriv->devs) {
348 struct BondedDevice *bdev;
349
350 bdev = devpriv->devs[devpriv->ndevs];
351 if (!bdev)
352 continue;
353 if (!(devs_closed & (0x1 << bdev->minor))) {
354 comedi_close(bdev->dev);
355 devs_closed |= (0x1 << bdev->minor);
356 }
357 kfree(bdev);
358 }
359 kfree(devpriv->devs);
360 devpriv->devs = NULL;
361 kfree(devpriv);
362 dev->private = NULL;
363 }
364}
365
366static struct comedi_driver bonding_driver = {
367 .driver_name = "comedi_bond",
368 .module = THIS_MODULE,
369 .attach = bonding_attach,
370 .detach = bonding_detach,
371};
372module_comedi_driver(bonding_driver);
373
374MODULE_AUTHOR("Calin A. Culianu");
375MODULE_DESCRIPTION("comedi_bond: A driver for COMEDI to bond multiple COMEDI "
376 "devices together as one. In the words of John Lennon: "
377 "'And the world will live as one...'");
378MODULE_LICENSE("GPL");
379