diff --git a/arch/arm/include/asm/arch-mx25/iomux-mx25.h b/arch/arm/include/asm/arch-mx25/iomux-mx25.h
index 220cf4e..e403eb9 100644
--- a/arch/arm/include/asm/arch-mx25/iomux-mx25.h
+++ b/arch/arm/include/asm/arch-mx25/iomux-mx25.h
@@ -287,15 +287,19 @@ enum {
 
 	MX25_PAD_CSI_D6__CSI_D6			= IOMUX_PAD(0x328, 0x130, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_CSI_D6__GPIO_1_31		= IOMUX_PAD(0x328, 0x130, 0x15, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSI_D6__CSPI3_SS0		= IOMUX_PAD(0x328, 0x130, 0x17, 0, 1, NO_PAD_CTRL),
 
 	MX25_PAD_CSI_D7__CSI_D7			= IOMUX_PAD(0x32c, 0x134, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_CSI_D7__GPIO_1_6		= IOMUX_PAD(0x32c, 0x134, 0x15, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSI_D7__CSPI3_SS1		= IOMUX_PAD(0x32c, 0x134, 0x17, 0, 1, NO_PAD_CTRL),
 
 	MX25_PAD_CSI_D8__CSI_D8			= IOMUX_PAD(0x330, 0x138, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_CSI_D8__GPIO_1_7		= IOMUX_PAD(0x330, 0x138, 0x15, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSI_D8__CSPI3_SS2		= IOMUX_PAD(0x330, 0x138, 0x17, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_CSI_D9__CSI_D9			= IOMUX_PAD(0x334, 0x13c, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_CSI_D9__GPIO_4_21		= IOMUX_PAD(0x334, 0x13c, 0x15, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSI_D9__CSPI3_SS3		= IOMUX_PAD(0x334, 0x13c, 0x17, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_CSI_MCLK__CSI_MCLK		= IOMUX_PAD(0x338, 0x140, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_CSI_MCLK__GPIO_1_8		= IOMUX_PAD(0x338, 0x140, 0x15, 0, 0, NO_PAD_CTRL),
@@ -315,16 +319,18 @@ enum {
 	MX25_PAD_I2C1_DAT__I2C1_DAT		= IOMUX_PAD(0x34c, 0x154, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_I2C1_DAT__GPIO_1_13		= IOMUX_PAD(0x34c, 0x154, 0x15, 0, 0, NO_PAD_CTRL),
 
-	MX25_PAD_CSPI1_MOSI__CSPI1_MOSI		= IOMUX_PAD(0x350, 0x158, 0x10, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSPI1_MOSI__CSPI1_MOSI		= IOMUX_PAD(0x350, 0x158, 0x10, 0x568, 0, NO_PAD_CTRL),
 	MX25_PAD_CSPI1_MOSI__GPIO_1_14		= IOMUX_PAD(0x350, 0x158, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_CSPI1_MISO__CSPI1_MISO		= IOMUX_PAD(0x354, 0x15c, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_CSPI1_MISO__GPIO_1_15		= IOMUX_PAD(0x354, 0x15c, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_CSPI1_SS0__CSPI1_SS0		= IOMUX_PAD(0x358, 0x160, 0x10, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSPI1_SS0__LD16		= IOMUX_PAD(0x358, 0x160, 0x11, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSPI1_SS0__PWM2_PWMO		= IOMUX_PAD(0x358, 0x160, 0x14, 0, 0, 0),
 	MX25_PAD_CSPI1_SS0__GPIO_1_16		= IOMUX_PAD(0x358, 0x160, 0x15, 0, 0, NO_PAD_CTRL),
 
-	MX25_PAD_CSPI1_SS1__CSPI1_SS1		= IOMUX_PAD(0x35c, 0x164, 0x10, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_CSPI1_SS1__CSPI1_SS1		= IOMUX_PAD(0x35c, 0x164, 0x10, 0x564, 0, NO_PAD_CTRL),
 	MX25_PAD_CSPI1_SS1__I2C3_DAT		= IOMUX_PAD(0x35c, 0x164, 0x11, 0x528, 1, NO_PAD_CTRL),
 	MX25_PAD_CSPI1_SS1__GPIO_1_17		= IOMUX_PAD(0x35c, 0x164, 0x15, 0, 0, NO_PAD_CTRL),
 
@@ -341,6 +347,7 @@ enum {
 	MX25_PAD_UART1_TXD__GPIO_4_23		= IOMUX_PAD(0x36c, 0x174, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_UART1_RTS__UART1_RTS		= IOMUX_PAD(0x370, 0x178, 0x10, 0, 0, PAD_CTL_PUS_100K_UP),
+	MX25_PAD_UART1_RTS__GPT3_CAPIN1		= IOMUX_PAD(0x370, 0x178, 0x12, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_UART1_RTS__CSI_D0		= IOMUX_PAD(0x370, 0x178, 0x11, 0x488, 1, NO_PAD_CTRL),
 	MX25_PAD_UART1_RTS__GPIO_4_24		= IOMUX_PAD(0x370, 0x178, 0x15, 0, 0, NO_PAD_CTRL),
 
@@ -356,6 +363,7 @@ enum {
 
 	MX25_PAD_UART2_RTS__UART2_RTS		= IOMUX_PAD(0x380, 0x188, 0x10, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_UART2_RTS__FEC_COL		= IOMUX_PAD(0x380, 0x188, 0x12, 0x504, 2, NO_PAD_CTRL),
+	MX25_PAD_UART2_RTS__GPT1_CAPIN1		= IOMUX_PAD(0x380, 0x188, 0x13, 0x504, 2, NO_PAD_CTRL),
 	MX25_PAD_UART2_RTS__GPIO_4_28		= IOMUX_PAD(0x380, 0x188, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_UART2_CTS__FEC_RX_ER		= IOMUX_PAD(0x384, 0x18c, 0x12, 0x518, 2, NO_PAD_CTRL),
@@ -385,17 +393,21 @@ enum {
 	MX25_PAD_SD1_DATA3__FEC_CRS		= IOMUX_PAD(0x39c, 0x1a4, 0x10, 0x508, 2, NO_PAD_CTRL),
 	MX25_PAD_SD1_DATA3__GPIO_2_28		= IOMUX_PAD(0x39c, 0x1a4, 0x15, 0, 0, NO_PAD_CTRL),
 
-	MX25_PAD_KPP_ROW0__KPP_ROW0		= IOMUX_PAD(0x3a0, 0x1a8, 0x10, 0, 0, MX25_KPP_ROW_PAD_CTRL),
+	MX25_PAD_KPP_ROW0__KPP_ROW0		= IOMUX_PAD(0x3a0, 0x1a8, 0x10, 0x568, 1, MX25_KPP_ROW_PAD_CTRL),
+	MX25_PAD_KPP_ROW0__UART3_RXD		= IOMUX_PAD(0x3a0, 0x1a8, 0x01, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_KPP_ROW0__GPIO_2_29		= IOMUX_PAD(0x3a0, 0x1a8, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_KPP_ROW1__KPP_ROW1		= IOMUX_PAD(0x3a4, 0x1ac, 0x10, 0, 0, MX25_KPP_ROW_PAD_CTRL),
+	MX25_PAD_KPP_ROW1__UART3_TXD		= IOMUX_PAD(0x3a4, 0x1ac, 0x01, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_KPP_ROW1__GPIO_2_30		= IOMUX_PAD(0x3a4, 0x1ac, 0x15, 0, 0, NO_PAD_CTRL),
 
-	MX25_PAD_KPP_ROW2__KPP_ROW2		= IOMUX_PAD(0x3a8, 0x1b0, 0x10, 0, 0, MX25_KPP_ROW_PAD_CTRL),
+	MX25_PAD_KPP_ROW2__KPP_ROW2		= IOMUX_PAD(0x3a8, 0x1b0, 0x10, 0x564, 0, MX25_KPP_ROW_PAD_CTRL),
+	MX25_PAD_KPP_ROW2__UART3_RTS		= IOMUX_PAD(0x3a8, 0x1b0, 0x01, 0, 1,  NO_PAD_CTRL),
 	MX25_PAD_KPP_ROW2__CSI_D0		= IOMUX_PAD(0x3a8, 0x1b0, 0x13, 0x488, 2, NO_PAD_CTRL),
 	MX25_PAD_KPP_ROW2__GPIO_2_31		= IOMUX_PAD(0x3a8, 0x1b0, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_KPP_ROW3__KPP_ROW3		= IOMUX_PAD(0x3ac, 0x1b4, 0x10, 0, 0, MX25_KPP_ROW_PAD_CTRL),
+	MX25_PAD_KPP_ROW3__UART3_CTS		= IOMUX_PAD(0x3ac, 0x1b4, 0x01, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_KPP_ROW3__CSI_LD1		= IOMUX_PAD(0x3ac, 0x1b4, 0x13, 0x48c, 2, NO_PAD_CTRL),
 	MX25_PAD_KPP_ROW3__GPIO_3_0		= IOMUX_PAD(0x3ac, 0x1b4, 0x15, 0, 0, NO_PAD_CTRL),
 
@@ -471,15 +483,15 @@ enum {
 	MX25_PAD_GPIO_C__CAN2_TX		= IOMUX_PAD(0x3f8, 0x1fc, 0x16, 0, 0, PAD_CTL_PUS_22K_UP),
 
 	MX25_PAD_GPIO_D__GPIO_D			= IOMUX_PAD(0x3fc, 0x200, 0x10, 0, 0, NO_PAD_CTRL),
-	MX25_PAD_GPIO_E__LD16			= IOMUX_PAD(0x400, 0x204, 0x02, 0, 0, PAD_CTL_SRE_FAST),
 	MX25_PAD_GPIO_D__CAN2_RX		= IOMUX_PAD(0x3fc, 0x200, 0x16, 0x484, 1, PAD_CTL_PUS_22K_UP),
 
 	MX25_PAD_GPIO_E__GPIO_E			= IOMUX_PAD(0x400, 0x204, 0x10, 0, 0, NO_PAD_CTRL),
-	MX25_PAD_GPIO_F__LD17			= IOMUX_PAD(0x404, 0x208, 0x02, 0, 0, PAD_CTL_SRE_FAST),
 	MX25_PAD_GPIO_E__I2C3_CLK		= IOMUX_PAD(0x400, 0x204, 0x11, 0x524, 2, NO_PAD_CTRL),
+	MX25_PAD_GPIO_E__LD16			= IOMUX_PAD(0x400, 0x204, 0x02, 0, 0, PAD_CTL_SRE_FAST),
 	MX25_PAD_GPIO_E__AUD7_TXD		= IOMUX_PAD(0x400, 0x204, 0x14, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_GPIO_F__GPIO_F			= IOMUX_PAD(0x404, 0x208, 0x10, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_GPIO_F__LD17			= IOMUX_PAD(0x404, 0x208, 0x02, 0, 0, PAD_CTL_SRE_FAST),
 	MX25_PAD_GPIO_F__AUD7_TXC		= IOMUX_PAD(0x404, 0x208, 0x14, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_EXT_ARMCLK__EXT_ARMCLK		= IOMUX_PAD(0x000, 0x20c, 0x10, 0, 0, NO_PAD_CTRL),
@@ -492,6 +504,7 @@ enum {
 	MX25_PAD_VSTBY_REQ__AUD7_TXFS		= IOMUX_PAD(0x408, 0x214, 0x14, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_VSTBY_REQ__GPIO_3_17		= IOMUX_PAD(0x408, 0x214, 0x15, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_VSTBY_ACK__VSTBY_ACK		= IOMUX_PAD(0x40c, 0x218, 0x10, 0, 0, NO_PAD_CTRL),
+	MX25_PAD_VSTBY_ACK__EPIT1_EPITO		= IOMUX_PAD(0x40c, 0x218, 0x13, 0, 0, NO_PAD_CTRL),
 	MX25_PAD_VSTBY_ACK__GPIO_3_18		= IOMUX_PAD(0x40c, 0x218, 0x15, 0, 0, NO_PAD_CTRL),
 
 	MX25_PAD_POWER_FAIL__POWER_FAIL		= IOMUX_PAD(0x410, 0x21c, 0x10, 0, 0, NO_PAD_CTRL),
diff --git a/arch/arm/include/asm/imx-common/iomux-v3.h b/arch/arm/include/asm/imx-common/iomux-v3.h
index dec11a1..d71d676 100644
--- a/arch/arm/include/asm/imx-common/iomux-v3.h
+++ b/arch/arm/include/asm/imx-common/iomux-v3.h
@@ -58,14 +58,18 @@ typedef u64 iomux_v3_cfg_t;
 
 #define MUX_MODE_SHIFT		36
 #define MUX_MODE_MASK		((iomux_v3_cfg_t)0x1f << MUX_MODE_SHIFT)
+#define MUX_MODE_SION_SHIFT	40
+#define MUX_MODE_SION_MASK	((iomux_v3_cfg_t)0x1 << MUX_MODE_SION_SHIFT)
 #define MUX_PAD_CTRL_SHIFT	41
 #define MUX_PAD_CTRL_MASK	((iomux_v3_cfg_t)0x3ffff << MUX_PAD_CTRL_SHIFT)
 #define MUX_SEL_INPUT_SHIFT	59
 #define MUX_SEL_INPUT_MASK	((iomux_v3_cfg_t)0xf << MUX_SEL_INPUT_SHIFT)
 
 #define MUX_MODE_SION		((iomux_v3_cfg_t)IOMUX_CONFIG_SION << \
-	MUX_MODE_SHIFT)
+				MUX_MODE_SHIFT)
 #define MUX_PAD_CTRL(x)		((iomux_v3_cfg_t)(x) << MUX_PAD_CTRL_SHIFT)
+#define MUX_SEL_INPUT(x)	((iomux_v3_cfg_t)(x) << MUX_SEL_INPUT_SHIFT)
+
 
 #define IOMUX_PAD(pad_ctrl_ofs, mux_ctrl_ofs, mux_mode, sel_input_ofs,	\
 		sel_input, pad_ctrl)					\
@@ -79,6 +83,13 @@ typedef u64 iomux_v3_cfg_t;
 #define NEW_PAD_CTRL(cfg, pad)	(((cfg) & ~MUX_PAD_CTRL_MASK) | \
 					MUX_PAD_CTRL(pad))
 
+#define NEW_SEL_INPUT(cfg, input)	(((cfg) & ~MUX_SEL_INPUT_MASK) | \
+					MUX_SEL_INPUT(input))
+
+#define SET_MODE_SION(cfg)	(((cfg) | MUX_MODE_SION_MASK))
+
+#define CLEAR_MODE_SION(cfg)	(((cfg) & ~MUX_MODE_SION_MASK))
+
 #define __NA_			0x000
 #define NO_MUX_I		0
 #define NO_PAD_I		0
diff --git a/arch/arm/lib/asm-offsets.c b/arch/arm/lib/asm-offsets.c
index b0c26e5..987cc8d 100644
--- a/arch/arm/lib/asm-offsets.c
+++ b/arch/arm/lib/asm-offsets.c
@@ -115,6 +115,15 @@ int main(void)
 	/* AHB <-> IP-Bus Interface */
 	DEFINE(AIPS_MPR_0_7, offsetof(struct aips_regs, mpr_0_7));
 	DEFINE(AIPS_MPR_8_15, offsetof(struct aips_regs, mpr_8_15));
+
+	/* M3IF */
+	DEFINE(M3IF_CTL, offsetof(struct m3if_regs, ctl));
+
+	/* WEIM */
+	DEFINE(WEIM_CSCR0U, offsetof(struct weim_regs, cscr0u));
+	DEFINE(WEIM_CSCR0L, offsetof(struct weim_regs, cscr0l));
+	DEFINE(WEIM_CSCR0A, offsetof(struct weim_regs, cscr0a));
+
 #endif
 
 #if defined(CONFIG_MX27)
diff --git a/board/syteco/zmx25/Makefile b/board/syteco/zmx25/Makefile
index d5edb48..1a8dd9e 100644
--- a/board/syteco/zmx25/Makefile
+++ b/board/syteco/zmx25/Makefile
@@ -5,5 +5,5 @@
 # SPDX-License-Identifier:	GPL-2.0+
 #
 
-obj-y	+= zmx25.o
+obj-y	+= zmx25.o fma1125.o mpr121.o polytouch.o
 obj-y	+= lowlevel_init.o
diff --git a/board/syteco/zmx25/fma1125.c b/board/syteco/zmx25/fma1125.c
new file mode 100644
index 0000000..02f0e44
--- /dev/null
+++ b/board/syteco/zmx25/fma1125.c
@@ -0,0 +1,193 @@
+/*
+ * (c) 2012 Graf-Syteco, Matthias Weisser
+ * <weisserm@arcor.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ *
+ */
+#include <common.h>
+#include <i2c.h>
+
+#include "fma1125.h"
+
+#define FMA1125_PA0_ALPHA			0x00
+#define FMA1125_PA1_ALPHA			0x01
+#define FMA1125_PA2_ALPHA			0x02
+#define FMA1125_PA3_ALPHA			0x03
+#define FMA1125_PA4_ALPHA			0x04
+#define FMA1125_PA5_ALPHA			0x05
+#define FMA1125_PA6_ALPHA			0x06
+#define FMA1125_PA7_ALPHA			0x07
+#define FMA1125_REFERENCE_DELAY			0x08
+#define FMA1125_BETA				0x09
+#define FMA1125_AIC_WAIT_TIME			0x0A
+#define FMA1125_PA0_STRENGTH_THRESHOLD		0x0B
+#define FMA1125_PA1_STRENGTH_THRESHOLD		0x0C
+#define FMA1125_PA2_STRENGTH_THRESHOLD		0x0D
+#define FMA1125_PA3_STRENGTH_THRESHOLD		0x0E
+#define FMA1125_PA4_STRENGTH_THRESHOLD		0x0F
+#define FMA1125_PA5_STRENGTH_THRESHOLD		0x10
+#define FMA1125_PA6_STRENGTH_THRESHOLD		0x11
+#define FMA1125_PA7_STRENGTH_THRESHOLD		0x12
+#define FMA1125_FEATURE_SELECT			0x13
+#define FMA1125_INTEGRATION_TIME		0x14
+#define FMA1125_IDLE_STATE_ENTER_TIME		0x15
+#define FMA1125_CONTROL_1			0x16
+#define FMA1125_CONTROL_2			0x17
+#define FMA1125_PA_DATA_OUT			0x18
+#define FMA1125_GPIO_DATA_OUT			0x19
+#define FMA1125_PA_DIRECTION			0x1A
+#define FMA1125_GPIO_DIRECTION			0x1B
+#define FMA1125_PA_CONFIGURATION		0x1C
+#define FMA1125_GPIO_CONFIGURATION		0x1D
+#define FMA1125_CALIBRATION_INTERVAL		0x1E
+#define FMA1125_GINT_INTERRUPT_MASK		0x1F
+#define FMA1125_GINT_INTERRUPT_CLEAR		0x20
+#define FMA1125_PA_EINT_ENABLE			0x21
+#define FMA1125_GPIO_EINT_ENABLE		0x22
+#define FMA1125_FILTER_PERIOD			0x23
+#define FMA1125_FILTER_THRESHOLD		0x24
+#define FMA1125_CONTROL_3			0x25
+#define FMA1125_GINT_INTERRUPT_EDGE_EN		0x26
+#define FMA1125_GPIO_INPUT_BOUNCE_PERIOD	0x27
+#define FMA1125_REGISTER_CHECK			0x28
+#define FMA1125_PA03_RESISTOR_SELECT		0x29
+#define FMA1125_PA47_RESISTOR_SELECT		0x2A
+#define FMA1125_REFERENCE_RESISTOR_SELECT	0x2B
+#define FMA1125_BETA_DISABLE			0x2C
+#define FMA1125_GPIO01_DIM_UNIT_PERIOD		0x2D
+#define FMA1125_GPIO23_DIM_UNIT_PERIOD		0x2E
+#define FMA1125_PA01_DIM_UNIT_PERIOD		0x2F
+#define FMA1125_PA23_DIM_UNIT_PERIOD		0x30
+#define FMA1125_GPIO0_DIMMING_CONTROL		0x31
+#define FMA1125_GPIO1_DIMMING_CONTROL		0x32
+#define FMA1125_GPIO2_DIMMING_CONTROL		0x33
+#define FMA1125_GPIO3_DIMMING_CONTROL		0x34
+#define FMA1125_PA0_DIMMING_CONTROL		0x35
+#define FMA1125_PA1_DIMMING_CONTROL		0x36
+#define FMA1125_PA2_DIMMING_CONTROL		0x37
+#define FMA1125_PA3_DIMMING_CONTROL		0x38
+#define FMA1125_GPIO03_DIMMING_MODE		0x39
+#define FMA1125_PA03_DIMMING_MODE		0x3A
+#define FMA1125_DIMMING_START			0x3B
+#define FMA1125_DIMMING_ENABLE			0x3C
+#define FMA1125_PA0_STRENGTH			0x50
+#define FMA1125_PA1_STRENGTH			0x51
+#define FMA1125_PA2_STRENGTH			0x52
+#define FMA1125_PA3_STRENGTH			0x53
+#define FMA1125_PA4_STRENGTH			0x54
+#define FMA1125_PA5_STRENGTH			0x55
+#define FMA1125_PA6_STRENGTH			0x56
+#define FMA1125_PA7_STRENGTH			0x57
+#define FMA1125_PA0_IMPEDANCE			0x58
+#define FMA1125_PA1_IMPEDANCE			0x59
+#define FMA1125_PA2_IMPEDANCE			0x5A
+#define FMA1125_PA3_IMPEDANCE			0x5B
+#define FMA1125_PA4_IMPEDANCE			0x5C
+#define FMA1125_PA5_IMPEDANCE			0x5D
+#define FMA1125_PA6_IMPEDANCE			0x5E
+#define FMA1125_PA7_IMPEDANCE			0x5F
+#define FMA1125_PA0_REF_IMPEDANCE		0x60
+#define FMA1125_PA1_REF_IMPEDANCE		0x61
+#define FMA1125_PA2_REF_IMPEDANCE		0x62
+#define FMA1125_PA3_REF_IMPEDANCE		0x63
+#define FMA1125_PA4_REF_IMPEDANCE		0x64
+#define FMA1125_PA5_REF_IMPEDANCE		0x65
+#define FMA1125_PA6_REF_IMPEDANCE		0x66
+#define FMA1125_PA7_REF_IMPEDANCE		0x67
+#define FMA1125_PA_TOUCH_BYTE			0x68
+#define FMA1125_GINT_INTERRUPT_PENDING		0x69
+#define FMA1125_PA_INPUT_DATA			0x6A
+#define FMA1125_GPIO_INPUT_DATA			0x6B
+#define FMA1125_PA_INPUT_BCU_DATA		0x6C
+#define FMA1125_GPIO_INPUT_BCU_DATA		0x6D
+#define FMA1125_PA_INPUT_FIFO_DATA		0x6E
+#define FMA1125_GPIO_INPUT_FIFO_DATA		0x6F
+#define FMA1125_CLK_EXTERNAL			0xF7
+#define FMA1125_CLK_INTERNAL			0xF8
+#define FMA1125_BIAS_OFF			0xF9
+#define FMA1125_BIAS_ON				0xFA
+#define FMA1125_LTB_ENABLE			0xFB
+#define FMA1125_WAKEUP_SLEEP			0xFC
+#define FMA1125_ENTER_SLEEP			0xFD
+#define FMA1125_COLD_RESET			0xFE
+#define FMA1125_WARM_RESET			0xFF
+
+void fma1125_init(void)
+{
+	int i;
+
+	/* cold reset */
+	i2c_reg_write(FMA1125_SA, FMA1125_COLD_RESET, 0x00);
+
+	/* register check */
+	i2c_reg_write(FMA1125_SA, FMA1125_REGISTER_CHECK, 0xFF);
+
+	/* Mask all interrupts besides touch */
+	i2c_reg_write(FMA1125_SA, FMA1125_GINT_INTERRUPT_MASK, 0xFE);
+	i2c_reg_write(FMA1125_SA, FMA1125_GINT_INTERRUPT_CLEAR, 0xFF);
+
+	/* Switch the LEDs off */
+	i2c_reg_write(FMA1125_SA, FMA1125_GPIO_DATA_OUT, 0x00);
+
+	/* All GPIOs are used as output */
+	i2c_reg_write(FMA1125_SA, FMA1125_GPIO_DIRECTION, 0x00);
+
+	/* All GPIOs are used as GPIOs (no direct sensor output) */
+	i2c_reg_write(FMA1125_SA, FMA1125_GPIO_CONFIGURATION, 0x0f);
+
+	/* Alpha */
+	for (i = 0; i < 8; i++)
+		i2c_reg_write(FMA1125_SA, FMA1125_PA0_ALPHA + i, 10);
+
+	/* Reference delay */
+	i2c_reg_write(FMA1125_SA, FMA1125_REFERENCE_DELAY, 80);
+
+	/* Beta */
+	i2c_reg_write(FMA1125_SA, FMA1125_BETA, 4);
+
+	/* AIC wait time */
+	i2c_reg_write(FMA1125_SA, FMA1125_AIC_WAIT_TIME, 0x27);
+
+	/* Strength threshold to 2/10 of integration time */
+	for (i = 0; i < 8; i++)
+		i2c_reg_write(FMA1125_SA,
+			FMA1125_PA0_STRENGTH_THRESHOLD + i, 40);
+
+	/* Set integration time to 200
+	 * (20kHz clock / 200 -> 10ms update rate)
+	 */
+	i2c_reg_write(FMA1125_SA, FMA1125_INTEGRATION_TIME, 200);
+
+	/* Enable APIS mode 2 */
+	i2c_reg_write(FMA1125_SA, FMA1125_FEATURE_SELECT, 4);
+
+	/* Always in ACTIVE mode and AIC on (set bit 7 for LED dimming) */
+	i2c_reg_write(FMA1125_SA, FMA1125_CONTROL_1, 0xA0);
+	i2c_reg_write(FMA1125_SA, FMA1125_CONTROL_2, 0x01);
+
+	/* PA configuration */
+	i2c_reg_write(FMA1125_SA, FMA1125_PA_CONFIGURATION, 0);
+
+	i2c_reg_write(FMA1125_SA, FMA1125_PA03_RESISTOR_SELECT, (1 << 0) |
+								(1 << 2) |
+								(1 << 4) |
+								(3 << 6));
+	i2c_reg_write(FMA1125_SA, FMA1125_PA47_RESISTOR_SELECT, (3 << 0) |
+								(2 << 2) |
+								(1 << 4) |
+								(1 << 6));
+
+	/* Perform a warm reset to make the new settings valid */
+	i2c_reg_write(FMA1125_SA, FMA1125_WARM_RESET, 0);
+}
+
+int fma1125_get_touch_bits(void)
+{
+	return i2c_reg_read(FMA1125_SA, FMA1125_PA_TOUCH_BYTE);
+}
+
+void fma1125_set_gpio_out(uint8_t value)
+{
+	i2c_reg_write(FMA1125_SA, FMA1125_GPIO_DATA_OUT, value);
+}
diff --git a/board/syteco/zmx25/fma1125.h b/board/syteco/zmx25/fma1125.h
new file mode 100644
index 0000000..d329dfb
--- /dev/null
+++ b/board/syteco/zmx25/fma1125.h
@@ -0,0 +1,19 @@
+/*
+ * (c) 2012 Graf-Syteco, Matthias Weisser
+ * <weisserm@arcor.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __FMA1125_H
+#define __FMA1125_H
+
+
+#define FMA1125_SA				0x68
+
+void fma1125_init(void);
+int fma1125_get_touch_bits(void);
+void fma1125_set_gpio_out(uint8_t value);
+
+#endif
+
diff --git a/board/syteco/zmx25/lowlevel_init.S b/board/syteco/zmx25/lowlevel_init.S
index 5eccf09..cfcc6d2 100644
--- a/board/syteco/zmx25/lowlevel_init.S
+++ b/board/syteco/zmx25/lowlevel_init.S
@@ -92,6 +92,26 @@
 lowlevel_init:
 	init_aips
 	init_max
+
+	/* Setup of NOR flash CS */
+	write32 IMX_WEIM_CTRL_BASE + WEIM_CSCR0U, \
+		WEIM_CSCR_U(0,0,0,0,0,0,0,0,0,14,0,0,2)
+	write32 IMX_WEIM_CTRL_BASE + WEIM_CSCR0L, \
+		WEIM_CSCR_L(1,1,0,0,0,0,5,0,0,0,0,1)
+	write32 IMX_WEIM_CTRL_BASE + WEIM_CSCR0A, \
+		WEIM_CSCR_A(0,0,1,1,0,0,0,0,0,0,0,0,0,0)
+
+	/* Set some memory access priorities */
+	write32 IMX_M3IF_CTRL_BASE + M3IF_CTL, M3IF_CTL_MRRP(1)
+	write32	IMX_MAX_BASE + MAX_MGPCR0, MAX_MGPCR_AULB(0x4)
+	write32	IMX_MAX_BASE + MAX_MGPCR1, MAX_MGPCR_AULB(0x4)
+	write32	IMX_MAX_BASE + MAX_MGPCR2, MAX_MGPCR_AULB(0x4)
+	write32	IMX_MAX_BASE + MAX_MGPCR3, MAX_MGPCR_AULB(0x4)
+	write32	IMX_MAX_BASE + MAX_MGPCR4, MAX_MGPCR_AULB(0x4)
+
+	/* Need this early for voltage sequence on G305 */
+	write32 IMX_IOPADMUX_BASE + 0x38c, 5 //Offset 0x38C IOPADMUX_SD1_CLK
+
 	init_clocks
 	init_lpddr
 	mov	pc, lr
diff --git a/board/syteco/zmx25/mpr121.c b/board/syteco/zmx25/mpr121.c
new file mode 100644
index 0000000..d1ea416
--- /dev/null
+++ b/board/syteco/zmx25/mpr121.c
@@ -0,0 +1,204 @@
+/*
+ * (c) 2012 Graf-Syteco, Matthias Weisser
+ * <weisserm@arcor.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ *
+ */
+#include <common.h>
+#include <i2c.h>
+
+#include "mpr121.h"
+
+#define MPR121_ELE0_7_TOUCH         0x00 /* Touch Status */
+#define MPR121_ELE8_11_TOUCH        0x01 /* ELEPROX Touch Status */
+#define MPR121_ELE0_7_OOR           0x02 /* OOR Status */
+#define MPR121_ELE8_11_OOR          0x03 /* ELEPROX OOR Status */
+#define MPR121_ELE0_L               0x04 /* Electrode Filtered Data LSB */
+#define MPR121_ELE0_H               0x05 /* Electrode Filtered Data MSB */
+#define MPR121_ELE1_L               0x06 /* Electrode Filtered Data LSB */
+#define MPR121_ELE1_H               0x07 /* Electrode Filtered Data MSB */
+#define MPR121_ELE2_L               0x08 /* Electrode Filtered Data LSB */
+#define MPR121_ELE2_H               0x09 /* Electrode Filtered Data MSB */
+#define MPR121_ELE3_L               0x0A /* Electrode Filtered Data LSB */
+#define MPR121_ELE3_H               0x0B /* Electrode Filtered Data MSB */
+#define MPR121_ELE4_L               0x0C /* Electrode Filtered Data LSB */
+#define MPR121_ELE4_H               0x0D /* Electrode Filtered Data MSB */
+#define MPR121_ELE5_L               0x0E /* Electrode Filtered Data LSB */
+#define MPR121_ELE5_H               0x0F /* Electrode Filtered Data MSB */
+#define MPR121_ELE6_L               0x10 /* Electrode Filtered Data LSB */
+#define MPR121_ELE6_H               0x11 /* Electrode Filtered Data MSB */
+#define MPR121_ELE7_L               0x12 /* Electrode Filtered Data LSB */
+#define MPR121_ELE7_H               0x13 /* Electrode Filtered Data MSB */
+#define MPR121_ELE8_L               0x14 /* Electrode Filtered Data LSB */
+#define MPR121_ELE8_H               0x15 /* Electrode Filtered Data MSB */
+#define MPR121_ELE9_L               0x16 /* Electrode Filtered Data LSB */
+#define MPR121_ELE9_H               0x17 /* Electrode Filtered Data MSB */
+#define MPR121_ELE10_L              0x18 /* Electrode Filtered Data LSB */
+#define MPR121_ELE10_H              0x19 /* Electrode Filtered Data MSB */
+#define MPR121_ELE11_L              0x1A /* Electrode Filtered Data LSB */
+#define MPR121_ELE11_H              0x1B /* Electrode Filtered Data MSB */
+#define MPR121_ELEPROX_L            0x1C /* Electrode Filtered Data LSB */
+#define MPR121_ELEPROX_H            0x1D /* Electrode Filtered Data MSB */
+#define MPR121_ELE0_BASELINE        0x1E /* Baseline Value */
+#define MPR121_ELE1_BASELINE        0x1F /* Baseline Value */
+#define MPR121_ELE2_BASELINE        0x20 /* Baseline Value */
+#define MPR121_ELE3_BASELINE        0x21 /* Baseline Value */
+#define MPR121_ELE4_BASELINE        0x22 /* Baseline Value */
+#define MPR121_ELE5_BASELINE        0x23 /* Baseline Value */
+#define MPR121_ELE6_BASELINE        0x24 /* Baseline Value */
+#define MPR121_ELE7_BASELINE        0x25 /* Baseline Value */
+#define MPR121_ELE8_BASELINE        0x26 /* Baseline Value */
+#define MPR121_ELE9_BASELINE        0x27 /* Baseline Value */
+#define MPR121_ELE10_BASELINE       0x28 /* Baseline Value */
+#define MPR121_ELE11_BASELINE       0x29 /* Baseline Value */
+#define MPR121_ELEPROX              0x2A /* Baseline Value */
+#define MPR121_MHD_RIS              0x2B /* Rising */
+#define MPR121_NHD_RIS              0x2C /* Amount Rising */
+#define MPR121_NCL_RIS              0x2D /* Rising */
+#define MPR121_FDL_RIS              0x2E /* Rising */
+#define MPR121_MHD_FALL             0x2F /* Falling */
+#define MPR121_NHD_FALL             0x30 /* Amount Falling */
+#define MPR121_NCL_FALL             0x31 /* Falling */
+#define MPR121_FDL_FALL             0x32 /* Falling */
+#define MPR121_NHD_TOUCH            0x33 /* Amount Touched */
+#define MPR121_NCL_TOUCH            0x34 /* Touched */
+#define MPR121_FDL_TOUCH            0x35 /* Touched */
+#define MPR121_ELEPROX_MHD_RIS      0x36 /* Rising */
+#define MPR121_ELEPROX_NHD_RIS      0x37 /* Amount Rising */
+#define MPR121_ELEPROX_NCL_RIS      0x38 /* Rising */
+#define MPR121_ELEPROX_FDL_RIS      0x39 /* Rising */
+#define MPR121_ELEPROX_MHD_FALL     0x3A /* Falling */
+#define MPR121_ELEPROX_NHD_FALL     0x3B /* Amount Falling */
+#define MPR121_ELEPROX_NCL_FALL     0x3C /* Falling */
+#define MPR121_ELEPROX_FDL_FALL     0x3D /* Falling */
+#define MPR121_ELEPROX_NHD_TOUCH    0x3E /* Amount Touched */
+#define MPR121_ELEPROX_NCL_TOUCH    0x3F /* Touched */
+#define MPR121_ELEPROX_FDL_TOUCH    0x40 /* Touched */
+#define MPR121_ELE0_THRESH_TOUCH    0x41 /* Touch Threshold */
+#define MPR121_ELE0_THRESH_REL      0x42 /* Release Threshold */
+#define MPR121_ELE1_THRESH_TOUCH    0x43 /* Touch Threshold */
+#define MPR121_ELE1_THRESH_REL      0x44 /* Release Threshold */
+#define MPR121_ELE2_THRESH_TOUCH    0x45 /* Touch Threshold */
+#define MPR121_ELE2_THRESH_REL      0x46 /* Release Threshold */
+#define MPR121_ELE3_THRESH_TOUCH    0x47 /* Touch Threshold */
+#define MPR121_ELE3_THRESH_REL      0x48 /* Release Threshold */
+#define MPR121_ELE4_THRESH_TOUCH    0x49 /* Touch Threshold */
+#define MPR121_ELE4_THRESH_REL      0x4A /* Release Threshold */
+#define MPR121_ELE5_THRESH_TOUCH    0x4B /* Touch Threshold */
+#define MPR121_ELE5_THRESH_REL      0x4C /* Release Threshold */
+#define MPR121_ELE6_THRESH_TOUCH    0x4D /* Touch Threshold */
+#define MPR121_ELE6_THRESH_REL      0x4E /* Release Threshold */
+#define MPR121_ELE7_THRESH_TOUCH    0x4F /* Touch Threshold */
+#define MPR121_ELE7_THRESH_REL      0x50 /* Release Threshold */
+#define MPR121_ELE8_THRESH_TOUCH    0x51 /* Touch Threshold */
+#define MPR121_ELE8_THRESH_REL      0x52 /* Release Threshold */
+#define MPR121_ELE9_THRESH_TOUCH    0x53 /* Touch Threshold */
+#define MPR121_ELE9_THRESH_REL      0x54 /* Release Threshold */
+#define MPR121_ELE10_THRESH_TOUCH   0x55 /* Touch Threshold */
+#define MPR121_ELE10_THRESH_REL     0x56 /* Release Threshold */
+#define MPR121_ELE11_THRESH_TOUCH   0x57 /* Touch Threshold */
+#define MPR121_ELE11_THRESH_REL     0x58 /* Release Threshold */
+#define MPR121_ELEPROX_THRESH_TOUCH 0x59 /* Touch Threshold */
+#define MPR121_ELEPROX_THRESH_REL   0x5A /* Release Threshold */
+#define MPR121_DBC                  0x5B /* Debounce Touch & Release */
+#define MPR121_FILT_CDC             0x5C /* Filter/Global CDC Configuration */
+#define MPR121_FILT_CDT             0x5D /* Filter/Global CDT Configuration */
+#define MPR121_ECR                  0x5E /* Electrode Configuration */
+#define MPR121_ELE0_CURRENT         0x5F /* Electrode Current */
+#define MPR121_ELE1_CURRENT         0x60 /* Electrode Current */
+#define MPR121_ELE2_CURRENT         0x61 /* Electrode Current */
+#define MPR121_ELE3_CURRENT         0x62 /* Electrode Current */
+#define MPR121_ELE4_CURRENT         0x63 /* Electrode Current */
+#define MPR121_ELE5_CURRENT         0x64 /* Electrode Current */
+#define MPR121_ELE6_CURRENT         0x65 /* Electrode Current */
+#define MPR121_ELE7_CURRENT         0x66 /* Electrode Current */
+#define MPR121_ELE8_CURRENT         0x67 /* Electrode Current */
+#define MPR121_ELE9_CURRENT         0x68 /* Electrode Current */
+#define MPR121_ELE10_CURRENT        0x69 /* Electrode Current */
+#define MPR121_ELE11_CURRENT        0x6A /* Electrode Current */
+#define MPR121_ELEPROX_CURRENT      0x6B /* Electrode Current */
+#define MPR121_ELE0_1_CHARGE        0x6C /* Charge Time */
+#define MPR121_ELE2_3_CHARGE        0x6D /* Charge Time */
+#define MPR121_ELE4_5_CHARGE        0x6E /* Charge Time */
+#define MPR121_ELE6_7_CHARGE        0x6F /* Charge Time */
+#define MPR121_ELE8_9_CHARGE        0x70 /* Charge Time */
+#define MPR121_ELE10_11_CHARGE      0x71 /* Charge Time */
+#define MPR121_ELEPROX_CHARGE       0x72 /* Charge Time */
+#define MPR121_GPIO_CTRL0           0x73 /* Control Register 0 */
+#define MPR121_GPIO_CTRL1           0x74 /* Control Register 1 */
+#define MPR121_GPIO_DATA            30x7 /* Data Register */
+#define MPR121_GPIO_DIR             0x76 /* Direction Register */
+#define MPR121_GPIO_EN              0x77 /* Enable Register */
+#define MPR121_GPIO_SET             0x78 /* Data Set Register */
+#define MPR121_GPIO_CLR             0x79 /* Data Clear Register */
+#define MPR121_GPIO_TOG             0x7A /* Data Toggle Register */
+#define MPR121_AUTO_CONFIG_CTRL0    0x7B /* Control Register 0 */
+#define MPR121_AUTO_CONFIG_CTRL1    0x7C /* Control Register 1 */
+#define MPR121_AUTO_CONFIG_USL      0x7D /* USL Register */
+#define MPR121_AUTO_CONFIG_LSL      0x7E /* LSL Register */
+#define MPR121_AUTO_CONFIG_TLR      0x7F /* Target Level Register */
+#define MPR121_SRST                 0x80 /* Soft Reset Register */
+
+static const uint8_t electrodeCurrent[] = {
+0x1f / 1.7,
+0x1f / 1.7,
+0x3b / 1.7,
+0x37 / 1.7,
+0x34 / 1.7,
+0x2f / 1.7,
+0x2b / 1.7,
+0x2a / 1.7,
+0x2d / 1.7,
+0x33 / 1.7,
+0x3a / 1.7,
+0x3c / 1.7,
+};
+
+void mpr121_init(u8 sa)
+{
+	int i;
+
+	/* Soft reset */
+	i2c_reg_write(sa, MPR121_ECR,      0x00);
+	i2c_reg_write(sa, MPR121_SRST,     0x63);
+
+	/* Settings according to AN3944 */
+	i2c_reg_write(sa, MPR121_MHD_RIS,  0x01);
+	i2c_reg_write(sa, MPR121_NHD_RIS,  0x01);
+	i2c_reg_write(sa, MPR121_NCL_RIS,  0x00);
+	i2c_reg_write(sa, MPR121_FDL_RIS,  0x00);
+
+	i2c_reg_write(sa, MPR121_MHD_FALL, 0x01);
+	i2c_reg_write(sa, MPR121_NHD_FALL, 0x01);
+	i2c_reg_write(sa, MPR121_NCL_FALL, 0xff);
+	i2c_reg_write(sa, MPR121_FDL_FALL, 0x02);
+
+	for (i = MPR121_ELE0_THRESH_TOUCH;
+	     i <= MPR121_ELE11_THRESH_TOUCH; i += 2) {
+		i2c_reg_write(sa, i + 0, 0x04);
+		i2c_reg_write(sa, i + 1, 0x02);
+	}
+
+	i2c_reg_write(sa, MPR121_FILT_CDT, 0x04);
+
+	for (i = 0; i < 12; i++)
+		i2c_reg_write(sa, MPR121_ELE0_CURRENT + i, electrodeCurrent[i]);
+
+
+	for (i = 0; i < 6; i++)
+		i2c_reg_write(sa, MPR121_ELE0_1_CHARGE + i, 0x22);
+
+	i2c_reg_write(sa, MPR121_ECR, 0xCC);
+}
+
+int mpr121_get_touch_bits(u8 sa)
+{
+	int res;
+
+	res = i2c_reg_read(sa, MPR121_ELE0_7_TOUCH);
+	res += i2c_reg_read(sa, MPR121_ELE8_11_TOUCH) * 256;
+
+	return res;
+}
+
diff --git a/board/syteco/zmx25/mpr121.h b/board/syteco/zmx25/mpr121.h
new file mode 100644
index 0000000..57cdda2
--- /dev/null
+++ b/board/syteco/zmx25/mpr121.h
@@ -0,0 +1,20 @@
+/*
+ * (c) 2012 Graf-Syteco, Matthias Weisser
+ * <weisserm@arcor.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ *
+ */
+
+#ifndef __MPR121_H
+#define __MPR121_H
+
+#define MPR121_SA_0                 0x5a
+#define MPR121_SA_1                 0x5b
+#define MPR121_SA_2                 0x5c
+#define MPR121_SA_3                 0x5d
+
+void mpr121_init(u8 sa);
+int mpr121_get_touch_bits(u8 sa);
+
+#endif
diff --git a/board/syteco/zmx25/polytouch.c b/board/syteco/zmx25/polytouch.c
new file mode 100644
index 0000000..3a35090
--- /dev/null
+++ b/board/syteco/zmx25/polytouch.c
@@ -0,0 +1,123 @@
+/*
+ * (c) 2013 Graf-Syteco, Matthias Weisser
+ * <weisserm@arcor.de>
+ *
+ * SPDX-License-Identif ier:	GPL-2.0+
+ *
+ */
+
+#include <common.h>
+#include <i2c.h>
+
+#include "polytouch.h"
+
+#define POLYTOUCH_CMD_GETVERSION    0xBB
+#define POLYTOUCH_CMD_GETTOUCH      0xF9
+#define POLYTOUCH_CMD_SETREG        0xFC
+#define POLYTOUCH_CMD_GETREG        0xFC
+
+#define POLYTOUCH_REG_THRESHOLD     0x00
+#define POLYTOUCH_REG_MONITORTIME   0x07
+#define POLYTOUCH_REG_REPORTRATE    0x08
+#define POLYTOUCH_REG_GAIN          0x30
+#define POLYTOUCH_REG_OFFSET        0x31
+#define POLYTOUCH_REG_XRES          0x33
+#define POLYTOUCH_REG_YRES          0x34
+#define POLYTOUCH_REG_HIBERNATE     0x3A
+
+#define POLYTOUCH_PACKET_LENGTH     26
+
+static int xres;
+static int yres;
+
+static int32_t polytouch_set_reg(uint32_t reg, uint32_t val)
+{
+	uint8_t buf[3];
+	int r;
+
+	buf[0] = reg;
+	buf[1] = val;
+	buf[2] = POLYTOUCH_CMD_SETREG ^ buf[0] ^ buf[1];
+
+	r = i2c_write(POLYTOUCH_SA, POLYTOUCH_CMD_SETREG, 1, buf, 3);
+
+	if (0 != r)
+		printf("i2c_write failed with %d\n", r);
+
+
+	return r;
+}
+
+static int32_t polytouch_get_reg(uint32_t reg)
+{
+	uint8_t buf;
+
+	if (i2c_read(POLYTOUCH_SA, (POLYTOUCH_CMD_GETREG<<8) +
+		((reg + 0x40)<<0), 2, &buf, 1) == 0)
+		return buf;
+
+	return -1;
+}
+
+void polytouch_init(void)
+{
+	char buf[32];
+
+	if (i2c_read(POLYTOUCH_SA, POLYTOUCH_CMD_GETVERSION, 1,
+		(uint8_t *)buf, 22) == 0) {
+		uint32_t gain = 7;
+		uint32_t threshold = 40;
+		uint32_t offset = 0;
+
+		buf[22] = '\0';
+
+		xres = polytouch_get_reg(POLYTOUCH_REG_XRES) * 64;
+		yres = polytouch_get_reg(POLYTOUCH_REG_YRES) * 64;
+
+		if (0 != strstr(buf, "EP035")) {
+			gain = 3;
+			threshold = 25;
+			offset = 34;
+		} else if (0 != strstr(buf, "EP043")) {
+			gain = 5;
+			threshold = 35;
+			offset = 34;
+		} else if (0 != strstr(buf, "EP057")) {
+			gain = 2;
+			threshold = 25;
+			offset = 34;
+		} else if (0 != strstr(buf, "EP070")) {
+			gain = 2;
+			threshold = 27;
+			offset = 34;
+		}
+
+		polytouch_set_reg(POLYTOUCH_REG_GAIN, gain);
+		polytouch_set_reg(POLYTOUCH_REG_THRESHOLD, threshold);
+		polytouch_set_reg(POLYTOUCH_REG_OFFSET, offset);
+		polytouch_set_reg(POLYTOUCH_REG_REPORTRATE, 8);
+		polytouch_set_reg(POLYTOUCH_REG_MONITORTIME, 0xC8);
+	}
+}
+
+int polytouch_is_touched_lower_left(void)
+{
+	uint8_t buf[POLYTOUCH_PACKET_LENGTH];
+
+	if (0 == i2c_read(POLYTOUCH_SA, POLYTOUCH_CMD_GETTOUCH, 1, buf,
+			  POLYTOUCH_PACKET_LENGTH)) {
+		if ((buf[0] == 0xAA) && (buf[1] == 0xAA) &&
+		    (buf[2] == 0x1A) && (buf[3] == 0x01)) {
+			uint16_t x;
+			uint16_t y;
+
+			x = ((buf[5] & 0x0F) << 8) + buf[6];
+			y = ((buf[7] & 0x0F) << 8) + buf[8];
+
+			if ((x < xres / 10) && (y > yres - yres / 10))
+				return 1;
+		}
+	}
+
+	return 0;
+}
diff --git a/board/syteco/zmx25/polytouch.h b/board/syteco/zmx25/polytouch.h
new file mode 100644
index 0000000..9e7cbbd
--- /dev/null
+++ b/board/syteco/zmx25/polytouch.h
@@ -0,0 +1,19 @@
+/*
+ * (c) 2012 Graf-Syteco, Matthias Weisser
+ * <weisserm@arcor.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ *
+ */
+
+#ifndef __POLYTOUCH_H
+#define __POLYTOUCH_H
+
+#define POLYTOUCH_SA		0x38	/* I2C slave address of PolyTouch */
+
+void polytouch_init(void);
+int polytouch_is_touched_lower_left(void);
+
+#endif
+
+
diff --git a/board/syteco/zmx25/zmx25.c b/board/syteco/zmx25/zmx25.c
index bdbf02a..5615862 100644
--- a/board/syteco/zmx25/zmx25.c
+++ b/board/syteco/zmx25/zmx25.c
@@ -1,4 +1,7 @@
 /*
+ * (c) 2014 Graf-Syteco, Thomas Diener
+ * <thomas-diener@t-online.de>
+ *
  * (c) 2011 Graf-Syteco, Matthias Weisser
  * <weisserm@arcor.de>
  *
@@ -15,62 +18,536 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 #include <common.h>
+#include <i2c.h>
+#include <spi.h>
+#include <video.h>
 #include <asm/gpio.h>
 #include <asm/io.h>
 #include <asm/arch/imx-regs.h>
 #include <asm/arch/iomux-mx25.h>
+#include <asm/imx-common/mxc_i2c.h>
+
+#include "fma1125.h"
+#include "mpr121.h"
+#include "polytouch.h"
+
+#define ZMX25_CPU_BOARD_TYPE_UNKNOWN	0
+#define ZMX25_CPU_BOARD_TYPE_G282A0	1
+#define ZMX25_CPU_BOARD_TYPE_G282A1	2
+#define ZMX25_CPU_BOARD_TYPE_G292	3
+#define ZMX25_CPU_BOARD_TYPE_G305	4
+
+#define ZMX25_IO_BOARD_TYPE_NONE	0
+#define ZMX25_IO_BOARD_TYPE_UNKNOWN	1
+#define ZMX25_IO_BOARD_TYPE_G283	2
+#define ZMX25_IO_BOARD_TYPE_G297	3
 
 DECLARE_GLOBAL_DATA_PTR;
 
-int board_init()
+static inline u32 hw_setup_common(void);
+
+static inline void hw_setup_cpu_g282g292(void);
+static inline void hw_setup_cpu_g282(void);
+static inline void hw_setup_cpu_g282a1(void);
+static inline void hw_setup_cpu_g292(void);
+static inline void hw_setup_cpu_g305(void);
+
+static inline void hw_setup_io_g283(void);
+static inline void hw_setup_io_g297(void);
+
+static inline void hw_setup_ethernet(void);
+static inline void hw_setup_video(void);
+
+static const iomux_v3_cfg_t can_pads[] = {
+	CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_GPIO_A__CAN1_TX, 0)),
+	CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_GPIO_B__CAN1_RX, PAD_CTL_PUS_100K_DOWN)),
+	CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_GPIO_C__CAN2_TX, 0)),
+	CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_GPIO_D__CAN2_RX, 0)),
+};
+
+static u32 cpu_board_type;
+static u32 io_board_type;
+static u32 have_touch_display;
+
+static inline u16 hw_g297_xfer_srio(u16 value)
+{
+	struct spi_slave *spi;
+	u8 spi_in[2];
+	u8 spi_out[2];
+
+	writeb(0x05 | (1<<6), 0xB4000000);
+
+	spi_out[0] = value >> 8;
+	spi_out[1] = value;
+
+	spi = spi_setup_slave(2, 0, 1000000, 0);
+	spi_claim_bus(spi);
+	spi_xfer(spi, 16, spi_out, spi_in, SPI_XFER_BEGIN | SPI_XFER_END);
+	spi_release_bus(spi);
+	spi_free_slave(spi);
+
+	writeb(0x03 | (1<<6), 0xB4000000);
+	udelay(1000);
+	writeb(0x05 | (1<<6), 0xB4000000);
+
+	return	spi_in[0] | (spi_in[1] << 8);
+}
+
+static inline u32 hw_setup_common(void)
+{
+	struct ccm_regs *ccm;
+	u32 bt_uart_src;
+	u32 res;
+
+	ccm = (struct ccm_regs *)IMX_CCM_BASE;
+
+	have_touch_display = 0;
+
+	/* Setup of CAN1 signals */
+	imx_iomux_v3_setup_multiple_pads(can_pads, ARRAY_SIZE(can_pads));
+
+	/* Set CPU clock to 399MHz */
+	writel(0x20034000, &ccm->cctl);
+
+	/*
+	 * We decide on which board we are running according to BT_UART_SRC
+	 * bits in CRDR register of CCM. 0 -> G292, 1 -> G282
+	 */
+	bt_uart_src = (readl(&ccm->crdr) >> CCM_CRDR_BT_UART_SRC_SHIFT) &
+		CCM_CRDR_BT_UART_SRC_MASK;
+
+	if (bt_uart_src == 0)
+		res = ZMX25_CPU_BOARD_TYPE_G292;
+	else if (bt_uart_src == 1)
+		res = ZMX25_CPU_BOARD_TYPE_G282A0;
+	else if (bt_uart_src == 2)
+		res = ZMX25_CPU_BOARD_TYPE_G282A1;
+	else if (bt_uart_src == 4)
+		res = ZMX25_CPU_BOARD_TYPE_G305;
+	else
+		res = ZMX25_CPU_BOARD_TYPE_UNKNOWN;
+
+	return res;
+}
+
+static inline void hw_setup_cpu_g282g292(void)
 {
+	static const iomux_v3_cfg_t i2c3_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_SS1__I2C3_DAT),
+		CLEAR_MODE_SION(MX25_PAD_GPIO_E__I2C3_CLK),
+	};
+
 	static const iomux_v3_cfg_t sdhc1_pads[] = {
 		NEW_PAD_CTRL(MX25_PAD_SD1_CMD__SD1_CMD, NO_PAD_CTRL),
-		NEW_PAD_CTRL(MX25_PAD_SD1_CLK__SD1_CLK, NO_PAD_CTRL),
+		NEW_PAD_CTRL(MX25_PAD_SD1_CLK__SD1_CLK, PAD_CTL_PUS_47K_UP | PAD_CTL_SRE_FAST),
 		NEW_PAD_CTRL(MX25_PAD_SD1_DATA0__SD1_DATA0, NO_PAD_CTRL),
 		NEW_PAD_CTRL(MX25_PAD_SD1_DATA1__SD1_DATA1, NO_PAD_CTRL),
 		NEW_PAD_CTRL(MX25_PAD_SD1_DATA2__SD1_DATA2, NO_PAD_CTRL),
 		NEW_PAD_CTRL(MX25_PAD_SD1_DATA3__SD1_DATA3, NO_PAD_CTRL),
 	};
 
-	static const iomux_v3_cfg_t dig_out_pads[] = {
-		MX25_PAD_CSI_D8__GPIO_1_7, /* Ouput 1 Ctrl */
-		MX25_PAD_CSI_D7__GPIO_1_6, /* Ouput 2 Ctrl */
-		NEW_PAD_CTRL(MX25_PAD_CSI_D6__GPIO_1_31, 0), /* Ouput 1 Stat */
-		NEW_PAD_CTRL(MX25_PAD_CSI_D5__GPIO_1_30, 0), /* Ouput 2 Stat */
+	bus_i2c_init((void *)IMX_I2C3_BASE, 400000, 0, NULL, NULL);
+	bus_i2c_init((void *)IMX_I2C_BASE, 400000, 0, NULL, NULL);
+
+	/* Setup of core voltage selection pin to run at 1.4V */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_EXT_ARMCLK__GPIO_3_15));
+	gpio_direction_output(IMX_GPIO_NR(3, 15), 1);
+
+	/* Setup of input daisy chains for SD card pins */
+	imx_iomux_v3_setup_multiple_pads(sdhc1_pads, ARRAY_SIZE(sdhc1_pads));
+
+	/* Setup of I2C3 signals */
+	imx_iomux_v3_setup_multiple_pads(i2c3_pads, ARRAY_SIZE(i2c3_pads));
+}
+
+static inline void hw_setup_cpu_g282(void)
+{
+	/* GPIO1[26] -> LCD_PWR */
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(
+			CLEAR_MODE_SION(MX25_PAD_PWM__GPIO_1_26), PAD_CTL_PUS_100K_UP));
+	udelay(1000);
+
+	/* GPIO3[19] -> Used as power fail interrupt */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_POWER_FAIL__GPIO_3_19));
+	gpio_direction_input(IMX_GPIO_NR(3, 19));
+
+	if (!gpio_get_value(IMX_GPIO_NR(1, 26)))
+		io_board_type = ZMX25_IO_BOARD_TYPE_G297;
+	else
+		io_board_type = ZMX25_IO_BOARD_TYPE_G283;
+}
+
+static inline void hw_setup_cpu_g282a1(void)
+{
+	struct ccm_regs *ccm;
+	struct weim_regs *weim;
+
+	static const iomux_v3_cfg_t fpga_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSI_MCLK__GPIO_1_8),	/* TMS */
+		CLEAR_MODE_SION(MX25_PAD_CSI_VSYNC__GPIO_1_9),	/* TCK */
+		CLEAR_MODE_SION(MX25_PAD_CSI_HSYNC__GPIO_1_10),/* TDI */
+		/* TDO */
+		CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_CSI_PIXCLK__GPIO_1_11, PAD_CTL_PUS_100K_UP | PAD_CTL_HYS)),
+		CLEAR_MODE_SION(MX25_PAD_KPP_COL2__GPIO_3_3),	/* Program */
+		CLEAR_MODE_SION(MX25_PAD_KPP_COL3__GPIO_3_4),	/* JTAGENB */
 	};
 
+	ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	weim = (struct weim_regs *)IMX_WEIM_CTRL_BASE;
+
+	/* --- Setup of FPGA program pins --- */
+	imx_iomux_v3_setup_multiple_pads(fpga_pads, ARRAY_SIZE(fpga_pads));
+
+	gpio_direction_output(IMX_GPIO_NR(1, 8), 0);
+	gpio_direction_output(IMX_GPIO_NR(1, 9), 0);
+	gpio_direction_output(IMX_GPIO_NR(1, 10), 0);
+
+	gpio_direction_input(IMX_GPIO_NR(1, 11));
+	gpio_direction_input(IMX_GPIO_NR(3, 3));
+	gpio_direction_input(IMX_GPIO_NR(3, 4));
+
+	/* --- Clock output (33,25MHz) --- */
+	writel((1<<30) | (1<<24) | (4<<20), &ccm->mcr);
+
+	/* --- Setup of FPGA CS --- */
+	writel(WEIM_CSCR_U(0, 0, 0, 0, 0, 0, 0, 0, 3, 20, 0, 0, 0),
+		&weim->cscr5u);
+	writel(WEIM_CSCR_L(6, 12, 4, 10, 0, 1, 3, 8, 0, 0, 0, 1),
+		&weim->cscr5l);
+	writel(WEIM_CSCR_A(0, 0, 6, 12, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0),
+		&weim->cscr5a);
+}
+
+static inline void hw_setup_cpu_g292(void)
+{
+	struct ccm_regs *ccm;
+	struct weim_regs *weim;
+
+	static const iomux_v3_cfg_t uart3_pads[] = {
+		MX25_PAD_KPP_ROW0__UART3_RXD,
+		MX25_PAD_KPP_ROW1__UART3_TXD,
+		MX25_PAD_KPP_ROW2__UART3_RTS,
+		MX25_PAD_KPP_ROW3__UART3_CTS,
+	};
+
+	static const iomux_v3_cfg_t lcd_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_SS0__LD16),
+		NEW_PAD_CTRL(MX25_PAD_GPIO_F__LD17, 0),
+		/* reset active */
+		CLEAR_MODE_SION(MX25_PAD_CSI_MCLK__GPIO_1_8),
+		CLEAR_MODE_SION(MX25_PAD_UART2_RTS__GPIO_4_28),/* low power */
+		CLEAR_MODE_SION(MX25_PAD_CSI_VSYNC__GPIO_1_9),	/* on */
+	};
+
+	static const iomux_v3_cfg_t detect_touchscreen_pads[] = {
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_D8__GPIO_1_7), PAD_CTL_PUS_22K_UP),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSPI1_SCLK__GPIO_1_18), PAD_CTL_PUS_22K_UP),
+	};
+
+	static const iomux_v3_cfg_t touchscreen_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSI_HSYNC__GPIO_1_10),/* Touch Int */
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_MISO__GPIO_1_15),/* Touch SCK */
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_RDY__GPIO_2_22),/* Touch MISO */
+	};
+
+	static const iomux_v3_cfg_t touch_keys_pads[] = {
+		/* Motor */
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL1__GPIO_3_2), 0),
+		/* Reset */
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL2__GPIO_3_3), 0),
+		/* GINT */
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL3__GPIO_3_4), 0),
+	};
+
+	static const iomux_v3_cfg_t fpga_pads[] = {
+		/* TMS */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D5__GPIO_1_30),
+		/* TCK */
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL0__GPIO_3_1), 0),
+		/* TDI */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D7__GPIO_1_6),
+		/* TDO */
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_PIXCLK__GPIO_1_11), PAD_CTL_PUS_100K_UP | PAD_CTL_HYS),
+	};
+
+	static const iomux_v3_cfg_t acc_sensor_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSI_D7__GPIO_1_6),	/* INT1 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D4__GPIO_1_29),	/* INT2 */
+	};
+
+	ccm = (struct ccm_regs *)IMX_CCM_BASE;
+	weim = (struct weim_regs *)IMX_WEIM_CTRL_BASE;
+
+	/* Setup of CAN2 signals */
+	imx_iomux_v3_setup_multiple_pads(can_pads, ARRAY_SIZE(can_pads));
+
+	/* Setup of UART3 signals */
+	imx_iomux_v3_setup_multiple_pads(uart3_pads, ARRAY_SIZE(uart3_pads));
+
+	/* Setup of LCD signals */
+	imx_iomux_v3_setup_multiple_pads(lcd_pads, ARRAY_SIZE(lcd_pads));
+	gpio_direction_output(IMX_GPIO_NR(1,  8), 0);
+	gpio_direction_output(IMX_GPIO_NR(4, 28), 0);
+	gpio_direction_output(IMX_GPIO_NR(1,  9), 1);
+
+	/* --- Setup of touch display related pins --- */
+	imx_iomux_v3_setup_multiple_pads(detect_touchscreen_pads, ARRAY_SIZE(detect_touchscreen_pads));
+	gpio_direction_input(IMX_GPIO_NR(1,  7));
+	gpio_direction_input(IMX_GPIO_NR(1, 18));
+
+	udelay(5000);
+
+	/* If GPIO1[7] and GPIO1[18] are high -> touch display */
+	if ((gpio_get_value(IMX_GPIO_NR(1,  7))) &&
+	    (gpio_get_value(IMX_GPIO_NR(1, 18)))) {
+		have_touch_display = 1;
+
+		/* Set reset to output and assert reset */
+		imx_iomux_v3_setup_multiple_pads(touchscreen_pads, ARRAY_SIZE(touchscreen_pads));
+		gpio_direction_output(IMX_GPIO_NR(1, 7), 0);
+		gpio_direction_input(IMX_GPIO_NR(1, 10));
+		gpio_direction_output(IMX_GPIO_NR(1, 15), 1);
+		gpio_direction_input(IMX_GPIO_NR(2, 22));
+
+	} else {
+		/* Without touch we disable the pullups */
+		imx_iomux_v3_setup_pad(NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_D8__GPIO_1_7), 0));
+		imx_iomux_v3_setup_pad(NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSPI1_SCLK__GPIO_1_18), 0));
+	}
+
+	/* --- Setup of touch keys related pins --- */
+	imx_iomux_v3_setup_multiple_pads(touch_keys_pads, ARRAY_SIZE(touch_keys_pads));
+	gpio_direction_output(IMX_GPIO_NR(3, 2), 0);
+	gpio_direction_output(IMX_GPIO_NR(3, 3), 0);
+	gpio_direction_input(IMX_GPIO_NR(3, 4));
+
+	/* --- Setup of FPGA program pins --- */
+	imx_iomux_v3_setup_multiple_pads(fpga_pads, ARRAY_SIZE(fpga_pads));
+
+	/* --- Setup of FPGA CS --- */
+	writel(WEIM_CSCR_U(0, 0, 0, 0, 0, 0, 0, 0, 3, 20, 0, 0, 0),
+		&weim->cscr5u);
+	writel(WEIM_CSCR_L(6, 12, 4, 10, 0, 1, 3, 8, 0, 0, 0, 1),
+		&weim->cscr5l);
+	writel(WEIM_CSCR_A(0, 0, 6, 12, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0),
+		&weim->cscr5a);
+
+	/* --- Clock output (33,25MHz) --- */
+	writel((1<<30) | (1<<24) | (4<<20), &ccm->mcr);
+
+	/* --- Setup of SD card detection pin --- */
+	/* GPIO1[14] -> SD_DET */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSPI1_MOSI__GPIO_1_14));
+	gpio_direction_input(IMX_GPIO_NR(1, 14));
+
+	/* --- Setup of INT pins of acceleration sensor --- */
+	imx_iomux_v3_setup_multiple_pads(acc_sensor_pads, ARRAY_SIZE(acc_sensor_pads));
+	gpio_direction_input(IMX_GPIO_NR(1, 6));
+	gpio_direction_input(IMX_GPIO_NR(1, 29));
+
+	/* GPIO3[19] -> Used as power fail interrupt */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_POWER_FAIL__GPIO_3_19));
+	gpio_direction_input(IMX_GPIO_NR(3, 19));
+
+	io_board_type = ZMX25_IO_BOARD_TYPE_NONE;
+}
+
+static inline void hw_setup_cpu_g305(void)
+{
+	struct pwm_regs *pwm1;
+	struct pwm_regs *pwm2;
+	struct epit_regs *epit1;
+
 	static const iomux_v3_cfg_t led_pads[] = {
-		MX25_PAD_CSI_D9__GPIO_4_21,
-		MX25_PAD_CSI_D4__GPIO_1_29,
+		MX25_PAD_NFCLE__GPIO_3_29,	/* LED CAN0 */
+		MX25_PAD_NFALE__GPIO_3_28,	/* LED CAN1 */
+		MX25_PAD_NFWE_B__GPIO_3_26,	/* LED STOP */
+		MX25_PAD_NFWP_B__GPIO_3_30,	/* LED STATUS */
 	};
 
-	static const iomux_v3_cfg_t can_pads[] = {
-		NEW_PAD_CTRL(MX25_PAD_GPIO_A__CAN1_TX, NO_PAD_CTRL),
-		NEW_PAD_CTRL(MX25_PAD_GPIO_B__CAN1_RX, NO_PAD_CTRL),
-		NEW_PAD_CTRL(MX25_PAD_GPIO_C__CAN2_TX, NO_PAD_CTRL),
-		NEW_PAD_CTRL(MX25_PAD_GPIO_D__CAN2_RX, NO_PAD_CTRL),
+	static const iomux_v3_cfg_t digital_outputs_pads[] = {
+		MX25_PAD_KPP_ROW1__GPIO_2_30,	/* OUT9 */
+		MX25_PAD_KPP_ROW0__GPIO_2_29,	/* OUT8 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D2__GPIO_1_27),	/* OUT7 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D3__GPIO_1_28),	/* OUT6 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D4__GPIO_1_29),	/* OUT5 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D5__GPIO_1_30),	/* OUT4 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D6__GPIO_1_31),	/* OUT3 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D7__GPIO_1_6),	/* OUT2 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D8__GPIO_1_7),	/* OUT1 */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D9__GPIO_4_21),	/* OUT0 */
 	};
 
-	static const iomux_v3_cfg_t i2c3_pads[] = {
-		MX25_PAD_CSPI1_SS1__I2C3_DAT,
-		MX25_PAD_GPIO_E__I2C3_CLK,
+	static const iomux_v3_cfg_t digital_inputs_pads[] = {
+		MX25_PAD_UART1_RTS__GPT3_CAPIN1,/* 0 */
+		MX25_PAD_UART2_RTS__GPT1_CAPIN1,/* 1 */
+		MX25_PAD_UART1_CTS__GPIO_4_25,	/* 2 */
+		MX25_PAD_UART1_TXD__GPIO_4_23,	/* 3 */
+		MX25_PAD_UART1_RXD__GPIO_4_22,	/* 4 */
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_RDY__GPIO_2_22),	/* 5 */
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_SCLK__GPIO_1_18),/* 6 */
+		MX25_PAD_CSPI1_SS1__GPIO_1_17,	/* 7 */
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_MISO__GPIO_1_15),/* 8 */
+		CLEAR_MODE_SION(MX25_PAD_CSPI1_MOSI__GPIO_1_14),/* 9 */
+		MX25_PAD_SD1_DATA2__GPIO_2_27,	/* 10 */
+		MX25_PAD_SD1_DATA1__GPIO_2_26,	/* 11 */
+		MX25_PAD_SD1_DATA0__GPIO_2_25,	/* 14 */
+		MX25_PAD_SD1_CMD__GPIO_2_23,	/* 15 */
 	};
 
-	icache_enable();
+	pwm1 = (struct pwm_regs *)IMX_PWM1_BASE;
+	pwm2 = (struct pwm_regs *)IMX_PWM2_BASE;
+	epit1 = (struct epit_regs *)IMX_EPIT1_BASE;
 
-	/* Setup of core voltage selection pin to run at 1.4V */
-	imx_iomux_v3_setup_pad(MX25_PAD_EXT_ARMCLK__GPIO_3_15); /* VCORE */
-	gpio_direction_output(IMX_GPIO_NR(3, 15), 1);
+	bus_i2c_init((void *)IMX_I2C_BASE, 400000, 0, NULL, NULL);
 
-	/* Setup of SD card pins*/
-	imx_iomux_v3_setup_multiple_pads(sdhc1_pads, ARRAY_SIZE(sdhc1_pads));
+	/* Setup of CAN1 and CAN2 signals */
+	imx_iomux_v3_setup_multiple_pads(can_pads, ARRAY_SIZE(can_pads));
 
-	/* Setup of digital output for USB power and OC */
-	imx_iomux_v3_setup_pad(MX25_PAD_CSI_D3__GPIO_1_28); /* USB Power */
+	/* EPIT1O -> Buzzer */
+	imx_iomux_v3_setup_pad(MX25_PAD_VSTBY_ACK__EPIT1_EPITO);
+
+	/* Switch the buzzer on */
+	writel(0x01420009, &epit1->cr);
+	writel(10000, &epit1->lr);
+	writel(5000, &epit1->cmpr);
+
+	/* --- Display --- */
+	/* LCD_PWRCTRL GPIO3[15] -> low power */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_EXT_ARMCLK__GPIO_3_15));
+	gpio_direction_output(IMX_GPIO_NR(3, 15), 0);
+
+	/* GPIO3[16] */
+	/* LCD_RESET GPIO3[0] -> reset active */
+	imx_iomux_v3_setup_pad(MX25_PAD_KPP_ROW3__GPIO_3_0);
+	gpio_direction_input(IMX_GPIO_NR(3, 0));
+	udelay(1000);
+	if (gpio_get_value(IMX_GPIO_NR(3, 0)))	{
+		have_touch_display = 1;
+
+		/* --- Touch --- */
+		/* TOUCH_WAKE GPIO3[16] */
+		imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_UPLL_BYPCLK__GPIO_3_16));
+		gpio_direction_output(IMX_GPIO_NR(3, 16), 1);
+
+		/* TOUCH_INT GPIO3[27] */
+		imx_iomux_v3_setup_pad(MX25_PAD_NFRE_B__GPIO_3_27);
+		gpio_direction_input(IMX_GPIO_NR(3, 27));
+	}
+	gpio_direction_output(IMX_GPIO_NR(3, 0), 0);
+
+	/* LCD_BL_EN GPIO3[31] -> off */
+	imx_iomux_v3_setup_pad(MX25_PAD_NFRB__GPIO_3_31);
+	gpio_direction_output(IMX_GPIO_NR(3, 31), 0);
+
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(MX25_PAD_GPIO_E__LD16, 0));
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(MX25_PAD_GPIO_F__LD17, 0));
+
+	/* --- Misc --- */
+	imx_iomux_v3_setup_multiple_pads(led_pads, ARRAY_SIZE(led_pads));
+	gpio_direction_output(IMX_GPIO_NR(3, 29), 0);
+	gpio_direction_output(IMX_GPIO_NR(3, 28), 0);
+	gpio_direction_output(IMX_GPIO_NR(3, 26), 0);
+	gpio_direction_output(IMX_GPIO_NR(3, 30), 0);
+
+	/* GPIO3[19] -> POWER FAIL */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_POWER_FAIL__GPIO_3_19));
+	gpio_direction_input(IMX_GPIO_NR(3, 19));
+
+	/* --- Incremental encoder inputs --- */
+	/* GPIO1[9] -> A */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSI_VSYNC__GPIO_1_9));
+	gpio_direction_input(IMX_GPIO_NR(1, 9));
+
+	/* GPIO1[10] -> B */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSI_HSYNC__GPIO_1_10));
+	gpio_direction_input(IMX_GPIO_NR(1, 10));
+
+	/* GPIO1[11] -> T */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSI_PIXCLK__GPIO_1_11));
+	gpio_direction_input(IMX_GPIO_NR(1, 11));
+
+	/* --- Digital outputs --- */
+	imx_iomux_v3_setup_multiple_pads(digital_outputs_pads,
+					 ARRAY_SIZE(digital_outputs_pads));
+	gpio_direction_output(IMX_GPIO_NR(2, 30), 0);
+	gpio_direction_output(IMX_GPIO_NR(2, 29), 0);
+	gpio_direction_output(IMX_GPIO_NR(1, 27), 1);
+	gpio_direction_output(IMX_GPIO_NR(1, 28), 1);
+	gpio_direction_output(IMX_GPIO_NR(1, 29), 1);
+	gpio_direction_output(IMX_GPIO_NR(1, 30), 1);
+	gpio_direction_output(IMX_GPIO_NR(1, 31), 1);
+	gpio_direction_output(IMX_GPIO_NR(1, 6), 1);
+	gpio_direction_output(IMX_GPIO_NR(1, 7), 1);
+	gpio_direction_output(IMX_GPIO_NR(4, 21), 1);
+
+	/* Status input */
+	imx_iomux_v3_setup_pad(MX25_PAD_SD1_DATA3__GPIO_2_28);
+	gpio_direction_input(IMX_GPIO_NR(2, 28));
+
+	/* --- PWM outputs --- */
+	imx_iomux_v3_setup_pad(MX25_PAD_PWM__PWM);		/* PWM0 */
+	imx_iomux_v3_setup_pad(MX25_PAD_CSPI1_SS0__PWM2_PWMO);	/* PWM1 */
+	writel(0x50001, &pwm1->cr);
+	writel(0x50001, &pwm2->cr);
+
+	/* --- Digital inputs --- */
+	imx_iomux_v3_setup_multiple_pads(digital_inputs_pads, ARRAY_SIZE(digital_inputs_pads));
+	gpio_direction_input(IMX_GPIO_NR(4, 25));
+	gpio_direction_input(IMX_GPIO_NR(4, 23));
+	gpio_direction_input(IMX_GPIO_NR(4, 22));
+	gpio_direction_input(IMX_GPIO_NR(2, 22));
+	gpio_direction_input(IMX_GPIO_NR(1, 18));
+	gpio_direction_input(IMX_GPIO_NR(1, 17));
+	gpio_direction_input(IMX_GPIO_NR(1, 15));
+	gpio_direction_input(IMX_GPIO_NR(1, 14));
+	gpio_direction_input(IMX_GPIO_NR(2, 27));
+	gpio_direction_input(IMX_GPIO_NR(2, 26));
+	gpio_direction_input(IMX_GPIO_NR(2, 25));
+	gpio_direction_input(IMX_GPIO_NR(2, 23));
+
+	/* --- Analog/digital input selection --- */
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL0__GPIO_3_1), 0));
+	gpio_direction_output(IMX_GPIO_NR(3, 1), 0);
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL1__GPIO_3_2), 0));
+	gpio_direction_output(IMX_GPIO_NR(3, 2), 0);
+
+	/* --- Analog type selection --- */
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL2__GPIO_3_3), 0));
+	gpio_direction_output(IMX_GPIO_NR(3, 3), 0);
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_KPP_COL3__GPIO_3_4), 0));
+	gpio_direction_output(IMX_GPIO_NR(3, 4), 0);
+
+	/* GPIO2[24] -> 5V EN */
+	imx_iomux_v3_setup_pad(MX25_PAD_SD1_CLK__GPIO_2_24);
+	gpio_direction_output(IMX_GPIO_NR(2, 24), 0);
+
+	io_board_type = ZMX25_IO_BOARD_TYPE_NONE;
+}
+
+static inline void hw_setup_io_g283(void)
+{
+	static const iomux_v3_cfg_t dig_out_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSI_D8__GPIO_1_7),/* Output 1 Ctrl */
+		CLEAR_MODE_SION(MX25_PAD_CSI_D7__GPIO_1_6),/* Output 2 Ctrl */
+		CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_CSI_D6__GPIO_1_31, 0)),/* Output 1 Stat */
+		CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_CSI_D5__GPIO_1_30, 0)),/* Output 2 Stat */
+	};
+
+	static const iomux_v3_cfg_t led_pads[] = {
+		CLEAR_MODE_SION(MX25_PAD_CSI_D9__GPIO_4_21),
+		CLEAR_MODE_SION(MX25_PAD_CSI_D4__GPIO_1_29),
+	};
+
+	/* Setup of CAN1 and CAN2 signals */
+	imx_iomux_v3_setup_multiple_pads(can_pads, ARRAY_SIZE(can_pads));
+
+	/* Setup of digital output for USB power */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSI_D3__GPIO_1_28));
 	gpio_direction_output(IMX_GPIO_NR(1, 28), 1);
 
-	imx_iomux_v3_setup_pad(MX25_PAD_CSI_D2__GPIO_1_27); /* USB OC */
+	/* Setup of digital output for OC */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSI_D2__GPIO_1_27));
 	gpio_direction_input(IMX_GPIO_NR(1, 18));
 
 	/* Setup of digital output control pins */
@@ -91,12 +568,178 @@ int board_init()
 	/* Switch both LEDs off */
 	gpio_direction_output(IMX_GPIO_NR(4, 21), 0);
 	gpio_direction_output(IMX_GPIO_NR(1, 29), 0);
+}
 
-	/* Setup of CAN1 and CAN2 signals */
-	imx_iomux_v3_setup_multiple_pads(can_pads, ARRAY_SIZE(can_pads));
+static inline void hw_setup_io_g297(void)
+{
+	struct weim_regs *weim;
+
+	static const iomux_v3_cfg_t cspi3_pads[] = {
+		NEW_SEL_INPUT(CLEAR_MODE_SION(MX25_PAD_CSI_D2__CSPI3_MOSI), 1),
+		CLEAR_MODE_SION(MX25_PAD_CSI_D3__CSPI3_MISO),
+		NEW_SEL_INPUT(CLEAR_MODE_SION(MX25_PAD_CSI_D4__CSPI3_SCLK), 1),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_D6__CSPI3_SS0), PAD_CTL_PUS_22K_UP),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_D7__CSPI3_SS1), PAD_CTL_PUS_22K_UP),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_D8__CSPI3_SS2), PAD_CTL_PUS_22K_UP),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_CSI_D9__CSPI3_SS3), PAD_CTL_PUS_22K_UP),
+	};
 
-	/* Setup of I2C3 signals */
-	imx_iomux_v3_setup_multiple_pads(i2c3_pads, ARRAY_SIZE(i2c3_pads));
+	weim = (struct weim_regs *)IMX_WEIM_CTRL_BASE;
+
+	/* --- Setup of IO CS --- */
+	writel(WEIM_CSCR_U(0, 0, 0, 0, 0, 0, 0, 0, 3, 20, 0, 0, 0),
+	       &weim->cscr4u);
+	writel(WEIM_CSCR_L(6, 12, 4, 10, 0, 1, 3, 8, 0, 0, 0, 1),
+	       &weim->cscr4l);
+	writel(WEIM_CSCR_A(0, 0, 6, 12, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0),
+	       &weim->cscr4a);
+
+	/* GPIO1[26] -> LCD_PWR -> off */
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_PWM__GPIO_1_26));
+	gpio_direction_output(IMX_GPIO_NR(1, 26), 0);
+
+	/* Setup of CSPI3 pins */
+	imx_iomux_v3_setup_multiple_pads(cspi3_pads, ARRAY_SIZE(cspi3_pads));
+
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_CSPI1_SS0__LD16));
+	imx_iomux_v3_setup_pad(NEW_PAD_CTRL(MX25_PAD_GPIO_F__LD17, 0));
+}
+
+#ifdef CONFIG_FEC_MXC
+static inline void hw_setup_ethernet(void)
+{
+	struct ccm_regs *ccm;
+
+	/*
+	 * Set up LAN-RESET and FEC_RX_ERR
+	 *
+	 * LAN-RESET:  GPIO3[16] is ALT 5 mode of pin U20
+	 * FEC_RX_ERR: FEC_RX_ERR is ALT 2 mode of pin R2
+	 */
+	ccm = (struct ccm_regs *)IMX_CCM_BASE;
+
+	/* --- Clock output (25MHz) --- */
+	if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G305) {
+		/* UPLL output to 250MHz */
+		/*  PD=2     MFD=6      MFI=10     MFN=5 */
+		writel((1<<26) | (11<<16) | (10<<10) | (5<<0), &ccm->upctl);
+
+		/* Reset UPLL */
+		writel(readl(&ccm->cctl) | (1<<26), &ccm->cctl);
+
+		/* Wait until PLL locked */
+		while ((readl(&ccm->upctl) & (1<<15)) == 0)
+			udelay(1000);
+
+		/* Enable clock output for UPLL at DIV = 10 */
+		writel((1<<30) | (9<<24) | (6<<20), &ccm->mcr);
+	}
+
+	if ((cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A0) ||
+	    (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A1) ||
+	    (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G292))
+		imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_UPLL_BYPCLK__GPIO_3_16));
+	else
+		imx_iomux_v3_setup_pad(MX25_PAD_KPP_ROW2__GPIO_2_31);
+
+	imx_iomux_v3_setup_pad(CLEAR_MODE_SION(MX25_PAD_UART2_CTS__FEC_RX_ER));
+
+	/* assert PHY reset (low) */
+	if ((cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A0) ||
+	    (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A1) ||
+	    (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G292))
+		gpio_direction_output(IMX_GPIO_NR(3, 16), 0);
+	else
+		gpio_direction_output(IMX_GPIO_NR(2, 31), 0);
+
+	udelay(5000);
+
+	/* deassert PHY reset */
+	if ((cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A0) ||
+	    (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A1) ||
+	    (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G292))
+		gpio_set_value(IMX_GPIO_NR(3, 16), 1);
+	else
+		gpio_set_value(IMX_GPIO_NR(2, 31), 1);
+
+	udelay(5000);
+}
+#else
+static inline void hw_setup_ethernet(void){}
+#endif
+
+#ifdef CONFIG_VIDEO
+static inline void hw_setup_video(void)
+{
+	if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G305) {
+		/* LCD power on */
+		gpio_direction_output(IMX_GPIO_NR(3, 31), 1);
+		udelay(1000);
+
+		/* LCD reset off */
+		gpio_direction_output(IMX_GPIO_NR(3, 0), 1);
+		udelay(100000);
+
+		/* LCD pwr ctrl off */
+		gpio_direction_output(IMX_GPIO_NR(3, 15), 1);
+
+	} else if ((cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A1) &&
+		   (io_board_type == ZMX25_IO_BOARD_TYPE_G297)) {
+		/* LCD power on */
+		gpio_set_value(IMX_GPIO_NR(1, 26), 1);
+
+		/* Set LCD PWR CTRL and release RESET */
+		hw_g297_xfer_srio(0x0011);
+
+	} else {
+		/* LCD reset off */
+		gpio_direction_output(IMX_GPIO_NR(1, 8), 1);
+		udelay(20000);
+
+		/* LCD pwr ctrl off */
+		gpio_direction_output(IMX_GPIO_NR(4, 28), 1);
+	}
+}
+#else
+static inline void hw_setup_ethernet(void){}
+#endif
+
+
+int board_init(void)
+{
+	cpu_board_type = hw_setup_common();
+
+	/* Setup of cpu board */
+	if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G292) {
+		hw_setup_cpu_g282g292();
+		hw_setup_cpu_g292();
+		puts("zmx25: G292\n");
+	} else if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G305) {
+		hw_setup_cpu_g305();
+		puts("zmx25: G305\n");
+	} else if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A0) {
+		hw_setup_cpu_g282g292();
+		hw_setup_cpu_g282();
+		puts("zmx25: G282A0\n");
+	} else if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G282A1) {
+		hw_setup_cpu_g282g292();
+		hw_setup_cpu_g282();
+		hw_setup_cpu_g282a1();
+		puts("zmx25: G282A1\n");
+	} else if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_UNKNOWN) {
+		puts("zmx25: UNKNOWN\n");
+	}
+
+	/* Setup of io board */
+	if (io_board_type == ZMX25_IO_BOARD_TYPE_G283) {
+		hw_setup_io_g283();
+		puts("zmx25: IO->G283\n");
+	} else if (io_board_type == ZMX25_IO_BOARD_TYPE_G297) {
+		hw_setup_io_g297();
+		puts("zmx25: IO->G297\n");
+	} else if (io_board_type == ZMX25_IO_BOARD_TYPE_UNKNOWN) {
+		puts("zmx25: IO->UNKNOWN\n");
+	}
 
 	gd->bd->bi_boot_params = PHYS_SDRAM + 0x100;
 
@@ -105,32 +748,32 @@ int board_init()
 
 int board_late_init(void)
 {
+	int have_fma1125;
+	int have_mpr121;
+	int slow_boot = 0;
 	const char *e;
+	struct epit_regs *epit1;
+
+	epit1 = (struct epit_regs *)IMX_EPIT1_BASE;
+	have_fma1125 = 0;
+	have_mpr121 = 0;
 
 #ifdef CONFIG_FEC_MXC
-/*
- * FIXME: need to revisit this
- * The original code enabled PUE and 100-k pull-down without PKE, so the right
- * value here is likely:
- *	0 for no pull
- * or:
- *	PAD_CTL_PUS_100K_DOWN for 100-k pull-down
- */
 #define FEC_OUT_PAD_CTRL	0
 
 	static const iomux_v3_cfg_t fec_pads[] = {
-		MX25_PAD_FEC_TX_CLK__FEC_TX_CLK,
-		MX25_PAD_FEC_RX_DV__FEC_RX_DV,
-		MX25_PAD_FEC_RDATA0__FEC_RDATA0,
-		NEW_PAD_CTRL(MX25_PAD_FEC_TDATA0__FEC_TDATA0, FEC_OUT_PAD_CTRL),
-		NEW_PAD_CTRL(MX25_PAD_FEC_TX_EN__FEC_TX_EN, FEC_OUT_PAD_CTRL),
-		NEW_PAD_CTRL(MX25_PAD_FEC_MDC__FEC_MDC, FEC_OUT_PAD_CTRL),
-		MX25_PAD_FEC_MDIO__FEC_MDIO,
-		MX25_PAD_FEC_RDATA1__FEC_RDATA1,
-		NEW_PAD_CTRL(MX25_PAD_FEC_TDATA1__FEC_TDATA1, FEC_OUT_PAD_CTRL),
-
-		MX25_PAD_UPLL_BYPCLK__GPIO_3_16, /* LAN-RESET */
-		MX25_PAD_UART2_CTS__FEC_RX_ER, /* FEC_RX_ERR */
+		CLEAR_MODE_SION(MX25_PAD_FEC_TX_CLK__FEC_TX_CLK),
+		CLEAR_MODE_SION(MX25_PAD_FEC_RX_DV__FEC_RX_DV),
+		CLEAR_MODE_SION(MX25_PAD_FEC_RDATA0__FEC_RDATA0),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_FEC_TDATA0__FEC_TDATA0), FEC_OUT_PAD_CTRL),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_FEC_TX_EN__FEC_TX_EN), FEC_OUT_PAD_CTRL),
+		NEW_PAD_CTRL(CLEAR_MODE_SION(MX25_PAD_FEC_MDC__FEC_MDC), FEC_OUT_PAD_CTRL),
+		CLEAR_MODE_SION(MX25_PAD_FEC_MDIO__FEC_MDIO),
+		CLEAR_MODE_SION(MX25_PAD_FEC_RDATA1__FEC_RDATA1),
+		CLEAR_MODE_SION(NEW_PAD_CTRL(MX25_PAD_FEC_TDATA1__FEC_TDATA1, FEC_OUT_PAD_CTRL)),
+
+		CLEAR_MODE_SION(MX25_PAD_UPLL_BYPCLK__GPIO_3_16),	/* LAN-RESET */
+		CLEAR_MODE_SION(MX25_PAD_UART2_CTS__FEC_RX_ER),	/* FEC_RX_ERR */
 	};
 
 	imx_iomux_v3_setup_multiple_pads(fec_pads, ARRAY_SIZE(fec_pads));
@@ -146,22 +789,174 @@ int board_late_init(void)
 	udelay(5000);
 #endif
 
+	if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G292) {
+		int delay = 0;
+
+		/* Release touch display reset */
+		if (have_touch_display)
+			gpio_set_value(IMX_GPIO_NR(1, 7), 1);
+
+		/* Release touch key reset */
+		gpio_set_value(IMX_GPIO_NR(3, 3), 1);
+		udelay(10000);
+
+		i2c_set_bus_num(0);
+		i2c_init(CONFIG_SYS_I2C_SPEED, CONFIG_SYS_I2C_SLAVE);
+		udelay(10000);
+
+		if (!i2c_probe(MPR121_SA_0))
+			have_mpr121 = MPR121_SA_0;
+		else if (!i2c_probe(MPR121_SA_1))
+			have_mpr121 = MPR121_SA_1;
+
+		if (have_mpr121) {
+			printf("zmx25: MPR121 detected\n");
+			mpr121_init(have_mpr121);
+			delay = 100000;
+		} else {
+			have_fma1125 = !i2c_probe(FMA1125_SA);
+			if (have_fma1125) {
+				printf("zmx25: FMA1125 detected\n");
+				fma1125_init();
+				delay = 25000;
+			}
+		}
+
+		if (have_touch_display) {
+			if (!i2c_probe(POLYTOUCH_SA)) {
+				printf("zmx25: Touch display detected\n");
+
+				/*if (!have_mpr121 && !have_fma1125) */{
+					polytouch_init();
+					delay = 300000;
+				}
+			}
+		}
+
+		udelay(delay);
+	}
+
+	hw_setup_ethernet();
+	hw_setup_video();
+
+	/* Disable buzzer */
+	if (cpu_board_type == ZMX25_CPU_BOARD_TYPE_G305)
+		writel(0, &epit1->cr);
+
 	e = getenv("gs_base_board");
 	if (e != NULL) {
 		if (strcmp(e, "G283") == 0) {
 			int key = gpio_get_value(IMX_GPIO_NR(2, 29));
 
 			if (key) {
-				/* Switch on both LEDs to inidcate boot mode */
+				/* Switch on both LEDs to indicate boot mode */
 				gpio_set_value(IMX_GPIO_NR(1, 29), 0);
 				gpio_set_value(IMX_GPIO_NR(4, 21), 0);
 
 				setenv("preboot", "run gs_slow_boot");
 			} else
 				setenv("preboot", "run gs_fast_boot");
+		} else if (strcmp(e, "G292") == 0) {
+			int key = 0;
+
+			if (have_mpr121)
+				key = mpr121_get_touch_bits(have_mpr121) & 0xFFC;
+			if (have_fma1125)
+				key = fma1125_get_touch_bits() & 0x3C;
+			if (have_touch_display)
+				if (!gpio_get_value(IMX_GPIO_NR(1, 10)))
+					key = polytouch_is_touched_lower_left();
+
+
+			if (key) {
+				/* Switch on key LEDs to indicate boot mode */
+				if (have_mpr121) {
+					i2c_reg_write(0x62, 0x00 | 0x80, 0x00);
+
+					i2c_reg_write(0x62, 0x1C | 0x80, 0xFF);
+
+					i2c_reg_write(0x62, 0x14 | 0x80, 0xaa);
+					i2c_reg_write(0x62, 0x15 | 0x80, 0xaa);
+					i2c_reg_write(0x62, 0x16 | 0x80, 0xaa);
+					i2c_reg_write(0x62, 0x17 | 0x80, 0xaa);
+
+					i2c_reg_write(0x62, 0x02 | 0x80, 0xff);
+					i2c_reg_write(0x62, 0x06 | 0x80, 0xff);
+				}
+				if (have_fma1125)
+					fma1125_set_gpio_out(0x09);
+				slow_boot = 1;
+				setenv("stdout", "vga");
+			}
+			} else if (strcmp(e, "G297") == 0) {
+				u16 srdata;
+
+				/* Key is connected to SRIO
+				 * -> check for press
+				 */
+				srdata = hw_g297_xfer_srio(0x0011);
+				if (srdata & (1<<3)) {
+					slow_boot = 1;
+					setenv("stdout", "vga");
+				}
+			}
+	} else {
+		slow_boot = 1;
+	}
+
+	if ((cpu_board_type == ZMX25_CPU_BOARD_TYPE_G305) &&
+	    (getenv("gs_print_infos"))) {
+		const char *fw = "UNKNOWN";
+		const char *sn = "";
+		char stdout_old[32];
+		u8 buffer[256];
+		char app[33];
+		char bi1[33];
+		char bi2[33];
+
+		e = getenv("gs_firmware_info");
+		if (NULL != e)
+			fw = e;
+
+		e = getenv("gs_dev_serialno");
+		if (NULL != e)
+			sn = e;
+
+		memset(app, 0, sizeof(app));
+		memset(bi1, 0, sizeof(bi1));
+		memset(bi2, 0, sizeof(bi2));
+
+		i2c_set_bus_num(1);
+		if ((i2c_read(0x50, 8192 - 256, 2, buffer, 256) == 0) ||
+		    (i2c_read(0x50, 8192 - 256, 2, buffer, 256) == 0)) {
+			memcpy(app, buffer + 0x80, 32);
+			memcpy(bi1, buffer + 0xA0, 32);
+			memcpy(bi2, buffer + 0xC0, 32);
 		}
+
+		strcpy(stdout_old, getenv("stdout"));
+		setenv("stdout", "vga");
+
+		if (video_get_pixel_height() == 480)
+			printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n");
+		else
+			printf("\n\n\n\n\n\n\n\n\n");
+		printf(" FW  : %s\n", fw);
+		printf(" SN  : MDC %s\n", sn);
+		printf(" APP : %s\n", app);
+		printf(" BI1 : %s\n", bi1);
+		printf(" BI2 : %s\n", bi2);
+
+		setenv("stdout", stdout_old);
 	}
 
+	if (slow_boot) {
+		writel(0x55AAAA55, (void *)0x78000000);
+		setenv("preboot", "run gs_slow_boot");
+	} else {
+		writel(0xAA5555AA, (void *)0x78000000);
+		setenv("preboot", "run gs_fast_boot");
+	}
 	return 0;
 }
 
diff --git a/include/configs/zmx25.h b/include/configs/zmx25.h
index 8ffe6f1..1f63f36 100644
--- a/include/configs/zmx25.h
+++ b/include/configs/zmx25.h
@@ -15,6 +15,7 @@
 #define CONFIG_ARM926EJS			/* arm926ejs CPU core */
 #define CONFIG_MX25
 #define CONFIG_SYS_TEXT_BASE		0xA0000000
+#define CONFIG_USE_PRIVATE_LIBGCC
 
 #define CONFIG_SYS_TIMER_RATE		32768
 #define CONFIG_SYS_TIMER_COUNTER	\
@@ -51,6 +52,11 @@
 #define CONFIG_MXC_GPIO
 
 /*
+ * SPI
+ */
+#define CONFIG_MXC_SPI
+
+/*
  * Serial
  */
 #define CONFIG_MXC_UART
@@ -59,6 +65,18 @@
 #define CONFIG_BAUDRATE		115200	/* Default baud rate */
 
 /*
+ * I2C
+ */
+#define MXC_IPG_PERCLK MXC_I2C_CLK
+#define CONFIG_HARD_I2C
+#define CONFIG_SYS_I2C_MXC
+#define CONFIG_I2C_MULTI_BUS
+#define CONFIG_SYS_I2C
+#define CONFIG_SYS_I2C_BASE	IMX_I2C3_BASE
+#define CONFIG_SYS_I2C_SPEED	400000
+#define CONFIG_SYS_I2C_SLAVE	0xfe
+
+/*
  * Ethernet
  */
 #define CONFIG_FEC_MXC
@@ -66,7 +84,28 @@
 #define CONFIG_MII
 
 /*
- * BOOTP options
+  * Video
+ */
+#define CONFIG_VIDEO
+#define CONFIG_VIDEO_IMX25LCDC
+#define CONFIG_SYS_WHITE_ON_BLACK
+#define CONFIG_CFB_CONSOLE
+#define CONFIG_SYS_CONSOLE_IS_IN_ENV
+#define CONFIG_SYS_CONSOLE_ENV_OVERWRITE
+#define CONFIG_SYS_CONSOLE_BG_COL            0xff
+#define CONFIG_SYS_CONSOLE_FG_COL            0x00
+#define CONFIG_VIDEO_LOGO
+#define CONFIG_SPLASH_SCREEN
+#define CONFIG_SPLASH_SCREEN_ALIGN
+#define CONFIG_VIDEO_BMP_LOGO
+#define CONFIG_VIDEO_BMP_GZIP
+#define CONFIG_VIDEO_BMP_RLE8
+#define CONFIG_SYS_VIDEO_LOGO_MAX_SIZE  (800*480 + 256*4 + 10*1024)
+#define CONFIG_VGA_AS_SINGLE_DEVICE
+#define VIDEO_FB_16BPP_WORD_SWAP
+
+/*
+* BOOTP options
  */
 #define CONFIG_BOOTP_BOOTFILESIZE
 #define CONFIG_BOOTP_BOOTPATH
@@ -88,6 +127,11 @@
 #define CONFIG_CMD_ELF
 #define CONFIG_CMD_FAT
 #define CONFIG_CMD_USB
+#define CONFIG_CMD_I2C
+#define CONFIG_CMD_BMP
+#define CONFIG_CMD_GPIO
+#define CONFIG_CMD_SPI
+#define CONFIG_CMD_BOOTZ
 
 #define CONFIG_SYS_HUSH_PARSER
 
@@ -114,6 +158,7 @@
 
 #define CONFIG_SYS_SDRAM_BASE	PHYS_SDRAM
 #define CONFIG_SYS_INIT_SP_ADDR	0x78020000	/* end of internal SRAM */
+#define CONFIG_SYS_MEM_TOP_HIDE	(4 << 20)
 
 /*
  * FLASH and environment organization
