5 files changed, 1026 insertions, 73 deletions

diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 687b1ea294b7..2a72799acc15 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -1047,6 +1047,14 @@ config SERIAL_MSM
 	depends on ARCH_QCOM
 	select SERIAL_CORE
 
+config SERIAL_MSM_SMD
+        bool "Enable tty device interface for some SMD ports"
+        default n
+        depends on ARCH_QCOM && QCOM_SMD
+        help
+          Enables userspace clients to read and write to some streaming SMD
+          ports via tty device interface for MSM chipset.
+
 config SERIAL_MSM_CONSOLE
 	bool "MSM serial console support"
 	depends on SERIAL_MSM=y
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 5ab41119b3dc..5b41963aa7f1 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -62,6 +62,7 @@ obj-$(CONFIG_SERIAL_SGI_IOC3) += ioc3_serial.o
 obj-$(CONFIG_SERIAL_ATMEL) += atmel_serial.o
 obj-$(CONFIG_SERIAL_UARTLITE) += uartlite.o
 obj-$(CONFIG_SERIAL_MSM) += msm_serial.o
+obj-$(CONFIG_SERIAL_MSM_SMD) += msm_smd_tty.o
 obj-$(CONFIG_SERIAL_NETX) += netx-serial.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM) += of_serial.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL) += nwpserial.o
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index b73889c8ed4b..2d779b1f010f 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -20,6 +20,8 @@
 #endif
 
 #include <linux/atomic.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
 #include <linux/hrtimer.h>
 #include <linux/module.h>
 #include <linux/io.h>
@@ -31,6 +33,7 @@
 #include <linux/tty_flip.h>
 #include <linux/serial_core.h>
 #include <linux/serial.h>
+#include <linux/slab.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
 #include <linux/delay.h>
@@ -39,6 +42,11 @@
 
 #include "msm_serial.h"
 
+#define UARTDM_BURST_SIZE	16   /* in bytes */
+#define UARTDM_TX_AIGN(x)	((x) & ~0x3) /* valid for > 1p3 */
+#define UARTDM_TX_MAX		256   /* in bytes, valid for <= 1p3 */
+#define UARTDM_RX_SIZE		(UART_XMIT_SIZE / 4)
+
 enum {
 	UARTDM_1P1 = 1,
 	UARTDM_1P2,
@@ -46,6 +54,17 @@ enum {
 	UARTDM_1P4,
 };
 
+struct msm_dma {
+	struct dma_chan		*chan;
+	enum dma_data_direction dir;
+	dma_addr_t		phys;
+	unsigned char		*virt;
+	dma_cookie_t		cookie;
+	u32			enable_bit;
+	unsigned int		count;
+	struct dma_async_tx_descriptor	*desc;
+};
+
 struct msm_port {
 	struct uart_port	uart;
 	char			name[16];
@@ -55,9 +74,153 @@ struct msm_port {
 	int			is_uartdm;
 	unsigned int		old_snap_state;
 	bool			break_detected;
+	struct msm_dma		tx_dma;
+	struct msm_dma		rx_dma;
 };
 
-static inline void wait_for_xmitr(struct uart_port *port)
+static void msm_handle_tx(struct uart_port *port);
+static void msm_start_rx_dma(struct msm_port *msm_port);
+
+void msm_stop_dma(struct uart_port *port, struct msm_dma *dma)
+{
+	struct device *dev = port->dev;
+	unsigned int mapped;
+	u32 val;
+
+	mapped = dma->count;
+	dma->count = 0;
+
+	dmaengine_terminate_all(dma->chan);
+
+	/*
+	 * DMA Stall happens if enqueue and flush command happens concurrently.
+	 * For example before changing the baud rate/protocol configuration and
+	 * sending flush command to ADM, disable the channel of UARTDM.
+	 * Note: should not reset the receiver here immediately as it is not
+	 * suggested to do disable/reset or reset/disable at the same time.
+	 */
+	val = msm_read(port, UARTDM_DMEN);
+	val &= ~dma->enable_bit;
+	msm_write(port, val, UARTDM_DMEN);
+
+	if (mapped)
+		dma_unmap_single(dev, dma->phys, mapped, dma->dir);
+}
+
+static void msm_release_dma(struct msm_port *msm_port)
+{
+	struct msm_dma *dma;
+
+	dma = &msm_port->tx_dma;
+	if (dma->chan) {
+		msm_stop_dma(&msm_port->uart, dma);
+		dma_release_channel(dma->chan);
+	}
+
+	memset(dma, 0, sizeof(*dma));
+
+	dma = &msm_port->rx_dma;
+	if (dma->chan) {
+		msm_stop_dma(&msm_port->uart, dma);
+		dma_release_channel(dma->chan);
+		kfree(dma->virt);
+	}
+
+	memset(dma, 0, sizeof(*dma));
+}
+
+static void msm_request_tx_dma(struct msm_port *msm_port, resource_size_t base)
+{
+	struct device *dev = msm_port->uart.dev;
+	struct dma_slave_config conf;
+	struct msm_dma *dma;
+	u32 crci = 0;
+	int ret;
+
+	dma = &msm_port->tx_dma;
+
+	/* allocate DMA resources, if available */
+	dma->chan = dma_request_slave_channel_reason(dev, "tx");
+	if (IS_ERR(dma->chan))
+		goto no_tx;
+
+	of_property_read_u32(dev->of_node, "qcom,tx-crci", &crci);
+
+	memset(&conf, 0, sizeof(conf));
+	conf.direction = DMA_MEM_TO_DEV;
+	conf.device_fc = true;
+	conf.dst_addr = base + UARTDM_TF;
+	conf.dst_maxburst = UARTDM_BURST_SIZE;
+	conf.slave_id = crci;
+
+	ret = dmaengine_slave_config(dma->chan, &conf);
+	if (ret)
+		goto rel_tx;
+
+	dma->dir = DMA_TO_DEVICE;
+
+	if (msm_port->is_uartdm < UARTDM_1P4)
+		dma->enable_bit = UARTDM_DMEN_TX_DM_ENABLE;
+	else
+		dma->enable_bit = UARTDM_DMEN_TX_BAM_ENABLE;
+
+	return;
+
+rel_tx:
+	dma_release_channel(dma->chan);
+no_tx:
+	memset(dma, 0, sizeof(*dma));
+}
+
+static void msm_request_rx_dma(struct msm_port *msm_port, resource_size_t base)
+{
+	struct device *dev = msm_port->uart.dev;
+	struct dma_slave_config conf;
+	struct msm_dma *dma;
+	u32 crci = 0;
+	int ret;
+
+	dma = &msm_port->rx_dma;
+
+	/* allocate DMA resources, if available */
+	dma->chan = dma_request_slave_channel_reason(dev, "rx");
+	if (IS_ERR(dma->chan))
+		goto no_rx;
+
+	of_property_read_u32(dev->of_node, "qcom,rx-crci", &crci);
+
+	dma->virt = kzalloc(UARTDM_RX_SIZE, GFP_KERNEL);
+	if (!dma->virt)
+		goto rel_rx;
+
+	memset(&conf, 0, sizeof(conf));
+	conf.direction = DMA_DEV_TO_MEM;
+	conf.device_fc = true;
+	conf.src_addr = base + UARTDM_RF;
+	conf.src_maxburst = UARTDM_BURST_SIZE;
+	conf.slave_id = crci;
+
+	ret = dmaengine_slave_config(dma->chan, &conf);
+	if (ret)
+		goto err;
+
+	dma->dir = DMA_FROM_DEVICE;
+
+	if (msm_port->is_uartdm < UARTDM_1P4)
+		dma->enable_bit = UARTDM_DMEN_RX_DM_ENABLE;
+	else
+		dma->enable_bit = UARTDM_DMEN_RX_BAM_ENABLE;
+
+	return;
+err:
+	kfree(dma->virt);
+rel_rx:
+	dma_release_channel(dma->chan);
+no_rx:
+	memset(dma, 0, sizeof(*dma));
+}
+
+static inline void msm_wait_for_xmitr(struct uart_port *port)
 {
 	while (!(msm_read(port, UART_SR) & UART_SR_TX_EMPTY)) {
 		if (msm_read(port, UART_ISR) & UART_ISR_TX_READY)
@@ -78,17 +241,277 @@ static void msm_stop_tx(struct uart_port *port)
 static void msm_start_tx(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
+	struct msm_dma *dma = &msm_port->tx_dma;
+
+	/* Already started in DMA mode */
+	if (dma->count)
+		return;
+
+	msm_port->imr |= UART_IMR_TXLEV;
+	msm_write(port, msm_port->imr, UART_IMR);
+}
+
+static void msm_reset_dm_count(struct uart_port *port, int count)
+{
+	msm_wait_for_xmitr(port);
+	msm_write(port, count, UARTDM_NCF_TX);
+	msm_read(port, UARTDM_NCF_TX);
+}
+
+static void msm_complete_tx_dma(void *args)
+{
+	struct msm_port *msm_port = args;
+	struct uart_port *port = &msm_port->uart;
+	struct circ_buf *xmit = &port->state->xmit;
+	struct msm_dma *dma = &msm_port->tx_dma;
+	struct dma_tx_state state;
+	enum dma_status status;
+	unsigned long flags;
+	unsigned int count;
+	u32 val;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	/* Already stopped */
+	if (!dma->count)
+		goto done;
+
+	status = dmaengine_tx_status(dma->chan, dma->cookie, &state);
+
+	dma_unmap_single(port->dev, dma->phys, dma->count, dma->dir);
 
+	val = msm_read(port, UARTDM_DMEN);
+	val &= ~dma->enable_bit;
+	msm_write(port, val, UARTDM_DMEN);
+
+	if (msm_port->is_uartdm > UARTDM_1P3) {
+		msm_write(port, UART_CR_CMD_RESET_TX, UART_CR);
+		msm_write(port, UART_CR_TX_ENABLE, UART_CR);
+	}
+
+	count = dma->count - state.residue;
+	port->icount.tx += count;
+	dma->count = 0;
+
+	xmit->tail += count;
+	xmit->tail &= UART_XMIT_SIZE - 1;
+
+	/* Restore "Tx FIFO below watermark" interrupt */
 	msm_port->imr |= UART_IMR_TXLEV;
 	msm_write(port, msm_port->imr, UART_IMR);
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+
+	msm_handle_tx(port);
+done:
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static int msm_handle_tx_dma(struct msm_port *msm_port, unsigned int count)
+{
+	struct circ_buf *xmit = &msm_port->uart.state->xmit;
+	struct uart_port *port = &msm_port->uart;
+	struct msm_dma *dma = &msm_port->tx_dma;
+	void *cpu_addr;
+	int ret;
+	u32 val;
+
+	cpu_addr = &xmit->buf[xmit->tail];
+
+	dma->phys = dma_map_single(port->dev, cpu_addr, count, dma->dir);
+	ret = dma_mapping_error(port->dev, dma->phys);
+	if (ret)
+		return ret;
+
+	dma->desc = dmaengine_prep_slave_single(dma->chan, dma->phys,
+						count, DMA_MEM_TO_DEV,
+						DMA_PREP_INTERRUPT |
+						DMA_PREP_FENCE);
+	if (!dma->desc) {
+		ret = -EIO;
+		goto unmap;
+	}
+
+	dma->desc->callback = msm_complete_tx_dma;
+	dma->desc->callback_param = msm_port;
+
+	dma->cookie = dmaengine_submit(dma->desc);
+	ret = dma_submit_error(dma->cookie);
+	if (ret)
+		goto unmap;
+
+	/*
+	 * Using DMA complete for Tx FIFO reload, no need for
+	 * "Tx FIFO below watermark" one, disable it
+	 */
+	msm_port->imr &= ~UART_IMR_TXLEV;
+	msm_write(port, msm_port->imr, UART_IMR);
+
+	dma->count = count;
+
+	val = msm_read(port, UARTDM_DMEN);
+	val |= dma->enable_bit;
+
+	if (msm_port->is_uartdm < UARTDM_1P4)
+		msm_write(port, val, UARTDM_DMEN);
+
+	msm_reset_dm_count(port, count);
+
+	if (msm_port->is_uartdm > UARTDM_1P3)
+		msm_write(port, val, UARTDM_DMEN);
+
+	dma_async_issue_pending(dma->chan);
+	return 0;
+unmap:
+	dma_unmap_single(port->dev, dma->phys, count, dma->dir);
+	return ret;
+}
+
+static void msm_complete_rx_dma(void *args)
+{
+	struct msm_port *msm_port = args;
+	struct uart_port *port = &msm_port->uart;
+	struct tty_port *tport = &port->state->port;
+	struct msm_dma *dma = &msm_port->rx_dma;
+	int count = 0, i, sysrq;
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	/* Already stopped */
+	if (!dma->count)
+		goto done;
+
+	val = msm_read(port, UARTDM_DMEN);
+	val &= ~dma->enable_bit;
+	msm_write(port, val, UARTDM_DMEN);
+
+	/* Restore interrupts */
+	msm_port->imr |= UART_IMR_RXLEV | UART_IMR_RXSTALE;
+	msm_write(port, msm_port->imr, UART_IMR);
+
+	if (msm_read(port, UART_SR) & UART_SR_OVERRUN) {
+		port->icount.overrun++;
+		tty_insert_flip_char(tport, 0, TTY_OVERRUN);
+		msm_write(port, UART_CR_CMD_RESET_ERR, UART_CR);
+	}
+
+	count = msm_read(port, UARTDM_RX_TOTAL_SNAP);
+
+	port->icount.rx += count;
+
+	dma->count = 0;
+
+	dma_unmap_single(port->dev, dma->phys, UARTDM_RX_SIZE, dma->dir);
+
+	for (i = 0; i < count; i++) {
+		char flag = TTY_NORMAL;
+
+		if (msm_port->break_detected && dma->virt[i] == 0) {
+			port->icount.brk++;
+			flag = TTY_BREAK;
+			msm_port->break_detected = false;
+			if (uart_handle_break(port))
+				continue;
+		}
+
+		if (!(port->read_status_mask & UART_SR_RX_BREAK))
+			flag = TTY_NORMAL;
+
+		spin_unlock_irqrestore(&port->lock, flags);
+		sysrq = uart_handle_sysrq_char(port, dma->virt[i]);
+		spin_lock_irqsave(&port->lock, flags);
+		if (!sysrq)
+			tty_insert_flip_char(tport, dma->virt[i], flag);
+	}
+
+	msm_start_rx_dma(msm_port);
+done:
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	if (count)
+		tty_flip_buffer_push(tport);
+}
+
+static void msm_start_rx_dma(struct msm_port *msm_port)
+{
+	struct msm_dma *dma = &msm_port->rx_dma;
+	struct uart_port *uart = &msm_port->uart;
+	u32 val;
+	int ret;
+
+	if (!dma->chan)
+		return;
+
+	dma->phys = dma_map_single(uart->dev, dma->virt,
+				   UARTDM_RX_SIZE, dma->dir);
+	ret = dma_mapping_error(uart->dev, dma->phys);
+	if (ret)
+		return;
+
+	dma->desc = dmaengine_prep_slave_single(dma->chan, dma->phys,
+						UARTDM_RX_SIZE, DMA_DEV_TO_MEM,
+						DMA_PREP_INTERRUPT);
+	if (!dma->desc)
+		goto unmap;
+
+	dma->desc->callback = msm_complete_rx_dma;
+	dma->desc->callback_param = msm_port;
+
+	dma->cookie = dmaengine_submit(dma->desc);
+	ret = dma_submit_error(dma->cookie);
+	if (ret)
+		goto unmap;
+	/*
+	 * Using DMA for FIFO off-load, no need for "Rx FIFO over
+	 * watermark" or "stale" interrupts, disable them
+	 */
+	msm_port->imr &= ~(UART_IMR_RXLEV | UART_IMR_RXSTALE);
+
+	/*
+	 * Well, when DMA is ADM3 engine(implied by <= UARTDM v1.3),
+	 * we need RXSTALE to flush input DMA fifo to memory
+	 */
+	if (msm_port->is_uartdm < UARTDM_1P4)
+		msm_port->imr |= UART_IMR_RXSTALE;
+
+	msm_write(uart, msm_port->imr, UART_IMR);
+
+	dma->count = UARTDM_RX_SIZE;
+
+	dma_async_issue_pending(dma->chan);
+
+	msm_write(uart, UART_CR_CMD_RESET_STALE_INT, UART_CR);
+	msm_write(uart, UART_CR_CMD_STALE_EVENT_ENABLE, UART_CR);
+
+	val = msm_read(uart, UARTDM_DMEN);
+	val |= dma->enable_bit;
+
+	if (msm_port->is_uartdm < UARTDM_1P4)
+		msm_write(uart, val, UARTDM_DMEN);
+
+	msm_write(uart, UARTDM_RX_SIZE, UARTDM_DMRX);
+
+	if (msm_port->is_uartdm > UARTDM_1P3)
+		msm_write(uart, val, UARTDM_DMEN);
+
+	return;
+unmap:
+	dma_unmap_single(uart->dev, dma->phys, UARTDM_RX_SIZE, dma->dir);
 }
 
 static void msm_stop_rx(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
+	struct msm_dma *dma = &msm_port->rx_dma;
 
 	msm_port->imr &= ~(UART_IMR_RXLEV | UART_IMR_RXSTALE);
 	msm_write(port, msm_port->imr, UART_IMR);
+
+	if (dma->chan)
+		msm_stop_dma(port, dma);
 }
 
 static void msm_enable_ms(struct uart_port *port)
@@ -99,7 +522,7 @@ static void msm_enable_ms(struct uart_port *port)
 	msm_write(port, msm_port->imr, UART_IMR);
 }
 
-static void handle_rx_dm(struct uart_port *port, unsigned int misr)
+static void msm_handle_rx_dm(struct uart_port *port, unsigned int misr)
 {
 	struct tty_port *tport = &port->state->port;
 	unsigned int sr;
@@ -169,9 +592,12 @@ static void handle_rx_dm(struct uart_port *port, unsigned int misr)
 		msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
 	msm_write(port, 0xFFFFFF, UARTDM_DMRX);
 	msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE, UART_CR);
+
+	/* Try to use DMA */
+	msm_start_rx_dma(msm_port);
 }
 
-static void handle_rx(struct uart_port *port)
+static void msm_handle_rx(struct uart_port *port)
 {
 	struct tty_port *tport = &port->state->port;
 	unsigned int sr;
@@ -224,18 +650,11 @@ static void handle_rx(struct uart_port *port)
 	spin_lock(&port->lock);
 }
 
-static void reset_dm_count(struct uart_port *port, int count)
-{
-	wait_for_xmitr(port);
-	msm_write(port, count, UARTDM_NCF_TX);
-	msm_read(port, UARTDM_NCF_TX);
-}
-
-static void handle_tx(struct uart_port *port)
+static void msm_handle_tx_pio(struct uart_port *port, unsigned int tx_count)
 {
 	struct circ_buf *xmit = &port->state->xmit;
 	struct msm_port *msm_port = UART_TO_MSM(port);
-	unsigned int tx_count, num_chars;
+	unsigned int num_chars;
 	unsigned int tf_pointer = 0;
 	void __iomem *tf;
 
@@ -244,20 +663,8 @@ static void handle_tx(struct uart_port *port)
 	else
 		tf = port->membase + UART_TF;
 
-	tx_count = uart_circ_chars_pending(xmit);
-	tx_count = min3(tx_count, (unsigned int)UART_XMIT_SIZE - xmit->tail,
-			port->fifosize);
-
-	if (port->x_char) {
-		if (msm_port->is_uartdm)
-			reset_dm_count(port, tx_count + 1);
-
-		iowrite8_rep(tf, &port->x_char, 1);
-		port->icount.tx++;
-		port->x_char = 0;
-	} else if (tx_count && msm_port->is_uartdm) {
-		reset_dm_count(port, tx_count);
-	}
+	if (tx_count && msm_port->is_uartdm)
+		msm_reset_dm_count(port, tx_count);
 
 	while (tf_pointer < tx_count) {
 		int i;
@@ -290,7 +697,60 @@ static void handle_tx(struct uart_port *port)
 		uart_write_wakeup(port);
 }
 
-static void handle_delta_cts(struct uart_port *port)
+static void msm_handle_tx(struct uart_port *port)
+{
+	struct msm_port *msm_port = UART_TO_MSM(port);
+	struct circ_buf *xmit = &msm_port->uart.state->xmit;
+	struct msm_dma *dma = &msm_port->tx_dma;
+	unsigned int pio_count, dma_count, dma_min;
+	void __iomem *tf;
+	int err = 0;
+
+	if (port->x_char) {
+		if (msm_port->is_uartdm)
+			tf = port->membase + UARTDM_TF;
+		else
+			tf = port->membase + UART_TF;
+
+		if (msm_port->is_uartdm)
+			msm_reset_dm_count(port, 1);
+
+		iowrite8_rep(tf, &port->x_char, 1);
+		port->icount.tx++;
+		port->x_char = 0;
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(port)) {
+		msm_stop_tx(port);
+		return;
+	}
+
+	pio_count = CIRC_CNT(xmit->head, xmit->tail, UART_XMIT_SIZE);
+	dma_count = CIRC_CNT_TO_END(xmit->head, xmit->tail, UART_XMIT_SIZE);
+
+	dma_min = 1;	/* Always DMA */
+	if (msm_port->is_uartdm > UARTDM_1P3) {
+		dma_count = UARTDM_TX_AIGN(dma_count);
+		dma_min = UARTDM_BURST_SIZE;
+	} else {
+		if (dma_count > UARTDM_TX_MAX)
+			dma_count = UARTDM_TX_MAX;
+	}
+
+	if (pio_count > port->fifosize)
+		pio_count = port->fifosize;
+
+	if (!dma->chan || dma_count < dma_min)
+		msm_handle_tx_pio(port, pio_count);
+	else
+		err = msm_handle_tx_dma(msm_port, dma_count);
+
+	if (err)	/* fall back to PIO mode */
+		msm_handle_tx_pio(port, pio_count);
+}
+
+static void msm_handle_delta_cts(struct uart_port *port)
 {
 	msm_write(port, UART_CR_CMD_RESET_CTS, UART_CR);
 	port->icount.cts++;
@@ -301,9 +761,12 @@ static irqreturn_t msm_irq(int irq, void *dev_id)
 {
 	struct uart_port *port = dev_id;
 	struct msm_port *msm_port = UART_TO_MSM(port);
+	struct msm_dma *dma = &msm_port->rx_dma;
+	unsigned long flags;
 	unsigned int misr;
+	u32 val;
 
-	spin_lock(&port->lock);
+	spin_lock_irqsave(&port->lock, flags);
 	misr = msm_read(port, UART_MISR);
 	msm_write(port, 0, UART_IMR); /* disable interrupt */
 
@@ -313,18 +776,29 @@ static irqreturn_t msm_irq(int irq, void *dev_id)
 	}
 
 	if (misr & (UART_IMR_RXLEV | UART_IMR_RXSTALE)) {
-		if (msm_port->is_uartdm)
-			handle_rx_dm(port, misr);
-		else
-			handle_rx(port);
+		if (dma->count) {
+			val = UART_CR_CMD_STALE_EVENT_DISABLE;
+			msm_write(port, val, UART_CR);
+			val = UART_CR_CMD_RESET_STALE_INT;
+			msm_write(port, val, UART_CR);
+			/*
+			 * Flush DMA input fifo to memory, this will also
+			 * trigger DMA RX completion
+			 */
+			dmaengine_terminate_all(dma->chan);
+		} else if (msm_port->is_uartdm) {
+			msm_handle_rx_dm(port, misr);
+		} else {
+			msm_handle_rx(port);
+		}
 	}
 	if (misr & UART_IMR_TXLEV)
-		handle_tx(port);
+		msm_handle_tx(port);
 	if (misr & UART_IMR_DELTA_CTS)
-		handle_delta_cts(port);
+		msm_handle_delta_cts(port);
 
 	msm_write(port, msm_port->imr, UART_IMR); /* restore interrupt */
-	spin_unlock(&port->lock);
+	spin_unlock_irqrestore(&port->lock, flags);
 
 	return IRQ_HANDLED;
 }
@@ -408,6 +882,7 @@ msm_find_best_baud(struct uart_port *port, unsigned int baud)
 		{    3, 0xdd,  8 },
 		{    2, 0xee, 16 },
 		{    1, 0xff, 31 },
+		{    0, 0xff, 31 },
 	};
 
 	divisor = uart_get_divisor(port, baud);
@@ -419,21 +894,41 @@ msm_find_best_baud(struct uart_port *port, unsigned int baud)
 	return entry; /* Default to smallest divider */
 }
 
-static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
+static int msm_set_baud_rate(struct uart_port *port, unsigned int baud,
+			      unsigned long *saved_flags)
 {
-	unsigned int rxstale, watermark;
+	unsigned int rxstale, watermark, mask;
 	struct msm_port *msm_port = UART_TO_MSM(port);
 	const struct msm_baud_map *entry;
+	unsigned long flags;
 
 	entry = msm_find_best_baud(port, baud);
 
 	msm_write(port, entry->code, UART_CSR);
 
+	if (baud > 460800)
+		port->uartclk = baud * 16;
+
+	flags = *saved_flags;
+	spin_unlock_irqrestore(&port->lock, flags);
+
+	clk_set_rate(msm_port->clk, port->uartclk);
+
+	spin_lock_irqsave(&port->lock, flags);
+	*saved_flags = flags;
+
 	/* RX stale watermark */
 	rxstale = entry->rxstale;
 	watermark = UART_IPR_STALE_LSB & rxstale;
-	watermark |= UART_IPR_RXSTALE_LAST;
-	watermark |= UART_IPR_STALE_TIMEOUT_MSB & (rxstale << 2);
+	if (msm_port->is_uartdm) {
+		mask = UART_DM_IPR_STALE_TIMEOUT_MSB;
+	} else {
+		watermark |= UART_IPR_RXSTALE_LAST;
+		mask = UART_IPR_STALE_TIMEOUT_MSB;
+	}
+
+	watermark |= mask & (rxstale << 2);
+
 	msm_write(port, watermark, UART_IPR);
 
 	/* set RX watermark */
@@ -476,7 +971,7 @@ static void msm_init_clock(struct uart_port *port)
 static int msm_startup(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
-	unsigned int data, rfr_level;
+	unsigned int data, rfr_level, mask;
 	int ret;
 
 	snprintf(msm_port->name, sizeof(msm_port->name),
@@ -496,11 +991,23 @@ static int msm_startup(struct uart_port *port)
 
 	/* set automatic RFR level */
 	data = msm_read(port, UART_MR1);
-	data &= ~UART_MR1_AUTO_RFR_LEVEL1;
+
+	if (msm_port->is_uartdm)
+		mask = UART_DM_MR1_AUTO_RFR_LEVEL1;
+	else
+		mask = UART_MR1_AUTO_RFR_LEVEL1;
+
+	data &= ~mask;
 	data &= ~UART_MR1_AUTO_RFR_LEVEL0;
-	data |= UART_MR1_AUTO_RFR_LEVEL1 & (rfr_level << 2);
+	data |= mask & (rfr_level << 2);
 	data |= UART_MR1_AUTO_RFR_LEVEL0 & rfr_level;
 	msm_write(port, data, UART_MR1);
+
+	if (msm_port->is_uartdm) {
+		msm_request_tx_dma(msm_port, msm_port->uart.mapbase);
+		msm_request_rx_dma(msm_port, msm_port->uart.mapbase);
+	}
+
 	return 0;
 }
 
@@ -511,6 +1018,9 @@ static void msm_shutdown(struct uart_port *port)
 	msm_port->imr = 0;
 	msm_write(port, 0, UART_IMR); /* disable interrupts */
 
+	if (msm_port->is_uartdm)
+		msm_release_dma(msm_port);
+
 	clk_disable_unprepare(msm_port->clk);
 
 	free_irq(port->irq, port);
@@ -519,14 +1029,19 @@ static void msm_shutdown(struct uart_port *port)
 static void msm_set_termios(struct uart_port *port, struct ktermios *termios,
 			    struct ktermios *old)
 {
+	struct msm_port *msm_port = UART_TO_MSM(port);
+	struct msm_dma *dma = &msm_port->rx_dma;
 	unsigned long flags;
 	unsigned int baud, mr;
 
 	spin_lock_irqsave(&port->lock, flags);
 
+	if (dma->chan) /* Terminate if any */
+		msm_stop_dma(port, dma);
+
 	/* calculate and set baud rate */
-	baud = uart_get_baud_rate(port, termios, old, 300, 115200);
-	baud = msm_set_baud_rate(port, baud);
+	baud = uart_get_baud_rate(port, termios, old, 300, 4000000);
+	baud = msm_set_baud_rate(port, baud, &flags);
 	if (tty_termios_baud_rate(termios))
 		tty_termios_encode_baud_rate(termios, baud, baud);
 
@@ -588,6 +1103,9 @@ static void msm_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	uart_update_timeout(port, termios->c_cflag, baud);
 
+	/* Try to use DMA */
+	msm_start_rx_dma(msm_port);
+
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
@@ -765,7 +1283,7 @@ static void msm_poll_put_char(struct uart_port *port, unsigned char c)
 	msm_write(port, 0, UART_IMR);
 
 	if (msm_port->is_uartdm)
-		reset_dm_count(port, 1);
+		msm_reset_dm_count(port, 1);
 
 	/* Wait until FIFO is empty */
 	while (!(msm_read(port, UART_SR) & UART_SR_TX_READY))
@@ -839,7 +1357,7 @@ static struct msm_port msm_uart_ports[] = {
 
 #define UART_NR	ARRAY_SIZE(msm_uart_ports)
 
-static inline struct uart_port *get_port_from_line(unsigned int line)
+static inline struct uart_port *msm_get_port_from_line(unsigned int line)
 {
 	return &msm_uart_ports[line].uart;
 }
@@ -866,7 +1384,7 @@ static void __msm_console_write(struct uart_port *port, const char *s,
 
 	spin_lock(&port->lock);
 	if (is_uartdm)
-		reset_dm_count(port, count);
+		msm_reset_dm_count(port, count);
 
 	i = 0;
 	while (i < count) {
@@ -911,7 +1429,7 @@ static void msm_console_write(struct console *co, const char *s,
 
 	BUG_ON(co->index < 0 || co->index >= UART_NR);
 
-	port = get_port_from_line(co->index);
+	port = msm_get_port_from_line(co->index);
 	msm_port = UART_TO_MSM(port);
 
 	__msm_console_write(port, s, count, msm_port->is_uartdm);
@@ -928,7 +1446,7 @@ static int __init msm_console_setup(struct console *co, char *options)
 	if (unlikely(co->index >= UART_NR || co->index < 0))
 		return -ENXIO;
 
-	port = get_port_from_line(co->index);
+	port = msm_get_port_from_line(co->index);
 
 	if (unlikely(!port->membase))
 		return -ENXIO;
@@ -1043,7 +1561,7 @@ static int msm_serial_probe(struct platform_device *pdev)
 
 	dev_info(&pdev->dev, "msm_serial: detected port #%d\n", line);
 
-	port = get_port_from_line(line);
+	port = msm_get_port_from_line(line);
 	port->dev = &pdev->dev;
 	msm_port = UART_TO_MSM(port);
 
diff --git a/drivers/tty/serial/msm_serial.h b/drivers/tty/serial/msm_serial.h
index 737f69fe7113..bc1d7b39eba8 100644
--- a/drivers/tty/serial/msm_serial.h
+++ b/drivers/tty/serial/msm_serial.h
@@ -20,11 +20,12 @@
 
 #define UART_MR1_AUTO_RFR_LEVEL0	0x3F
 #define UART_MR1_AUTO_RFR_LEVEL1	0x3FF00
-#define UART_MR1_RX_RDY_CTL    		(1 << 7)
-#define UART_MR1_CTS_CTL       		(1 << 6)
+#define UART_DM_MR1_AUTO_RFR_LEVEL1	0xFFFFFF00
+#define UART_MR1_RX_RDY_CTL		BIT(7)
+#define UART_MR1_CTS_CTL		BIT(6)
 
 #define UART_MR2			0x0004
-#define UART_MR2_ERROR_MODE		(1 << 6)
+#define UART_MR2_ERROR_MODE		BIT(6)
 #define UART_MR2_BITS_PER_CHAR		0x30
 #define UART_MR2_BITS_PER_CHAR_5	(0x0 << 4)
 #define UART_MR2_BITS_PER_CHAR_6	(0x1 << 4)
@@ -58,26 +59,28 @@
 #define UART_CR_CMD_SET_RFR		(13 << 4)
 #define UART_CR_CMD_RESET_RFR		(14 << 4)
 #define UART_CR_CMD_PROTECTION_EN	(16 << 4)
-#define UART_CR_CMD_STALE_EVENT_ENABLE	(80 << 4)
+#define UART_CR_CMD_STALE_EVENT_DISABLE	(6 << 8)
+#define UART_CR_CMD_STALE_EVENT_ENABLE	(5 << 8)
 #define UART_CR_CMD_FORCE_STALE		(4 << 8)
 #define UART_CR_CMD_RESET_TX_READY	(3 << 8)
-#define UART_CR_TX_DISABLE		(1 << 3)
-#define UART_CR_TX_ENABLE		(1 << 2)
-#define UART_CR_RX_DISABLE		(1 << 1)
-#define UART_CR_RX_ENABLE		(1 << 0)
+#define UART_CR_TX_DISABLE		BIT(3)
+#define UART_CR_TX_ENABLE		BIT(2)
+#define UART_CR_RX_DISABLE		BIT(1)
+#define UART_CR_RX_ENABLE		BIT(0)
 #define UART_CR_CMD_RESET_RXBREAK_START	((1 << 11) | (2 << 4))
 
 #define UART_IMR		0x0014
-#define UART_IMR_TXLEV		(1 << 0)
-#define UART_IMR_RXSTALE	(1 << 3)
-#define UART_IMR_RXLEV		(1 << 4)
-#define UART_IMR_DELTA_CTS	(1 << 5)
-#define UART_IMR_CURRENT_CTS	(1 << 6)
-#define UART_IMR_RXBREAK_START	(1 << 10)
+#define UART_IMR_TXLEV			BIT(0)
+#define UART_IMR_RXSTALE		BIT(3)
+#define UART_IMR_RXLEV			BIT(4)
+#define UART_IMR_DELTA_CTS		BIT(5)
+#define UART_IMR_CURRENT_CTS		BIT(6)
+#define UART_IMR_RXBREAK_START		BIT(10)
 
 #define UART_IPR_RXSTALE_LAST		0x20
 #define UART_IPR_STALE_LSB		0x1F
 #define UART_IPR_STALE_TIMEOUT_MSB	0x3FF80
+#define UART_DM_IPR_STALE_TIMEOUT_MSB	0xFFFFFF80
 
 #define UART_IPR	0x0018
 #define UART_TFWR	0x001C
@@ -96,20 +99,20 @@
 #define UART_TEST_CTRL		0x0050
 
 #define UART_SR			0x0008
-#define UART_SR_HUNT_CHAR	(1 << 7)
-#define UART_SR_RX_BREAK	(1 << 6)
-#define UART_SR_PAR_FRAME_ERR	(1 << 5)
-#define UART_SR_OVERRUN		(1 << 4)
-#define UART_SR_TX_EMPTY	(1 << 3)
-#define UART_SR_TX_READY	(1 << 2)
-#define UART_SR_RX_FULL		(1 << 1)
-#define UART_SR_RX_READY	(1 << 0)
+#define UART_SR_HUNT_CHAR	BIT(7)
+#define UART_SR_RX_BREAK	BIT(6)
+#define UART_SR_PAR_FRAME_ERR	BIT(5)
+#define UART_SR_OVERRUN		BIT(4)
+#define UART_SR_TX_EMPTY	BIT(3)
+#define UART_SR_TX_READY	BIT(2)
+#define UART_SR_RX_FULL		BIT(1)
+#define UART_SR_RX_READY	BIT(0)
 
 #define UART_RF			0x000C
 #define UARTDM_RF		0x0070
 #define UART_MISR		0x0010
 #define UART_ISR		0x0014
-#define UART_ISR_TX_READY	(1 << 7)
+#define UART_ISR_TX_READY	BIT(7)
 
 #define UARTDM_RXFS		0x50
 #define UARTDM_RXFS_BUF_SHIFT	0x7
@@ -119,6 +122,12 @@
 #define UARTDM_DMEN_RX_SC_ENABLE BIT(5)
 #define UARTDM_DMEN_TX_SC_ENABLE BIT(4)
 
+#define UARTDM_DMEN_TX_BAM_ENABLE BIT(2)	/* UARTDM_1P4 */
+#define UARTDM_DMEN_TX_DM_ENABLE  BIT(0)	/* < UARTDM_1P4 */
+
+#define UARTDM_DMEN_RX_BAM_ENABLE BIT(3)	/* UARTDM_1P4 */
+#define UARTDM_DMEN_RX_DM_ENABLE  BIT(1)	/* < UARTDM_1P4 */
+
 #define UARTDM_DMRX		0x34
 #define UARTDM_NCF_TX		0x40
 #define UARTDM_RX_TOTAL_SNAP	0x38
diff --git a/drivers/tty/serial/msm_smd_tty.c b/drivers/tty/serial/msm_smd_tty.c
new file mode 100644
index 000000000000..1ffab5ea0d92
--- /dev/null
+++ b/drivers/tty/serial/msm_smd_tty.c
@@ -0,0 +1,417 @@
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/soc/qcom/smd.h>
+#include <linux/suspend.h>
+
+#include <linux/tty.h>
+#include <linux/tty_driver.h>
+#include <linux/tty_flip.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#define	MAX_SMD_TTY_PORTS	32
+#define	CHANNEL_PROBE_TIMEOUT	60000
+
+static struct tty_driver *smd_tty_driver;
+
+struct msm_smd_port_info_s {
+	char	*name;
+	u32	flags;
+	int	index;
+	int	smd_tty_info_index;
+};
+
+#define	SMD_DROP_ONE_TX_BYTE	(1)
+#define	SMD_TTY_ACL		(1 << 1)
+#define	SMD_TTY_CMD		(1 << 2)
+
+static struct msm_smd_port_info_s msm_smd_port_info[] = {
+	{
+		.name	= "DUMMY_SMD_TTY0",
+		.flags	= 0,
+		.index	= 0,
+		.smd_tty_info_index = 0,
+	}, {
+		.name	= "DUMMY_SMD_TTY1",
+		.flags	= 0,
+		.index	= 1,
+		.smd_tty_info_index = 1,
+	}, {
+		.name	= "APPS_RIVA_BT_ACL",
+		.flags	= SMD_DROP_ONE_TX_BYTE | SMD_TTY_ACL,
+		.index	= 2,
+		.smd_tty_info_index = 3,
+	}, {
+		.name	= "APPS_RIVA_BT_CMD",
+		.flags	= SMD_DROP_ONE_TX_BYTE | SMD_TTY_CMD,
+		.index	= 3,
+		.smd_tty_info_index = 3,
+	},
+};
+
+struct msm_smd_port_info_s *match_smd_port_info(char *name)
+{
+	int i;
+	struct msm_smd_port_info_s *pinfo = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(msm_smd_port_info); i++) {
+		if (!strcmp(name, msm_smd_port_info[i].name)) {
+			pinfo = &msm_smd_port_info[i];
+			break;
+		}
+	}
+
+	return pinfo;
+}
+
+struct msm_smd_channel_info {
+        struct	qcom_smd_device *cmd_sdev;
+        struct	qcom_smd_channel *channel;
+
+        struct	completion ack;
+        struct	mutex lock;
+
+	u32	flags;
+	int	smd_tty_info_index;
+};
+
+struct msm_smd_tty_info_s {
+	/* For some special case (like BT), out/in channel are differnet.
+	 * In most case, they should be same.
+	 */
+	struct	msm_smd_channel_info *cmd_info;
+	struct	msm_smd_channel_info *acl_info;
+
+        struct	tty_port port;
+        struct	device *tty;
+
+	struct	mutex open_lock;
+	struct	completion probe_done;
+	int	opened;
+};
+
+void dump_buf(unsigned char *buf, int len)
+{
+        int i;
+        unsigned char *tmp = buf;
+
+        printk("\n\t");
+
+        for (i = 0; i < len; i++) {
+                printk("0x%02x ", tmp[i]);
+                if (((i + 1) % 16) == 0)
+                        printk("\n\t");
+        }
+}
+
+static struct msm_smd_tty_info_s msm_smd_tty_info[MAX_SMD_TTY_PORTS];
+
+static int qcom_smd_tty_probe(struct qcom_smd_device *sdev)
+{
+        struct msm_smd_channel_info *ch_info;
+	struct msm_smd_port_info_s *pinfo;
+	int idx = 0;
+
+        ch_info = devm_kzalloc(&sdev->dev, sizeof(*ch_info), GFP_KERNEL);
+        if (!ch_info)
+                return -ENOMEM;
+
+        mutex_init(&ch_info->lock);
+        init_completion(&ch_info->ack);
+        ch_info->channel = sdev->channel;
+
+	pinfo = match_smd_port_info(ch_info->channel->name);
+	if (!pinfo) {
+		pr_err("Can't match channel %s\n", ch_info->channel->name);
+		kfree(ch_info);
+		return -EINVAL;
+	}
+
+	ch_info->flags = pinfo->flags;
+	idx = ch_info->smd_tty_info_index = pinfo->smd_tty_info_index;
+
+	if (pinfo->flags & SMD_TTY_ACL) {
+		msm_smd_tty_info[idx].acl_info = ch_info;
+	} else if (pinfo->flags & SMD_TTY_CMD) {
+		msm_smd_tty_info[idx].cmd_info = ch_info;
+	} else {
+		msm_smd_tty_info[idx].cmd_info =
+		msm_smd_tty_info[idx].acl_info = ch_info;
+	}
+
+	/* We use all here to prevent the maltiple port open
+	 * blocking.
+	 */
+	complete_all(&msm_smd_tty_info[pinfo->index].probe_done);
+
+        dev_set_drvdata(&sdev->dev, ch_info);
+	return 0;
+}
+
+static void qcom_smd_tty_remove(struct qcom_smd_device *sdev)
+{
+        struct msm_smd_channel_info *ch_info = dev_get_drvdata(&sdev->dev);
+
+	kfree(ch_info);
+	return;
+}
+
+static int qcom_smd_tty_callback(struct qcom_smd_device *sdev,
+                                 const void *data,
+                                 size_t count)
+{
+	unsigned char *ptr, *tmp;
+	int buf_len;
+        struct msm_smd_channel_info *ch_info;
+	struct tty_struct *tty;
+
+	ch_info = dev_get_drvdata(&sdev->dev);
+	tty = tty_port_tty_get(
+		&msm_smd_tty_info[ch_info->smd_tty_info_index].port);
+	if (!tty) {
+		pr_err("can't get tty for index: %d\n",
+			ch_info->smd_tty_info_index);
+		return 0;
+	}
+
+	if (ch_info->flags & SMD_DROP_ONE_TX_BYTE)
+		buf_len = count + 1;
+	else
+		buf_len = count;
+
+	if (buf_len > ch_info->channel->fifo_size)
+		buf_len = ch_info->channel->fifo_size;
+
+        buf_len = tty_prepare_flip_string(tty->port, &ptr, buf_len);
+	if (buf_len <= 0) {
+		pr_err("can't get flip buffer for tty index: %d\n",
+			ch_info->smd_tty_info_index);
+		tty_kref_put(tty);
+		return 0;
+	}
+
+	tmp = ptr;
+	if (ch_info->flags & SMD_TTY_ACL) {
+		*ptr++ = HCI_ACLDATA_PKT;
+	} else if (ch_info->flags & SMD_TTY_CMD) {
+		*ptr++ = HCI_EVENT_PKT;
+	}
+
+	memcpy_fromio(ptr, data, count);
+
+	tty_flip_buffer_push(tty->port);
+	tty_kref_put(tty);
+
+	return 0;
+}
+
+static const struct of_device_id qcom_smd_tty_of_match[] = {
+	{ .compatible = "qcom,hci-smd-bt-cmd"},
+	{ .compatible = "qcom,hci-smd-bt-acl"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, qcom_smd_tty_of_match);
+
+static struct qcom_smd_driver qcom_smd_tty_driver = {
+	.probe = qcom_smd_tty_probe,
+	.remove = qcom_smd_tty_remove,
+	.callback = qcom_smd_tty_callback,
+	.driver  = {
+		.name  = "qcom_smd_tty",
+		.owner = THIS_MODULE,
+		.of_match_table = qcom_smd_tty_of_match,
+	},
+};
+
+static int smd_tty_open(struct tty_struct *tty, struct file *f)
+{
+	struct msm_smd_tty_info_s *info = msm_smd_tty_info + tty->index;
+
+	return tty_port_open(&info->port, tty, f);
+}
+
+static void smd_tty_close(struct tty_struct *tty, struct file *f)
+{
+	struct msm_smd_tty_info_s *info = tty->driver_data;
+
+	tty_port_close(&info->port, tty, f);
+}
+
+static int smd_tty_write(struct tty_struct *tty, const unsigned char *buf,
+									int len)
+{
+	int ret = 0;
+	int avail = len;
+	struct msm_smd_tty_info_s *info = tty->driver_data;
+	struct msm_smd_channel_info *cinfo;
+
+	if ((info->acl_info->flags & SMD_DROP_ONE_TX_BYTE) ||
+		(info->cmd_info->flags & SMD_DROP_ONE_TX_BYTE)) {
+		if (HCI_COMMAND_PKT == *buf)
+			cinfo = info->cmd_info;
+		else
+			cinfo = info->acl_info;
+		buf++;
+		avail--;
+	}
+
+        mutex_lock(&cinfo->lock);
+        ret = qcom_smd_send(cinfo->channel, buf, avail);
+        if (ret) {
+                pr_err("smd_tty_write failed on smd tty %d\n", tty->index);
+        }
+        mutex_unlock(&cinfo->lock);
+
+	return len;
+}
+
+static int smd_tty_port_activate(struct tty_port *tport,
+                                 struct tty_struct *tty)
+{
+	int ret = 0;
+        unsigned int n = tty->index;
+        struct msm_smd_tty_info_s *info;
+
+        if (n >= ARRAY_SIZE(msm_smd_port_info))
+                return -ENODEV;
+
+        info = &msm_smd_tty_info[n];
+
+        mutex_lock(&info->open_lock);
+        tty->driver_data = info;
+	if (info->opened) {
+        	mutex_unlock(&info->open_lock);
+		return 0;
+	}
+
+	/* We'd better to add the channel probe for BT here.
+	 * So the open will be blocked till channel is probe.
+	 */
+	ret = wait_for_completion_interruptible_timeout(
+		&info->probe_done, msecs_to_jiffies(CHANNEL_PROBE_TIMEOUT));
+	if (ret == 0) {
+		pr_err("Wait for smd channel ready timeout\n");
+		info->opened--;
+		return -ETIMEDOUT;
+	} else if (ret < 0) {
+		pr_err("Error waiting for smd channel ready\n");
+		info->opened--;
+		return ret;
+	}
+
+	info->opened++;
+        mutex_unlock(&info->open_lock);
+
+        return 0;
+}
+
+static void smd_tty_port_shutdown(struct tty_port *tport)
+{
+        struct msm_smd_tty_info_s *info;
+        struct tty_struct *tty = tty_port_tty_get(tport);
+
+        info = tty->driver_data;
+
+        mutex_lock(&info->open_lock);
+	info->opened--;
+	if (info->opened > 0) {
+		mutex_unlock(&info->open_lock);
+        	tty_kref_put(tty);
+		return;
+	}
+	mutex_unlock(&info->open_lock);
+        tty_kref_put(tty);
+	return;
+}
+
+static const struct tty_port_operations smd_tty_port_ops = {
+        .shutdown = smd_tty_port_shutdown,
+        .activate = smd_tty_port_activate,
+};
+
+static const struct tty_operations smd_tty_ops = {
+	.open = smd_tty_open,
+	.close = smd_tty_close,
+	.write = smd_tty_write,
+};
+
+static void smd_tty_device_init(int idx)
+{
+        struct tty_port *port;
+
+        port = &msm_smd_tty_info[idx].port;
+        tty_port_init(port);
+	mutex_init(&msm_smd_tty_info[idx].open_lock);
+	init_completion(&msm_smd_tty_info[idx].probe_done);
+
+	msm_smd_tty_info[idx].opened = 0;
+
+        port->ops = &smd_tty_port_ops;
+        msm_smd_tty_info[idx].tty = tty_port_register_device(port,
+			smd_tty_driver, idx, NULL);
+	return;
+}
+
+static int smd_tty_register_driver(void)
+{
+	int ret = 0;
+
+	smd_tty_driver = tty_alloc_driver(MAX_SMD_TTY_PORTS,
+		TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV |
+		TTY_DRIVER_RESET_TERMIOS);
+	if (IS_ERR(smd_tty_driver)) {
+		ret = PTR_ERR(smd_tty_driver);
+		return ret;
+	}
+
+	smd_tty_driver->owner = THIS_MODULE;
+	smd_tty_driver->driver_name = "smd_tty_driver";
+	smd_tty_driver->name = "smd";
+	smd_tty_driver->major = 0;
+	smd_tty_driver->minor_start = 0;
+	smd_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
+	smd_tty_driver->subtype = SERIAL_TYPE_NORMAL;
+	smd_tty_driver->init_termios = tty_std_termios;
+	smd_tty_driver->init_termios.c_iflag = 0;
+	smd_tty_driver->init_termios.c_oflag = 0;
+	smd_tty_driver->init_termios.c_cflag = B38400 | CS8 | CREAD;
+	smd_tty_driver->init_termios.c_lflag = 0;
+	tty_set_operations(smd_tty_driver, &smd_tty_ops);
+
+	ret = tty_register_driver(smd_tty_driver);
+	if (ret) {
+		put_tty_driver(smd_tty_driver);
+		pr_err("register tty driver failed (%d) for smd tty\n", ret);
+	}
+
+	return ret;
+}
+
+static int __init smd_tty_init(void)
+{
+	int ret = 0;
+	int i = 0;
+	ret = smd_tty_register_driver();
+	if (ret)
+		return ret;
+
+	for (i = 0; i < ARRAY_SIZE(msm_smd_port_info); i++)
+		smd_tty_device_init(i);
+
+	qcom_smd_driver_register(&qcom_smd_tty_driver);
+
+	return 0;
+}
+
+module_init(smd_tty_init);