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