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

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