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