tegra serial: Improving the rx flow.

Following improvements are done in the uart rx and tx path:
- Separate locks for rx and tx paths.
- Implementing the continuous double buffering with same buffer for
  uart rx.
- Deactivating the rts line only incase of if dma does not read the
  fifo i.e. nonmultiple of 4.
- Handling the modem control signal chnage properly.
- Created the separate workqueue for the receive to handle dma thresold
 callback as well as the rx uart interrupts.

Tested on harmony and whistler with uart testcase and BT on harmony.
we tested the simulatenous file transfer and the A2DP on BT-harmony
and did not observe any glitches.

Change-Id: Id451ca2f42e246b524a7c9e4bbf3aa832cd0d535
Reviewed-on: http://git-master/r/1172
Reviewed-by: Gary King <gking@nvidia.com>
Reviewed-by: Anantha Idapalapati <aidapalapati@nvidia.com>
Tested-by: Anantha Idapalapati <aidapalapati@nvidia.com>

1 files changed, 588 insertions, 260 deletions

diff --git a/drivers/serial/tegra_hsuart.c b/drivers/serial/tegra_hsuart.c
index 667bb65f1eee..228921d7fd3e 100644
--- a/drivers/serial/tegra_hsuart.c
+++ b/drivers/serial/tegra_hsuart.c
@@ -47,16 +47,26 @@
 #include "nvrm_interrupt.h"
 #include "nvrm_power.h"
 
-
-#define function_trace(i) printk(KERN_ERR "%s()%s \n",__func__, (i)?"++":"--");
+#if 0
+#define function_trace(i) printk(KERN_ERR "%s()%s \n",__func__, (i)?"++":"--")
+#else
+#define function_trace(i)
+#endif
 #define TX_EMPTY_STATUS (NV_DRF_DEF(UART, LSR, TMTY, EMPTY) | \
 	NV_DRF_DEF(UART, LSR, THRE, EMPTY))
 
-#define UART_RX_DMA_BUFFER_SIZE    (4 *  1024 * 16)
+/* double buffering- mode 32K of single buffer size */
+#define UART_RX_DMA_BUFFER_SIZE    (2 *  32 * 1024)
 
+#define USE_DMA_TX_THRESOLD 16
 static int tx_force_pio = 0;
 static int rx_force_pio = 0;
 
+typedef enum {
+	UART_STOPPED = 0x0,
+	UART_RUNNING = 0x1,
+} uart_state_t;
+
 struct tegra_uart_port {
 	struct uart_port	uport;
 	char			port_name[32];
@@ -107,58 +117,225 @@ struct tegra_uart_port {
 
 	bool			tx_pio_inflight;
 
-	struct work_struct	work;
-	struct workqueue_struct	*work_queue;
+	struct work_struct	tx_work;
+	struct workqueue_struct	*tx_work_queue;
+
+	struct work_struct	rx_work;
+	struct workqueue_struct	*rx_work_queue;
+	int last_read_index;
+	int already_read_bytecount;
+	int last_transfer_count;
+
+	spinlock_t		rx_lock;
+	spinlock_t		tx_lock;
+	spinlock_t		reg_access_lock;
 
+	uart_state_t tx_state;
+
+	uart_state_t rx_state;
 };
 
 static void tegra_set_baudrate(struct tegra_uart_port *t, unsigned int baud);
 static void tegra_set_mctrl(struct uart_port *u, unsigned int mctrl);
-static void do_handle_rx_pio(struct uart_port *u);
 static void set_rts(struct tegra_uart_port *t, bool active);
 static void set_dtr(struct tegra_uart_port *t, bool active);
 
-static inline int tegra_uart_isbreak(struct uart_port *u)
+static inline int tegra_uart_isbreak(struct tegra_uart_port *t)
+{
+	unsigned char lsr;
+	unsigned char rx_char;
+	bool is_break_detected = false;
+
+	lsr = readb(t->regs + UART_LSR_0);
+	if (lsr & NV_DRF_DEF(UART, LSR, BRK, BREAK)) {
+		is_break_detected = true;
+
+		/* Again read the LSR and if there is fifo error with data then
+		 * reset here */
+		lsr = readb(t->regs + UART_LSR_0);
+		if (lsr & NV_DRF_DEF(UART, LSR, FIFOE, ERR)) {
+			if (lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO))
+				return is_break_detected;
+
+			/* Fifo error without rx data, read fifo till rx fifo
+			 * empty: hw issue to detect next break */
+			do {
+				lsr = readb(t->regs + UART_LSR_0);
+				if (!(lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO)))
+					break;
+				rx_char = readb(t->regs + UART_THR_DLAB_0_0);
+			} while(1);
+		}
+	}
+	return is_break_detected;
+}
+
+
+static char do_decode_rx_error(struct uart_port *u)
 {
 	struct tegra_uart_port *t;
+	char flag = TTY_NORMAL;
 	unsigned char lsr;
+	unsigned char rx_char;
 
 	t = container_of(u, struct tegra_uart_port, uport);
+
 	lsr = readb(t->regs + UART_LSR_0);
-	if (lsr & NV_DRF_DEF(UART, LSR, BRK, BREAK)) {
-		/* If FIFO read error without any data, reset the Rx FIFO */
+	if (lsr & NV_DRF_DEF(UART, LSR, OVRF, OVERRUN_ERROR)) {
+		/* Overrrun error  */
+		flag |= TTY_OVERRUN;
+		t->uport.icount.overrun++;
+		dev_err(u->dev, "Got overrun errors\n");
+	} else if (lsr & NV_DRF_DEF(UART, LSR, PERR, PARITY_ERR)) {
+		/* Parity error */
+		flag |= TTY_PARITY;
+		t->uport.icount.parity++;
+		dev_err(u->dev, "Got Parity errors\n");
+	} else if (lsr & NV_DRF_DEF(UART, LSR, FERR, FRAME_ERR)) {
+		flag |= TTY_FRAME;
+		dev_err(u->dev, "Got frame errors\n");
+	} else if (lsr & NV_DRF_DEF(UART, LSR, BRK, BREAK)) {
+		flag |= TTY_BREAK;
+		dev_err(u->dev, "Got Break \n");
+
 		lsr = readb(t->regs + UART_LSR_0);
-		if (!(lsr & 0x1) && (lsr & 0x80))  {
-			unsigned char fcr = t->fcr_shadow;
-			fcr |= 0x2;
-			writeb(fcr, t->regs + UART_IIR_FCR_0);
+		if (lsr & NV_DRF_DEF(UART, LSR, FIFOE, ERR)) {
+			if (lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO))
+				return flag;
+			/* Fifo error without rx data, read fifo till rx fifo
+			 * empty: hw issue to detect next break */
+			do {
+				lsr = readb(t->regs + UART_LSR_0);
+				if (!(lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO)))
+					break;
+				rx_char = readb(t->regs + UART_THR_DLAB_0_0);
+			} while(1);
 		}
-		return 1;
 	}
-	return 0;
+	return flag;
 }
 
-void tegra_rx_dma_threshold_callback(struct tegra_dma_req *req, int err)
+static int do_handle_rx_pio(struct uart_port *u)
 {
-	struct uart_port *u = req->data;
 	struct tegra_uart_port *t;
-	unsigned long flags;
+	struct tty_struct *tty = u->info->port.tty;
+	int new_read_count = 0;
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	do {
+		char flag = TTY_NORMAL;
+		unsigned char lsr;
+		unsigned char ch;
+
+		flag =  do_decode_rx_error(u);
+
+		lsr = readb(t->regs + UART_LSR_0);
+		if (!(lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO)))
+			break;
+
+		ch = readb(t->regs + UART_THR_DLAB_0_0);
+		dev_vdbg(u->dev, "%c\n", ch);
+		if (!uart_handle_sysrq_char(u, c)) {
+			tty_insert_flip_char(tty, ch, flag);
+			t->uport.icount.rx++;
+			new_read_count++;
+		}
+	} while (1);
+
+	return new_read_count;
+}
+
+static void do_handle_rx_dma(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+	unsigned char ier;
+	unsigned long irq_flag;
 
 	t = container_of(u, struct tegra_uart_port, uport);
 
-	spin_lock_irqsave(&u->lock, flags);
+	spin_lock_irqsave(&t->reg_access_lock, irq_flag);
 
-	if (t->rts_active)
-		set_rts(t, false);
-	tegra_dma_dequeue(t->rx_dma);
+	/* Disable interrups */
+	ier = t->ier_shadow;
+	/* Sw workaround to disable the rx timeout interrupt.*/
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RHR, ENABLE, ier);
+	writeb(ier, t->regs + UART_IER_DLAB_0_0);
 
-	/* enqueue the request again */
-	tegra_dma_enqueue_req(t->rx_dma, req);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RHR, DISABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RXS, DISABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RX_TIMEOUT, DISABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_EORD, DISABLE, ier);
+	t->ier_shadow = ier;
+	writeb(ier, t->regs + UART_IER_DLAB_0_0);
 
-	if (t->rts_active)
-		set_rts(t, true);
+	spin_unlock_irqrestore(&t->reg_access_lock, irq_flag);
 
-	spin_unlock_irqrestore(&u->lock, flags);
+	/*Schedule the receive workqueue */
+	queue_work(t->rx_work_queue, &t->rx_work);
+}
+
+static int copy_dma_buffer_to_tty_buffer(struct tegra_uart_port *t,
+			int new_trans_count)
+{
+	int copied_count;
+	unsigned char *dma_virt_buf = (char *)t->rx_dma_req.virt_addr;
+	struct uart_port *u = &t->uport;
+	int ret_copied;
+
+/*	printk(KERN_ERR "newtranscount %x and last trans count %x \n",
+			new_trans_count, t->last_transfer_count);*/
+	if (new_trans_count < t->last_transfer_count) {
+		/* dma buffer roundoff */
+		copied_count = UART_RX_DMA_BUFFER_SIZE - t->last_transfer_count;
+		ret_copied = tty_insert_flip_string(u->info->port.tty,
+					dma_virt_buf + t->last_read_index,
+					copied_count);
+		if (copied_count != ret_copied) {
+			printk(KERN_ERR "dma_to_tty lost data(1): Trying %x got %x lost %x\n",
+			copied_count,ret_copied,  copied_count - ret_copied);
+		}
+		if (new_trans_count) {
+			copied_count += new_trans_count;
+			ret_copied = tty_insert_flip_string(u->info->port.tty, dma_virt_buf,
+						new_trans_count);
+			if (copied_count != ret_copied) {
+				printk(KERN_ERR "dma_to_tty lost data(2): Trying"
+						" %x got %x lost %x\n",
+						copied_count,ret_copied,
+						copied_count - ret_copied);
+			}
+		}
+		t->uport.icount.rx += copied_count;
+		t->last_read_index = new_trans_count;
+		t->last_transfer_count = new_trans_count;
+	} else {
+		copied_count = new_trans_count - t->last_transfer_count;
+		if (copied_count) {
+			ret_copied = tty_insert_flip_string(u->info->port.tty,
+					dma_virt_buf + t->last_read_index,
+					copied_count);
+			if (copied_count != ret_copied) {
+				printk(KERN_ERR "Lost some data last data(3): Trying"
+						" %x got %x lost %x\n",
+						copied_count,ret_copied,
+						copied_count - ret_copied);
+			}
+			t->uport.icount.rx += copied_count;
+			t->last_read_index += copied_count;
+			t->last_transfer_count = new_trans_count;
+		}
+	}
+/*	printk(KERN_ERR "rx count %x  and copied count %x\n",t->uport.icount.rx, copied_count);*/
+	dev_dbg(u->dev, "Received %d bytes\n", copied_count);
+	return copied_count;
+}
+
+void tegra_rx_dma_threshold_callback(struct tegra_dma_req *req, int err)
+{
+	struct uart_port *u = req->data;
+	struct tegra_uart_port *t;
+	t = container_of(u, struct tegra_uart_port, uport);
+	queue_work(t->rx_work_queue, &t->rx_work);
 }
 
 /* It is expected that the callers take the UART lock when this API is called.
@@ -176,109 +353,215 @@ void tegra_rx_dma_complete_callback(struct tegra_dma_req *req, int err)
 {
 	struct uart_port *u = req->data;
 	struct tegra_uart_port *t;
-	struct tty_struct *tty = u->info->port.tty;
+	int dma_read_count = 0;
+	int pio_read_count = 0;
 
 	/* If we are here, DMA is stopped */
-
 	t = container_of(u, struct tegra_uart_port, uport);
-	if (req->bytes_transferred) {
-		t->uport.icount.rx += req->bytes_transferred;
-		tty_insert_flip_string(tty,
-			((unsigned char *)(req->virt_addr)),
-			req->bytes_transferred);
-		dev_dbg(u->dev, "Received %d bytes\n", req->bytes_transferred);
+
+	spin_lock(&t->rx_lock);
+	dma_read_count = copy_dma_buffer_to_tty_buffer(t, req->bytes_transferred);
+	pio_read_count = do_handle_rx_pio(u);
+	spin_unlock(&t->rx_lock);
+
+	if ((dma_read_count > 0) || pio_read_count) {
+		tty_flip_buffer_push(u->info->port.tty);
+	}
+
+}
+
+static void tegra_rx_dma_workqueue(struct work_struct *w)
+{
+	struct tegra_uart_port *t;
+	unsigned char lsr;
+	bool is_dma_stopped = false;
+	int dma_trans_count;
+	int dma_read_count = 0;
+	int pio_read_count = 0;
+	int start_status;
+	unsigned long irq_flags;
+	bool is_break_detected = false;
+	struct uart_port *u;
+	unsigned char ier;
+
+	t = container_of(w, struct tegra_uart_port, rx_work);
+	u = &t->uport;
+
+	spin_lock(&t->rx_lock);
+
+	is_break_detected = tegra_uart_isbreak(t);
+	lsr = readb(t->regs + UART_LSR_0);
+	if (lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO)) {
+		/* Data available in fifo */
+		if (t->rts_active)
+			set_rts(t, false);
+
+		is_dma_stopped = true;
+		dma_trans_count = tegra_dma_get_transfer_count(t->rx_dma,
+					&t->rx_dma_req, true);
+		if (dma_trans_count < 0) {
+			spin_unlock(&t->rx_lock);
+			/* Failure if dma is not running and reached here */
+			BUG();
+			return;
+		}
+
+		dma_read_count = copy_dma_buffer_to_tty_buffer(t, dma_trans_count);
+		pio_read_count = do_handle_rx_pio(u);
+		t->last_read_index = 0;
+		t->already_read_bytecount = 0;
+		t->last_transfer_count = 0;
+		start_status = tegra_dma_start_dma(t->rx_dma, &t->rx_dma_req);
+		if (start_status < 0) {
+			spin_unlock(&t->rx_lock);
+			/* Failure if Dma is not started */
+			BUG();
+			return;
+		}
+		/* enable the rts now */
+		if (t->rts_active)
+			set_rts(t, true);
+	}  else {
+		is_dma_stopped = false;
+		dma_trans_count = tegra_dma_get_transfer_count(t->rx_dma,
+					&t->rx_dma_req, false);
+		if (dma_trans_count < 0) {
+			/* Failure if dma is not running and reached here */
+			spin_unlock(&t->rx_lock);
+			BUG();
+			return;
+		}
+		dma_read_count = copy_dma_buffer_to_tty_buffer(t, dma_trans_count);
 	}
 
-	if (req->status == -TEGRA_DMA_REQ_ERROR_ABOTRED) {
-		do_handle_rx_pio(u);
+	if (is_break_detected){
+		tty_insert_flip_char(u->info->port.tty, 0, TTY_BREAK);
+		t->uport.icount.rx++;
 	}
+	spin_unlock(&t->rx_lock);
 
-	spin_unlock(&u->lock);
-	tty_flip_buffer_push(u->info->port.tty);
-	spin_lock(&u->lock);
+	if (dma_read_count || pio_read_count) {
+		tty_flip_buffer_push(u->info->port.tty);
+	}
 
+	spin_lock_irqsave(&t->reg_access_lock, irq_flags);
+	/* Enable the interrupts */
+	ier = t->ier_shadow;
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RXS, ENABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RX_TIMEOUT, ENABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_EORD, ENABLE, ier);
+	t->ier_shadow = ier;
+	writeb(ier, t->regs + UART_IER_DLAB_0_0);
+	spin_unlock_irqrestore(&t->reg_access_lock, irq_flags);
 }
 
-/* Lock already taken */
-static void do_handle_rx_dma(struct uart_port *u)
+static void tegra_stop_rx(struct uart_port *u)
 {
 	struct tegra_uart_port *t;
+	unsigned long irq_flags;
+	unsigned char ier;
 
 	t = container_of(u, struct tegra_uart_port, uport);
+
+	spin_lock_irqsave(&t->reg_access_lock, irq_flags);
 	if (t->rts_active)
 		set_rts(t, false);
-	tegra_dma_dequeue(t->rx_dma);
 
-	/* enqueue the request again */
-	tegra_dma_enqueue_req(t->rx_dma, &t->rx_dma_req);
+	t->rx_state = UART_STOPPED;
 
-	if (t->rts_active)
-		set_rts(t, true);
-}
+	/* Enable the interrupts */
+	ier = t->ier_shadow;
 
-static char do_decode_rx_error(struct uart_port *u)
-{
-	struct tegra_uart_port *t;
-	char flag = TTY_NORMAL;
-	unsigned char lsr;
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RHR, ENABLE, ier);
+	writeb(ier, t->regs + UART_IER_DLAB_0_0);
 
-	t = container_of(u, struct tegra_uart_port, uport);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RHR, DISABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RXS, DISABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_RX_TIMEOUT, DISABLE, ier);
+	ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_EORD, DISABLE, ier);
+	t->ier_shadow = ier;
+	writeb(ier, t->regs + UART_IER_DLAB_0_0);
 
-	lsr = readb(t->regs + UART_LSR_0);
-	if (lsr & NV_DRF_DEF(UART, LSR, OVRF, OVERRUN_ERROR)) {
-		/* Overrrun error  */
-		flag |= TTY_OVERRUN;
-		t->uport.icount.overrun++;
-		dev_err(u->dev, "Got overrun errors\n");
-	} else if (lsr & NV_DRF_DEF(UART, LSR, PERR, PARITY_ERR)) {
-		/* Parity error */
-		flag |= TTY_PARITY;
-		t->uport.icount.parity++;
-		dev_err(u->dev, "Got Parity errors\n");
-	} else if (lsr & NV_DRF_DEF(UART, LSR, FERR, FRAME_ERR)) {
-		flag |= TTY_FRAME;
-		dev_err(u->dev, "Got frame errors\n");
-	} else if (lsr & NV_DRF_DEF(UART, LSR, BRK, BREAK)) {
-		dev_err(u->dev, "Got Break \n");
-		/* If FIFO read error without any data, reset Rx FIFO */
-		if (!(lsr & 0x1) && (lsr & 0x80)) {
-			unsigned char fcr = t->fcr_shadow;
-			fcr |= 0x2;
-			writeb(fcr, t->regs + UART_IIR_FCR_0);
-		}
-	}
-	return flag;
+	spin_unlock_irqrestore(&t->reg_access_lock, irq_flags);
+
+	tegra_dma_dequeue(t->rx_dma);
+	return;
 }
 
-static void do_handle_rx_pio(struct uart_port *u)
+static void do_handle_tx_pio(struct uart_port *u)
 {
 	struct tegra_uart_port *t;
-	struct tty_struct *tty = u->info->port.tty;
+	struct circ_buf *xmit = &u->info->xmit;
+	int count = 0;
+	unsigned long irq_flags;
 
+	function_trace(1);
 	t = container_of(u, struct tegra_uart_port, uport);
 
-	do {
-		char flag = TTY_NORMAL;
-		unsigned char lsr;
-		unsigned char ch;
-
-		flag =  do_decode_rx_error(u);
+	if (t->tx_state == UART_STOPPED) {
+		spin_lock_irqsave(&t->reg_access_lock, irq_flags);
+		/* Disable interrupts on FIFO empty and break */
+		t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
+			IE_THR, DISABLE, t->ier_shadow);
+		writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
+		t->tx_pio_inflight = false;
+		spin_unlock_irqrestore(&t->reg_access_lock, irq_flags);
+		return;
+	}
 
-		lsr = readb(t->regs + UART_LSR_0);
-		if (!(lsr & NV_DRF_DEF(UART, LSR, RDR, DATA_IN_FIFO)))
+	/* As long as there is room in the FIFO write the buffer without
+	 * polling fifo status register */
+	while (count < t->tx_low_watermark) {
+		if (uart_circ_empty(xmit)) {
+			spin_lock_irqsave(&t->reg_access_lock, irq_flags);
+			/* Disable interrupts on FIFO empty and break */
+			t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
+				IE_THR, DISABLE, t->ier_shadow);
+			writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
+			t->tx_pio_inflight = false;
+			t->tx_state = UART_STOPPED;
+			spin_unlock_irqrestore(&t->reg_access_lock, irq_flags);
 			break;
+		}
 
-		ch = readb(t->regs + UART_THR_DLAB_0_0);
-		dev_vdbg(u->dev, "%c\n", ch);
-		if (!uart_handle_sysrq_char(u, c)) {
-			tty_insert_flip_char(tty, ch, flag);
-			t->uport.icount.rx ++;
+		writeb(xmit->buf[xmit->tail], t->regs + UART_THR_DLAB_0_0);
+		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+		u->icount.tx++;
+		count++;
+	}
+
+	/* Efficient way: If pending characters are more than thresold size then
+	 * schedule the work queue to use the dma */
+	if (t->use_tx_dma) {
+		if (uart_circ_chars_pending(xmit) > USE_DMA_TX_THRESOLD) {
+			spin_lock_irqsave(&t->reg_access_lock, irq_flags);
+			/* Disable interrupts on FIFO empty and break */
+			t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
+				IE_THR, DISABLE, t->ier_shadow);
+			writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
+			t->tx_pio_inflight = false;
+			t->tx_state = UART_RUNNING;
+			spin_unlock_irqrestore(&t->reg_access_lock, irq_flags);
+			queue_work(t->tx_work_queue, &t->tx_work);
+			return;
 		}
-	} while (1);
+	}
 
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(u);
 	return;
 }
 
+void tegra_tx_dma_complete_callback(struct tegra_dma_req *req, int err)
+{
+	struct uart_port *u = (struct uart_port *)req->data;
+	struct tegra_uart_port *t;
+	if (err == -TEGRA_DMA_REQ_ERROR_ABOTRED)
+		return;
+
+	t  = container_of(u, struct tegra_uart_port, uport);
+	queue_work(t->tx_work_queue, &t->tx_work);
+}
 
 static void tegra_tx_dma_workqueue(struct work_struct *w)
 {
@@ -286,27 +569,30 @@ static void tegra_tx_dma_workqueue(struct work_struct *w)
 	struct tegra_uart_port *t;
 	struct circ_buf *xmit;
 	int to_end;
-	unsigned long flags;
+	unsigned long irq_flags;
 	int pending_chars;
-	int fifo_timeout;
 
-	t = container_of(w, struct tegra_uart_port, work);
+	t = container_of(w, struct tegra_uart_port, tx_work);
 	u = &t->uport;
 	xmit = &u->info->xmit;
 
-	spin_lock_irqsave(&u->lock, flags);
+	function_trace(1);
+	spin_lock(&t->tx_lock);
+	if (t->tx_state == UART_STOPPED) {
+		spin_unlock(&t->tx_lock);
+		return;
 
-	t = container_of(u, struct tegra_uart_port, uport);
+	}
 
 	/* DMA request is already queued, just return */
 	if (tegra_dma_is_req_inflight(t->tx_dma, &t->tx_dma_req)) {
-		spin_unlock_irqrestore(&u->lock, flags);
+		spin_unlock(&t->tx_lock);
 		return;
 	}
 
 	/* PIO is in flight. Just return */
 	if (t->tx_pio_inflight == true) {
-		spin_unlock_irqrestore(&u->lock, flags);
+		spin_unlock(&t->tx_lock);
 		return;
 	}
 
@@ -317,18 +603,19 @@ static void tegra_tx_dma_workqueue(struct work_struct *w)
 	t->tx_dma_req.size = 0;
 
 	if (uart_circ_empty(xmit)) {
-		spin_unlock_irqrestore(&u->lock, flags);
 		uart_write_wakeup(u);
+		t->tx_state = UART_STOPPED;
+		spin_unlock(&t->tx_lock);
 		return;
 	}
 
 	pending_chars = uart_circ_chars_pending(xmit);
-	if (pending_chars > u->fifosize) {
+	if (pending_chars > USE_DMA_TX_THRESOLD) {
 		int data_size_to_dma = (pending_chars < t->tx_dma_size)?
 						pending_chars : t->tx_dma_size;
 		data_size_to_dma &= ~3;
 		dev_dbg(u->dev, "Tx DMA starting 0x%x size %d\n",
-			xmit->tail, data_size_to_dma);
+						xmit->tail, data_size_to_dma);
 
 		to_end = CIRC_CNT_TO_END(xmit->head, xmit->tail,
 							UART_XMIT_SIZE);
@@ -351,83 +638,152 @@ static void tegra_tx_dma_workqueue(struct work_struct *w)
 		t->tx_pio_inflight = false;
 
 		tegra_dma_enqueue_req(t->tx_dma, &t->tx_dma_req);
-		spin_unlock_irqrestore(&u->lock, flags);
 	} else {
 		/* Transfer in PIO mode */
 		t->fcr_shadow = NV_FLD_SET_DRF_DEF(UART, IIR_FCR, TX_TRIG,
 			FIFO_COUNT_GREATER_8, t->fcr_shadow);
 		writeb(t->fcr_shadow, t->regs + UART_IIR_FCR_0);
 
+		spin_lock_irqsave(&t->reg_access_lock, irq_flags);
 		if (!t->tx_pio_inflight) {
 			t->tx_pio_inflight = true;
 			t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
 				IE_THR, ENABLE, t->ier_shadow);
 			writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
 		}
-		spin_unlock_irqrestore(&u->lock, flags);
+		spin_unlock_irqrestore(&t->reg_access_lock, irq_flags);
 	}
+	spin_unlock(&t->tx_lock);
 	return;
 }
 
-static void do_handle_modem_signal(struct uart_port *u)
+static unsigned int tegra_tx_empty(struct uart_port *u)
 {
-	unsigned char msr;
 	struct tegra_uart_port *t;
+	unsigned char lsr;
+	unsigned int ret;
 
 	t = container_of(u, struct tegra_uart_port, uport);
-	msr = readb(t->regs + UART_MSR_0);
-	if (msr & (NV_DRF_DEF(UART, MSR, CTS, ENABLE))) {
-		dev_dbg(u->dev, "CTS triggered\n");
+	dev_vdbg(u->dev, "+tegra_tx_empty\n");
+
+	if (t->use_tx_dma) {
+		if (tegra_dma_is_req_inflight(t->tx_dma, &t->tx_dma_req))
+			return 0;
 	}
-	if (msr & (NV_DRF_DEF(UART, MSR, DSR, ENABLE))) {
-		dev_dbg(u->dev, "DSR enabled\n");
+	lsr = readb(t->regs + UART_LSR_0);
+	if ((lsr & TX_EMPTY_STATUS) == TX_EMPTY_STATUS)
+		ret = TIOCSER_TEMT;
+	else
+		ret = 0;
+	dev_vdbg(u->dev, "-tegra_tx_empty\n");
+	return 0;
+}
+
+static void tegra_stop_tx(struct uart_port *u)
+{
+	struct tegra_uart_port *t;
+	unsigned long irq_flag;
+	t = container_of(u, struct tegra_uart_port, uport);
+
+	function_trace(1);
+	spin_lock(&t->tx_lock);
+	if (t->tx_state == UART_STOPPED) {
+		//printk(KERN_ERR "%s(): already stopped \n", __func__);
+		spin_unlock(&t->tx_lock);
+		return;
 	}
-	if (msr & (NV_DRF_DEF(UART, MSR, CD, ENABLE))) {
-		dev_dbg(u->dev, "CD enabled\n");
+
+	//printk(KERN_ERR "%s(): stopped \n", __func__);
+	t->tx_state = UART_STOPPED;
+	if (t->use_tx_dma) {
+		tegra_dma_dequeue_req(t->tx_dma, &t->tx_dma_req);
+		t->tx_dma_req.size = 0;
 	}
-	if (msr & (NV_DRF_DEF(UART, MSR, RI, ENABLE))) {
-		dev_dbg(u->dev, "RI enabled\n");
+	spin_unlock(&t->tx_lock);
+
+	/* Disable Tx interrupt for the non dma mode transmit */
+	spin_lock_irqsave(&t->reg_access_lock, irq_flag);
+	if (t->tx_pio_inflight) {
+		t->tx_pio_inflight = false;
+		t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
+			IE_THR, DISABLE, t->ier_shadow);
+		writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
 	}
+	spin_unlock_irqrestore(&t->reg_access_lock, irq_flag);
 	return;
 }
-static void do_handle_tx_pio(struct uart_port *u)
+
+static void tegra_start_tx_locked(struct uart_port *u)
 {
 	struct tegra_uart_port *t;
-	struct circ_buf *xmit = &u->info->xmit;
-	int count = 0;
-
+	unsigned long irq_flag;
+	function_trace(1);
 	t = container_of(u, struct tegra_uart_port, uport);
-	/* As long as there is room in the FIFO write the buffer without
-	 * polling fifo status register */
-	while (count < t->tx_low_watermark) {
-		if (uart_circ_empty(xmit)) {
-			/* Disable interrupts on FIFO empty and break */
+
+	dev_vdbg(t->uport.dev, "+tegra_start_tx_locked\n");
+
+	spin_lock(&t->tx_lock);
+	if (t->tx_state == UART_RUNNING) {
+		spin_unlock(&t->tx_lock);
+		//printk(KERN_ERR "%s(): Already running \n", __func__);
+		return;
+	}
+		//printk(KERN_ERR "%s(): started \n", __func__);
+	t->tx_state = UART_RUNNING;
+	spin_unlock(&t->tx_lock);
+
+	/* Enable Tx interrupt for the non dma mode transmit other schedule
+	 * tx work queue*/
+	if (!t->use_tx_dma) {
+		/* Enable interrupt on transmit FIFO empty, if it is disabled */
+		spin_lock_irqsave(&t->reg_access_lock, irq_flag);
+		if (!t->tx_pio_inflight) {
+			t->tx_pio_inflight = true;
 			t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
-				IE_THR, DISABLE, t->ier_shadow);
+				IE_THR, ENABLE, t->ier_shadow);
 			writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
-			t->tx_pio_inflight = false;
-			break;
 		}
-
-		writeb(xmit->buf[xmit->tail], t->regs + UART_THR_DLAB_0_0);
-		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
-		u->icount.tx++;
-		count++;
+		spin_unlock_irqrestore(&t->reg_access_lock, irq_flag);
+	} else {
+		queue_work(t->tx_work_queue, &t->tx_work);
 	}
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
-			uart_write_wakeup(u);
-	return;
+	dev_vdbg(t->uport.dev, "-tegra_start_tx_locked\n");
 }
 
-void tegra_tx_dma_complete_callback(struct tegra_dma_req *req, int err)
-{
-	struct uart_port *u = (struct uart_port *)req->data;
-	struct tegra_uart_port *t;
-	if (err == -TEGRA_DMA_REQ_ERROR_ABOTRED)
-		return;
 
-	t  = container_of(u, struct tegra_uart_port, uport);
-	queue_work(t->work_queue, &t->work);
+static void do_handle_modem_signal(struct uart_port *u, unsigned char msr)
+{
+	bool is_change = false;
+
+	if (msr & (NV_DRF_DEF(UART, MSR, DCTS, ENABLE))) {
+		/* status = (msr & NV_DRF_DEF(UART, MSR, CTS, ENABLE))?TIOCM_CTS:0;
+		 uart_handle_cts_change(u, status);*/
+		u->icount.cts++;
+		is_change = true;
+		dev_dbg(u->dev, "CTS changed\n");
+	}
+	if (msr & (NV_DRF_DEF(UART, MSR, DDSR, ENABLE))) {
+		u->icount.dsr++;
+		is_change = true;
+		dev_dbg(u->dev, "DSR changed\n");
+	}
+	if (msr & (NV_DRF_DEF(UART, MSR, DCD, ENABLE))) {
+		/*
+		status = (msr & NV_DRF_DEF(UART, MSR, CD, ENABLE))?TIOCM_CD:0;
+		uart_handle_dcd_change(u, status);
+		*/
+		u->icount.dcd++;
+		is_change = true;
+		dev_dbg(u->dev, "CD changed\n");
+	}
+	if (msr & (NV_DRF_DEF(UART, MSR, DRI, ENABLE))) {
+		 u->icount.rng++;
+		 is_change = true;
+		dev_dbg(u->dev, "RI changed\n");
+	}
+	if (is_change)
+		wake_up_interruptible(&u->info->delta_msr_wait);
+	return;
 }
 
 static irqreturn_t tegra_uart_isr(int irq, void *data)
@@ -435,49 +791,33 @@ static irqreturn_t tegra_uart_isr(int irq, void *data)
 	struct uart_port *u = (struct uart_port *)data;
 	struct tegra_uart_port *t;
 	unsigned char iir_fcr;
-	unsigned char ier;
-	unsigned long flags;
+	unsigned char msr;
 
-	spin_lock_irqsave(&u->lock, flags);
 	t  = container_of(u, struct tegra_uart_port, uport);
 	/* FIXME why do we need to loop here? */
 	while (1) {
 		iir_fcr = readb(t->regs + UART_IIR_FCR_0);
 		if (iir_fcr & NV_DRF_DEF(UART, IIR_FCR, IS_STA, NO_INTR_PEND)) {
-			spin_unlock_irqrestore(&u->lock, flags);
 			return IRQ_HANDLED;
 		}
 
 		// dev_vdbg(u->dev, "tegra_uart_isr iir = 0x%x\n", iir_fcr);
 		switch ((iir_fcr >> 1 ) & 0x7) {
 		case 0: /* Modem signal change interrupt */
-			do_handle_modem_signal(u);
+			msr = readb(t->regs + UART_MSR_0);
+			do_handle_modem_signal(u, msr);
 			break;
 		case 1: /* Transmit interrupt only triggered when using PIO */
 			do_handle_tx_pio(u);
 			break;
 		case 4: /* End of data */
-			/* As per hw spec, to clear EORD interrupt, we need
-			 * to disable and then re-enable the interrupt.
-			 */
-			ier = t->ier_shadow;
-			ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_EORD,
-				DISABLE, ier);
-			writeb(ier, t->regs + UART_IER_DLAB_0_0);
-			ier = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0, IE_EORD,
-				ENABLE, ier);
-			writeb(ier, t->regs + UART_IER_DLAB_0_0);
-			/* fallthrough */
 		case 2: /* Receive */
 		case 6: /* Rx timeout */
 			if (likely(t->use_rx_dma)) {
 				do_handle_rx_dma(u);
 			} else {
 				do_handle_rx_pio(u);
-
-				spin_unlock_irqrestore(&u->lock, flags);
 				tty_flip_buffer_push(u->info->port.tty);
-				spin_lock_irqsave(&u->lock, flags);
 			}
 			break;
 		case 3: /* Receive error */
@@ -491,26 +831,19 @@ static irqreturn_t tegra_uart_isr(int irq, void *data)
 	}
 }
 
-static void tegra_stop_rx(struct uart_port *u)
-{
-	struct tegra_uart_port *t;
-
-	t = container_of(u, struct tegra_uart_port, uport);
-
-	if (t->rts_active)
-		set_rts(t, false);
-	tegra_dma_dequeue(t->rx_dma);
-
-	return;
-}
-
 static void tegra_uart_hw_deinit(struct tegra_uart_port *t)
 {
 	unsigned char fcr;
+	unsigned long flag;
+
+	spin_lock_irqsave(&t->reg_access_lock, flag);
 
 	/* Disable interrupts */
 	writeb(0, t->regs + UART_IER_DLAB_0_0);
 
+	t->tx_state = UART_STOPPED;
+	t->rx_state = UART_STOPPED;
+
 	/* Reset the Rx and Tx FIFOs */
 	fcr = t->fcr_shadow;
 	fcr = NV_FLD_SET_DRF_DEF(UART, IIR_FCR, TX_CLR, CLEAR, fcr);
@@ -519,6 +852,8 @@ static void tegra_uart_hw_deinit(struct tegra_uart_port *t)
 
 	NvRmModuleReset(s_hRmGlobal, t->modid);
 	clk_disable(t->clk);
+	spin_unlock_irqrestore(&t->reg_access_lock, flag);
+
 	NvRmSetModuleTristate(s_hRmGlobal, t->modid, NV_TRUE);
 	t->baud = 0;
 }
@@ -546,6 +881,11 @@ static int tegra_uart_hw_init(struct tegra_uart_port *t)
 	clk_enable(t->clk);
 	NvRmModuleReset(s_hRmGlobal, t->modid);
 
+	spin_lock(&t->reg_access_lock);
+
+	t->tx_state = UART_STOPPED;
+	t->rx_state = UART_STOPPED;
+
 	/* Reset the FIFO twice with some delay to make sure that the FIFOs are
 	 * really flushed. Wait is needed as the clearing needs to cross
 	 * multiple clock domains.
@@ -605,6 +945,10 @@ static int tegra_uart_hw_init(struct tegra_uart_port *t)
 		t->fcr_shadow = NV_FLD_SET_DRF_DEF(UART, IIR_FCR, DMA, CHANGE,
 			t->fcr_shadow);
 		writeb(t->fcr_shadow, t->regs + UART_IIR_FCR_0);
+
+		t->last_read_index = 0;
+		t->already_read_bytecount = 0;
+		t->last_transfer_count = 0;
 		if (tegra_dma_enqueue_req(t->rx_dma, &t->rx_dma_req)) {
 			dev_err(t->uport.dev, "Could not enqueue Rx DMA req\n");
 			tegra_dma_free_channel(t->rx_dma);
@@ -624,6 +968,8 @@ static int tegra_uart_hw_init(struct tegra_uart_port *t)
 		writeb(t->fcr_shadow, t->regs + UART_IIR_FCR_0);
 	}
 
+	t->rx_state = UART_RUNNING;
+
 	/*
 	 *  Enable IE_RXS for the receive status interrupts like line errros.
 	 *  Enable IE_RX_TIMEOUT to get the bytes which cannot be DMA'd.
@@ -654,6 +1000,8 @@ static int tegra_uart_hw_init(struct tegra_uart_port *t)
 	t->ier_shadow = ier;
 	writeb(ier, t->regs + UART_IER_DLAB_0_0);
 
+	spin_unlock(&t->reg_access_lock);
+
 	dev_vdbg(t->uport.dev, "-tegra_uart_hw_init\n");
 	return 0;
 
@@ -667,7 +1015,8 @@ static int tegra_uart_init_rx_dma(struct tegra_uart_port *t)
 	dma_addr_t rx_dma_phys;
 	void *rx_dma_virt;
 
-	t->rx_dma = tegra_dma_allocate_channel(TEGRA_DMA_MODE_CONTINOUS);
+	t->rx_dma = tegra_dma_allocate_channel(
+					TEGRA_DMA_MODE_CONTINUOUS_SAME_BUFFER);
 	if (t->rx_dma < 0)
 		return -ENODEV;
 
@@ -805,15 +1154,25 @@ static void tegra_shutdown(struct uart_port *u)
 
 static unsigned int tegra_get_mctrl(struct uart_port *u)
 {
-	/* RI - Ring detector is active
-	 * CD/DCD/CAR - Carrier detect is always active. For some reason
-	 *			  linux has different names for carrier detect.
-	 * DSR - Data Set ready is active as the hardware doesn't support it.
-	 *	   Don't know if the linux support this yet?
-	 * CTS - Clear to send. Always set to active, as the hardware handles
-	 *	   CTS automatically.
-	 * */
-	return TIOCM_RI | TIOCM_CD | TIOCM_DSR | TIOCM_CTS;
+	int mctrl = 0;
+	unsigned char msr;
+	struct tegra_uart_port *t;
+
+	t = container_of(u, struct tegra_uart_port, uport);
+	msr = readb(t->regs + UART_MSR_0);
+	if (msr & (NV_DRF_DEF(UART, MSR, CTS, ENABLE))) {
+		mctrl |= TIOCM_CTS;
+	}
+	if (msr & (NV_DRF_DEF(UART, MSR, DSR, ENABLE))) {
+		mctrl |= TIOCM_DSR;
+	}
+	if (msr & (NV_DRF_DEF(UART, MSR, CD, ENABLE))) {
+		mctrl |= TIOCM_CD;
+	}
+	if (msr & (NV_DRF_DEF(UART, MSR, RI, ENABLE))) {
+		mctrl |= TIOCM_RI;
+	}
+	return mctrl;
 }
 
 static void set_rts(struct tegra_uart_port *t, bool active)
@@ -896,61 +1255,6 @@ static void tegra_release_port(struct uart_port *u)
 
 }
 
-static unsigned int tegra_tx_empty(struct uart_port *u)
-{
-	struct tegra_uart_port *t;
-	unsigned char lsr;
-	unsigned int ret;
-
-	t = container_of(u, struct tegra_uart_port, uport);
-	dev_vdbg(u->dev, "+tegra_tx_empty\n");
-
-	if (t->use_tx_dma) {
-		if (tegra_dma_is_req_inflight(t->tx_dma, &t->tx_dma_req))
-			return 0;
-	}
-	lsr = readb(t->regs + UART_LSR_0);
-	if ((lsr & TX_EMPTY_STATUS) == TX_EMPTY_STATUS)
-		ret = TIOCSER_TEMT;
-	else
-		ret = 0;
-	dev_vdbg(u->dev, "-tegra_tx_empty\n");
-	return 0;
-}
-
-static void tegra_stop_tx(struct uart_port *u)
-{
-	struct tegra_uart_port *t;
-	t = container_of(u, struct tegra_uart_port, uport);
-
-	if (t->use_tx_dma) {
-		tegra_dma_dequeue_req(t->tx_dma, &t->tx_dma_req);
-		t->tx_dma_req.size = 0;
-	}
-	return;
-}
-
-static void tegra_start_tx_locked(struct uart_port *u)
-{
-	struct tegra_uart_port *t;
-
-	t = container_of(u, struct tegra_uart_port, uport);
-
-	dev_vdbg(t->uport.dev, "+tegra_start_tx_locked\n");
-
-	if (!t->use_tx_dma) {
-		/* Enable interrupt on transmit FIFO empty, if it is disabled */
-		if (!t->tx_pio_inflight) {
-			t->tx_pio_inflight = true;
-			t->ier_shadow = NV_FLD_SET_DRF_DEF(UART, IER_DLAB_0,
-				IE_THR, ENABLE, t->ier_shadow);
-			writeb(t->ier_shadow, t->regs + UART_IER_DLAB_0_0);
-		}
-	} else {
-		queue_work(t->work_queue, &t->work);
-	}
-	dev_vdbg(t->uport.dev, "-tegra_start_tx_locked\n");
-}
 
 static void tegra_enable_ms(struct uart_port *u)
 {
@@ -1232,7 +1536,8 @@ static int __devexit tegra_uart_remove(struct platform_device *pdev)
 
 	u = &t->uport;
 	uart_remove_one_port(&tegra_uart_driver, u);
-	destroy_workqueue(t->work_queue);
+	destroy_workqueue(t->tx_work_queue);
+	destroy_workqueue(t->rx_work_queue);
 
 	platform_set_drvdata(pdev, NULL);
 
@@ -1276,39 +1581,62 @@ static int __init tegra_uart_probe(struct platform_device *pdev)
 	t->clk = clk_get(&pdev->dev, clk_name);
 	if (!t->clk) {
 		dev_err(&pdev->dev, "Couldn't get the clock bailing out\n");
-		goto fail;
+		ret = -ENODEV;
+		goto clk_fail;
 	}
 
-	if (NvRmSetModuleTristate(s_hRmGlobal,
-		t->modid, NV_FALSE) != NvSuccess) {
+	if (NvRmSetModuleTristate(s_hRmGlobal, t->modid,
+						NV_TRUE) != NvSuccess) {
 		dev_err(u->dev, "No Pin Mux - not registering the port\n");
-		goto fail;
+		ret = -ENODEV;
+		goto clk_fail;
 	}
-	/* Tristate the pins in boot. Drive it only when using the pins */
-	NvRmSetModuleTristate(s_hRmGlobal, t->modid, NV_TRUE);
 
 	ret = uart_add_one_port(&tegra_uart_driver, u);
 	if (ret) {
 		printk(KERN_INFO "%s: Failed(%d) to add uart port %s%d\n",
 			__func__, ret, tegra_uart_driver.dev_name, u->line);
-		kfree(t);
-		platform_set_drvdata(pdev, NULL);
-		return ret;
+		goto clk_fail;
 	}
 
-	snprintf(name, sizeof(name), "tegra_hsuart_%d", u->line);
-	t->work_queue = create_singlethread_workqueue(name);
-	if (t->work_queue == NULL) {
+	/* create the workqueue for the Tx path */
+	snprintf(name, sizeof(name), "tegra_hsuart_tx_%d", u->line);
+	t->tx_work_queue = create_singlethread_workqueue(name);
+	if (t->tx_work_queue == NULL) {
 		dev_err(u->dev, "Failed to create work queue\n");
-		goto fail;
+		ret = -ENODEV;
+		goto tx_workq_fail;
+	}
+	INIT_WORK(&t->tx_work, tegra_tx_dma_workqueue);
+
+	/* Create the workqueue for the Rx Path */
+	snprintf(name, sizeof(name), "tegra_hsuart_rx_%d", u->line);
+	t->rx_work_queue = create_singlethread_workqueue(name);
+	if (t->rx_work_queue == NULL) {
+		dev_err(u->dev, "Failed to create work queue\n");
+		ret = -ENODEV;
+		goto rx_workq_fail;
 	}
-	INIT_WORK(&t->work, tegra_tx_dma_workqueue);
+	INIT_WORK(&t->rx_work, tegra_rx_dma_workqueue);
+
+
+	spin_lock_init(&t->tx_lock);
+	spin_lock_init(&t->rx_lock);
+	spin_lock_init(&t->reg_access_lock);
+
 	printk(KERN_INFO "Registered UART port %s%d\n",
-		tegra_uart_driver.dev_name, u->line);
+					tegra_uart_driver.dev_name, u->line);
+
 	return ret;
-fail:
+
+rx_workq_fail:
+	destroy_workqueue(t->tx_work_queue);
+tx_workq_fail:
+	uart_remove_one_port(&tegra_uart_driver, u);
+clk_fail:
 	kfree(t);
-	return -ENODEV;
+	platform_set_drvdata(pdev, NULL);
+	return ret;
 }
 
 static int __init tegra_uart_init(void)