From e74a1633aa3f33e9149c427b431915988ceedbce Mon Sep 17 00:00:00 2001
From: Sascha Hauer <s.hauer@pengutronix.de>
Date: Thu, 5 Jul 2007 18:02:17 +0200
Subject: [PATCH] svn_rev_686

implement console baudrate switching
---
 common/console.c                | 46 ++++++++++++++++++++++++++++++---
 drivers/serial/serial_mpc5xxx.c | 21 ++++++---------
 include/console.h               |  5 ++++
 3 files changed, 55 insertions(+), 17 deletions(-)

diff --git a/common/console.c b/common/console.c
index 8f8e433b7..dbee1c444 100644
--- a/common/console.c
+++ b/common/console.c
@@ -34,7 +34,8 @@
 
 static struct console_device *first_console;
 
-static int console_std_set(struct device_d *dev, struct param_d *param, const char *val)
+static int console_std_set(struct device_d *dev, struct param_d *param,
+		const char *val)
 {
 	struct console_device *cdev = dev->type_data;
 	unsigned int flag = 0, i = 0;
@@ -60,15 +61,50 @@ static int console_std_set(struct device_d *dev, struct param_d *param, const ch
 	return 0;
 }
 
+static int console_baudrate_set(struct device_d *dev, struct param_d *param,
+		const char *val)
+{
+	struct console_device *cdev = dev->type_data;
+	int baudrate;
+
+	baudrate = simple_strtoul(val, NULL, 10);
+
+	if (cdev->f_active) {
+		printf("## Switch baudrate to %d bps and press ENTER ...\n",
+			baudrate);
+		mdelay(50);
+		cdev->setbrg(cdev, baudrate);
+		mdelay(50);
+		while (getc() != '\r');
+	} else
+		cdev->setbrg(cdev, baudrate);
+
+	sprintf(cdev->baudrate_string, "%d", baudrate);
+
+	return 0;
+}
+
 int console_register(struct console_device *newcdev)
 {
         struct console_device *cdev = first_console;
 	struct device_d *dev = newcdev->dev;
 
+	if (cdev->setbrg) {
+		newcdev->baudrate_param.set = console_baudrate_set;
+		newcdev->baudrate_param.name = "baudrate";
+		sprintf(newcdev->baudrate_string, "%d",
+			CONFIG_BAUDRATE);
+		console_baudrate_set(dev, &newcdev->baudrate_param,
+			newcdev->baudrate_string);
+		cdev->baudrate_param.value = newcdev->baudrate_string;
+		dev_add_param(dev, &newcdev->baudrate_param);
+	}
+
 	newcdev->active_param.set = console_std_set;
 	newcdev->active_param.name  = "active";
 	newcdev->active_param.value = newcdev->active;
 	dev_add_param(dev, &newcdev->active_param);
+
 #ifdef CONFIG_CONSOLE_ACTIVATE_ALL
 	console_std_set(dev, &newcdev->active_param, "ioe");
 #endif
@@ -128,8 +164,11 @@ void console_putc(unsigned int ch, char c)
 	struct console_device *cdev = first_console;
 
 	while (cdev) {
-		if (cdev->f_active & ch)
+		if (cdev->f_active & ch) {
 			cdev->putc(cdev, c);
+			if (c == '\n')
+				cdev->putc(cdev, '\r');
+		}
 		cdev = cdev->next;
 	}
 }
@@ -154,9 +193,8 @@ void console_puts(unsigned int ch, const char *str)
 			const char *s = str;
 			while (*s) {
 				cdev->putc(cdev, *s);
-				if (*s == '\n') {
+				if (*s == '\n')
 					cdev->putc(cdev, '\r');
-				}
 				s++;
 			}
 		}
diff --git a/drivers/serial/serial_mpc5xxx.c b/drivers/serial/serial_mpc5xxx.c
index 54c2d02ba..c008cfd51 100644
--- a/drivers/serial/serial_mpc5xxx.c
+++ b/drivers/serial/serial_mpc5xxx.c
@@ -41,36 +41,32 @@
 #include <xfuncs.h>
 #include <asm/arch/clocks.h>
 
-static void mpc5xxx_serial_setbrg(struct console_device *cdev)
+static int mpc5xxx_serial_setbrg(struct console_device *cdev, int baudrate)
 {
 	struct device_d *dev = cdev->dev;
 	volatile struct mpc5xxx_psc *psc = (struct mpc5xxx_psc *)dev->map_base;
 	unsigned long baseclk;
 	int div;
-return;
-	printf("%s: ipb\n", __FUNCTION__);
+
 #if defined(CONFIG_MGT5100)
 	baseclk = (CFG_MPC5XXX_CLKIN + 16) / 32;
 #elif defined(CONFIG_MPC5200)
 	baseclk = (get_ipb_clock() + 16) / 32;
 #endif
-	printf("done: %d\n", get_ipb_clock());
+
 	/* set up UART divisor */
-#if 0
-	div = (baseclk + (gd->baudrate/2)) / gd->baudrate;
-#else
-#warning mpc5200 serial: temporary baudrate hack
-	div = (baseclk + (115200 / 2)) / 115200;
-#endif
+	div = (baseclk + (baudrate/2)) / baudrate;
 	psc->ctur = (div >> 8) & 0xFF;
 	psc->ctlr =  div & 0xff;
+
+	return 0;
 }
 
 static int mpc5xxx_serial_init(struct console_device *cdev)
 {
 	struct device_d *dev = cdev->dev;
 	volatile struct mpc5xxx_psc *psc = (struct mpc5xxx_psc *)dev->map_base;
-return 0;
+
 	/* reset PSC */
 	psc->command = PSC_SEL_MODE_REG_1;
 
@@ -92,8 +88,6 @@ return 0;
 #endif
 	psc->mode = PSC_MODE_ONE_STOP;
 
-	mpc5xxx_serial_setbrg(cdev);
-
 	/* disable all interrupts */
 	psc->psc_imr = 0;
 
@@ -148,6 +142,7 @@ static int mpc5xxx_serial_probe(struct device_d *dev)
 	cdev->tstc = mpc5xxx_serial_tstc;
 	cdev->putc = mpc5xxx_serial_putc;
 	cdev->getc = mpc5xxx_serial_getc;
+	cdev->setbrg = mpc5xxx_serial_setbrg;
 
 	mpc5xxx_serial_init(cdev);
 
diff --git a/include/console.h b/include/console.h
index d6e2fb55c..a5b585d11 100644
--- a/include/console.h
+++ b/include/console.h
@@ -36,11 +36,16 @@ struct console_device {
 	int (*tstc)(struct console_device *cdev);
 	void (*putc)(struct console_device *cdev, char c);
 	int  (*getc)(struct console_device *cdev);
+	int (*setbrg)(struct console_device *cdev, int baudrate);
+
 	struct console_device *next;
 
 	unsigned char f_caps;
 	unsigned char f_active;
 
+	struct param_d baudrate_param;
+	char baudrate_string[8];
+
 	struct param_d active_param;
 	char active[4];
 };