2 files changed, 96 insertions, 20 deletions

diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 6439f378f6cc..64b47de052bb 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -434,32 +434,21 @@ static long evdev_ioctl_handler(struct file *file, unsigned int cmd,
                 case EVIOCGKEYCODE:
                         if (get_user(t, ip))
                                 return -EFAULT;
-                        if (t < 0 || t >= dev->keycodemax || !dev->keycodesize)
-                                return -EINVAL;
-                        if (put_user(INPUT_KEYCODE(dev, t), ip + 1))
+
+                        error = dev->getkeycode(dev, t, &v);
+                        if (error)
+                                return error;
+
+                        if (put_user(v, ip + 1))
                                 return -EFAULT;
+
                         return 0;
 
                 case EVIOCSKEYCODE:
-                        if (get_user(t, ip))
-                                return -EFAULT;
-                        if (t < 0 || t >= dev->keycodemax || !dev->keycodesize)
-                                return -EINVAL;
-                        if (get_user(v, ip + 1))
+                        if (get_user(t, ip) || get_user(v, ip + 1))
                                 return -EFAULT;
-                        if (v < 0 || v > KEY_MAX)
-                                return -EINVAL;
-                        if (dev->keycodesize < sizeof(v) && (v >> (dev->keycodesize * 8)))
-                                return -EINVAL;
-
-                        u = SET_INPUT_KEYCODE(dev, t, v);
-                        clear_bit(u, dev->keybit);
-                        set_bit(v, dev->keybit);
-                        for (i = 0; i < dev->keycodemax; i++)
-                                if (INPUT_KEYCODE(dev, i) == u)
-                                        set_bit(u, dev->keybit);
 
-                        return 0;
+                        return dev->setkeycode(dev, t, v);
 
                 case EVIOCSFF:
                         if (copy_from_user(&effect, p, sizeof(effect)))
diff --git a/drivers/input/input.c b/drivers/input/input.c
index 4486402fbf5d..26393a606e6f 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -299,6 +299,87 @@ void input_close_device(struct input_handle *handle)
 }
 EXPORT_SYMBOL(input_close_device);
 
+static int input_fetch_keycode(struct input_dev *dev, int scancode)
+{
+        switch (dev->keycodesize) {
+                case 1:
+                        return ((u8 *)dev->keycode)[scancode];
+
+                case 2:
+                        return ((u16 *)dev->keycode)[scancode];
+
+                default:
+                        return ((u32 *)dev->keycode)[scancode];
+        }
+}
+
+static int input_default_getkeycode(struct input_dev *dev,
+                                    int scancode, int *keycode)
+{
+        if (!dev->keycodesize)
+                return -EINVAL;
+
+        if (scancode < 0 || scancode >= dev->keycodemax)
+                return -EINVAL;
+
+        *keycode = input_fetch_keycode(dev, scancode);
+
+        return 0;
+}
+
+static int input_default_setkeycode(struct input_dev *dev,
+                                    int scancode, int keycode)
+{
+        int old_keycode;
+        int i;
+
+        if (scancode < 0 || scancode >= dev->keycodemax)
+                return -EINVAL;
+
+        if (keycode < 0 || keycode > KEY_MAX)
+                return -EINVAL;
+
+        if (!dev->keycodesize)
+                return -EINVAL;
+
+        if (dev->keycodesize < sizeof(keycode) && (keycode >> (dev->keycodesize * 8)))
+                return -EINVAL;
+
+        switch (dev->keycodesize) {
+                case 1: {
+                        u8 *k = (u8 *)dev->keycode;
+                        old_keycode = k[scancode];
+                        k[scancode] = keycode;
+                        break;
+                }
+                case 2: {
+                        u16 *k = (u16 *)dev->keycode;
+                        old_keycode = k[scancode];
+                        k[scancode] = keycode;
+                        break;
+                }
+                default: {
+                        u32 *k = (u32 *)dev->keycode;
+                        old_keycode = k[scancode];
+                        k[scancode] = keycode;
+                        break;
+                }
+        }
+
+        clear_bit(old_keycode, dev->keybit);
+        set_bit(keycode, dev->keybit);
+
+        for (i = 0; i < dev->keycodemax; i++) {
+                if (input_fetch_keycode(dev, i) == old_keycode) {
+                        set_bit(old_keycode, dev->keybit);
+                        break; /* Setting the bit twice is useless, so break */
+                }
+        }
+
+        return 0;
+}
+
+
 static void input_link_handle(struct input_handle *handle)
 {
         list_add_tail(&handle->d_node, &handle->dev->h_list);
@@ -978,6 +1059,12 @@ int input_register_device(struct input_dev *dev)
                 dev->rep[REP_PERIOD] = 33;
         }
 
+        if (!dev->getkeycode)
+                dev->getkeycode = input_default_getkeycode;
+
+        if (!dev->setkeycode)
+                dev->setkeycode = input_default_setkeycode;
+
         list_add_tail(&dev->node, &input_dev_list);
 
         snprintf(dev->cdev.class_id, sizeof(dev->cdev.class_id),