]> git.kernelconcepts.de Git - karo-tx-uboot.git/blobdiff - board/karo/tx28/tx28.c
mxs_gpio: correctly use the GPIO API
[karo-tx-uboot.git] / board / karo / tx28 / tx28.c
index 48af1246a4c9d2f86e3a83eac2482b8ac6566636..b7ece9272e330ccf2df69e4927b5d05125262ca4 100644 (file)
@@ -1,14 +1,13 @@
 /*
- * Copyright (C) 2011 Lothar Waßmann <LW@KARO-electronics.de>
- * based on: board/freesclae/mx28_evk.c (C) 2010 Freescale Semiconductor, Inc.
+ * Copyright (C) 2011-2013 Lothar Waßmann <LW@KARO-electronics.de>
+ * based on: board/freescale/mx28_evk.c (C) 2010 Freescale Semiconductor, Inc.
  *
  * 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.
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
  *
  * This program is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 #include <lcd.h>
 #include <netdev.h>
 #include <mmc.h>
+#include <mxcfb.h>
+#include <video_fb.h>
 #include <linux/list.h>
 #include <linux/fb.h>
 #include <asm/io.h>
 #include <asm/gpio.h>
+#include <asm/arch/iomux.h>
 #include <asm/arch/iomux-mx28.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/imx-regs.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
-#define MXS_GPIO_NR(p, o)      (((p) << 5) | (o))
+#define TX28_LCD_PWR_GPIO      MXS_PAD_TO_GPIO(MX28_PAD_LCD_ENABLE__GPIO_1_31)
+#define TX28_LCD_RST_GPIO      MXS_PAD_TO_GPIO(MX28_PAD_LCD_RESET__GPIO_3_30)
+#define TX28_LCD_BACKLIGHT_GPIO        MXS_PAD_TO_GPIO(MX28_PAD_PWM0__GPIO_3_16)
 
-#define TX28_LCD_PWR_GPIO      MX28_PAD_LCD_ENABLE__GPIO_1_31
-#define TX28_LCD_RST_GPIO      MX28_PAD_LCD_RESET__GPIO_3_30
-#define TX28_LCD_BACKLIGHT_GPIO        MX28_PAD_PWM0__GPIO_3_16
+#define TX28_USBH_VBUSEN_GPIO  MXS_PAD_TO_GPIO(MX28_PAD_SPDIF__GPIO_3_27)
+#define TX28_USBH_OC_GPIO      MXS_PAD_TO_GPIO(MX28_PAD_JTAG_RTCK__GPIO_4_20)
+#define TX28_USBOTG_VBUSEN_GPIO        MXS_PAD_TO_GPIO(MX28_PAD_GPMI_CE2N__GPIO_0_18)
+#define TX28_USBOTG_OC_GPIO    MXS_PAD_TO_GPIO(MX28_PAD_GPMI_CE3N__GPIO_0_19)
+#define TX28_USBOTG_ID_GPIO    MXS_PAD_TO_GPIO(MX28_PAD_PWM2__GPIO_3_18)
 
-#define TX28_USBH_VBUSEN_GPIO  MX28_PAD_SPDIF__GPIO_3_27
-#define TX28_USBH_OC_GPIO      MX28_PAD_JTAG_RTCK__GPIO_4_20
-#define TX28_USBOTG_VBUSEN_GPIO        MX28_PAD_GPMI_CE2N__GPIO_0_18
-#define TX28_USBOTG_OC_GPIO    MX28_PAD_GPMI_CE3N__GPIO_0_19
-#define TX28_USBOTG_ID_GPIO    MX28_PAD_PWM2__GPIO_3_18
+#define TX28_LED_GPIO          MXS_PAD_TO_GPIO(MX28_PAD_ENET0_RXD3__GPIO_4_10)
 
-#define TX28_LED_GPIO          MX28_PAD_ENET0_RXD3__GPIO_4_10
+#define STK5_CAN_XCVR_PAD      MX28_PAD_LCD_D00__GPIO_1_0
+#define STK5_CAN_XCVR_GPIO     MXS_PAD_TO_GPIO(STK5_CAN_XCVR_PAD)
+
+#define ENET_PAD_CTRL          (MXS_PAD_3V3 | MXS_PAD_4MA | MXS_PAD_PULLUP)
+#define GPIO_PAD_CTRL          (MXS_PAD_3V3 | MXS_PAD_4MA | MXS_PAD_PULLUP)
+#define I2C_PAD_CTRL           (MXS_PAD_3V3 | MXS_PAD_12MA | MXS_PAD_PULLUP)
+
+#ifndef CONFIG_CONS_INDEX
+struct serial_device *default_serial_console(void)
+{
+       return NULL;
+}
+#endif
 
 static const struct gpio tx28_gpios[] = {
-       { TX28_USBH_VBUSEN_GPIO, GPIOF_OUTPUT_INIT_LOW, "USBH VBUSEN", },
-       { TX28_USBH_OC_GPIO, GPIOF_INPUT, "USBH OC", },
-       { TX28_USBOTG_VBUSEN_GPIO, GPIOF_OUTPUT_INIT_LOW, "USBOTG VBUSEN", },
-       { TX28_USBOTG_OC_GPIO, GPIOF_INPUT, "USBOTG OC", },
-       { TX28_USBOTG_ID_GPIO, GPIOF_INPUT, "USBOTG ID", },
+       { TX28_USBH_VBUSEN_GPIO, GPIOFLAG_OUTPUT_INIT_LOW, "USBH VBUSEN", },
+       { TX28_USBH_OC_GPIO, GPIOFLAG_INPUT, "USBH OC", },
+       { TX28_USBOTG_VBUSEN_GPIO, GPIOFLAG_OUTPUT_INIT_LOW, "USBOTG VBUSEN", },
+       { TX28_USBOTG_OC_GPIO, GPIOFLAG_INPUT, "USBOTG OC", },
+       { TX28_USBOTG_ID_GPIO, GPIOFLAG_INPUT, "USBOTG ID", },
 };
 
 static const iomux_cfg_t tx28_pads[] = {
-       /* UART pads */
-#if CONFIG_CONS_INDEX == 0
-       MX28_PAD_AUART0_RX__DUART_CTS,
-       MX28_PAD_AUART0_TX__DUART_RTS,
-       MX28_PAD_AUART0_CTS__DUART_RX,
-       MX28_PAD_AUART0_RTS__DUART_TX,
-#elif CONFIG_CONS_INDEX == 1
-       MX28_PAD_AUART1_RX__AUART1_RX,
-       MX28_PAD_AUART1_TX__AUART1_TX,
-       MX28_PAD_AUART1_CTS__AUART1_CTS,
-       MX28_PAD_AUART1_RTS__AUART1_RTS,
-#elif CONFIG_CONS_INDEX == 2
-       MX28_PAD_AUART3_RX__AUART3_RX,
-       MX28_PAD_AUART3_TX__AUART3_TX,
-       MX28_PAD_AUART3_CTS__AUART3_CTS,
-       MX28_PAD_AUART3_RTS__AUART3_RTS,
-#endif
        /* I2C bus for internal DS1339, PCA9554 and on DIMM pins 40/41 */
-       MX28_PAD_I2C0_SCL__I2C0_SCL,
-       MX28_PAD_I2C0_SDA__I2C0_SDA,
+       MX28_PAD_I2C0_SCL__I2C0_SCL | I2C_PAD_CTRL,
+       MX28_PAD_I2C0_SDA__I2C0_SDA | I2C_PAD_CTRL,
 
        /* USBH VBUSEN, OC */
        MX28_PAD_SPDIF__GPIO_3_27,
@@ -98,7 +95,7 @@ static const iomux_cfg_t tx28_pads[] = {
 /* provide at least _some_ sort of randomness */
 #define MAX_LOOPS       100
 
-static u32 random;
+static u32 random __attribute__((section("data")));
 
 static inline void random_init(void)
 {
@@ -107,9 +104,11 @@ static inline void random_init(void)
        int i;
 
        for (i = 0; i < MAX_LOOPS; i++) {
-               unsigned int usec = readl(&digctl_regs->hw_digctl_microseconds);
+               u32 hclk = readl(&digctl_regs->hw_digctl_hclkcount);
+               u32 entropy = readl(&digctl_regs->hw_digctl_entropy);
+               u32 usec = readl(&digctl_regs->hw_digctl_microseconds);
 
-               seed = get_timer(usec + random + seed);
+               seed = get_timer(hclk ^ entropy ^ usec ^ random ^ seed);
                srand(seed);
                random = rand();
        }
@@ -143,7 +142,7 @@ int board_early_init_f(void)
        while ((rtc_stat = readl(&rtc_regs->hw_rtc_stat)) &
                RTC_STAT_STALE_REGS_PERSISTENT0) {
                if (timeout-- < 0)
-                       return 0;
+                       return 1;
                udelay(1);
        }
        boot_cause = readl(&rtc_regs->hw_rtc_persistent0);
@@ -158,11 +157,14 @@ int board_early_init_f(void)
 
 rtc_err:
        serial_puts("Inconsistent value in RTC_PERSISTENT0 register; power-on-reset required\n");
-       return 0;
+       return 1;
 }
 
 int board_init(void)
 {
+       if (ctrlc())
+               printf("CTRL-C detected; safeboot enabled\n");
+
        /* Address of boot parameters */
 #ifdef CONFIG_OF_LIBFDT
        gd->bd->bi_arch_number = -1;
@@ -191,7 +193,7 @@ int board_mmc_init(bd_t *bis)
 #ifdef CONFIG_FEC_MXC
 #ifdef CONFIG_GET_FEC_MAC_ADDR_FROM_IIM
 
-#ifdef CONFIG_FEC_MXC_MULTI
+#ifndef CONFIG_TX28_S
 #define FEC_MAX_IDX                    1
 #else
 #define FEC_MAX_IDX                    0
@@ -232,8 +234,11 @@ static int fec_get_mac_addr(int index)
                        val = readl(&cust[index * 8 + i]);
                mac[i] = val >> shift;
        }
-       if (!is_valid_ether_addr(mac))
+       if (!is_valid_ethaddr(mac)) {
+               if (index == 0)
+                       printf("No valid MAC address programmed\n");
                return 0;
+       }
 
        if (index == 0) {
                printf("MAC addr from fuse: %pM\n", mac);
@@ -244,12 +249,72 @@ static int fec_get_mac_addr(int index)
        eth_setenv_enetaddr(env_name, mac);
        return 0;
 }
+
+static inline int tx28_fec1_enabled(void)
+{
+       const char *status;
+       int off;
+
+       if (!gd->fdt_blob)
+               return 0;
+
+       off = fdt_path_offset(gd->fdt_blob, "ethernet1");
+       if (off < 0)
+               return 0;
+
+       status = fdt_getprop(gd->fdt_blob, off, "status", NULL);
+       return status && (strcmp(status, "okay") == 0);
+}
+
+static void tx28_init_mac(void)
+{
+       int ret;
+
+       ret = fec_get_mac_addr(0);
+       if (ret < 0) {
+               printf("Failed to read FEC0 MAC address from OCOTP\n");
+               return;
+       }
+#ifdef CONFIG_TX28_S
+       if (tx28_fec1_enabled()) {
+               ret = fec_get_mac_addr(1);
+               if (ret < 0) {
+                       printf("Failed to read FEC1 MAC address from OCOTP\n");
+                       return;
+               }
+       }
+#endif
+}
+#else
+static inline void tx28_init_mac(void)
+{
+}
 #endif /* CONFIG_GET_FEC_MAC_ADDR_FROM_IIM */
 
 static const iomux_cfg_t tx28_fec_pads[] = {
-       MX28_PAD_ENET0_RX_EN__ENET0_RX_EN,
-       MX28_PAD_ENET0_RXD0__ENET0_RXD0,
-       MX28_PAD_ENET0_RXD1__ENET0_RXD1,
+       MX28_PAD_ENET0_RX_EN__ENET0_RX_EN | ENET_PAD_CTRL,
+       MX28_PAD_ENET0_RXD0__ENET0_RXD0 | ENET_PAD_CTRL,
+       MX28_PAD_ENET0_RXD1__ENET0_RXD1 | ENET_PAD_CTRL,
+};
+
+static struct gpio tx28_fec_strap_gpios[] = {
+       /* first entry must be RESET pin */
+       { MXS_PAD_TO_GPIO(MX28_PAD_ENET0_RX_CLK__GPIO_4_13),
+         GPIOFLAG_OUTPUT_INIT_LOW, "PHY Reset", },
+
+       { MXS_PAD_TO_GPIO(MX28_PAD_PWM4__GPIO_3_29),
+         GPIOFLAG_OUTPUT_INIT_HIGH, "PHY Power", },
+
+       /* Pull strap pins to high */
+       { MXS_PAD_TO_GPIO(MX28_PAD_ENET0_RX_EN__GPIO_4_2),
+         GPIOFLAG_OUTPUT_INIT_HIGH, "PHY Mode0", },
+       { MXS_PAD_TO_GPIO(MX28_PAD_ENET0_RXD0__GPIO_4_3),
+         GPIOFLAG_OUTPUT_INIT_HIGH, "PHY Mode1", },
+       { MXS_PAD_TO_GPIO(MX28_PAD_ENET0_RXD1__GPIO_4_4),
+         GPIOFLAG_OUTPUT_INIT_HIGH, "PHY Mode2", },
+
+       { MXS_PAD_TO_GPIO(MX28_PAD_ENET0_TX_CLK__GPIO_4_5),
+         GPIOFLAG_INPUT, "PHY INT", },
 };
 
 int board_eth_init(bd_t *bis)
@@ -257,19 +322,13 @@ int board_eth_init(bd_t *bis)
        int ret;
 
        /* Reset the external phy */
-       gpio_direction_output(MX28_PAD_ENET0_RX_CLK__GPIO_4_13, 0);
-
-       /* Power on the external phy */
-       gpio_direction_output(MX28_PAD_PWM4__GPIO_3_29, 1);
-
-       /* Pull strap pins to high */
-       gpio_direction_output(MX28_PAD_ENET0_RX_EN__GPIO_4_2, 1);
-       gpio_direction_output(MX28_PAD_ENET0_RXD0__GPIO_4_3, 1);
-       gpio_direction_output(MX28_PAD_ENET0_RXD1__GPIO_4_4, 1);
-       gpio_direction_input(MX28_PAD_ENET0_TX_CLK__GPIO_4_5);
+       ret = gpio_request_array(tx28_fec_strap_gpios,
+                               ARRAY_SIZE(tx28_fec_strap_gpios));
+       if (ret)
+               printf("Failed to request FEC GPIOs: %d\n", ret);
 
        udelay(25000);
-       gpio_set_value(MX28_PAD_ENET0_RX_CLK__GPIO_4_13, 1);
+       gpio_set_value(tx28_fec_strap_gpios[0].gpio, 1);
        udelay(100);
 
        mxs_iomux_setup_multiple_pads(tx28_fec_pads, ARRAY_SIZE(tx28_fec_pads));
@@ -280,7 +339,7 @@ int board_eth_init(bd_t *bis)
                return ret;
        }
 
-#ifdef CONFIG_FEC_MXC_MULTI
+#ifndef CONFIG_TX28_S
        if (getenv("ethaddr")) {
                ret = fecmxc_initialize_multi(bis, 0, 0, MXS_ENET0_BASE);
                if (ret) {
@@ -307,53 +366,65 @@ int board_eth_init(bd_t *bis)
 #endif
        return 0;
 }
+#else
+static inline void tx28_init_mac(void)
+{
+}
 #endif /* CONFIG_FEC_MXC */
 
 enum {
        LED_STATE_INIT = -1,
        LED_STATE_OFF,
        LED_STATE_ON,
+       LED_STATE_DISABLED,
 };
 
+static int led_state = LED_STATE_DISABLED;
+
 void show_activity(int arg)
 {
-       static int led_state = LED_STATE_INIT;
        static ulong last;
+       int ret;
 
-       if (led_state == LED_STATE_INIT) {
+       if (led_state == LED_STATE_DISABLED) {
+               return;
+       } else if (led_state == LED_STATE_INIT) {
                last = get_timer(0);
-               gpio_set_value(TX28_LED_GPIO, 1);
-               led_state = LED_STATE_ON;
+               ret = gpio_request_one(TX28_LED_GPIO,
+                               GPIOFLAG_OUTPUT_INIT_HIGH, "Activity");
+               if (ret == 0)
+                       led_state = LED_STATE_ON;
+               else
+                       led_state = LED_STATE_DISABLED;
        } else {
                if (get_timer(last) > CONFIG_SYS_HZ) {
                        last = get_timer(0);
                        if (led_state == LED_STATE_ON) {
                                gpio_set_value(TX28_LED_GPIO, 0);
+                               led_state = LED_STATE_OFF;
                        } else {
                                gpio_set_value(TX28_LED_GPIO, 1);
+                               led_state = LED_STATE_ON;
                        }
-                       led_state = 1 - led_state;
                }
        }
 }
 
 static const iomux_cfg_t stk5_pads[] = {
        /* SW controlled LED on STK5 baseboard */
-       MX28_PAD_ENET0_RXD3__GPIO_4_10,
+       MX28_PAD_ENET0_RXD3__GPIO_4_10 | GPIO_PAD_CTRL,
 };
 
 static const struct gpio stk5_gpios[] = {
 };
 
 #ifdef CONFIG_LCD
-static ushort tx28_cmap[256];
 vidinfo_t panel_info = {
        /* set to max. size supported by SoC */
        .vl_col = 1600,
        .vl_row = 1200,
 
-       .vl_bpix = LCD_COLOR24,    /* Bits per pixel, 0: 1bpp, 1: 2bpp, 2: 4bpp, 3: 8bpp ... */
-       .cmap = tx28_cmap,
+       .vl_bpix = LCD_COLOR32,    /* Bits per pixel, 0: 1bpp, 1: 2bpp, 2: 4bpp, 3: 8bpp ... */
 };
 
 static struct fb_videomode tx28_fb_modes[] = {
@@ -422,6 +493,7 @@ static struct fb_videomode tx28_fb_modes[] = {
                .upper_margin   = 2,
                .vsync_len      = 10,
                .lower_margin   = 2,
+               .sync           = FB_SYNC_CLK_LAT_FALL,
                .vmode          = FB_VMODE_NONINTERLACED,
        },
        {
@@ -482,6 +554,12 @@ static struct fb_videomode tx28_fb_modes[] = {
 };
 
 static int lcd_enabled = 1;
+static int lcd_bl_polarity;
+
+static int lcd_backlight_polarity(void)
+{
+       return lcd_bl_polarity;
+}
 
 void lcd_enable(void)
 {
@@ -499,7 +577,8 @@ void lcd_enable(void)
                udelay(100);
                gpio_set_value(TX28_LCD_RST_GPIO, 1);
                udelay(300000);
-               gpio_set_value(TX28_LCD_BACKLIGHT_GPIO, 0);
+               gpio_set_value(TX28_LCD_BACKLIGHT_GPIO,
+                       lcd_backlight_polarity());
        }
 }
 
@@ -511,7 +590,8 @@ void lcd_panel_disable(void)
 {
        if (lcd_enabled) {
                debug("Switching LCD off\n");
-               gpio_set_value(TX28_LCD_BACKLIGHT_GPIO, 1);
+               gpio_set_value(TX28_LCD_BACKLIGHT_GPIO,
+                       !lcd_backlight_polarity());
                gpio_set_value(TX28_LCD_RST_GPIO, 0);
                gpio_set_value(TX28_LCD_PWR_GPIO, 0);
        }
@@ -554,23 +634,19 @@ static const iomux_cfg_t stk5_lcd_pads[] = {
        MX28_PAD_LCD_WR_RWN__LCD_HSYNC | MXS_PAD_CTRL,
        MX28_PAD_LCD_RS__LCD_DOTCLK | MXS_PAD_CTRL,
        MX28_PAD_LCD_CS__LCD_CS | MXS_PAD_CTRL,
-       MX28_PAD_LCD_VSYNC__LCD_VSYNC | MXS_PAD_CTRL,
-       MX28_PAD_LCD_HSYNC__LCD_HSYNC | MXS_PAD_CTRL,
-       MX28_PAD_LCD_DOTCLK__LCD_DOTCLK | MXS_PAD_CTRL,
 };
 
 static const struct gpio stk5_lcd_gpios[] = {
-       { TX28_LCD_RST_GPIO, GPIOF_OUTPUT_INIT_LOW, "LCD RESET", },
-       { TX28_LCD_PWR_GPIO, GPIOF_OUTPUT_INIT_LOW, "LCD POWER", },
-       { TX28_LCD_BACKLIGHT_GPIO, GPIOF_OUTPUT_INIT_HIGH, "LCD BACKLIGHT", },
+       { TX28_LCD_RST_GPIO, GPIOFLAG_OUTPUT_INIT_LOW, "LCD RESET", },
+       { TX28_LCD_PWR_GPIO, GPIOFLAG_OUTPUT_INIT_LOW, "LCD POWER", },
+       { TX28_LCD_BACKLIGHT_GPIO, GPIOFLAG_OUTPUT_INIT_HIGH, "LCD BACKLIGHT", },
 };
 
-extern void video_hw_init(void *lcdbase);
-
 void lcd_ctrl_init(void *lcdbase)
 {
        int color_depth = 24;
-       char *vm;
+       const char *video_mode = karo_get_vmode(getenv("video_mode"));
+       const char *vm;
        unsigned long val;
        int refresh = 60;
        struct fb_videomode *p = tx28_fb_modes;
@@ -582,24 +658,34 @@ void lcd_ctrl_init(void *lcdbase)
                return;
        }
 
-       if (tstc()) {
+       if (had_ctrlc()) {
                debug("Disabling LCD\n");
                lcd_enabled = 0;
+               setenv("splashimage", NULL);
                return;
        }
 
        karo_fdt_move_fdt();
+       lcd_bl_polarity = karo_fdt_get_backlight_polarity(working_fdt);
 
-       vm = getenv("video_mode");
-       if (vm == NULL) {
+       if (video_mode == NULL) {
                debug("Disabling LCD\n");
                lcd_enabled = 0;
                return;
        }
-       if (karo_fdt_get_fb_mode(working_fdt, vm, &fb_mode) == 0) {
+       vm = video_mode;
+       if (karo_fdt_get_fb_mode(working_fdt, video_mode, &fb_mode) == 0) {
                p = &fb_mode;
                debug("Using video mode from FDT\n");
                vm += strlen(vm);
+               if (fb_mode.xres > panel_info.vl_col ||
+                       fb_mode.yres > panel_info.vl_row) {
+                       printf("video resolution from DT: %dx%d exceeds hardware limits: %dx%d\n",
+                               fb_mode.xres, fb_mode.yres,
+                               panel_info.vl_col, panel_info.vl_row);
+                       lcd_enabled = 0;
+                       return;
+               }
        }
        if (p->name != NULL)
                debug("Trying compiled-in video modes\n");
@@ -623,11 +709,13 @@ void lcd_ctrl_init(void *lcdbase)
                                        if (val > panel_info.vl_col)
                                                val = panel_info.vl_col;
                                        p->xres = val;
+                                       panel_info.vl_col = val;
                                        xres_set = 1;
                                } else if (!yres_set) {
                                        if (val > panel_info.vl_row)
                                                val = panel_info.vl_row;
                                        p->yres = val;
+                                       panel_info.vl_row = val;
                                        yres_set = 1;
                                } else if (!bpp_set) {
                                        switch (val) {
@@ -680,6 +768,12 @@ void lcd_ctrl_init(void *lcdbase)
                printf("\n");
                return;
        }
+       if (p->xres > panel_info.vl_col || p->yres > panel_info.vl_row) {
+               printf("video resolution: %dx%d exceeds hardware limits: %dx%d\n",
+                       p->xres, p->yres, panel_info.vl_col, panel_info.vl_row);
+               lcd_enabled = 0;
+               return;
+       }
        panel_info.vl_col = p->xres;
        panel_info.vl_row = p->yres;
 
@@ -691,7 +785,7 @@ void lcd_ctrl_init(void *lcdbase)
                panel_info.vl_bpix = LCD_COLOR16;
                break;
        default:
-               panel_info.vl_bpix = LCD_COLOR24;
+               panel_info.vl_bpix = LCD_COLOR32;
        }
 
        p->pixclock = KHZ2PICOS(refresh *
@@ -701,6 +795,17 @@ void lcd_ctrl_init(void *lcdbase)
        debug("Pixel clock set to %lu.%03lu MHz\n",
                PICOS2KHZ(p->pixclock) / 1000, PICOS2KHZ(p->pixclock) % 1000);
 
+       if (p != &fb_mode) {
+               int ret;
+
+               debug("Creating new display-timing node from '%s'\n",
+                       video_mode);
+               ret = karo_fdt_create_fb_mode(working_fdt, video_mode, p);
+               if (ret)
+                       printf("Failed to create new display-timing node from '%s': %d\n",
+                               video_mode, ret);
+       }
+
        gpio_request_array(stk5_lcd_gpios, ARRAY_SIZE(stk5_lcd_gpios));
        mxs_iomux_setup_multiple_pads(stk5_lcd_pads,
                                ARRAY_SIZE(stk5_lcd_pads));
@@ -709,15 +814,18 @@ void lcd_ctrl_init(void *lcdbase)
                color_depth, refresh);
 
        if (karo_load_splashimage(0) == 0) {
-               char vmode[32];
+               char vmode[128];
 
                /* setup env variable for mxsfb display driver */
-               snprintf(vmode, sizeof(vmode), "%dx%dMR-%d@%d",
-                       p->xres, p->yres, color_depth, refresh);
+               snprintf(vmode, sizeof(vmode),
+                       "x:%d,y:%d,le:%d,ri:%d,up:%d,lo:%d,hs:%d,vs:%d,sync:%d,pclk:%d,depth:%d",
+                       p->xres, p->yres, p->left_margin, p->right_margin,
+                       p->upper_margin, p->lower_margin, p->hsync_len,
+                       p->vsync_len, p->sync, p->pixclock, color_depth);
                setenv("videomode", vmode);
 
                debug("Initializing LCD controller\n");
-               video_hw_init(lcdbase);
+               video_hw_init();
                setenv("videomode", NULL);
        } else {
                debug("Skipping initialization of LCD controller\n");
@@ -735,6 +843,7 @@ static void stk5_board_init(void)
 
 static void stk5v3_board_init(void)
 {
+       led_state = LED_STATE_INIT;
        stk5_board_init();
 }
 
@@ -743,24 +852,30 @@ static void stk5v5_board_init(void)
        stk5_board_init();
 
        /* init flexcan transceiver enable GPIO */
-       gpio_request_one(MXS_GPIO_NR(0, 1), GPIOF_OUTPUT_INIT_HIGH,
+       gpio_request_one(STK5_CAN_XCVR_GPIO, GPIOFLAG_OUTPUT_INIT_HIGH,
                        "Flexcan Transceiver");
-       mxs_iomux_setup_pad(MX28_PAD_LCD_D00__GPIO_1_0);
+       mxs_iomux_setup_pad(STK5_CAN_XCVR_PAD);
 }
 
 int board_late_init(void)
 {
-       int ret;
+       int ret = 0;
        const char *baseboard;
 
-       karo_fdt_move_fdt();
+       env_cleanup();
+
+       if (had_ctrlc())
+               setenv_ulong("safeboot", 1);
+       else
+               karo_fdt_move_fdt();
 
        baseboard = getenv("baseboard");
        if (!baseboard)
-               return 0;
+               goto exit;
+
+       printf("Baseboard: %s\n", baseboard);
 
        if (strncmp(baseboard, "stk5", 4) == 0) {
-               printf("Baseboard: %s\n", baseboard);
                if ((strlen(baseboard) == 4) ||
                        strcmp(baseboard, "stk5-v3") == 0) {
                        stk5v3_board_init();
@@ -780,22 +895,14 @@ int board_late_init(void)
        } else {
                printf("WARNING: Unsupported baseboard: '%s'\n",
                        baseboard);
-               return -EINVAL;
+               if (!had_ctrlc())
+                       ret = -EINVAL;
        }
 
-       ret = fec_get_mac_addr(0);
-       if (ret < 0) {
-               printf("Failed to read FEC0 MAC address from OCOTP\n");
-               return ret;
-       }
-#ifdef CONFIG_FEC_MXC_MULTI
-       ret = fec_get_mac_addr(1);
-       if (ret < 0) {
-               printf("Failed to read FEC1 MAC address from OCOTP\n");
-               return ret;
-       }
-#endif
-       return 0;
+exit:
+       tx28_init_mac();
+       clear_ctrlc();
+       return ret;
 }
 
 #define BOOT_CAUSE_MASK                (RTC_PERSISTENT0_EXTERNAL_RESET |       \
@@ -824,7 +931,8 @@ int checkboard(void)
        const char *dlm = "";
 
        printf("Board: Ka-Ro TX28-4%sx%d\n", TX28_MOD_SUFFIX,
-               CONFIG_SDRAM_SIZE / SZ_128M);
+               CONFIG_SYS_SDRAM_SIZE / SZ_128M +
+               CONFIG_SYS_NAND_BLOCKS / 2048 * 2);
 
        printf("POWERUP Source: ");
        if (pwrup_src & (3 << 0)) {
@@ -884,98 +992,31 @@ int checkboard(void)
 #ifdef CONFIG_FDT_FIXUP_PARTITIONS
 #include <jffs2/jffs2.h>
 #include <mtd_node.h>
-struct node_info tx28_nand_nodes[] = {
+static struct node_info tx28_nand_nodes[] = {
        { "fsl,imx28-gpmi-nand", MTD_DEV_TYPE_NAND, },
 };
 #else
 #define fdt_fixup_mtdparts(b,n,c) do { } while (0)
 #endif
 
-static int flexcan_enabled(void *blob)
-{
-       const char *status;
-       int off = fdt_path_offset(blob, "can0");
-
-       if (off < 0) {
-               printf("node 'can0' not found\n");
-       } else {
-               status = fdt_getprop(blob, off, "status", NULL);
-               if (status && strcmp(status, "okay") == 0) {
-                       printf("can0 is enabled\n");
-                       return 1;
-               }
-       }
-       off = fdt_path_offset(blob, "can1");
-       if (off < 0) {
-               printf("node 'can1' not found\n");
-               return 0;
-       }
-       status = fdt_getprop(blob, off, "status", NULL);
-       if (status && strcmp(status, "okay") == 0) {
-               printf("can1 is enabled\n");
-               return 1;
-       }
-       printf("can driver disabled\n");
-       return 0;
-}
-
-static void tx28_set_lcd_pins(void *blob, const char *name)
-{
-       int off = fdt_path_offset(blob, name);
-       u32 ph;
-       const struct fdt_property *pc;
-       int len;
-
-       if (off < 0)
-               return;
-
-       ph = fdt32_to_cpu(fdt_create_phandle(blob, off));
-       if (!ph)
-               return;
-
-       off = fdt_path_offset(blob, "lcdif");
-       if (off < 0)
-               return;
-
-       pc = fdt_get_property(blob, off, "pinctrl-0", &len);
-       if (!pc || len < sizeof(ph))
-               return;
-
-       memcpy((void *)pc->data, &ph, sizeof(ph));
-       fdt_setprop(blob, off, "pinctrl-0", pc->data, len);
-}
-
-static void tx28_fixup_flexcan(void *blob, int stk5_v5)
-{
-       const char *can_xcvr = "disabled";
-
-       if (stk5_v5) {
-               if (flexcan_enabled(blob)) {
-                       tx28_set_lcd_pins(blob, "lcdif_23bit_pins_a");
-                       can_xcvr = "okay";
-               } else {
-                       tx28_set_lcd_pins(blob, "lcdif_24bit_pins_a");
-               }
-       } else {
-               const char *otg_mode = getenv("otg_mode");
-
-               if (otg_mode && (strcmp(otg_mode, "host") == 0))
-                       karo_fdt_enable_node(blob, "can1", 0);
-       }
-       fdt_find_and_setprop(blob, "/regulators/can-xcvr", "status",
-                       can_xcvr, strlen(can_xcvr) + 1, 1);
-}
-
-static void tx28_fixup_fec(void *blob)
-{
-       karo_fdt_enable_node(blob, "ethernet1", 0);
-}
+static const char *tx28_touchpanels[] = {
+       "ti,tsc2007",
+       "edt,edt-ft5x06",
+       "fsl,imx28-lradc",
+};
 
-void ft_board_setup(void *blob, bd_t *bd)
+int ft_board_setup(void *blob, bd_t *bd)
 {
        const char *baseboard = getenv("baseboard");
        int stk5_v5 = baseboard != NULL && (strcmp(baseboard, "stk5-v5") == 0);
+       const char *video_mode = karo_get_vmode(getenv("video_mode"));
+       int ret;
 
+       ret = fdt_increase_size(blob, 4096);
+       if (ret) {
+               printf("Failed to increase FDT size: %s\n", fdt_strerror(ret));
+               return ret;
+       }
 #ifdef CONFIG_TX28_S
        /* TX28-41xx (aka TX28S) has no external RTC
         * and no I2C GPIO extender
@@ -983,18 +1024,17 @@ void ft_board_setup(void *blob, bd_t *bd)
        karo_fdt_remove_node(blob, "ds1339");
        karo_fdt_remove_node(blob, "gpio5");
 #endif
-       if (stk5_v5) {
-               karo_fdt_remove_node(blob, "stk5led");
-       } else {
-               tx28_fixup_fec(blob);
-       }
-       tx28_fixup_flexcan(blob, stk5_v5);
+       if (stk5_v5)
+               karo_fdt_enable_node(blob, "stk5led", 0);
 
        fdt_fixup_mtdparts(blob, tx28_nand_nodes, ARRAY_SIZE(tx28_nand_nodes));
-       fdt_fixup_ethernet(blob);
 
-       karo_fdt_fixup_touchpanel(blob);
-       karo_fdt_fixup_usb_otg(blob, "usbotg", "fsl,usbphy");
-       karo_fdt_update_fb_mode(blob, getenv("video_mode"));
+       karo_fdt_fixup_touchpanel(blob, tx28_touchpanels,
+                               ARRAY_SIZE(tx28_touchpanels));
+       karo_fdt_fixup_usb_otg(blob, "usbotg", "fsl,usbphy", "vbus-supply");
+       karo_fdt_fixup_flexcan(blob, stk5_v5);
+       karo_fdt_update_fb_mode(blob, video_mode);
+
+       return 0;
 }
-#endif
+#endif /* CONFIG_OF_BOARD_SETUP */