[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-2014 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");

/* Some chips have banked register ranges */

static u8 bank_reg[MAX_CHIPS];
module_param_array(bank_reg, byte, NULL, S_IRUGO);
MODULE_PARM_DESC(bank_reg, "Bank register");

static u8 bank_mask[MAX_CHIPS];
module_param_array(bank_mask, byte, NULL, S_IRUGO);
MODULE_PARM_DESC(bank_mask, "Bank value mask");

static u8 bank_start[MAX_CHIPS];
module_param_array(bank_start, byte, NULL, S_IRUGO);
MODULE_PARM_DESC(bank_start, "First banked register");

static u8 bank_end[MAX_CHIPS];
module_param_array(bank_end, byte, NULL, S_IRUGO);
MODULE_PARM_DESC(bank_end, "Last banked register");

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;

	/* For chips with banks, extra registers are allocated dynamically */
	u8 bank_reg;
	u8 bank_shift;
	u8 bank_mask;
	u8 bank_sel;		/* Currently selected bank */
	u8 bank_start;
	u8 bank_end;
	u16 bank_size;
	u16 *bank_words;	/* Room for bank_mask * bank_size registers */
};

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;
}

static u16 *stub_get_wordp(struct stub_chip *chip, u8 offset)
{
	if (chip->bank_sel &&
	    offset >= chip->bank_start && offset <= chip->bank_end)
		return chip->bank_words +
		       (chip->bank_sel - 1) * chip->bank_size +
		       offset - chip->bank_start;
	else
		return chip->words + offset;
}

/* 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;
	u16 *wordp;

	/* 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 {
			wordp = stub_get_wordp(chip, chip->pointer++);
			data->byte = *wordp & 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:
		wordp = stub_get_wordp(chip, command);
		if (read_write == I2C_SMBUS_WRITE) {
			*wordp &= 0xff00;
			*wordp |= data->byte;
			dev_dbg(&adap->dev,
				"smbus byte data - addr 0x%02x, wrote 0x%02x at 0x%02x.\n",
				addr, data->byte, command);

			/* Set the bank as needed */
			if (chip->bank_words && command == chip->bank_reg) {
				chip->bank_sel =
					(data->byte >> chip->bank_shift)
					& chip->bank_mask;
				dev_dbg(&adap->dev,
					"switching to bank %u.\n",
					chip->bank_sel);
			}
		} else {
			data->byte = *wordp & 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:
		wordp = stub_get_wordp(chip, command);
		if (read_write == I2C_SMBUS_WRITE) {
			*wordp = 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 = *wordp;
			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:
		/*
		 * We ignore banks here, because banked chips don't use I2C
		 * block transfers
		 */
		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:
		/*
		 * We ignore banks here, because chips typically don't use both
		 * banks and SMBus block transfers
		 */
		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_allocate_banks(int i)
{
	struct stub_chip *chip = stub_chips + i;

	chip->bank_reg = bank_reg[i];
	chip->bank_start = bank_start[i];
	chip->bank_end = bank_end[i];
	chip->bank_size = bank_end[i] - bank_start[i] + 1;

	/* We assume that all bits in the mask are contiguous */
	chip->bank_mask = bank_mask[i];
	while (!(chip->bank_mask & 1)) {
		chip->bank_shift++;
		chip->bank_mask >>= 1;
	}

	chip->bank_words = kzalloc(chip->bank_mask * chip->bank_size *
				   sizeof(u16), GFP_KERNEL);
	if (!chip->bank_words)
		return -ENOMEM;

	pr_debug("i2c-stub: Allocated %u banks of %u words each (registers 0x%02x to 0x%02x)\n",
		 chip->bank_mask, chip->bank_size, chip->bank_start,
		 chip->bank_end);

	return 0;
}

static void i2c_stub_free(void)
{
	int i;

	for (i = 0; i < stub_chips_nr; i++)
		kfree(stub_chips[i].bank_words);
	kfree(stub_chips);
}

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);

		/* Allocate extra memory for banked register ranges */
		if (bank_mask[i]) {
			ret = i2c_stub_allocate_banks(i);
			if (ret)
				goto fail_free;
		}
	}

	ret = i2c_add_adapter(&stub_adapter);
	if (ret)
		goto fail_free;

	return 0;

 fail_free:
	i2c_stub_free();
	return ret;
}

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

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>
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>
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
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 {
9d90c1fd JD	82	u8 pointer;
569be443 JD	83	u16 words[256]; /* Byte operations use the LSB as per SMBus
569be443 JD	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,
31d178bf JD	136	char read_write, u8 command, int size, union i2c_smbus_data *data)
1da177e4 LT	137	{
1da177e4 LT	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
9d90c1fd JD	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;
7a8d29ce JD	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++);
2408c17f JD	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;
2408c17f JD	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	*/
47103178 JD	229	len = data->block[0];
	230	if (read_write == I2C_SMBUS_WRITE) {
	231	for (i = 0; i < len; i++) {
	232	chip->words[command + i] &= 0xff00;
	233	chip->words[command + i] \|= data->block[1 + i];
	234	}
31d178bf JD	235	dev_dbg(&adap->dev,
	236	"i2c block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
	237	addr, len, command);
47103178 JD	238	} else {
	239	for (i = 0; i < len; i++) {
	240	data->block[1 + i] =
	241	chip->words[command + i] & 0xff;
	242	}
31d178bf JD	243	dev_dbg(&adap->dev,
	244	"i2c block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
	245	addr, len, command);
47103178 JD	246	}
	247
	248	ret = 0;
	249	break;
	250
6f16b75a	251	case I2C_SMBUS_BLOCK_DATA:
2408c17f JD	252	/*
	253	* We ignore banks here, because chips typically don't use both
	254	* banks and SMBus block transfers
	255	*/
6f16b75a GR	256	b = stub_find_block(&adap->dev, chip, command, false);
	257	if (read_write == I2C_SMBUS_WRITE) {
	258	len = data->block[0];
	259	if (len == 0 \|\| len > I2C_SMBUS_BLOCK_MAX) {
	260	ret = -EINVAL;
	261	break;
	262	}
	263	if (b == NULL) {
	264	b = stub_find_block(&adap->dev, chip, command,
	265	true);
	266	if (b == NULL) {
	267	ret = -ENOMEM;
	268	break;
	269	}
	270	}
	271	/* Largest write sets read block length */
	272	if (len > b->len)
	273	b->len = len;
	274	for (i = 0; i < len; i++)
	275	b->block[i] = data->block[i + 1];
	276	/* update for byte and word commands */
	277	chip->words[command] = (b->block[0] << 8) \| b->len;
	278	dev_dbg(&adap->dev,
	279	"smbus block data - addr 0x%02x, wrote %d bytes at 0x%02x.\n",
	280	addr, len, command);
	281	} else {
	282	if (b == NULL) {
	283	dev_dbg(&adap->dev,
	284	"SMBus block read command without prior block write not supported\n");
	285	ret = -EOPNOTSUPP;
	286	break;
	287	}
	288	len = b->len;
	289	data->block[0] = len;
	290	for (i = 0; i < len; i++)
	291	data->block[i + 1] = b->block[i];
	292	dev_dbg(&adap->dev,
	293	"smbus block data - addr 0x%02x, read %d bytes at 0x%02x.\n",
	294	addr, len, command);
	295	}
	296
	297	ret = 0;
	298	break;
	299
1da177e4 LT	300	default:
1da177e4 LT	301	dev_dbg(&adap->dev, "Unsupported I2C/SMBus command\n");
97140342	302	ret = -EOPNOTSUPP;
1da177e4 LT	303	break;
	304	} /* switch (size) */
	305
	306	return ret;
	307	}
	308
	309	static u32 stub_func(struct i2c_adapter *adapter)
	310	{
6f16b75a	311	return STUB_FUNC_ALL & functionality;
1da177e4 LT	312	}
1da177e4 LT	313
8f9082c5	314	static const struct i2c_algorithm smbus_algorithm = {
1da177e4 LT	315	.functionality = stub_func,
	316	.smbus_xfer = stub_xfer,
	317	};
	318
	319	static struct i2c_adapter stub_adapter = {
	320	.owner = THIS_MODULE,
3401b2ff	321	.class = I2C_CLASS_HWMON \| I2C_CLASS_SPD,
1da177e4 LT	322	.algo = &smbus_algorithm,
	323	.name = "SMBus stub driver",
	324	};
	325
2408c17f JD	326	static int __init i2c_stub_allocate_banks(int i)
	327	{
	328	struct stub_chip *chip = stub_chips + i;
	329
	330	chip->bank_reg = bank_reg[i];
	331	chip->bank_start = bank_start[i];
	332	chip->bank_end = bank_end[i];
	333	chip->bank_size = bank_end[i] - bank_start[i] + 1;
	334
	335	/* We assume that all bits in the mask are contiguous */
	336	chip->bank_mask = bank_mask[i];
	337	while (!(chip->bank_mask & 1)) {
	338	chip->bank_shift++;
	339	chip->bank_mask >>= 1;
	340	}
	341
	342	chip->bank_words = kzalloc(chip->bank_mask * chip->bank_size *
	343	sizeof(u16), GFP_KERNEL);
	344	if (!chip->bank_words)
	345	return -ENOMEM;
	346
	347	pr_debug("i2c-stub: 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
	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
1da177e4 LT	363	static int __init i2c_stub_init(void)
1da177e4 LT	364	{
9d90c1fd JD	365	int i, ret;
	366
	367	if (!chip_addr[0]) {
31d178bf	368	pr_err("i2c-stub: Please specify a chip address\n");
7a8d29ce JD	369	return -ENODEV;
7a8d29ce JD	370	}
9d90c1fd JD	371
	372	for (i = 0; i < MAX_CHIPS && chip_addr[i]; i++) {
	373	if (chip_addr[i] < 0x03 \|\| chip_addr[i] > 0x77) {
31d178bf JD	374	pr_err("i2c-stub: Invalid chip address 0x%02x\n",
31d178bf JD	375	chip_addr[i]);
9d90c1fd JD	376	return -EINVAL;
	377	}
	378
31d178bf	379	pr_info("i2c-stub: Virtual chip at 0x%02x\n", chip_addr[i]);
7a8d29ce JD	380	}
7a8d29ce JD	381
9d90c1fd	382	/* Allocate memory for all chips at once */
1dff5983 JD	383	stub_chips_nr = i;
	384	stub_chips = kcalloc(stub_chips_nr, sizeof(struct stub_chip),
	385	GFP_KERNEL);
9d90c1fd	386	if (!stub_chips) {
31d178bf	387	pr_err("i2c-stub: Out of memory\n");
9d90c1fd JD	388	return -ENOMEM;
9d90c1fd JD	389	}
2408c17f	390	for (i = 0; i < stub_chips_nr; i++) {
6f16b75a	391	INIT_LIST_HEAD(&stub_chips[i].smbus_blocks);
9d90c1fd	392
2408c17f JD	393	/* Allocate extra memory for banked register ranges */
	394	if (bank_mask[i]) {
	395	ret = i2c_stub_allocate_banks(i);
	396	if (ret)
	397	goto fail_free;
	398	}
	399	}
	400
9d90c1fd JD	401	ret = i2c_add_adapter(&stub_adapter);
9d90c1fd JD	402	if (ret)
2408c17f JD	403	goto fail_free;
	404
	405	return 0;
	406
	407	fail_free:
	408	i2c_stub_free();
9d90c1fd	409	return ret;
1da177e4 LT	410	}
	411
	412	static void __exit i2c_stub_exit(void)
	413	{
	414	i2c_del_adapter(&stub_adapter);
2408c17f	415	i2c_stub_free();
1da177e4 LT	416	}
	417
	418	MODULE_AUTHOR("Mark M. Hoffman <mhoffman@lightlink.com>");
	419	MODULE_DESCRIPTION("I2C stub driver");
	420	MODULE_LICENSE("GPL");
	421
	422	module_init(i2c_stub_init);
	423	module_exit(i2c_stub_exit);