diff --git a/display/Makefile b/display/Makefile
index 7f5f170..d852108 100644
--- a/display/Makefile
+++ b/display/Makefile
@@ -71,13 +71,13 @@ COMMONDEPS = src/config.h
 default: bin/firmware.hex
 
 inc/lpc111x.h:
-	ln -s ../$(CODEBASEDIR)/lpc111x.h $@
+	ln -sf ../$(CODEBASEDIR)/lpc111x.h $@
 
 inc/sysdefs.h:
-	ln -s ../$(CODEBASEDIR)/sysdefs.h $@
+	ln -sf ../$(CODEBASEDIR)/sysdefs.h $@
 
 inc/projectconfig.h:
-	ln -s ../src/config.h $@
+	ln -sf ../src/config.h $@
 
 src/generated_font_data.inc: $(GENFONT)
 	$(GENFONT) --no-flip \
diff --git a/display/src/comm_i2c.inc.c b/display/src/comm_i2c.inc.c
index a74fe33..4d3b076 100644
--- a/display/src/comm_i2c.inc.c
+++ b/display/src/comm_i2c.inc.c
@@ -214,6 +214,7 @@ void I2C_IRQHandler()
                     backbuffer_ready = true;
                 }
                 NVIC_SetPendingIRQ(received_irqn);
+                __attribute__((fallthrough));
             }
             case MSG_ADDRESS_HOST:
             {
diff --git a/display/src/comm_uart.inc.c b/display/src/comm_uart.inc.c
index 543abd1..1b03083 100644
--- a/display/src/comm_uart.inc.c
+++ b/display/src/comm_uart.inc.c
@@ -45,14 +45,11 @@ static struct msg_header_t ping_header VAR_RAM =
         MSG_ADDRESS_HOST,
         0,
         MSG_FLAG_ACK | MSG_FLAG_ECHO);
-static volatile uint32_t buffer = 0;
 
 static inline void uart_init(const uint32_t baudrate)
 {
     NVIC_DisableIRQ(UART_IRQn);
 
-    buffer = 0xdeadbeef;
-
     /* Set 1.6 UART RXD */
     IOCON_PIO1_6 &= ~IOCON_PIO1_6_FUNC_MASK;
     IOCON_PIO1_6 |= IOCON_PIO1_6_FUNC_UART_RXD;
@@ -141,6 +138,7 @@ void uart_tx_irq()
             uart.state.trns_src = (const volatile uint8_t*)uart.queue.items[uart.queue.active_item].header;
             uart.state.trns_end = uart.state.trns_src + sizeof(struct msg_header_t);
         }
+        __attribute__((fallthrough));
     }
     case TXU_SEND_HEADER:
     {
@@ -298,6 +296,7 @@ void uart_rx_irq()
         // missing break is intentional: receive the first bytes immediately
         // turn the timer on
         TMR_COMM_TIMEOUT_TCR = (1<<0) | (0<<1);
+        __attribute__((fallthrough));
     }
     case RXU_RECEIVE_HEADER:
     {
@@ -376,6 +375,7 @@ void uart_rx_irq()
         uart.state.recv_dest = &uart.state.dest_msg->msg.data[0];
         uart.state.recv_end = uart.state.recv_dest + HDR_GET_PAYLOAD_LENGTH(uart.state.curr_header);
         // we can smoothly continue here if more data is available
+        __attribute__((fallthrough));
     }
     case RXU_RECEIVE_PAYLOAD:
     {
@@ -385,6 +385,7 @@ void uart_rx_irq()
         uart_rx_state = RXU_RECEIVE_CHECKSUM;
         uart.state.recv_dest = (uint8_t*)&uart.state.dest_msg->msg.checksum;
         uart.state.recv_end = uart.state.recv_dest + sizeof(msg_checksum_t);
+        __attribute__((fallthrough));
     }
     case RXU_RECEIVE_CHECKSUM:
     {
diff --git a/display/src/draw.c b/display/src/draw.c
index f826d97..29989a8 100644
--- a/display/src/draw.c
+++ b/display/src/draw.c
@@ -2,7 +2,7 @@
 
 #include "lcd.h"
 
-inline void clamp_x(coord_int_t *x)
+static inline void clamp_x(coord_int_t *x)
 {
     if (*x < 0) {
         *x = 0;
@@ -11,7 +11,7 @@ inline void clamp_x(coord_int_t *x)
     }
 }
 
-inline void clamp_y(coord_int_t *y)
+static inline void clamp_y(coord_int_t *y)
 {
     if (*y < 0) {
         *y = 0;
@@ -20,8 +20,8 @@ inline void clamp_y(coord_int_t *y)
     }
 }
 
-inline void rectangle_clamp(coord_int_t *x0, coord_int_t *y0,
-                            coord_int_t *x1, coord_int_t *y1)
+static inline void rectangle_clamp(coord_int_t *x0, coord_int_t *y0,
+                                   coord_int_t *x1, coord_int_t *y1)
 {
     clamp_x(x0);
     clamp_x(x1);
@@ -29,8 +29,8 @@ inline void rectangle_clamp(coord_int_t *x0, coord_int_t *y0,
     clamp_y(y1);
 }
 
-inline void rectangle_clamp_and_swap(coord_int_t *x0, coord_int_t *y0,
-                                     coord_int_t *x1, coord_int_t *y1)
+static inline void rectangle_clamp_and_swap(coord_int_t *x0, coord_int_t *y0,
+                                            coord_int_t *x1, coord_int_t *y1)
 {
     rectangle_clamp(x0, y0, x1, y1);
 
diff --git a/display/src/font.c b/display/src/font.c
index a199b4a..4cf5c50 100644
--- a/display/src/font.c
+++ b/display/src/font.c
@@ -2,8 +2,8 @@
 
 #include <stdlib.h>
 
-inline const struct glyph_t *font_find_glyph(const struct font_t *font,
-                                             const codepoint_t codepoint)
+static inline const struct glyph_t *font_find_glyph(const struct font_t *font,
+                                                    const codepoint_t codepoint)
 {
     uint16_t offset = 0;
 
diff --git a/display/src/lcd.c b/display/src/lcd.c
index 0ee5baf..328e407 100644
--- a/display/src/lcd.c
+++ b/display/src/lcd.c
@@ -31,14 +31,14 @@ static uint16_t lcd_brightness_awake_backup VAR_RAM = 0xC000;
 
 /* alphabetical order broken so that inlines appear before their use */
 
-inline void _configure_pwm()
+static inline void _configure_pwm()
 {
     GPIO_GPIO1DIR |= (1<<9);
     GPIO_GPIO1DATA &= ~(1<<9);
     IOCON_PIO1_9 = (0x1<<0);
 }
 
-inline void _disable_pwm()
+static inline void _disable_pwm()
 {
     TMR_TMR16B0TCR = 0; // pwm timer
     TMR_TMR16B1TCR = 0; // fade timer
@@ -46,7 +46,7 @@ inline void _disable_pwm()
     GPIO_GPIO1DATA &= ~(1<<9);
 }
 
-inline void _enable_pwm()
+static inline void _enable_pwm()
 {
     GPIO_GPIO1DATA &= ~(1<<9);
     TMR_TMR16B0TC = 0;
@@ -58,7 +58,7 @@ inline void _enable_pwm()
     TMR_TMR16B1TCR = 1; // fade timer
 }
 
-inline void lcd_wrcmd8(uint8_t cmd)
+void lcd_wrcmd8(uint8_t cmd)
 {
     LCD_MASKED_GPIO(LCD_RS_MASK, 0);
     LCD_MASKED_GPIO(LCD_WR_MASK, 0);
@@ -68,7 +68,7 @@ inline void lcd_wrcmd8(uint8_t cmd)
     LCD_MASKED_GPIO(LCD_RS_MASK, LCD_RS_MASK);
 }
 
-inline void lcd_wrdata8(uint8_t data)
+void lcd_wrdata8(uint8_t data)
 {
     LCD_MASKED_GPIO(LCD_WR_MASK, 0);
     LCD_MASKED_GPIO(LCD_DATA_MASK, data);
@@ -76,33 +76,33 @@ inline void lcd_wrdata8(uint8_t data)
     LCD_MASKED_GPIO(LCD_WR_MASK, LCD_WR_MASK);
 }
 
-inline void lcd_wrdata16(uint16_t data)
+void lcd_wrdata16(uint16_t data)
 {
     lcd_wrdata8((data >> 8) & 0xff);
     lcd_wrdata8(data & 0xff);
 }
 
-inline void lcd_draw(uint16_t colour)
+void lcd_draw(uint16_t colour)
 {
     lcd_wrdata16(colour);
 }
 
-inline void lcd_disable()
+void lcd_disable()
 {
     LCD_MASKED_GPIO(LCD_CS_MASK, LCD_CS_MASK);
 }
 
-inline void lcd_enable()
+void lcd_enable()
 {
     LCD_MASKED_GPIO(LCD_CS_MASK, 0);
 }
 
-inline void lcd_drawstart()
+void lcd_drawstart()
 {
     lcd_wrcmd8(LCD_CMD_WRITE);
 }
 
-inline void lcd_drawstop()
+void lcd_drawstop()
 {
 
 }
diff --git a/display/src/main.c b/display/src/main.c
index 4cc3992..773a601 100644
--- a/display/src/main.c
+++ b/display/src/main.c
@@ -49,14 +49,11 @@ void uint32_to_hex(const uint32_t c, uint8_t *dest)
     } while (shift);
 }
 
-static int irq_called VAR_RAM = 0;
 static volatile bool msg_pending = false;
 
 
 void ADC_IRQHandler(void)
 {
-    // only used for touch right now
-    irq_called = 1;
     touch_intr_sm();
 }
 
@@ -112,23 +109,28 @@ static coord_int_t prevz VAR_RAM = 0;
 
 static inline enum event_t wait_for_event()
 {
-    struct ticks_t t1 = ticks_get();
     while (1) {
-        do {
-            if (msg_pending) {
-                msg_pending = false;
-                return EV_COMM;
+        __asm__ volatile("wfi");
+        struct ticks_t t1 = ticks_get();
+        if (ticks_delta(&last_touch_sample, &t1) > TOUCH_SAMPLE_INTERVAL) {
+            touch_intr_start();
+        }
+
+        if (msg_pending) {
+            msg_pending = false;
+            return EV_COMM;
+        }
+
+        if (touch_pending) {
+            last_touch_sample = ticks_get();
+            touch_pending = false;
+            if (touch_get_raw_z() > TOUCH_MIN_PRESSURE) {
+                prevz = 1;
+                return EV_TOUCH;
+            } else if (prevz > 0) {
+                prevz = 0;
+                return EV_TOUCH;
             }
-            t1 = ticks_get();
-        } while (ticks_delta(&last_touch_sample, &t1) < TOUCH_SAMPLE_INTERVAL);
-        touch_sample();
-        last_touch_sample = ticks_get();
-        if (touch_get_raw_z() != 0) {
-            prevz = 1;
-            return EV_TOUCH;
-        } else if (prevz > 0) {
-            prevz = 0;
-            return EV_TOUCH;
         }
     }
 }
@@ -421,7 +423,6 @@ int main(void)
     DISABLE_IRQ();
 
     SCB_PDRUNCFG &= ~SCB_PDRUNCFG_ADC;
-    //~ NVIC_EnableIRQ(ADC_IRQn);
 
     SCB_SYSAHBCLKCTRL |= SCB_SYSAHBCLKCTRL_GPIO
                       |  SCB_SYSAHBCLKCTRL_IOCON
@@ -509,6 +510,8 @@ int main(void)
         touch_wait_for_clear();
     }
 
+    NVIC_EnableIRQ(ADC_IRQn);
+
     //~ comm_debug_tx_pong();
 
     fill_rectangle(0, 0, LCD_WIDTH-1, LCD_HEIGHT-1, 0x0000);
@@ -541,26 +544,21 @@ int main(void)
             z = touch_get_z();
 
             if ((z > 0) && (abs(prevx - x) + abs(prevy - y) <= 3)) {
-                prevx = x;
-                prevy = y;
                 break;
             }
 
-            prevx = x;
-            prevy = y;
-
-            if (z == 0) {
-                prevx = -100;
-                prevy = -100;
-            }
-
             //~ lcd_enable();
             //~ fill_rectangle(x-2, y-2, x+2, y+2, 0x001f);
             //~ lcd_disable();
 
             msg_payload.subject = LPC_SUBJECT_TOUCH_EVENT;
-            msg_payload.payload.touch_ev.x = x;
-            msg_payload.payload.touch_ev.y = y;
+            if (z == 0) {
+                msg_payload.payload.touch_ev.x = prevx;
+                msg_payload.payload.touch_ev.y = prevy;
+            } else {
+                msg_payload.payload.touch_ev.x = x;
+                msg_payload.payload.touch_ev.y = y;
+            }
             msg_payload.payload.touch_ev.z = z;
             HDR_SET_PAYLOAD_LENGTH(msg_header, sizeof(struct lpc_msg_t));
             msg_checksum = checksum((const uint8_t*)&msg_payload,
@@ -570,6 +568,14 @@ int main(void)
                             (const uint8_t*)&msg_payload,
                             msg_checksum);
 
+            prevx = x;
+            prevy = y;
+
+            if (z == 0) {
+                prevx = -100;
+                prevy = -100;
+            }
+
             break;
         }
         case EV_NONE:
diff --git a/display/src/touch.c b/display/src/touch.c
index 3b0b0c3..b290619 100644
--- a/display/src/touch.c
+++ b/display/src/touch.c
@@ -5,8 +5,6 @@
 #include "config.h"
 #include "utils.h"
 
-#define MIN_PRESSURE 200
-
 #define IOCON_PIO                      (0x00<<0) //pio
 #define IOCON_R_PIO                    (0x01<<0) //pio (reserved pins)
 #define IOCON_ADC                      (0x01<<0) //adc
@@ -96,15 +94,15 @@ _Static_assert(GPIO_GPIOn_BASE(3) == GPIO_GPIO3_BASE, "GPIOn_BASE does not work
 #define HIZDOWN(which)  ANY_SET_HIZDOWN(which ## _IOCON, which ## _PORT, which ## _PIN)
 #define ADC(which)      ANY_SET_ADC(which ## _IOCON, which ## _PORT, which ## _PIN)
 
-static coord_int_t raw_x VAR_RAM;
-static coord_int_t raw_y VAR_RAM;
-static coord_int_t raw_z VAR_RAM;
-static coord_int_t last_x VAR_RAM;
-static coord_int_t last_y VAR_RAM;
+static volatile coord_int_t raw_x VAR_RAM;
+static volatile coord_int_t raw_y VAR_RAM;
+static volatile coord_int_t raw_z VAR_RAM;
+static volatile coord_int_t last_x VAR_RAM;
+static volatile coord_int_t last_y VAR_RAM;
 
 /* static coord_int_t intr_tmp VAR_RAM; */
 
-volatile int touch_ev VAR_RAM;
+volatile bool touch_pending VAR_RAM;
 volatile enum touch_intr_state_t touch_intr_state VAR_RAM = TOUCH_STATE_IDLE;
 
 static struct touch_calibration_t calibration VAR_RAM;
@@ -116,7 +114,7 @@ int touch_intr_start()
         return 1;
     }
 
-    touch_intr_state = TOUCH_STATE_SAMPLING_Z;
+    touch_intr_state = TOUCH_STATE_SAMPLING_Z1;
     ADC(XM);
     ADC(YP);
     // set X+ to Vcc, Y- to GND
@@ -125,14 +123,16 @@ int touch_intr_start()
 
     *(pREG32(ADC_AD0INTEN)) |= ADC_AD0INTEN_ADINTEN2;
     ADC_AD0CR = ADC_AD0CR_BURST_HWSCANMODE
-              | ADC_AD0CR_SEL_AD2
-              | ADC_AD0CR_SEL_AD1
-              | ADC_AD0CR_CLKS_10BITS
-              | (ADC_AD0CR & ADC_AD0CR_CLKDIV_MASK);
+        | ADC_AD0CR_SEL_AD2
+        | ADC_AD0CR_CLKS_10BITS
+        | (ADC_AD0CR & ADC_AD0CR_CLKDIV_MASK);
 
     return 0;
 }
 
+static volatile coord_int_t z1;
+static volatile coord_int_t z2;
+
 void touch_intr_sm()
 {
     // first, disable sampling
@@ -144,17 +144,26 @@ void touch_intr_sm()
         // this is invalid!
         break;
     }
-    case TOUCH_STATE_SAMPLING_Z:
+    case TOUCH_STATE_SAMPLING_Z1:
+    {
+        z1 = ADC_EXTRACT(XM_AD);
+        touch_intr_state = TOUCH_STATE_SAMPLING_Z2;
+        *(pREG32(ADC_AD0INTEN)) |= ADC_AD0INTEN_ADINTEN1;
+        ADC_AD0CR = ADC_AD0CR_BURST_HWSCANMODE
+            | ADC_AD0CR_SEL_AD1
+            | ADC_AD0CR_CLKS_10BITS
+            | (ADC_AD0CR & ADC_AD0CR_CLKDIV_MASK);
+        break;
+    }
+    case TOUCH_STATE_SAMPLING_Z2:
     {
-        coord_int_t z1, z2;
-        z1 = *(pREG32(ADC_AD0DR1)) & 0x3FF;
-        z2 = *(pREG32(ADC_AD0DR2)) & 0x3FF;
-
-        raw_z = (0x3FF-z2) + z1;
-        if (raw_z < MIN_PRESSURE) {
-            raw_x = 0xfff;
-            raw_y = 0xfff;
+        z2 = ADC_EXTRACT(YP_AD);
+
+        raw_z = ((0x3FF-z1) + z2);
+        if (raw_z < TOUCH_MIN_PRESSURE) {
+            raw_z = 0;
             touch_intr_state = TOUCH_STATE_IDLE;
+            touch_pending = true;
             break;
         }
 
@@ -192,7 +201,7 @@ void touch_intr_sm()
     }*/
     case TOUCH_STATE_SAMPLING_X:
     {
-        raw_x = *(pREG32(ADC_AD0DR3)) & 0x3FF;
+        raw_y = ADC_EXTRACT(YM_AD);
 
         ADC(XP);
         ADC(XM);
@@ -209,8 +218,9 @@ void touch_intr_sm()
     }
     case TOUCH_STATE_SAMPLING_Y:
     {
-        raw_y = *(pREG32(ADC_AD0DR2)) & 0x3FF;
+        raw_x = 1023 - ADC_EXTRACT(XM_AD);
         touch_intr_state = TOUCH_STATE_IDLE;
+        touch_pending = true;
 
         HIZUP(XP);
         HIZUP(XM);
@@ -236,7 +246,7 @@ void touch_init()
     HIZUP(YM);
 }
 
-inline void calculate_calibration_xy(
+static inline void calculate_calibration_xy(
     const fp11_4_t lcd1,
     const fp11_4_t lcd2,
     const fp11_4_t touch1,
@@ -404,7 +414,7 @@ void touch_sample()
 
     raw_z = z;
 
-    if(z > MIN_PRESSURE) //valid touch?
+    if(z > TOUCH_MIN_PRESSURE) //valid touch?
     {
         //cal_z = 0;
 
@@ -487,4 +497,3 @@ void touch_wait_for_clear()
         delay_ms(50);
     }
 }
-
diff --git a/display/src/touch.h b/display/src/touch.h
index ea0576b..9115852 100644
--- a/display/src/touch.h
+++ b/display/src/touch.h
@@ -2,6 +2,7 @@
 #define _TOUCH_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 #include "draw.h"
 #include "fp11_4.h"
@@ -10,6 +11,8 @@
  * firmware code, as no specification for the touchpad could be found.
  */
 
+#define TOUCH_MIN_PRESSURE 200
+
 struct touch_calibration_t {
     /* offset_{x,y} is in ±11.4 format, scale_{x,y} is in ±0.15
      * format */
@@ -22,12 +25,13 @@ struct touch_calibration_t {
 
 enum touch_intr_state_t {
     TOUCH_STATE_IDLE,
-    TOUCH_STATE_SAMPLING_Z,
+    TOUCH_STATE_SAMPLING_Z1,
+    TOUCH_STATE_SAMPLING_Z2,
     TOUCH_STATE_SAMPLING_X,
     TOUCH_STATE_SAMPLING_Y
 };
 
-extern volatile int touch_ev;
+extern volatile bool touch_pending;
 extern volatile enum touch_intr_state_t touch_intr_state;
 
 int touch_intr_start();
diff --git a/display/src/utils.h b/display/src/utils.h
index d9389ed..7d31c68 100644
--- a/display/src/utils.h
+++ b/display/src/utils.h
@@ -10,13 +10,15 @@
 #define DISABLE_IRQ()                  __asm__ volatile ("cpsid i")
 
 #define ADC_AD0DRn(chn) (*pREG32(ADC_AD0DR0 + (chn*4)))
+#define ADC_READY(chn) (ADC_AD0DRn(chn) & (1<<31))
+#define ADC_EXTRACT(chn) ((ADC_AD0DRn(chn)>>6) & 0x3FF)
 #define ADC_READ(chn, x)             { \
     ADC_AD0CR |= (1<<chn) | ADC_AD0CR_START_STARTNOW; \
     /* wait until conversion finishes (bit 31 set) */ \
-    while (!(ADC_AD0DRn(chn) & (1<<31))); \
+    while (!ADC_READY(chn));                          \
     /* disable conversion, disable all channels */ \
     ADC_AD0CR &= 0xF8FFFF00; \
-    x = (ADC_AD0DRn(chn)>>6) & 0x3FF;}
+    x = ADC_EXTRACT(chn);}
 
 inline void delay_ms(uint16_t ms)
 {