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