PageRenderTime 79ms CodeModel.GetById 9ms app.highlight 63ms RepoModel.GetById 1ms app.codeStats 1ms

/arch/powerpc/platforms/powermac/pfunc_core.c

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