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