diff --git a/arch/arm/mach-mvebu/Kconfig b/arch/arm/mach-mvebu/Kconfig
index f99bd3bf65672ee770ea10cd4d28fe7ad894e0bf..a832e1dc8c9e65469acc4c65f1033c1e5ef4344e 100644
--- a/arch/arm/mach-mvebu/Kconfig
+++ b/arch/arm/mach-mvebu/Kconfig
@@ -116,6 +116,13 @@ config TARGET_DB_88F6820_AMC
 config TARGET_TURRIS_OMNIA
 	bool "Support Turris Omnia"
 	select 88F6820
+	select BOARD_LATE_INIT
+	select DM_I2C
+	select I2C_MUX
+	select I2C_MUX_PCA954x
+	select SPL_I2C_MUX
+	select SYS_I2C_MVTWSI
+	select ATSHA204A
 
 config TARGET_TURRIS_MOX
 	bool "Support Turris Mox"
diff --git a/board/CZ.NIC/turris_mox/turris_mox.c b/board/CZ.NIC/turris_mox/turris_mox.c
index 8a4872343b65d634edbc3751889e4372324716f4..3818e3752a00a0426f7e1395d6defc8c89da2cdf 100644
--- a/board/CZ.NIC/turris_mox/turris_mox.c
+++ b/board/CZ.NIC/turris_mox/turris_mox.c
@@ -16,10 +16,6 @@
 #include <fdt_support.h>
 #include <environment.h>
 
-#ifdef CONFIG_WDT_ARMADA_37XX
-#include <wdt.h>
-#endif
-
 #include "mox_sp.h"
 
 #define MAX_MOX_MODULES		10
diff --git a/board/CZ.NIC/turris_omnia/turris_omnia.c b/board/CZ.NIC/turris_omnia/turris_omnia.c
index 4c08f810a240712245cad9e2f1adfe33c6b5c537..ad6e29021e882755cb96e85d7cdd2fe3d1ca6edf 100644
--- a/board/CZ.NIC/turris_omnia/turris_omnia.c
+++ b/board/CZ.NIC/turris_omnia/turris_omnia.c
@@ -18,40 +18,38 @@
 #include <dm/uclass.h>
 #include <fdt_support.h>
 #include <time.h>
-
-#ifdef CONFIG_ATSHA204A
 # include <atsha204a-i2c.h>
-#endif
-
-#ifdef CONFIG_WDT_ORION
-# include <wdt.h>
-#endif
 
 #include "../drivers/ddr/marvell/a38x/ddr3_init.h"
 #include <../serdes/a38x/high_speed_env_spec.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
-#define OMNIA_I2C_EEPROM_DM_NAME	"i2c@11000->i2cmux@70->i2c@0"
-#define OMNIA_I2C_EEPROM		0x54
-#define OMNIA_I2C_EEPROM_CONFIG_ADDR	0x0
-#define OMNIA_I2C_EEPROM_ADDRLEN	2
+#define OMNIA_I2C_BUS_NAME		"i2c@11000->i2cmux@70->i2c@0"
+
+#define OMNIA_I2C_MCU_CHIP_ADDR		0x2a
+#define OMNIA_I2C_MCU_CHIP_LEN		1
+
+#define OMNIA_I2C_EEPROM_CHIP_ADDR	0x54
+#define OMNIA_I2C_EEPROM_CHIP_LEN	2
 #define OMNIA_I2C_EEPROM_MAGIC		0x0341a034
 
-#define OMNIA_I2C_MCU_DM_NAME		"i2c@11000->i2cmux@70->i2c@0"
-#define OMNIA_I2C_MCU_ADDR_STATUS	0x1
-#define OMNIA_I2C_MCU_SATA		0x20
-#define OMNIA_I2C_MCU_CARDDET		0x10
-#define OMNIA_I2C_MCU			0x2a
-#define OMNIA_I2C_MCU_WDT_ADDR		0x0b
+enum mcu_commands {
+	CMD_GET_STATUS_WORD	= 0x01,
+	CMD_GET_RESET		= 0x09,
+	CMD_WATCHDOG_STATE	= 0x0b,
+};
+
+enum status_word_bits {
+	CARD_DET_STSBIT		= 0x0010,
+	MSATA_IND_STSBIT	= 0x0020,
+};
 
 #define OMNIA_ATSHA204_OTP_VERSION	0
 #define OMNIA_ATSHA204_OTP_SERIAL	1
 #define OMNIA_ATSHA204_OTP_MAC0		3
 #define OMNIA_ATSHA204_OTP_MAC1		4
 
-#define MVTWSI_ARMADA_DEBUG_REG		0x8c
-
 /*
  * Those values and defines are taken from the Marvell U-Boot version
  * "u-boot-2013.01-2014_T3.0"
@@ -87,48 +85,97 @@ static struct serdes_map board_serdes_map_sata[] = {
 	{SGMII2, SERDES_SPEED_1_25_GBPS, SERDES_DEFAULT_MODE, 0, 0}
 };
 
-static bool omnia_detect_sata(void)
+static struct udevice *omnia_get_i2c_chip(const char *name, uint addr,
+					  uint offset_len)
 {
 	struct udevice *bus, *dev;
-	int ret, retry = 3;
-	u16 mode;
-
-	puts("SERDES0 card detect: ");
+	int ret;
 
-	if (uclass_get_device_by_name(UCLASS_I2C, OMNIA_I2C_MCU_DM_NAME, &bus)) {
-		puts("Cannot find MCU bus!\n");
-		return false;
+	ret = uclass_get_device_by_name(UCLASS_I2C, OMNIA_I2C_BUS_NAME, &bus);
+	if (ret) {
+		printf("Cannot get I2C bus %s: uclass_get_device_by_name failed: %i\n",
+		       OMNIA_I2C_BUS_NAME, ret);
+		return NULL;
 	}
 
-	ret = i2c_get_chip(bus, OMNIA_I2C_MCU, 1, &dev);
+	ret = i2c_get_chip(bus, addr, offset_len, &dev);
 	if (ret) {
-		puts("Cannot get MCU chip!\n");
-		return false;
+		printf("Cannot get %s I2C chip: i2c_get_chip failed: %i\n",
+		       name, ret);
+		return NULL;
 	}
 
-	for (; retry > 0; --retry) {
-		ret = dm_i2c_read(dev, OMNIA_I2C_MCU_ADDR_STATUS, (uchar *) &mode, 2);
-		if (!ret)
-			break;
-	}
+	return dev;
+}
+
+static int omnia_mcu_read(u8 cmd, void *buf, int len)
+{
+	struct udevice *chip;
+
+	chip = omnia_get_i2c_chip("MCU", OMNIA_I2C_MCU_CHIP_ADDR,
+				  OMNIA_I2C_MCU_CHIP_LEN);
+	if (!chip)
+		return -ENODEV;
+
+	return dm_i2c_read(chip, cmd, buf, len);
+}
 
-	if (!retry) {
-		puts("I2C read failed! Default PEX\n");
+#ifndef CONFIG_SPL_BUILD
+static int omnia_mcu_write(u8 cmd, const void *buf, int len)
+{
+	struct udevice *chip;
+
+	chip = omnia_get_i2c_chip("MCU", OMNIA_I2C_MCU_CHIP_ADDR,
+				  OMNIA_I2C_MCU_CHIP_LEN);
+	if (!chip)
+		return -ENODEV;
+
+	return dm_i2c_write(chip, cmd, buf, len);
+}
+
+static bool disable_mcu_watchdog(void)
+{
+	int ret;
+
+	puts("Disabling MCU watchdog... ");
+
+	ret = omnia_mcu_write(CMD_WATCHDOG_STATE, "\x00", 1);
+	if (ret) {
+		printf("omnia_mcu_write failed: %i\n", ret);
 		return false;
 	}
 
-	if (!(mode & OMNIA_I2C_MCU_CARDDET)) {
-		puts("NONE\n");
+	puts("disabled\n");
+
+	return true;
+}
+#endif
+
+static bool omnia_detect_sata(void)
+{
+	int ret;
+	u16 stsword;
+
+	puts("MiniPCIe/mSATA card detection... ");
+
+	ret = omnia_mcu_read(CMD_GET_STATUS_WORD, &stsword, sizeof(stsword));
+	if (ret) {
+		printf("omnia_mcu_read failed: %i, defaulting to MiniPCIe card\n",
+		       ret);
 		return false;
 	}
 
-	if (mode & OMNIA_I2C_MCU_SATA) {
-		puts("SATA\n");
-		return true;
-	} else {
-		puts("PEX\n");
+	if (!(stsword & CARD_DET_STSBIT)) {
+		puts("none\n");
 		return false;
 	}
+
+	if (stsword & MSATA_IND_STSBIT)
+		puts("mSATA\n");
+	else
+		puts("MiniPCIe\n");
+
+	return stsword & MSATA_IND_STSBIT ? true : false;
 }
 
 int hws_board_topology_load(struct serdes_map **serdes_map_array, u8 *count)
@@ -153,48 +200,63 @@ struct omnia_eeprom {
 
 static bool omnia_read_eeprom(struct omnia_eeprom *oep)
 {
-	struct udevice *bus, *dev;
-	int ret, crc, retry = 3;
+	struct udevice *chip;
+	u32 crc;
+	int ret;
+
+	chip = omnia_get_i2c_chip("EEPROM", OMNIA_I2C_EEPROM_CHIP_ADDR,
+				  OMNIA_I2C_EEPROM_CHIP_LEN);
 
-	if (uclass_get_device_by_name(UCLASS_I2C, OMNIA_I2C_EEPROM_DM_NAME, &bus)) {
-		puts("Cannot find EEPROM bus\n");
+	if (!chip)
 		return false;
-	}
 
-	ret = i2c_get_chip(bus, OMNIA_I2C_EEPROM, OMNIA_I2C_EEPROM_ADDRLEN, &dev);
+	ret = dm_i2c_read(chip, 0, (void *)oep, sizeof(*oep));
 	if (ret) {
-		puts("Cannot get EEPROM chip\n");
+		printf("dm_i2c_read failed: %i, cannot read EEPROM\n", ret);
 		return false;
 	}
 
-	for (; retry > 0; --retry) {
-		ret = dm_i2c_read(dev, OMNIA_I2C_EEPROM_CONFIG_ADDR, (uchar *) oep, sizeof(struct omnia_eeprom));
-		if (ret)
-			continue;
-
-		if (oep->magic != OMNIA_I2C_EEPROM_MAGIC) {
-			puts("I2C EEPROM missing magic number!\n");
-			continue;
-		}
-
-		crc = crc32(0, (unsigned char *) oep,
-			    sizeof(struct omnia_eeprom) - 4);
-		if (crc == oep->crc) {
-			break;
-		} else {
-			printf("CRC of EEPROM memory config failed! "
-			       "calc=0x%04x saved=0x%04x\n", crc, oep->crc);
-		}
+	if (oep->magic != OMNIA_I2C_EEPROM_MAGIC) {
+		printf("bad EEPROM magic number (%08x, should be %08x)\n",
+		       oep->magic, OMNIA_I2C_EEPROM_MAGIC);
+		return false;
 	}
 
-	if (!retry) {
-		puts("I2C EEPROM read failed!\n");
+	crc = crc32(0, (void *)oep, sizeof(*oep) - 4);
+	if (crc != oep->crc) {
+		printf("bad EEPROM CRC (stored %08x, computed %08x)\n",
+		       oep->crc, crc);
 		return false;
 	}
 
 	return true;
 }
 
+static int omnia_get_ram_size_gb(void)
+{
+	static int ram_size;
+	struct omnia_eeprom oep;
+
+	if (!ram_size) {
+		/* Get the board config from EEPROM */
+		if (omnia_read_eeprom(&oep)) {
+			debug("Memory config in EEPROM: 0x%02x\n", oep.ramsize);
+
+			if (oep.ramsize == 0x2)
+				ram_size = 2;
+			else
+				ram_size = 1;
+		} else {
+			/* Hardcoded fallback */
+			puts("Memory config from EEPROM read failed!\n");
+			puts("Falling back to default 1 GiB!\n");
+			ram_size = 1;
+		}
+	}
+
+	return ram_size;
+}
+
 /*
  * Define the DDR layout / topology here in the board file. This will
  * be used by the DDR3 init code in the SPL U-Boot version to configure
@@ -246,37 +308,10 @@ static struct mv_ddr_topology_map board_topology_map_2g = {
 
 struct mv_ddr_topology_map *mv_ddr_topology_map_get(void)
 {
-	static int mem = 0;
-	struct omnia_eeprom oep;
-
-	/* Get the board config from EEPROM */
-	if (mem == 0) {
-		if(!omnia_read_eeprom(&oep))
-			goto out;
-
-		printf("Memory config in EEPROM: 0x%02x\n", oep.ramsize);
-
-		if (oep.ramsize == 0x2)
-			mem = 2;
-		else
-			mem = 1;
-	}
-
-out:
-	/* Hardcoded fallback */
-	if (mem == 0) {
-		puts("WARNING: Memory config from EEPROM read failed.\n");
-		puts("Falling back to default 1GiB map.\n");
-		mem = 1;
-	}
-
-	/* Return the board topology as defined in the board code */
-	if (mem == 1)
-		return &board_topology_map_1g;
-	if (mem == 2)
+	if (omnia_get_ram_size_gb() == 2)
 		return &board_topology_map_2g;
-
-	return &board_topology_map_1g;
+	else
+		return &board_topology_map_1g;
 }
 
 #ifndef CONFIG_SPL_BUILD
@@ -293,12 +328,47 @@ static int set_regdomain(void)
 	printf("Regdomain set to %s\n", rd);
 	return env_set("regdomain", rd);
 }
+
+/*
+ * default factory reset bootcommand on Omnia first sets all the front LEDs
+ * to green and then tries to load the rescue image from SPI flash memory and
+ * boot it
+ */
+#define OMNIA_FACTORY_RESET_BOOTCMD \
+	"i2c dev 2; " \
+	"i2c mw 0x2a.1 0x3 0x1c 1; " \
+	"i2c mw 0x2a.1 0x4 0x1c 1; " \
+	"mw.l 0x01000000 0x00ff000c; " \
+	"i2c write 0x01000000 0x2a.1 0x5 4 -s; " \
+	"setenv bootargs \"$bootargs omniarescue=$omnia_reset\"; " \
+	"sf probe; " \
+	"sf read 0x1000000 0x100000 0x700000; " \
+	"bootm 0x1000000; " \
+	"bootz 0x1000000"
+
+static void handle_reset_button(void)
+{
+	int ret;
+	u8 reset_status;
+
+	ret = omnia_mcu_read(CMD_GET_RESET, &reset_status, 1);
+	if (ret) {
+		printf("omnia_mcu_read failed: %i, reset status unknown!\n",
+		       ret);
+		return;
+	}
+
+	env_set_ulong("omnia_reset", reset_status);
+
+	if (reset_status) {
+		printf("RESET button was pressed, overwriting bootcmd!\n");
+		env_set("bootcmd", OMNIA_FACTORY_RESET_BOOTCMD);
+	}
+}
 #endif
 
 int board_early_init_f(void)
 {
-	u32 i2c_debug_reg;
-
 	/* Configure MPP */
 	writel(0x11111111, MVEBU_MPP_BASE + 0x00);
 	writel(0x11111111, MVEBU_MPP_BASE + 0x04);
@@ -321,59 +391,16 @@ int board_early_init_f(void)
 	writel(OMNIA_GPP_OUT_ENA_LOW, MVEBU_GPIO0_BASE + 0x04);
 	writel(OMNIA_GPP_OUT_ENA_MID, MVEBU_GPIO1_BASE + 0x04);
 
-	/*
-	 * Disable I2C debug mode blocking 0x64 I2C address.
-	 * Note: that would be redundant once Turris Omnia migrates to DM_I2C,
-	 * because the mvtwsi driver includes equivalent code.
-	 */
-	i2c_debug_reg = readl(MVEBU_TWSI_BASE + MVTWSI_ARMADA_DEBUG_REG);
-	i2c_debug_reg &= ~(1<<18);
-	writel(i2c_debug_reg, MVEBU_TWSI_BASE + MVTWSI_ARMADA_DEBUG_REG);
-
 	return 0;
 }
 
-#ifndef CONFIG_SPL_BUILD
-static bool disable_mcu_watchdog(void)
-{
-	struct udevice *bus, *dev;
-	int ret, retry = 3;
-	uchar buf[1] = {0x0};
-
-	if (uclass_get_device_by_name(UCLASS_I2C, OMNIA_I2C_MCU_DM_NAME, &bus)) {
-		puts("Cannot find MCU bus! Can not disable MCU WDT.\n");
-		return false;
-	}
-
-	ret = i2c_get_chip(bus, OMNIA_I2C_MCU, 1, &dev);
-	if (ret) {
-		puts("Cannot get MCU chip! Can not disable MCU WDT.\n");
-		return false;
-	}
-
-	for (; retry > 0; --retry)
-		if (!dm_i2c_write(dev, OMNIA_I2C_MCU_WDT_ADDR, (uchar *) buf, 1))
-			break;
-
-	if (retry <= 0) {
-		puts("I2C MCU watchdog failed to disable!\n");
-		return false;
-	}
-
-	return true;
-}
-#endif
-
 int board_init(void)
 {
-	/* adress of boot parameters */
+	/* address of boot parameters */
 	gd->bd->bi_boot_params = mvebu_sdram_bar(0) + 0x100;
 
 #ifndef CONFIG_SPL_BUILD
-	if (disable_mcu_watchdog())
-		puts("Disabled MCU startup watchdog.\n");
-
-	set_regdomain();
+	disable_mcu_watchdog();
 #endif
 
 	return 0;
@@ -383,17 +410,17 @@ int board_late_init(void)
 {
 #ifndef CONFIG_SPL_BUILD
 	set_regdomain();
+	handle_reset_button();
 #endif
 
 	return 0;
 }
 
-#ifdef CONFIG_ATSHA204A
 static struct udevice *get_atsha204a_dev(void)
 {
-	static struct udevice *dev = NULL;
+	static struct udevice *dev;
 
-	if (dev != NULL)
+	if (dev)
 		return dev;
 
 	if (uclass_get_device_by_name(UCLASS_MISC, "atsha204a@64", &dev)) {
@@ -403,14 +430,12 @@ static struct udevice *get_atsha204a_dev(void)
 
 	return dev;
 }
-#endif
 
 int checkboard(void)
 {
 	u32 version_num, serial_num;
 	int err = 1;
 
-#ifdef CONFIG_ATSHA204A
 	struct udevice *dev = get_atsha204a_dev();
 
 	if (dev) {
@@ -420,13 +445,13 @@ int checkboard(void)
 
 		err = atsha204a_read(dev, ATSHA204A_ZONE_OTP, false,
 				     OMNIA_ATSHA204_OTP_VERSION,
-				     (u8 *) &version_num);
+				     (u8 *)&version_num);
 		if (err)
 			goto out;
 
 		err = atsha204a_read(dev, ATSHA204A_ZONE_OTP, false,
 				     OMNIA_ATSHA204_OTP_SERIAL,
-				     (u8 *) &serial_num);
+				     (u8 *)&serial_num);
 		if (err)
 			goto out;
 
@@ -434,13 +459,13 @@ int checkboard(void)
 	}
 
 out:
-#endif
-
+	printf("Turris Omnia:\n");
+	printf("  RAM size: %i MiB\n", omnia_get_ram_size_gb() * 1024);
 	if (err)
-		printf("Board: Turris Omnia (ver N/A). SN: N/A\n");
+		printf("  Serial Number: unknown\n");
 	else
-		printf("Board: Turris Omnia SNL %08X%08X\n",
-		       be32_to_cpu(version_num), be32_to_cpu(serial_num));
+		printf("  Serial Number: %08X%08X\n", be32_to_cpu(version_num),
+		       be32_to_cpu(serial_num));
 
 	return 0;
 }
@@ -458,7 +483,6 @@ static void increment_mac(u8 *mac)
 
 int misc_init_r(void)
 {
-#ifdef CONFIG_ATSHA204A
 	int err;
 	struct udevice *dev = get_atsha204a_dev();
 	u8 mac0[4], mac1[4], mac[6];
@@ -503,8 +527,6 @@ int misc_init_r(void)
 		eth_env_set_enetaddr("eth2addr", mac);
 
 out:
-#endif
-
 	return 0;
 }
 
diff --git a/configs/turris_omnia_defconfig b/configs/turris_omnia_defconfig
index 999425e460da22d24d29667d02ca40b4d8692c89..e04156311f4956145fc7ad8378ac3f2f52471848 100644
--- a/configs/turris_omnia_defconfig
+++ b/configs/turris_omnia_defconfig
@@ -2,6 +2,7 @@ CONFIG_ARM=y
 CONFIG_SPL_SYS_THUMB_BUILD=y
 CONFIG_ARCH_MVEBU=y
 CONFIG_SYS_TEXT_BASE=0x00800000
+CONFIG_SPL_GPIO_SUPPORT=y
 CONFIG_SPL_LIBCOMMON_SUPPORT=y
 CONFIG_SPL_LIBGENERIC_SUPPORT=y
 CONFIG_SYS_MALLOC_F_LEN=0x2000
@@ -14,7 +15,10 @@ CONFIG_DEBUG_UART_CLOCK=250000000
 CONFIG_SPL_SPI_FLASH_SUPPORT=y
 CONFIG_SPL_SPI_SUPPORT=y
 CONFIG_DEBUG_UART=y
+CONFIG_AHCI=y
 CONFIG_DISTRO_DEFAULTS=y
+CONFIG_FIT=y
+CONFIG_FIT_VERBOSE=y
 CONFIG_BOOTDELAY=3
 CONFIG_SYS_CONSOLE_INFO_QUIET=y
 CONFIG_MISC_INIT_R=y
@@ -23,16 +27,22 @@ CONFIG_DISPLAY_BOARDINFO_LATE=y
 CONFIG_SPL_TEXT_BASE=0x40000030
 CONFIG_SPL_I2C_SUPPORT=y
 CONFIG_SPL_SPI_LOAD=y
+CONFIG_CMD_SHA1SUM=y
+CONFIG_CMD_LZMADEC=y
 # CONFIG_CMD_FLASH is not set
+CONFIG_CMD_GPIO=y
 CONFIG_CMD_I2C=y
 CONFIG_CMD_MMC=y
 CONFIG_CMD_PCI=y
+CONFIG_CMD_SATA=y
 CONFIG_CMD_SF=y
 CONFIG_CMD_SPI=y
 CONFIG_CMD_USB=y
 CONFIG_CMD_TFTPPUT=y
 CONFIG_CMD_CACHE=y
 CONFIG_CMD_TIME=y
+CONFIG_CMD_AES=y
+CONFIG_CMD_HASH=y
 CONFIG_CMD_BTRFS=y
 # CONFIG_SPL_PARTITION_UUIDS is not set
 CONFIG_DEFAULT_DEVICE_TREE="armada-385-turris-omnia"
@@ -40,8 +50,11 @@ CONFIG_ENV_IS_IN_SPI_FLASH=y
 CONFIG_USE_ENV_SPI_MAX_HZ=y
 CONFIG_ENV_SPI_MAX_HZ=50000000
 CONFIG_SPL_OF_TRANSLATE=y
-CONFIG_SCSI_AHCI=y
-CONFIG_ATSHA204A=y
+CONFIG_AHCI_PCI=y
+CONFIG_AHCI_MVEBU=y
+CONFIG_DM_GPIO=y
+# CONFIG_MVEBU_GPIO is not set
+CONFIG_DM_PCA953X=y
 CONFIG_DM_MMC=y
 CONFIG_MMC_SDHCI=y
 CONFIG_MMC_SDHCI_MV=y
@@ -53,11 +66,13 @@ CONFIG_MVNETA=y
 CONFIG_MII=y
 CONFIG_PCI=y
 CONFIG_PCI_MVEBU=y
+CONFIG_SCSI=y
 CONFIG_DEBUG_UART_SHIFT=2
 CONFIG_SYS_NS16550=y
 CONFIG_KIRKWOOD_SPI=y
 CONFIG_USB=y
 CONFIG_DM_USB=y
+CONFIG_USB_XHCI_HCD=y
 CONFIG_USB_EHCI_HCD=y
 CONFIG_WDT=y
 CONFIG_WDT_ORION=y
diff --git a/drivers/i2c/mvtwsi.c b/drivers/i2c/mvtwsi.c
index 74ac0a4aa78976487f8314ec7c531a7f781d9299..0a2dafcec6caceb1b0b25f1417112ae8da7657af 100644
--- a/drivers/i2c/mvtwsi.c
+++ b/drivers/i2c/mvtwsi.c
@@ -271,6 +271,17 @@ static int twsi_wait(struct mvtwsi_registers *twsi, int expected_status,
 	do {
 		control = readl(&twsi->control);
 		if (control & MVTWSI_CONTROL_IFLG) {
+			/*
+			 * On Armada 38x it seems that the controller works as
+			 * if it first set the MVTWSI_CONTROL_IFLAG in the
+			 * control register and only after that it changed the
+			 * status register.
+			 * This sometimes caused weird bugs which only appeared
+			 * on selected I2C speeds and even then only sometimes.
+			 * We therefore add here a simple ndealy(100), which
+			 * seems to fix this weird bug.
+			 */
+			ndelay(100);
 			status = readl(&twsi->status);
 			if (status == expected_status)
 				return 0;
diff --git a/include/configs/turris_omnia.h b/include/configs/turris_omnia.h
index 5a148873c78cd703db2195933d98c3c1d2cc6f0b..018f54428bccc94cf066be59477ea622abeedbf3 100644
--- a/include/configs/turris_omnia.h
+++ b/include/configs/turris_omnia.h
@@ -18,31 +18,6 @@
  */
 #define CONFIG_SYS_TCLK		250000000	/* 250MHz */
 
-/*
- * Commands configuration
- */
-
-/* I2C support */
-#define CONFIG_DM_I2C
-#define CONFIG_I2C_MUX
-#define CONFIG_I2C_MUX_PCA954x
-#define CONFIG_SPL_I2C_MUX
-#define CONFIG_SYS_I2C_MVTWSI
-
-/*
- * SDIO/MMC Card Configuration
- */
-#define CONFIG_SYS_MMC_BASE		MVEBU_SDIO_BASE
-
-/*
- * SATA/SCSI/AHCI configuration
- */
-#define CONFIG_SCSI_AHCI_PLAT
-#define CONFIG_SYS_SCSI_MAX_SCSI_ID	2
-#define CONFIG_SYS_SCSI_MAX_LUN		1
-#define CONFIG_SYS_SCSI_MAX_DEVICE	(CONFIG_SYS_SCSI_MAX_SCSI_ID * \
-					 CONFIG_SYS_SCSI_MAX_LUN)
-
 /* USB/EHCI configuration */
 #define CONFIG_EHCI_IS_TDI
 
@@ -114,9 +89,16 @@
 #define BOOT_TARGET_DEVICES_USB(func)
 #endif
 
+#ifdef CONFIG_SCSI
+#define BOOT_TARGET_DEVICES_SCSI(func) func(SCSI, scsi, 0)
+#else
+#define BOOT_TARGET_DEVICES_SCSI(func)
+#endif
+
 #define BOOT_TARGET_DEVICES(func) \
 	BOOT_TARGET_DEVICES_MMC(func) \
 	BOOT_TARGET_DEVICES_USB(func) \
+	BOOT_TARGET_DEVICES_SCSI(func) \
 	func(PXE, pxe, na) \
 	func(DHCP, dhcp, na)
 
diff --git a/tools/kwbimage.c b/tools/kwbimage.c
index dffaf9043a040815efedac502338308e3228eb9d..b8f8d38212f5c00ee79bf89dd63ef585b27eacb2 100644
--- a/tools/kwbimage.c
+++ b/tools/kwbimage.c
@@ -701,7 +701,7 @@ int kwb_verify(RSA *key, void *data, int datasz, struct sig_v1 *sig,
 		goto err_ctx;
 	}
 
-	if (!EVP_VerifyFinal(ctx, sig->sig, sizeof(sig->sig), evp_key)) {
+	if (EVP_VerifyFinal(ctx, sig->sig, sizeof(sig->sig), evp_key) != 1) {
 		ret = openssl_err("Could not verify signature");
 		goto err_ctx;
 	}