From 709ea543b92489e7729d2d7ddd6c9f451e52158c Mon Sep 17 00:00:00 2001
From: Simon Glass <sjg@chromium.org>
Date: Wed, 23 Jul 2014 06:54:59 -0600
Subject: [PATCH] stdio: Pass device pointer to stdio methods

At present stdio device functions do not get any clue as to which stdio
device is being acted on. Some implementations go to great lengths to work
around this, such as defining a whole separate set of functions for each
possible device.

For driver model we need to associate a stdio_dev with a device. It doesn't
seem possible to continue with this work-around approach.

Instead, add a stdio_dev pointer to each of the stdio member functions.

Note: The serial drivers have the same problem, but it is not strictly
necessary to fix that to get driver model running. Also, if we convert
serial over to driver model the problem will go away.

Code size increases by 244 bytes for Thumb2 and 428 for PowerPC.

22: stdio: Pass device pointer to stdio methods
       arm: (for 2/2 boards)  all +244.0  bss -4.0  text +248.0
   powerpc: (for 1/1 boards)  all +428.0  text +428.0

Signed-off-by: Simon Glass <sjg@chromium.org>
Acked-by: Marek Vasut <marex@denx.de>
Reviewed-by: Marek Vasut <marex@denx.de>
---
 arch/blackfin/cpu/jtag-console.c  | 10 +++---
 arch/powerpc/cpu/mpc512x/serial.c | 10 +++---
 arch/powerpc/cpu/mpc8xx/video.c   |  6 ++--
 arch/x86/lib/video.c              |  4 +--
 board/mpl/common/kbd.c            |  4 +--
 board/mpl/common/kbd.h            |  6 ++--
 board/mpl/pati/pati.c             |  8 ++---
 board/nokia/rx51/rx51.c           |  6 ++--
 common/cmd_log.c                  | 11 ++++---
 common/console.c                  | 18 +++++------
 common/lcd.c                      | 14 ++++++--
 common/stdio.c                    | 34 +++++++++++++++----
 common/usb_kbd.c                  |  4 +--
 drivers/input/cros_ec_keyb.c      |  6 ++--
 drivers/input/i8042.c             |  4 +--
 drivers/input/keyboard.c          |  4 +--
 drivers/input/tegra-kbc.c         |  6 ++--
 drivers/misc/cbmem_console.c      |  6 ++--
 drivers/net/netconsole.c          | 10 +++---
 drivers/serial/serial.c           | 54 +++++++++++++++++++++++++++----
 drivers/serial/usbtty.c           |  8 ++---
 drivers/video/cfb_console.c       |  6 ++--
 include/common.h                  |  5 +++
 include/configs/ELPPC.h           |  4 +--
 include/configs/MHPC.h            |  4 +--
 include/configs/jadecpu.h         |  4 +--
 include/configs/nokia_rx51.h      |  5 +--
 include/i8042.h                   |  6 ++--
 include/stdio_dev.h               | 15 +++++----
 include/video.h                   |  8 +++--
 30 files changed, 189 insertions(+), 101 deletions(-)

diff --git a/arch/blackfin/cpu/jtag-console.c b/arch/blackfin/cpu/jtag-console.c
index 7cddb85a7f..b8be3182a0 100644
--- a/arch/blackfin/cpu/jtag-console.c
+++ b/arch/blackfin/cpu/jtag-console.c
@@ -112,11 +112,11 @@ static void jtag_send(const char *raw_str, uint32_t len)
 	if (cooked_str != raw_str)
 		free((char *)cooked_str);
 }
-static void jtag_putc(const char c)
+static void jtag_putc(struct stdio_dev *dev, const char c)
 {
 	jtag_send(&c, 1);
 }
-static void jtag_puts(const char *s)
+static void jtag_puts(struct stdio_dev *dev, const char *s)
 {
 	jtag_send(s, strlen(s));
 }
@@ -133,7 +133,7 @@ static int jtag_tstc_dbg(void)
 }
 
 /* Higher layers want to know when any data is available */
-static int jtag_tstc(void)
+static int jtag_tstc(struct stdio_dev *dev)
 {
 	return jtag_tstc_dbg() || leftovers_len;
 }
@@ -142,7 +142,7 @@ static int jtag_tstc(void)
  * [32bit length][actual data]
  */
 static uint32_t leftovers;
-static int jtag_getc(void)
+static int jtag_getc(struct stdio_dev *dev)
 {
 	int ret;
 	uint32_t emudat;
@@ -173,7 +173,7 @@ static int jtag_getc(void)
 		leftovers = emudat;
 	}
 
-	return jtag_getc();
+	return jtag_getc(dev);
 }
 
 int drv_jtag_console_init(void)
diff --git a/arch/powerpc/cpu/mpc512x/serial.c b/arch/powerpc/cpu/mpc512x/serial.c
index 42e0dc96f7..4105a28509 100644
--- a/arch/powerpc/cpu/mpc512x/serial.c
+++ b/arch/powerpc/cpu/mpc512x/serial.c
@@ -384,7 +384,7 @@ struct stdio_dev *open_port(int num, int baudrate)
 		sprintf(env_val, "%d", baudrate);
 		setenv(env_var, env_val);
 
-		if (port->start())
+		if (port->start(port))
 			return NULL;
 
 		set_bit(num, &initialized);
@@ -407,7 +407,7 @@ int close_port(int num)
 	if (!port)
 		return -1;
 
-	ret = port->stop();
+	ret = port->stop(port);
 	clear_bit(num, &initialized);
 
 	return ret;
@@ -418,7 +418,7 @@ int write_port(struct stdio_dev *port, char *buf)
 	if (!port || !buf)
 		return -1;
 
-	port->puts(buf);
+	port->puts(port, buf);
 
 	return 0;
 }
@@ -433,8 +433,8 @@ int read_port(struct stdio_dev *port, char *buf, int size)
 	if (!size)
 		return 0;
 
-	while (port->tstc()) {
-		buf[cnt++] = port->getc();
+	while (port->tstc(port)) {
+		buf[cnt++] = port->getc(port);
 		if (cnt > size)
 			break;
 	}
diff --git a/arch/powerpc/cpu/mpc8xx/video.c b/arch/powerpc/cpu/mpc8xx/video.c
index 2fd5b11fe4..9590bfd3fd 100644
--- a/arch/powerpc/cpu/mpc8xx/video.c
+++ b/arch/powerpc/cpu/mpc8xx/video.c
@@ -948,7 +948,7 @@ static inline void console_newline (void)
 	}
 }
 
-void video_putc (const char c)
+void video_putc(struct stdio_dev *dev, const char c)
 {
 	if (!video_enable) {
 		serial_putc (c);
@@ -985,7 +985,7 @@ void video_putc (const char c)
 	}
 }
 
-void video_puts (const char *s)
+void video_puts(struct stdio_dev *dev, const char *s)
 {
 	int count = strlen (s);
 
@@ -994,7 +994,7 @@ void video_puts (const char *s)
 			serial_putc (*s++);
 	else
 		while (count--)
-			video_putc (*s++);
+			video_putc(dev, *s++);
 }
 
 /************************************************************************/
diff --git a/arch/x86/lib/video.c b/arch/x86/lib/video.c
index eb9c595a4e..975949daa3 100644
--- a/arch/x86/lib/video.c
+++ b/arch/x86/lib/video.c
@@ -104,7 +104,7 @@ static void __video_putc(const char c, int *x, int *y)
 	}
 }
 
-static void video_putc(const char c)
+static void video_putc(struct stdio_dev *dev, const char c)
 {
 	int x, y, pos;
 
@@ -123,7 +123,7 @@ static void video_putc(const char c)
 	outb_p(0xff & (pos >> 1), vidport+1);
 }
 
-static void video_puts(const char *s)
+static void video_puts(struct stdio_dev *dev, const char *s)
 {
 	int x, y, pos;
 	char c;
diff --git a/board/mpl/common/kbd.c b/board/mpl/common/kbd.c
index f56545ec03..99de2cad66 100644
--- a/board/mpl/common/kbd.c
+++ b/board/mpl/common/kbd.c
@@ -248,7 +248,7 @@ void kbd_put_queue(char data)
 }
 
 /* test if a character is in the queue */
-int kbd_testc(void)
+int kbd_testc(struct stdio_dev *dev)
 {
 	if(in_pointer==out_pointer)
 		return(0); /* no data */
@@ -256,7 +256,7 @@ int kbd_testc(void)
 		return(1);
 }
 /* gets the character from the queue */
-int kbd_getc(void)
+int kbd_getc(struct stdio_dev *dev)
 {
 	char c;
 	while(in_pointer==out_pointer);
diff --git a/board/mpl/common/kbd.h b/board/mpl/common/kbd.h
index 7b19b37259..b549e20ea4 100644
--- a/board/mpl/common/kbd.h
+++ b/board/mpl/common/kbd.h
@@ -8,8 +8,10 @@
 #ifndef _KBD_H_
 #define _KBD_H_
 
-extern int kbd_testc(void);
-extern int kbd_getc(void);
+struct stdio_dev;
+
+int kbd_testc(struct stdio_dev *sdev);
+int kbd_getc(struct stdio_dev *sdev);
 extern void kbd_interrupt(void);
 extern char *kbd_initialize(void);
 
diff --git a/board/mpl/pati/pati.c b/board/mpl/pati/pati.c
index 8ca9bb31d0..5d701a7931 100644
--- a/board/mpl/pati/pati.c
+++ b/board/mpl/pati/pati.c
@@ -445,7 +445,7 @@ void pci_con_put_it(const char c)
 	PCICON_SET_REG(PCICON_DBELL_REG,PCIMSG_CON_DATA);
 }
 
-void pci_con_putc(const char c)
+void pci_con_putc(struct stdio_dev *dev, const char c)
 {
 	pci_con_put_it(c);
 	if(c == '\n')
@@ -453,7 +453,7 @@ void pci_con_putc(const char c)
 }
 
 
-int pci_con_getc(void)
+int pci_con_getc(struct stdio_dev *dev)
 {
 	int res;
 	int diff;
@@ -473,14 +473,14 @@ int pci_con_getc(void)
 	return res;
 }
 
-int pci_con_tstc(void)
+int pci_con_tstc(struct stdio_dev *dev)
 {
 	if(r_ptr==(volatile int)w_ptr)
 		return 0;
 	return 1;
 }
 
-void pci_con_puts (const char *s)
+void pci_con_puts(struct stdio_dev *dev, const char *s)
 {
 	while (*s) {
 		pci_con_putc(*s);
diff --git a/board/nokia/rx51/rx51.c b/board/nokia/rx51/rx51.c
index 3e419efe39..c2e07dbd9b 100644
--- a/board/nokia/rx51/rx51.c
+++ b/board/nokia/rx51/rx51.c
@@ -585,7 +585,7 @@ static void rx51_kp_fill(u8 k, u8 mods)
  * Routine: rx51_kp_tstc
  * Description: Test if key was pressed (from buffer).
  */
-int rx51_kp_tstc(void)
+int rx51_kp_tstc(struct stdio_dev *sdev)
 {
 	u8 c, r, dk, i;
 	u8 intr;
@@ -641,10 +641,10 @@ int rx51_kp_tstc(void)
  * Routine: rx51_kp_getc
  * Description: Get last pressed key (from buffer).
  */
-int rx51_kp_getc(void)
+int rx51_kp_getc(struct stdio_dev *sdev)
 {
 	keybuf_head %= KEYBUF_SIZE;
-	while (!rx51_kp_tstc())
+	while (!rx51_kp_tstc(sdev))
 		WATCHDOG_RESET();
 	return keybuf[keybuf_head++];
 }
diff --git a/common/cmd_log.c b/common/cmd_log.c
index 38d0f5edfd..873ee40371 100644
--- a/common/cmd_log.c
+++ b/common/cmd_log.c
@@ -33,8 +33,8 @@
 DECLARE_GLOBAL_DATA_PTR;
 
 /* Local prototypes */
-static void logbuff_putc(const char c);
-static void logbuff_puts(const char *s);
+static void logbuff_putc(struct stdio_dev *dev, const char c);
+static void logbuff_puts(struct stdio_dev *dev, const char *s);
 static int logbuff_printk(const char *line);
 
 static char buf[1024];
@@ -143,7 +143,7 @@ int drv_logbuff_init(void)
 	return (rc == 0) ? 1 : rc;
 }
 
-static void logbuff_putc(const char c)
+static void logbuff_putc(struct stdio_dev *dev, const char c)
 {
 	char buf[2];
 	buf[0] = c;
@@ -151,7 +151,7 @@ static void logbuff_putc(const char c)
 	logbuff_printk(buf);
 }
 
-static void logbuff_puts(const char *s)
+static void logbuff_puts(struct stdio_dev *dev, const char *s)
 {
 	logbuff_printk (s);
 }
@@ -181,6 +181,7 @@ void logbuff_log(char *msg)
  */
 int do_log(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
+	struct stdio_dev *sdev = NULL;
 	char *s;
 	unsigned long i, start, size;
 
@@ -188,7 +189,7 @@ int do_log(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		/* Log concatenation of all arguments separated by spaces */
 		for (i = 2; i < argc; i++) {
 			logbuff_printk(argv[i]);
-			logbuff_putc((i < argc - 1) ? ' ' : '\n');
+			logbuff_putc(sdev, (i < argc - 1) ? ' ' : '\n');
 		}
 		return 0;
 	}
diff --git a/common/console.c b/common/console.c
index 5453726f69..11c102a726 100644
--- a/common/console.c
+++ b/common/console.c
@@ -109,7 +109,7 @@ static int console_setfile(int file, struct stdio_dev * dev)
 	case stderr:
 		/* Start new device */
 		if (dev->start) {
-			error = dev->start();
+			error = dev->start(dev);
 			/* If it's not started dont use it */
 			if (error < 0)
 				break;
@@ -159,7 +159,7 @@ static int console_getc(int file)
 	unsigned char ret;
 
 	/* This is never called with testcdev == NULL */
-	ret = tstcdev->getc();
+	ret = tstcdev->getc(tstcdev);
 	tstcdev = NULL;
 	return ret;
 }
@@ -173,7 +173,7 @@ static int console_tstc(int file)
 	for (i = 0; i < cd_count[file]; i++) {
 		dev = console_devices[file][i];
 		if (dev->tstc != NULL) {
-			ret = dev->tstc();
+			ret = dev->tstc(dev);
 			if (ret > 0) {
 				tstcdev = dev;
 				disable_ctrlc(0);
@@ -194,7 +194,7 @@ static void console_putc(int file, const char c)
 	for (i = 0; i < cd_count[file]; i++) {
 		dev = console_devices[file][i];
 		if (dev->putc != NULL)
-			dev->putc(c);
+			dev->putc(dev, c);
 	}
 }
 
@@ -206,7 +206,7 @@ static void console_puts(int file, const char *s)
 	for (i = 0; i < cd_count[file]; i++) {
 		dev = console_devices[file][i];
 		if (dev->puts != NULL)
-			dev->puts(s);
+			dev->puts(dev, s);
 	}
 }
 
@@ -222,22 +222,22 @@ static inline void console_doenv(int file, struct stdio_dev *dev)
 #else
 static inline int console_getc(int file)
 {
-	return stdio_devices[file]->getc();
+	return stdio_devices[file]->getc(stdio_devices[file]);
 }
 
 static inline int console_tstc(int file)
 {
-	return stdio_devices[file]->tstc();
+	return stdio_devices[file]->tstc(stdio_devices[file]);
 }
 
 static inline void console_putc(int file, const char c)
 {
-	stdio_devices[file]->putc(c);
+	stdio_devices[file]->putc(stdio_devices[file], c);
 }
 
 static inline void console_puts(int file, const char *s)
 {
-	stdio_devices[file]->puts(s);
+	stdio_devices[file]->puts(stdio_devices[file], s);
 }
 
 static inline void console_printdevs(int file)
diff --git a/common/lcd.c b/common/lcd.c
index 19b86b7c55..feb913a720 100644
--- a/common/lcd.c
+++ b/common/lcd.c
@@ -214,6 +214,11 @@ static inline void console_newline(void)
 
 /*----------------------------------------------------------------------*/
 
+static void lcd_stub_putc(struct stdio_dev *dev, const char c)
+{
+	lcd_putc(c);
+}
+
 void lcd_putc(const char c)
 {
 	if (!lcd_is_enabled) {
@@ -253,6 +258,11 @@ void lcd_putc(const char c)
 
 /*----------------------------------------------------------------------*/
 
+static void lcd_stub_puts(struct stdio_dev *dev, const char *s)
+{
+	lcd_puts(s);
+}
+
 void lcd_puts(const char *s)
 {
 	if (!lcd_is_enabled) {
@@ -426,8 +436,8 @@ int drv_lcd_init(void)
 	strcpy(lcddev.name, "lcd");
 	lcddev.ext   = 0;			/* No extensions */
 	lcddev.flags = DEV_FLAGS_OUTPUT;	/* Output only */
-	lcddev.putc  = lcd_putc;		/* 'putc' function */
-	lcddev.puts  = lcd_puts;		/* 'puts' function */
+	lcddev.putc  = lcd_stub_putc;		/* 'putc' function */
+	lcddev.puts  = lcd_stub_puts;		/* 'puts' function */
 
 	rc = stdio_register(&lcddev);
 
diff --git a/common/stdio.c b/common/stdio.c
index 844f98c184..dd402cc23d 100644
--- a/common/stdio.c
+++ b/common/stdio.c
@@ -35,23 +35,43 @@ char *stdio_names[MAX_FILES] = { "stdin", "stdout", "stderr" };
 
 
 #ifdef CONFIG_SYS_DEVICE_NULLDEV
-void nulldev_putc(const char c)
+void nulldev_putc(struct stdio_dev *dev, const char c)
 {
 	/* nulldev is empty! */
 }
 
-void nulldev_puts(const char *s)
+void nulldev_puts(struct stdio_dev *dev, const char *s)
 {
 	/* nulldev is empty! */
 }
 
-int nulldev_input(void)
+int nulldev_input(struct stdio_dev *dev)
 {
 	/* nulldev is empty! */
 	return 0;
 }
 #endif
 
+void stdio_serial_putc(struct stdio_dev *dev, const char c)
+{
+	serial_putc(c);
+}
+
+void stdio_serial_puts(struct stdio_dev *dev, const char *s)
+{
+	serial_puts(s);
+}
+
+int stdio_serial_getc(struct stdio_dev *dev)
+{
+	return serial_getc();
+}
+
+int stdio_serial_tstc(struct stdio_dev *dev)
+{
+	return serial_tstc();
+}
+
 /**************************************************************************
  * SYSTEM DRIVERS
  **************************************************************************
@@ -65,10 +85,10 @@ static void drv_system_init (void)
 
 	strcpy (dev.name, "serial");
 	dev.flags = DEV_FLAGS_OUTPUT | DEV_FLAGS_INPUT | DEV_FLAGS_SYSTEM;
-	dev.putc = serial_putc;
-	dev.puts = serial_puts;
-	dev.getc = serial_getc;
-	dev.tstc = serial_tstc;
+	dev.putc = stdio_serial_putc;
+	dev.puts = stdio_serial_puts;
+	dev.getc = stdio_serial_getc;
+	dev.tstc = stdio_serial_tstc;
 	stdio_register (&dev);
 
 #ifdef CONFIG_SYS_DEVICE_NULLDEV
diff --git a/common/usb_kbd.c b/common/usb_kbd.c
index 371e5bc144..c34fd5c786 100644
--- a/common/usb_kbd.c
+++ b/common/usb_kbd.c
@@ -360,7 +360,7 @@ static inline void usb_kbd_poll_for_event(struct usb_device *dev)
 }
 
 /* test if a character is in the queue */
-static int usb_kbd_testc(void)
+static int usb_kbd_testc(struct stdio_dev *sdev)
 {
 	struct stdio_dev *dev;
 	struct usb_device *usb_kbd_dev;
@@ -386,7 +386,7 @@ static int usb_kbd_testc(void)
 }
 
 /* gets the character from the queue */
-static int usb_kbd_getc(void)
+static int usb_kbd_getc(struct stdio_dev *sdev)
 {
 	struct stdio_dev *dev;
 	struct usb_device *usb_kbd_dev;
diff --git a/drivers/input/cros_ec_keyb.c b/drivers/input/cros_ec_keyb.c
index a2501e0206..47502b1763 100644
--- a/drivers/input/cros_ec_keyb.c
+++ b/drivers/input/cros_ec_keyb.c
@@ -93,7 +93,7 @@ static int check_for_keys(struct keyb *config,
  *
  * @return 0 if no keys available, 1 if keys are available
  */
-static int kbd_tstc(void)
+static int kbd_tstc(struct stdio_dev *dev)
 {
 	/* Just get input to do this for us */
 	return config.inited ? input_tstc(&config.input) : 0;
@@ -104,7 +104,7 @@ static int kbd_tstc(void)
  *
  * @return ASCII key code, or 0 if no key, or -1 if error
  */
-static int kbd_getc(void)
+static int kbd_getc(struct stdio_dev *dev)
 {
 	/* Just get input to do this for us */
 	return config.inited ? input_getc(&config.input) : 0;
@@ -214,7 +214,7 @@ static int cros_ec_keyb_decode_fdt(const void *blob, int node,
  *
  * @return 0 if ok, -1 on error
  */
-static int cros_ec_init_keyboard(void)
+static int cros_ec_init_keyboard(struct stdio_dev *dev)
 {
 	const void *blob = gd->fdt_blob;
 	int node;
diff --git a/drivers/input/i8042.c b/drivers/input/i8042.c
index 35fa0bb504..ca1604c540 100644
--- a/drivers/input/i8042.c
+++ b/drivers/input/i8042.c
@@ -398,7 +398,7 @@ int i8042_kbd_init(void)
  * i8042_tstc - test if keyboard input is available
  *		option: cursor blinking if called in a loop
  */
-int i8042_tstc(void)
+int i8042_tstc(struct stdio_dev *dev)
 {
 	unsigned char scan_code = 0;
 
@@ -432,7 +432,7 @@ int i8042_tstc(void)
  * i8042_getc - wait till keyboard input is available
  *		option: turn on/off cursor while waiting
  */
-int i8042_getc(void)
+int i8042_getc(struct stdio_dev *dev)
 {
 	int ret_chr;
 	unsigned char scan_code;
diff --git a/drivers/input/keyboard.c b/drivers/input/keyboard.c
index 5ef1cc06ed..be0f3330db 100644
--- a/drivers/input/keyboard.c
+++ b/drivers/input/keyboard.c
@@ -70,7 +70,7 @@ static void kbd_put_queue(char data)
 }
 
 /* test if a character is in the queue */
-static int kbd_testc(void)
+static int kbd_testc(struct stdio_dev *dev)
 {
 #if defined(CONFIG_MPC5xxx) || defined(CONFIG_MPC8540) || defined(CONFIG_MPC8541) || defined(CONFIG_MPC8555)
 	/* no ISR is used, so received chars must be polled */
@@ -83,7 +83,7 @@ static int kbd_testc(void)
 }
 
 /* gets the character from the queue */
-static int kbd_getc(void)
+static int kbd_getc(struct stdio_dev *dev)
 {
 	char c;
 	while(in_pointer==out_pointer) {
diff --git a/drivers/input/tegra-kbc.c b/drivers/input/tegra-kbc.c
index f137f930a9..7e36db0a71 100644
--- a/drivers/input/tegra-kbc.c
+++ b/drivers/input/tegra-kbc.c
@@ -194,7 +194,7 @@ int tegra_kbc_check(struct input_config *input)
  *
  * @return 0 if no keys available, 1 if keys are available
  */
-static int kbd_tstc(void)
+static int kbd_tstc(struct stdio_dev *dev)
 {
 	/* Just get input to do this for us */
 	return input_tstc(&config.input);
@@ -207,7 +207,7 @@ static int kbd_tstc(void)
  *
  * @return ASCII key code, or 0 if no key, or -1 if error
  */
-static int kbd_getc(void)
+static int kbd_getc(struct stdio_dev *dev)
 {
 	/* Just get input to do this for us */
 	return input_getc(&config.input);
@@ -289,7 +289,7 @@ static void tegra_kbc_open(void)
  *
  * @return 0 if ok, -ve on error
  */
-static int init_tegra_keyboard(void)
+static int init_tegra_keyboard(struct stdio_dev *dev)
 {
 	/* check if already created */
 	if (config.created)
diff --git a/drivers/misc/cbmem_console.c b/drivers/misc/cbmem_console.c
index 80a84fdf8f..5f85ccf21e 100644
--- a/drivers/misc/cbmem_console.c
+++ b/drivers/misc/cbmem_console.c
@@ -31,7 +31,7 @@ struct cbmem_console {
 
 static struct cbmem_console *cbmem_console_p;
 
-void cbmemc_putc(char data)
+void cbmemc_putc(struct stdio_dev *dev, char data)
 {
 	int cursor;
 
@@ -40,12 +40,12 @@ void cbmemc_putc(char data)
 		cbmem_console_p->buffer_body[cursor] = data;
 }
 
-void cbmemc_puts(const char *str)
+void cbmemc_puts(struct stdio_dev *dev, const char *str)
 {
 	char c;
 
 	while ((c = *str++) != 0)
-		cbmemc_putc(c);
+		cbmemc_putc(dev, c);
 }
 
 int cbmemc_init(void)
diff --git a/drivers/net/netconsole.c b/drivers/net/netconsole.c
index 65c747e14b..623f7492c7 100644
--- a/drivers/net/netconsole.c
+++ b/drivers/net/netconsole.c
@@ -215,7 +215,7 @@ static void nc_send_packet(const char *buf, int len)
 	}
 }
 
-static int nc_start(void)
+static int nc_start(struct stdio_dev *dev)
 {
 	int retval;
 
@@ -235,7 +235,7 @@ static int nc_start(void)
 	return 0;
 }
 
-static void nc_putc(char c)
+static void nc_putc(struct stdio_dev *dev, char c)
 {
 	if (output_recursion)
 		return;
@@ -246,7 +246,7 @@ static void nc_putc(char c)
 	output_recursion = 0;
 }
 
-static void nc_puts(const char *s)
+static void nc_puts(struct stdio_dev *dev, const char *s)
 {
 	int len;
 
@@ -265,7 +265,7 @@ static void nc_puts(const char *s)
 	output_recursion = 0;
 }
 
-static int nc_getc(void)
+static int nc_getc(struct stdio_dev *dev)
 {
 	uchar c;
 
@@ -286,7 +286,7 @@ static int nc_getc(void)
 	return c;
 }
 
-static int nc_tstc(void)
+static int nc_tstc(struct stdio_dev *dev)
 {
 	struct eth_device *eth;
 
diff --git a/drivers/serial/serial.c b/drivers/serial/serial.c
index fd61a5e545..803d8506d9 100644
--- a/drivers/serial/serial.c
+++ b/drivers/serial/serial.c
@@ -254,6 +254,48 @@ void serial_initialize(void)
 	serial_assign(default_serial_console()->name);
 }
 
+int serial_stub_start(struct stdio_dev *sdev)
+{
+	struct serial_device *dev = sdev->priv;
+
+	return dev->start();
+}
+
+int serial_stub_stop(struct stdio_dev *sdev)
+{
+	struct serial_device *dev = sdev->priv;
+
+	return dev->stop();
+}
+
+void serial_stub_putc(struct stdio_dev *sdev, const char ch)
+{
+	struct serial_device *dev = sdev->priv;
+
+	dev->putc(ch);
+}
+
+void serial_stub_puts(struct stdio_dev *sdev, const char *str)
+{
+	struct serial_device *dev = sdev->priv;
+
+	dev->puts(str);
+}
+
+int serial_stub_getc(struct stdio_dev *sdev)
+{
+	struct serial_device *dev = sdev->priv;
+
+	return dev->getc();
+}
+
+int serial_stub_tstc(struct stdio_dev *sdev)
+{
+	struct serial_device *dev = sdev->priv;
+
+	return dev->tstc();
+}
+
 /**
  * serial_stdio_init() - Register serial ports with STDIO core
  *
@@ -272,12 +314,12 @@ void serial_stdio_init(void)
 		strcpy(dev.name, s->name);
 		dev.flags = DEV_FLAGS_OUTPUT | DEV_FLAGS_INPUT;
 
-		dev.start = s->start;
-		dev.stop = s->stop;
-		dev.putc = s->putc;
-		dev.puts = s->puts;
-		dev.getc = s->getc;
-		dev.tstc = s->tstc;
+		dev.start = serial_stub_start;
+		dev.stop = serial_stub_stop;
+		dev.putc = serial_stub_putc;
+		dev.puts = serial_stub_puts;
+		dev.getc = serial_stub_getc;
+		dev.tstc = serial_stub_tstc;
 
 		stdio_register(&dev);
 
diff --git a/drivers/serial/usbtty.c b/drivers/serial/usbtty.c
index 6b912efafd..b030526b6a 100644
--- a/drivers/serial/usbtty.c
+++ b/drivers/serial/usbtty.c
@@ -389,7 +389,7 @@ static void str2wide (char *str, u16 * wide)
  * Test whether a character is in the RX buffer
  */
 
-int usbtty_tstc (void)
+int usbtty_tstc(struct stdio_dev *dev)
 {
 	struct usb_endpoint_instance *endpoint =
 		&endpoint_instance[rx_endpoint];
@@ -409,7 +409,7 @@ int usbtty_tstc (void)
  * written into its argument c.
  */
 
-int usbtty_getc (void)
+int usbtty_getc(struct stdio_dev *dev)
 {
 	char c;
 	struct usb_endpoint_instance *endpoint =
@@ -429,7 +429,7 @@ int usbtty_getc (void)
 /*
  * Output a single byte to the usb client port.
  */
-void usbtty_putc (const char c)
+void usbtty_putc(struct stdio_dev *dev, const char c)
 {
 	if (!usbtty_configured ())
 		return;
@@ -484,7 +484,7 @@ static void __usbtty_puts (const char *str, int len)
 	}
 }
 
-void usbtty_puts (const char *str)
+void usbtty_puts(struct stdio_dev *dev, const char *str)
 {
 	int n;
 	int len;
diff --git a/drivers/video/cfb_console.c b/drivers/video/cfb_console.c
index 1cf8660510..9231927879 100644
--- a/drivers/video/cfb_console.c
+++ b/drivers/video/cfb_console.c
@@ -944,7 +944,7 @@ static void parse_putc(const char c)
 		CURSOR_SET;
 }
 
-void video_putc(const char c)
+void video_putc(struct stdio_dev *dev, const char c)
 {
 #ifdef CONFIG_CFB_CONSOLE_ANSI
 	int i;
@@ -1158,12 +1158,12 @@ void video_putc(const char c)
 		flush_cache(VIDEO_FB_ADRS, VIDEO_SIZE);
 }
 
-void video_puts(const char *s)
+void video_puts(struct stdio_dev *dev, const char *s)
 {
 	int count = strlen(s);
 
 	while (count--)
-		video_putc(*s++);
+		video_putc(dev, *s++);
 }
 
 /*
diff --git a/include/common.h b/include/common.h
index 82c0a5a9a6..a75fc25c5f 100644
--- a/include/common.h
+++ b/include/common.h
@@ -639,6 +639,11 @@ void	serial_puts   (const char *);
 int	serial_getc   (void);
 int	serial_tstc   (void);
 
+/* These versions take a stdio_dev pointer */
+struct stdio_dev;
+int serial_stub_getc(struct stdio_dev *sdev);
+int serial_stub_tstc(struct stdio_dev *sdev);
+
 void	_serial_setbrg (const int);
 void	_serial_putc   (const char, const int);
 void	_serial_putc_raw(const char, const int);
diff --git a/include/configs/ELPPC.h b/include/configs/ELPPC.h
index 0ffbd41b49..debfc3697e 100644
--- a/include/configs/ELPPC.h
+++ b/include/configs/ELPPC.h
@@ -234,8 +234,8 @@
 #define CONFIG_VIDEO
 #define CONFIG_CFB_CONSOLE
 #define VIDEO_KBD_INIT_FCT    (simple_strtol (getenv("console"), NULL, 10))
-#define VIDEO_TSTC_FCT        serial_tstc
-#define VIDEO_GETC_FCT        serial_getc
+#define VIDEO_TSTC_FCT		serial_stub_tstc
+#define VIDEO_GETC_FCT		serial_stub_getc
 
 #define CONFIG_VIDEO_SMI_LYNXEM
 #define CONFIG_VIDEO_LOGO
diff --git a/include/configs/MHPC.h b/include/configs/MHPC.h
index 6314b5380d..d45be0f609 100644
--- a/include/configs/MHPC.h
+++ b/include/configs/MHPC.h
@@ -96,8 +96,8 @@
 #define CONFIG_VIDEO_LOGO
 
 #define VIDEO_KBD_INIT_FCT	0		/* no KBD dev on MHPC - use serial */
-#define VIDEO_TSTC_FCT		serial_tstc
-#define VIDEO_GETC_FCT		serial_getc
+#define VIDEO_TSTC_FCT		serial_stub_tstc
+#define VIDEO_GETC_FCT		serial_stub_getc
 
 #define CONFIG_BR0_WORKAROUND	1
 
diff --git a/include/configs/jadecpu.h b/include/configs/jadecpu.h
index b34e3422da..759e1129c2 100644
--- a/include/configs/jadecpu.h
+++ b/include/configs/jadecpu.h
@@ -87,8 +87,8 @@
 #define CONFIG_SYS_VIDEO_LOGO_MAX_SIZE  (800*480 + 256*4 + 10*1024)
 #define VIDEO_FB_16BPP_WORD_SWAP
 #define VIDEO_KBD_INIT_FCT		0
-#define VIDEO_TSTC_FCT			serial_tstc
-#define VIDEO_GETC_FCT			serial_getc
+#define VIDEO_TSTC_FCT		serial_stub_tstc
+#define VIDEO_GETC_FCT		serial_stub_getc
 
 /*
  * BOOTP options
diff --git a/include/configs/nokia_rx51.h b/include/configs/nokia_rx51.h
index e0c0fac8e1..53cb3902f3 100644
--- a/include/configs/nokia_rx51.h
+++ b/include/configs/nokia_rx51.h
@@ -263,9 +263,10 @@
 #define VIDEO_TSTC_FCT			rx51_kp_tstc
 #define VIDEO_GETC_FCT			rx51_kp_getc
 #ifndef __ASSEMBLY__
+struct stdio_dev;
 int rx51_kp_init(void);
-int rx51_kp_tstc(void);
-int rx51_kp_getc(void);
+int rx51_kp_tstc(struct stdio_dev *sdev);
+int rx51_kp_getc(struct stdio_dev *sdev);
 #endif
 
 #ifndef MTDPARTS_DEFAULT
diff --git a/include/i8042.h b/include/i8042.h
index 963061920c..58c85ec5f0 100644
--- a/include/i8042.h
+++ b/include/i8042.h
@@ -72,8 +72,10 @@ void i8042_flush(void);
  */
 int i8042_disable(void);
 
+struct stdio_dev;
+
 int i8042_kbd_init(void);
-int i8042_tstc(void);
-int i8042_getc(void);
+int i8042_tstc(struct stdio_dev *dev);
+int i8042_getc(struct stdio_dev *dev);
 
 #endif /* _I8042_H_ */
diff --git a/include/stdio_dev.h b/include/stdio_dev.h
index e6dc12ac39..45870054c0 100644
--- a/include/stdio_dev.h
+++ b/include/stdio_dev.h
@@ -27,18 +27,21 @@ struct stdio_dev {
 
 /* GENERAL functions */
 
-	int (*start) (void);		/* To start the device			*/
-	int (*stop) (void);		/* To stop the device			*/
+	int (*start)(struct stdio_dev *dev);	/* To start the device */
+	int (*stop)(struct stdio_dev *dev);	/* To stop the device */
 
 /* OUTPUT functions */
 
-	void (*putc) (const char c);	/* To put a char			*/
-	void (*puts) (const char *s);	/* To put a string (accelerator)	*/
+	/* To put a char */
+	void (*putc)(struct stdio_dev *dev, const char c);
+	/* To put a string (accelerator) */
+	void (*puts)(struct stdio_dev *dev, const char *s);
 
 /* INPUT functions */
 
-	int (*tstc) (void);		/* To test if a char is ready...	*/
-	int (*getc) (void);		/* To get that char			*/
+	/* To test if a char is ready... */
+	int (*tstc)(struct stdio_dev *dev);
+	int (*getc)(struct stdio_dev *dev);	/* To get that char */
 
 /* Other functions */
 
diff --git a/include/video.h b/include/video.h
index 0ff857bc9f..673aa2ec56 100644
--- a/include/video.h
+++ b/include/video.h
@@ -11,9 +11,11 @@
 
 /* Video functions */
 
-int	video_init	(void *videobase);
-void	video_putc	(const char c);
-void	video_puts	(const char *s);
+struct stdio_dev;
+
+int	video_init(void *videobase);
+void	video_putc(struct stdio_dev *dev, const char c);
+void	video_puts(struct stdio_dev *dev, const char *s);
 
 /**
  * Display a BMP format bitmap on the screen
-- 
2.39.5