diff --git a/board/ads5121/Makefile b/board/ads5121/Makefile
index 5b95682..52d0d3c 100644
--- a/board/ads5121/Makefile
+++ b/board/ads5121/Makefile
@@ -27,7 +27,7 @@
 
 LIB	= $(obj)lib$(BOARD).a
 
-COBJS-y	:= $(BOARD).o iopin.o
+COBJS-y	:= $(BOARD).o
 COBJS-${CONFIG_FSL_DIU_FB} += ads5121_diu.o
 COBJS-${CONFIG_FSL_DIU_FB} += ../freescale/common/fsl_diu_fb.o
 COBJS-${CONFIG_FSL_DIU_FB} += ../freescale/common/fsl_logo_bmp.o
diff --git a/board/ads5121/ads5121.c b/board/ads5121/ads5121.c
index 8452054..ba3d7d2 100644
--- a/board/ads5121/ads5121.c
+++ b/board/ads5121/ads5121.c
@@ -23,14 +23,12 @@
 
 #include <common.h>
 #include <mpc512x.h>
-#include "iopin.h"
 #include <asm/bitops.h>
 #include <command.h>
 #include <fdt_support.h>
 #ifdef CONFIG_MISC_INIT_R
 #include <i2c.h>
 #endif
-#include "iopin.h"	/* for iopin_initialize() prototype */
 
 /* Clocks in use */
 #define SCCR1_CLOCKS_EN	(CLOCK_SCCR1_CFG_EN |				\
@@ -124,7 +122,7 @@
 	u32 i;
 
 	/* Initialize IO Control */
-	im->io_ctrl.regs[MEM_IDX] = IOCTRL_MUX_DDR;
+	im->io_ctrl.regs[IOCTL_MEM/4] = IOCTRL_MUX_DDR;
 
 	/* Initialize DDR Local Window */
 	im->sysconf.ddrlaw.bar = CFG_DDR_BASE & 0xFFFFF000;
@@ -237,6 +235,56 @@
 
 	return 0;
 }
+static  iopin_t ioregs_init[] = {
+	/* FUNC1=FEC_RX_DV Sets Next 3 to FEC pads */
+	{
+		IOCTL_SPDIF_TXCLK, 3, 0,
+		IO_PIN_FMUX(1) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
+	},
+	/* Set highest Slew on 9 PATA pins */
+	{
+		IOCTL_PATA_CE1, 9, 1,
+		IO_PIN_FMUX(0) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
+	},
+	/* FUNC1=FEC_COL Sets Next 15 to FEC pads */
+	{
+		IOCTL_PSC0_0, 15, 0,
+		IO_PIN_FMUX(1) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
+	},
+	/* FUNC1=SPDIF_TXCLK */
+	{
+		IOCTL_LPC_CS1, 1, 0,
+		IO_PIN_FMUX(1) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(1) | IO_PIN_DS(3)
+	},
+	/* FUNC2=SPDIF_TX and sets Next pin to SPDIF_RX */
+	{
+		IOCTL_I2C1_SCL, 2, 0,
+		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(1) | IO_PIN_DS(3)
+	},
+	/* FUNC2=DIU CLK */
+	{
+		IOCTL_PSC6_0, 1, 0,
+		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(1) | IO_PIN_DS(3)
+	},
+	/* FUNC2=DIU_HSYNC */
+	{
+		IOCTL_PSC6_1, 1, 0,
+		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
+	},
+	/* FUNC2=DIUVSYNC Sets Next 26 to DIU Pads */
+	{
+		IOCTL_PSC6_4, 26, 0,
+		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
+		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
+	}
+};
 
 int checkboard (void)
 {
@@ -246,7 +294,9 @@
 	printf ("Board: ADS5121 rev. 0x%04x (CPLD rev. 0x%02x)\n",
 		brd_rev, cpld_rev);
 	/* initialize function mux & slew rate IO inter alia on IO Pins  */
-	iopin_initialize();
+
+
+	iopin_initialize(ioregs_init, sizeof(ioregs_init) / sizeof(ioregs_init[0]));
 
 	return 0;
 }
diff --git a/board/ads5121/iopin.c b/board/ads5121/iopin.c
deleted file mode 100644
index a6792a0..0000000
--- a/board/ads5121/iopin.c
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
- * (C) Copyright 2008
- * Martha J Marx, Silicon Turnkey Express, mmarx@silicontkx.com
- * mpc512x I/O pin/pad initialization for the ADS5121 board
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-
-#include <common.h>
-#include <linux/types.h>
-#include "iopin.h"
-
-/* IO pin fields */
-#define IO_PIN_FMUX(v)	((v) << 7)	/* pin function */
-#define IO_PIN_HOLD(v)	((v) << 5)	/* hold time, pci only */
-#define IO_PIN_PUD(v)	((v) << 4)	/* if PUE, 0=pull-down, 1=pull-up */
-#define IO_PIN_PUE(v)	((v) << 3)	/* pull up/down enable */
-#define IO_PIN_ST(v)	((v) << 2)	/* schmitt trigger */
-#define IO_PIN_DS(v)	((v))		/* slew rate */
-
-static struct iopin_t {
-	int p_offset;		/* offset from IOCTL_MEM_OFFSET */
-	int nr_pins;		/* number of pins to set this way */
-	int bit_or;		/* or in the value instead of overwrite */
-	u_long val;		/* value to write or or */
-} ioregs_init[] = {
-	/* FUNC1=FEC_RX_DV Sets Next 3 to FEC pads */
-	{
-		IOCTL_SPDIF_TXCLK, 3, 0,
-		IO_PIN_FMUX(1) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
-	},
-	/* Set highest Slew on 9 PATA pins */
-	{
-		IOCTL_PATA_CE1, 9, 1,
-		IO_PIN_FMUX(0) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
-	},
-	/* FUNC1=FEC_COL Sets Next 15 to FEC pads */
-	{
-		IOCTL_PSC0_0, 15, 0,
-		IO_PIN_FMUX(1) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
-	},
-	/* FUNC1=SPDIF_TXCLK */
-	{
-		IOCTL_LPC_CS1, 1, 0,
-		IO_PIN_FMUX(1) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(1) | IO_PIN_DS(3)
-	},
-	/* FUNC2=SPDIF_TX and sets Next pin to SPDIF_RX */
-	{
-		IOCTL_I2C1_SCL, 2, 0,
-		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(1) | IO_PIN_DS(3)
-	},
-	/* FUNC2=DIU CLK */
-	{
-		IOCTL_PSC6_0, 1, 0,
-		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(1) | IO_PIN_DS(3)
-	},
-	/* FUNC2=DIU_HSYNC */
-	{
-		IOCTL_PSC6_1, 1, 0,
-		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
-	},
-	/* FUNC2=DIUVSYNC Sets Next 26 to DIU Pads */
-	{
-		IOCTL_PSC6_4, 26, 0,
-		IO_PIN_FMUX(2) | IO_PIN_HOLD(0) | IO_PIN_PUD(0) |
-		IO_PIN_PUE(0) | IO_PIN_ST(0) | IO_PIN_DS(3)
-	}
-};
-
-void iopin_initialize(void)
-{
-	short i, j, n, p;
-	u_long *reg;
-	immap_t *im = (immap_t *)CFG_IMMR;
-
-	reg = (u_long *)&(im->io_ctrl.regs[0]);
-
-	if (sizeof(ioregs_init) == 0)
-		return;
-
-	n = sizeof(ioregs_init) / sizeof(ioregs_init[0]);
-
-	for (i = 0; i < n; i++) {
-		for (p = 0, j = ioregs_init[i].p_offset / sizeof(u_long);
-			p < ioregs_init[i].nr_pins; p++, j++) {
-			if (ioregs_init[i].bit_or)
-				reg[j] |= ioregs_init[i].val;
-			else
-				reg[j] = ioregs_init[i].val;
-		}
-	}
-	return;
-}
diff --git a/board/ads5121/iopin.h b/board/ads5121/iopin.h
deleted file mode 100644
index 7ef8472..0000000
--- a/board/ads5121/iopin.h
+++ /dev/null
@@ -1,222 +0,0 @@
-/*
- * (C) Copyright 2008
- * Martha J Marx, Silicon Turnkey Express, mmarx@silicontkx.com
- * mpc512x I/O pin/pad initialization for the ADS5121 board
- * See file CREDITS for list of people who contributed to this
- * project.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program 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 General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
- * MA 02111-1307 USA
- */
-
-#define IOCTL_MEM		0x000
-#define IOCTL_GP		0x004
-#define IOCTL_LPC_CLK		0x008
-#define IOCTL_LPC_OE		0x00C
-#define IOCTL_LPC_RWB		0x010
-#define IOCTL_LPC_ACK		0x014
-#define IOCTL_LPC_CS0		0x018
-#define IOCTL_NFC_CE0		0x01C
-#define IOCTL_LPC_CS1		0x020
-#define IOCTL_LPC_CS2		0x024
-#define IOCTL_LPC_AX03		0x028
-#define IOCTL_EMB_AX02		0x02C
-#define IOCTL_EMB_AX01		0x030
-#define IOCTL_EMB_AX00		0x034
-#define IOCTL_EMB_AD31		0x038
-#define IOCTL_EMB_AD30		0x03C
-#define IOCTL_EMB_AD29		0x040
-#define IOCTL_EMB_AD28		0x044
-#define IOCTL_EMB_AD27		0x048
-#define IOCTL_EMB_AD26		0x04C
-#define IOCTL_EMB_AD25		0x050
-#define IOCTL_EMB_AD24		0x054
-#define IOCTL_EMB_AD23		0x058
-#define IOCTL_EMB_AD22		0x05C
-#define IOCTL_EMB_AD21		0x060
-#define IOCTL_EMB_AD20		0x064
-#define IOCTL_EMB_AD19		0x068
-#define IOCTL_EMB_AD18		0x06C
-#define IOCTL_EMB_AD17		0x070
-#define IOCTL_EMB_AD16		0x074
-#define IOCTL_EMB_AD15		0x078
-#define IOCTL_EMB_AD14		0x07C
-#define IOCTL_EMB_AD13		0x080
-#define IOCTL_EMB_AD12		0x084
-#define IOCTL_EMB_AD11		0x088
-#define IOCTL_EMB_AD10		0x08C
-#define IOCTL_EMB_AD09		0x090
-#define IOCTL_EMB_AD08		0x094
-#define IOCTL_EMB_AD07		0x098
-#define IOCTL_EMB_AD06		0x09C
-#define IOCTL_EMB_AD05		0x0A0
-#define IOCTL_EMB_AD04		0x0A4
-#define IOCTL_EMB_AD03		0x0A8
-#define IOCTL_EMB_AD02		0x0AC
-#define IOCTL_EMB_AD01		0x0B0
-#define IOCTL_EMB_AD00		0x0B4
-#define IOCTL_PATA_CE1		0x0B8
-#define IOCTL_PATA_CE2		0x0BC
-#define IOCTL_PATA_ISOLATE	0x0C0
-#define IOCTL_PATA_IOR		0x0C4
-#define IOCTL_PATA_IOW		0x0C8
-#define IOCTL_PATA_IOCHRDY	0x0CC
-#define IOCTL_PATA_INTRQ	0x0D0
-#define IOCTL_PATA_DRQ		0x0D4
-#define IOCTL_PATA_DACK		0x0D8
-#define IOCTL_NFC_WP		0x0DC
-#define IOCTL_NFC_RB		0x0E0
-#define IOCTL_NFC_ALE		0x0E4
-#define IOCTL_NFC_CLE		0x0E8
-#define IOCTL_NFC_WE		0x0EC
-#define IOCTL_NFC_RE		0x0F0
-#define IOCTL_PCI_AD31		0x0F4
-#define IOCTL_PCI_AD30		0x0F8
-#define IOCTL_PCI_AD29		0x0FC
-#define IOCTL_PCI_AD28		0x100
-#define IOCTL_PCI_AD27		0x104
-#define IOCTL_PCI_AD26		0x108
-#define IOCTL_PCI_AD25		0x10C
-#define IOCTL_PCI_AD24		0x110
-#define IOCTL_PCI_AD23		0x114
-#define IOCTL_PCI_AD22		0x118
-#define IOCTL_PCI_AD21		0x11C
-#define IOCTL_PCI_AD20		0x120
-#define IOCTL_PCI_AD19		0x124
-#define IOCTL_PCI_AD18		0x128
-#define IOCTL_PCI_AD17		0x12C
-#define IOCTL_PCI_AD16		0x130
-#define IOCTL_PCI_AD15		0x134
-#define IOCTL_PCI_AD14		0x138
-#define IOCTL_PCI_AD13		0x13C
-#define IOCTL_PCI_AD12		0x140
-#define IOCTL_PCI_AD11		0x144
-#define IOCTL_PCI_AD10		0x148
-#define IOCTL_PCI_AD09		0x14C
-#define IOCTL_PCI_AD08		0x150
-#define IOCTL_PCI_AD07		0x154
-#define IOCTL_PCI_AD06		0x158
-#define IOCTL_PCI_AD05		0x15C
-#define IOCTL_PCI_AD04		0x160
-#define IOCTL_PCI_AD03		0x164
-#define IOCTL_PCI_AD02		0x168
-#define IOCTL_PCI_AD01		0x16C
-#define IOCTL_PCI_AD00		0x170
-#define IOCTL_PCI_CBE0		0x174
-#define IOCTL_PCI_CBE1		0x178
-#define IOCTL_PCI_CBE2		0x17C
-#define IOCTL_PCI_CBE3		0x180
-#define IOCTL_PCI_GNT2		0x184
-#define IOCTL_PCI_REQ2		0x188
-#define IOCTL_PCI_GNT1		0x18C
-#define IOCTL_PCI_REQ1		0x190
-#define IOCTL_PCI_GNT0		0x194
-#define IOCTL_PCI_REQ0		0x198
-#define IOCTL_PCI_INTA		0x19C
-#define IOCTL_PCI_CLK		0x1A0
-#define IOCTL_PCI_RST_OUT	0x1A4
-#define IOCTL_PCI_FRAME		0x1A8
-#define IOCTL_PCI_IDSEL		0x1AC
-#define IOCTL_PCI_DEVSEL	0x1B0
-#define IOCTL_PCI_IRDY		0x1B4
-#define IOCTL_PCI_TRDY		0x1B8
-#define IOCTL_PCI_STOP		0x1BC
-#define IOCTL_PCI_PAR		0x1C0
-#define IOCTL_PCI_PERR		0x1C4
-#define IOCTL_PCI_SERR		0x1C8
-#define IOCTL_SPDIF_TXCLK	0x1CC
-#define IOCTL_SPDIF_TX		0x1D0
-#define IOCTL_SPDIF_RX		0x1D4
-#define IOCTL_I2C0_SCL		0x1D8
-#define IOCTL_I2C0_SDA		0x1DC
-#define IOCTL_I2C1_SCL		0x1E0
-#define IOCTL_I2C1_SDA		0x1E4
-#define IOCTL_I2C2_SCL		0x1E8
-#define IOCTL_I2C2_SDA		0x1EC
-#define IOCTL_IRQ0		0x1F0
-#define IOCTL_IRQ1		0x1F4
-#define IOCTL_CAN1_TX		0x1F8
-#define IOCTL_CAN2_TX		0x1FC
-#define IOCTL_J1850_TX		0x200
-#define IOCTL_J1850_RX		0x204
-#define IOCTL_PSC_MCLK_IN	0x208
-#define IOCTL_PSC0_0		0x20C
-#define IOCTL_PSC0_1		0x210
-#define IOCTL_PSC0_2		0x214
-#define IOCTL_PSC0_3		0x218
-#define IOCTL_PSC0_4		0x21C
-#define IOCTL_PSC1_0		0x220
-#define IOCTL_PSC1_1		0x224
-#define IOCTL_PSC1_2		0x228
-#define IOCTL_PSC1_3		0x22C
-#define IOCTL_PSC1_4		0x230
-#define IOCTL_PSC2_0		0x234
-#define IOCTL_PSC2_1		0x238
-#define IOCTL_PSC2_2		0x23C
-#define IOCTL_PSC2_3		0x240
-#define IOCTL_PSC2_4		0x244
-#define IOCTL_PSC3_0		0x248
-#define IOCTL_PSC3_1		0x24C
-#define IOCTL_PSC3_2		0x250
-#define IOCTL_PSC3_3		0x254
-#define IOCTL_PSC3_4		0x258
-#define IOCTL_PSC4_0		0x25C
-#define IOCTL_PSC4_1		0x260
-#define IOCTL_PSC4_2		0x264
-#define IOCTL_PSC4_3		0x268
-#define IOCTL_PSC4_4		0x26C
-#define IOCTL_PSC5_0		0x270
-#define IOCTL_PSC5_1		0x274
-#define IOCTL_PSC5_2		0x278
-#define IOCTL_PSC5_3		0x27C
-#define IOCTL_PSC5_4		0x280
-#define IOCTL_PSC6_0		0x284
-#define IOCTL_PSC6_1		0x288
-#define IOCTL_PSC6_2		0x28C
-#define IOCTL_PSC6_3		0x290
-#define IOCTL_PSC6_4		0x294
-#define IOCTL_PSC7_0		0x298
-#define IOCTL_PSC7_1		0x29C
-#define IOCTL_PSC7_2		0x2A0
-#define IOCTL_PSC7_3		0x2A4
-#define IOCTL_PSC7_4		0x2A8
-#define IOCTL_PSC8_0		0x2AC
-#define IOCTL_PSC8_1		0x2B0
-#define IOCTL_PSC8_2		0x2B4
-#define IOCTL_PSC8_3		0x2B8
-#define IOCTL_PSC8_4		0x2BC
-#define IOCTL_PSC9_0		0x2C0
-#define IOCTL_PSC9_1		0x2C4
-#define IOCTL_PSC9_2		0x2C8
-#define IOCTL_PSC9_3		0x2CC
-#define IOCTL_PSC9_4		0x2D0
-#define IOCTL_PSC10_0		0x2D4
-#define IOCTL_PSC10_1		0x2D8
-#define IOCTL_PSC10_2		0x2DC
-#define IOCTL_PSC10_3		0x2E0
-#define IOCTL_PSC10_4		0x2E4
-#define IOCTL_PSC11_0		0x2E8
-#define IOCTL_PSC11_1		0x2EC
-#define IOCTL_PSC11_2		0x2F0
-#define IOCTL_PSC11_3		0x2F4
-#define IOCTL_PSC11_4		0x2F8
-#define IOCTL_HRESET		0x2FC
-#define IOCTL_SRESET		0x300
-#define IOCTL_CKSTP_OUT		0x304
-#define IOCTL_USB2_VBUS_PWR_FAULT	0x308
-#define IOCTL_USB2_VBUS_PWR_SELECT	0x30C
-#define IOCTL_USB2_PHY_DRVV_BUS		0x310
-
-extern void iopin_initialize(void);
diff --git a/cpu/mpc512x/Makefile b/cpu/mpc512x/Makefile
index 2be35b2..8ba8ae8 100644
--- a/cpu/mpc512x/Makefile
+++ b/cpu/mpc512x/Makefile
@@ -25,7 +25,7 @@
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
-COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o serial.o fec.o i2c.o
+COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o serial.o fec.o i2c.o iopin.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mpc512x/iopin.c b/cpu/mpc512x/iopin.c
new file mode 100644
index 0000000..01ab34a
--- /dev/null
+++ b/cpu/mpc512x/iopin.c
@@ -0,0 +1,49 @@
+/*
+ * (C) Copyright 2008
+ * Martha J Marx, Silicon Turnkey Express, mmarx@silicontkx.com
+ * mpc512x I/O pin/pad initialization for the ADS5121 board
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program 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 General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <linux/types.h>
+#include <mpc512x.h>
+
+void iopin_initialize(iopin_t *ioregs_init, int len)
+{
+	short i, j, n, p;
+	u_long *reg;
+	immap_t *im = (immap_t *)CFG_IMMR;
+
+	reg = (u_long *)&(im->io_ctrl.regs[0]);
+
+	if (sizeof(ioregs_init) == 0)
+		return;
+
+	for (i = 0; i < len; i++) {
+		for (p = 0, j = ioregs_init[i].p_offset / sizeof(u_long);
+			p < ioregs_init[i].nr_pins; p++, j++) {
+			if (ioregs_init[i].bit_or)
+				reg[j] |= ioregs_init[i].val;
+			else
+				reg[j] = ioregs_init[i].val;
+		}
+	}
+	return;
+}
diff --git a/include/mpc512x.h b/include/mpc512x.h
index b4cc2b9..a76b1ca 100644
--- a/include/mpc512x.h
+++ b/include/mpc512x.h
@@ -347,41 +347,226 @@
 
 /* IO Control Register
  */
+#define IOCTL_MEM		0x000
+#define IOCTL_GP		0x004
+#define IOCTL_LPC_CLK		0x008
+#define IOCTL_LPC_OE		0x00C
+#define IOCTL_LPC_RWB		0x010
+#define IOCTL_LPC_ACK		0x014
+#define IOCTL_LPC_CS0		0x018
+#define IOCTL_NFC_CE0		0x01C
+#define IOCTL_LPC_CS1		0x020
+#define IOCTL_LPC_CS2		0x024
+#define IOCTL_LPC_AX03		0x028
+#define IOCTL_EMB_AX02		0x02C
+#define IOCTL_EMB_AX01		0x030
+#define IOCTL_EMB_AX00		0x034
+#define IOCTL_EMB_AD31		0x038
+#define IOCTL_EMB_AD30		0x03C
+#define IOCTL_EMB_AD29		0x040
+#define IOCTL_EMB_AD28		0x044
+#define IOCTL_EMB_AD27		0x048
+#define IOCTL_EMB_AD26		0x04C
+#define IOCTL_EMB_AD25		0x050
+#define IOCTL_EMB_AD24		0x054
+#define IOCTL_EMB_AD23		0x058
+#define IOCTL_EMB_AD22		0x05C
+#define IOCTL_EMB_AD21		0x060
+#define IOCTL_EMB_AD20		0x064
+#define IOCTL_EMB_AD19		0x068
+#define IOCTL_EMB_AD18		0x06C
+#define IOCTL_EMB_AD17		0x070
+#define IOCTL_EMB_AD16		0x074
+#define IOCTL_EMB_AD15		0x078
+#define IOCTL_EMB_AD14		0x07C
+#define IOCTL_EMB_AD13		0x080
+#define IOCTL_EMB_AD12		0x084
+#define IOCTL_EMB_AD11		0x088
+#define IOCTL_EMB_AD10		0x08C
+#define IOCTL_EMB_AD09		0x090
+#define IOCTL_EMB_AD08		0x094
+#define IOCTL_EMB_AD07		0x098
+#define IOCTL_EMB_AD06		0x09C
+#define IOCTL_EMB_AD05		0x0A0
+#define IOCTL_EMB_AD04		0x0A4
+#define IOCTL_EMB_AD03		0x0A8
+#define IOCTL_EMB_AD02		0x0AC
+#define IOCTL_EMB_AD01		0x0B0
+#define IOCTL_EMB_AD00		0x0B4
+#define IOCTL_PATA_CE1		0x0B8
+#define IOCTL_PATA_CE2		0x0BC
+#define IOCTL_PATA_ISOLATE	0x0C0
+#define IOCTL_PATA_IOR		0x0C4
+#define IOCTL_PATA_IOW		0x0C8
+#define IOCTL_PATA_IOCHRDY	0x0CC
+#define IOCTL_PATA_INTRQ	0x0D0
+#define IOCTL_PATA_DRQ		0x0D4
+#define IOCTL_PATA_DACK		0x0D8
+#define IOCTL_NFC_WP		0x0DC
+#define IOCTL_NFC_RB		0x0E0
+#define IOCTL_NFC_ALE		0x0E4
+#define IOCTL_NFC_CLE		0x0E8
+#define IOCTL_NFC_WE		0x0EC
+#define IOCTL_NFC_RE		0x0F0
+#define IOCTL_PCI_AD31		0x0F4
+#define IOCTL_PCI_AD30		0x0F8
+#define IOCTL_PCI_AD29		0x0FC
+#define IOCTL_PCI_AD28		0x100
+#define IOCTL_PCI_AD27		0x104
+#define IOCTL_PCI_AD26		0x108
+#define IOCTL_PCI_AD25		0x10C
+#define IOCTL_PCI_AD24		0x110
+#define IOCTL_PCI_AD23		0x114
+#define IOCTL_PCI_AD22		0x118
+#define IOCTL_PCI_AD21		0x11C
+#define IOCTL_PCI_AD20		0x120
+#define IOCTL_PCI_AD19		0x124
+#define IOCTL_PCI_AD18		0x128
+#define IOCTL_PCI_AD17		0x12C
+#define IOCTL_PCI_AD16		0x130
+#define IOCTL_PCI_AD15		0x134
+#define IOCTL_PCI_AD14		0x138
+#define IOCTL_PCI_AD13		0x13C
+#define IOCTL_PCI_AD12		0x140
+#define IOCTL_PCI_AD11		0x144
+#define IOCTL_PCI_AD10		0x148
+#define IOCTL_PCI_AD09		0x14C
+#define IOCTL_PCI_AD08		0x150
+#define IOCTL_PCI_AD07		0x154
+#define IOCTL_PCI_AD06		0x158
+#define IOCTL_PCI_AD05		0x15C
+#define IOCTL_PCI_AD04		0x160
+#define IOCTL_PCI_AD03		0x164
+#define IOCTL_PCI_AD02		0x168
+#define IOCTL_PCI_AD01		0x16C
+#define IOCTL_PCI_AD00		0x170
+#define IOCTL_PCI_CBE0		0x174
+#define IOCTL_PCI_CBE1		0x178
+#define IOCTL_PCI_CBE2		0x17C
+#define IOCTL_PCI_CBE3		0x180
+#define IOCTL_PCI_GNT2		0x184
+#define IOCTL_PCI_REQ2		0x188
+#define IOCTL_PCI_GNT1		0x18C
+#define IOCTL_PCI_REQ1		0x190
+#define IOCTL_PCI_GNT0		0x194
+#define IOCTL_PCI_REQ0		0x198
+#define IOCTL_PCI_INTA		0x19C
+#define IOCTL_PCI_CLK		0x1A0
+#define IOCTL_PCI_RST_OUT	0x1A4
+#define IOCTL_PCI_FRAME		0x1A8
+#define IOCTL_PCI_IDSEL		0x1AC
+#define IOCTL_PCI_DEVSEL	0x1B0
+#define IOCTL_PCI_IRDY		0x1B4
+#define IOCTL_PCI_TRDY		0x1B8
+#define IOCTL_PCI_STOP		0x1BC
+#define IOCTL_PCI_PAR		0x1C0
+#define IOCTL_PCI_PERR		0x1C4
+#define IOCTL_PCI_SERR		0x1C8
+#define IOCTL_SPDIF_TXCLK	0x1CC
+#define IOCTL_SPDIF_TX		0x1D0
+#define IOCTL_SPDIF_RX		0x1D4
+#define IOCTL_I2C0_SCL		0x1D8
+#define IOCTL_I2C0_SDA		0x1DC
+#define IOCTL_I2C1_SCL		0x1E0
+#define IOCTL_I2C1_SDA		0x1E4
+#define IOCTL_I2C2_SCL		0x1E8
+#define IOCTL_I2C2_SDA		0x1EC
+#define IOCTL_IRQ0		0x1F0
+#define IOCTL_IRQ1		0x1F4
+#define IOCTL_CAN1_TX		0x1F8
+#define IOCTL_CAN2_TX		0x1FC
+#define IOCTL_J1850_TX		0x200
+#define IOCTL_J1850_RX		0x204
+#define IOCTL_PSC_MCLK_IN	0x208
+#define IOCTL_PSC0_0		0x20C
+#define IOCTL_PSC0_1		0x210
+#define IOCTL_PSC0_2		0x214
+#define IOCTL_PSC0_3		0x218
+#define IOCTL_PSC0_4		0x21C
+#define IOCTL_PSC1_0		0x220
+#define IOCTL_PSC1_1		0x224
+#define IOCTL_PSC1_2		0x228
+#define IOCTL_PSC1_3		0x22C
+#define IOCTL_PSC1_4		0x230
+#define IOCTL_PSC2_0		0x234
+#define IOCTL_PSC2_1		0x238
+#define IOCTL_PSC2_2		0x23C
+#define IOCTL_PSC2_3		0x240
+#define IOCTL_PSC2_4		0x244
+#define IOCTL_PSC3_0		0x248
+#define IOCTL_PSC3_1		0x24C
+#define IOCTL_PSC3_2		0x250
+#define IOCTL_PSC3_3		0x254
+#define IOCTL_PSC3_4		0x258
+#define IOCTL_PSC4_0		0x25C
+#define IOCTL_PSC4_1		0x260
+#define IOCTL_PSC4_2		0x264
+#define IOCTL_PSC4_3		0x268
+#define IOCTL_PSC4_4		0x26C
+#define IOCTL_PSC5_0		0x270
+#define IOCTL_PSC5_1		0x274
+#define IOCTL_PSC5_2		0x278
+#define IOCTL_PSC5_3		0x27C
+#define IOCTL_PSC5_4		0x280
+#define IOCTL_PSC6_0		0x284
+#define IOCTL_PSC6_1		0x288
+#define IOCTL_PSC6_2		0x28C
+#define IOCTL_PSC6_3		0x290
+#define IOCTL_PSC6_4		0x294
+#define IOCTL_PSC7_0		0x298
+#define IOCTL_PSC7_1		0x29C
+#define IOCTL_PSC7_2		0x2A0
+#define IOCTL_PSC7_3		0x2A4
+#define IOCTL_PSC7_4		0x2A8
+#define IOCTL_PSC8_0		0x2AC
+#define IOCTL_PSC8_1		0x2B0
+#define IOCTL_PSC8_2		0x2B4
+#define IOCTL_PSC8_3		0x2B8
+#define IOCTL_PSC8_4		0x2BC
+#define IOCTL_PSC9_0		0x2C0
+#define IOCTL_PSC9_1		0x2C4
+#define IOCTL_PSC9_2		0x2C8
+#define IOCTL_PSC9_3		0x2CC
+#define IOCTL_PSC9_4		0x2D0
+#define IOCTL_PSC10_0		0x2D4
+#define IOCTL_PSC10_1		0x2D8
+#define IOCTL_PSC10_2		0x2DC
+#define IOCTL_PSC10_3		0x2E0
+#define IOCTL_PSC10_4		0x2E4
+#define IOCTL_PSC11_0		0x2E8
+#define IOCTL_PSC11_1		0x2EC
+#define IOCTL_PSC11_2		0x2F0
+#define IOCTL_PSC11_3		0x2F4
+#define IOCTL_PSC11_4		0x2F8
+#define IOCTL_HRESET		0x2FC
+#define IOCTL_SRESET		0x300
+#define IOCTL_CKSTP_OUT		0x304
+#define IOCTL_USB2_VBUS_PWR_FAULT	0x308
+#define IOCTL_USB2_VBUS_PWR_SELECT	0x30C
+#define IOCTL_USB2_PHY_DRVV_BUS		0x310
+
+#ifndef __ASSEMBLY__
+
+
+/* IO pin fields */
+#define IO_PIN_FMUX(v)	((v) << 7)	/* pin function */
+#define IO_PIN_HOLD(v)	((v) << 5)	/* hold time, pci only */
+#define IO_PIN_PUD(v)	((v) << 4)	/* if PUE, 0=pull-down, 1=pull-up */
+#define IO_PIN_PUE(v)	((v) << 3)	/* pull up/down enable */
+#define IO_PIN_ST(v)	((v) << 2)	/* schmitt trigger */
+#define IO_PIN_DS(v)	((v))		/* slew rate */
+
+typedef struct iopin_t {
+	int p_offset;		/* offset from IOCTL_MEM_OFFSET */
+	int nr_pins;		/* number of pins to set this way */
+	int bit_or;		/* or in the value instead of overwrite */
+	u_long val;		/* value to write or or */
+}iopin_t;
+
+void iopin_initialize(iopin_t *,int);
+#endif
 
 /* Indexes in regs array */
-#define MEM_IDX			0x00
-#define PATA_CE1_IDX		0x2e
-#define PATA_CE2_IDX		0x2f
-#define PATA_ISOLATE_IDX	0x30
-#define PATA_IOR_IDX		0x31
-#define PATA_IOW_IDX		0x32
-#define PATA_IOCHRDY_IDX	0x33
-#define PATA_INTRQ_IDX		0x34
-#define PATA_DRQ_IDX		0x35
-#define PATA_DACK_IDX		0x36
-#define SPDIF_TXCLOCK_IDX	0x73
-#define SPDIF_TX_IDX		0x74
-#define SPDIF_RX_IDX		0x75
-#define PSC0_0_IDX		0x83
-#define PSC0_1_IDX		0x84
-#define PSC0_2_IDX		0x85
-#define PSC0_3_IDX		0x86
-#define PSC0_4_IDX		0x87
-#define PSC1_0_IDX		0x88
-#define PSC1_1_IDX		0x89
-#define PSC1_2_IDX		0x8a
-#define PSC1_3_IDX		0x8b
-#define PSC1_4_IDX		0x8c
-#define PSC2_0_IDX		0x8d
-#define PSC2_1_IDX		0x8e
-#define PSC2_2_IDX		0x8f
-#define PSC2_3_IDX		0x90
-#define PSC2_4_IDX		0x91
-
-#define IOCTRL_FUNCMUX_SHIFT	7
-#define IOCTRL_FUNCMUX_FEC	1
-#define IOCTRL_MUX_FEC		(IOCTRL_FUNCMUX_FEC << IOCTRL_FUNCMUX_SHIFT)
-
 /* Set for DDR */
 #define IOCTRL_MUX_DDR		0x00000036