[linux-2.6-block.git] / drivers / input / keyboard / matrix_keypad.c

/*
 *  GPIO driven matrix keyboard driver
 *
 *  Copyright (c) 2008 Marek Vasut <marek.vasut@gmail.com>
 *
 *  Based on corgikbd.c
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License version 2 as
 *  published by the Free Software Foundation.
 *
 */

#include <linux/types.h>
#include <linux/delay.h>
#include <linux/platform_device.h>
#include <linux/init.h>
#include <linux/input.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/jiffies.h>
#include <linux/module.h>
#include <linux/gpio.h>
#include <linux/input/matrix_keypad.h>

struct matrix_keypad {
	const struct matrix_keypad_platform_data *pdata;
	struct input_dev *input_dev;
	unsigned short *keycodes;
	unsigned int row_shift;

	uint32_t last_key_state[MATRIX_MAX_COLS];
	struct delayed_work work;
	bool scan_pending;
	bool stopped;
	spinlock_t lock;
};

/*
 * NOTE: normally the GPIO has to be put into HiZ when de-activated to cause
 * minmal side effect when scanning other columns, here it is configured to
 * be input, and it should work on most platforms.
 */
static void __activate_col(const struct matrix_keypad_platform_data *pdata,
			   int col, bool on)
{
	bool level_on = !pdata->active_low;

	if (on) {
		gpio_direction_output(pdata->col_gpios[col], level_on);
	} else {
		gpio_set_value_cansleep(pdata->col_gpios[col], !level_on);
		gpio_direction_input(pdata->col_gpios[col]);
	}
}

static void activate_col(const struct matrix_keypad_platform_data *pdata,
			 int col, bool on)
{
	__activate_col(pdata, col, on);

	if (on && pdata->col_scan_delay_us)
		udelay(pdata->col_scan_delay_us);
}

static void activate_all_cols(const struct matrix_keypad_platform_data *pdata,
			      bool on)
{
	int col;

	for (col = 0; col < pdata->num_col_gpios; col++)
		__activate_col(pdata, col, on);
}

static bool row_asserted(const struct matrix_keypad_platform_data *pdata,
			 int row)
{
	return gpio_get_value_cansleep(pdata->row_gpios[row]) ?
			!pdata->active_low : pdata->active_low;
}

static void enable_row_irqs(struct matrix_keypad *keypad)
{
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	int i;

	for (i = 0; i < pdata->num_row_gpios; i++)
		enable_irq(gpio_to_irq(pdata->row_gpios[i]));
}

static void disable_row_irqs(struct matrix_keypad *keypad)
{
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	int i;

	for (i = 0; i < pdata->num_row_gpios; i++)
		disable_irq_nosync(gpio_to_irq(pdata->row_gpios[i]));
}

/*
 * This gets the keys from keyboard and reports it to input subsystem
 */
static void matrix_keypad_scan(struct work_struct *work)
{
	struct matrix_keypad *keypad =
		container_of(work, struct matrix_keypad, work.work);
	struct input_dev *input_dev = keypad->input_dev;
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	uint32_t new_state[MATRIX_MAX_COLS];
	int row, col, code;

	/* de-activate all columns for scanning */
	activate_all_cols(pdata, false);

	memset(new_state, 0, sizeof(new_state));

	/* assert each column and read the row status out */
	for (col = 0; col < pdata->num_col_gpios; col++) {

		activate_col(pdata, col, true);

		for (row = 0; row < pdata->num_row_gpios; row++)
			new_state[col] |=
				row_asserted(pdata, row) ? (1 << row) : 0;

		activate_col(pdata, col, false);
	}

	for (col = 0; col < pdata->num_col_gpios; col++) {
		uint32_t bits_changed;

		bits_changed = keypad->last_key_state[col] ^ new_state[col];
		if (bits_changed == 0)
			continue;

		for (row = 0; row < pdata->num_row_gpios; row++) {
			if ((bits_changed & (1 << row)) == 0)
				continue;

			code = MATRIX_SCAN_CODE(row, col, keypad->row_shift);
			input_event(input_dev, EV_MSC, MSC_SCAN, code);
			input_report_key(input_dev,
					 keypad->keycodes[code],
					 new_state[col] & (1 << row));
		}
	}
	input_sync(input_dev);

	memcpy(keypad->last_key_state, new_state, sizeof(new_state));

	activate_all_cols(pdata, true);

	/* Enable IRQs again */
	spin_lock_irq(&keypad->lock);
	keypad->scan_pending = false;
	enable_row_irqs(keypad);
	spin_unlock_irq(&keypad->lock);
}

static irqreturn_t matrix_keypad_interrupt(int irq, void *id)
{
	struct matrix_keypad *keypad = id;
	unsigned long flags;

	spin_lock_irqsave(&keypad->lock, flags);

	/*
	 * See if another IRQ beaten us to it and scheduled the
	 * scan already. In that case we should not try to
	 * disable IRQs again.
	 */
	if (unlikely(keypad->scan_pending || keypad->stopped))
		goto out;

	disable_row_irqs(keypad);
	keypad->scan_pending = true;
	schedule_delayed_work(&keypad->work,
		msecs_to_jiffies(keypad->pdata->debounce_ms));

out:
	spin_unlock_irqrestore(&keypad->lock, flags);
	return IRQ_HANDLED;
}

static int matrix_keypad_start(struct input_dev *dev)
{
	struct matrix_keypad *keypad = input_get_drvdata(dev);

	keypad->stopped = false;
	mb();

	/*
	 * Schedule an immediate key scan to capture current key state;
	 * columns will be activated and IRQs be enabled after the scan.
	 */
	schedule_delayed_work(&keypad->work, 0);

	return 0;
}

static void matrix_keypad_stop(struct input_dev *dev)
{
	struct matrix_keypad *keypad = input_get_drvdata(dev);

	keypad->stopped = true;
	mb();
	flush_work(&keypad->work.work);
	/*
	 * matrix_keypad_scan() will leave IRQs enabled;
	 * we should disable them now.
	 */
	disable_row_irqs(keypad);
}

#ifdef CONFIG_PM
static int matrix_keypad_suspend(struct platform_device *pdev, pm_message_t state)
{
	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	int i;

	matrix_keypad_stop(keypad->input_dev);

	if (device_may_wakeup(&pdev->dev))
		for (i = 0; i < pdata->num_row_gpios; i++)
			enable_irq_wake(gpio_to_irq(pdata->row_gpios[i]));

	return 0;
}

static int matrix_keypad_resume(struct platform_device *pdev)
{
	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	int i;

	if (device_may_wakeup(&pdev->dev))
		for (i = 0; i < pdata->num_row_gpios; i++)
			disable_irq_wake(gpio_to_irq(pdata->row_gpios[i]));

	matrix_keypad_start(keypad->input_dev);

	return 0;
}
#else
#define matrix_keypad_suspend	NULL
#define matrix_keypad_resume	NULL
#endif

static int __devinit init_matrix_gpio(struct platform_device *pdev,
					struct matrix_keypad *keypad)
{
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	int i, err = -EINVAL;

	/* initialized strobe lines as outputs, activated */
	for (i = 0; i < pdata->num_col_gpios; i++) {
		err = gpio_request(pdata->col_gpios[i], "matrix_kbd_col");
		if (err) {
			dev_err(&pdev->dev,
				"failed to request GPIO%d for COL%d\n",
				pdata->col_gpios[i], i);
			goto err_free_cols;
		}

		gpio_direction_output(pdata->col_gpios[i], !pdata->active_low);
	}

	for (i = 0; i < pdata->num_row_gpios; i++) {
		err = gpio_request(pdata->row_gpios[i], "matrix_kbd_row");
		if (err) {
			dev_err(&pdev->dev,
				"failed to request GPIO%d for ROW%d\n",
				pdata->row_gpios[i], i);
			goto err_free_rows;
		}

		gpio_direction_input(pdata->row_gpios[i]);
	}

	for (i = 0; i < pdata->num_row_gpios; i++) {
		err = request_irq(gpio_to_irq(pdata->row_gpios[i]),
				matrix_keypad_interrupt,
				IRQF_DISABLED |
				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
				"matrix-keypad", keypad);
		if (err) {
			dev_err(&pdev->dev,
				"Unable to acquire interrupt for GPIO line %i\n",
				pdata->row_gpios[i]);
			goto err_free_irqs;
		}
	}

	/* initialized as disabled - enabled by input->open */
	disable_row_irqs(keypad);
	return 0;

err_free_irqs:
	while (--i >= 0)
		free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
	i = pdata->num_row_gpios;
err_free_rows:
	while (--i >= 0)
		gpio_free(pdata->row_gpios[i]);
	i = pdata->num_col_gpios;
err_free_cols:
	while (--i >= 0)
		gpio_free(pdata->col_gpios[i]);

	return err;
}

static int __devinit matrix_keypad_probe(struct platform_device *pdev)
{
	const struct matrix_keypad_platform_data *pdata;
	const struct matrix_keymap_data *keymap_data;
	struct matrix_keypad *keypad;
	struct input_dev *input_dev;
	unsigned short *keycodes;
	unsigned int row_shift;
	int i;
	int err;

	pdata = pdev->dev.platform_data;
	if (!pdata) {
		dev_err(&pdev->dev, "no platform data defined\n");
		return -EINVAL;
	}

	keymap_data = pdata->keymap_data;
	if (!keymap_data) {
		dev_err(&pdev->dev, "no keymap data defined\n");
		return -EINVAL;
	}

	row_shift = get_count_order(pdata->num_col_gpios);

	keypad = kzalloc(sizeof(struct matrix_keypad), GFP_KERNEL);
	keycodes = kzalloc((pdata->num_row_gpios << row_shift) *
				sizeof(*keycodes),
			   GFP_KERNEL);
	input_dev = input_allocate_device();
	if (!keypad || !keycodes || !input_dev) {
		err = -ENOMEM;
		goto err_free_mem;
	}

	keypad->input_dev = input_dev;
	keypad->pdata = pdata;
	keypad->keycodes = keycodes;
	keypad->row_shift = row_shift;
	keypad->stopped = true;
	INIT_DELAYED_WORK(&keypad->work, matrix_keypad_scan);
	spin_lock_init(&keypad->lock);

	input_dev->name		= pdev->name;
	input_dev->id.bustype	= BUS_HOST;
	input_dev->dev.parent	= &pdev->dev;
	input_dev->evbit[0]	= BIT_MASK(EV_KEY) | BIT_MASK(EV_REP);
	input_dev->open		= matrix_keypad_start;
	input_dev->close	= matrix_keypad_stop;

	input_dev->keycode	= keycodes;
	input_dev->keycodesize	= sizeof(*keycodes);
	input_dev->keycodemax	= pdata->num_row_gpios << keypad->row_shift;

	for (i = 0; i < keymap_data->keymap_size; i++) {
		unsigned int key = keymap_data->keymap[i];
		unsigned int row = KEY_ROW(key);
		unsigned int col = KEY_COL(key);
		unsigned short code = KEY_VAL(key);

		keycodes[MATRIX_SCAN_CODE(row, col, row_shift)] = code;
		__set_bit(code, input_dev->keybit);
	}
	__clear_bit(KEY_RESERVED, input_dev->keybit);

	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
	input_set_drvdata(input_dev, keypad);

	err = init_matrix_gpio(pdev, keypad);
	if (err)
		goto err_free_mem;

	err = input_register_device(keypad->input_dev);
	if (err)
		goto err_free_mem;

	device_init_wakeup(&pdev->dev, pdata->wakeup);
	platform_set_drvdata(pdev, keypad);

	return 0;

err_free_mem:
	input_free_device(input_dev);
	kfree(keycodes);
	kfree(keypad);
	return err;
}

static int __devexit matrix_keypad_remove(struct platform_device *pdev)
{
	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	int i;

	device_init_wakeup(&pdev->dev, 0);

	for (i = 0; i < pdata->num_row_gpios; i++) {
		free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
		gpio_free(pdata->row_gpios[i]);
	}

	for (i = 0; i < pdata->num_col_gpios; i++)
		gpio_free(pdata->col_gpios[i]);

	input_unregister_device(keypad->input_dev);
	platform_set_drvdata(pdev, NULL);
	kfree(keypad->keycodes);
	kfree(keypad);

	return 0;
}

static struct platform_driver matrix_keypad_driver = {
	.probe		= matrix_keypad_probe,
	.remove		= __devexit_p(matrix_keypad_remove),
	.suspend	= matrix_keypad_suspend,
	.resume		= matrix_keypad_resume,
	.driver		= {
		.name	= "matrix-keypad",
		.owner	= THIS_MODULE,
	},
};

static int __init matrix_keypad_init(void)
{
	return platform_driver_register(&matrix_keypad_driver);
}

static void __exit matrix_keypad_exit(void)
{
	platform_driver_unregister(&matrix_keypad_driver);
}

module_init(matrix_keypad_init);
module_exit(matrix_keypad_exit);

MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
MODULE_DESCRIPTION("GPIO Driven Matrix Keypad Driver");
MODULE_LICENSE("GPL v2");
MODULE_ALIAS("platform:matrix-keypad");
Commit	Line	Data
bab7614d EM	1	/*
	2	* GPIO driven matrix keyboard driver
	3	*
	4	* Copyright (c) 2008 Marek Vasut <marek.vasut@gmail.com>
	5	*
	6	* Based on corgikbd.c
	7	*
	8	* This program is free software; you can redistribute it and/or modify
	9	* it under the terms of the GNU General Public License version 2 as
	10	* published by the Free Software Foundation.
	11	*
	12	*/
	13
	14	#include <linux/types.h>
	15	#include <linux/delay.h>
	16	#include <linux/platform_device.h>
	17	#include <linux/init.h>
	18	#include <linux/input.h>
	19	#include <linux/irq.h>
	20	#include <linux/interrupt.h>
	21	#include <linux/jiffies.h>
	22	#include <linux/module.h>
	23	#include <linux/gpio.h>
	24	#include <linux/input/matrix_keypad.h>
	25
	26	struct matrix_keypad {
	27	const struct matrix_keypad_platform_data *pdata;
	28	struct input_dev *input_dev;
	29	unsigned short *keycodes;
d82f1c35	30	unsigned int row_shift;
bab7614d EM	31
	32	uint32_t last_key_state[MATRIX_MAX_COLS];
	33	struct delayed_work work;
	34	bool scan_pending;
	35	bool stopped;
	36	spinlock_t lock;
	37	};
	38
	39	/*
	40	* NOTE: normally the GPIO has to be put into HiZ when de-activated to cause
	41	* minmal side effect when scanning other columns, here it is configured to
	42	* be input, and it should work on most platforms.
	43	*/
	44	static void __activate_col(const struct matrix_keypad_platform_data *pdata,
	45	int col, bool on)
	46	{
	47	bool level_on = !pdata->active_low;
	48
	49	if (on) {
	50	gpio_direction_output(pdata->col_gpios[col], level_on);
	51	} else {
	52	gpio_set_value_cansleep(pdata->col_gpios[col], !level_on);
	53	gpio_direction_input(pdata->col_gpios[col]);
	54	}
	55	}
	56
	57	static void activate_col(const struct matrix_keypad_platform_data *pdata,
	58	int col, bool on)
	59	{
	60	__activate_col(pdata, col, on);
	61
	62	if (on && pdata->col_scan_delay_us)
	63	udelay(pdata->col_scan_delay_us);
	64	}
	65
	66	static void activate_all_cols(const struct matrix_keypad_platform_data *pdata,
	67	bool on)
	68	{
	69	int col;
	70
	71	for (col = 0; col < pdata->num_col_gpios; col++)
	72	__activate_col(pdata, col, on);
	73	}
	74
	75	static bool row_asserted(const struct matrix_keypad_platform_data *pdata,
	76	int row)
	77	{
	78	return gpio_get_value_cansleep(pdata->row_gpios[row]) ?
	79	!pdata->active_low : pdata->active_low;
	80	}
	81
	82	static void enable_row_irqs(struct matrix_keypad *keypad)
	83	{
	84	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	85	int i;
	86
	87	for (i = 0; i < pdata->num_row_gpios; i++)
	88	enable_irq(gpio_to_irq(pdata->row_gpios[i]));
	89	}
	90
	91	static void disable_row_irqs(struct matrix_keypad *keypad)
	92	{
	93	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	94	int i;
95
96	for (i = 0; i < pdata->num_row_gpios; i++)
97	disable_irq_nosync(gpio_to_irq(pdata->row_gpios[i]));
98	}
99
100	/*
101	* This gets the keys from keyboard and reports it to input subsystem
102	*/
103	static void matrix_keypad_scan(struct work_struct *work)
104	{
105	struct matrix_keypad *keypad =
106	container_of(work, struct matrix_keypad, work.work);
107	struct input_dev *input_dev = keypad->input_dev;
108	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
109	uint32_t new_state[MATRIX_MAX_COLS];
110	int row, col, code;
111
112	/* de-activate all columns for scanning */
113	activate_all_cols(pdata, false);
114
115	memset(new_state, 0, sizeof(new_state));
116
117	/* assert each column and read the row status out */
118	for (col = 0; col < pdata->num_col_gpios; col++) {
119
120	activate_col(pdata, col, true);
121
122	for (row = 0; row < pdata->num_row_gpios; row++)
123	new_state[col] \|=
124	row_asserted(pdata, row) ? (1 << row) : 0;
125
126	activate_col(pdata, col, false);
127	}
128
129	for (col = 0; col < pdata->num_col_gpios; col++) {
130	uint32_t bits_changed;
131
132	bits_changed = keypad->last_key_state[col] ^ new_state[col];
133	if (bits_changed == 0)
134	continue;
135
136	for (row = 0; row < pdata->num_row_gpios; row++) {
137	if ((bits_changed & (1 << row)) == 0)
138	continue;
139
d82f1c35	140	code = MATRIX_SCAN_CODE(row, col, keypad->row_shift);
bab7614d EM	141	input_event(input_dev, EV_MSC, MSC_SCAN, code);
	142	input_report_key(input_dev,
	143	keypad->keycodes[code],
	144	new_state[col] & (1 << row));
	145	}
	146	}
	147	input_sync(input_dev);
	148
	149	memcpy(keypad->last_key_state, new_state, sizeof(new_state));
	150
	151	activate_all_cols(pdata, true);
	152
	153	/* Enable IRQs again */
	154	spin_lock_irq(&keypad->lock);
	155	keypad->scan_pending = false;
	156	enable_row_irqs(keypad);
	157	spin_unlock_irq(&keypad->lock);
	158	}
	159
	160	static irqreturn_t matrix_keypad_interrupt(int irq, void *id)
	161	{
	162	struct matrix_keypad *keypad = id;
	163	unsigned long flags;
	164
	165	spin_lock_irqsave(&keypad->lock, flags);
	166
	167	/*
	168	* See if another IRQ beaten us to it and scheduled the
	169	* scan already. In that case we should not try to
	170	* disable IRQs again.
	171	*/
	172	if (unlikely(keypad->scan_pending \|\| keypad->stopped))
	173	goto out;
	174
	175	disable_row_irqs(keypad);
	176	keypad->scan_pending = true;
	177	schedule_delayed_work(&keypad->work,
	178	msecs_to_jiffies(keypad->pdata->debounce_ms));
	179
	180	out:
	181	spin_unlock_irqrestore(&keypad->lock, flags);
	182	return IRQ_HANDLED;
	183	}
	184
	185	static int matrix_keypad_start(struct input_dev *dev)
	186	{
	187	struct matrix_keypad *keypad = input_get_drvdata(dev);
	188
	189	keypad->stopped = false;
	190	mb();
	191
	192	/*
	193	* Schedule an immediate key scan to capture current key state;
	194	* columns will be activated and IRQs be enabled after the scan.
	195	*/
	196	schedule_delayed_work(&keypad->work, 0);
	197
	198	return 0;
	199	}
	200
	201	static void matrix_keypad_stop(struct input_dev *dev)
	202	{
	203	struct matrix_keypad *keypad = input_get_drvdata(dev);
	204
205	keypad->stopped = true;
206	mb();
207	flush_work(&keypad->work.work);
208	/*
209	* matrix_keypad_scan() will leave IRQs enabled;
210	* we should disable them now.
211	*/
212	disable_row_irqs(keypad);
213	}
214
215	#ifdef CONFIG_PM
216	static int matrix_keypad_suspend(struct platform_device *pdev, pm_message_t state)
217	{
218	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
219	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
220	int i;
221
222	matrix_keypad_stop(keypad->input_dev);
223
224	if (device_may_wakeup(&pdev->dev))
225	for (i = 0; i < pdata->num_row_gpios; i++)
226	enable_irq_wake(gpio_to_irq(pdata->row_gpios[i]));
227
228	return 0;
229	}
230
231	static int matrix_keypad_resume(struct platform_device *pdev)
232	{
233	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
234	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
235	int i;
236
237	if (device_may_wakeup(&pdev->dev))
238	for (i = 0; i < pdata->num_row_gpios; i++)
239	disable_irq_wake(gpio_to_irq(pdata->row_gpios[i]));
240
241	matrix_keypad_start(keypad->input_dev);
242
243	return 0;
244	}
245	#else
246	#define matrix_keypad_suspend NULL
247	#define matrix_keypad_resume NULL
248	#endif
249
250	static int __devinit init_matrix_gpio(struct platform_device *pdev,
251	struct matrix_keypad *keypad)
252	{
253	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
254	int i, err = -EINVAL;
255
256	/* initialized strobe lines as outputs, activated */
257	for (i = 0; i < pdata->num_col_gpios; i++) {
258	err = gpio_request(pdata->col_gpios[i], "matrix_kbd_col");
259	if (err) {
260	dev_err(&pdev->dev,
261	"failed to request GPIO%d for COL%d\n",
262	pdata->col_gpios[i], i);
263	goto err_free_cols;
264	}
265
266	gpio_direction_output(pdata->col_gpios[i], !pdata->active_low);
267	}
268
269	for (i = 0; i < pdata->num_row_gpios; i++) {
270	err = gpio_request(pdata->row_gpios[i], "matrix_kbd_row");
271	if (err) {
272	dev_err(&pdev->dev,
273	"failed to request GPIO%d for ROW%d\n",
274	pdata->row_gpios[i], i);
275	goto err_free_rows;
276	}
277
278	gpio_direction_input(pdata->row_gpios[i]);
279	}
280
281	for (i = 0; i < pdata->num_row_gpios; i++) {
282	err = request_irq(gpio_to_irq(pdata->row_gpios[i]),
283	matrix_keypad_interrupt,
284	IRQF_DISABLED \|
285	IRQF_TRIGGER_RISING \| IRQF_TRIGGER_FALLING,
286	"matrix-keypad", keypad);
287	if (err) {
288	dev_err(&pdev->dev,
289	"Unable to acquire interrupt for GPIO line %i\n",
290	pdata->row_gpios[i]);
291	goto err_free_irqs;
292	}
293	}
294
295	/* initialized as disabled - enabled by input->open */
296	disable_row_irqs(keypad);
297	return 0;
298
299	err_free_irqs:
300	while (--i >= 0)
301	free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
302	i = pdata->num_row_gpios;
303	err_free_rows:
304	while (--i >= 0)
305	gpio_free(pdata->row_gpios[i]);
306	i = pdata->num_col_gpios;
307	err_free_cols:
308	while (--i >= 0)
309	gpio_free(pdata->col_gpios[i]);
310
311	return err;
312	}
313
314	static int __devinit matrix_keypad_probe(struct platform_device *pdev)
315	{
316	const struct matrix_keypad_platform_data *pdata;
317	const struct matrix_keymap_data *keymap_data;
318	struct matrix_keypad *keypad;
319	struct input_dev *input_dev;
320	unsigned short *keycodes;
d82f1c35	321	unsigned int row_shift;
bab7614d EM	322	int i;
	323	int err;
	324
	325	pdata = pdev->dev.platform_data;
	326	if (!pdata) {
	327	dev_err(&pdev->dev, "no platform data defined\n");
	328	return -EINVAL;
	329	}
	330
	331	keymap_data = pdata->keymap_data;
	332	if (!keymap_data) {
	333	dev_err(&pdev->dev, "no keymap data defined\n");
	334	return -EINVAL;
	335	}
	336
d82f1c35	337	row_shift = get_count_order(pdata->num_col_gpios);
bab7614d EM	338
bab7614d EM	339	keypad = kzalloc(sizeof(struct matrix_keypad), GFP_KERNEL);
d82f1c35 EM	340	keycodes = kzalloc((pdata->num_row_gpios << row_shift) *
d82f1c35 EM	341	sizeof(*keycodes),
bab7614d EM	342	GFP_KERNEL);
	343	input_dev = input_allocate_device();
	344	if (!keypad \|\| !keycodes \|\| !input_dev) {
	345	err = -ENOMEM;
	346	goto err_free_mem;
	347	}
	348
	349	keypad->input_dev = input_dev;
	350	keypad->pdata = pdata;
	351	keypad->keycodes = keycodes;
d82f1c35	352	keypad->row_shift = row_shift;
bab7614d EM	353	keypad->stopped = true;
	354	INIT_DELAYED_WORK(&keypad->work, matrix_keypad_scan);
	355	spin_lock_init(&keypad->lock);
	356
	357	input_dev->name = pdev->name;
	358	input_dev->id.bustype = BUS_HOST;
	359	input_dev->dev.parent = &pdev->dev;
	360	input_dev->evbit[0] = BIT_MASK(EV_KEY) \| BIT_MASK(EV_REP);
	361	input_dev->open = matrix_keypad_start;
	362	input_dev->close = matrix_keypad_stop;
	363
	364	input_dev->keycode = keycodes;
	365	input_dev->keycodesize = sizeof(*keycodes);
d82f1c35	366	input_dev->keycodemax = pdata->num_row_gpios << keypad->row_shift;
bab7614d EM	367
	368	for (i = 0; i < keymap_data->keymap_size; i++) {
	369	unsigned int key = keymap_data->keymap[i];
	370	unsigned int row = KEY_ROW(key);
	371	unsigned int col = KEY_COL(key);
	372	unsigned short code = KEY_VAL(key);
	373
d82f1c35	374	keycodes[MATRIX_SCAN_CODE(row, col, row_shift)] = code;
bab7614d EM	375	__set_bit(code, input_dev->keybit);
	376	}
	377	__clear_bit(KEY_RESERVED, input_dev->keybit);
	378
	379	input_set_capability(input_dev, EV_MSC, MSC_SCAN);
	380	input_set_drvdata(input_dev, keypad);
	381
	382	err = init_matrix_gpio(pdev, keypad);
	383	if (err)
	384	goto err_free_mem;
	385
	386	err = input_register_device(keypad->input_dev);
	387	if (err)
	388	goto err_free_mem;
	389
	390	device_init_wakeup(&pdev->dev, pdata->wakeup);
	391	platform_set_drvdata(pdev, keypad);
	392
	393	return 0;
	394
	395	err_free_mem:
	396	input_free_device(input_dev);
	397	kfree(keycodes);
	398	kfree(keypad);
	399	return err;
	400	}
	401
	402	static int __devexit matrix_keypad_remove(struct platform_device *pdev)
	403	{
	404	struct matrix_keypad *keypad = platform_get_drvdata(pdev);
	405	const struct matrix_keypad_platform_data *pdata = keypad->pdata;
	406	int i;
	407
	408	device_init_wakeup(&pdev->dev, 0);
	409
	410	for (i = 0; i < pdata->num_row_gpios; i++) {
	411	free_irq(gpio_to_irq(pdata->row_gpios[i]), keypad);
	412	gpio_free(pdata->row_gpios[i]);
	413	}
	414
	415	for (i = 0; i < pdata->num_col_gpios; i++)
	416	gpio_free(pdata->col_gpios[i]);
	417
	418	input_unregister_device(keypad->input_dev);
	419	platform_set_drvdata(pdev, NULL);
	420	kfree(keypad->keycodes);
	421	kfree(keypad);
	422
	423	return 0;
	424	}
	425
	426	static struct platform_driver matrix_keypad_driver = {
	427	.probe = matrix_keypad_probe,
	428	.remove = __devexit_p(matrix_keypad_remove),
	429	.suspend = matrix_keypad_suspend,
	430	.resume = matrix_keypad_resume,
	431	.driver = {
	432	.name = "matrix-keypad",
	433	.owner = THIS_MODULE,
	434	},
	435	};
	436
	437	static int __init matrix_keypad_init(void)
	438	{
439	return platform_driver_register(&matrix_keypad_driver);
440	}
441
442	static void __exit matrix_keypad_exit(void)
443	{
444	platform_driver_unregister(&matrix_keypad_driver);
445	}
446
447	module_init(matrix_keypad_init);
448	module_exit(matrix_keypad_exit);
449
450	MODULE_AUTHOR("Marek Vasut <marek.vasut@gmail.com>");
451	MODULE_DESCRIPTION("GPIO Driven Matrix Keypad Driver");
452	MODULE_LICENSE("GPL v2");
453	MODULE_ALIAS("platform:matrix-keypad");