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