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