diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 8caef367e..29a6073a4 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -50,6 +50,7 @@
 #endif
 
 #include "ultralcd.h"
+#include "pat9125.h"
 #include "Configuration_prusa.h"
 #include "planner.h"
 #include "stepper.h"
@@ -1043,6 +1044,9 @@ void setup()
   world2machine_reset();
   
   lcd_init();
+    
+  pat9125_init(200, 200);
+    
   if (!READ(BTN_ENC))
   {
 	  _delay_ms(1000);
@@ -1317,6 +1321,8 @@ void loop()
   checkHitEndstops();
   lcd_update();
 
+  pat9125_update();
+    
   tmc2130_check_overtemp();
 }
 
diff --git a/Firmware/pat9125.cpp b/Firmware/pat9125.cpp
new file mode 100755
index 000000000..d09c55e10
--- /dev/null
+++ b/Firmware/pat9125.cpp
@@ -0,0 +1,74 @@
+#include "pat9125.h"
+#include "swspi.h"
+
+
+#ifdef SWSPI_RPI
+//	#include <bcm2835.h>
+	#define DELAY(delay) usleep(delay)
+#endif //SWSPI_RPI
+
+#ifdef SWSPI_AVR
+	#include "Arduino.h"
+	#define DELAY(delay) delayMicroseconds(delay)
+#endif //SWSPI_AVR
+
+unsigned char ucPID1 = 0;
+unsigned char ucPID2 = 0;
+int pat9125_x = 0;
+int pat9125_y = 0;
+
+int pat9125_init(unsigned char xres, unsigned char yres)
+{
+	swspi_init();
+	ucPID1 = pat9125_rd_reg(PAT9125_PID1);
+	ucPID2 = pat9125_rd_reg(PAT9125_PID2);
+	if ((ucPID1 != 0x31) || (ucPID2 != 0x91))
+	{
+		pat9125_wr_reg(PAT9125_RES_X, xres);
+		pat9125_wr_reg(PAT9125_RES_Y, yres);
+		return 0;
+	}
+	return 1;
+}
+
+int pat9125_update()
+{
+	if ((ucPID1 == 0x31) && (ucPID2 == 0x91))
+	{
+		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
+		if (ucMotion & 0x80)
+		{
+			int iDX = pat9125_rd_reg(PAT9125_DELTA_XL);
+			int iDY = pat9125_rd_reg(PAT9125_DELTA_YL);
+			if (iDX >= 0x80) iDX = iDX - 256;
+			if (iDY >= 0x80) iDY = iDY - 256;
+			pat9125_x += iDX;
+			pat9125_y += iDY;
+			return 1;
+		}
+	}
+	return 0;
+}
+
+unsigned char pat9125_rd_reg(unsigned char addr)
+{
+	swspi_start();
+	DELAY(100);
+	swspi_tx(addr & 0x7f);
+	DELAY(100);
+	unsigned char data = swspi_rx();
+	swspi_stop();
+	DELAY(100);
+	return data;
+}
+
+void pat9125_wr_reg(unsigned char addr, unsigned char data)
+{
+	swspi_start();
+	DELAY(100);
+	swspi_tx(addr | 0x80);
+	DELAY(100);
+	swspi_tx(data);
+	swspi_stop();
+	DELAY(100);
+}
diff --git a/Firmware/pat9125.h b/Firmware/pat9125.h
new file mode 100755
index 000000000..d7600a7b7
--- /dev/null
+++ b/Firmware/pat9125.h
@@ -0,0 +1,38 @@
+#ifndef PAT9125_H
+#define PAT9125_H
+
+//#define PAT9125_RPI
+#define PAT9125_AVR
+
+//PAT9125 registers
+#define PAT9125_PID1			0x00
+#define PAT9125_PID2			0x01
+#define PAT9125_MOTION			0x02
+#define PAT9125_DELTA_XL		0x03
+#define PAT9125_DELTA_YL		0x04
+#define PAT9125_MODE			0x05
+#define PAT9125_CONFIG			0x06
+#define PAT9125_WP				0x09
+#define PAT9125_SLEEP1			0x0a
+#define PAT9125_SLEEP2			0x0b
+#define PAT9125_RES_X			0x0d
+#define PAT9125_RES_Y			0x0e
+#define PAT9125_DELTA_XYH		0x12
+#define PAT9125_SHUTTER			0x14
+#define PAT9125_FRAME			0x17
+#define PAT9125_ORIENTATION		0x19
+
+extern unsigned char ucPID1;
+extern unsigned char ucPID2;
+
+extern int pat9125_x;
+extern int pat9125_y;
+
+int pat9125_init(unsigned char xres, unsigned char yres);
+int pat9125_update();
+
+unsigned char pat9125_rd_reg(unsigned char addr);
+void pat9125_wr_reg(unsigned char addr, unsigned char data);
+
+
+#endif //PAT9125_H
diff --git a/Firmware/swspi.cpp b/Firmware/swspi.cpp
new file mode 100755
index 000000000..ca681e909
--- /dev/null
+++ b/Firmware/swspi.cpp
@@ -0,0 +1,107 @@
+#include "swspi.h"
+
+
+#ifdef SWSPI_RPI
+	#include <bcm2835.h>
+	#define GPIO_INP(gpio) bcm2835_gpio_fsel(gpio, BCM2835_GPIO_FSEL_INPT)
+	#define GPIO_OUT(gpio) bcm2835_gpio_fsel(gpio, BCM2835_GPIO_FSEL_OUTP)
+	#define GPIO_SET(gpio) bcm2835_gpio_write(gpio, HIGH)
+	#define GPIO_CLR(gpio) bcm2835_gpio_write(gpio, LOW)
+	#define GPIO_GET(gpio) (bcm2835_gpio_lev(gpio) != LOW)
+	#define DELAY(delay) usleep(delay)
+#endif //SWSPI_RPI
+
+#ifdef SWSPI_AVR
+	#include "Arduino.h"
+	#define GPIO_INP(gpio) pinMode(gpio, INPUT)
+	#define GPIO_OUT(gpio) pinMode(gpio, OUTPUT)
+	#define GPIO_SET(gpio) digitalWrite(gpio, HIGH)
+	#define GPIO_CLR(gpio) digitalWrite(gpio, LOW)
+	#define GPIO_GET(gpio) (digitalRead(gpio) != LOW)
+	#define DELAY(delay) delayMicroseconds(delay)
+#endif //SWSPI_AVR
+
+#if (SWSPI_POL != 0)
+	#define SWSPI_SCK_UP GPIO_CLR(SWSPI_SCK)
+	#define SWSPI_SCK_DN GPIO_SET(SWSPI_SCK)
+#else
+	#define SWSPI_SCK_UP GPIO_SET(SWSPI_SCK)
+	#define SWSPI_SCK_DN GPIO_CLR(SWSPI_SCK)
+#endif
+
+
+void swspi_init()
+{
+	GPIO_INP(SWSPI_MISO);
+	GPIO_OUT(SWSPI_MOSI);
+	GPIO_OUT(SWSPI_SCK);
+	GPIO_OUT(SWSPI_CS);
+	GPIO_CLR(SWSPI_MOSI);
+	SWSPI_SCK_DN;
+	GPIO_SET(SWSPI_CS);
+}
+
+#if (SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_tx(unsigned char tx)
+{
+	GPIO_OUT(SWSPI_MOSI);
+	unsigned char i = 0; for (; i < 8; i++)
+	{
+		if (tx & 0x80) GPIO_SET(SWSPI_MOSI);
+		else GPIO_CLR(SWSPI_MOSI);
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_UP;
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_DN;
+		tx <<= 1;
+	}
+}
+
+unsigned char swspi_rx()
+{
+	GPIO_INP(SWSPI_MISO);
+	unsigned char rx = 0;
+	unsigned char i = 0; for (; i < 8; i++)
+	{
+		rx <<= 1;
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_UP;
+	    DELAY(SWSPI_DEL);
+		rx |= GPIO_GET(SWSPI_MISO)?1:0;
+		SWSPI_SCK_DN;
+	}
+	return rx;
+}
+
+#else //(SWSPI_MOSI == SWSPI_MISO)
+
+unsigned char swspi_txrx(unsigned char tx)
+{
+	unsigned char rx = 0;
+	unsigned char i = 0; for (; i < 8; i++)
+	{
+		rx <<= 1;
+		if (tx & 0x80) GPIO_SET(SWSPI_MOSI);
+		else GPIO_CLR(SWSPI_MOSI);
+	    DELAY(SWSPI_DEL);
+		SWSPI_SCK_UP;
+	    DELAY(SWSPI_DEL);
+		rx |= GPIO_GET(SWSPI_MISO)?1:0;
+		SWSPI_SCK_DN;
+		tx <<= 1;
+	}
+	return rx;
+}
+
+#endif //(SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_start()
+{
+	GPIO_CLR(SWSPI_CS);
+}
+
+void swspi_stop()
+{
+	GPIO_SET(SWSPI_CS);
+}
diff --git a/Firmware/swspi.h b/Firmware/swspi.h
new file mode 100755
index 000000000..f601acd54
--- /dev/null
+++ b/Firmware/swspi.h
@@ -0,0 +1,48 @@
+#ifndef SWSPI_H
+#define SWSPI_H
+
+
+//#define SWSPI_RPI
+#define SWSPI_AVR
+
+#ifdef SWSPI_RPI
+//#define SWSPI_MISO	9
+#define SWSPI_MISO	10
+#define SWSPI_MOSI	10
+#define SWSPI_SCK	11
+#define SWSPI_CS	7
+#endif //SWSPI_RPI
+
+#ifdef SWSPI_AVR
+#define SWSPI_MISO	16
+#define SWSPI_MOSI	16
+#define SWSPI_SCK	17
+#define SWSPI_CS	20
+#endif //SWSPI_AVR
+
+#define SWSPI_POL	1 //polarity
+#define SWSPI_PHA	0 //phase
+#define SWSPI_DOR	0 //data order
+#define SWSPI_DEL	100 //delay
+
+
+void swspi_init();
+
+#if (SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_tx(unsigned char tx);
+unsigned char swspi_rx();
+
+#else //(SWSPI_MOSI == SWSPI_MISO)
+
+#define swspi_tx swspi_txrx
+#define swspi_rx swspi_txrx
+unsigned char swspi_txrx(unsigned char tx);
+
+#endif //(SWSPI_MOSI == SWSPI_MISO)
+
+void swspi_start();
+void swspi_stop();
+
+
+#endif //SWSPI_H
diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index aa22fe265..316d29f11 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -14,6 +14,7 @@
 //#include "Configuration.h"
 
 #include "SdFatUtil.h"
+#include "pat9125.h"
 
 #define _STRINGIFY(s) #s
 
@@ -503,6 +504,11 @@ static void lcd_status_screen()
 	  lcd.setCursor(0, 3);
 	  lcd_printPGM(MSG_PRINTER_DISCONNECTED);
   }
+    
+    lcd.setCursor(0, 3);
+    lcd_implementation_print(pat9125_x);
+    lcd.setCursor(10, 3);
+    lcd_implementation_print(pat9125_y);
 
 }