i2c-stub.c 9.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. i2c-stub.c - I2C/SMBus chip emulator
  4. Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
  5. Copyright (C) 2007-2014 Jean Delvare <jdelvare@suse.de>
  6. */
  7. #define DEBUG 1
  8. #define pr_fmt(fmt) "i2c-stub: " fmt
  9. #include <linux/errno.h>
  10. #include <linux/i2c.h>
  11. #include <linux/init.h>
  12. #include <linux/kernel.h>
  13. #include <linux/list.h>
  14. #include <linux/module.h>
  15. #include <linux/slab.h>
  16. #define MAX_CHIPS 10
  17. /*
  18. * Support for I2C_FUNC_SMBUS_BLOCK_DATA is disabled by default and must
  19. * be enabled explicitly by setting the I2C_FUNC_SMBUS_BLOCK_DATA bits
  20. * in the 'functionality' module parameter.
  21. */
  22. #define STUB_FUNC_DEFAULT \
  23. (I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \
  24. I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \
  25. I2C_FUNC_SMBUS_I2C_BLOCK)
  26. #define STUB_FUNC_ALL \
  27. (STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA)
  28. static unsigned short chip_addr[MAX_CHIPS];
  29. module_param_array(chip_addr, ushort, NULL, S_IRUGO);
  30. MODULE_PARM_DESC(chip_addr,
  31. "Chip addresses (up to 10, between 0x03 and 0x77)");
  32. static unsigned long functionality = STUB_FUNC_DEFAULT;
  33. module_param(functionality, ulong, S_IRUGO | S_IWUSR);
  34. MODULE_PARM_DESC(functionality, "Override functionality bitfield");
  35. /* Some chips have banked register ranges */
  36. static u8 bank_reg[MAX_CHIPS];
  37. module_param_array(bank_reg, byte, NULL, S_IRUGO);
  38. MODULE_PARM_DESC(bank_reg, "Bank register");
  39. static u8 bank_mask[MAX_CHIPS];
  40. module_param_array(bank_mask, byte, NULL, S_IRUGO);
  41. MODULE_PARM_DESC(bank_mask, "Bank value mask");
  42. static u8 bank_start[MAX_CHIPS];
  43. module_param_array(bank_start, byte, NULL, S_IRUGO);
  44. MODULE_PARM_DESC(bank_start, "First banked register");
  45. static u8 bank_end[MAX_CHIPS];
  46. module_param_array(bank_end, byte, NULL, S_IRUGO);
  47. MODULE_PARM_DESC(bank_end, "Last banked register");
  48. struct smbus_block_data {
  49. struct list_head node;
  50. u8 command;
  51. u8 len;
  52. u8 block[I2C_SMBUS_BLOCK_MAX];
  53. };
  54. struct stub_chip {
  55. u8 pointer;
  56. u16 words[256]; /* Byte operations use the LSB as per SMBus
  57. specification */
  58. struct list_head smbus_blocks;
  59. /* For chips with banks, extra registers are allocated dynamically */
  60. u8 bank_reg;
  61. u8 bank_shift;
  62. u8 bank_mask;
  63. u8 bank_sel; /* Currently selected bank */
  64. u8 bank_start;
  65. u8 bank_end;
  66. u16 bank_size;
  67. u16 *bank_words; /* Room for bank_mask * bank_size registers */
  68. };
  69. static struct stub_chip *stub_chips;
  70. static int stub_chips_nr;
  71. static struct smbus_block_data *stub_find_block(struct device *dev,
  72. struct stub_chip *chip,
  73. u8 command, bool create)
  74. {
  75. struct smbus_block_data *b, *rb = NULL;
  76. list_for_each_entry(b, &chip->smbus_blocks, node) {
  77. if (b->command == command) {
  78. rb = b;
  79. break;
  80. }
  81. }
  82. if (rb == NULL && create) {
  83. rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL);
  84. if (rb == NULL)
  85. return rb;
  86. rb->command = command;
  87. list_add(&rb->node, &chip->smbus_blocks);
  88. }
  89. return rb;
  90. }
  91. static u16 *stub_get_wordp(struct stub_chip *chip, u8 offset)
  92. {
  93. if (chip->bank_sel &&
  94. offset >= chip->bank_start && offset <= chip->bank_end)
  95. return chip->bank_words +
  96. (chip->bank_sel - 1) * chip->bank_size +
  97. offset - chip->bank_start;
  98. else
  99. return chip->words + offset;
  100. }
  101. /* Return negative errno on error. */
  102. static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags,
  103. char read_write, u8 command, int size, union i2c_smbus_data *data)
  104. {
  105. s32 ret;
  106. int i, len;
  107. struct stub_chip *chip = NULL;
  108. struct smbus_block_data *b;
  109. u16 *wordp;
  110. /* Search for the right chip */
  111. for (i = 0; i < stub_chips_nr; i++) {
  112. if (addr == chip_addr[i]) {
  113. chip = stub_chips + i;
  114. break;
  115. }
  116. }
  117. if (!chip)
  118. return -ENODEV;
  119. switch (size) {
  120. case I2C_SMBUS_QUICK:
  121. dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr);
  122. ret = 0;
  123. break;
  124. case I2C_SMBUS_BYTE:
  125. if (read_write == I2C_SMBUS_WRITE) {
  126. chip->pointer = command;
  127. dev_dbg(&adap->dev,
  128. "smbus byte - addr 0x%02x, wrote 0x%02x.\n",
  129. addr, command);
  130. } else {
  131. wordp = stub_get_wordp(chip, chip->pointer++);
  132. data->byte = *wordp & 0xff;
  133. dev_dbg(&adap->dev,
  134. "smbus byte - addr 0x%02x, read 0x%02x.\n",
  135. addr, data->byte);
  136. }
  137. ret = 0;
  138. break;
  139. case I2C_SMBUS_BYTE_DATA:
  140. wordp = stub_get_wordp(chip, command);
  141. if (read_write == I2C_SMBUS_WRITE) {
  142. *wordp &= 0xff00;
  143. *wordp |= data->byte;
  144. dev_dbg(&adap->dev,
  145. "smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
  146. addr, data->byte, command);
  147. /* Set the bank as needed */
  148. if (chip->bank_words && command == chip->bank_reg) {
  149. chip->bank_sel =
  150. (data->byte >> chip->bank_shift)
  151. & chip->bank_mask;
  152. dev_dbg(&adap->dev,
  153. "switching to bank %u.\n",
  154. chip->bank_sel);
  155. }
  156. } else {
  157. data->byte = *wordp & 0xff;
  158. dev_dbg(&adap->dev,
  159. "smbus byte data - addr 0x%02x, read 0x%02x at 0x%02x.\n",
  160. addr, data->byte, command);
  161. }
  162. chip->pointer = command + 1;
  163. ret = 0;
  164. break;
  165. case I2C_SMBUS_WORD_DATA:
  166. wordp = stub_get_wordp(chip, command);
  167. if (read_write == I2C_SMBUS_WRITE) {
  168. *wordp = data->word;
  169. dev_dbg(&adap->dev,
  170. "smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",
  171. addr, data->word, command);
  172. } else {
  173. data->word = *wordp;
  174. dev_dbg(&adap->dev,
  175. "smbus word data - addr 0x%02x, read 0x%04x at 0x%02x.\n",
  176. addr, data->word, command);
  177. }
  178. ret = 0;
  179. break;
  180. case I2C_SMBUS_I2C_BLOCK_DATA:
  181. /*
  182. * We ignore banks here, because banked chips don't use I2C
  183. * block transfers
  184. */
  185. if (data->block[0] > 256 - command) /* Avoid overrun */
  186. data->block[0] = 256 - command;
  187. len = data->block[0];
  188. if (read_write == I2C_SMBUS_WRITE) {
  189. for (i = 0; i < len; i++) {
  190. chip->words[command + i] &= 0xff00;
  191. chip->words[command + i] |= data->block[1 + i];
  192. }
  193. dev_dbg(&adap->dev,
  194. "i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
  195. addr, len, command);
  196. } else {
  197. for (i = 0; i < len; i++) {
  198. data->block[1 + i] =
  199. chip->words[command + i] & 0xff;
  200. }
  201. dev_dbg(&adap->dev,
  202. "i2c block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
  203. addr, len, command);
  204. }
  205. ret = 0;
  206. break;
  207. case I2C_SMBUS_BLOCK_DATA:
  208. /*
  209. * We ignore banks here, because chips typically don't use both
  210. * banks and SMBus block transfers
  211. */
  212. b = stub_find_block(&adap->dev, chip, command, false);
  213. if (read_write == I2C_SMBUS_WRITE) {
  214. len = data->block[0];
  215. if (len == 0 || len > I2C_SMBUS_BLOCK_MAX) {
  216. ret = -EINVAL;
  217. break;
  218. }
  219. if (b == NULL) {
  220. b = stub_find_block(&adap->dev, chip, command,
  221. true);
  222. if (b == NULL) {
  223. ret = -ENOMEM;
  224. break;
  225. }
  226. }
  227. /* Largest write sets read block length */
  228. if (len > b->len)
  229. b->len = len;
  230. for (i = 0; i < len; i++)
  231. b->block[i] = data->block[i + 1];
  232. /* update for byte and word commands */
  233. chip->words[command] = (b->block[0] << 8) | b->len;
  234. dev_dbg(&adap->dev,
  235. "smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
  236. addr, len, command);
  237. } else {
  238. if (b == NULL) {
  239. dev_dbg(&adap->dev,
  240. "SMBus block read command without prior block write not supported\n");
  241. ret = -EOPNOTSUPP;
  242. break;
  243. }
  244. len = b->len;
  245. data->block[0] = len;
  246. for (i = 0; i < len; i++)
  247. data->block[i + 1] = b->block[i];
  248. dev_dbg(&adap->dev,
  249. "smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
  250. addr, len, command);
  251. }
  252. ret = 0;
  253. break;
  254. default:
  255. dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n");
  256. ret = -EOPNOTSUPP;
  257. break;
  258. } /* switch (size) */
  259. return ret;
  260. }
  261. static u32 stub_func(struct i2c_adapter *adapter)
  262. {
  263. return STUB_FUNC_ALL & functionality;
  264. }
  265. static const struct i2c_algorithm smbus_algorithm = {
  266. .functionality = stub_func,
  267. .smbus_xfer = stub_xfer,
  268. };
  269. static struct i2c_adapter stub_adapter = {
  270. .owner = THIS_MODULE,
  271. .class = I2C_CLASS_HWMON | I2C_CLASS_SPD,
  272. .algo = &smbus_algorithm,
  273. .name = "SMBus stub driver",
  274. };
  275. static int __init i2c_stub_allocate_banks(int i)
  276. {
  277. struct stub_chip *chip = stub_chips + i;
  278. chip->bank_reg = bank_reg[i];
  279. chip->bank_start = bank_start[i];
  280. chip->bank_end = bank_end[i];
  281. chip->bank_size = bank_end[i] - bank_start[i] + 1;
  282. /* We assume that all bits in the mask are contiguous */
  283. chip->bank_mask = bank_mask[i];
  284. while (!(chip->bank_mask & 1)) {
  285. chip->bank_shift++;
  286. chip->bank_mask >>= 1;
  287. }
  288. chip->bank_words = kcalloc(chip->bank_mask * chip->bank_size,
  289. sizeof(u16),
  290. GFP_KERNEL);
  291. if (!chip->bank_words)
  292. return -ENOMEM;
  293. pr_debug("Allocated %u banks of %u words each (registers 0x%02x to 0x%02x)\n",
  294. chip->bank_mask, chip->bank_size, chip->bank_start,
  295. chip->bank_end);
  296. return 0;
  297. }
  298. static void i2c_stub_free(void)
  299. {
  300. int i;
  301. for (i = 0; i < stub_chips_nr; i++)
  302. kfree(stub_chips[i].bank_words);
  303. kfree(stub_chips);
  304. }
  305. static int __init i2c_stub_init(void)
  306. {
  307. int i, ret;
  308. if (!chip_addr[0]) {
  309. pr_err("Please specify a chip address\n");
  310. return -ENODEV;
  311. }
  312. for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
  313. if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) {
  314. pr_err("Invalid chip address 0x%02x\n",
  315. chip_addr[i]);
  316. return -EINVAL;
  317. }
  318. pr_info("Virtual chip at 0x%02x\n", chip_addr[i]);
  319. }
  320. /* Allocate memory for all chips at once */
  321. stub_chips_nr = i;
  322. stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),
  323. GFP_KERNEL);
  324. if (!stub_chips)
  325. return -ENOMEM;
  326. for (i = 0; i < stub_chips_nr; i++) {
  327. INIT_LIST_HEAD(&stub_chips[i].smbus_blocks);
  328. /* Allocate extra memory for banked register ranges */
  329. if (bank_mask[i]) {
  330. ret = i2c_stub_allocate_banks(i);
  331. if (ret)
  332. goto fail_free;
  333. }
  334. }
  335. ret = i2c_add_adapter(&stub_adapter);
  336. if (ret)
  337. goto fail_free;
  338. return 0;
  339. fail_free:
  340. i2c_stub_free();
  341. return ret;
  342. }
  343. static void __exit i2c_stub_exit(void)
  344. {
  345. i2c_del_adapter(&stub_adapter);
  346. i2c_stub_free();
  347. }
  348. MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
  349. MODULE_DESCRIPTION("I2C stub driver");
  350. MODULE_LICENSE("GPL");
  351. module_init(i2c_stub_init);
  352. module_exit(i2c_stub_exit);