arch/powerpc/platforms/powermac/pfunc_core.c

   1 /*
   2  *
   3  * FIXME: Properly make this race free with refcounting etc...
   4  *
   5  * FIXME: LOCKING !!!
   6  */
   7
   8 #include <linux/config.h>
   9 #include <linux/init.h>
  10 #include <linux/delay.h>
  11 #include <linux/kernel.h>
  12 #include <linux/spinlock.h>
  13 #include <linux/module.h>
  14
  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