[mirror_ubuntu-bionic-kernel.git] / drivers / i2c / i2c-stub.c

/*
    i2c-stub.c - I2C/SMBus chip emulator

    Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
    Copyright (C) 2007, 2012 Jean Delvare <jdelvare@suse.de>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#define DEBUG 1

#include <linux/init.h>
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/slab.h>
#include <linux/errno.h>
#include <linux/i2c.h>
#include <linux/list.h>

#define MAX_CHIPS 10

/*
 * Support for I2C_FUNC_SMBUS_BLOCK_DATA is disabled by default and must
 * be enabled explicitly by setting the I2C_FUNC_SMBUS_BLOCK_DATA bits
 * in the 'functionality' module parameter.
 */
#define STUB_FUNC_DEFAULT \
		(I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE | \
		 I2C_FUNC_SMBUS_BYTE_DATA | I2C_FUNC_SMBUS_WORD_DATA | \
		 I2C_FUNC_SMBUS_I2C_BLOCK)

#define STUB_FUNC_ALL \
		(STUB_FUNC_DEFAULT | I2C_FUNC_SMBUS_BLOCK_DATA)

static unsigned short chip_addr[MAX_CHIPS];
module_param_array(chip_addr, ushort, NULL, S_IRUGO);
MODULE_PARM_DESC(chip_addr,
		 "Chip addresses (up to 10, between 0x03 and 0x77)");

static unsigned long functionality = STUB_FUNC_DEFAULT;
module_param(functionality, ulong, S_IRUGO | S_IWUSR);
MODULE_PARM_DESC(functionality, "Override functionality bitfield");

struct smbus_block_data {
	struct list_head node;
	u8 command;
	u8 len;
	u8 block[I2C_SMBUS_BLOCK_MAX];
};

struct stub_chip {
	u8 pointer;
	u16 words[256];		/* Byte operations use the LSB as per SMBus
				   specification */
	struct list_head smbus_blocks;
};

static struct stub_chip *stub_chips;
static int stub_chips_nr;

static struct smbus_block_data *stub_find_block(struct device *dev,
						struct stub_chip *chip,
						u8 command, bool create)
{
	struct smbus_block_data *b, *rb = NULL;

	list_for_each_entry(b, &chip->smbus_blocks, node) {
		if (b->command == command) {
			rb = b;
			break;
		}
	}
	if (rb == NULL && create) {
		rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL);
		if (rb == NULL)
			return rb;
		rb->command = command;
		list_add(&rb->node, &chip->smbus_blocks);
	}
	return rb;
}

/* Return negative errno on error. */
static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags,
	char read_write, u8 command, int size, union i2c_smbus_data *data)
{
	s32 ret;
	int i, len;
	struct stub_chip *chip = NULL;
	struct smbus_block_data *b;

	/* Search for the right chip */
	for (i = 0; i < stub_chips_nr; i++) {
		if (addr == chip_addr[i]) {
			chip = stub_chips + i;
			break;
		}
	}
	if (!chip)
		return -ENODEV;

	switch (size) {

	case I2C_SMBUS_QUICK:
		dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr);
		ret = 0;
		break;

	case I2C_SMBUS_BYTE:
		if (read_write == I2C_SMBUS_WRITE) {
			chip->pointer = command;
			dev_dbg(&adap->dev,
				"smbus byte - addr 0x%02x, wrote 0x%02x.\n",
				addr, command);
		} else {
			data->byte = chip->words[chip->pointer++] & 0xff;
			dev_dbg(&adap->dev,
				"smbus byte - addr 0x%02x, read  0x%02x.\n",
				addr, data->byte);
		}

		ret = 0;
		break;

	case I2C_SMBUS_BYTE_DATA:
		if (read_write == I2C_SMBUS_WRITE) {
			chip->words[command] &= 0xff00;
			chip->words[command] |= data->byte;
			dev_dbg(&adap->dev,
				"smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
				addr, data->byte, command);
		} else {
			data->byte = chip->words[command] & 0xff;
			dev_dbg(&adap->dev,
				"smbus byte data - addr 0x%02x, read  0x%02x at 0x%02x.\n",
				addr, data->byte, command);
		}
		chip->pointer = command + 1;

		ret = 0;
		break;

	case I2C_SMBUS_WORD_DATA:
		if (read_write == I2C_SMBUS_WRITE) {
			chip->words[command] = data->word;
			dev_dbg(&adap->dev,
				"smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",
				addr, data->word, command);
		} else {
			data->word = chip->words[command];
			dev_dbg(&adap->dev,
				"smbus word data - addr 0x%02x, read  0x%04x at 0x%02x.\n",
				addr, data->word, command);
		}

		ret = 0;
		break;

	case I2C_SMBUS_I2C_BLOCK_DATA:
		len = data->block[0];
		if (read_write == I2C_SMBUS_WRITE) {
			for (i = 0; i < len; i++) {
				chip->words[command + i] &= 0xff00;
				chip->words[command + i] |= data->block[1 + i];
			}
			dev_dbg(&adap->dev,
				"i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
				addr, len, command);
		} else {
			for (i = 0; i < len; i++) {
				data->block[1 + i] =
					chip->words[command + i] & 0xff;
			}
			dev_dbg(&adap->dev,
				"i2c block data - addr 0x%02x, read  %d bytes at 0x%02x.\n",
				addr, len, command);
		}

		ret = 0;
		break;

	case I2C_SMBUS_BLOCK_DATA:
		b = stub_find_block(&adap->dev, chip, command, false);
		if (read_write == I2C_SMBUS_WRITE) {
			len = data->block[0];
			if (len == 0 || len > I2C_SMBUS_BLOCK_MAX) {
				ret = -EINVAL;
				break;
			}
			if (b == NULL) {
				b = stub_find_block(&adap->dev, chip, command,
						    true);
				if (b == NULL) {
					ret = -ENOMEM;
					break;
				}
			}
			/* Largest write sets read block length */
			if (len > b->len)
				b->len = len;
			for (i = 0; i < len; i++)
				b->block[i] = data->block[i + 1];
			/* update for byte and word commands */
			chip->words[command] = (b->block[0] << 8) | b->len;
			dev_dbg(&adap->dev,
				"smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
				addr, len, command);
		} else {
			if (b == NULL) {
				dev_dbg(&adap->dev,
					"SMBus block read command without prior block write not supported\n");
				ret = -EOPNOTSUPP;
				break;
			}
			len = b->len;
			data->block[0] = len;
			for (i = 0; i < len; i++)
				data->block[i + 1] = b->block[i];
			dev_dbg(&adap->dev,
				"smbus block data - addr 0x%02x, read  %d bytes at 0x%02x.\n",
				addr, len, command);
		}

		ret = 0;
		break;

	default:
		dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n");
		ret = -EOPNOTSUPP;
		break;
	} /* switch (size) */

	return ret;
}

static u32 stub_func(struct i2c_adapter *adapter)
{
	return STUB_FUNC_ALL & functionality;
}

static const struct i2c_algorithm smbus_algorithm = {
	.functionality	= stub_func,
	.smbus_xfer	= stub_xfer,
};

static struct i2c_adapter stub_adapter = {
	.owner		= THIS_MODULE,
	.class		= I2C_CLASS_HWMON | I2C_CLASS_SPD,
	.algo		= &smbus_algorithm,
	.name		= "SMBus stub driver",
};

static int __init i2c_stub_init(void)
{
	int i, ret;

	if (!chip_addr[0]) {
		pr_err("i2c-stub: Please specify a chip address\n");
		return -ENODEV;
	}

	for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
		if (chip_addr[i] < 0x03 || chip_addr[i] > 0x77) {
			pr_err("i2c-stub: Invalid chip address 0x%02x\n",
			       chip_addr[i]);
			return -EINVAL;
		}

		pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr[i]);
	}

	/* Allocate memory for all chips at once */
	stub_chips_nr = i;
	stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),
			     GFP_KERNEL);
	if (!stub_chips) {
		pr_err("i2c-stub: Out of memory\n");
		return -ENOMEM;
	}
	for (i = 0; i < stub_chips_nr; i++)
		INIT_LIST_HEAD(&stub_chips[i].smbus_blocks);

	ret = i2c_add_adapter(&stub_adapter);
	if (ret)
		kfree(stub_chips);
	return ret;
}

static void __exit i2c_stub_exit(void)
{
	i2c_del_adapter(&stub_adapter);
	kfree(stub_chips);
}

MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
MODULE_DESCRIPTION("I2C stub driver");
MODULE_LICENSE("GPL");

module_init(i2c_stub_init);
module_exit(i2c_stub_exit);
Commit	Line	Data
1da177e4	1	/*
569be443	2	i2c-stub.c - I2C/SMBus chip emulator
1da177e4 LT	3
1da177e4 LT	4	Copyright (c) 2004 Mark M. Hoffman <mhoffman@lightlink.com>
7c81c60f	5	Copyright (C) 2007, 2012 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>
1da177e4 LT	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
6f16b75a GR	56	struct smbus_block_data {
	57	struct list_head node;
	58	u8 command;
	59	u8 len;
	60	u8 block[I2C_SMBUS_BLOCK_MAX];
	61	};
	62
9d90c1fd JD	63	struct stub_chip {
9d90c1fd JD	64	u8 pointer;
569be443 JD	65	u16 words[256]; /* Byte operations use the LSB as per SMBus
569be443 JD	66	specification */
6f16b75a	67	struct list_head smbus_blocks;
9d90c1fd JD	68	};
	69
	70	static struct stub_chip *stub_chips;
1dff5983	71	static int stub_chips_nr;
1da177e4	72
6f16b75a GR	73	static struct smbus_block_data stub_find_block(struct device dev,
	74	struct stub_chip *chip,
	75	u8 command, bool create)
	76	{
	77	struct smbus_block_data b, rb = NULL;
	78
	79	list_for_each_entry(b, &chip->smbus_blocks, node) {
	80	if (b->command == command) {
	81	rb = b;
	82	break;
	83	}
	84	}
	85	if (rb == NULL && create) {
	86	rb = devm_kzalloc(dev, sizeof(*rb), GFP_KERNEL);
	87	if (rb == NULL)
	88	return rb;
	89	rb->command = command;
	90	list_add(&rb->node, &chip->smbus_blocks);
	91	}
	92	return rb;
	93	}
	94
97140342	95	/* Return negative errno on error. */
31d178bf JD	96	static s32 stub_xfer(struct i2c_adapter *adap, u16 addr, unsigned short flags,
31d178bf JD	97	char read_write, u8 command, int size, union i2c_smbus_data *data)
1da177e4 LT	98	{
1da177e4 LT	99	s32 ret;
47103178	100	int i, len;
9d90c1fd	101	struct stub_chip *chip = NULL;
6f16b75a	102	struct smbus_block_data *b;
9d90c1fd JD	103
9d90c1fd JD	104	/* Search for the right chip */
1dff5983	105	for (i = 0; i < stub_chips_nr; i++) {
9d90c1fd JD	106	if (addr == chip_addr[i]) {
	107	chip = stub_chips + i;
	108	break;
	109	}
	110	}
	111	if (!chip)
7a8d29ce JD	112	return -ENODEV;
7a8d29ce JD	113
1da177e4 LT	114	switch (size) {
	115
	116	case I2C_SMBUS_QUICK:
	117	dev_dbg(&adap->dev, "smbus quick - addr 0x%02x\n", addr);
	118	ret = 0;
	119	break;
	120
	121	case I2C_SMBUS_BYTE:
	122	if (read_write == I2C_SMBUS_WRITE) {
9d90c1fd	123	chip->pointer = command;
31d178bf JD	124	dev_dbg(&adap->dev,
	125	"smbus byte - addr 0x%02x, wrote 0x%02x.\n",
	126	addr, command);
1da177e4	127	} else {
569be443	128	data->byte = chip->words[chip->pointer++] & 0xff;
31d178bf JD	129	dev_dbg(&adap->dev,
	130	"smbus byte - addr 0x%02x, read 0x%02x.\n",
	131	addr, data->byte);
1da177e4 LT	132	}
	133
	134	ret = 0;
	135	break;
	136
	137	case I2C_SMBUS_BYTE_DATA:
	138	if (read_write == I2C_SMBUS_WRITE) {
569be443 JD	139	chip->words[command] &= 0xff00;
569be443 JD	140	chip->words[command] \|= data->byte;
31d178bf JD	141	dev_dbg(&adap->dev,
	142	"smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
	143	addr, data->byte, command);
1da177e4	144	} else {
569be443	145	data->byte = chip->words[command] & 0xff;
31d178bf JD	146	dev_dbg(&adap->dev,
	147	"smbus byte data - addr 0x%02x, read 0x%02x at 0x%02x.\n",
	148	addr, data->byte, command);
1da177e4	149	}
9d90c1fd	150	chip->pointer = command + 1;
1da177e4 LT	151
	152	ret = 0;
	153	break;
	154
	155	case I2C_SMBUS_WORD_DATA:
	156	if (read_write == I2C_SMBUS_WRITE) {
9d90c1fd	157	chip->words[command] = data->word;
31d178bf JD	158	dev_dbg(&adap->dev,
	159	"smbus word data - addr 0x%02x, wrote 0x%04x at 0x%02x.\n",
	160	addr, data->word, command);
1da177e4	161	} else {
9d90c1fd	162	data->word = chip->words[command];
31d178bf JD	163	dev_dbg(&adap->dev,
	164	"smbus word data - addr 0x%02x, read 0x%04x at 0x%02x.\n",
	165	addr, data->word, command);
1da177e4 LT	166	}
	167
	168	ret = 0;
	169	break;
	170
47103178 JD	171	case I2C_SMBUS_I2C_BLOCK_DATA:
	172	len = data->block[0];
	173	if (read_write == I2C_SMBUS_WRITE) {
	174	for (i = 0; i < len; i++) {
	175	chip->words[command + i] &= 0xff00;
	176	chip->words[command + i] \|= data->block[1 + i];
	177	}
31d178bf JD	178	dev_dbg(&adap->dev,
	179	"i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
	180	addr, len, command);
47103178 JD	181	} else {
	182	for (i = 0; i < len; i++) {
	183	data->block[1 + i] =
	184	chip->words[command + i] & 0xff;
	185	}
31d178bf JD	186	dev_dbg(&adap->dev,
	187	"i2c block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
	188	addr, len, command);
47103178 JD	189	}
	190
	191	ret = 0;
	192	break;
	193
6f16b75a GR	194	case I2C_SMBUS_BLOCK_DATA:
	195	b = stub_find_block(&adap->dev, chip, command, false);
	196	if (read_write == I2C_SMBUS_WRITE) {
	197	len = data->block[0];
	198	if (len == 0 \|\| len > I2C_SMBUS_BLOCK_MAX) {
	199	ret = -EINVAL;
	200	break;
	201	}
	202	if (b == NULL) {
	203	b = stub_find_block(&adap->dev, chip, command,
	204	true);
	205	if (b == NULL) {
	206	ret = -ENOMEM;
	207	break;
	208	}
	209	}
	210	/* Largest write sets read block length */
	211	if (len > b->len)
	212	b->len = len;
	213	for (i = 0; i < len; i++)
	214	b->block[i] = data->block[i + 1];
	215	/* update for byte and word commands */
	216	chip->words[command] = (b->block[0] << 8) \| b->len;
	217	dev_dbg(&adap->dev,
	218	"smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
	219	addr, len, command);
	220	} else {
	221	if (b == NULL) {
	222	dev_dbg(&adap->dev,
	223	"SMBus block read command without prior block write not supported\n");
	224	ret = -EOPNOTSUPP;
	225	break;
	226	}
	227	len = b->len;
	228	data->block[0] = len;
	229	for (i = 0; i < len; i++)
	230	data->block[i + 1] = b->block[i];
	231	dev_dbg(&adap->dev,
	232	"smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
	233	addr, len, command);
	234	}
	235
	236	ret = 0;
	237	break;
	238
1da177e4 LT	239	default:
1da177e4 LT	240	dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n");
97140342	241	ret = -EOPNOTSUPP;
1da177e4 LT	242	break;
	243	} /* switch (size) */
	244
	245	return ret;
	246	}
	247
	248	static u32 stub_func(struct i2c_adapter *adapter)
	249	{
6f16b75a	250	return STUB_FUNC_ALL & functionality;
1da177e4 LT	251	}
1da177e4 LT	252
8f9082c5	253	static const struct i2c_algorithm smbus_algorithm = {
1da177e4 LT	254	.functionality = stub_func,
	255	.smbus_xfer = stub_xfer,
	256	};
	257
	258	static struct i2c_adapter stub_adapter = {
	259	.owner = THIS_MODULE,
3401b2ff	260	.class = I2C_CLASS_HWMON \| I2C_CLASS_SPD,
1da177e4 LT	261	.algo = &smbus_algorithm,
	262	.name = "SMBus stub driver",
	263	};
	264
	265	static int __init i2c_stub_init(void)
	266	{
9d90c1fd JD	267	int i, ret;
	268
	269	if (!chip_addr[0]) {
31d178bf	270	pr_err("i2c-stub: Please specify a chip address\n");
7a8d29ce JD	271	return -ENODEV;
7a8d29ce JD	272	}
9d90c1fd JD	273
	274	for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
	275	if (chip_addr[i] < 0x03 \|\| chip_addr[i] > 0x77) {
31d178bf JD	276	pr_err("i2c-stub: Invalid chip address 0x%02x\n",
31d178bf JD	277	chip_addr[i]);
9d90c1fd JD	278	return -EINVAL;
	279	}
	280
31d178bf	281	pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr[i]);
7a8d29ce JD	282	}
7a8d29ce JD	283
9d90c1fd	284	/* Allocate memory for all chips at once */
1dff5983 JD	285	stub_chips_nr = i;
	286	stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),
	287	GFP_KERNEL);
9d90c1fd	288	if (!stub_chips) {
31d178bf	289	pr_err("i2c-stub: Out of memory\n");
9d90c1fd JD	290	return -ENOMEM;
9d90c1fd JD	291	}
1dff5983	292	for (i = 0; i < stub_chips_nr; i++)
6f16b75a	293	INIT_LIST_HEAD(&stub_chips[i].smbus_blocks);
9d90c1fd JD	294
	295	ret = i2c_add_adapter(&stub_adapter);
	296	if (ret)
	297	kfree(stub_chips);
	298	return ret;
1da177e4 LT	299	}
	300
	301	static void __exit i2c_stub_exit(void)
	302	{
	303	i2c_del_adapter(&stub_adapter);
9d90c1fd	304	kfree(stub_chips);
1da177e4 LT	305	}
	306
	307	MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
	308	MODULE_DESCRIPTION("I2C stub driver");
	309	MODULE_LICENSE("GPL");
	310
	311	module_init(i2c_stub_init);
	312	module_exit(i2c_stub_exit);