[mirror_ubuntu-bionic-kernel.git] / drivers / gpio / gpio-sch311x.c

/*
 * GPIO driver for the SMSC SCH311x Super-I/O chips
 *
 * Copyright (C) 2013 Bruno Randolf <br1@einfach.org>
 *
 * SuperIO functions and chip detection:
 * (c) Copyright 2008 Wim Van Sebroeck <wim@iguana.be>.
 *
 * 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.
 */

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/init.h>
#include <linux/platform_device.h>
#include <linux/gpio.h>
#include <linux/bitops.h>
#include <linux/io.h>

#define DRV_NAME			"gpio-sch311x"

#define SCH311X_GPIO_CONF_OUT		0x00
#define SCH311X_GPIO_CONF_IN		0x01
#define SCH311X_GPIO_CONF_INVERT	0x02
#define SCH311X_GPIO_CONF_OPEN_DRAIN	0x80

#define SIO_CONFIG_KEY_ENTER		0x55
#define SIO_CONFIG_KEY_EXIT		0xaa

#define GP1				0x4b

static int sch311x_ioports[] = { 0x2e, 0x4e, 0x162e, 0x164e };

static struct platform_device *sch311x_gpio_pdev;

struct sch311x_pdev_data {		/* platform device data */
	unsigned short runtime_reg;	/* runtime register base address */
};

struct sch311x_gpio_block {		/* one GPIO block runtime data */
	struct gpio_chip chip;
	unsigned short data_reg;	/* from definition below */
	unsigned short *config_regs;	/* pointer to definition below */
	unsigned short runtime_reg;	/* runtime register */
	spinlock_t lock;		/* lock for this GPIO block */
};

struct sch311x_gpio_priv {		/* driver private data */
	struct sch311x_gpio_block blocks[6];
};

struct sch311x_gpio_block_def {		/* register address definitions */
	unsigned short data_reg;
	unsigned short config_regs[8];
	unsigned short base;
};

/* Note: some GPIOs are not available, these are marked with 0x00 */

static struct sch311x_gpio_block_def sch311x_gpio_blocks[] = {
	{
		.data_reg = 0x4b,	/* GP1 */
		.config_regs = {0x23, 0x24, 0x25, 0x26, 0x27, 0x29, 0x2a, 0x2b},
		.base = 10,
	},
	{
		.data_reg = 0x4c,	/* GP2 */
		.config_regs = {0x00, 0x2c, 0x2d, 0x00, 0x00, 0x00, 0x00, 0x32},
		.base = 20,
	},
	{
		.data_reg = 0x4d,	/* GP3 */
		.config_regs = {0x33, 0x34, 0x35, 0x36, 0x37, 0x00, 0x39, 0x3a},
		.base = 30,
	},
	{
		.data_reg = 0x4e,	/* GP4 */
		.config_regs = {0x3b, 0x00, 0x3d, 0x00, 0x6e, 0x6f, 0x72, 0x73},
		.base = 40,
	},
	{
		.data_reg = 0x4f,	/* GP5 */
		.config_regs = {0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46},
		.base = 50,
	},
	{
		.data_reg = 0x50,	/* GP6 */
		.config_regs = {0x47, 0x48, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59},
		.base = 60,
	},
};

static inline struct sch311x_gpio_block *
to_sch311x_gpio_block(struct gpio_chip *chip)
{
	return container_of(chip, struct sch311x_gpio_block, chip);
}


/*
 *	Super-IO functions
 */

static inline int sch311x_sio_enter(int sio_config_port)
{
	/* Don't step on other drivers' I/O space by accident. */
	if (!request_muxed_region(sio_config_port, 2, DRV_NAME)) {
		pr_err(DRV_NAME "I/O address 0x%04x already in use\n",
		       sio_config_port);
		return -EBUSY;
	}

	outb(SIO_CONFIG_KEY_ENTER, sio_config_port);
	return 0;
}

static inline void sch311x_sio_exit(int sio_config_port)
{
	outb(SIO_CONFIG_KEY_EXIT, sio_config_port);
	release_region(sio_config_port, 2);
}

static inline int sch311x_sio_inb(int sio_config_port, int reg)
{
	outb(reg, sio_config_port);
	return inb(sio_config_port + 1);
}

static inline void sch311x_sio_outb(int sio_config_port, int reg, int val)
{
	outb(reg, sio_config_port);
	outb(val, sio_config_port + 1);
}


/*
 *	GPIO functions
 */

static int sch311x_gpio_request(struct gpio_chip *chip, unsigned offset)
{
	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);

	if (block->config_regs[offset] == 0) /* GPIO is not available */
		return -ENODEV;

	if (!request_region(block->runtime_reg + block->config_regs[offset],
			    1, DRV_NAME)) {
		dev_err(chip->dev, "Failed to request region 0x%04x.\n",
			block->runtime_reg + block->config_regs[offset]);
		return -EBUSY;
	}
	return 0;
}

static void sch311x_gpio_free(struct gpio_chip *chip, unsigned offset)
{
	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);

	if (block->config_regs[offset] == 0) /* GPIO is not available */
		return;

	release_region(block->runtime_reg + block->config_regs[offset], 1);
}

static int sch311x_gpio_get(struct gpio_chip *chip, unsigned offset)
{
	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
	unsigned char data;

	spin_lock(&block->lock);
	data = inb(block->runtime_reg + block->data_reg);
	spin_unlock(&block->lock);

	return !!(data & BIT(offset));
}

static void __sch311x_gpio_set(struct sch311x_gpio_block *block,
			       unsigned offset, int value)
{
	unsigned char data = inb(block->runtime_reg + block->data_reg);
	if (value)
		data |= BIT(offset);
	else
		data &= ~BIT(offset);
	outb(data, block->runtime_reg + block->data_reg);
}

static void sch311x_gpio_set(struct gpio_chip *chip, unsigned offset,
			     int value)
{
	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);

	spin_lock(&block->lock);
	 __sch311x_gpio_set(block, offset, value);
	spin_unlock(&block->lock);
}

static int sch311x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
{
	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);

	spin_lock(&block->lock);
	outb(SCH311X_GPIO_CONF_IN, block->runtime_reg +
	     block->config_regs[offset]);
	spin_unlock(&block->lock);

	return 0;
}

static int sch311x_gpio_direction_out(struct gpio_chip *chip, unsigned offset,
				      int value)
{
	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);

	spin_lock(&block->lock);

	outb(SCH311X_GPIO_CONF_OUT, block->runtime_reg +
	     block->config_regs[offset]);

	__sch311x_gpio_set(block, offset, value);

	spin_unlock(&block->lock);
	return 0;
}

static int sch311x_gpio_probe(struct platform_device *pdev)
{
	struct sch311x_pdev_data *pdata = pdev->dev.platform_data;
	struct sch311x_gpio_priv *priv;
	struct sch311x_gpio_block *block;
	int err, i;

	/* we can register all GPIO data registers at once */
	if (!request_region(pdata->runtime_reg + GP1, 6, DRV_NAME)) {
		dev_err(&pdev->dev, "Failed to request region 0x%04x-0x%04x.\n",
			pdata->runtime_reg + GP1, pdata->runtime_reg + GP1 + 5);
		return -EBUSY;
	}

	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
	if (!priv)
		return -ENOMEM;

	platform_set_drvdata(pdev, priv);

	for (i = 0; i < ARRAY_SIZE(priv->blocks); i++) {
		block = &priv->blocks[i];

		spin_lock_init(&block->lock);

		block->chip.label = DRV_NAME;
		block->chip.owner = THIS_MODULE;
		block->chip.request = sch311x_gpio_request;
		block->chip.free = sch311x_gpio_free;
		block->chip.direction_input = sch311x_gpio_direction_in;
		block->chip.direction_output = sch311x_gpio_direction_out;
		block->chip.get = sch311x_gpio_get;
		block->chip.set = sch311x_gpio_set;
		block->chip.ngpio = 8;
		block->chip.dev = &pdev->dev;
		block->chip.base = sch311x_gpio_blocks[i].base;
		block->config_regs = sch311x_gpio_blocks[i].config_regs;
		block->data_reg = sch311x_gpio_blocks[i].data_reg;
		block->runtime_reg = pdata->runtime_reg;

		err = gpiochip_add(&block->chip);
		if (err < 0) {
			dev_err(&pdev->dev,
				"Could not register gpiochip, %d\n", err);
			goto exit_err;
		}
		dev_info(&pdev->dev,
			 "SMSC SCH311x GPIO block %d registered.\n", i);
	}

	return 0;

exit_err:
	release_region(pdata->runtime_reg + GP1, 6);
	/* release already registered chips */
	for (--i; i >= 0; i--)
		gpiochip_remove(&priv->blocks[i].chip);
	return err;
}

static int sch311x_gpio_remove(struct platform_device *pdev)
{
	struct sch311x_pdev_data *pdata = pdev->dev.platform_data;
	struct sch311x_gpio_priv *priv = platform_get_drvdata(pdev);
	int err, i;

	release_region(pdata->runtime_reg + GP1, 6);

	for (i = 0; i < ARRAY_SIZE(priv->blocks); i++) {
		err = gpiochip_remove(&priv->blocks[i].chip);
		if (err)
			return err;
		dev_info(&pdev->dev,
			 "SMSC SCH311x GPIO block %d unregistered.\n", i);
	}
	return 0;
}

static struct platform_driver sch311x_gpio_driver = {
	.driver.name	= DRV_NAME,
	.driver.owner	= THIS_MODULE,
	.probe		= sch311x_gpio_probe,
	.remove		= sch311x_gpio_remove,
};


/*
 *	Init & exit routines
 */

static int __init sch311x_detect(int sio_config_port, unsigned short *addr)
{
	int err = 0, reg;
	unsigned short base_addr;
	unsigned char dev_id;

	err = sch311x_sio_enter(sio_config_port);
	if (err)
		return err;

	/* Check device ID. */
	reg = sch311x_sio_inb(sio_config_port, 0x20);
	switch (reg) {
	case 0x7c: /* SCH3112 */
		dev_id = 2;
		break;
	case 0x7d: /* SCH3114 */
		dev_id = 4;
		break;
	case 0x7f: /* SCH3116 */
		dev_id = 6;
		break;
	default:
		err = -ENODEV;
		goto exit;
	}

	/* Select logical device A (runtime registers) */
	sch311x_sio_outb(sio_config_port, 0x07, 0x0a);

	/* Check if Logical Device Register is currently active */
	if ((sch311x_sio_inb(sio_config_port, 0x30) & 0x01) == 0)
		pr_info("Seems that LDN 0x0a is not active...\n");

	/* Get the base address of the runtime registers */
	base_addr = (sch311x_sio_inb(sio_config_port, 0x60) << 8) |
			   sch311x_sio_inb(sio_config_port, 0x61);
	if (!base_addr) {
		pr_err("Base address not set\n");
		err = -ENODEV;
		goto exit;
	}
	*addr = base_addr;

	pr_info("Found an SMSC SCH311%d chip at 0x%04x\n", dev_id, base_addr);

exit:
	sch311x_sio_exit(sio_config_port);
	return err;
}

static int __init sch311x_gpio_pdev_add(const unsigned short addr)
{
	struct sch311x_pdev_data pdata;
	int err;

	pdata.runtime_reg = addr;

	sch311x_gpio_pdev = platform_device_alloc(DRV_NAME, -1);
	if (!sch311x_gpio_pdev)
		return -ENOMEM;

	err = platform_device_add_data(sch311x_gpio_pdev,
				       &pdata, sizeof(pdata));
	if (err) {
		pr_err(DRV_NAME "Platform data allocation failed\n");
		goto err;
	}

	err = platform_device_add(sch311x_gpio_pdev);
	if (err) {
		pr_err(DRV_NAME "Device addition failed\n");
		goto err;
	}
	return 0;

err:
	platform_device_put(sch311x_gpio_pdev);
	return err;
}

static int __init sch311x_gpio_init(void)
{
	int err, i;
	unsigned short addr = 0;

	for (i = 0; i < ARRAY_SIZE(sch311x_ioports); i++)
		if (sch311x_detect(sch311x_ioports[i], &addr) == 0)
			break;

	if (!addr)
		return -ENODEV;

	err = platform_driver_register(&sch311x_gpio_driver);
	if (err)
		return err;

	err = sch311x_gpio_pdev_add(addr);
	if (err)
		goto unreg_platform_driver;

	return 0;

unreg_platform_driver:
	platform_driver_unregister(&sch311x_gpio_driver);
	return err;
}

static void __exit sch311x_gpio_exit(void)
{
	platform_device_unregister(sch311x_gpio_pdev);
	platform_driver_unregister(&sch311x_gpio_driver);
}

module_init(sch311x_gpio_init);
module_exit(sch311x_gpio_exit);

MODULE_AUTHOR("Bruno Randolf <br1@einfach.org>");
MODULE_DESCRIPTION("SMSC SCH311x GPIO Driver");
MODULE_LICENSE("GPL");
MODULE_ALIAS("platform:gpio-sch311x");
Commit	Line	Data
12262bef BR	1	/*
	2	* GPIO driver for the SMSC SCH311x Super-I/O chips
	3	*
	4	* Copyright (C) 2013 Bruno Randolf <br1@einfach.org>
	5	*
	6	* SuperIO functions and chip detection:
	7	* (c) Copyright 2008 Wim Van Sebroeck <wim@iguana.be>.
	8	*
	9	* This program is free software; you can redistribute it and/or modify
	10	* it under the terms of the GNU General Public License as published by
	11	* the Free Software Foundation; either version 2 of the License, or
	12	* (at your option) any later version.
	13	*/
	14
	15	#include <linux/module.h>
	16	#include <linux/kernel.h>
	17	#include <linux/init.h>
	18	#include <linux/platform_device.h>
	19	#include <linux/gpio.h>
	20	#include <linux/bitops.h>
523639e6	21	#include <linux/io.h>
12262bef BR	22
	23	#define DRV_NAME "gpio-sch311x"
	24
	25	#define SCH311X_GPIO_CONF_OUT 0x00
	26	#define SCH311X_GPIO_CONF_IN 0x01
	27	#define SCH311X_GPIO_CONF_INVERT 0x02
	28	#define SCH311X_GPIO_CONF_OPEN_DRAIN 0x80
	29
	30	#define SIO_CONFIG_KEY_ENTER 0x55
	31	#define SIO_CONFIG_KEY_EXIT 0xaa
	32
	33	#define GP1 0x4b
	34
	35	static int sch311x_ioports[] = { 0x2e, 0x4e, 0x162e, 0x164e };
	36
	37	static struct platform_device *sch311x_gpio_pdev;
	38
	39	struct sch311x_pdev_data { /* platform device data */
	40	unsigned short runtime_reg; /* runtime register base address */
	41	};
	42
	43	struct sch311x_gpio_block { /* one GPIO block runtime data */
	44	struct gpio_chip chip;
	45	unsigned short data_reg; /* from definition below */
	46	unsigned short config_regs; / pointer to definition below */
	47	unsigned short runtime_reg; /* runtime register */
	48	spinlock_t lock; /* lock for this GPIO block */
	49	};
	50
	51	struct sch311x_gpio_priv { /* driver private data */
	52	struct sch311x_gpio_block blocks[6];
	53	};
	54
	55	struct sch311x_gpio_block_def { /* register address definitions */
	56	unsigned short data_reg;
	57	unsigned short config_regs[8];
	58	unsigned short base;
	59	};
	60
	61	/* Note: some GPIOs are not available, these are marked with 0x00 */
	62
	63	static struct sch311x_gpio_block_def sch311x_gpio_blocks[] = {
	64	{
	65	.data_reg = 0x4b, /* GP1 */
	66	.config_regs = {0x23, 0x24, 0x25, 0x26, 0x27, 0x29, 0x2a, 0x2b},
	67	.base = 10,
	68	},
	69	{
	70	.data_reg = 0x4c, /* GP2 */
	71	.config_regs = {0x00, 0x2c, 0x2d, 0x00, 0x00, 0x00, 0x00, 0x32},
	72	.base = 20,
	73	},
	74	{
	75	.data_reg = 0x4d, /* GP3 */
	76	.config_regs = {0x33, 0x34, 0x35, 0x36, 0x37, 0x00, 0x39, 0x3a},
	77	.base = 30,
	78	},
	79	{
	80	.data_reg = 0x4e, /* GP4 */
	81	.config_regs = {0x3b, 0x00, 0x3d, 0x00, 0x6e, 0x6f, 0x72, 0x73},
	82	.base = 40,
	83	},
	84	{
	85	.data_reg = 0x4f, /* GP5 */
86	.config_regs = {0x3f, 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46},
87	.base = 50,
88	},
89	{
90	.data_reg = 0x50, /* GP6 */
91	.config_regs = {0x47, 0x48, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59},
92	.base = 60,
93	},
94	};
95
96	static inline struct sch311x_gpio_block *
97	to_sch311x_gpio_block(struct gpio_chip *chip)
98	{
99	return container_of(chip, struct sch311x_gpio_block, chip);
100	}
101
102
103	/*
104	* Super-IO functions
105	*/
106
107	static inline int sch311x_sio_enter(int sio_config_port)
108	{
109	/* Don't step on other drivers' I/O space by accident. */
110	if (!request_muxed_region(sio_config_port, 2, DRV_NAME)) {
111	pr_err(DRV_NAME "I/O address 0x%04x already in use\n",
112	sio_config_port);
113	return -EBUSY;
114	}
115
116	outb(SIO_CONFIG_KEY_ENTER, sio_config_port);
117	return 0;
118	}
119
120	static inline void sch311x_sio_exit(int sio_config_port)
121	{
122	outb(SIO_CONFIG_KEY_EXIT, sio_config_port);
123	release_region(sio_config_port, 2);
124	}
125
126	static inline int sch311x_sio_inb(int sio_config_port, int reg)
127	{
128	outb(reg, sio_config_port);
129	return inb(sio_config_port + 1);
130	}
131
132	static inline void sch311x_sio_outb(int sio_config_port, int reg, int val)
133	{
134	outb(reg, sio_config_port);
135	outb(val, sio_config_port + 1);
136	}
137
138
139	/*
140	* GPIO functions
141	*/
142
143	static int sch311x_gpio_request(struct gpio_chip *chip, unsigned offset)
144	{
145	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
146
147	if (block->config_regs[offset] == 0) /* GPIO is not available */
148	return -ENODEV;
149
150	if (!request_region(block->runtime_reg + block->config_regs[offset],
151	1, DRV_NAME)) {
152	dev_err(chip->dev, "Failed to request region 0x%04x.\n",
153	block->runtime_reg + block->config_regs[offset]);
154	return -EBUSY;
155	}
156	return 0;
157	}
158
159	static void sch311x_gpio_free(struct gpio_chip *chip, unsigned offset)
160	{
161	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
162
163	if (block->config_regs[offset] == 0) /* GPIO is not available */
164	return;
165
166	release_region(block->runtime_reg + block->config_regs[offset], 1);
167	}
168
169	static int sch311x_gpio_get(struct gpio_chip *chip, unsigned offset)
170	{
171	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
172	unsigned char data;
173
174	spin_lock(&block->lock);
175	data = inb(block->runtime_reg + block->data_reg);
176	spin_unlock(&block->lock);
177
178	return !!(data & BIT(offset));
179	}
180
181	static void __sch311x_gpio_set(struct sch311x_gpio_block *block,
182	unsigned offset, int value)
183	{
184	unsigned char data = inb(block->runtime_reg + block->data_reg);
185	if (value)
186	data \|= BIT(offset);
187	else
188	data &= ~BIT(offset);
189	outb(data, block->runtime_reg + block->data_reg);
190	}
191
192	static void sch311x_gpio_set(struct gpio_chip *chip, unsigned offset,
193	int value)
194	{
195	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
196
197	spin_lock(&block->lock);
198	__sch311x_gpio_set(block, offset, value);
199	spin_unlock(&block->lock);
200	}
201
202	static int sch311x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
203	{
204	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
205
206	spin_lock(&block->lock);
207	outb(SCH311X_GPIO_CONF_IN, block->runtime_reg +
208	block->config_regs[offset]);
209	spin_unlock(&block->lock);
210
211	return 0;
212	}
213
214	static int sch311x_gpio_direction_out(struct gpio_chip *chip, unsigned offset,
215	int value)
216	{
217	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
218
219	spin_lock(&block->lock);
220
221	outb(SCH311X_GPIO_CONF_OUT, block->runtime_reg +
222	block->config_regs[offset]);
223
224	__sch311x_gpio_set(block, offset, value);
225
226	spin_unlock(&block->lock);
227	return 0;
228	}
229
230	static int sch311x_gpio_probe(struct platform_device *pdev)
231	{
232	struct sch311x_pdev_data *pdata = pdev->dev.platform_data;
233	struct sch311x_gpio_priv *priv;
234	struct sch311x_gpio_block *block;
235	int err, i;
236
237	/* we can register all GPIO data registers at once */
238	if (!request_region(pdata->runtime_reg + GP1, 6, DRV_NAME)) {
239	dev_err(&pdev->dev, "Failed to request region 0x%04x-0x%04x.\n",
240	pdata->runtime_reg + GP1, pdata->runtime_reg + GP1 + 5);
241	return -EBUSY;
242	}
243
244	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
245	if (!priv)
246	return -ENOMEM;
247
248	platform_set_drvdata(pdev, priv);
249
250	for (i = 0; i < ARRAY_SIZE(priv->blocks); i++) {
251	block = &priv->blocks[i];
252
253	spin_lock_init(&block->lock);
254
255	block->chip.label = DRV_NAME;
256	block->chip.owner = THIS_MODULE;
257	block->chip.request = sch311x_gpio_request;
258	block->chip.free = sch311x_gpio_free;
259	block->chip.direction_input = sch311x_gpio_direction_in;
260	block->chip.direction_output = sch311x_gpio_direction_out;
261	block->chip.get = sch311x_gpio_get;
262	block->chip.set = sch311x_gpio_set;
263	block->chip.ngpio = 8;
264	block->chip.dev = &pdev->dev;
265	block->chip.base = sch311x_gpio_blocks[i].base;
266	block->config_regs = sch311x_gpio_blocks[i].config_regs;
267	block->data_reg = sch311x_gpio_blocks[i].data_reg;
268	block->runtime_reg = pdata->runtime_reg;
269
270	err = gpiochip_add(&block->chip);
271	if (err < 0) {
272	dev_err(&pdev->dev,
273	"Could not register gpiochip, %d\n", err);
274	goto exit_err;
275	}
276	dev_info(&pdev->dev,
277	"SMSC SCH311x GPIO block %d registered.\n", i);
278	}
279
280	return 0;
281
282	exit_err:
283	release_region(pdata->runtime_reg + GP1, 6);
284	/* release already registered chips */
285	for (--i; i >= 0; i--)
286	gpiochip_remove(&priv->blocks[i].chip);
287	return err;
288	}
289
290	static int sch311x_gpio_remove(struct platform_device *pdev)
291	{
292	struct sch311x_pdev_data *pdata = pdev->dev.platform_data;
293	struct sch311x_gpio_priv *priv = platform_get_drvdata(pdev);
294	int err, i;
295
296	release_region(pdata->runtime_reg + GP1, 6);
297
298	for (i = 0; i < ARRAY_SIZE(priv->blocks); i++) {
299	err = gpiochip_remove(&priv->blocks[i].chip);
300	if (err)
301	return err;
302	dev_info(&pdev->dev,
303	"SMSC SCH311x GPIO block %d unregistered.\n", i);
304	}
305	return 0;
306	}
307
308	static struct platform_driver sch311x_gpio_driver = {
309	.driver.name = DRV_NAME,
310	.driver.owner = THIS_MODULE,
311	.probe = sch311x_gpio_probe,
312	.remove = sch311x_gpio_remove,
313	};
314
315
316	/*
317	* Init & exit routines
318	*/
319
320	static int __init sch311x_detect(int sio_config_port, unsigned short *addr)
321	{
322	int err = 0, reg;
323	unsigned short base_addr;
324	unsigned char dev_id;
325
326	err = sch311x_sio_enter(sio_config_port);
327	if (err)
328	return err;
329
7e9fcc9d	330	/* Check device ID. */
12262bef	331	reg = sch311x_sio_inb(sio_config_port, 0x20);
7e9fcc9d AL	332	switch (reg) {
	333	case 0x7c: /* SCH3112 */
	334	dev_id = 2;
	335	break;
	336	case 0x7d: /* SCH3114 */
	337	dev_id = 4;
	338	break;
	339	case 0x7f: /* SCH3116 */
	340	dev_id = 6;
	341	break;
	342	default:
12262bef BR	343	err = -ENODEV;
	344	goto exit;
	345	}
12262bef BR	346
	347	/* Select logical device A (runtime registers) */
	348	sch311x_sio_outb(sio_config_port, 0x07, 0x0a);
	349
	350	/* Check if Logical Device Register is currently active */
	351	if ((sch311x_sio_inb(sio_config_port, 0x30) & 0x01) == 0)
	352	pr_info("Seems that LDN 0x0a is not active...\n");
	353
	354	/* Get the base address of the runtime registers */
	355	base_addr = (sch311x_sio_inb(sio_config_port, 0x60) << 8) \|
	356	sch311x_sio_inb(sio_config_port, 0x61);
	357	if (!base_addr) {
	358	pr_err("Base address not set\n");
	359	err = -ENODEV;
	360	goto exit;
	361	}
	362	*addr = base_addr;
	363
	364	pr_info("Found an SMSC SCH311%d chip at 0x%04x\n", dev_id, base_addr);
	365
	366	exit:
	367	sch311x_sio_exit(sio_config_port);
	368	return err;
	369	}
	370
	371	static int __init sch311x_gpio_pdev_add(const unsigned short addr)
	372	{
	373	struct sch311x_pdev_data pdata;
	374	int err;
	375
	376	pdata.runtime_reg = addr;
	377
	378	sch311x_gpio_pdev = platform_device_alloc(DRV_NAME, -1);
	379	if (!sch311x_gpio_pdev)
	380	return -ENOMEM;
	381
	382	err = platform_device_add_data(sch311x_gpio_pdev,
	383	&pdata, sizeof(pdata));
	384	if (err) {
	385	pr_err(DRV_NAME "Platform data allocation failed\n");
	386	goto err;
	387	}
	388
	389	err = platform_device_add(sch311x_gpio_pdev);
	390	if (err) {
	391	pr_err(DRV_NAME "Device addition failed\n");
	392	goto err;
	393	}
	394	return 0;
	395
	396	err:
	397	platform_device_put(sch311x_gpio_pdev);
	398	return err;
	399	}
	400
	401	static int __init sch311x_gpio_init(void)
	402	{
	403	int err, i;
	404	unsigned short addr = 0;
	405
	406	for (i = 0; i < ARRAY_SIZE(sch311x_ioports); i++)
	407	if (sch311x_detect(sch311x_ioports[i], &addr) == 0)
	408	break;
	409
410	if (!addr)
411	return -ENODEV;
412
413	err = platform_driver_register(&sch311x_gpio_driver);
414	if (err)
415	return err;
416
417	err = sch311x_gpio_pdev_add(addr);
418	if (err)
419	goto unreg_platform_driver;
420
421	return 0;
422
423	unreg_platform_driver:
424	platform_driver_unregister(&sch311x_gpio_driver);
425	return err;
426	}
427
428	static void __exit sch311x_gpio_exit(void)
429	{
430	platform_device_unregister(sch311x_gpio_pdev);
431	platform_driver_unregister(&sch311x_gpio_driver);
432	}
433
434	module_init(sch311x_gpio_init);
435	module_exit(sch311x_gpio_exit);
436
437	MODULE_AUTHOR("Bruno Randolf <br1@einfach.org>");
438	MODULE_DESCRIPTION("SMSC SCH311x GPIO Driver");
439	MODULE_LICENSE("GPL");
440	MODULE_ALIAS("platform:gpio-sch311x");