1
2
3
4
5
6
7
8#include <linux/delay.h>
9#include <linux/kernel.h>
10#include <linux/spinlock.h>
11#include <linux/slab.h>
12#include <linux/module.h>
13#include <linux/mutex.h>
14
15#include <asm/prom.h>
16#include <asm/pmac_pfunc.h>
17
18
19#define LOG_PARSE(fmt...)
20#define LOG_ERROR(fmt...) printk(fmt)
21#define LOG_BLOB(t,b,c)
22
23#undef DEBUG
24#ifdef DEBUG
25#define DBG(fmt...) printk(fmt)
26#else
27#define DBG(fmt...)
28#endif
29
30
31#define PMF_CMD_LIST 0
32#define PMF_CMD_WRITE_GPIO 1
33#define PMF_CMD_READ_GPIO 2
34#define PMF_CMD_WRITE_REG32 3
35#define PMF_CMD_READ_REG32 4
36#define PMF_CMD_WRITE_REG16 5
37#define PMF_CMD_READ_REG16 6
38#define PMF_CMD_WRITE_REG8 7
39#define PMF_CMD_READ_REG8 8
40#define PMF_CMD_DELAY 9
41#define PMF_CMD_WAIT_REG32 10
42#define PMF_CMD_WAIT_REG16 11
43#define PMF_CMD_WAIT_REG8 12
44#define PMF_CMD_READ_I2C 13
45#define PMF_CMD_WRITE_I2C 14
46#define PMF_CMD_RMW_I2C 15
47#define PMF_CMD_GEN_I2C 16
48#define PMF_CMD_SHIFT_BYTES_RIGHT 17
49#define PMF_CMD_SHIFT_BYTES_LEFT 18
50#define PMF_CMD_READ_CFG 19
51#define PMF_CMD_WRITE_CFG 20
52#define PMF_CMD_RMW_CFG 21
53#define PMF_CMD_READ_I2C_SUBADDR 22
54#define PMF_CMD_WRITE_I2C_SUBADDR 23
55#define PMF_CMD_SET_I2C_MODE 24
56#define PMF_CMD_RMW_I2C_SUBADDR 25
57#define PMF_CMD_READ_REG32_MASK_SHR_XOR 26
58#define PMF_CMD_READ_REG16_MASK_SHR_XOR 27
59#define PMF_CMD_READ_REG8_MASK_SHR_XOR 28
60#define PMF_CMD_WRITE_REG32_SHL_MASK 29
61#define PMF_CMD_WRITE_REG16_SHL_MASK 30
62#define PMF_CMD_WRITE_REG8_SHL_MASK 31
63#define PMF_CMD_MASK_AND_COMPARE 32
64#define PMF_CMD_COUNT 33
65
66
67
68
69struct pmf_cmd {
70 const void *cmdptr;
71 const void *cmdend;
72 struct pmf_function *func;
73 void *instdata;
74 struct pmf_args *args;
75 int error;
76};
77
78#if 0
79
80static void print_blob(const char *title, const void *blob, int bytes)
81{
82 printk("%s", title);
83 while(bytes--) {
84 printk("%02x ", *((u8 *)blob));
85 blob += 1;
86 }
87 printk("\n");
88}
89#endif
90
91
92
93
94
95static u32 pmf_next32(struct pmf_cmd *cmd)
96{
97 u32 value;
98 if ((cmd->cmdend - cmd->cmdptr) < 4) {
99 cmd->error = 1;
100 return 0;
101 }
102 value = *((u32 *)cmd->cmdptr);
103 cmd->cmdptr += 4;
104 return value;
105}
106
107static const void* pmf_next_blob(struct pmf_cmd *cmd, int count)
108{
109 const void *value;
110 if ((cmd->cmdend - cmd->cmdptr) < count) {
111 cmd->error = 1;
112 return NULL;
113 }
114 value = cmd->cmdptr;
115 cmd->cmdptr += count;
116 return value;
117}
118
119
120
121
122
123#define PMF_PARSE_CALL(name, cmd, handlers, p...) \
124 do { \
125 if (cmd->error) \
126 return -ENXIO; \
127 if (handlers == NULL) \
128 return 0; \
129 if (handlers->name) \
130 return handlers->name(cmd->func, cmd->instdata, \
131 cmd->args, p); \
132 return -1; \
133 } while(0) \
134
135
136static int pmf_parser_write_gpio(struct pmf_cmd *cmd, struct pmf_handlers *h)
137{
138 u8 value = (u8)pmf_next32(cmd);
139 u8 mask = (u8)pmf_next32(cmd);
140
141 LOG_PARSE("pmf: write_gpio(value: %02x, mask: %02x)\n", value, mask);
142
143 PMF_PARSE_CALL(write_gpio, cmd, h, value, mask);
144}
145
146static int pmf_parser_read_gpio(struct pmf_cmd *cmd, struct pmf_handlers *h)
147{
148 u8 mask = (u8)pmf_next32(cmd);
149 int rshift = (int)pmf_next32(cmd);
150 u8 xor = (u8)pmf_next32(cmd);
151
152 LOG_PARSE("pmf: read_gpio(mask: %02x, rshift: %d, xor: %02x)\n",
153 mask, rshift, xor);
154
155 PMF_PARSE_CALL(read_gpio, cmd, h, mask, rshift, xor);
156}
157
158static int pmf_parser_write_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
159{
160 u32 offset = pmf_next32(cmd);
161 u32 value = pmf_next32(cmd);
162 u32 mask = pmf_next32(cmd);
163
164 LOG_PARSE("pmf: write_reg32(offset: %08x, value: %08x, mask: %08x)\n",
165 offset, value, mask);
166
167 PMF_PARSE_CALL(write_reg32, cmd, h, offset, value, mask);
168}
169
170static int pmf_parser_read_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
171{
172 u32 offset = pmf_next32(cmd);
173
174 LOG_PARSE("pmf: read_reg32(offset: %08x)\n", offset);
175
176 PMF_PARSE_CALL(read_reg32, cmd, h, offset);
177}
178
179
180static int pmf_parser_write_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
181{
182 u32 offset = pmf_next32(cmd);
183 u16 value = (u16)pmf_next32(cmd);
184 u16 mask = (u16)pmf_next32(cmd);
185
186 LOG_PARSE("pmf: write_reg16(offset: %08x, value: %04x, mask: %04x)\n",
187 offset, value, mask);
188
189 PMF_PARSE_CALL(write_reg16, cmd, h, offset, value, mask);
190}
191
192static int pmf_parser_read_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
193{
194 u32 offset = pmf_next32(cmd);
195
196 LOG_PARSE("pmf: read_reg16(offset: %08x)\n", offset);
197
198 PMF_PARSE_CALL(read_reg16, cmd, h, offset);
199}
200
201
202static int pmf_parser_write_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
203{
204 u32 offset = pmf_next32(cmd);
205 u8 value = (u16)pmf_next32(cmd);
206 u8 mask = (u16)pmf_next32(cmd);
207
208 LOG_PARSE("pmf: write_reg8(offset: %08x, value: %02x, mask: %02x)\n",
209 offset, value, mask);
210
211 PMF_PARSE_CALL(write_reg8, cmd, h, offset, value, mask);
212}
213
214static int pmf_parser_read_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
215{
216 u32 offset = pmf_next32(cmd);
217
218 LOG_PARSE("pmf: read_reg8(offset: %08x)\n", offset);
219
220 PMF_PARSE_CALL(read_reg8, cmd, h, offset);
221}
222
223static int pmf_parser_delay(struct pmf_cmd *cmd, struct pmf_handlers *h)
224{
225 u32 duration = pmf_next32(cmd);
226
227 LOG_PARSE("pmf: delay(duration: %d us)\n", duration);
228
229 PMF_PARSE_CALL(delay, cmd, h, duration);
230}
231
232static int pmf_parser_wait_reg32(struct pmf_cmd *cmd, struct pmf_handlers *h)
233{
234 u32 offset = pmf_next32(cmd);
235 u32 value = pmf_next32(cmd);
236 u32 mask = pmf_next32(cmd);
237
238 LOG_PARSE("pmf: wait_reg32(offset: %08x, comp_value: %08x,mask: %08x)\n",
239 offset, value, mask);
240
241 PMF_PARSE_CALL(wait_reg32, cmd, h, offset, value, mask);
242}
243
244static int pmf_parser_wait_reg16(struct pmf_cmd *cmd, struct pmf_handlers *h)
245{
246 u32 offset = pmf_next32(cmd);
247 u16 value = (u16)pmf_next32(cmd);
248 u16 mask = (u16)pmf_next32(cmd);
249
250 LOG_PARSE("pmf: wait_reg16(offset: %08x, comp_value: %04x,mask: %04x)\n",
251 offset, value, mask);
252
253 PMF_PARSE_CALL(wait_reg16, cmd, h, offset, value, mask);
254}
255
256static int pmf_parser_wait_reg8(struct pmf_cmd *cmd, struct pmf_handlers *h)
257{
258 u32 offset = pmf_next32(cmd);
259 u8 value = (u8)pmf_next32(cmd);
260 u8 mask = (u8)pmf_next32(cmd);
261
262 LOG_PARSE("pmf: wait_reg8(offset: %08x, comp_value: %02x,mask: %02x)\n",
263 offset, value, mask);
264
265 PMF_PARSE_CALL(wait_reg8, cmd, h, offset, value, mask);
266}
267
268static int pmf_parser_read_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
269{
270 u32 bytes = pmf_next32(cmd);
271
272 LOG_PARSE("pmf: read_i2c(bytes: %ud)\n", bytes);
273
274 PMF_PARSE_CALL(read_i2c, cmd, h, bytes);
275}
276
277static int pmf_parser_write_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
278{
279 u32 bytes = pmf_next32(cmd);
280 const void *blob = pmf_next_blob(cmd, bytes);
281
282 LOG_PARSE("pmf: write_i2c(bytes: %ud) ...\n", bytes);
283 LOG_BLOB("pmf: data: \n", blob, bytes);
284
285 PMF_PARSE_CALL(write_i2c, cmd, h, bytes, blob);
286}
287
288
289static int pmf_parser_rmw_i2c(struct pmf_cmd *cmd, struct pmf_handlers *h)
290{
291 u32 maskbytes = pmf_next32(cmd);
292 u32 valuesbytes = pmf_next32(cmd);
293 u32 totalbytes = pmf_next32(cmd);
294 const void *maskblob = pmf_next_blob(cmd, maskbytes);
295 const void *valuesblob = pmf_next_blob(cmd, valuesbytes);
296
297 LOG_PARSE("pmf: rmw_i2c(maskbytes: %ud, valuebytes: %ud, "
298 "totalbytes: %d) ...\n",
299 maskbytes, valuesbytes, totalbytes);
300 LOG_BLOB("pmf: mask data: \n", maskblob, maskbytes);
301 LOG_BLOB("pmf: values data: \n", valuesblob, valuesbytes);
302
303 PMF_PARSE_CALL(rmw_i2c, cmd, h, maskbytes, valuesbytes, totalbytes,
304 maskblob, valuesblob);
305}
306
307static int pmf_parser_read_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
308{
309 u32 offset = pmf_next32(cmd);
310 u32 bytes = pmf_next32(cmd);
311
312 LOG_PARSE("pmf: read_cfg(offset: %x, bytes: %ud)\n", offset, bytes);
313
314 PMF_PARSE_CALL(read_cfg, cmd, h, offset, bytes);
315}
316
317
318static int pmf_parser_write_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
319{
320 u32 offset = pmf_next32(cmd);
321 u32 bytes = pmf_next32(cmd);
322 const void *blob = pmf_next_blob(cmd, bytes);
323
324 LOG_PARSE("pmf: write_cfg(offset: %x, bytes: %ud)\n", offset, bytes);
325 LOG_BLOB("pmf: data: \n", blob, bytes);
326
327 PMF_PARSE_CALL(write_cfg, cmd, h, offset, bytes, blob);
328}
329
330static int pmf_parser_rmw_cfg(struct pmf_cmd *cmd, struct pmf_handlers *h)
331{
332 u32 offset = pmf_next32(cmd);
333 u32 maskbytes = pmf_next32(cmd);
334 u32 valuesbytes = pmf_next32(cmd);
335 u32 totalbytes = pmf_next32(cmd);
336 const void *maskblob = pmf_next_blob(cmd, maskbytes);
337 const void *valuesblob = pmf_next_blob(cmd, valuesbytes);
338
339 LOG_PARSE("pmf: rmw_cfg(maskbytes: %ud, valuebytes: %ud,"
340 " totalbytes: %d) ...\n",
341 maskbytes, valuesbytes, totalbytes);
342 LOG_BLOB("pmf: mask data: \n", maskblob, maskbytes);
343 LOG_BLOB("pmf: values data: \n", valuesblob, valuesbytes);
344
345 PMF_PARSE_CALL(rmw_cfg, cmd, h, offset, maskbytes, valuesbytes,
346 totalbytes, maskblob, valuesblob);
347}
348
349
350static int pmf_parser_read_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
351{
352 u8 subaddr = (u8)pmf_next32(cmd);
353 u32 bytes = pmf_next32(cmd);
354
355 LOG_PARSE("pmf: read_i2c_sub(subaddr: %x, bytes: %ud)\n",
356 subaddr, bytes);
357
358 PMF_PARSE_CALL(read_i2c_sub, cmd, h, subaddr, bytes);
359}
360
361static int pmf_parser_write_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
362{
363 u8 subaddr = (u8)pmf_next32(cmd);
364 u32 bytes = pmf_next32(cmd);
365 const void *blob = pmf_next_blob(cmd, bytes);
366
367 LOG_PARSE("pmf: write_i2c_sub(subaddr: %x, bytes: %ud) ...\n",
368 subaddr, bytes);
369 LOG_BLOB("pmf: data: \n", blob, bytes);
370
371 PMF_PARSE_CALL(write_i2c_sub, cmd, h, subaddr, bytes, blob);
372}
373
374static int pmf_parser_set_i2c_mode(struct pmf_cmd *cmd, struct pmf_handlers *h)
375{
376 u32 mode = pmf_next32(cmd);
377
378 LOG_PARSE("pmf: set_i2c_mode(mode: %d)\n", mode);
379
380 PMF_PARSE_CALL(set_i2c_mode, cmd, h, mode);
381}
382
383
384static int pmf_parser_rmw_i2c_sub(struct pmf_cmd *cmd, struct pmf_handlers *h)
385{
386 u8 subaddr = (u8)pmf_next32(cmd);
387 u32 maskbytes = pmf_next32(cmd);
388 u32 valuesbytes = pmf_next32(cmd);
389 u32 totalbytes = pmf_next32(cmd);
390 const void *maskblob = pmf_next_blob(cmd, maskbytes);
391 const void *valuesblob = pmf_next_blob(cmd, valuesbytes);
392
393 LOG_PARSE("pmf: rmw_i2c_sub(subaddr: %x, maskbytes: %ud, valuebytes: %ud"
394 ", totalbytes: %d) ...\n",
395 subaddr, maskbytes, valuesbytes, totalbytes);
396 LOG_BLOB("pmf: mask data: \n", maskblob, maskbytes);
397 LOG_BLOB("pmf: values data: \n", valuesblob, valuesbytes);
398
399 PMF_PARSE_CALL(rmw_i2c_sub, cmd, h, subaddr, maskbytes, valuesbytes,
400 totalbytes, maskblob, valuesblob);
401}
402
403static int pmf_parser_read_reg32_msrx(struct pmf_cmd *cmd,
404 struct pmf_handlers *h)
405{
406 u32 offset = pmf_next32(cmd);
407 u32 mask = pmf_next32(cmd);
408 u32 shift = pmf_next32(cmd);
409 u32 xor = pmf_next32(cmd);
410
411 LOG_PARSE("pmf: read_reg32_msrx(offset: %x, mask: %x, shift: %x,"
412 " xor: %x\n", offset, mask, shift, xor);
413
414 PMF_PARSE_CALL(read_reg32_msrx, cmd, h, offset, mask, shift, xor);
415}
416
417static int pmf_parser_read_reg16_msrx(struct pmf_cmd *cmd,
418 struct pmf_handlers *h)
419{
420 u32 offset = pmf_next32(cmd);
421 u32 mask = pmf_next32(cmd);
422 u32 shift = pmf_next32(cmd);
423 u32 xor = pmf_next32(cmd);
424
425 LOG_PARSE("pmf: read_reg16_msrx(offset: %x, mask: %x, shift: %x,"
426 " xor: %x\n", offset, mask, shift, xor);
427
428 PMF_PARSE_CALL(read_reg16_msrx, cmd, h, offset, mask, shift, xor);
429}
430static int pmf_parser_read_reg8_msrx(struct pmf_cmd *cmd,
431 struct pmf_handlers *h)
432{
433 u32 offset = pmf_next32(cmd);
434 u32 mask = pmf_next32(cmd);
435 u32 shift = pmf_next32(cmd);
436 u32 xor = pmf_next32(cmd);
437
438 LOG_PARSE("pmf: read_reg8_msrx(offset: %x, mask: %x, shift: %x,"
439 " xor: %x\n", offset, mask, shift, xor);
440
441 PMF_PARSE_CALL(read_reg8_msrx, cmd, h, offset, mask, shift, xor);
442}
443
444static int pmf_parser_write_reg32_slm(struct pmf_cmd *cmd,
445 struct pmf_handlers *h)
446{
447 u32 offset = pmf_next32(cmd);
448 u32 shift = pmf_next32(cmd);
449 u32 mask = pmf_next32(cmd);
450
451 LOG_PARSE("pmf: write_reg32_slm(offset: %x, shift: %x, mask: %x\n",
452 offset, shift, mask);
453
454 PMF_PARSE_CALL(write_reg32_slm, cmd, h, offset, shift, mask);
455}
456
457static int pmf_parser_write_reg16_slm(struct pmf_cmd *cmd,
458 struct pmf_handlers *h)
459{
460 u32 offset = pmf_next32(cmd);
461 u32 shift = pmf_next32(cmd);
462 u32 mask = pmf_next32(cmd);
463
464 LOG_PARSE("pmf: write_reg16_slm(offset: %x, shift: %x, mask: %x\n",
465 offset, shift, mask);
466
467 PMF_PARSE_CALL(write_reg16_slm, cmd, h, offset, shift, mask);
468}
469
470static int pmf_parser_write_reg8_slm(struct pmf_cmd *cmd,
471 struct pmf_handlers *h)
472{
473 u32 offset = pmf_next32(cmd);
474 u32 shift = pmf_next32(cmd);
475 u32 mask = pmf_next32(cmd);
476
477 LOG_PARSE("pmf: write_reg8_slm(offset: %x, shift: %x, mask: %x\n",
478 offset, shift, mask);
479
480 PMF_PARSE_CALL(write_reg8_slm, cmd, h, offset, shift, mask);
481}
482
483static int pmf_parser_mask_and_compare(struct pmf_cmd *cmd,
484 struct pmf_handlers *h)
485{
486 u32 bytes = pmf_next32(cmd);
487 const void *maskblob = pmf_next_blob(cmd, bytes);
488 const void *valuesblob = pmf_next_blob(cmd, bytes);
489
490 LOG_PARSE("pmf: mask_and_compare(length: %ud ...\n", bytes);
491 LOG_BLOB("pmf: mask data: \n", maskblob, bytes);
492 LOG_BLOB("pmf: values data: \n", valuesblob, bytes);
493
494 PMF_PARSE_CALL(mask_and_compare, cmd, h,
495 bytes, maskblob, valuesblob);
496}
497
498
499typedef int (*pmf_cmd_parser_t)(struct pmf_cmd *cmd, struct pmf_handlers *h);
500
501static pmf_cmd_parser_t pmf_parsers[PMF_CMD_COUNT] =
502{
503 NULL,
504 pmf_parser_write_gpio,
505 pmf_parser_read_gpio,
506 pmf_parser_write_reg32,
507 pmf_parser_read_reg32,
508 pmf_parser_write_reg16,
509 pmf_parser_read_reg16,
510 pmf_parser_write_reg8,
511 pmf_parser_read_reg8,
512 pmf_parser_delay,
513 pmf_parser_wait_reg32,
514 pmf_parser_wait_reg16,
515 pmf_parser_wait_reg8,
516 pmf_parser_read_i2c,
517 pmf_parser_write_i2c,
518 pmf_parser_rmw_i2c,
519 NULL,
520 NULL,
521 NULL,
522 pmf_parser_read_cfg,
523 pmf_parser_write_cfg,
524 pmf_parser_rmw_cfg,
525 pmf_parser_read_i2c_sub,
526 pmf_parser_write_i2c_sub,
527 pmf_parser_set_i2c_mode,
528 pmf_parser_rmw_i2c_sub,
529 pmf_parser_read_reg32_msrx,
530 pmf_parser_read_reg16_msrx,
531 pmf_parser_read_reg8_msrx,
532 pmf_parser_write_reg32_slm,
533 pmf_parser_write_reg16_slm,
534 pmf_parser_write_reg8_slm,
535 pmf_parser_mask_and_compare,
536};
537
538struct pmf_device {
539 struct list_head link;
540 struct device_node *node;
541 struct pmf_handlers *handlers;
542 struct list_head functions;
543 struct kref ref;
544};
545
546static LIST_HEAD(pmf_devices);
547static DEFINE_SPINLOCK(pmf_lock);
548static DEFINE_MUTEX(pmf_irq_mutex);
549
550static void pmf_release_device(struct kref *kref)
551{
552 struct pmf_device *dev = container_of(kref, struct pmf_device, ref);
553 kfree(dev);
554}
555
556static inline void pmf_put_device(struct pmf_device *dev)
557{
558 kref_put(&dev->ref, pmf_release_device);
559}
560
561static inline struct pmf_device *pmf_get_device(struct pmf_device *dev)
562{
563 kref_get(&dev->ref);
564 return dev;
565}
566
567static inline struct pmf_device *pmf_find_device(struct device_node *np)
568{
569 struct pmf_device *dev;
570
571 list_for_each_entry(dev, &pmf_devices, link) {
572 if (dev->node == np)
573 return pmf_get_device(dev);
574 }
575 return NULL;
576}
577
578static int pmf_parse_one(struct pmf_function *func,
579 struct pmf_handlers *handlers,
580 void *instdata, struct pmf_args *args)
581{
582 struct pmf_cmd cmd;
583 u32 ccode;
584 int count, rc;
585
586 cmd.cmdptr = func->data;
587 cmd.cmdend = func->data + func->length;
588 cmd.func = func;
589 cmd.instdata = instdata;
590 cmd.args = args;
591 cmd.error = 0;
592
593 LOG_PARSE("pmf: func %s, %d bytes, %s...\n",
594 func->name, func->length,
595 handlers ? "executing" : "parsing");
596
597
598 count = 1;
599
600 while(count-- && cmd.cmdptr < cmd.cmdend) {
601
602 ccode = pmf_next32(&cmd);
603
604 if (ccode == 0) {
605 count = pmf_next32(&cmd) - 1;
606 ccode = pmf_next32(&cmd);
607 }
608 if (cmd.error) {
609 LOG_ERROR("pmf: parse error, not enough data\n");
610 return -ENXIO;
611 }
612 if (ccode >= PMF_CMD_COUNT) {
613 LOG_ERROR("pmf: command code %d unknown !\n", ccode);
614 return -ENXIO;
615 }
616 if (pmf_parsers[ccode] == NULL) {
617 LOG_ERROR("pmf: no parser for command %d !\n", ccode);
618 return -ENXIO;
619 }
620 rc = pmf_parsers[ccode](&cmd, handlers);
621 if (rc != 0) {
622 LOG_ERROR("pmf: parser for command %d returned"
623 " error %d\n", ccode, rc);
624 return rc;
625 }
626 }
627
628
629 if (handlers == NULL)
630 func->length = cmd.cmdptr - func->data;
631
632 return 0;
633}
634
635static int pmf_add_function_prop(struct pmf_device *dev, void *driverdata,
636 const char *name, u32 *data,
637 unsigned int length)
638{
639 int count = 0;
640 struct pmf_function *func = NULL;
641
642 DBG("pmf: Adding functions for platform-do-%s\n", name);
643
644 while (length >= 12) {
645
646 func = kzalloc(sizeof(struct pmf_function), GFP_KERNEL);
647 if (func == NULL)
648 goto bail;
649 kref_init(&func->ref);
650 INIT_LIST_HEAD(&func->irq_clients);
651 func->node = dev->node;
652 func->driver_data = driverdata;
653 func->name = name;
654 func->phandle = data[0];
655 func->flags = data[1];
656 data += 2;
657 length -= 8;
658 func->data = data;
659 func->length = length;
660 func->dev = dev;
661 DBG("pmf: idx %d: flags=%08x, phandle=%08x "
662 " %d bytes remaining, parsing...\n",
663 count+1, func->flags, func->phandle, length);
664 if (pmf_parse_one(func, NULL, NULL, NULL)) {
665 kfree(func);
666 goto bail;
667 }
668 length -= func->length;
669 data = (u32 *)(((u8 *)data) + func->length);
670 list_add(&func->link, &dev->functions);
671 pmf_get_device(dev);
672 count++;
673 }
674 bail:
675 DBG("pmf: Added %d functions\n", count);
676
677 return count;
678}
679
680static int pmf_add_functions(struct pmf_device *dev, void *driverdata)
681{
682 struct property *pp;
683#define PP_PREFIX "platform-do-"
684 const int plen = strlen(PP_PREFIX);
685 int count = 0;
686
687 for (pp = dev->node->properties; pp != 0; pp = pp->next) {
688 const char *name;
689 if (strncmp(pp->name, PP_PREFIX, plen) != 0)
690 continue;
691 name = pp->name + plen;
692 if (strlen(name) && pp->length >= 12)
693 count += pmf_add_function_prop(dev, driverdata, name,
694 pp->value, pp->length);
695 }
696 return count;
697}
698
699
700int pmf_register_driver(struct device_node *np,
701 struct pmf_handlers *handlers,
702 void *driverdata)
703{
704 struct pmf_device *dev;
705 unsigned long flags;
706 int rc = 0;
707
708 if (handlers == NULL)
709 return -EINVAL;
710
711 DBG("pmf: registering driver for node %s\n", np->full_name);
712
713 spin_lock_irqsave(&pmf_lock, flags);
714 dev = pmf_find_device(np);
715 spin_unlock_irqrestore(&pmf_lock, flags);
716 if (dev != NULL) {
717 DBG("pmf: already there !\n");
718 pmf_put_device(dev);
719 return -EBUSY;
720 }
721
722 dev = kzalloc(sizeof(struct pmf_device), GFP_KERNEL);
723 if (dev == NULL) {
724 DBG("pmf: no memory !\n");
725 return -ENOMEM;
726 }
727 kref_init(&dev->ref);
728 dev->node = of_node_get(np);
729 dev->handlers = handlers;
730 INIT_LIST_HEAD(&dev->functions);
731
732 rc = pmf_add_functions(dev, driverdata);
733 if (rc == 0) {
734 DBG("pmf: no functions, disposing.. \n");
735 of_node_put(np);
736 kfree(dev);
737 return -ENODEV;
738 }
739
740 spin_lock_irqsave(&pmf_lock, flags);
741 list_add(&dev->link, &pmf_devices);
742 spin_unlock_irqrestore(&pmf_lock, flags);
743
744 return 0;
745}
746EXPORT_SYMBOL_GPL(pmf_register_driver);
747
748struct pmf_function *pmf_get_function(struct pmf_function *func)
749{
750 if (!try_module_get(func->dev->handlers->owner))
751 return NULL;
752 kref_get(&func->ref);
753 return func;
754}
755EXPORT_SYMBOL_GPL(pmf_get_function);
756
757static void pmf_release_function(struct kref *kref)
758{
759 struct pmf_function *func =
760 container_of(kref, struct pmf_function, ref);
761 pmf_put_device(func->dev);
762 kfree(func);
763}
764
765static inline void __pmf_put_function(struct pmf_function *func)
766{
767 kref_put(&func->ref, pmf_release_function);
768}
769
770void pmf_put_function(struct pmf_function *func)
771{
772 if (func == NULL)
773 return;
774 module_put(func->dev->handlers->owner);
775 __pmf_put_function(func);
776}
777EXPORT_SYMBOL_GPL(pmf_put_function);
778
779void pmf_unregister_driver(struct device_node *np)
780{
781 struct pmf_device *dev;
782 unsigned long flags;
783
784 DBG("pmf: unregistering driver for node %s\n", np->full_name);
785
786 spin_lock_irqsave(&pmf_lock, flags);
787 dev = pmf_find_device(np);
788 if (dev == NULL) {
789 DBG("pmf: not such driver !\n");
790 spin_unlock_irqrestore(&pmf_lock, flags);
791 return;
792 }
793 list_del(&dev->link);
794
795 while(!list_empty(&dev->functions)) {
796 struct pmf_function *func =
797 list_entry(dev->functions.next, typeof(*func), link);
798 list_del(&func->link);
799 __pmf_put_function(func);
800 }
801
802 pmf_put_device(dev);
803 spin_unlock_irqrestore(&pmf_lock, flags);
804}
805EXPORT_SYMBOL_GPL(pmf_unregister_driver);
806
807struct pmf_function *__pmf_find_function(struct device_node *target,
808 const char *name, u32 flags)
809{
810 struct device_node *actor = of_node_get(target);
811 struct pmf_device *dev;
812 struct pmf_function *func, *result = NULL;
813 char fname[64];
814 const u32 *prop;
815 u32 ph;
816
817
818
819
820
821 snprintf(fname, 63, "platform-%s", name);
822 prop = of_get_property(target, fname, NULL);
823 if (prop == NULL)
824 goto find_it;
825 ph = *prop;
826 if (ph == 0)
827 goto find_it;
828
829
830
831
832
833 of_node_put(actor);
834 actor = of_find_node_by_phandle(ph);
835 if (actor == NULL)
836 return NULL;
837 find_it:
838 dev = pmf_find_device(actor);
839 if (dev == NULL) {
840 result = NULL;
841 goto out;
842 }
843
844 list_for_each_entry(func, &dev->functions, link) {
845 if (name && strcmp(name, func->name))
846 continue;
847 if (func->phandle && target->phandle != func->phandle)
848 continue;
849 if ((func->flags & flags) == 0)
850 continue;
851 result = func;
852 break;
853 }
854 pmf_put_device(dev);
855out:
856 of_node_put(actor);
857 return result;
858}
859
860
861int pmf_register_irq_client(struct device_node *target,
862 const char *name,
863 struct pmf_irq_client *client)
864{
865 struct pmf_function *func;
866 unsigned long flags;
867
868 spin_lock_irqsave(&pmf_lock, flags);
869 func = __pmf_find_function(target, name, PMF_FLAGS_INT_GEN);
870 if (func)
871 func = pmf_get_function(func);
872 spin_unlock_irqrestore(&pmf_lock, flags);
873 if (func == NULL)
874 return -ENODEV;
875
876
877 mutex_lock(&pmf_irq_mutex);
878 if (list_empty(&func->irq_clients))
879 func->dev->handlers->irq_enable(func);
880
881
882 spin_lock_irqsave(&pmf_lock, flags);
883 list_add(&client->link, &func->irq_clients);
884 spin_unlock_irqrestore(&pmf_lock, flags);
885
886 client->func = func;
887 mutex_unlock(&pmf_irq_mutex);
888
889 return 0;
890}
891EXPORT_SYMBOL_GPL(pmf_register_irq_client);
892
893void pmf_unregister_irq_client(struct pmf_irq_client *client)
894{
895 struct pmf_function *func = client->func;
896 unsigned long flags;
897
898 BUG_ON(func == NULL);
899
900
901 mutex_lock(&pmf_irq_mutex);
902 client->func = NULL;
903
904
905 spin_lock_irqsave(&pmf_lock, flags);
906 list_del(&client->link);
907 spin_unlock_irqrestore(&pmf_lock, flags);
908
909 if (list_empty(&func->irq_clients))
910 func->dev->handlers->irq_disable(func);
911 mutex_unlock(&pmf_irq_mutex);
912 pmf_put_function(func);
913}
914EXPORT_SYMBOL_GPL(pmf_unregister_irq_client);
915
916
917void pmf_do_irq(struct pmf_function *func)
918{
919 unsigned long flags;
920 struct pmf_irq_client *client;
921
922
923
924
925 spin_lock_irqsave(&pmf_lock, flags);
926 list_for_each_entry(client, &func->irq_clients, link) {
927 if (!try_module_get(client->owner))
928 continue;
929 client->handler(client->data);
930 module_put(client->owner);
931 }
932 spin_unlock_irqrestore(&pmf_lock, flags);
933}
934EXPORT_SYMBOL_GPL(pmf_do_irq);
935
936
937int pmf_call_one(struct pmf_function *func, struct pmf_args *args)
938{
939 struct pmf_device *dev = func->dev;
940 void *instdata = NULL;
941 int rc = 0;
942
943 DBG(" ** pmf_call_one(%s/%s) **\n", dev->node->full_name, func->name);
944
945 if (dev->handlers->begin)
946 instdata = dev->handlers->begin(func, args);
947 rc = pmf_parse_one(func, dev->handlers, instdata, args);
948 if (dev->handlers->end)
949 dev->handlers->end(func, instdata);
950
951 return rc;
952}
953EXPORT_SYMBOL_GPL(pmf_call_one);
954
955int pmf_do_functions(struct device_node *np, const char *name,
956 u32 phandle, u32 fflags, struct pmf_args *args)
957{
958 struct pmf_device *dev;
959 struct pmf_function *func, *tmp;
960 unsigned long flags;
961 int rc = -ENODEV;
962
963 spin_lock_irqsave(&pmf_lock, flags);
964
965 dev = pmf_find_device(np);
966 if (dev == NULL) {
967 spin_unlock_irqrestore(&pmf_lock, flags);
968 return -ENODEV;
969 }
970 list_for_each_entry_safe(func, tmp, &dev->functions, link) {
971 if (name && strcmp(name, func->name))
972 continue;
973 if (phandle && func->phandle && phandle != func->phandle)
974 continue;
975 if ((func->flags & fflags) == 0)
976 continue;
977 if (pmf_get_function(func) == NULL)
978 continue;
979 spin_unlock_irqrestore(&pmf_lock, flags);
980 rc = pmf_call_one(func, args);
981 pmf_put_function(func);
982 spin_lock_irqsave(&pmf_lock, flags);
983 }
984 pmf_put_device(dev);
985 spin_unlock_irqrestore(&pmf_lock, flags);
986
987 return rc;
988}
989EXPORT_SYMBOL_GPL(pmf_do_functions);
990
991
992struct pmf_function *pmf_find_function(struct device_node *target,
993 const char *name)
994{
995 struct pmf_function *func;
996 unsigned long flags;
997
998 spin_lock_irqsave(&pmf_lock, flags);
999 func = __pmf_find_function(target, name, PMF_FLAGS_ON_DEMAND);
1000 if (func)
1001 func = pmf_get_function(func);
1002 spin_unlock_irqrestore(&pmf_lock, flags);
1003 return func;
1004}
1005EXPORT_SYMBOL_GPL(pmf_find_function);
1006
1007int pmf_call_function(struct device_node *target, const char *name,
1008 struct pmf_args *args)
1009{
1010 struct pmf_function *func = pmf_find_function(target, name);
1011 int rc;
1012
1013 if (func == NULL)
1014 return -ENODEV;
1015
1016 rc = pmf_call_one(func, args);
1017 pmf_put_function(func);
1018 return rc;
1019}
1020EXPORT_SYMBOL_GPL(pmf_call_function);
1021
1022
