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