diff --git a/configure b/configure
index c47b186..7fee365 100755
--- a/configure
+++ b/configure
@@ -14,7 +14,6 @@ Help:
 Building options:
     --soc=[BCM2835|BCM2836|AM33XX|A10|A13|A20|H3]
                                 SoC type to be used. [configure autodetected]
-    --platform-type=<TYPE>      Type of platform
     --cpu-flags=<CPUFLAGS>      CPU defining/optimizing flags to be used. [configure autodetected]
     --extra-cflags=<CFLAGS>     Extra C flags passed to C compilation. []
     --extra-cxxflags=<CXXFLAGS> Extra C++ flags passed to C++ compilation. []
@@ -109,7 +108,6 @@ function detect_machine {
     fi
     local soc="unknown"
     local tp="unknown"
-    local rev="unknown"
 
     if [ -z "$cpu" ]; then
         cpu="unknown"
@@ -120,13 +118,12 @@ function detect_machine {
         soc="BCM2835"
         if [[ $machine == "Raspberry"* ]]; then
             tp="RPi"
-            rev=($(detect_rpi_revision))
         fi
         ;;
     BCM2709)
         soc="BCM2836"
         if [[ $machine == "Raspberry"* ]]; then
-            rev=($(detect_rpi_revision))
+            local rev=($(detect_rpi_revision))
             if [[ $rev == "a02082" || $rev == "a22082" ]]; then
                 tp="RPi3"
             else
@@ -187,7 +184,7 @@ function detect_machine {
     *)
         soc="unknown"
     esac
-    echo "${soc} ${tp} ${cpu} ${rev}"
+    echo "${soc} ${tp} ${cpu}"
 }
 
 function gcc_cpu_flags {
@@ -409,12 +406,7 @@ if [ -z "${SOC}" ]; then
     SOC=${info[0]}
     TYPE=${info[1]}
     CPU=${info[2]}
-    REV=${info[3]}
-    echo "[OK] machine detected: SoC=${SOC}, Type=${TYPE}, CPU=${CPU}, REV=${REV}."
-fi
-
-if [ "${PT}" ]; then
-    TYPE=${PT}
+    echo "[OK] machine detected: SoC=${SOC}, Type=${TYPE}, CPU=${CPU}."
 fi
 
 if [ -z "${CPUFLAGS}" ]; then
@@ -425,10 +417,6 @@ if [[ $SOC == "BCM2835" || $SOC == "BCM2836" ]]; then
     CPPFLAGS="-DLINUX_ARCH_RASPBERRYPI $CPPFLAGS"
 fi
 
-if [[ $TYPE == "RPi2" || $TYPE == "RPi3" || $REV == "0010" ]]; then
-    CPPFLAGS="-D__RPI_BPLUS $CPPFLAGS"
-fi
-
 if [[ ${debug} == "enable" ]]; then
     CPPFLAGS="-DMY_DEBUG $CPPFLAGS"
 fi
diff --git a/core/MyHwRPi.h b/core/MyHwRPi.h
index 2252a22..a4f3c64 100644
--- a/core/MyHwRPi.h
+++ b/core/MyHwRPi.h
@@ -20,6 +20,7 @@
 #ifndef MyHwRPi_h
 #define MyHwRPi_h
 
+#include <stdint.h>
 #include <stdlib.h>
 #include "MyHwLinuxGeneric.h"
 #include "log.h"
diff --git a/drivers/RPi/cpuinfo.c b/drivers/RPi/cpuinfo.c
new file mode 100644
index 0000000..5fa48dc
--- /dev/null
+++ b/drivers/RPi/cpuinfo.c
@@ -0,0 +1,216 @@
+/*
+Copyright (c) 2012-2016 Ben Croston
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#include "cpuinfo.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+int get_rpi_info(rpi_info *info)
+{
+   FILE *fp;
+   char buffer[1024];
+   char hardware[1024];
+   char revision[1024];
+   char *rev;
+   int found = 0;
+   int len;
+
+   if ((fp = fopen("/proc/cpuinfo", "r")) == NULL)
+      return -1;
+   while(!feof(fp)) {
+      fgets(buffer, sizeof(buffer), fp);
+      sscanf(buffer, "Hardware	: %s", hardware);
+      if (strcmp(hardware, "BCM2708") == 0 ||
+          strcmp(hardware, "BCM2709") == 0 ||
+          strcmp(hardware, "BCM2835") == 0 ||
+          strcmp(hardware, "BCM2836") == 0 ||
+          strcmp(hardware, "BCM2837") == 0 ) {
+         found = 1;
+      }
+      sscanf(buffer, "Revision	: %s", revision);
+   }
+   fclose(fp);
+
+   if (!found)
+      return -1;
+
+   if ((len = strlen(revision)) == 0)
+      return -1;
+
+   if (len >= 6 && strtol((char[]){revision[len-6],0}, NULL, 16) & 8) {
+      // new scheme
+      //info->rev = revision[len-1]-'0';
+      strcpy(info->revision, revision);
+      switch (revision[len-2]) {
+		  case '0': info->type = "Model A"; info->p1_revision = 2; break;
+		  case '1': info->type = "Model B"; info->p1_revision = 2; break;
+		  case '2': info->type = "Model A+"; info->p1_revision = 3; break;
+		  case '3': info->type = "Model B+"; info->p1_revision = 3; break;
+		  case '4': info->type = "Pi 2 Model B"; info->p1_revision = 3; break;
+		  case '5': info->type = "Alpha"; info->p1_revision = 3; break;
+		  case '6': info->type = "Compute"; info->p1_revision = 0; break;
+                  case '8': info->type = "Pi 3 Model B"; info->p1_revision = 3; break;
+                  case '9': info->type = "Zero"; info->p1_revision = 3; break;
+		  default : info->type = "Unknown"; info->p1_revision = 3; break;
+	  }
+	  switch (revision[len-4]) {
+		  case '0': info->processor = "BCM2835"; break;
+		  case '1': info->processor = "BCM2836"; break;
+                  case '2': info->processor = "BCM2837"; break;
+		  default : info->processor = "Unknown"; break;
+	  }
+	  switch (revision[len-5]) {
+		  case '0': info->manufacturer = "Sony"; break;
+		  case '1': info->manufacturer = "Egoman"; break;
+		  case '2': info->manufacturer = "Embest"; break;
+		  case '4': info->manufacturer = "Embest"; break;
+		  default : info->manufacturer = "Unknown"; break;
+	  }
+	  switch (strtol((char[]){revision[len-6],0}, NULL, 16) & 7) {
+		  case 0: info->ram = "256M"; break;
+		  case 1: info->ram = "512M"; break;
+		  case 2: info->ram = "1024M"; break;
+		  default: info->ram = "Unknown"; break;
+      }
+   } else {
+      // old scheme
+      info->ram = "Unknown";
+      info->manufacturer = "Unknown";
+      info->processor = "Unknown";
+      info->type = "Unknown";
+      strcpy(info->revision, revision);
+
+      // get last four characters (ignore preceeding 1000 for overvolt)
+      if (len > 4)
+         rev = (char *)&revision+len-4;
+      else
+         rev = revision;
+
+      if ((strcmp(rev, "0002") == 0) ||
+          (strcmp(rev, "0003") == 0)) {
+         info->type = "Model B";
+         info->p1_revision = 1;
+         info->ram = "256M";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0004") == 0) {
+         info->type = "Model B";
+         info->p1_revision = 2;
+         info->ram = "256M";
+         info->manufacturer = "Sony";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0005") == 0) {
+         info->type = "Model B";
+         info->p1_revision = 2;
+         info->ram = "256M";
+         info->manufacturer = "Qisda";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0006") == 0) {
+         info->type = "Model B";
+         info->p1_revision = 2;
+         info->ram = "256M";
+         info->manufacturer = "Egoman";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0007") == 0) {
+         info->type = "Model A";
+         info->p1_revision = 2;
+         info->ram = "256M";
+         info->manufacturer = "Egoman";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0008") == 0) {
+         info->type = "Model A";
+         info->p1_revision = 2;
+         info->ram = "256M";
+         info->manufacturer = "Sony";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0009") == 0) {
+         info->type = "Model A";
+         info->p1_revision = 2;
+         info->ram = "256M";
+         info->manufacturer = "Qisda";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "000d") == 0) {
+         info->type = "Model B";
+         info->p1_revision = 2;
+         info->ram = "512M";
+         info->manufacturer = "Egoman";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "000e") == 0) {
+         info->type = "Model B";
+         info->p1_revision = 2;
+         info->ram = "512M";
+         info->manufacturer = "Sony";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "000f") == 0) {
+         info->type = "Model B";
+         info->p1_revision = 2;
+         info->ram = "512M";
+         info->manufacturer = "Qisda";
+         info->processor = "BCM2835";
+      } else if ((strcmp(rev, "0011") == 0) ||
+                 (strcmp(rev, "0014") == 0)) {
+         info->type = "Compute Module";
+         info->p1_revision = 0;
+         info->ram = "512M";
+         info->processor = "BCM2835";
+      } else if (strcmp(rev, "0012") == 0) {
+         info->type = "Model A+";
+         info->p1_revision = 3;
+         info->ram = "256M";
+         info->processor = "BCM2835";
+      } else if ((strcmp(rev, "0010") == 0) ||
+                 (strcmp(rev, "0013") == 0)) {
+         info->type = "Model B+";
+         info->p1_revision = 3;
+         info->ram = "512M";
+         info->processor = "BCM2835";
+      } else {  // don't know - assume revision 3 p1 connector
+         info->p1_revision = 3;
+      }
+   }
+   return 0;
+}
+
+/*
+
+32 bits
+NEW                   23: will be 1 for the new scheme, 0 for the old scheme
+MEMSIZE             20: 0=256M 1=512M 2=1G
+MANUFACTURER  16: 0=SONY 1=EGOMAN
+PROCESSOR         12: 0=2835 1=2836
+TYPE                   04: 0=MODELA 1=MODELB 2=MODELA+ 3=MODELB+ 4=Pi2 MODEL B 5=ALPHA 6=CM
+REV                     00: 0=REV0 1=REV1 2=REV2
+
+pi2 = 1<<23 | 2<<20 | 1<<12 | 4<<4 = 0xa01040
+
+--------------------
+
+SRRR MMMM PPPP TTTT TTTT VVVV
+
+S scheme (0=old, 1=new)
+R RAM (0=256, 1=512, 2=1024)
+M manufacturer (0='SONY',1='EGOMAN',2='EMBEST',3='UNKNOWN',4='EMBEST')
+P processor (0=2835, 1=2836 2=2837)
+T type (0='A', 1='B', 2='A+', 3='B+', 4='Pi 2 B', 5='Alpha', 6='Compute Module')
+V revision (0-15)
+
+*/
diff --git a/drivers/RPi/cpuinfo.h b/drivers/RPi/cpuinfo.h
new file mode 100644
index 0000000..b3c43fe
--- /dev/null
+++ b/drivers/RPi/cpuinfo.h
@@ -0,0 +1,48 @@
+/*
+Copyright (c) 2012-2015 Ben Croston
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+#ifndef CPUINFO_H
+#define CPUINFO_H
+#if !DOXYGEN
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct
+{
+   int p1_revision;
+   char *ram;
+   char *manufacturer;
+   char *processor;
+   char *type;
+   char revision[1024];
+} rpi_info;
+
+int get_rpi_info(rpi_info *info);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+#endif /* CPUINFO_H */
diff --git a/drivers/RPi/rpi_util.cpp b/drivers/RPi/rpi_util.cpp
index 0888554..f3ea8fe 100644
--- a/drivers/RPi/rpi_util.cpp
+++ b/drivers/RPi/rpi_util.cpp
@@ -19,6 +19,7 @@
  * Based on wiringPi Copyright (c) 2012 Gordon Henderson.
  */
 
+#include "rpi_util.h"
 #include <pthread.h>
 #include <stdlib.h>
 #include <sys/ioctl.h>
@@ -31,9 +32,9 @@
 #include <fcntl.h>
 #include <stropts.h>
 #include <errno.h>
-#include "rpi_util.h"
 #include "SPI.h"
 #include "log.h"
+#include "cpuinfo.h"
 
 extern "C" {
 	int piHiPri(const int pri);
@@ -44,6 +45,9 @@ struct ThreadArgs {
 	int gpioPin;
 };
 
+static const int *pin_to_gpio = 0;
+static rpi_info rpiinfo;
+
 volatile bool interruptsEnabled = true;
 static pthread_mutex_t intMutex = PTHREAD_MUTEX_INITIALIZER;
 
@@ -59,66 +63,37 @@ static int sysFds[64] =
 	-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
 };
 
-#ifdef __RPI_BPLUS
-static uint8_t physToGpio[64] =
+int get_gpio_number(uint8_t physPin, uint8_t *gpio)
 {
-	255,		// 0
-	255, 255,	// 1, 2
-	  2, 255,
-	  3, 255,
-	  4,  14,
-	255,  15,
-	 17,  18,
-	 27, 255,
-	 22,  23,
-	255,  24,
-	 10, 255,
-	  9,  25,
-	 11,   8,
-	255,   7,	// 25, 26
-// B+
-	  0,   1,
-	  5, 255,
-	  6,  12,
-	 13, 255,
-	 19,  16,
-	 26,  20,
-	255,  21,
-// the P5 connector on the Rev 2 boards:
-	255, 255,
-	255, 255,
-	255, 255,
-	255, 255,
-	255, 255,
-	 28,  29,
-	 30,  31,
-	255, 255,
-	255, 255,
-	255, 255,
-	255, 255,
-};
-#else
-static uint8_t physToGpio[64] =
-{
-	255,		// 0
-	255, 255,	// 1, 2
-	  0, 255,
-	  1, 255,
-	  4,  14,
-	255,  15,
-	 17,  18,
-	 21, 255,
-	 22,  23,
-	255,  24,
-	 10, 255,
-	  9,  25,
-	 11,   8,
-	255,   7,	// 25, 26
-														   255, 255, 255, 255, 255,	// ... 31
-	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,	// ... 47
-	255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,	// ... 63
-};
-#endif
+	if (pin_to_gpio == 0) {
+		// detect board revision and set up accordingly
+		if (get_rpi_info(&rpiinfo)) {
+			logError("This module can only be run on a Raspberry Pi!\n");
+			exit(1);
+		}
+
+		if (rpiinfo.p1_revision == 1) {
+			pin_to_gpio = &pin_to_gpio_rev1[0];
+		} else if (rpiinfo.p1_revision == 2) {
+			pin_to_gpio = &pin_to_gpio_rev2[0];
+		} else { // assume model B+ or A+ or 2B
+			pin_to_gpio = &pin_to_gpio_rev3[0];
+		}
+	}
+
+	if ((rpiinfo.p1_revision != 3 && physPin > 26)
+		|| (rpiinfo.p1_revision == 3 && physPin > 40)) {
+		return -1;
+	}
+
+	if (*(pin_to_gpio+physPin) == -1) {
+		return -1;
+	} else {
+		*gpio = *(pin_to_gpio+physPin);
+	}
+
+	return 0;
+}
 
 void *interruptHandler(void *args) {
 	int fd, ret;
@@ -168,11 +143,13 @@ void *interruptHandler(void *args) {
 }
 
 void rpi_util::pinMode(uint8_t physPin, uint8_t mode) {
-	uint8_t gpioPin = (physPin > 63)? 255 : physToGpio[physPin];
-	if (gpioPin == 255) {
+	uint8_t gpioPin;
+
+	if (get_gpio_number(physPin, &gpioPin)) {
 		logError("pinMode: invalid pin: %d\n", physPin);
 		return;
 	}
+
 	// Check if SPI is in use and target pin is related to SPI
 	if (SPIClass::is_initialized() && gpioPin >= RPI_GPIO_P1_26 && gpioPin <= RPI_GPIO_P1_23) {
 		return;
@@ -182,11 +159,13 @@ void rpi_util::pinMode(uint8_t physPin, uint8_t mode) {
 }
 
 void rpi_util::digitalWrite(uint8_t physPin, uint8_t value) {
-	uint8_t gpioPin = (physPin > 63)? 255 : physToGpio[physPin];
-	if (gpioPin == 255) {
+	uint8_t gpioPin;
+
+	if (get_gpio_number(physPin, &gpioPin)) {
 		logError("digitalWrite: invalid pin: %d\n", physPin);
 		return;
 	}
+
 	// Check if SPI is in use and target pin is related to SPI
 	if (SPIClass::is_initialized() && gpioPin >= RPI_GPIO_P1_26 && gpioPin <= RPI_GPIO_P1_23) {
 		if (value == LOW && (gpioPin == RPI_GPIO_P1_24 || gpioPin == RPI_GPIO_P1_26)) {
@@ -200,11 +179,13 @@ void rpi_util::digitalWrite(uint8_t physPin, uint8_t value) {
 }
 
 uint8_t rpi_util::digitalRead(uint8_t physPin) {
-	uint8_t gpioPin = (physPin > 63)? 255 : physToGpio[physPin];
-	if (gpioPin == 255) {
+	uint8_t gpioPin;
+
+	if (get_gpio_number(physPin, &gpioPin)) {
 		logError("digitalRead: invalid pin: %d\n", physPin);
 		return 0;
 	}
+
 	// Check if SPI is in use and target pin is related to SPI
 	if (SPIClass::is_initialized() && gpioPin >= RPI_GPIO_P1_26 && gpioPin <= RPI_GPIO_P1_23) {
 		return 0;
@@ -218,9 +199,9 @@ void rpi_util::attachInterrupt(uint8_t physPin, void (*func)(), uint8_t mode) {
 	char fName[40];
 	char c;
 	int count, i;
+	uint8_t gpioPin;
 
-	uint8_t gpioPin = (physPin > 63)? 255 : physToGpio[physPin];
-	if (gpioPin == 255) {
+	if (get_gpio_number(physPin, &gpioPin)) {
 		logError("attachInterrupt: invalid pin: %d\n", physPin);
 		return;
 	}
@@ -295,8 +276,9 @@ void rpi_util::attachInterrupt(uint8_t physPin, void (*func)(), uint8_t mode) {
 }
 
 void rpi_util::detachInterrupt(uint8_t physPin) {
-	uint8_t gpioPin = (physPin > 63)? 255 : physToGpio[physPin];
-	if (gpioPin == 255) {
+	uint8_t gpioPin;
+
+	if (get_gpio_number(physPin, &gpioPin)) {
 		logError("detachInterrupt: invalid pin: %d\n", physPin);
 		return;
 	}
diff --git a/drivers/RPi/rpi_util.h b/drivers/RPi/rpi_util.h
index f7447ba..0a10981 100644
--- a/drivers/RPi/rpi_util.h
+++ b/drivers/RPi/rpi_util.h
@@ -19,12 +19,16 @@
  * Based on wiringPi Copyright (c) 2012 Gordon Henderson.
  */
 
-#ifndef pins_io_h
-#define pins_io_h
+#ifndef rpi_util_h
+#define rpi_util_h
 
 #include <stdint.h>
 #include "bcm2835.h"
 
+const int pin_to_gpio_rev1[41] = {-1, -1, -1, 0, -1, 1, -1, 4, 14, -1, 15, 17, 18, 21, -1, 22, 23, -1, 24, 10, -1, 9, 25, 11, 8, -1, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 };
+const int pin_to_gpio_rev2[41] = {-1, -1, -1, 2, -1, 3, -1, 4, 14, -1, 15, 17, 18, 27, -1, 22, 23, -1, 24, 10, -1, 9, 25, 11, 8, -1, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1 };
+const int pin_to_gpio_rev3[41] = {-1, -1, -1, 2, -1, 3, -1, 4, 14, -1, 15, 17, 18, 27, -1, 22, 23, -1, 24, 10, -1, 9, 25, 11, 8, -1, 7, -1, -1, 5, -1, 6, 12, 13, -1, 19, 16, 26, 20, -1, 21 };
+
 namespace rpi_util {
 
 typedef enum {