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