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