diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index b36241253..9deb51055 100755
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -90,18 +90,13 @@
 #include "la10compat.h"
 #endif
 
-#ifdef SWSPI
-#include "swspi.h"
-#endif //SWSPI
-
 #include "spi.h"
 
-#ifdef SWI2C
-#include "swi2c.h"
-#endif //SWI2C
-
 #ifdef FILAMENT_SENSOR
 #include "fsensor.h"
+#ifdef IR_SENSOR
+#include "pat9125.h" // for pat9125_probe
+#endif
 #endif //FILAMENT_SENSOR
 
 #ifdef TMC2130
@@ -922,9 +917,7 @@ static void check_if_fw_is_on_right_printer(){
 #ifdef FILAMENT_SENSOR
   if((PRINTER_TYPE == PRINTER_MK3) || (PRINTER_TYPE == PRINTER_MK3S)){
     #ifdef IR_SENSOR
-    swi2c_init();
-    const uint8_t pat9125_detected = swi2c_readByte_A8(PAT9125_I2C_ADDR,0x00,NULL);
-      if (pat9125_detected){
+      if (pat9125_probe()){
         lcd_show_fullscreen_message_and_wait_P(_i("MK3S firmware detected on MK3 printer"));}////c=20 r=3
     #endif //IR_SENSOR
 
diff --git a/Firmware/Sd2PinMap.h b/Firmware/Sd2PinMap.h
index 8a608731e..da50958f0 100644
--- a/Firmware/Sd2PinMap.h
+++ b/Firmware/Sd2PinMap.h
@@ -37,10 +37,6 @@ struct pin_map_t {
 || defined(__AVR_ATmega2560__)
 // Mega
 
-// Two Wire (aka I2C) ports
-uint8_t const SDA_PIN = 20;  // D1
-uint8_t const SCL_PIN = 21;  // D0
-
 #undef MOSI_PIN
 #undef MISO_PIN
 // SPI port
@@ -365,4 +361,4 @@ static inline __attribute__((always_inline))
 #endif  // Sd2PinMap_h
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/Firmware/config.h b/Firmware/config.h
index 1a0a97008..922a53989 100644
--- a/Firmware/config.h
+++ b/Firmware/config.h
@@ -23,7 +23,6 @@
 #define ADC_CALLBACK      adc_ready //callback function ()
 
 //SWI2C configuration
-#define SWI2C
 //#define SWI2C_SDA         20 //SDA on P3
 //#define SWI2C_SCL         21 //SCL on P3
 #define SWI2C_A8
@@ -31,7 +30,13 @@
 #define SWI2C_TMO         2048 //2048 cycles timeout
 
 //PAT9125 configuration
-#define PAT9125_SWI2C
+//#define PAT9125_SWSPI // software SPI mode (incomplete)
+#ifdef SWI2C_SCL
+#define PAT9125_SWI2C   // software I2C mode
+#else
+#define PAT9125_I2C     // hardware I2C mode
+#endif
+
 #define PAT9125_I2C_ADDR  0x75  //ID=LO
 //#define PAT9125_I2C_ADDR  0x79  //ID=HI
 //#define PAT9125_I2C_ADDR  0x73  //ID=NC
diff --git a/Firmware/fastio.h b/Firmware/fastio.h
index 19c278d4a..855c000e2 100644
--- a/Firmware/fastio.h
+++ b/Firmware/fastio.h
@@ -9,6 +9,7 @@
 #include <avr/io.h>
 #include "macros.h"
 
+
 /*
   magic I/O routines
   now you can simply SET_OUTPUT(STEP); WRITE(STEP, 1); WRITE(STEP, 0);
diff --git a/Firmware/pat9125.c b/Firmware/pat9125.c
index de4693d29..58308a9a7 100644
--- a/Firmware/pat9125.c
+++ b/Firmware/pat9125.c
@@ -26,12 +26,15 @@
 #define PAT9125_BANK_SELECTION	0x7f
 
 
-#ifdef PAT9125_SWSPI
+#if defined(PAT9125_SWSPI)
 #include "swspi.h"
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
+#elif defined(PAT9125_SWI2C)
 #include "swi2c.h"
-#endif //PAT9125_SWI2C
+#elif defined(PAT9125_I2C)
+#include "twi.h"
+#else
+#error unknown PAT9125 communication method
+#endif
 
 
 uint8_t pat9125_PID1 = 0;
@@ -103,14 +106,31 @@ extern FILE _uartout;
 #define uartout (&_uartout)
 
 
+uint8_t pat9125_probe()
+{
+#if defined(PAT9125_SWSPI)
+    swspi_init();
+  #error not implemented
+#elif defined(PAT9125_SWI2C)
+    swi2c_init();
+    return swi2c_readByte_A8(PAT9125_I2C_ADDR,0x00,NULL);
+#elif defined(PAT9125_I2C)
+    twi_init();
+  #ifdef IR_SENSOR
+    // NOTE: this is called from the MK3S variant, so it should be kept minimal
+    uint8_t data;
+    return (twi_r8(PAT9125_I2C_ADDR,PAT9125_PID1,&data) == 0);
+  #else
+    return (pat9125_rd_reg(PAT9125_PID1) != 0);
+  #endif
+#endif
+}
+
 uint8_t pat9125_init(void)
 {
-#ifdef PAT9125_SWSPI
-	swspi_init();
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
-	swi2c_init();
-#endif //PAT9125_SWI2C
+    if (!pat9125_probe())
+        return 0;
+
 	// Verify that the sensor responds with its correct product ID.
 	pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
 	pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
@@ -234,39 +254,46 @@ uint8_t pat9125_update_bs(void)
 uint8_t pat9125_rd_reg(uint8_t addr)
 {
 	uint8_t data = 0;
-#ifdef PAT9125_SWSPI
+#if defined(PAT9125_SWSPI)
 	swspi_start();
 	swspi_tx(addr & 0x7f);
 	data = swspi_rx();
 	swspi_stop();
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
+#elif defined(PAT9125_SWI2C)
 	if (!swi2c_readByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
-	{
-		pat9125_PID1 = 0xff;
-		pat9125_PID2 = 0xff;
-		return 0;
-	}
-#endif //PAT9125_SWI2C
+        goto error;
+#elif defined(PAT9125_I2C)
+	if (twi_r8(PAT9125_I2C_ADDR,addr,&data))
+        goto error;
+#endif
 	return data;
+
+ error:
+    pat9125_PID1 = 0xff;
+    pat9125_PID2 = 0xff;
+    return 0;
 }
 
 void pat9125_wr_reg(uint8_t addr, uint8_t data)
 {
-#ifdef PAT9125_SWSPI
+#if defined(PAT9125_SWSPI)
 	swspi_start();
 	swspi_tx(addr | 0x80);
 	swspi_tx(data);
 	swspi_stop();
-#endif //PAT9125_SWSPI
-#ifdef PAT9125_SWI2C
+#elif defined(PAT9125_SWI2C)
 	if (!swi2c_writeByte_A8(PAT9125_I2C_ADDR, addr, &data)) //NO ACK error
-	{
-		pat9125_PID1 = 0xff;
-		pat9125_PID2 = 0xff;
-		return;
-	}
-#endif //PAT9125_SWI2C
+        goto error;
+#elif defined(PAT9125_I2C)
+	if (twi_w8(PAT9125_I2C_ADDR,addr,data))
+        goto error;
+#endif
+    return;
+
+ error:
+    pat9125_PID1 = 0xff;
+    pat9125_PID2 = 0xff;
+    return;
 }
 
 uint8_t pat9125_wr_reg_verify(uint8_t addr, uint8_t data)
diff --git a/Firmware/pat9125.h b/Firmware/pat9125.h
index 12f7fe949..1fb2539f4 100755
--- a/Firmware/pat9125.h
+++ b/Firmware/pat9125.h
@@ -18,6 +18,7 @@ extern int16_t pat9125_y;
 extern uint8_t pat9125_b;
 extern uint8_t pat9125_s;
 
+extern uint8_t pat9125_probe(void);     // Return non-zero if PAT9125 can be trivially detected
 extern uint8_t pat9125_init(void);
 extern uint8_t pat9125_update(void);    // update all sensor data
 extern uint8_t pat9125_update_y(void);  // update _y only
diff --git a/Firmware/pins.h b/Firmware/pins.h
index 5d3b4f832..1c20a001a 100644
--- a/Firmware/pins.h
+++ b/Firmware/pins.h
@@ -25,6 +25,11 @@
 #error Unknown MOTHERBOARD value in configuration.h
 #endif
 
+#if !defined(SDA_PIN) && (defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__))
+#define SDA_PIN 20
+#define SCL_PIN 21
+#endif
+
 //List of pins which to ignore when asked to change by gcode, 0 and 1 are RX and TX, do not mess with those!
 #define _E0_PINS E0_STEP_PIN, E0_DIR_PIN, E0_ENABLE_PIN, HEATER_0_PIN,
 #if EXTRUDERS > 1
diff --git a/Firmware/pins_Einsy_1_0.h b/Firmware/pins_Einsy_1_0.h
index 88713e6ac..70660bca8 100755
--- a/Firmware/pins_Einsy_1_0.h
+++ b/Firmware/pins_Einsy_1_0.h
@@ -18,12 +18,6 @@
 #define W25X20CL                 // external 256kB flash
 #define BOOTAPP                  // bootloader support
 
-
-#define SWI2C_SDA      20 //SDA on P3
-#define SWI2C_SCL      21 //SCL on P3
-
-
-
 #define X_TMC2130_CS           41
 #define X_TMC2130_DIAG         64 // !!! changed from 40 (EINY03)
 #define X_STEP_PIN             37
diff --git a/Firmware/pins_Rambo_1_3.h b/Firmware/pins_Rambo_1_3.h
index 538fb4f35..522ad28f8 100644
--- a/Firmware/pins_Rambo_1_3.h
+++ b/Firmware/pins_Rambo_1_3.h
@@ -11,9 +11,6 @@
 
 #define PINDA_THERMISTOR
 
-#define SWI2C_SDA      20 //SDA on P3
-#define SWI2C_SCL      21 //SCL on P3
-
 #ifdef MICROMETER_LOGGING
 #define D_DATACLOCK		24	//Y_MAX (green)
 #define D_DATA			30	//X_MAX (blue)
diff --git a/Firmware/swi2c.c b/Firmware/swi2c.c
index 02cca2208..4b3119aa4 100644
--- a/Firmware/swi2c.c
+++ b/Firmware/swi2c.c
@@ -8,6 +8,7 @@
 #include "pins.h"
 #include "fastio.h"
 
+#ifdef SWI2C_SCL
 
 #define SWI2C_RMSK   0x01 //read mask (bit0 = 1)
 #define SWI2C_WMSK   0x00 //write mask (bit0 = 0)
@@ -187,3 +188,5 @@ uint8_t swi2c_writeByte_A16(uint8_t dev_addr, unsigned short addr, uint8_t* pbyt
 }
 
 #endif //SWI2C_A16
+
+#endif //SWI2C_SCL
diff --git a/Firmware/tone04.c b/Firmware/tone04.c
index 42b19682f..55a0a0e9c 100644
--- a/Firmware/tone04.c
+++ b/Firmware/tone04.c
@@ -7,8 +7,6 @@
 
 #ifdef SYSTEM_TIMER_2
 
-#include <avr/io.h>
-#include <avr/interrupt.h>
 #include "pins.h"
 #include "fastio.h"
 #include "macros.h"
diff --git a/Firmware/twi.c b/Firmware/twi.c
new file mode 100644
index 000000000..e8c9a378e
--- /dev/null
+++ b/Firmware/twi.c
@@ -0,0 +1,137 @@
+/*
+  twi.c - Stripped-down TWI/I2C library
+  Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+
+  Modified 2012 by Todd Krein (todd@krein.org) to implement repeated starts
+*/
+
+
+#include <math.h>
+#include "config.h"
+#include "fastio.h"
+#include "twi.h"
+
+
+void twi_init(void)
+{
+  // activate internal pullups for twi.
+  WRITE(SDA_PIN, 1);
+  WRITE(SCL_PIN, 1);
+
+  // initialize twi prescaler and bit rate
+  TWSR &= ~(_BV(TWPS0) | _BV(TWPS1));
+  TWBR = ((F_CPU / TWI_FREQ) - 16) / 2;
+
+  /* twi bit rate formula from atmega128 manual pg 204
+  SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR))
+  note: TWBR should be 10 or higher for master mode
+  It is 72 for a 16mhz Wiring board with 100kHz TWI */
+}
+
+void twi_disable(void)
+{
+  // deactivate internal pullups for twi.
+  WRITE(SDA_PIN, 0);
+  WRITE(SCL_PIN, 0);
+}
+
+
+static void twi_stop()
+{
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTO);
+}
+
+
+static uint8_t twi_wait(uint8_t status)
+{
+  while(!(TWCR & _BV(TWINT)));
+  if(TW_STATUS != status)
+  {
+      twi_stop();
+      return 1;
+  }
+  return 0;
+}
+
+
+static uint8_t twi_start(uint8_t address, uint8_t reg)
+{
+  // send start condition
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTA);
+  if(twi_wait(TW_START))
+      return 1;
+
+  // send address
+  TWDR = TW_WRITE | (address << 1);
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MT_SLA_ACK))
+      return 2;
+
+  // send register
+  TWDR = reg;
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MT_DATA_ACK))
+      return 3;
+
+  return 0;
+}
+
+
+uint8_t twi_r8(uint8_t address, uint8_t reg, uint8_t* data)
+{
+  if(twi_start(address, reg))
+      return 1;
+
+  // repeat start
+  TWCR = _BV(TWEN) | _BV(TWINT) | _BV(TWSTA);
+  if(twi_wait(TW_REP_START))
+      return 2;
+
+  // start receiving
+  TWDR = TW_READ | (address << 1);
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MR_SLA_ACK))
+      return 3;
+
+  // receive data
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MR_DATA_NACK))
+      return 4;
+
+  *data = TWDR;
+
+  // send stop
+  twi_stop();
+  return 0;
+}
+
+
+uint8_t twi_w8(uint8_t address, uint8_t reg, uint8_t data)
+{
+  if(twi_start(address, reg))
+      return 1;
+
+  // send data
+  TWDR = data;
+  TWCR = _BV(TWEN) | _BV(TWINT);
+  if(twi_wait(TW_MT_DATA_ACK))
+      return 2;
+
+  // send stop
+  twi_stop();
+  return 0;
+}
diff --git a/Firmware/twi.h b/Firmware/twi.h
new file mode 100644
index 000000000..bdb617fcb
--- /dev/null
+++ b/Firmware/twi.h
@@ -0,0 +1,63 @@
+/*
+  twi.h - Stripped-down TWI/I2C library
+  Copyright (c) 2006 Nicholas Zambetti.  All right reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#pragma once
+
+#include <inttypes.h>
+#include <compat/twi.h>
+
+#ifndef TWI_FREQ
+#define TWI_FREQ 400000L
+#endif
+
+/*
+ * Function twi_init
+ * Desc     readys twi pins and sets twi bitrate
+ * Input    none
+ * Output   none
+ */
+void twi_init(void);
+
+/*
+ * Function twi_disable
+ * Desc     disables twi pins
+ * Input    none
+ * Output   none
+ */
+void twi_disable(void);
+
+/*
+ * Function twi_r8
+ * Desc     read a single byte from a device
+ * Input    address: 7bit i2c device address
+ *          reg: register address
+ *          data: pointer to byte for result
+ * Output   0 on success
+ */
+uint8_t twi_r8(uint8_t address, uint8_t reg, uint8_t* data);
+
+/*
+ * Function twi_w8
+ * Desc     write a single byte from a device
+ * Input    address: 7bit i2c device address
+ *          reg: register address
+ *          data: byte to write
+ * Output   0 on success
+ */
+uint8_t twi_w8(uint8_t address, uint8_t reg, uint8_t data);