[PATCH v4 5/7] usb: serial: implement set_termios for F81232

[Peter Hung <hpeter@gmail.com>]

The original driver had do not any h/w change in driver.
This patch implements with configure H/W for
baud/parity/word length/stop bits functional.

Signed-off-by: Peter Hung <hpeter+linux_kernel@gmail.com>
---
 drivers/usb/serial/f81232.c | 144 +++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 137 insertions(+), 7 deletions(-)

diff --git a/drivers/usb/serial/f81232.c b/drivers/usb/serial/f81232.c
index 12e1ae4..248f40d 100644
--- a/drivers/usb/serial/f81232.c
+++ b/drivers/usb/serial/f81232.c
@@ -51,6 +51,10 @@ MODULE_DEVICE_TABLE(usb, id_table);
 #define F81232_USB_TIMEOUT 3000
 
 #define SERIAL_BASE_ADDRESS	   (0x0120)
+#define RECEIVE_BUFFER_REGISTER    (0x00 + SERIAL_BASE_ADDRESS)
+#define INTERRUPT_ENABLE_REGISTER  (0x01 + SERIAL_BASE_ADDRESS)
+#define FIFO_CONTROL_REGISTER      (0x02 + SERIAL_BASE_ADDRESS)
+#define LINE_CONTROL_REGISTER      (0x03 + SERIAL_BASE_ADDRESS)
 #define MODEM_STATUS_REGISTER      (0x06 + SERIAL_BASE_ADDRESS)
 struct f81232_private {
 	spinlock_t lock;
@@ -61,6 +65,20 @@ struct f81232_private {
 	struct usb_serial_port *port;
 };
 
+static inline int calc_baud_divisor(u32 baudrate)
+{
+	u32 divisor, rem;
+
+	divisor = 115200L / baudrate;
+	rem = 115200L % baudrate;
+
+	/* Round to nearest divisor */
+	if (((rem * 2) >= baudrate) && (baudrate != 110))
+		divisor++;
+
+	return divisor;
+}
+
 static inline int f81232_get_register(struct usb_device *dev,
 							  u16 reg, u8 *data)
 {
@@ -84,6 +102,29 @@ static inline int f81232_get_register(struct usb_device *dev,
 	return status;
 }
 
+static inline int f81232_set_register(struct usb_device *dev,
+							  u16 reg, u8 data)
+{
+	int status;
+
+	status = usb_control_msg(dev,
+			usb_sndctrlpipe(dev, 0),
+			REGISTER_REQUEST,
+			SET_REGISTER,
+			reg,
+			0,
+			&data,
+			1,
+			F81232_USB_TIMEOUT);
+
+	if (status < 0) {
+		dev_dbg(&dev->dev,
+			"%s status: %d\n", __func__, status);
+	}
+
+	return status;
+}
+
 static void f81232_read_msr(struct f81232_private *priv)
 {
 	int status;
@@ -240,15 +281,104 @@ static void f81232_break_ctl(struct tty_struct *tty, int break_state)
 static void f81232_set_termios(struct tty_struct *tty,
 		struct usb_serial_port *port, struct ktermios *old_termios)
 {
-	/* FIXME - Stubbed out for now */
+	u16 divisor;
+	u16 new_lcr = 0;
+	u8 data;
+	int status;
+	struct ktermios *termios = &tty->termios;
+	struct usb_device *dev = port->serial->dev;
+	unsigned int cflag = termios->c_cflag;
 
-	/* Don't change anything if nothing has changed */
-	if (old_termios && !tty_termios_hw_change(&tty->termios, old_termios))
-		return;
+	divisor = calc_baud_divisor(tty_get_baud_rate(tty));
+
+	status = f81232_set_register(dev, LINE_CONTROL_REGISTER,
+			 UART_LCR_DLAB); /* DLAB */
+
+	if (status < 0) {
+		dev_dbg(&dev->dev,
+			"%s status: %d line:%d\n", __func__, status, __LINE__);
+	}
+
+	status = f81232_set_register(dev, RECEIVE_BUFFER_REGISTER,
+			 divisor & 0x00ff); /* low */
+
+	if (status < 0) {
+		dev_dbg(&dev->dev,
+			"%s status: %d line:%d\n", __func__, status, __LINE__);
+	}
+
+	status = f81232_set_register(dev, INTERRUPT_ENABLE_REGISTER,
+			 (divisor & 0xff00) >> 8); /* high */
+
+	if (status < 0) {
+		dev_dbg(&dev->dev,
+			"%s status: %d line:%d\n", __func__, status, __LINE__);
+	}
+
+	status = f81232_set_register(dev, LINE_CONTROL_REGISTER, 0x00);
+
+	if (status < 0) {
+		dev_dbg(&dev->dev,
+			"%s status: %d line:%d\n", __func__, status, __LINE__);
+	}
+
+	if (cflag & PARENB) {
+		new_lcr |= UART_LCR_PARITY; /* default parity on/odd */
+
+		if (cflag & CMSPAR) {
+			if (cflag & PARODD) {
+				/* stick mark */
+				new_lcr |= UART_LCR_SPAR;
+			} else {
+				 /* stick space */
+				new_lcr |= (UART_LCR_EPAR | UART_LCR_SPAR);
+			}
+		} else {
+			if (!(cflag & PARODD)) {
+				/* even */
+				new_lcr |= UART_LCR_EPAR;
+			}
+		}
+	}
+
+	if (cflag & CSTOPB)
+		new_lcr |= UART_LCR_STOP;
+	else
+		new_lcr &= ~UART_LCR_STOP;
+
+	switch (cflag & CSIZE) {
+	case CS5:
+		new_lcr |= UART_LCR_WLEN5;
+		break;
+	case CS6:
+		new_lcr |= UART_LCR_WLEN6;
+		break;
+	case CS7:
+		new_lcr |= UART_LCR_WLEN7;
+		break;
+	default:
+	case CS8:
+		new_lcr |= UART_LCR_WLEN8;
+		break;
+	}
+
+	status |= f81232_set_register(dev, LINE_CONTROL_REGISTER, new_lcr);
+
+	/* fifo on, trigger8, clear TX/RX*/
+	data = UART_FCR_TRIGGER_8 | UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR
+		| UART_FCR_CLEAR_XMIT;
+
+	status |= f81232_set_register(dev, FIFO_CONTROL_REGISTER, data);
+
+	data = UART_IER_MSI | UART_IER_RLSI | UART_IER_THRI | UART_IER_RDI;
+
+	status |= f81232_set_register(dev,
+			  INTERRUPT_ENABLE_REGISTER, 0xf); /* IER */
+
+	if (status < 0)
+		dev_err(&port->dev, "LINE_CONTROL_REGISTER set error: %d\n",
+			status);
 
-	/* Do the real work here... */
-	if (old_termios)
-		tty_termios_copy_hw(&tty->termios, old_termios);
 }
 
 static int f81232_tiocmget(struct tty_struct *tty)
-- 
1.9.1