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