[U-Boot] [PATCH] serial: ns16550: add setconfig support

[Simon Goldschmidt <simon.k.r.goldschmidt@gmail.com>]

Add possibility to update the serial parity used.

Signed-off-by: Simon Goldschmidt <simon.k.r.goldschmidt@gmail.com>
---

 drivers/serial/ns16550.c | 43 ++++++++++++++++++++++++++++++++++++++--
 1 file changed, 41 insertions(+), 2 deletions(-)

diff --git a/drivers/serial/ns16550.c b/drivers/serial/ns16550.c
index f9041aa626..88b312d0be 100644
--- a/drivers/serial/ns16550.c
+++ b/drivers/serial/ns16550.c
@@ -148,10 +148,13 @@ int ns16550_calc_divisor(NS16550_t port, int clock, int baudrate)
 
 static void NS16550_setbrg(NS16550_t com_port, int baud_divisor)
 {
-	serial_out(UART_LCR_BKSE | UART_LCRVAL, &com_port->lcr);
+	/* to keep serial format, read lcr before writing BKSE */
+	int lcr_val = serial_in(&com_port->lcr) & ~UART_LCR_BKSE;
+
+	serial_out(UART_LCR_BKSE | lcr_val, &com_port->lcr);
 	serial_out(baud_divisor & 0xff, &com_port->dll);
 	serial_out((baud_divisor >> 8) & 0xff, &com_port->dlm);
-	serial_out(UART_LCRVAL, &com_port->lcr);
+	serial_out(lcr_val, &com_port->lcr);
 }
 
 void NS16550_init(NS16550_t com_port, int baud_divisor)
@@ -181,6 +184,8 @@ void NS16550_init(NS16550_t com_port, int baud_divisor)
 
 	serial_out(UART_MCRVAL, &com_port->mcr);
 	serial_out(ns16550_getfcr(com_port), &com_port->fcr);
+	/* initialize serial config to 8N1 before writing baudrate */
+	serial_out(UART_LCRVAL, &com_port->lcr);
 	if (baud_divisor != -1)
 		NS16550_setbrg(com_port, baud_divisor);
 #if defined(CONFIG_ARCH_OMAP2PLUS) || defined(CONFIG_SOC_DA8XX) || \
@@ -334,6 +339,39 @@ static int ns16550_serial_setbrg(struct udevice *dev, int baudrate)
 	return 0;
 }
 
+static int ns16550_serial_setconfig(struct udevice *dev, uint serial_config)
+{
+	struct NS16550 *const com_port = dev_get_priv(dev);
+	int lcr_val = UART_LCR_WLS_8;
+	uint parity = SERIAL_GET_PARITY(serial_config);
+	uint bits = SERIAL_GET_BITS(serial_config);
+	uint stop = SERIAL_GET_STOP(serial_config);
+
+	/*
+	 * only parity config is implemented, check if other serial settings
+	 * are the default one.
+	 */
+	if (bits != SERIAL_8_BITS || stop != SERIAL_ONE_STOP)
+		return -ENOTSUPP; /* not supported in driver*/
+
+	switch (parity) {
+	case SERIAL_PAR_NONE:
+		/* no bits to add */
+		break;
+	case SERIAL_PAR_ODD:
+		lcr_val |= UART_LCR_PEN;
+		break;
+	case SERIAL_PAR_EVEN:
+		lcr_val |= UART_LCR_PEN | UART_LCR_EPS;
+		break;
+	default:
+		return -ENOTSUPP; /* not supported in driver*/
+	}
+
+	serial_out(lcr_val, &com_port->lcr);
+	return 0;
+}
+
 int ns16550_serial_probe(struct udevice *dev)
 {
 	struct NS16550 *const com_port = dev_get_priv(dev);
@@ -440,6 +478,7 @@ const struct dm_serial_ops ns16550_serial_ops = {
 	.pending = ns16550_serial_pending,
 	.getc = ns16550_serial_getc,
 	.setbrg = ns16550_serial_setbrg,
+	.setconfig = ns16550_serial_setconfig
 };
 
 #if CONFIG_IS_ENABLED(OF_CONTROL) && !CONFIG_IS_ENABLED(OF_PLATDATA)
-- 
2.17.1