1 files changed, 31 insertions, 1 deletions

diff --git a/firmware/target/arm/tms320dm320/mrobe-500/button-mr500.c b/firmware/target/arm/tms320dm320/mrobe-500/button-mr500.c
index b157ae2d12..c30422fc79 100644
--- a/firmware/target/arm/tms320dm320/mrobe-500/button-mr500.c
+++ b/firmware/target/arm/tms320dm320/mrobe-500/button-mr500.c
@@ -38,6 +38,36 @@
                                 /* but always the same one for the session? */
 static short last_x, last_y, last_z1, last_z2; /* for the touch screen */
 static int last_touch;
+
+static struct touch_calibration_point topleft, bottomright;
+static bool using_calibration = false;
+void use_calibration(bool enable)
+{
+    using_calibration = enable;
+}
+
+void set_calibration_points(struct touch_calibration_point *tl,
+                            struct touch_calibration_point *br)
+{
+    memcpy(&topleft, tl, sizeof(struct touch_calibration_point));
+    memcpy(&bottomright, br, sizeof(struct touch_calibration_point));
+}
+static int touch_to_pixels(short val_x, short val_y)
+{
+    short x,y;
+    int x1,x2;
+    if (!using_calibration)
+        return (val_x<<16)|val_y;
+    x1 = topleft.val_x; x2 = bottomright.val_x;
+    x = (val_x-x1)*(bottomright.px_x - topleft.px_x) / (x2 - x1) + topleft.px_x;
+    x1 = topleft.val_y; x2 = bottomright.val_y;
+    y = (val_y-x1)*(bottomright.px_y - topleft.px_y) / (x2 - x1) + topleft.px_y;
+    if (x < 0)
+        x = 0;
+    if (y < 0)
+        y = 0;
+    return (x<<16)|y;
+}
 void button_init_device(void)
 {
     last_touch = 0;
@@ -128,6 +158,6 @@ void GIO14(void)
 {
     tsc2100_read_values(&last_x,  &last_y,
                         &last_z1, &last_z2);
-    last_touch = (last_x<<16)|last_y;
+    last_touch = touch_to_pixels(last_x, last_y);
     IO_INTC_IRQ2 = (1<<3);
 }