touchpad.diff

diff --git a/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c b/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
index 1474e06..4ef17b5 100644
--- a/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
+++ b/firmware/target/arm/imx233/sansa-fuzeplus/button-fuzeplus.c
@@ -200,11 +200,16 @@ static struct button_area_t button_areas[] =
 
 #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    (60 * HZ)
 
 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 int find_button(int x, int y)
 {
@@ -238,53 +243,76 @@ void touchpad_set_sensitivity(int level)
     queue_post(&rmi_queue, RMI_SET_SENSITIVITY, level);
 }
 
+void touchpad_enable(bool en)
+{
+    queue_post(&rmi_queue, RMI_SET_SLEEP_MODE, en ? RMI_SLEEP_MODE_NORMAL : RMI_SLEEP_MODE_SLEEP);
+}
+
+static void do_interrupt(void)
+{
+    /* FIXME: rmi_set_sleep_mode() should not do anything if the current mode
+     * is the one requested, it should use a cached value to do this */
+    rmi_set_sleep_mode(RMI_SLEEP_MODE_NORMAL);
+    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);
+}
+
 static void rmi_thread(void)
 {
     struct queue_event ev;
-    
+
     while(1)
     {
-        queue_wait(&rmi_queue, &ev);
+        queue_wait_w_tmo(&rmi_queue, &ev, HZ);
         /* handle usb connect and ignore all messages except rmi interrupts */
-        if(ev.id == SYS_USB_CONNECTED)
-        {
-            usb_acknowledge(SYS_USB_CONNECTED_ACK);
-            continue;
-        }
-        else if(ev.id == RMI_SET_SENSITIVITY)
+        switch(ev.id)
         {
-            /* handle negative values as well ! */
-            rmi_write_single(RMI_2D_SENSITIVITY_ADJ, (unsigned char)(int8_t)ev.data);
-            continue;
+            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))
+                    rmi_set_sleep_mode(RMI_SLEEP_MODE_LOW_POWER);
+                break;
         }
-        else if(ev.id != RMI_INTERRUPT)
-            continue;
-        /* 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);
     }
 }