diff --git a/arch/arm/dts/tegra20-paz00.dts b/arch/arm/dts/tegra20-paz00.dts
index 780203c..9906f3a 100644
--- a/arch/arm/dts/tegra20-paz00.dts
+++ b/arch/arm/dts/tegra20-paz00.dts
@@ -40,7 +40,13 @@
 	};
 
 	i2c@7000c500 {
-		status = "disabled";
+		status = "okay";
+		clock-frequency = <40000>;
+		slave-addr = <138>;
+		nvec {
+			compatible = "nvidia,tegra20-nvec";
+			request-gpios = <&gpio 170 0>; /* gpio PV2 */
+		};
 	};
 
 	i2c@7000d000 {
diff --git a/arch/arm/include/asm/arch-tegra/tegra_i2c.h b/arch/arm/include/asm/arch-tegra/tegra_i2c.h
index 7ca6907..1e74db9 100644
--- a/arch/arm/include/asm/arch-tegra/tegra_i2c.h
+++ b/arch/arm/include/asm/arch-tegra/tegra_i2c.h
@@ -134,13 +134,19 @@ struct i2c_ctlr {
 				(1 << DVC_CTRL_REG3_I2C_HW_SW_PROG_SHIFT)
 
 /* I2C_CNFG */
+#define I2C_CNFG_DEBOUNCE_CNT_SHIFT	12
+#define I2C_CNFG_DEBOUNCE_CNT_MASK	(0x2 << I2C_CNFG_DEBOUNCE_CNT_SHIFT)
 #define I2C_CNFG_NEW_MASTER_FSM_SHIFT	11
 #define I2C_CNFG_NEW_MASTER_FSM_MASK	(1 << I2C_CNFG_NEW_MASTER_FSM_SHIFT)
 #define I2C_CNFG_PACKET_MODE_SHIFT	10
 #define I2C_CNFG_PACKET_MODE_MASK	(1 << I2C_CNFG_PACKET_MODE_SHIFT)
 
 /* I2C_SL_CNFG */
+#define I2C_SL_CNFG_RESP_SHIFT		0
+#define I2C_SL_CNFG_NACK_SHIFT		1
 #define I2C_SL_CNFG_NEWSL_SHIFT		2
+#define I2C_SL_CNFG_IRQ_SHIFT		3
+#define I2C_SL_CNFG_END_TRANS_SHIFT	4
 #define I2C_SL_CNFG_NEWSL_MASK		(1 << I2C_SL_CNFG_NEWSL_SHIFT)
 
 /* I2C_FIFO_STATUS */
diff --git a/arch/arm/include/asm/arch-tegra/tegra_nvec.h b/arch/arm/include/asm/arch-tegra/tegra_nvec.h
new file mode 100644
index 0000000..f1cec0d
--- /dev/null
+++ b/arch/arm/include/asm/arch-tegra/tegra_nvec.h
@@ -0,0 +1,130 @@
+/*
+ * (C) Copyright 2014
+ * Andrey Danin <danindrey@mail.ru>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef _TEGRA_NVEC_H_
+#define _TEGRA_NVEC_H_
+
+#define I2C_CNFG			0x00
+#define I2C_CNFG_PACKET_MODE_EN		(1<<10)
+#define I2C_CNFG_NEW_MASTER_SFM		(1<<11)
+#define I2C_CNFG_DEBOUNCE_CNT_SHIFT	12
+
+#define I2C_SL_CNFG		0x20
+#define I2C_SL_NEWSL		(1<<2)
+#define I2C_SL_NACK		(1<<1)
+#define I2C_SL_RESP		(1<<0)
+#define I2C_SL_IRQ		(1<<3)
+#define END_TRANS		(1<<4)
+#define RCVD			(1<<2)
+#define RNW			(1<<1)
+
+#define I2C_SL_RCVD		0x24
+#define I2C_SL_STATUS		0x28
+#define I2C_SL_ADDR1		0x2c
+#define I2C_SL_ADDR2		0x30
+#define I2C_SL_DELAY_COUNT	0x3c
+
+
+enum nvec_msg_type {
+	NVEC_KEYBOARD = 0,
+	NVEC_SYS = 1,
+	NVEC_BAT,
+	NVEC_GPIO,
+	NVEC_SLEEP,
+	NVEC_KBD,
+	NVEC_PS2,
+	NVEC_CNTL,
+	NVEC_OEM0 = 0x0d,
+	NVEC_KB_EVT = 0x80,
+	NVEC_PS2_EVT,
+	NVEC_LAST_MSG,
+};
+
+enum nvec_event_size {
+	NVEC_2BYTES,
+	NVEC_3BYTES,
+	NVEC_VAR_SIZE,
+};
+
+enum sys_subcmds {
+	SYS_GET_STATUS,
+	SYS_CNFG_EVENT_REPORTING,
+	SYS_ACK_STATUS,
+	SYS_CNFG_WAKE = 0xfd,
+};
+
+enum kbd_subcmds {
+	CNFG_WAKE = 3,
+	CNFG_WAKE_KEY_REPORTING,
+	SET_LEDS = 0xed,
+	ENABLE_KBD = 0xf4,
+	DISABLE_KBD,
+};
+
+enum cntl_subcmds {
+	CNTL_RESET_EC = 0x00,
+	CNTL_SELF_TEST = 0x01,
+	CNTL_NOOP = 0x02,
+	CNTL_GET_EC_SPEC_VER = 0x10,
+	CNTL_GET_FIRMWARE_VERSION = 0x15,
+};
+
+enum nvec_sleep_subcmds {
+	GLOBAL_EVENTS,
+	AP_PWR_DOWN,
+	AP_SUSPEND,
+};
+
+enum ps2_subcmds {
+	MOUSE_SEND_CMD = 0x01
+};
+
+#define MOUSE_RESET 0xff
+
+typedef int (*periph_start)(void);
+typedef int (*periph_process_msg)(const unsigned char *);
+
+struct nvec_periph {
+	periph_start		start;
+	periph_process_msg	process_msg;
+};
+
+
+int board_nvec_init(void);
+
+/**
+ * Read all available events.
+ *
+ * @return count of available events if ok, -1 on error
+ */
+int nvec_read_events(void);
+
+int nvec_msg_is_event(const unsigned char *msg);
+int nvec_msg_event_type(const unsigned char *msg);
+
+/**
+ * Register perepherial device driver.
+ *
+ * @param msg_type	type of messages that divece processes.
+ * @param periph	pointer to device description.
+ *
+ * @return 0 if ok, -1 on error
+ */
+int nvec_register_periph(int msg_type, struct nvec_periph *periph);
+
+/**
+ * Send request and read response. If write or read failed
+ * operation will be repeated NVEC_ATTEMPTS_MAX times.
+ *
+ * @param buf		request data
+ * @param size		request data size
+ * @return 0 if ok, -1 on error
+ */
+int nvec_do_request(char *buf, int size);
+
+
+#endif /* _TEGRA_NVEC_H_ */
diff --git a/arch/arm/include/asm/arch-tegra/tegra_nvec_keyboard.h b/arch/arm/include/asm/arch-tegra/tegra_nvec_keyboard.h
new file mode 100644
index 0000000..5f789f4
--- /dev/null
+++ b/arch/arm/include/asm/arch-tegra/tegra_nvec_keyboard.h
@@ -0,0 +1,304 @@
+/*
+ * (C) Copyright 2014
+ * Andrey Danin <danindrey@mail.ru>
+ * (C) Copyright 2009
+ * NVIDIA Corporation.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef _TEGRA_NVEC_KEYBOARD_H_
+#define _TEGRA_NVEC_KEYBOARD_H_
+
+#include <linux/input.h>
+
+
+/* Keytables */
+
+static unsigned short code_tab_102us[] = {
+	/* 0x00 */
+	KEY_GRAVE,
+	KEY_ESC,
+	KEY_1,
+	KEY_2,
+	KEY_3,
+	KEY_4,
+	KEY_5,
+	KEY_6,
+	KEY_7,
+	KEY_8,
+	KEY_9,
+	KEY_0,
+	KEY_MINUS,
+	KEY_EQUAL,
+	KEY_BACKSPACE,
+	KEY_TAB,
+	/* 0x10 */
+	KEY_Q,
+	KEY_W,
+	KEY_E,
+	KEY_R,
+	KEY_T,
+	KEY_Y,
+	KEY_U,
+	KEY_I,
+	KEY_O,
+	KEY_P,
+	KEY_LEFTBRACE,
+	KEY_RIGHTBRACE,
+	KEY_ENTER,
+	KEY_LEFTCTRL,
+	KEY_A,
+	KEY_S,
+	/* 0x20 */
+	KEY_D,
+	KEY_F,
+	KEY_G,
+	KEY_H,
+	KEY_J,
+	KEY_K,
+	KEY_L,
+	KEY_SEMICOLON,
+	KEY_APOSTROPHE,
+	KEY_GRAVE,
+	KEY_LEFTSHIFT,
+	KEY_BACKSLASH,
+	KEY_Z,
+	KEY_X,
+	KEY_C,
+	KEY_V,
+	/* 0x30 */
+	KEY_B,
+	KEY_N,
+	KEY_M,
+	KEY_COMMA,
+	KEY_DOT,
+	KEY_SLASH,
+	KEY_RIGHTSHIFT,
+	KEY_KPASTERISK,
+	KEY_LEFTALT,
+	KEY_SPACE,
+	KEY_CAPSLOCK,
+	KEY_F1,
+	KEY_F2,
+	KEY_F3,
+	KEY_F4,
+	KEY_F5,
+	/* 0x40 */
+	KEY_F6,
+	KEY_F7,
+	KEY_F8,
+	KEY_F9,
+	KEY_F10,
+	KEY_FN,
+	/* VK_SCROLL */
+	0,
+	KEY_KP7,
+	KEY_KP8,
+	KEY_KP9,
+	KEY_KPMINUS,
+	KEY_KP4,
+	KEY_KP5,
+	KEY_KP6,
+	KEY_KPPLUS,
+	KEY_KP1,
+	/* 0x50 */
+	KEY_KP2,
+	KEY_KP3,
+	KEY_KP0,
+	KEY_KPDOT,
+	/* VK_SNAPSHOT */
+	0, /* KEY_MENU, */
+	KEY_POWER,
+	/* VK_OEM_102 */
+	KEY_102ND,
+	KEY_F11,
+	KEY_F12,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x60 */
+	0,
+	0,
+	0,
+	0, /* KEY_SEARCH, */
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x70 */
+	0,
+	0,
+	0,
+	KEY_KP5,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	KEY_KP9,
+};
+
+static unsigned short extcode_tab_us102[] = {
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x10 */
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* VK_MEDIA_NEXT_TRACK */
+	0,
+	0,
+	0,
+	/* VK_RETURN */
+	0,
+	KEY_RIGHTCTRL,
+	0,
+	0,
+	/* 0x20 */
+	KEY_MUTE,
+	/* VK_LAUNCH_APP1 */
+	0,
+	/* VK_MEDIA_PLAY_PAUSE */
+	0,
+	0,
+	/* VK_MEDIA_STOP */
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* 0x30 */
+	KEY_VOLUMEUP,
+	0,
+	/* VK_BROWSER_HOME */
+	0,
+	0,
+	0,
+	/* VK_DIVIDE */
+	KEY_KPSLASH,
+	0,
+	/* VK_SNAPSHOT */
+	KEY_SYSRQ,
+	/* VK_RMENU */
+	KEY_RIGHTALT,
+	/* VK_OEM_NV_BACKLIGHT_UP */
+	0,
+	/* VK_OEM_NV_BACKLIGHT_DN */
+	0,
+	/* VK_OEM_NV_BACKLIGHT_AUTOTOGGLE */
+	0,
+	/* VK_OEM_NV_POWER_INFO */
+	0,
+	/* VK_OEM_NV_WIFI_TOGGLE */
+	0,
+	/* VK_OEM_NV_DISPLAY_SELECT */
+	0,
+	/* VK_OEM_NV_AIRPLANE_TOGGLE */
+	0,
+	/* 0x40 */
+	0,
+	KEY_LEFT,
+	0,
+	0,
+	0,
+	0,
+	0, /* KEY_CANCEL, */
+	KEY_HOME,
+	KEY_UP,
+	KEY_PAGEUP,
+	0,
+	KEY_LEFT,
+	0,
+	KEY_RIGHT,
+	0,
+	KEY_END,
+	/* 0x50 */
+	KEY_DOWN,
+	KEY_PAGEDOWN,
+	KEY_INSERT,
+	KEY_DELETE,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	KEY_LEFTMETA,
+	0,
+	KEY_ESC,
+	KEY_KPMINUS,
+	0,
+	0,
+	0,
+	0,
+	0,
+	0,
+	/* VK_BROWSER_SEARCH */
+	0,
+	/* VK_BROWSER_FAVORITES */
+	0,
+	/* VK_BROWSER_REFRESH */
+	0,
+	/* VK_BROWSER_STOP */
+	0,
+	/* VK_BROWSER_FORWARD */
+	0,
+	/* VK_BROWSER_BACK */
+	0,
+	/* VK_LAUNCH_APP2 */
+	0,
+	/* VK_LAUNCH_MAIL */
+	0,
+	/* VK_LAUNCH_MEDIA_SELECT */
+	0,
+};
+
+static unsigned short *code_tabs[] = { code_tab_102us, extcode_tab_us102 };
+
+
+#endif /* _TEGRA_NVEC_KEYBOARD_H_ */
diff --git a/board/nvidia/common/board.c b/board/nvidia/common/board.c
index d01abce..8f7f5cb 100644
--- a/board/nvidia/common/board.c
+++ b/board/nvidia/common/board.c
@@ -37,6 +37,9 @@
 #include <asm/arch-tegra/tegra_mmc.h>
 #include <asm/arch-tegra/mmc.h>
 #endif
+#ifdef CONFIG_SYS_I2C_TEGRA_NVEC
+#include <asm/arch-tegra/tegra_nvec.h>
+#endif
 #include <i2c.h>
 #include <spi.h>
 #include "emc.h"
@@ -197,10 +200,19 @@ int board_early_init_f(void)
 
 int board_late_init(void)
 {
+	__maybe_unused int err;
+
 #ifdef CONFIG_LCD
 	/* Make sure we finish initing the LCD */
 	tegra_lcd_check_next_stage(gd->fdt_blob, 1);
 #endif
+
+#ifdef CONFIG_SYS_I2C_TEGRA_NVEC
+	err = board_nvec_init();
+	if (err)
+		debug("NVEC controller init failed: %d\n", err);
+#endif
+
 	return 0;
 }
 
diff --git a/common/Makefile b/common/Makefile
index de5cce8..aeeb626 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -200,6 +200,7 @@ obj-$(CONFIG_CMD_ZFS) += cmd_zfs.o
 # others
 obj-$(CONFIG_BOOTSTAGE) += bootstage.o
 obj-$(CONFIG_CONSOLE_MUX) += iomux.o
+obj-$(CONFIG_ANSI_CONSOLE) += ansi_console.o
 obj-y += flash.o
 obj-$(CONFIG_CMD_KGDB) += kgdb.o kgdb_stubs.o
 obj-$(CONFIG_I2C_EDID) += edid.o
diff --git a/common/ansi_console.c b/common/ansi_console.c
new file mode 100644
index 0000000..f68e9c5
--- /dev/null
+++ b/common/ansi_console.c
@@ -0,0 +1,210 @@
+#include <ansi_console.h>
+
+
+void ansi_putc(struct ansi_console_t* console, const char c)
+{
+	int i;
+
+	if (c == 27) {
+		for (i = 0; i < console->ansi_buf_size; ++i)
+			console->putc(console->ansi_buf[i]);
+		console->ansi_buf[0] = 27;
+		console->ansi_buf_size = 1;
+		return;
+	}
+
+	if (console->ansi_buf_size > 0) {
+		/*
+		 * 0 - ESC
+		 * 1 - [
+		 * 2 - num1
+		 * 3 - ..
+		 * 4 - ;
+		 * 5 - num2
+		 * 6 - ..
+		 * - cchar
+		 */
+		int next = 0;
+
+		int flush = 0;
+		int fail = 0;
+
+		int num1 = 0;
+		int num2 = 0;
+		int cchar = 0;
+
+		console->ansi_buf[console->ansi_buf_size++] = c;
+
+		if (console->ansi_buf_size >= sizeof(console->ansi_buf))
+			fail = 1;
+
+		for (i = 0; i < console->ansi_buf_size; ++i) {
+			if (fail)
+				break;
+
+			switch (next) {
+			case 0:
+				if (console->ansi_buf[i] == 27)
+					next = 1;
+				else
+					fail = 1;
+				break;
+
+			case 1:
+				if (console->ansi_buf[i] == '[')
+					next = 2;
+				else
+					fail = 1;
+				break;
+
+			case 2:
+				if (console->ansi_buf[i] >= '0' && console->ansi_buf[i] <= '9') {
+					num1 = console->ansi_buf[i]-'0';
+					next = 3;
+				} else if (console->ansi_buf[i] != '?') {
+					--i;
+					num1 = 1;
+					next = 4;
+				}
+				break;
+
+			case 3:
+				if (console->ansi_buf[i] >= '0' && console->ansi_buf[i] <= '9') {
+					num1 *= 10;
+					num1 += console->ansi_buf[i]-'0';
+				} else {
+					--i;
+					next = 4;
+				}
+				break;
+
+			case 4:
+				if (console->ansi_buf[i] != ';') {
+					--i;
+					next = 7;
+				} else
+					next = 5;
+				break;
+
+			case 5:
+				if (console->ansi_buf[i] >= '0' && console->ansi_buf[i] <= '9') {
+					num2 = console->ansi_buf[i]-'0';
+					next = 6;
+				} else
+					fail = 1;
+				break;
+
+			case 6:
+				if (console->ansi_buf[i] >= '0' && console->ansi_buf[i] <= '9') {
+					num2 *= 10;
+					num2 += console->ansi_buf[i]-'0';
+				} else {
+					--i;
+					next = 7;
+				}
+				break;
+
+			case 7:
+				if ((console->ansi_buf[i] >= 'A' && console->ansi_buf[i] <= 'H')
+					|| console->ansi_buf[i] == 'J'
+					|| console->ansi_buf[i] == 'K'
+					|| console->ansi_buf[i] == 'h'
+					|| console->ansi_buf[i] == 'l'
+					|| console->ansi_buf[i] == 'm') {
+					cchar = console->ansi_buf[i];
+					flush = 1;
+				} else
+					fail = 1;
+				break;
+			}
+		}
+
+		if (fail) {
+			for (i = 0; i < console->ansi_buf_size; ++i)
+				console->putc(console->ansi_buf[i]);
+			console->ansi_buf_size = 0;
+			return;
+		}
+
+		if (flush) {
+			if (!console->ansi_cursor_hidden && console->cursor_enable)
+				console->cursor_enable(0);
+			console->ansi_buf_size = 0;
+			switch (cchar) {
+			case 'A':
+				/* move cursor num1 rows up */
+				console->cursor_up(num1);
+				break;
+			case 'B':
+				/* move cursor num1 rows down */
+				console->cursor_down(num1);
+				break;
+			case 'C':
+				/* move cursor num1 columns forward */
+				console->cursor_right(num1);
+				break;
+			case 'D':
+				/* move cursor num1 columns back */
+				console->cursor_left(num1);
+				break;
+			case 'E':
+				/* move cursor num1 rows up at begin of row */
+				console->previous_line(num1);
+				break;
+			case 'F':
+				/* move cursor num1 rows down at begin of row */
+				console->new_line(num1);
+				break;
+			case 'G':
+				/* move cursor to column num1 */
+				console->set_position(-1, num1-1);
+				break;
+			case 'H':
+				/* move cursor to row num1, column num2 */
+				console->set_position(num1-1, num2-1);
+				break;
+			case 'J':
+				/* clear console and move cursor to 0, 0 */
+				console->clear();
+				console->set_position(0, 0);
+				break;
+			case 'K':
+				/* clear line */
+				if (num1 == 0)
+					console->clear_line(*console->console_row,
+							*console->console_col,
+							-1);
+				else if (num1 == 1)
+					console->clear_line(*console->console_row,
+							0, *console->console_col);
+				else
+					console->clear_line(*console->console_row,
+							0, -1);
+				break;
+			case 'h':
+				console->ansi_cursor_hidden = 0;
+				break;
+			case 'l':
+				console->ansi_cursor_hidden = 1;
+				break;
+			case 'm':
+				if (num1 == 0) { /* reset swapped colors */
+					if (console->ansi_colors_need_revert && console->swap_colors) {
+						console->swap_colors();
+						console->ansi_colors_need_revert = 0;
+					}
+				} else if (num1 == 7) { /* once swap colors */
+					if (!console->ansi_colors_need_revert && console->swap_colors) {
+						console->swap_colors();
+						console->ansi_colors_need_revert = 1;
+					}
+				}
+				break;
+			}
+			if (!console->ansi_cursor_hidden && console->cursor_set)
+				console->cursor_set();
+		}
+	} else {
+		console->putc(c);
+	}
+}
diff --git a/common/lcd.c b/common/lcd.c
index 19b86b7..f708a79 100644
--- a/common/lcd.c
+++ b/common/lcd.c
@@ -25,6 +25,7 @@
 #include <post.h>
 #endif
 #include <lcd.h>
+#include <ansi_console.h>
 #include <watchdog.h>
 #include <asm/unaligned.h>
 #include <splash.h>
@@ -108,6 +109,7 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+void lcd_position_cursor_n(int col, int row);
 static void lcd_drawchars(ushort x, ushort y, uchar *str, int count);
 static inline void lcd_puts_xy(ushort x, ushort y, uchar *s);
 static inline void lcd_putc_xy(ushort x, ushort y, uchar  c);
@@ -119,6 +121,9 @@ static void *lcd_logo(void);
 static int lcd_getbgcolor(void);
 static void lcd_setfgcolor(int color);
 static void lcd_setbgcolor(int color);
+#ifdef CONFIG_ANSI_CONSOLE
+static void lcd_swap_colors(void);
+#endif
 
 static int lcd_color_fg;
 static int lcd_color_bg;
@@ -126,14 +131,24 @@ int lcd_line_length;
 
 char lcd_is_enabled = 0;
 
+#ifdef CONFIG_ANSI_CONSOLE
+static int console_col;
+static int console_row;
+#else
 static short console_col;
 static short console_row;
+#endif
 
 static void *lcd_console_address;
 static void *lcd_base;			/* Start of framebuffer memory	*/
 
 static char lcd_flush_dcache;	/* 1 to flush dcache after each lcd update */
 
+#ifdef CONFIG_ANSI_CONSOLE
+struct ansi_console_t ansi_console;
+#endif
+
+
 /************************************************************************/
 
 /* Flush LCD activity to the caches */
@@ -167,9 +182,9 @@ void lcd_set_flush_dcache(int flush)
 
 /*----------------------------------------------------------------------*/
 
-static void console_scrollup(void)
+static void console_scrollup_n(int n)
 {
-	const int rows = CONFIG_CONSOLE_SCROLL_LINES;
+	const int rows = n;
 
 	/* Copy up rows ignoring those that will be overwritten */
 	memcpy(CONSOLE_ROW_FIRST,
@@ -199,19 +214,85 @@ static inline void console_back(void)
 		console_row * VIDEO_FONT_HEIGHT, ' ');
 }
 
+
 /*----------------------------------------------------------------------*/
 
+static inline void console_newline_n(int n)
+{
+	console_col = 0;
+
+	console_row += n;
+	/* Check if we need to scroll the terminal */
+	if (console_row >= CONSOLE_ROWS)
+		console_scrollup_n(CONSOLE_ROWS - console_row + 1);
+	else
+		lcd_sync();
+}
+
 static inline void console_newline(void)
 {
+	console_newline_n(1);
+}
+
+/*----------------------------------------------------------------------*/
+
+static inline void console_prevline_n(int n)
+{
 	console_col = 0;
 
+	console_row -= n;
 	/* Check if we need to scroll the terminal */
-	if (++console_row >= CONSOLE_ROWS)
-		console_scrollup();
+	if (console_row < 0)
+		console_scrollup_n(1 - console_row);
 	else
 		lcd_sync();
 }
 
+static inline void console_prevline(void)
+{
+	console_prevline_n(1);
+}
+
+/*----------------------------------------------------------------------*/
+
+static inline void console_cursor_up(int n)
+{
+	console_row -= n;
+	if (console_row < 0)
+		console_row = 0;
+}
+
+static inline void console_cursor_down(int n)
+{
+	console_row += n;
+	if (console_row >= CONSOLE_ROWS)
+		console_row = CONSOLE_ROWS-1;
+}
+
+static inline void console_cursor_left(int n)
+{
+	console_col -= n;
+	if (console_col < 0)
+		console_col = 0;
+}
+
+static inline void console_cursor_right(int n)
+{
+	console_col += n;
+	if (console_col >= CONSOLE_COLS)
+		console_col = CONSOLE_COLS-1;
+}
+
+/*----------------------------------------------------------------------*/
+
+static inline void console_clear_line(int line, int begin, int end)
+{
+	short i = 0;
+	for (i = begin; i < end; ++i)
+		lcd_putc_xy(i * VIDEO_FONT_WIDTH,
+			line * VIDEO_FONT_HEIGHT, ' ');
+}
+
 /*----------------------------------------------------------------------*/
 
 void lcd_putc(const char c)
@@ -262,7 +343,11 @@ void lcd_puts(const char *s)
 	}
 
 	while (*s)
+#ifdef CONFIG_ANSI_CONSOLE
+		ansi_putc(&ansi_console, *s++);
+#else
 		lcd_putc(*s++);
+#endif
 
 	lcd_sync();
 }
@@ -525,6 +610,28 @@ static int lcd_init(void *lcdbase)
 	console_row = 1;	/* leave 1 blank line below logo */
 #endif
 
+#ifdef CONFIG_ANSI_CONSOLE
+	memset(&ansi_console, 0, sizeof(ansi_console));
+	ansi_console.putc = lcd_putc;
+#if defined(CONFIG_CONSOLE_CURSOR) || defined(CONFIG_VIDEO_SW_CURSOR)
+	ansi_console.cursor_set = lcd_set_cursor;
+	ansi_console.cursor_enable = lcd_cursor;
+	/* TODO Add on/off */
+#endif
+	ansi_console.cursor_up = console_cursor_up;
+	ansi_console.cursor_down = console_cursor_down;
+	ansi_console.cursor_left = console_cursor_left;
+	ansi_console.cursor_right = console_cursor_right;
+	ansi_console.previous_line = console_prevline_n;
+	ansi_console.new_line = console_newline_n;
+	ansi_console.set_position = lcd_position_cursor_n;
+	ansi_console.clear_line = console_clear_line;
+	ansi_console.clear = lcd_clear;
+	ansi_console.swap_colors = lcd_swap_colors;
+	ansi_console.console_col = &console_col;
+	ansi_console.console_row = &console_row;
+#endif /* CONFIG_CONSOLE_ANSI */
+
 	return 0;
 }
 
@@ -590,6 +697,17 @@ static int lcd_getbgcolor(void)
 	return lcd_color_bg;
 }
 
+/*----------------------------------------------------------------------*/
+
+#ifdef CONFIG_ANSI_CONSOLE
+static void lcd_swap_colors(void)
+{
+	int tmp = lcd_color_bg;
+	lcd_color_bg = lcd_color_fg;
+	lcd_color_fg = tmp;
+}
+#endif
+
 /************************************************************************/
 /* ** Chipset depending Bitmap / Logo stuff...                          */
 /************************************************************************/
@@ -1141,6 +1259,11 @@ void lcd_position_cursor(unsigned col, unsigned row)
 	console_row = min(row, CONSOLE_ROWS - 1);
 }
 
+void lcd_position_cursor_n(int row, int col)
+{
+	lcd_position_cursor((unsigned)col, (unsigned)row);
+}
+
 int lcd_get_pixel_width(void)
 {
 	return panel_info.vl_col;
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index 96bd45d..d536330 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -29,4 +29,5 @@ obj-$(CONFIG_SYS_I2C_S3C24X0) += s3c24x0_i2c.o
 obj-$(CONFIG_SYS_I2C_SH) += sh_i2c.o
 obj-$(CONFIG_SYS_I2C_SOFT) += soft_i2c.o
 obj-$(CONFIG_SYS_I2C_TEGRA) += tegra_i2c.o
+obj-$(CONFIG_SYS_I2C_TEGRA_NVEC) += tegra_nvec.o
 obj-$(CONFIG_SYS_I2C_ZYNQ) += zynq_i2c.o
diff --git a/drivers/i2c/i2c_core.c b/drivers/i2c/i2c_core.c
index 18d6736..105aa0a 100644
--- a/drivers/i2c/i2c_core.c
+++ b/drivers/i2c/i2c_core.c
@@ -395,6 +395,19 @@ void i2c_reg_write(uint8_t addr, uint8_t reg, uint8_t val)
 	i2c_write(addr, reg, 1, &val, 1);
 }
 
+int i2c_slave_io(struct i2c_transaction *trans)
+{
+	struct i2c_adapter *cur = I2C_ADAP;
+
+	if (!cur->slave_io) {
+		printf("Error: slave IO is not supported on adap %d\n",
+		       cur->hwadapnr);
+		return -1;
+	}
+
+	return cur->slave_io(cur, trans);
+}
+
 void __i2c_init(int speed, int slaveaddr)
 {
 	i2c_init_bus(i2c_get_bus_num(), speed, slaveaddr);
diff --git a/drivers/i2c/tegra_i2c.c b/drivers/i2c/tegra_i2c.c
index 257b72f..a10e7be 100644
--- a/drivers/i2c/tegra_i2c.c
+++ b/drivers/i2c/tegra_i2c.c
@@ -22,6 +22,7 @@ DECLARE_GLOBAL_DATA_PTR;
 /* Information about i2c controller */
 struct i2c_bus {
 	int			id;
+	int			node;
 	enum periph_id		periph_id;
 	int			speed;
 	int			pinmux_config;
@@ -30,10 +31,31 @@ struct i2c_bus {
 	int			is_dvc;	/* DVC type, rather than I2C */
 	int			is_scs;	/* single clock source (T114+) */
 	int			inited;	/* bus is inited */
+	int			slave_addr;
 };
 
 static struct i2c_bus i2c_controllers[TEGRA_I2C_NUM_CONTROLLERS];
 
+/**
+ * Init i2c controller to operate in slave mode.
+ *
+ * @param bus	i2c bus/controller state struct
+ */
+static void set_slave_mode(struct i2c_bus *bus)
+{
+	unsigned long val;
+
+	val = I2C_CNFG_NEW_MASTER_FSM_MASK | I2C_CNFG_PACKET_MODE_MASK |
+	    I2C_CNFG_DEBOUNCE_CNT_MASK;
+	writel(val, &bus->regs->cnfg);
+
+	writel(I2C_SL_CNFG_NEWSL_MASK, &bus->regs->sl_cnfg);
+	writel(0x1E, &bus->regs->sl_delay_count);
+
+	writel(bus->slave_addr >> 1, &bus->regs->sl_addr1);
+	writel(0, &bus->regs->sl_addr2);
+}
+
 static void set_packet_mode(struct i2c_bus *i2c_bus)
 {
 	u32 config;
@@ -59,8 +81,12 @@ static void i2c_reset_controller(struct i2c_bus *i2c_bus)
 	/* Reset I2C controller. */
 	reset_periph(i2c_bus->periph_id, 1);
 
-	/* re-program config register to packet mode */
-	set_packet_mode(i2c_bus);
+	if (i2c_bus->slave_addr == 0) {
+		/* re-program config register to packet mode */
+		set_packet_mode(i2c_bus);
+	} else {
+		set_slave_mode(i2c_bus);
+	}
 }
 
 static void i2c_init_controller(struct i2c_bus *i2c_bus)
@@ -196,6 +222,121 @@ static int wait_for_transfer_complete(struct i2c_control *control)
 	return -1;
 }
 
+
+#define I2C_SL_IRQ		(1<<3)
+#define END_TRANS		(1<<4)
+#define RCVD			(1<<2)
+#define RNW			(1<<1)
+
+
+static inline int is_ready(unsigned long status)
+{
+	return status & I2C_SL_IRQ;
+}
+
+static inline int is_read(unsigned long status)
+{
+	return (status & RNW) == 0;
+}
+
+static inline int is_trans_start(unsigned long status)
+{
+	return status & RCVD;
+}
+
+static inline int is_trans_end(unsigned long status)
+{
+	return status & END_TRANS;
+}
+
+
+/**
+ * Send or receive packet in slave mode.
+ *
+ * @param i2c_bus	pointer to bus structure
+ * @param trans		I2C transaction object
+ *
+ * @return	0 if succeeded,
+ *		1 if not ready,
+ *		2 if operation timed out,
+ *		3 if not our packet,
+ *		other - unknown error.
+ */
+static int slave_send_recv_packets(struct i2c_bus *i2c_bus,
+				   struct i2c_transaction *trans)
+{
+	unsigned int poll_start_ms = 0;
+	unsigned long status;
+
+	unsigned int received = 0;
+	unsigned int to_send = 0;
+	unsigned int timer_ms = 0;
+	int addr = -1;
+
+	poll_start_ms = get_timer(0);
+
+	while (1) {
+		status = readl(&i2c_bus->regs->sl_status);
+		if (!is_ready(status)) {
+			timer_ms = get_timer(poll_start_ms);
+			if (addr != i2c_bus->slave_addr &&
+			    trans->start_timeout &&
+			    timer_ms > trans->start_timeout) {
+				trans->res = 1;
+				return 1; /*not ready*/
+			}
+
+			if (timer_ms > trans->timeout) {
+				trans->res = 2;
+				return 2; /*timeout*/
+			}
+
+			udelay(100);
+			continue;
+		}
+
+		if (!is_trans_start(status) && addr != i2c_bus->slave_addr) {
+			trans->res = 3;
+			return 3; /* not our packet, retry */
+		}
+
+		if (is_trans_start(status)) {
+			if (!is_read(status) && addr != i2c_bus->slave_addr) {
+				trans->res = 3;
+				return 3; /* not our packet, retry */
+			}
+			if (is_read(status)) {
+				addr = readl(&i2c_bus->regs->sl_rcvd);
+				trans->rx_buf[trans->rx_pos++] = addr;
+				continue;
+			}
+		}
+
+		if (is_trans_end(status)) {
+			/* Check for repeated start */
+			if (!is_trans_start(status)) {
+				trans->res = 0;
+				return 0;
+			}
+		}
+
+		if (is_read(status)) {
+			/* TODO Check sizes */
+			received = readl(&i2c_bus->regs->sl_rcvd);
+			trans->rx_buf[trans->rx_pos++] = received;
+		} else {
+			/* TODO Check sizes */
+			to_send = trans->tx_buf[trans->tx_pos++];
+			writel(to_send, &i2c_bus->regs->sl_rcvd);
+		}
+	}
+
+	/* not reachable */
+	trans->res = 4;
+	return 4;
+}
+
+
 static int send_recv_packets(struct i2c_bus *i2c_bus,
 			     struct i2c_trans_info *trans)
 {
@@ -351,6 +492,7 @@ static unsigned int tegra_i2c_set_bus_speed(struct i2c_adapter *adap,
 
 static int i2c_get_config(const void *blob, int node, struct i2c_bus *i2c_bus)
 {
+	i2c_bus->node = node;
 	i2c_bus->regs = (struct i2c_ctlr *)fdtdec_get_addr(blob, node, "reg");
 
 	/*
@@ -373,6 +515,8 @@ static int i2c_get_config(const void *blob, int node, struct i2c_bus *i2c_bus)
 	if (i2c_bus->periph_id == -1)
 		return -FDT_ERR_NOTFOUND;
 
+	i2c_bus->slave_addr = fdtdec_get_int(blob, node, "slave-addr", -1);
+
 	return 0;
 }
 
@@ -430,6 +574,28 @@ static int process_nodes(const void *blob, int node_list[], int count,
 	return 0;
 }
 
+static int tegra_i2c_slave_io(struct i2c_adapter *adap,
+			      struct i2c_transaction *trans)
+{
+	struct i2c_bus *bus;
+	debug("tegra_i2c_slave_io: hwadapnr=%d\n", adap->hwadapnr);
+
+	bus = tegra_i2c_get_bus(adap);
+	if (!bus) {
+		error("tegra_i2c_slave_io: no bus for adapter %d\n",
+		      adap->hwadapnr);
+		return -1;
+	}
+
+	if (!bus->slave_addr) {
+		error("tegra_i2c_slave_io: adapter %d isn't in slave mode\n",
+		      adap->hwadapnr);
+		return -2;
+	}
+
+	return slave_send_recv_packets(bus, trans);
+}
+
 /* Sadly there is no error return from this function */
 void i2c_init_board(void)
 {
@@ -461,11 +627,20 @@ void i2c_init_board(void)
 
 static void tegra_i2c_init(struct i2c_adapter *adap, int speed, int slaveaddr)
 {
+	struct i2c_bus *bus;
+
 	/* No i2c support prior to relocation */
 	if (!(gd->flags & GD_FLG_RELOC))
 		return;
 
-	/* This will override the speed selected in the fdt for that port */
+	bus = tegra_i2c_get_bus(adap);
+	if (bus) {
+		adap->slave_io = tegra_i2c_slave_io;
+		debug("i2c_init: ignore static init for adapter %d\n",
+		      adap->hwadapnr);
+		return;
+	}
+
 	debug("i2c_init(speed=%u, slaveaddr=0x%x)\n", speed, slaveaddr);
 	i2c_set_bus_speed(speed);
 }
@@ -624,6 +799,24 @@ int tegra_i2c_get_dvc_bus_num(void)
 	return -1;
 }
 
+/**
+ * Find the I2C bus number by given a FDT I2C node.
+ *
+ * @param blob  Device tree blbo
+ * @param node  FDT I2C node to find
+ * @return the number of I2C bus (zero based), or -1 on error
+ */
+int i2c_get_bus_num_fdt(int node)
+{
+	int i;
+
+	for (i = 0; i < TEGRA_I2C_NUM_CONTROLLERS; ++i)
+		if (i2c_controllers[i].node == node)
+			return i2c_controllers[i].id;
+
+	return -1;
+}
+
 /*
  * Register soft i2c adapters
  */
diff --git a/drivers/i2c/tegra_nvec.c b/drivers/i2c/tegra_nvec.c
new file mode 100644
index 0000000..b568988
--- /dev/null
+++ b/drivers/i2c/tegra_nvec.c
@@ -0,0 +1,294 @@
+/*
+ * (C) Copyright 2014
+ * Andrey Danin <danindrey@mail.ru>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <fdtdec.h>
+#include <i2c.h>
+#include <asm/io.h>
+#include <asm/gpio.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/funcmux.h>
+#include <asm/arch-tegra/tegra_nvec.h>
+
+#ifndef CONFIG_SYS_I2C_TEGRA_NVEC
+#error "You should enable CONFIG_SYS_I2C_TEGRA_NVEC"
+#endif
+
+DECLARE_GLOBAL_DATA_PTR;
+
+struct nvec_periph devices[NVEC_LAST_MSG];
+
+
+struct nvec_t {
+	int			i2c_bus;
+	struct fdt_gpio_state	gpio;
+} nvec_data;
+
+
+/* nvec commands */
+char noop[] = { NVEC_CNTL, CNTL_NOOP };
+
+void nvec_signal_request(void)
+{
+	gpio_set_value(nvec_data.gpio.gpio, 0);
+}
+
+
+void nvec_clear_request(void)
+{
+	gpio_set_value(nvec_data.gpio.gpio, 1);
+}
+
+
+int nvec_msg_is_event(const unsigned char *msg)
+{
+	return msg[0] >> 7;
+}
+
+
+int nvec_msg_event_type(const unsigned char *msg)
+{
+	return msg[0] & 0x0f;
+}
+
+
+/**
+ * Process incoming io message.
+ * If message is keyboard event then key code will
+ * be added to keys buffer.
+ *
+ * @param nvec	nvec state struct
+ */
+void nvec_process_msg(struct nvec_t *nvec, struct i2c_transaction *trans)
+{
+	(void) nvec;
+
+	const unsigned char *msg = (const unsigned char *)&trans->rx_buf[1];
+	int event_type;
+
+	if (!nvec_msg_is_event(msg))
+		return;
+
+	event_type = nvec_msg_event_type(msg);
+
+	if (event_type < NVEC_KEYBOARD || event_type >= NVEC_LAST_MSG)
+		return;
+
+	if (devices[event_type].process_msg)
+		devices[event_type].process_msg(msg);
+}
+
+
+/**
+ * Perform complete io operation (read or write).
+ * NOTE: function will wait NVEC_TIMEOUT_MIN (20ms)
+ * before status check to avoid nvec hang.
+ *
+ * @param nvec		nvec state struct
+ * @param wait_for_ec	if 1(NVEC_WAIT_FOR_EC) operation
+ *			timeout is NVEC_TIMEOUT_MAX (600ms),
+ *			otherwise function will return if io
+ *			is not ready.
+ *
+ * @return nvec_io_* code
+ */
+int nvec_do_io(struct nvec_t *nvec)
+{
+	static unsigned int prev_timer;
+	unsigned int poll_start_ms = get_timer(prev_timer);
+	struct i2c_transaction trans;
+	int res;
+
+	if (poll_start_ms > 30000) {
+		if (prev_timer != 0)
+			nvec_do_request(noop, sizeof(noop));
+		prev_timer = get_timer(0);
+	}
+
+	memset(&trans, 0, sizeof(trans));
+	trans.start_timeout = 1;
+	trans.timeout = 6000;
+
+	res = i2c_slave_io(&trans);
+	if (res == 0) {
+		nvec_process_msg(nvec, &trans);
+		return 0;
+	}
+
+	if (res != -1)
+		debug("Error: i2c slave io failed with code %d\n", res);
+
+	return -1;
+}
+
+/**
+ * Send request and read response. If write or read failed
+ * operation will be repeated NVEC_ATTEMPTS_MAX times.
+ *
+ * @param buf	request data
+ * @param size	request data size
+ * @return 0 if ok, -1 on error
+ */
+int nvec_do_request(char *buf, int size)
+{
+	int res;
+	struct i2c_transaction trans;
+	int i;
+
+	nvec_signal_request();
+
+	/* Request */
+	for (i = 0; i < 10; ++i) {
+		memset(&trans, 0, sizeof(trans));
+		trans.start_timeout = 600;
+		trans.timeout = 6000;
+
+		trans.tx_buf[0] = (char)size;
+		memcpy(&trans.tx_buf[1], buf, size);
+		trans.tx_size = size + 1;
+
+		res = i2c_slave_io(&trans);
+		if (res == 0) {
+			if (trans.tx_pos == trans.tx_size)
+				break;
+
+			debug("Request was not sent completely");
+		} else if (res != -1) {
+			debug("Unknown error while slave io");
+		}
+	}
+	nvec_clear_request();
+	if (res != 0) {
+		error("nvec failed to perform request\n");
+		return -1;
+	}
+
+	/* Response */
+	for (i = 0; i < 10; ++i) {
+		memset(&trans, 0, sizeof(trans));
+		trans.start_timeout = 600;
+		trans.timeout = 6000;
+
+		res = i2c_slave_io(&trans);
+		if (res == 0)
+			break;
+	}
+	if (res != 0) {
+		error("nvec failed to read response\n");
+		return -1;
+	}
+
+	/* TODO Parse response */
+
+	return 0;
+}
+
+
+/**
+ * Decode the nvec information from the fdt.
+ *
+ * @param blob		fdt blob
+ * @param nvec		nvec device sturct
+ * @return 0 if ok, -ve on error
+ */
+static int nvec_decode_config(const void *blob,
+			      struct nvec_t *nvec)
+{
+	int node, parent;
+	int i2c_bus;
+
+	node = fdtdec_next_compatible(blob, 0, COMPAT_NVIDIA_TEGRA20_NVEC);
+	if (node < 0) {
+		error("Cannot find NVEC node in fdt\n");
+		return node;
+	}
+
+	parent = fdt_parent_offset(blob, node);
+	if (parent < 0) {
+		debug("%s: Cannot find node parent\n", __func__);
+		return -1;
+	}
+
+	i2c_bus = i2c_get_bus_num_fdt(parent);
+	if (i2c_bus < 0)
+		return -1;
+	nvec->i2c_bus = i2c_bus;
+
+	if (fdtdec_decode_gpio(blob, node, "request-gpios",
+			       &nvec->gpio)) {
+		error("No NVEC request gpio\n");
+		return -1;
+	}
+
+	debug("NVEC: i2c:%d, gpio:%s(%u)\n",
+	      nvec->i2c_bus, nvec->gpio.name, nvec->gpio.gpio);
+	return 0;
+}
+
+
+int board_nvec_init(void)
+{
+	int res = 0;
+	int i;
+
+	if (nvec_decode_config(gd->fdt_blob, &nvec_data)) {
+		error("Can't parse NVEC node in device tree\n");
+		return -1;
+	}
+
+	debug("NVEC initialization...\n");
+
+	res = gpio_request(nvec_data.gpio.gpio, NULL);
+	if (res != 0)
+		error("NVEC: err, gpio_request\n");
+	res = gpio_direction_output(nvec_data.gpio.gpio, 1);
+	if (res != 0)
+		error("NVEC: err, gpio_direction\n");
+	res = gpio_set_value(nvec_data.gpio.gpio, 1);
+	if (res != 0)
+		error("NVEC: err, gpio_set_value\n");
+	udelay(100);
+
+	i2c_set_bus_num(nvec_data.i2c_bus);
+
+	for (i = NVEC_KEYBOARD; i < NVEC_LAST_MSG; ++i)
+		if (devices[i].start) {
+			debug("Starting device %d(0x%x)\n", i, i);
+			devices[i].start();
+		}
+
+	return 1;
+}
+
+
+int nvec_read_events(void)
+{
+	int res;
+	int cnt = 0;
+
+	while (++cnt <= 8) {
+		res = nvec_do_io(&nvec_data);
+		if (res)
+			break;
+
+		/* TODO Process nvec communication errors */
+	}
+
+	return cnt;
+}
+
+
+int nvec_register_periph(int msg_type, struct nvec_periph *periph)
+{
+	if (devices[msg_type].start || devices[msg_type].process_msg) {
+		error("Device for msg %d already registered\n", msg_type);
+		return -1;
+	}
+
+	devices[msg_type] = *periph;
+	return 0;
+}
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index a8e9be2..38c576f 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -7,6 +7,9 @@
 
 obj-$(CONFIG_I8042_KBD) += i8042.o
 obj-$(CONFIG_TEGRA_KEYBOARD) += tegra-kbc.o
+ifdef CONFIG_SYS_I2C_TEGRA_NVEC
+obj-$(CONFIG_TEGRA_NVEC_KEYBOARD) += tegra-nvec-kbc.o
+endif
 obj-$(CONFIG_CROS_EC_KEYB) += cros_ec_keyb.o
 ifdef CONFIG_PS2KBD
 obj-y += keyboard.o pc_keyb.o
diff --git a/drivers/input/tegra-nvec-kbc.c b/drivers/input/tegra-nvec-kbc.c
new file mode 100644
index 0000000..f5832b1
--- /dev/null
+++ b/drivers/input/tegra-nvec-kbc.c
@@ -0,0 +1,215 @@
+/*
+ *  (C) Copyright 2014
+ *  Andrey Danin <danindrey@mail.ru>
+ *  (C) Copyright 2011
+ *  NVIDIA Corporation <www.nvidia.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <input.h>
+#include <asm/arch-tegra/tegra_nvec.h>
+#include <asm/arch-tegra/tegra_nvec_keyboard.h>
+
+
+enum {
+	KBC_MAX_KPENT = 8,
+	KBC_REPEAT_RATE_MS	= 30,
+	KBC_REPEAT_DELAY_MS	= 240,
+};
+
+/* keyboard config/state */
+static struct keyb {
+	int registered;
+	struct input_config input;	/* The input layer */
+	int pressed_keys[KBC_MAX_KPENT];
+	int pressed_keys_cnt;
+} config;
+
+
+/* 0 - pressed, other - released */
+char keys[256];
+
+
+/* nvec commands */
+static char enable_kbd[] = { NVEC_KBD, ENABLE_KBD };
+static char reset_kbd[] = { NVEC_PS2, MOUSE_SEND_CMD, MOUSE_RESET, 3 };
+static char clear_leds[] = { NVEC_KBD, SET_LEDS, 0 };
+
+
+/**
+ * Check the tegra nvec keyboard, and send any keys that are pressed.
+ *
+ * This is called by input_tstc() and input_getc() when they need more
+ * characters
+ *
+ * @param input		Input configuration
+ * @return 1, to indicate that we have something to look at
+ */
+int tegra_nvec_kbc_check(struct input_config *input)
+{
+	int i;
+
+	nvec_read_events();
+
+	config.pressed_keys_cnt = 0;
+	/* TODO Optimization required */
+	for (i = 0; i < sizeof(keys); ++i) {
+		if (keys[i] == 0 && config.pressed_keys_cnt < KBC_MAX_KPENT) {
+			config.pressed_keys[config.pressed_keys_cnt] = i;
+			++config.pressed_keys_cnt;
+		}
+	}
+
+	input_send_keycodes(input,
+			    config.pressed_keys,
+			    config.pressed_keys_cnt);
+
+	return config.pressed_keys_cnt;
+}
+
+
+/**
+ * Test if keys are available to be read
+ *
+ * @return 0 if no keys available, 1 if keys are available
+ */
+static int kbd_tstc(void)
+{
+	/* Just get input to do this for us */
+	return input_tstc(&config.input);
+}
+
+
+/**
+ * Read a key
+ *
+ * TODO: U-Boot wants 0 for no key, but Ctrl-@ is a valid key...
+ *
+ * @return ASCII key code, or 0 if no key, or -1 if error
+ */
+static int kbd_getc(void)
+{
+	/* Just get input to do this for us */
+	return input_getc(&config.input);
+}
+
+
+int tegra_nvec_process_keyboard_msg(const unsigned char *msg)
+{
+	int code, state;
+	int event_type;
+	int _size;
+	int key;
+
+
+	event_type = nvec_msg_event_type(msg);
+	if (event_type != NVEC_KEYBOARD)
+		return -1;
+
+	_size = (msg[0] & (3 << 5)) >> 5;
+
+	if (_size == NVEC_VAR_SIZE) {
+		debug("Skip unsupported msg (size: %d)\n", _size);
+		return -1;
+	}
+
+	if (_size == NVEC_3BYTES)
+		msg++;
+
+	code = msg[1] & 0x7f;
+	state = msg[1] & 0x80;
+
+	key = code_tabs[_size][code];
+	keys[key] = state;
+
+	return 0;
+}
+
+
+int tegra_nvec_enable_kbd_events(void)
+{
+	if (nvec_do_request(reset_kbd, 4)) {
+		error("NVEC: failed to reset keyboard\n");
+		return -1;
+	}
+	if (nvec_do_request(clear_leds, 3)) {
+		error("NVEC: failed to clear leds\n");
+		return -1;
+	}
+	if (nvec_do_request(enable_kbd, 2)) {
+		error("NVEC: failed to enable keyboard\n");
+		return -1;
+	}
+
+	debug("NVEC: keyboard initialization finished\n");
+
+	return 0;
+}
+
+
+static int tegra_nvec_kbc_start(void)
+{
+	if (config.registered)
+		return 0;
+
+	struct nvec_periph nvec_keyboard;
+	memset(&nvec_keyboard, 0, sizeof(nvec_keyboard));
+	nvec_keyboard.start = tegra_nvec_enable_kbd_events;
+	nvec_keyboard.process_msg = tegra_nvec_process_keyboard_msg;
+
+	if (nvec_register_periph(NVEC_KEYBOARD, &nvec_keyboard)) {
+		error("NVEC: failed to register keyboard perephirial device");
+		return -1;
+	}
+
+	config.registered = 1;
+
+	return 0;
+}
+
+
+int drv_keyboard_init(void)
+{
+	struct stdio_dev dev;
+	int error;
+#ifdef CONFIG_CONSOLE_MUX
+	char *stdinname = getenv("stdin");
+#endif
+
+	memset(keys, 1, sizeof(keys));
+
+	config.registered = 0;
+
+	if (input_init(&config.input, 0)) {
+		printf("nvec kbc: cannot set up input\n");
+		return -1;
+	}
+	input_set_delays(&config.input, KBC_REPEAT_DELAY_MS,
+			 KBC_REPEAT_RATE_MS);
+	config.input.read_keys = tegra_nvec_kbc_check;
+
+	memset(&dev, '\0', sizeof(dev));
+	strcpy(dev.name, "tegra-nvec-kbc");
+	dev.flags = DEV_FLAGS_INPUT | DEV_FLAGS_SYSTEM;
+	dev.getc = kbd_getc;
+	dev.tstc = kbd_tstc;
+	dev.start = tegra_nvec_kbc_start;
+
+	/* Register the device. tegra_nvec_kbc_start() will be called soon */
+	error = input_stdio_register(&dev);
+	if (error) {
+		printf("nvec kbc: failed to register stdio device, %d\n",
+		       error);
+		return error;
+	}
+#ifdef CONFIG_CONSOLE_MUX
+	error = iomux_doenv(stdin, stdinname);
+	if (error) {
+		printf("nvec kbc: iomux_doenv failed, %d\n", error);
+		return error;
+	}
+#endif
+	return 0;
+}
diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index b52e9ed..0793f73 100644
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -90,6 +90,7 @@
 #include <version.h>
 #include <malloc.h>
 #include <linux/compiler.h>
+#include <ansi_console.h>
 
 /*
  * Console device defines with SMI graphic
@@ -371,10 +372,7 @@ static u32 eorx, fgx, bgx;	/* color pats */
 static int cfb_do_flush_cache;
 
 #ifdef CONFIG_CFB_CONSOLE_ANSI
-static char ansi_buf[10];
-static int ansi_buf_size;
-static int ansi_colors_need_revert;
-static int ansi_cursor_hidden;
+struct ansi_console_t ansi_console;
 #endif
 
 static const int video_font_draw_table8[] = {
@@ -866,7 +864,7 @@ static void console_swap_colors(void)
 
 static inline int console_cursor_is_visible(void)
 {
-	return !ansi_cursor_hidden;
+	return !ansi_console.ansi_cursor_hidden;
 }
 #else
 static inline int console_cursor_is_visible(void)
@@ -947,210 +945,7 @@ static void parse_putc(const char c)
 void video_putc(const char c)
 {
 #ifdef CONFIG_CFB_CONSOLE_ANSI
-	int i;
-
-	if (c == 27) {
-		for (i = 0; i < ansi_buf_size; ++i)
-			parse_putc(ansi_buf[i]);
-		ansi_buf[0] = 27;
-		ansi_buf_size = 1;
-		return;
-	}
-
-	if (ansi_buf_size > 0) {
-		/*
-		 * 0 - ESC
-		 * 1 - [
-		 * 2 - num1
-		 * 3 - ..
-		 * 4 - ;
-		 * 5 - num2
-		 * 6 - ..
-		 * - cchar
-		 */
-		int next = 0;
-
-		int flush = 0;
-		int fail = 0;
-
-		int num1 = 0;
-		int num2 = 0;
-		int cchar = 0;
-
-		ansi_buf[ansi_buf_size++] = c;
-
-		if (ansi_buf_size >= sizeof(ansi_buf))
-			fail = 1;
-
-		for (i = 0; i < ansi_buf_size; ++i) {
-			if (fail)
-				break;
-
-			switch (next) {
-			case 0:
-				if (ansi_buf[i] == 27)
-					next = 1;
-				else
-					fail = 1;
-				break;
-
-			case 1:
-				if (ansi_buf[i] == '[')
-					next = 2;
-				else
-					fail = 1;
-				break;
-
-			case 2:
-				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
-					num1 = ansi_buf[i]-'0';
-					next = 3;
-				} else if (ansi_buf[i] != '?') {
-					--i;
-					num1 = 1;
-					next = 4;
-				}
-				break;
-
-			case 3:
-				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
-					num1 *= 10;
-					num1 += ansi_buf[i]-'0';
-				} else {
-					--i;
-					next = 4;
-				}
-				break;
-
-			case 4:
-				if (ansi_buf[i] != ';') {
-					--i;
-					next = 7;
-				} else
-					next = 5;
-				break;
-
-			case 5:
-				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
-					num2 = ansi_buf[i]-'0';
-					next = 6;
-				} else
-					fail = 1;
-				break;
-
-			case 6:
-				if (ansi_buf[i] >= '0' && ansi_buf[i] <= '9') {
-					num2 *= 10;
-					num2 += ansi_buf[i]-'0';
-				} else {
-					--i;
-					next = 7;
-				}
-				break;
-
-			case 7:
-				if ((ansi_buf[i] >= 'A' && ansi_buf[i] <= 'H')
-					|| ansi_buf[i] == 'J'
-					|| ansi_buf[i] == 'K'
-					|| ansi_buf[i] == 'h'
-					|| ansi_buf[i] == 'l'
-					|| ansi_buf[i] == 'm') {
-					cchar = ansi_buf[i];
-					flush = 1;
-				} else
-					fail = 1;
-				break;
-			}
-		}
-
-		if (fail) {
-			for (i = 0; i < ansi_buf_size; ++i)
-				parse_putc(ansi_buf[i]);
-			ansi_buf_size = 0;
-			return;
-		}
-
-		if (flush) {
-			if (!ansi_cursor_hidden)
-				CURSOR_OFF;
-			ansi_buf_size = 0;
-			switch (cchar) {
-			case 'A':
-				/* move cursor num1 rows up */
-				console_cursor_up(num1);
-				break;
-			case 'B':
-				/* move cursor num1 rows down */
-				console_cursor_down(num1);
-				break;
-			case 'C':
-				/* move cursor num1 columns forward */
-				console_cursor_right(num1);
-				break;
-			case 'D':
-				/* move cursor num1 columns back */
-				console_cursor_left(num1);
-				break;
-			case 'E':
-				/* move cursor num1 rows up at begin of row */
-				console_previousline(num1);
-				break;
-			case 'F':
-				/* move cursor num1 rows down at begin of row */
-				console_newline(num1);
-				break;
-			case 'G':
-				/* move cursor to column num1 */
-				console_cursor_set_position(-1, num1-1);
-				break;
-			case 'H':
-				/* move cursor to row num1, column num2 */
-				console_cursor_set_position(num1-1, num2-1);
-				break;
-			case 'J':
-				/* clear console and move cursor to 0, 0 */
-				console_clear();
-				console_cursor_set_position(0, 0);
-				break;
-			case 'K':
-				/* clear line */
-				if (num1 == 0)
-					console_clear_line(console_row,
-							console_col,
-							CONSOLE_COLS-1);
-				else if (num1 == 1)
-					console_clear_line(console_row,
-							0, console_col);
-				else
-					console_clear_line(console_row,
-							0, CONSOLE_COLS-1);
-				break;
-			case 'h':
-				ansi_cursor_hidden = 0;
-				break;
-			case 'l':
-				ansi_cursor_hidden = 1;
-				break;
-			case 'm':
-				if (num1 == 0) { /* reset swapped colors */
-					if (ansi_colors_need_revert) {
-						console_swap_colors();
-						ansi_colors_need_revert = 0;
-					}
-				} else if (num1 == 7) { /* once swap colors */
-					if (!ansi_colors_need_revert) {
-						console_swap_colors();
-						ansi_colors_need_revert = 1;
-					}
-				}
-				break;
-			}
-			if (!ansi_cursor_hidden)
-				CURSOR_SET;
-		}
-	} else {
-		parse_putc(c);
-	}
+	ansi_putc(&ansi_console, c);
 #else
 	parse_putc(c);
 #endif
@@ -2233,6 +2028,32 @@ static int video_init(void)
 	if (cfb_do_flush_cache)
 		flush_cache(VIDEO_FB_ADRS, VIDEO_SIZE);
 
+	memset(&ansi_console, 0, sizeof(ansi_console));
+	ansi_console.putc = parse_putc;
+
+#if defined(CONFIG_CONSOLE_CURSOR) || defined(CONFIG_VIDEO_SW_CURSOR)
+	ansi_console.cursor_set = video_set_cursor;
+	ansi_console.cursor_enable = console_cursor;
+	/* TODO Add on/off */
+#endif
+	ansi_console.cursor_up = console_cursor_up;
+	ansi_console.cursor_up = console_cursor_up;
+	ansi_console.cursor_down = console_cursor_down;
+	ansi_console.cursor_left = console_cursor_left;
+	ansi_console.cursor_right = console_cursor_right;
+	ansi_console.previous_line = console_previousline;
+	ansi_console.new_line = console_newline;
+
+	ansi_console.set_position = console_cursor_set_position;
+
+	ansi_console.clear_line = console_clear_line;
+
+	ansi_console.clear = console_clear;
+	ansi_console.swap_colors = console_swap_colors;
+
+	ansi_console.console_col = &console_col;
+	ansi_console.console_row = &console_row;
+
 	return 0;
 }
 
diff --git a/include/ansi_console.h b/include/ansi_console.h
new file mode 100644
index 0000000..1f70747
--- /dev/null
+++ b/include/ansi_console.h
@@ -0,0 +1,42 @@
+/*
+ * (C) Copyright 2012
+ * Pali Rohár <pali.rohar@gmail.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+/*
+ * ANSI terminal
+ */
+
+#include <common.h>
+
+struct ansi_console_t {
+	void (*putc)(const char c);
+
+	void (*cursor_set)(void);
+	void (*cursor_enable)(int enable);
+	void (*cursor_up)(int n);
+	void (*cursor_down)(int n);
+	void (*cursor_left)(int n);
+	void (*cursor_right)(int n);
+	void (*previous_line)(int n);
+	void (*new_line)(int n);
+
+	void (*set_position)(int x, int y);
+
+	void (*clear_line)(int n, int start, int end);
+
+	void (*clear)(void);
+	void (*swap_colors)(void);
+
+	int* console_col;
+	int* console_row;
+
+	char ansi_buf[10];
+	int ansi_buf_size;
+	int ansi_colors_need_revert;
+	int ansi_cursor_hidden;
+};
+
+void ansi_putc(struct ansi_console_t* console, const char c);
diff --git a/include/configs/paz00.h b/include/configs/paz00.h
index dd0abf8..27ca0ba 100644
--- a/include/configs/paz00.h
+++ b/include/configs/paz00.h
@@ -72,6 +72,22 @@
 #define CONFIG_SYS_WHITE_ON_BLACK
 #define CONFIG_CONSOLE_SCROLL_LINES	10
 
+/* Keyboard support */
+#define CONFIG_KEYBOARD
+#define CONFIG_TEGRA_NVEC_KEYBOARD
+/* NVEC support */
+#define CONFIG_SYS_I2C
+#define CONFIG_SYS_I2C_TEGRA
+#define CONFIG_SYS_I2C_INIT_BOARD
+#define CONFIG_SYS_I2C_TEGRA_NVEC
+
+#define CONFIG_ANSI_CONSOLE
+#define CONFIG_CMD_BOOTMENU
+#define CONFIG_MENU
+#define CONFIG_AUTOBOOT_KEYED
+
 #include "tegra-common-post.h"
 
+#define CONFIG_INITRD_TAG 1
+
 #endif /* __CONFIG_H */
diff --git a/include/configs/tegra-common-post.h b/include/configs/tegra-common-post.h
index 1c770c9..2d0b07a 100644
--- a/include/configs/tegra-common-post.h
+++ b/include/configs/tegra-common-post.h
@@ -142,6 +142,8 @@
 
 #ifdef CONFIG_TEGRA_KEYBOARD
 #define STDIN_KBD_KBC ",tegra-kbc"
+#elif defined(CONFIG_TEGRA_NVEC_KEYBOARD)
+#define STDIN_KBD_KBC ",tegra-nvec-kbc"
 #else
 #define STDIN_KBD_KBC ""
 #endif
diff --git a/include/fdtdec.h b/include/fdtdec.h
index a7e6ee7..5be662a 100644
--- a/include/fdtdec.h
+++ b/include/fdtdec.h
@@ -64,6 +64,7 @@ enum fdt_compat_id {
 	COMPAT_NVIDIA_TEGRA20_SDMMC,	/* Tegra20 SDMMC controller */
 	COMPAT_NVIDIA_TEGRA20_SFLASH,	/* Tegra 2 SPI flash controller */
 	COMPAT_NVIDIA_TEGRA20_SLINK,	/* Tegra 2 SPI SLINK controller */
+	COMPAT_NVIDIA_TEGRA20_NVEC,	/* Tegra 2 EC controller */
 	COMPAT_NVIDIA_TEGRA114_SPI,	/* Tegra 114 SPI controller */
 	COMPAT_SMSC_LAN9215,		/* SMSC 10/100 Ethernet LAN9215 */
 	COMPAT_SAMSUNG_EXYNOS5_SROMC,	/* Exynos5 SROMC */
diff --git a/include/i2c.h b/include/i2c.h
index 1b4078e..1e32c5d 100644
--- a/include/i2c.h
+++ b/include/i2c.h
@@ -55,6 +55,20 @@
 #define CONFIG_SYS_SPD_BUS_NUM		0
 #endif
 
+struct i2c_transaction {
+	char rx_buf[34];
+	int rx_pos;
+
+	char tx_buf[34];
+	int tx_pos;
+	int tx_size;
+
+	unsigned int start_timeout;
+	unsigned int timeout;
+
+	int res;
+};
+
 struct i2c_adapter {
 	void		(*init)(struct i2c_adapter *adap, int speed,
 				int slaveaddr);
@@ -65,6 +79,8 @@ struct i2c_adapter {
 	int		(*write)(struct i2c_adapter *adap, uint8_t chip,
 				uint addr, int alen, uint8_t *buffer,
 				int len);
+	int		(*slave_io)(struct i2c_adapter *adap,
+				struct i2c_transaction *trans);
 	uint		(*set_bus_speed)(struct i2c_adapter *adap,
 				uint speed);
 	int		speed;
@@ -82,12 +98,13 @@ struct i2c_adapter {
 		.probe		=	_probe, \
 		.read		=	_read, \
 		.write		=	_write, \
+		.slave_io	=	0, \
 		.set_bus_speed	=	_set_speed, \
 		.speed		=	_speed, \
 		.slaveaddr	=	_slaveaddr, \
 		.init_done	=	0, \
 		.hwadapnr	=	_hwadapnr, \
-		.name		=	#_name \
+		.name		=	#_name, \
 };
 
 #define U_BOOT_I2C_ADAP_COMPLETE(_name, _init, _probe, _read, _write, \
@@ -451,4 +468,15 @@ int i2c_get_bus_num_fdt(int node);
  * @return 0 if port was reset, -1 if not found
  */
 int i2c_reset_port_fdt(const void *blob, int node);
+
+/**
+ * Perform I2C transaction with master device.
+ *
+ * @param trans  I2C transaction object
+ * @return 0 if succeeded, -1 if not supported,
+ *         1 if not ready, 2 if operation timed out,
+ *         3 if not our packet, other - unknown error.
+ */
+int i2c_slave_io(struct i2c_transaction *trans);
+
 #endif	/* _I2C_H_ */
diff --git a/lib/fdtdec.c b/lib/fdtdec.c
index aaa6620..06bea03 100644
--- a/lib/fdtdec.c
+++ b/lib/fdtdec.c
@@ -38,6 +38,7 @@ static const char * const compat_names[COMPAT_COUNT] = {
 	COMPAT(NVIDIA_TEGRA20_SDMMC, "nvidia,tegra20-sdhci"),
 	COMPAT(NVIDIA_TEGRA20_SFLASH, "nvidia,tegra20-sflash"),
 	COMPAT(NVIDIA_TEGRA20_SLINK, "nvidia,tegra20-slink"),
+	COMPAT(NVIDIA_TEGRA20_NVEC, "nvidia,tegra20-nvec"),
 	COMPAT(NVIDIA_TEGRA114_SPI, "nvidia,tegra114-spi"),
 	COMPAT(SMSC_LAN9215, "smsc,lan9215"),
 	COMPAT(SAMSUNG_EXYNOS5_SROMC, "samsung,exynos-sromc"),