linux/arch/powerpc/platforms/powermac/pfunc_core.c
<<
>>
Prefs
   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 */
  69struct 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 */
  80static 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
  95static 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
 107static 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
 136static 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
 146static 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
 158static 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
 170static 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
 180static 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
 192static 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
 202static 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
 214static 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
 223static 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
 232static 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
 244static 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
 256static 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
 268static 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
 277static 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
 289static 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
 307static 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
 318static 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
 330static 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
 350static 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
 361static 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
 374static 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
 384static 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
 403static 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
 417static 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}
 430static 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
 444static 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
 457static 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
 470static 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
 483static 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
 499typedef int (*pmf_cmd_parser_t)(struct pmf_cmd *cmd, struct pmf_handlers *h);
 500
 501static 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
 538struct 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
 546static LIST_HEAD(pmf_devices);
 547static DEFINE_SPINLOCK(pmf_lock);
 548static DEFINE_MUTEX(pmf_irq_mutex);
 549
 550static 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
 556static inline void pmf_put_device(struct pmf_device *dev)
 557{
 558        kref_put(&dev->ref, pmf_release_device);
 559}
 560
 561static inline struct pmf_device *pmf_get_device(struct pmf_device *dev)
 562{
 563        kref_get(&dev->ref);
 564        return dev;
 565}
 566
 567static 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
 578static 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
 635static 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
 680static 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
 700int 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}
 746EXPORT_SYMBOL_GPL(pmf_register_driver);
 747
 748struct 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}
 755EXPORT_SYMBOL_GPL(pmf_get_function);
 756
 757static 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
 765static inline void __pmf_put_function(struct pmf_function *func)
 766{
 767        kref_put(&func->ref, pmf_release_function);
 768}
 769
 770void 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}
 777EXPORT_SYMBOL_GPL(pmf_put_function);
 778
 779void 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}
 805EXPORT_SYMBOL_GPL(pmf_unregister_driver);
 806
 807struct 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);
 855out:
 856        of_node_put(actor);
 857        return result;
 858}
 859
 860
 861int 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}
 891EXPORT_SYMBOL_GPL(pmf_register_irq_client);
 892
 893void 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}
 914EXPORT_SYMBOL_GPL(pmf_unregister_irq_client);
 915
 916
 917void 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}
 934EXPORT_SYMBOL_GPL(pmf_do_irq);
 935
 936
 937int 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}
 953EXPORT_SYMBOL_GPL(pmf_call_one);
 954
 955int 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}
 989EXPORT_SYMBOL_GPL(pmf_do_functions);
 990
 991
 992struct 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}
1005EXPORT_SYMBOL_GPL(pmf_find_function);
1006
1007int 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}
1020EXPORT_SYMBOL_GPL(pmf_call_function);
1021
1022