input: Add device tree parsing support for matrix keymap

Add support for parsing the "linux,keymap" property.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

2 files changed, 51 insertions, 2 deletions

diff --git a/drivers/input/imx_keypad.c b/drivers/input/imx_keypad.c
index bc74d7dee6..d3b5a8509e 100644
--- a/drivers/input/imx_keypad.c
+++ b/drivers/input/imx_keypad.c
@@ -396,8 +396,10 @@ static int __init imx_keypad_probe(struct device_d *dev)
 	dev_dbg(dev, "enabled rows mask: %x\n", keypad->rows_en_mask);
 	dev_dbg(dev, "enabled cols mask: %x\n", keypad->cols_en_mask);
 
-	matrix_keypad_build_keymap(keymap_data, MATRIX_ROW_SHIFT,
+	ret = matrix_keypad_build_keymap(dev, keymap_data, MATRIX_ROW_SHIFT,
 				keypad->keycodes);
+	if (ret)
+		return ret;
 
 	imx_keypad_config(keypad);
 
diff --git a/drivers/input/matrix-keymap.c b/drivers/input/matrix-keymap.c
index d56eccc29e..288b6a4b53 100644
--- a/drivers/input/matrix-keymap.c
+++ b/drivers/input/matrix-keymap.c
@@ -12,6 +12,47 @@
 #include <common.h>
 #include <input/matrix_keypad.h>
 
+static int matrix_keypad_parse_of_keymap(struct device_d *dev,
+					 unsigned int row_shift,
+					 unsigned short *keymap)
+{
+	unsigned int proplen, i, size;
+	const __be32 *prop;
+	struct device_node *np = dev->device_node;
+	const char *propname = "linux,keymap";
+
+	prop = of_get_property(np, propname, &proplen);
+	if (!prop) {
+		dev_err(dev, "OF: %s property not defined in %s\n",
+			propname, np->full_name);
+		return -ENOENT;
+	}
+
+	if (proplen % sizeof(u32)) {
+		dev_err(dev, "OF: Malformed keycode property %s in %s\n",
+			propname, np->full_name);
+		return -EINVAL;
+	}
+
+	size = proplen / sizeof(u32);
+
+	for (i = 0; i < size; i++) {
+		unsigned int key = be32_to_cpup(prop + i);
+
+		unsigned int row = KEY_ROW(key);
+		unsigned int col = KEY_COL(key);
+		unsigned short code = KEY_VAL(key);
+
+		if (row >= MATRIX_MAX_ROWS || col >= MATRIX_MAX_COLS) {
+			dev_err(dev, "rows/cols out of range\n");
+			continue;
+		}
+
+		keymap[MATRIX_SCAN_CODE(row, col, row_shift)] = code;
+	}
+
+	return 0;
+}
 /**
  * matrix_keypad_build_keymap - convert platform keymap into matrix keymap
  * @keymap_data: keymap supplied by the platform code
@@ -23,12 +64,18 @@
  * an array of keycodes that is suitable for using in a standard matrix
  * keyboard driver that uses row and col as indices.
  */
-int matrix_keypad_build_keymap(const struct matrix_keymap_data *keymap_data,
+int matrix_keypad_build_keymap(struct device_d *dev, const struct matrix_keymap_data *keymap_data,
 			   unsigned int row_shift,
 			   unsigned short *keymap)
 {
 	int i;
 
+	if (IS_ENABLED(CONFIG_OFDEVICE) && dev->device_node)
+		return matrix_keypad_parse_of_keymap(dev, row_shift, keymap);
+
+	if (!keymap_data)
+		return -EINVAL;
+
 	for (i = 0; i < keymap_data->keymap_size; i++) {
 		unsigned int key = keymap_data->keymap[i];
 		unsigned int row = KEY_ROW(key);