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