fuze+: rewrite touchpad driver

The old driver was bad in many respect, it had some race conditions, it was
using a thread to serialize transfers because of the legacy i2c interface.
It also had huge latency (typically 50ms but delays up to 300ms can happen),
thus some presses were missed.
The new driver takes advantage of the new i2c driver to do everything
asynchronously. It also does not need a thread anymore because queueing
ensures proper serialization. It provides much better and reliable latency
(typically ~2ms).
Also fix the debug screen which was horribly broken. The new screen also
displays the deadzones.

Change-Id: I69b7f99b75053e6b1d3d56beb4453c004fd2076e

4 files changed, 496 insertions, 351 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 2cace81c8b..34d2db39b2 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1465,7 +1465,6 @@ target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
 #endif /* SANSA_FUZEV2 */
 
 #ifdef SANSA_FUZEPLUS
-drivers/synaptics-rmi.c
 #ifndef BOOTLOADER
 drivers/generic_i2c.c
 target/arm/imx233/fmradio-imx233.c
diff --git a/firmware/drivers/synaptics-rmi.c b/firmware/drivers/synaptics-rmi.c
deleted file mode 100644
index 1b7cf2f8ef..0000000000
--- a/firmware/drivers/synaptics-rmi.c
+++ /dev/null
@@ -1,104 +0,0 @@
-/***************************************************************************
- *             __________               __   ___.
- *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
- *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
- *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
- *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
- *                     \/            \/     \/    \/            \/
- * $Id$
- *
- * Copyright (C) 2011 by Amaury Pouly
- *
- * 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 software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
- * KIND, either express or implied.
- *
- ****************************************************************************/
-#include "system.h"
-#include "synaptics-rmi.h"
-#include "i2c.h"
-
-static int rmi_cur_page;
-static int rmi_i2c_addr;
-
-static unsigned char dev_ctl_reg;
-
-/* NOTE:
- * RMI over i2c supports some special aliases on page 0x2 but this driver don't
- * use them */
-
-int rmi_init(int i2c_dev_addr)
-{
-    rmi_i2c_addr = i2c_dev_addr;
-    rmi_cur_page = 0x4;
-    dev_ctl_reg = rmi_read_single(RMI_DEVICE_CONTROL);
-    return 0;
-}
-
-static int rmi_select_page(unsigned char page)
-{
-    /* Lazy page select */
-    if(page != rmi_cur_page)
-    {
-        rmi_cur_page = page;
-        return i2c_writemem(rmi_i2c_addr, RMI_PAGE_SELECT, &page, 1);
-    }
-    else
-        return 0;
-}
-
-int rmi_read(int address, int byte_count, unsigned char *buffer)
-{
-    int ret;
-    if((ret = rmi_select_page(address >> 8)) < 0)
-        return ret;
-    return i2c_readmem(rmi_i2c_addr, address & 0xff, buffer, byte_count);
-}
-
-int rmi_read_single(int address)
-{
-    unsigned char c;
-    int ret = rmi_read(address, 1, &c);
-    return ret < 0 ? ret : c;
-}
-
-int rmi_write(int address, int byte_count, const unsigned char *buffer)
-{
-    int ret;
-    if((ret = rmi_select_page(address >> 8)) < 0)
-        return ret;
-    return i2c_writemem(rmi_i2c_addr, address & 0xff, buffer, byte_count);
-}
-
-int rmi_write_single(int address, unsigned char byte)
-{
-    return rmi_write(address, 1, &byte);
-}
-
-/* set the device to the given sleep mode */
-void rmi_set_sleep_mode(unsigned char sleep_mode)
-{
-    /* valid value different from the actual one*/
-    if((dev_ctl_reg & RMI_SLEEP_MODE_BM) != sleep_mode)
-    {
-        dev_ctl_reg &= ~RMI_SLEEP_MODE_BM;
-        dev_ctl_reg |= sleep_mode;
-        rmi_write_single(RMI_DEVICE_CONTROL, dev_ctl_reg);
-    }
-}
-
-/* set the device's report rate to the given value */
-void rmi_set_report_rate(unsigned char report_rate)
-{
-    /* valid value different from the actual one*/
-    if((dev_ctl_reg & RMI_REPORT_RATE_BM) != report_rate)
-    {
-        dev_ctl_reg &= ~RMI_REPORT_RATE_BM;
-        dev_ctl_reg |= report_rate;
-        rmi_write_single(RMI_DEVICE_CONTROL, dev_ctl_reg);
-    }
-}
diff --git a/firmware/export/synaptics-rmi.h b/firmware/export/synaptics-rmi.h
index 9d9d285fb6..e7fe14d7d5 100644
--- a/firmware/export/synaptics-rmi.h
+++ b/firmware/export/synaptics-rmi.h
@@ -123,26 +123,4 @@ struct rmi_2d_gesture_data_t
     unsigned char flick;
 } __attribute__((packed));
 
-/* Initialize the RMI driver, the i2c_bus_index is the bus index returned by
- * the generic_i2c driver; the i2c_dev_addr is the i2c address of the device.
- * NOTE: the driver automatically handles the page select mechanism used for
- *       RMI over i2c and assumes a standard page select register at 0xff. */
-int rmi_init(int i2c_dev_addr);
-/* Read one or more registers.
- * WARNING: don't cross a page boundary ! */
-int rmi_read(int address, int byte_count, unsigned char *buffer);
-/* Read a single register (return -1 on error)
- * WARNING: beware of register consistency (N x read 1 byte != reads N bytes) */
-int rmi_read_single(int address); /* return byte value or <0 in case of error */
-/* Write one of more register
- * WARNING: don't cross a page boundary ! */
-int rmi_write(int address, int byte_count, const unsigned char *buffer);
-/* Write one register
- * WARNING: don't cross a page boundary ! */
-int rmi_write_single(int address, unsigned char byte);
-/* set the device to the given sleep mode */
-void rmi_set_sleep_mode(unsigned char sleep_mode);
-/* set the device's report rate to the given value */
-void rmi_set_report_rate(unsigned char report_rate);
-
 #endif
diff --git a/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c b/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
index ea4aa083e5..2c3358e7dc 100644
--- a/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
+++ b/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
@@ -22,7 +22,7 @@
 #include "system.h"
 #include "system-target.h"
 #include "pinctrl-imx233.h"
-#include "generic_i2c.h"
+#include "i2c-imx233.h"
 #include "synaptics-rmi.h"
 #include "lcd.h"
 #include "string.h"
@@ -30,6 +30,7 @@
 #include "button-imx233.h"
 #include "touchpad.h"
 #include "stdio.h"
+#include "font.h"
 
 struct imx233_button_map_t imx233_button_map[] =
 {
@@ -39,7 +40,428 @@ struct imx233_button_map_t imx233_button_map[] =
     IMX233_BUTTON_(END, END(), "")
 };
 
+/**
+ * RMI API
+ */
+
+#define RMI_I2C_ADDR    0x40
+
+static unsigned char dev_ctl_reg; /* cached value of control register */
+
+/* NOTE:
+ * RMI over i2c supports some special aliases on page 0x2 but this driver don't
+ * use them */
+
+struct rmi_xfer_t;
+typedef void (*rmi_xfer_cb_t)(struct rmi_xfer_t *xfer);
+
+/* Represent a typical RMI transaction: a first transfer to select the page
+ * and a second transfer to read/write registers. The API takes care of annoying
+ * details and will simply call the callback at the end of the transfer. */
+struct rmi_xfer_t
+{
+    struct imx233_i2c_xfer_t xfer_page; /* first transfer: page select */
+    struct imx233_i2c_xfer_t xfer_rw; /* second transfer: read/write */
+    uint8_t sel_page[2]; /* write command to select page */
+    uint8_t sel_reg; /* write command to select register */
+    volatile enum imx233_i2c_error_t status; /* transfer status */
+    rmi_xfer_cb_t callback; /* callback */
+};
+
+/* Synchronous transfer: add a semaphore to block */
+struct rmi_xfer_sync_t
+{
+    struct rmi_xfer_t xfer;
+    struct semaphore sema; /* semaphore for completion */
+};
+
+/* callback for first transfer: record error if any */
+static void rmi_i2c_first_callback(struct imx233_i2c_xfer_t *xfer, enum imx233_i2c_error_t status)
+{
+    struct rmi_xfer_t *rxfer = container_of(xfer, struct rmi_xfer_t, xfer_page);
+    /* record status */
+    rxfer->status = status;
+}
+
+/* callback for first transfer: handle error and callback */
+static void rmi_i2c_second_callback(struct imx233_i2c_xfer_t *xfer, enum imx233_i2c_error_t status)
+{
+    struct rmi_xfer_t *rxfer = container_of(xfer, struct rmi_xfer_t, xfer_rw);
+    /* record status, only if not skipping (ie the error was in first transfer) */
+    if(status != I2C_SKIP)
+        rxfer->status = status;
+    /* callback */
+    if(rxfer->callback)
+        rxfer->callback(rxfer);
+}
+
+/* build a rmi transaction to read/write registers; do NOT cross page boundary ! */
+static void rmi_build_xfer(struct rmi_xfer_t *xfer, bool read, int address,
+    int byte_count, unsigned char *buffer, rmi_xfer_cb_t callback)
+{
+    /* first transfer: change page */
+    xfer->xfer_page.next = &xfer->xfer_rw;
+    xfer->xfer_page.fast_mode = true;
+    xfer->xfer_page.dev_addr = RMI_I2C_ADDR;
+    xfer->xfer_page.mode = I2C_WRITE;
+    xfer->xfer_page.count[0] = 2;
+    xfer->xfer_page.data[0] = &xfer->sel_page;
+    xfer->xfer_page.count[1] = 0;
+    xfer->xfer_page.tmo_ms = 1000;
+    xfer->xfer_page.callback = &rmi_i2c_first_callback;
+    /* second transfer: read/write */
+    xfer->xfer_rw.next = NULL;
+    xfer->xfer_rw.fast_mode = true;
+    xfer->xfer_rw.dev_addr = RMI_I2C_ADDR;
+    xfer->xfer_rw.mode = read ? I2C_READ : I2C_WRITE;
+    xfer->xfer_rw.count[0] = 1;
+    xfer->xfer_rw.data[0] = &xfer->sel_reg;
+    xfer->xfer_rw.count[1] = byte_count;
+    xfer->xfer_rw.data[1] = buffer;
+    xfer->xfer_rw.tmo_ms = 1000;
+    xfer->xfer_rw.callback = &rmi_i2c_second_callback;
+    /* general things */
+    xfer->callback = callback;
+    xfer->sel_page[0] = RMI_PAGE_SELECT;
+    xfer->sel_page[1] = address >> 8;
+    xfer->sel_reg = address & 0xff;
+}
+
+/** IMPORTANT NOTE
+ *
+ * All transfers are built using rmi_build_xfer which constructs a transaction
+ * consisting in a page select and register read/writes. Since transactions are
+ * executed "atomically" and are queued, it is safe to call transfers functions
+ * concurrently. However only asynchronous transfers can be used in IRQ context.
+ * In all cases, make sure the the rmi_xfer_t structure lives at least until the
+ * completion of the transfer (callback).
+ */
+
+/* queue transfer to change sleep mode, return true if transfer was queued
+ * and false if ignored because requested mode is already the current one.
+ * call must provide a transfer structure that must exist until completion */
+static bool rmi_set_sleep_mode_async(struct rmi_xfer_t *xfer, uint8_t *buf,
+    unsigned char sleep_mode, rmi_xfer_cb_t callback)
+{
+    /* avoid any race with concurrent changes to the mode */
+    unsigned long cpsr = disable_irq_save();
+    /* valid value different from the actual one */
+    if((dev_ctl_reg & RMI_SLEEP_MODE_BM) != sleep_mode)
+    {
+        /* change cached version */
+        dev_ctl_reg &= ~RMI_SLEEP_MODE_BM;
+        dev_ctl_reg |= sleep_mode;
+        *buf = dev_ctl_reg;
+        restore_irq(cpsr);
+        /* build transfer and kick */
+        rmi_build_xfer(xfer, false, RMI_DEVICE_CONTROL, 1, buf, callback);
+        imx233_i2c_transfer(&xfer->xfer_page);
+        return true;
+    }
+    else
+    {
+        restore_irq(cpsr);
+        return false;
+    }
+}
+
+/* synchronous callback: release semaphore */
+static void rmi_i2c_sync_callback(struct rmi_xfer_t *xfer)
+{
+    struct rmi_xfer_sync_t *sxfer = (void *)xfer;
+    semaphore_release(&sxfer->sema);
+}
+
+/* synchronous read/write */
+static void rmi_rw(bool read, int address, int byte_count, unsigned char *buffer)
+{
+    struct rmi_xfer_sync_t xfer;
+    rmi_build_xfer(&xfer.xfer, read, address, byte_count, buffer, rmi_i2c_sync_callback);
+    semaphore_init(&xfer.sema, 1, 0);
+    /* kick and wait */
+    imx233_i2c_transfer(&xfer.xfer.xfer_page);
+    semaphore_wait(&xfer.sema, TIMEOUT_BLOCK);
+    if(xfer.xfer.status != I2C_SUCCESS)
+        panicf("rmi: i2c err %d", xfer.xfer.status);
+}
+
+/* read registers synchronously */
+static void rmi_read(int address, int byte_count, unsigned char *buffer)
+{
+    rmi_rw(true, address, byte_count, buffer);
+}
+
+/* read single register synchronously */
+static int rmi_read_single(int address)
+{
+    unsigned char c;
+    rmi_rw(true, address, 1, &c);
+    return c;
+}
+
+/* write single register synchronously */
+static void rmi_write_single(int address, unsigned char byte)
+{
+    return rmi_rw(false, address, 1, &byte);
+}
+
+/* synchronously change sleep mode, this is a nop if current mode is the same as requested */
+static void rmi_set_sleep_mode(unsigned char sleep_mode)
+{
+    struct rmi_xfer_sync_t xfer;
+    uint8_t buf;
+    semaphore_init(&xfer.sema, 1, 0);
+    /* kick asynchronous transfer and only wait if mode was actually changed */
+    if(rmi_set_sleep_mode_async(&xfer.xfer, &buf, sleep_mode, &rmi_i2c_sync_callback))
+    {
+        semaphore_wait(&xfer.sema, TIMEOUT_BLOCK);
+        if(xfer.xfer.status != I2C_SUCCESS)
+            panicf("rmi: i2c err %d", xfer.xfer.status);
+    }
+}
+
+static void rmi_init(void)
+{
+    /* cache control register */
+    dev_ctl_reg = rmi_read_single(RMI_DEVICE_CONTROL);
+}
+
+
+/**
+ * Touchpad API
+ */
+
 #ifndef BOOTLOADER
+/* we emulate a 3x3 grid, this gives the button mapping */
+int button_mapping[3][3] =
+{
+    {BUTTON_BOTTOMLEFT, BUTTON_LEFT, BUTTON_BACK},
+    {BUTTON_DOWN, BUTTON_SELECT, BUTTON_UP},
+    {BUTTON_BOTTOMRIGHT, BUTTON_RIGHT, BUTTON_PLAYPAUSE},
+};
+
+/* timeout before lowering touchpad power from lack of activity */
+#define ACTIVITY_TMO        (5 * HZ)
+#define TOUCHPAD_WIDTH      3010
+#define TOUCHPAD_HEIGHT     1975
+#define DEADZONE_MULTIPLIER 2 /* deadzone multiplier */
+
+/* power level when touchpad is active: experiments show that "low power" reduce
+ * power consumption and hardly makes a difference in quality. */
+#define ACTIVE_POWER_LEVEL  RMI_SLEEP_MODE_LOW_POWER
+
+static int touchpad_btns = 0; /* button bitmap for the touchpad */
+static unsigned last_activity = 0; /* tick of the last touchpad activity */
+static bool t_enable = true; /* is touchpad enabled? */
+static int deadzone; /* deadzone size */
+static struct timeout activity_tmo; /* activity timeout */
+
+/* Ignore deadzone function. If outside of the pad, project to border. */
+static int find_button_no_deadzone(int x, int y)
+{
+    /* compute grid coordinate */
+    int gx = MAX(MIN(x * 3 / TOUCHPAD_WIDTH, 2), 0);
+    int gy = MAX(MIN(y * 3 / TOUCHPAD_HEIGHT, 2), 0);
+
+    return button_mapping[gx][gy];
+}
+
+static int find_button(int x, int y)
+{
+    /* find button ignoring deadzones */
+    int btn = find_button_no_deadzone(x, y);
+    /* To check if we are in a deadzone, we try to shift the coordinates
+     * and see if we get the same button. Not that we do not want to apply
+     * the deadzone in the borders ! The code works even in the borders because
+     * the find_button_no_deadzone() project out-of-bound coordinates to the
+     * borders */
+    if(find_button_no_deadzone(x + deadzone, y) != btn ||
+            find_button_no_deadzone(x - deadzone, y) != btn ||
+            find_button_no_deadzone(x, y + deadzone) != btn ||
+            find_button_no_deadzone(x, y - deadzone) != btn)
+        return 0;
+    return btn;
+}
+
+void touchpad_set_deadzone(int touchpad_deadzone)
+{
+    deadzone = touchpad_deadzone * DEADZONE_MULTIPLIER;
+}
+
+static int touchpad_read_device(void)
+{
+    return touchpad_btns;
+}
+
+/* i2c transfer only used for irq processing
+ * NOTE we use two sets of transfers because we setup one in the callback of the
+ * other, using one would be unsafe */
+static struct rmi_xfer_t rmi_irq_xfer[2];
+static uint8_t rmi_irq_buf; /* buffer to hold irq status register and sleep mode */
+static union
+{
+    unsigned char data[10];
+    struct
+    {
+        struct rmi_2d_absolute_data_t absolute;
+        struct rmi_2d_relative_data_t relative;
+        struct rmi_2d_gesture_data_t gesture;
+    }s;
+}rmi_irq_data; /* buffer to hold touchpad data */
+
+static void rmi_attn_cb(int bank, int pin, intptr_t user);
+
+/* callback for i2c transfer to change power level after irq */
+static void rmi_power_irq_cb(struct rmi_xfer_t *xfer)
+{
+    /* we do not recover from error for now */
+    if(xfer->status != I2C_SUCCESS)
+        panicf("rmi: clear i2c err %d", xfer->status);
+    /* now that interrupt is cleared, we can renable attention irq */
+    imx233_pinctrl_setup_irq(0, 27, true, true, false, &rmi_attn_cb, 0);
+}
+
+/* callback for i2c transfer to read/clear interrupt status register */
+static void rmi_clear_irq_cb(struct rmi_xfer_t *xfer)
+{
+    /* we do not recover from error for now */
+    if(xfer->status != I2C_SUCCESS)
+        panicf("rmi: clear i2c err %d", xfer->status);
+    /* at this point, we might have processed an event and the touchpad still be
+     * in very low power mode because of some previous inactivity; if it's the case,
+     * schedule another transfer to switch to a higher power mode before accepting the
+     * next event */
+    /* kick asynchronous transfer and only wait if mode was actually changed */
+    if(!rmi_set_sleep_mode_async(&rmi_irq_xfer[0], &rmi_irq_buf, ACTIVE_POWER_LEVEL,
+            &rmi_power_irq_cb))
+        /* now that interrupt is cleared, we can renable attention irq */
+        imx233_pinctrl_setup_irq(0, 27, true, true, false, &rmi_attn_cb, 0);
+}
+
+/* callback for i2c transfer to read touchpad data registers */
+static void rmi_data_irq_cb(struct rmi_xfer_t *xfer)
+{
+    /* we do not recover from error for now */
+    if(xfer->status != I2C_SUCCESS)
+        panicf("rmi: data i2c err %d", xfer->status);
+    /* now that we have the data, setup another transfer to clear interrupt */
+    rmi_build_xfer(&rmi_irq_xfer[1], true, RMI_INTERRUPT_REQUEST, 1,
+        &rmi_irq_buf, &rmi_clear_irq_cb);
+    /* kick transfer */
+    imx233_i2c_transfer(&rmi_irq_xfer[1].xfer_page);
+    /* now process touchpad data */
+    int absolute_x = rmi_irq_data.s.absolute.x_msb << 8 | rmi_irq_data.s.absolute.x_lsb;
+    int absolute_y = rmi_irq_data.s.absolute.y_msb << 8 | rmi_irq_data.s.absolute.y_lsb;
+    int nr_fingers = rmi_irq_data.s.absolute.misc & 7;
+    if(nr_fingers == 1)
+        touchpad_btns = find_button(absolute_x, absolute_y);
+    else
+        touchpad_btns = 0;
+}
+
+/* touchpad attention line interrupt */
+static void rmi_attn_cb(int bank, int pin, intptr_t user)
+{
+    (void) bank;
+    (void) pin;
+    (void) user;
+   /* build transfer to read data registers */
+    rmi_build_xfer(&rmi_irq_xfer[0], true, RMI_DATA_REGISTER(0),
+        sizeof(rmi_irq_data.data), rmi_irq_data.data, &rmi_data_irq_cb);
+    /* kick transfer */
+    imx233_i2c_transfer(&rmi_irq_xfer[0].xfer_page);
+    /* update last activity */
+    last_activity = current_tick;
+}
+
+void touchpad_enable_device(bool en)
+{
+    t_enable = en;
+    rmi_set_sleep_mode(en ? ACTIVE_POWER_LEVEL : RMI_SLEEP_MODE_SENSOR_SLEEP);
+}
+
+void touchpad_set_sensitivity(int level)
+{
+    /* handle negative values as well ! */
+    rmi_write_single(RMI_2D_SENSITIVITY_ADJ, (unsigned char)(int8_t)level);
+}
+
+/* transfer used by the activity timeout to change power level */
+static struct rmi_xfer_t rmi_tmo_xfer;
+static uint8_t rmi_tmo_buf;
+
+/* activity timeout: lower power level after some inactivity */
+static int activity_monitor(struct timeout *tmo)
+{
+    (void) tmo;
+    if(TIME_AFTER(current_tick, last_activity + ACTIVITY_TMO))
+    {
+        /* don't change power mode if touchpad is disabled, it's already in sleep mode */
+        if(t_enable)
+            rmi_set_sleep_mode_async(&rmi_tmo_xfer, &rmi_tmo_buf,
+                RMI_SLEEP_MODE_VERY_LOW_POWER, NULL);
+    }
+
+    return HZ; /* next check in 1 second */
+}
+
+void touchpad_init(void)
+{
+    /* Synaptics TouchPad information:
+     * - product id: 1533
+     * - nr function: 1 (0x10 = 2D touchpad)
+     * 2D Touchpad information (function 0x10)
+     * - nr data sources: 3
+     * - standard layout
+     * - extra data registers: 7
+     * - nr sensors: 1
+     * 2D Touchpad Sensor #0 information:
+     * - has relative data: yes
+     * - has palm detect: yes
+     * - has multi finger: yes
+     * - has enhanced gesture: yes
+     * - has scroller: no
+     * - has 2D scrollers: no
+     * - Maximum X: 3009
+     * - Maxumum Y: 1974
+     * - Resolution: 82
+     */
+
+    imx233_pinctrl_acquire(0, 26, "touchpad power");
+    imx233_pinctrl_set_function(0, 26, PINCTRL_FUNCTION_GPIO);
+    imx233_pinctrl_enable_gpio(0, 26, false);
+    imx233_pinctrl_set_drive(0, 26, PINCTRL_DRIVE_8mA);
+
+    /* use a timer to monitor touchpad activity and manage power level */
+    last_activity = current_tick;
+    timeout_register(&activity_tmo, activity_monitor, HZ, 0);
+
+    rmi_init();
+
+    char product_id[RMI_PRODUCT_ID_LEN];
+    rmi_read(RMI_PRODUCT_ID, RMI_PRODUCT_ID_LEN, product_id);
+    /* The OF adjust the sensitivity based on product_id[1] compared to 2.
+     * Since it doesn't seem to work great, just hardcode the sensitivity to
+     * some reasonable value for now. */
+    rmi_write_single(RMI_2D_SENSITIVITY_ADJ, 13);
+
+    rmi_write_single(RMI_2D_GESTURE_SETTINGS,
+        RMI_2D_GESTURE_PRESS_TIME_300MS |
+        RMI_2D_GESTURE_FLICK_DIST_4MM << RMI_2D_GESTURE_FLICK_DIST_BP |
+        RMI_2D_GESTURE_FLICK_TIME_700MS << RMI_2D_GESTURE_FLICK_TIME_BP);
+
+    /* we don't know in which mode the touchpad start so use a sane default */
+    rmi_set_sleep_mode(ACTIVE_POWER_LEVEL);
+    /* enable interrupt */
+    imx233_pinctrl_acquire(0, 27, "touchpad int");
+    imx233_pinctrl_set_function(0, 27, PINCTRL_FUNCTION_GPIO);
+    imx233_pinctrl_enable_gpio(0, 27, false);
+    imx233_pinctrl_setup_irq(0, 27, true, true, false, &rmi_attn_cb, 0);
+}
+
+/**
+ * Debug screen
+ */
 
 bool button_debug_screen(void)
 {
@@ -82,9 +504,27 @@ bool button_debug_screen(void)
     gesture_vp.y = zone_y - 80;
     gesture_vp.width = LCD_WIDTH / 2;
     gesture_vp.height = 80;
-
+    /* remember tick of last gestures */
+    #define GESTURE_TMO     HZ / 2
+    int tick_last_tap = current_tick - GESTURE_TMO;
+    int tick_last_doubletap = current_tick - GESTURE_TMO;
+    int tick_last_taphold = current_tick - GESTURE_TMO;
+    int tick_last_flick = current_tick - GESTURE_TMO;
+    int flick_x = 0, flick_y = 0;
+
+    /* BUG the data register are usually read by the IRQ already and it is
+     * important to not read them again, otherwise we could miss some events
+     * (most notable gestures). However, we only read registers when the
+     * touchpad is active so the data might be outdated if touchpad is
+     * inactive. We should implement a continuous reading mode for the debug
+     * screen. */
+
+    lcd_setfont(FONT_SYSFIXED);
     while(1)
     {
+        /* call button_get() to avoid an overflow in the button queue */
+        button_get(false);
+
         unsigned char sleep_mode = rmi_read_single(RMI_DEVICE_CONTROL) & RMI_SLEEP_MODE_BM;
         lcd_set_viewport(NULL);
         lcd_clear_display();
@@ -92,10 +532,9 @@ bool button_debug_screen(void)
         lcd_putsf(0, 0, "button bitmap: %x", btns);
         lcd_putsf(0, 1, "RMI: id=%s info=%s", product_id, product_info_str);
         lcd_putsf(0, 2, "xmax=%d ymax=%d res=%d", x_max, y_max, sensor_resol);
-        lcd_putsf(0, 3, "attn=%d ctl=%x int=%x",
+        lcd_putsf(0, 3, "attn=%d ctl=%x",
             imx233_pinctrl_get_gpio(0, 27) ? 0 : 1,
-            rmi_read_single(RMI_DEVICE_CONTROL),
-            rmi_read_single(RMI_INTERRUPT_REQUEST));
+            rmi_read_single(RMI_DEVICE_CONTROL));
         lcd_putsf(0, 4, "sensi: %d min_dist: %d", (int)sensitivity.value, min_dist);
         lcd_putsf(0, 5, "gesture: %x", gesture_settings);
 
@@ -109,21 +548,49 @@ bool button_debug_screen(void)
                 struct rmi_2d_gesture_data_t gesture;
             }s;
         }u;
+
+        /* Disable IRQs when reading to avoid reading incorrect data */
+        unsigned long cpsr = disable_irq_save();
+        memcpy(&u, &rmi_irq_data, sizeof(u));
+        restore_irq(cpsr);
+
         int absolute_x = u.s.absolute.x_msb << 8 | u.s.absolute.x_lsb;
         int absolute_y = u.s.absolute.y_msb << 8 | u.s.absolute.y_lsb;
         int nr_fingers = u.s.absolute.misc & 7;
         bool gesture = (u.s.absolute.misc & 8) == 8;
         int palm_width = u.s.absolute.misc >> 4;
 
-        rmi_read(RMI_DATA_REGISTER(0), 10, u.data);
         lcd_putsf(0, 6, "abs: %d %d %d", absolute_x, absolute_y, (int)u.s.absolute.z);
         lcd_putsf(0, 7, "rel: %d %d", (int)u.s.relative.x, (int)u.s.relative.y);
         lcd_putsf(0, 8, "gesture: %x %x", u.s.gesture.misc, u.s.gesture.flick);
         lcd_putsf(0, 9, "misc: w=%d g=%d f=%d", palm_width, gesture, nr_fingers);
-        lcd_putsf(30, 7, "sleep_mode: %d", 1 - sleep_mode);
+        lcd_putsf(0, 10, "sleep_mode: %d", sleep_mode);
+        lcd_putsf(0, 11, "deadzone: %d", deadzone);
+        /* display virtual touchpad with deadzones */
         lcd_set_viewport(&report_vp);
+        lcd_set_drawinfo(DRMODE_SOLID, LCD_RGBPACK(0xff, 0xff, 0), LCD_BLACK);
+        for(int i = 0; i < 3; i++)
+            for(int j = 0; j < 3; j++)
+            {
+                int x = j * x_max / 3;
+                if(j != 0)
+                    x += deadzone;
+                int x2 = (j + 1) * x_max / 3;
+                if(j != 2)
+                    x2 -= deadzone;
+                int y = i * y_max / 3;
+                if(i != 0)
+                    y += deadzone;
+                int y2 = (i + 1) * y_max / 3;
+                if(i != 2)
+                    y2 -= deadzone;
+                x = DX2SX(x); x2 = DX2SX(x2); y = DY2SY(y); y2 = DY2SY(y2);
+                lcd_drawrect(x, y2, x2 - x + 1, y - y2 + 1);
+            }
         lcd_set_drawinfo(DRMODE_SOLID, LCD_RGBPACK(0xff, 0, 0), LCD_BLACK);
         lcd_drawrect(0, 0, zone_w, zone_h);
+        /* put a done at the reported position of the finger
+         * also display relative motion by a line as reported by the device */
         if(nr_fingers == 1)
         {
             lcd_set_drawinfo(DRMODE_SOLID, LCD_RGBPACK(0, 0, 0xff), LCD_BLACK);
@@ -141,27 +608,37 @@ bool button_debug_screen(void)
             {
                 case RMI_2D_GEST_MISC_NO_TAP: break;
                 case RMI_2D_GEST_MISC_SINGLE_TAP:
-                    lcd_putsf(0, 0, "TAP!");
+                    tick_last_tap = current_tick;
                     break;
                 case RMI_2D_GEST_MISC_DOUBLE_TAP:
-                    lcd_putsf(0, 0, "DOUBLE TAP!");
+                    tick_last_doubletap = current_tick;
                     break;
                 case RMI_2D_GEST_MISC_TAP_AND_HOLD:
-                    lcd_putsf(0, 0, "TAP & HOLD!");
+                    tick_last_taphold = current_tick;
                     break;
                 default: break;
             }
 
             if(u.s.gesture.misc & RMI_2D_GEST_MISC_FLICK)
             {
-                lcd_putsf(0, 1, "FLICK!");
-                int flick_x = u.s.gesture.flick & RMI_2D_GEST_FLICK_X_BM;
-                int flick_y = (u.s.gesture.flick & RMI_2D_GEST_FLICK_Y_BM) >> RMI_2D_GEST_FLICK_Y_BP;
+                tick_last_flick = current_tick;
+                flick_x = u.s.gesture.flick & RMI_2D_GEST_FLICK_X_BM;
+                flick_y = (u.s.gesture.flick & RMI_2D_GEST_FLICK_Y_BM) >> RMI_2D_GEST_FLICK_Y_BP;
                 #define SIGN4EXT(a) \
                     if(a & 8) a = -((a ^ 0xf) + 1);
                 SIGN4EXT(flick_x);
                 SIGN4EXT(flick_y);
+            }
 
+            if(TIME_BEFORE(current_tick, tick_last_tap + GESTURE_TMO))
+                lcd_putsf(0, 0, "TAP!");
+            if(TIME_BEFORE(current_tick, tick_last_doubletap + GESTURE_TMO))
+                lcd_putsf(0, 1, "DOUBLE TAP!");
+            if(TIME_BEFORE(current_tick, tick_last_taphold + GESTURE_TMO))
+                lcd_putsf(0, 2, "TAP & HOLD!");
+            if(TIME_BEFORE(current_tick, tick_last_flick + GESTURE_TMO))
+            {
+                lcd_putsf(0, 3, "FLICK!");
                 int center_x = (LCD_WIDTH * 2) / 3;
                 int center_y = 40;
                 lcd_drawline(center_x, center_y, center_x + flick_x * 5, center_y - flick_y * 5);
@@ -185,228 +662,23 @@ bool button_debug_screen(void)
                 volkeys_delay_counter = 0;
             }
         }
-
-        yield();
     }
 
+    lcd_set_viewport(NULL);
+    lcd_setfont(FONT_UI);
     return true;
 }
 
-/* we emulate a 3x3 grid, this gives the button mapping */
-int button_mapping[3][3] =
-{
-    {BUTTON_BOTTOMLEFT, BUTTON_LEFT, BUTTON_BACK},
-    {BUTTON_DOWN, BUTTON_SELECT, BUTTON_UP},
-    {BUTTON_BOTTOMRIGHT, BUTTON_RIGHT, BUTTON_PLAYPAUSE},
-};
-
-#define RMI_INTERRUPT       1
-#define RMI_SET_SENSITIVITY 2
-#define RMI_SET_SLEEP_MODE  3
-/* timeout before lowering touchpad power from lack of activity */
-#define ACTIVITY_TMO        (5 * HZ)
-#define TOUCHPAD_WIDTH      3010
-#define TOUCHPAD_HEIGHT     1975
-#define DEADZONE_MULTIPLIER 2 /* deadzone multiplier */
-
-static int touchpad_btns = 0;
-static long rmi_stack [DEFAULT_STACK_SIZE/sizeof(long)];
-static const char rmi_thread_name[] = "rmi";
-static struct event_queue rmi_queue;
-static unsigned last_activity = 0;
-static bool t_enable = true;
-static int deadzone;
-
-/* Ignore deadzone function. If outside of the pad, project to border. */
-static int find_button_no_deadzone(int x, int y)
-{
-    /* compute grid coordinate */
-    int gx = MAX(MIN(x * 3 / TOUCHPAD_WIDTH, 2), 0);
-    int gy = MAX(MIN(y * 3 / TOUCHPAD_HEIGHT, 2), 0);
-
-    return button_mapping[gx][gy];
-}
-
-static int find_button(int x, int y)
-{
-    /* find button ignoring deadzones */
-    int btn = find_button_no_deadzone(x, y);
-    /* To check if we are in a deadzone, we try to shift the coordinates
-     * and see if we get the same button. Not that we do not want to apply
-     * the deadzone in the borders ! The code works even in the borders because
-     * the find_button_no_deadzone() project out-of-bound coordinates to the
-     * borders */
-    if(find_button_no_deadzone(x + deadzone, y) != btn ||
-            find_button_no_deadzone(x - deadzone, y) != btn ||
-            find_button_no_deadzone(x, y + deadzone) != btn ||
-            find_button_no_deadzone(x, y - deadzone) != btn)
-        return 0;
-    return btn;
-}
-
-void touchpad_set_deadzone(int touchpad_deadzone)
-{
-    deadzone = touchpad_deadzone * DEADZONE_MULTIPLIER;
-}
-
-static int touchpad_read_device(void)
-{
-    return touchpad_btns;
-}
-
-static void rmi_attn_cb(int bank, int pin, intptr_t user)
-{
-    (void) bank;
-    (void) pin;
-    (void) user;
-    /* the callback will not be fired until interrupt is enabled back so
-     * the queue will not overflow or contain multiple RMI_INTERRUPT events */
-    queue_post(&rmi_queue, RMI_INTERRUPT, 0);
-}
-
-static void do_interrupt(void)
-{
-    /* rmi_set_sleep_mode() does not do anything if the value
-     * it is given is already the one setted */
-    rmi_set_sleep_mode(RMI_SLEEP_MODE_LOW_POWER);
-    last_activity = current_tick;
-    /* clear interrupt */
-    rmi_read_single(RMI_INTERRUPT_REQUEST);
-    /* read data */
-    union
-    {
-        unsigned char data[10];
-        struct
-        {
-            struct rmi_2d_absolute_data_t absolute;
-            struct rmi_2d_relative_data_t relative;
-            struct rmi_2d_gesture_data_t gesture;
-        }s;
-    }u;
-    rmi_read(RMI_DATA_REGISTER(0), 10, u.data);
-    int absolute_x = u.s.absolute.x_msb << 8 | u.s.absolute.x_lsb;
-    int absolute_y = u.s.absolute.y_msb << 8 | u.s.absolute.y_lsb;
-    int nr_fingers = u.s.absolute.misc & 7;
-
-    if(nr_fingers == 1)
-        touchpad_btns = find_button(absolute_x, absolute_y);
-    else
-        touchpad_btns = 0;
-
-    /* enable interrupt */
-    imx233_pinctrl_setup_irq(0, 27, true, true, false, &rmi_attn_cb, 0);
-}
-
-void touchpad_enable_device(bool en)
-{
-    t_enable = en;
-    queue_post(&rmi_queue, RMI_SET_SLEEP_MODE, en ? RMI_SLEEP_MODE_LOW_POWER : RMI_SLEEP_MODE_SENSOR_SLEEP);
-}
-
-void touchpad_set_sensitivity(int level)
-{
-    queue_post(&rmi_queue, RMI_SET_SENSITIVITY, level);
-}
-
-static void rmi_thread(void)
-{
-    struct queue_event ev;
-
-    while(1)
-    {
-        /* make sure to timeout often enough for the activity timeout to take place */
-        queue_wait_w_tmo(&rmi_queue, &ev, HZ);
-        /* handle usb connect and ignore all messages except rmi interrupts */
-        switch(ev.id)
-        {
-            case SYS_USB_CONNECTED:
-                usb_acknowledge(SYS_USB_CONNECTED_ACK);
-                break;
-            case RMI_SET_SENSITIVITY:
-                /* handle negative values as well ! */
-                rmi_write_single(RMI_2D_SENSITIVITY_ADJ, (unsigned char)(int8_t)ev.data);
-                break;
-            case RMI_SET_SLEEP_MODE:
-                /* reset activity */
-                last_activity = current_tick;
-                rmi_set_sleep_mode(ev.data);
-                break;
-            case RMI_INTERRUPT:
-                do_interrupt();
-                break;
-            default:
-                /* activity timeout */
-                if(TIME_AFTER(current_tick, last_activity + ACTIVITY_TMO))
-                {
-                    /* don't change power mode if touchpad is disabled, it's already in sleep mode */
-                    if(t_enable)
-                        rmi_set_sleep_mode(RMI_SLEEP_MODE_VERY_LOW_POWER);
-                }
-                break;
-        }
-    }
-}
-
-static void touchpad_init(void)
-{
-    /* Synaptics TouchPad information:
-     * - product id: 1533
-     * - nr function: 1 (0x10 = 2D touchpad)
-     * 2D Touchpad information (function 0x10)
-     * - nr data sources: 3
-     * - standard layout
-     * - extra data registers: 7
-     * - nr sensors: 1
-     * 2D Touchpad Sensor #0 information:
-     * - has relative data: yes
-     * - has palm detect: yes
-     * - has multi finger: yes
-     * - has enhanced gesture: yes
-     * - has scroller: no
-     * - has 2D scrollers: no
-     * - Maximum X: 3009
-     * - Maxumum Y: 1974
-     * - Resolution: 82
-     */
-
-    imx233_pinctrl_acquire(0, 26, "touchpad power");
-    imx233_pinctrl_set_function(0, 26, PINCTRL_FUNCTION_GPIO);
-    imx233_pinctrl_enable_gpio(0, 26, false);
-    imx233_pinctrl_set_drive(0, 26, PINCTRL_DRIVE_8mA);
-
-    rmi_init(0x40);
-
-    char product_id[RMI_PRODUCT_ID_LEN];
-    rmi_read(RMI_PRODUCT_ID, RMI_PRODUCT_ID_LEN, product_id);
-    /* The OF adjust the sensitivity based on product_id[1] compared to 2.
-     * Since it doesn't to work great, just hardcode the sensitivity to
-     * some reasonable value for now. */
-    rmi_write_single(RMI_2D_SENSITIVITY_ADJ, 13);
-
-    rmi_write_single(RMI_2D_GESTURE_SETTINGS,
-        RMI_2D_GESTURE_PRESS_TIME_300MS |
-        RMI_2D_GESTURE_FLICK_DIST_4MM << RMI_2D_GESTURE_FLICK_DIST_BP |
-        RMI_2D_GESTURE_FLICK_TIME_700MS << RMI_2D_GESTURE_FLICK_TIME_BP);
-
-    queue_init(&rmi_queue, true);
-    create_thread(rmi_thread, rmi_stack, sizeof(rmi_stack), 0,
-            rmi_thread_name IF_PRIO(, PRIORITY_USER_INTERFACE) IF_COP(, CPU));
-    /* low power mode seems to be enough for normal use */
-    rmi_set_sleep_mode(RMI_SLEEP_MODE_LOW_POWER);
-    /* enable interrupt */
-    imx233_pinctrl_acquire(0, 27, "touchpad int");
-    imx233_pinctrl_set_function(0, 27, PINCTRL_FUNCTION_GPIO);
-    imx233_pinctrl_enable_gpio(0, 27, false);
-    imx233_pinctrl_setup_irq(0, 27, true, true, false, &rmi_attn_cb, 0);
-}
-
-#else
+#else /* BOOTLOADER */
 int touchpad_read_device(void)
 {
     return 0;
 }
 #endif
 
+/**
+ * Button API
+ */
 void button_init_device(void)
 {
 #ifndef BOOTLOADER