[linux-2.6-block.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.
*/

#define DEBUG 1

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