1 files changed, 39 insertions, 25 deletions

diff --git a/firmware/drivers/ft6x06.c b/firmware/drivers/ft6x06.c
index 538ca10480..c605ee0944 100644
--- a/firmware/drivers/ft6x06.c
+++ b/firmware/drivers/ft6x06.c
@@ -24,6 +24,16 @@
 #include "i2c-async.h"
 #include <string.h>
 
+#define BYTES_PER_POINT 6
+
+#ifdef FT6x06_SWAP_AXES
+# define POS_X pos_y
+# define POS_Y pos_x
+#else
+# define POS_X pos_x
+# define POS_Y pos_y
+#endif
+
 struct ft6x06_driver {
     /* i2c bus data */
     int i2c_cookie;
@@ -33,39 +43,42 @@ struct ft6x06_driver {
     ft6x06_event_cb event_cb;
 
     /* buffer for I2C transfers */
-    uint8_t raw_data[6];
+    uint8_t raw_data[1 + 2 + BYTES_PER_POINT * FT6x06_NUM_POINTS];
 };
 
 static struct ft6x06_driver ft_drv;
 struct ft6x06_state ft6x06_state;
 
+static inline void ft6x06_convert_point(const uint8_t* raw,
+                                        struct ft6x06_point* pt)
+{
+    pt->event    = (raw[0] >> 6) & 0x3;
+    pt->touch_id = (raw[2] >> 4) & 0xf;
+    pt->POS_X    = ((raw[0] & 0xf) << 8) | raw[1];
+    pt->POS_Y    = ((raw[2] & 0xf) << 8) | raw[3];
+    pt->weight   = raw[4];
+    pt->area     = (raw[5] >> 4) & 0xf;
+}
+
 static void ft6x06_i2c_callback(int status, i2c_descriptor* desc)
 {
     (void)desc;
     if(status != I2C_STATUS_OK)
         return;
 
-    int evt = ft_drv.raw_data[1] >> 6;
-    int tx = ft_drv.raw_data[2] | ((ft_drv.raw_data[1] & 0xf) << 8);
-    int ty = ft_drv.raw_data[4] | ((ft_drv.raw_data[3] & 0xf) << 8);
-
-    ft6x06_state.event = evt;
-#ifdef FT6x06_SWAP_AXES
-    ft6x06_state.pos_x = ty;
-    ft6x06_state.pos_y = tx;
-#else
-    ft6x06_state.pos_x = tx;
-    ft6x06_state.pos_y = ty;
-#endif
+    ft6x06_state.gesture = ft_drv.raw_data[1];
+    ft6x06_state.nr_points = ft_drv.raw_data[2] & 0xf;
+    for(int i = 0; i < FT6x06_NUM_POINTS; ++i) {
+        ft6x06_convert_point(&ft_drv.raw_data[3 + i * BYTES_PER_POINT],
+                             &ft6x06_state.points[i]);
+    }
 
-    ft_drv.event_cb(evt, ft6x06_state.pos_x, ft6x06_state.pos_y);
+    ft_drv.event_cb(&ft6x06_state);
 }
 
-static void ft6x06_dummy_event_cb(int evt, int tx, int ty)
+static void ft6x06_dummy_event_cb(struct ft6x06_state* state)
 {
-    (void)evt;
-    (void)tx;
-    (void)ty;
+    (void)state;
 }
 
 void ft6x06_init(void)
@@ -74,9 +87,10 @@ void ft6x06_init(void)
     memset(&ft_drv, 0, sizeof(ft_drv));
     ft_drv.event_cb = ft6x06_dummy_event_cb;
 
-    ft6x06_state.event = FT6x06_EVT_NONE;
-    ft6x06_state.pos_x = 0;
-    ft6x06_state.pos_y = 0;
+    memset(&ft6x06_state, 0, sizeof(struct ft6x06_state));
+    ft6x06_state.gesture = -1;
+    for(int i = 0; i < FT6x06_NUM_POINTS; ++i)
+        ft6x06_state.points[i].event = FT6x06_EVT_NONE;
 
     /* Reserve bus management cookie */
     ft_drv.i2c_cookie = i2c_async_reserve_cookies(FT6x06_BUS, 1);
@@ -85,16 +99,16 @@ void ft6x06_init(void)
     ft_drv.i2c_desc.slave_addr = FT6x06_ADDR;
     ft_drv.i2c_desc.bus_cond   = I2C_START | I2C_STOP;
     ft_drv.i2c_desc.tran_mode  = I2C_READ;
-    ft_drv.i2c_desc.buffer[0]  = &ft_drv.raw_data[5];
+    ft_drv.i2c_desc.buffer[0]  = &ft_drv.raw_data[0];
     ft_drv.i2c_desc.count[0]   = 1;
-    ft_drv.i2c_desc.buffer[1]  = &ft_drv.raw_data[0];
-    ft_drv.i2c_desc.count[1]   = 5;
+    ft_drv.i2c_desc.buffer[1]  = &ft_drv.raw_data[1];
+    ft_drv.i2c_desc.count[1]   = sizeof(ft_drv.raw_data) - 1;
     ft_drv.i2c_desc.callback   = ft6x06_i2c_callback;
     ft_drv.i2c_desc.arg        = 0;
     ft_drv.i2c_desc.next       = NULL;
 
     /* Set I2C register address */
-    ft_drv.raw_data[5] = 0x02;
+    ft_drv.raw_data[0] = 0x01;
 }
 
 void ft6x06_set_event_cb(ft6x06_event_cb cb)