Merge branch 'release' of git://lm-sensors.org/kernel/mhoffman/hwmon-2.6
[linux-2.6] / arch / powerpc / platforms / powermac / pfunc_core.c
1 /*
2  *
3  * FIXME: Properly make this race free with refcounting etc...
4  *
5  * FIXME: LOCKING !!!
6  */
7
8 #include <linux/init.h>
9 #include <linux/delay.h>
10 #include <linux/kernel.h>
11 #include <linux/spinlock.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 /* Debug */
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 /* Command numbers */
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 /* This structure holds the state of the parser while walking through
67  * a function definition
68  */
69 struct 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 /* Debug output */
80 static 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  * Parser helpers
93  */
94
95 static 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
107 static 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  * Individual command parsers
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
136 static 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
146 static 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
158 static 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
170 static 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
180 static 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
192 static 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
202 static 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
214 static 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
223 static 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
232 static 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
244 static 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
256 static 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
268 static 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
277 static 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
289 static 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
307 static 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
318 static 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
330 static 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
350 static 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
361 static 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
374 static 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
384 static 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
403 static 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
417 static 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 }
430 static 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
444 static 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
457 static 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
470 static 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
483 static 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
499 typedef int (*pmf_cmd_parser_t)(struct pmf_cmd *cmd, struct pmf_handlers *h);
500
501 static 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, /* Bogus command */
520         NULL, /* Shift bytes right: NYI */
521         NULL, /* Shift bytes left: NYI */
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
538 struct 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
546 static LIST_HEAD(pmf_devices);
547 static DEFINE_SPINLOCK(pmf_lock);
548 static DEFINE_MUTEX(pmf_irq_mutex);
549
550 static 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
556 static inline void pmf_put_device(struct pmf_device *dev)
557 {
558         kref_put(&dev->ref, pmf_release_device);
559 }
560
561 static inline struct pmf_device *pmf_get_device(struct pmf_device *dev)
562 {
563         kref_get(&dev->ref);
564         return dev;
565 }
566
567 static 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
578 static 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         /* One subcommand to parse for now */
598         count = 1;
599
600         while(count-- && cmd.cmdptr < cmd.cmdend) {
601                 /* Get opcode */
602                 ccode = pmf_next32(&cmd);
603                 /* Check if we are hitting a command list, fetch new count */
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         /* We are doing an initial parse pass, we need to adjust the size */
629         if (handlers == NULL)
630                 func->length = cmd.cmdptr - func->data;
631
632         return 0;
633 }
634
635 static 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                 /* Allocate a structure */
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
680 static 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                 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
700 int 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 }
746 EXPORT_SYMBOL_GPL(pmf_register_driver);
747
748 struct 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 }
755 EXPORT_SYMBOL_GPL(pmf_get_function);
756
757 static 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
765 static inline void __pmf_put_function(struct pmf_function *func)
766 {
767         kref_put(&func->ref, pmf_release_function);
768 }
769
770 void 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 }
777 EXPORT_SYMBOL_GPL(pmf_put_function);
778
779 void 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 }
805 EXPORT_SYMBOL_GPL(pmf_unregister_driver);
806
807 struct 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          * Look for a "platform-*" function reference. If we can't find
819          * one, then we fallback to a direct call attempt
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          * Ok, now try to find the actor. If we can't find it, we fail,
831          * there is no point in falling back there
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                 return NULL;
841
842         list_for_each_entry(func, &dev->functions, link) {
843                 if (name && strcmp(name, func->name))
844                         continue;
845                 if (func->phandle && target->node != func->phandle)
846                         continue;
847                 if ((func->flags & flags) == 0)
848                         continue;
849                 result = func;
850                 break;
851         }
852         of_node_put(actor);
853         pmf_put_device(dev);
854         return result;
855 }
856
857
858 int pmf_register_irq_client(struct device_node *target,
859                             const char *name,
860                             struct pmf_irq_client *client)
861 {
862         struct pmf_function *func;
863         unsigned long flags;
864
865         spin_lock_irqsave(&pmf_lock, flags);
866         func = __pmf_find_function(target, name, PMF_FLAGS_INT_GEN);
867         if (func)
868                 func = pmf_get_function(func);
869         spin_unlock_irqrestore(&pmf_lock, flags);
870         if (func == NULL)
871                 return -ENODEV;
872
873         /* guard against manipulations of list */
874         mutex_lock(&pmf_irq_mutex);
875         if (list_empty(&func->irq_clients))
876                 func->dev->handlers->irq_enable(func);
877
878         /* guard against pmf_do_irq while changing list */
879         spin_lock_irqsave(&pmf_lock, flags);
880         list_add(&client->link, &func->irq_clients);
881         spin_unlock_irqrestore(&pmf_lock, flags);
882
883         client->func = func;
884         mutex_unlock(&pmf_irq_mutex);
885
886         return 0;
887 }
888 EXPORT_SYMBOL_GPL(pmf_register_irq_client);
889
890 void pmf_unregister_irq_client(struct pmf_irq_client *client)
891 {
892         struct pmf_function *func = client->func;
893         unsigned long flags;
894
895         BUG_ON(func == NULL);
896
897         /* guard against manipulations of list */
898         mutex_lock(&pmf_irq_mutex);
899         client->func = NULL;
900
901         /* guard against pmf_do_irq while changing list */
902         spin_lock_irqsave(&pmf_lock, flags);
903         list_del(&client->link);
904         spin_unlock_irqrestore(&pmf_lock, flags);
905
906         if (list_empty(&func->irq_clients))
907                 func->dev->handlers->irq_disable(func);
908         mutex_unlock(&pmf_irq_mutex);
909         pmf_put_function(func);
910 }
911 EXPORT_SYMBOL_GPL(pmf_unregister_irq_client);
912
913
914 void pmf_do_irq(struct pmf_function *func)
915 {
916         unsigned long flags;
917         struct pmf_irq_client *client;
918
919         /* For now, using a spinlock over the whole function. Can be made
920          * to drop the lock using 2 lists if necessary
921          */
922         spin_lock_irqsave(&pmf_lock, flags);
923         list_for_each_entry(client, &func->irq_clients, link) {
924                 if (!try_module_get(client->owner))
925                         continue;
926                 client->handler(client->data);
927                 module_put(client->owner);
928         }
929         spin_unlock_irqrestore(&pmf_lock, flags);
930 }
931 EXPORT_SYMBOL_GPL(pmf_do_irq);
932
933
934 int pmf_call_one(struct pmf_function *func, struct pmf_args *args)
935 {
936         struct pmf_device *dev = func->dev;
937         void *instdata = NULL;
938         int rc = 0;
939
940         DBG(" ** pmf_call_one(%s/%s) **\n", dev->node->full_name, func->name);
941
942         if (dev->handlers->begin)
943                 instdata = dev->handlers->begin(func, args);
944         rc = pmf_parse_one(func, dev->handlers, instdata, args);
945         if (dev->handlers->end)
946                 dev->handlers->end(func, instdata);
947
948         return rc;
949 }
950 EXPORT_SYMBOL_GPL(pmf_call_one);
951
952 int pmf_do_functions(struct device_node *np, const char *name,
953                      u32 phandle, u32 fflags, struct pmf_args *args)
954 {
955         struct pmf_device *dev;
956         struct pmf_function *func, *tmp;
957         unsigned long flags;
958         int rc = -ENODEV;
959
960         spin_lock_irqsave(&pmf_lock, flags);
961
962         dev = pmf_find_device(np);
963         if (dev == NULL) {
964                 spin_unlock_irqrestore(&pmf_lock, flags);
965                 return -ENODEV;
966         }
967         list_for_each_entry_safe(func, tmp, &dev->functions, link) {
968                 if (name && strcmp(name, func->name))
969                         continue;
970                 if (phandle && func->phandle && phandle != func->phandle)
971                         continue;
972                 if ((func->flags & fflags) == 0)
973                         continue;
974                 if (pmf_get_function(func) == NULL)
975                         continue;
976                 spin_unlock_irqrestore(&pmf_lock, flags);
977                 rc = pmf_call_one(func, args);
978                 pmf_put_function(func);
979                 spin_lock_irqsave(&pmf_lock, flags);
980         }
981         pmf_put_device(dev);
982         spin_unlock_irqrestore(&pmf_lock, flags);
983
984         return rc;
985 }
986 EXPORT_SYMBOL_GPL(pmf_do_functions);
987
988
989 struct pmf_function *pmf_find_function(struct device_node *target,
990                                        const char *name)
991 {
992         struct pmf_function *func;
993         unsigned long flags;
994
995         spin_lock_irqsave(&pmf_lock, flags);
996         func = __pmf_find_function(target, name, PMF_FLAGS_ON_DEMAND);
997         if (func)
998                 func = pmf_get_function(func);
999         spin_unlock_irqrestore(&pmf_lock, flags);
1000         return func;
1001 }
1002 EXPORT_SYMBOL_GPL(pmf_find_function);
1003
1004 int pmf_call_function(struct device_node *target, const char *name,
1005                       struct pmf_args *args)
1006 {
1007         struct pmf_function *func = pmf_find_function(target, name);
1008         int rc;
1009
1010         if (func == NULL)
1011                 return -ENODEV;
1012
1013         rc = pmf_call_one(func, args);
1014         pmf_put_function(func);
1015         return rc;
1016 }
1017 EXPORT_SYMBOL_GPL(pmf_call_function);
1018