diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index a5781f6db269..9d0955289796 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -3432,9 +3432,6 @@ static int __init diagchar_init(void)
 	if (ret)
 		goto fail;
 	ret = diagfwd_init();
-	if (ret)
-		goto fail;
-	ret = diagfwd_bridge_init();
 	if (ret)
 		goto fail;
 	ret = diagfwd_cntl_init();
@@ -3467,6 +3464,9 @@ static int __init diagchar_init(void)
 		goto fail;
 
 	pr_debug("diagchar initialized now");
+	ret = diagfwd_bridge_init();
+	if (ret)
+		diagfwd_bridge_exit();
 	return 0;
 
 fail:
@@ -3482,6 +3482,7 @@ fail:
 	diag_masks_exit();
 	diag_remote_exit();
 	return -1;
+
 }
 
 static void diagchar_exit(void)
diff --git a/drivers/char/diag/diagfwd_glink.c b/drivers/char/diag/diagfwd_glink.c
index a2ffabe43c86..ce523ac35a51 100644
--- a/drivers/char/diag/diagfwd_glink.c
+++ b/drivers/char/diag/diagfwd_glink.c
@@ -455,6 +455,8 @@ static void diag_glink_transport_notify_state(void *handle, const void *priv,
 			"%s received channel remote disconnect for periph:%d\n",
 			 glink_info->name, glink_info->peripheral);
 		atomic_set(&glink_info->opened, 0);
+		diagfwd_channel_close(glink_info->fwd_ctxt);
+		atomic_set(&glink_info->tx_intent_ready, 0);
 		break;
 	default:
 		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
@@ -501,6 +503,7 @@ static void diag_glink_close_work_fn(struct work_struct *work)
 
 	glink_close(glink_info->hdl);
 	atomic_set(&glink_info->opened, 0);
+	atomic_set(&glink_info->tx_intent_ready, 0);
 	glink_info->hdl = NULL;
 	diagfwd_channel_close(glink_info->fwd_ctxt);
 }
diff --git a/drivers/char/diag/diagfwd_mhi.c b/drivers/char/diag/diagfwd_mhi.c
index f7b1e98f22b0..df26e2522baf 100644
--- a/drivers/char/diag/diagfwd_mhi.c
+++ b/drivers/char/diag/diagfwd_mhi.c
@@ -49,6 +49,7 @@ struct diag_mhi_info diag_mhi[NUM_MHI_DEV] = {
 		.enabled = 0,
 		.num_read = 0,
 		.mempool = POOL_TYPE_MDM,
+		.mempool_init = 0,
 		.mhi_wq = NULL,
 		.read_ch = {
 			.chan = MHI_CLIENT_DIAG_IN,
@@ -68,6 +69,7 @@ struct diag_mhi_info diag_mhi[NUM_MHI_DEV] = {
 		.enabled = 0,
 		.num_read = 0,
 		.mempool = POOL_TYPE_MDM_DCI,
+		.mempool_init = 0,
 		.mhi_wq = NULL,
 		.read_ch = {
 			.chan = MHI_CLIENT_DCI_IN,
@@ -684,6 +686,7 @@ int diag_mhi_init()
 		strlcpy(wq_name, "diag_mhi_", DIAG_MHI_STRING_SZ);
 		strlcat(wq_name, mhi_info->name, sizeof(mhi_info->name));
 		diagmem_init(driver, mhi_info->mempool);
+		mhi_info->mempool_init = 1;
 		mhi_info->mhi_wq = create_singlethread_workqueue(wq_name);
 		if (!mhi_info->mhi_wq)
 			goto fail;
@@ -725,7 +728,8 @@ void diag_mhi_exit()
 		if (mhi_info->mhi_wq)
 			destroy_workqueue(mhi_info->mhi_wq);
 		mhi_close(mhi_info->id);
-		diagmem_exit(driver, mhi_info->mempool);
+		if (mhi_info->mempool_init)
+			diagmem_exit(driver, mhi_info->mempool);
 	}
 }
 
diff --git a/drivers/char/diag/diagfwd_mhi.h b/drivers/char/diag/diagfwd_mhi.h
index 8332efdf5efb..a4466977ca97 100644
--- a/drivers/char/diag/diagfwd_mhi.h
+++ b/drivers/char/diag/diagfwd_mhi.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -65,6 +65,7 @@ struct diag_mhi_info {
 	int id;
 	int dev_id;
 	int mempool;
+	int mempool_init;
 	int num_read;
 	uint8_t enabled;
 	char name[DIAG_MHI_NAME_SZ];
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 40fdcbaaf31a..c78a5f4fbe74 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -438,6 +438,7 @@ int diagfwd_peripheral_init(void)
 			fwd_info->read_bytes = 0;
 			fwd_info->write_bytes = 0;
 			spin_lock_init(&fwd_info->buf_lock);
+			spin_lock_init(&fwd_info->write_buf_lock);
 			mutex_init(&fwd_info->data_mutex);
 		}
 	}
@@ -453,6 +454,7 @@ int diagfwd_peripheral_init(void)
 			fwd_info->read_bytes = 0;
 			fwd_info->write_bytes = 0;
 			spin_lock_init(&fwd_info->buf_lock);
+			spin_lock_init(&fwd_info->write_buf_lock);
 			mutex_init(&fwd_info->data_mutex);
 			/*
 			 * This state shouldn't be set for Control channels
@@ -686,16 +688,19 @@ void *diagfwd_request_write_buf(struct diagfwd_info *fwd_info)
 {
 	void *buf = NULL;
 	int index;
+	unsigned long flags;
 
+	spin_lock_irqsave(&fwd_info->write_buf_lock, flags);
 	for (index = 0 ; index < NUM_WRITE_BUFFERS; index++) {
 		if (!atomic_read(&(fwd_info->buf_ptr[index]->in_busy))) {
+			atomic_set(&(fwd_info->buf_ptr[index]->in_busy), 1);
 			buf = fwd_info->buf_ptr[index]->data;
 			if (!buf)
 				return NULL;
-			atomic_set(&(fwd_info->buf_ptr[index]->in_busy), 1);
 			break;
 		}
 	}
+	spin_unlock_irqrestore(&fwd_info->write_buf_lock, flags);
 	return buf;
 }
 
@@ -760,7 +765,6 @@ int diagfwd_write(uint8_t peripheral, uint8_t type, void *buf, int len)
 
 static void __diag_fwd_open(struct diagfwd_info *fwd_info)
 {
-	int i;
 	if (!fwd_info)
 		return;
 
@@ -775,10 +779,7 @@ static void __diag_fwd_open(struct diagfwd_info *fwd_info)
 
 	if (fwd_info->p_ops && fwd_info->p_ops->open)
 		fwd_info->p_ops->open(fwd_info->ctxt);
-	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
-		if (fwd_info->buf_ptr[i])
-			atomic_set(&fwd_info->buf_ptr[i]->in_busy, 0);
-	}
+
 	diagfwd_queue_read(fwd_info);
 }
 
@@ -839,6 +840,7 @@ void diagfwd_close(uint8_t peripheral, uint8_t type)
 
 int diagfwd_channel_open(struct diagfwd_info *fwd_info)
 {
+	int i;
 	if (!fwd_info)
 		return -EIO;
 
@@ -859,6 +861,10 @@ int diagfwd_channel_open(struct diagfwd_info *fwd_info)
 	diagfwd_write_buffers_init(fwd_info);
 	if (fwd_info && fwd_info->c_ops && fwd_info->c_ops->open)
 		fwd_info->c_ops->open(fwd_info);
+	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
+		if (fwd_info->buf_ptr[i])
+			atomic_set(&fwd_info->buf_ptr[i]->in_busy, 0);
+	}
 	diagfwd_queue_read(fwd_info);
 	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "p: %d t: %d considered opened\n",
 		 fwd_info->peripheral, fwd_info->type);
@@ -873,6 +879,7 @@ int diagfwd_channel_open(struct diagfwd_info *fwd_info)
 
 int diagfwd_channel_close(struct diagfwd_info *fwd_info)
 {
+	int i;
 	if (!fwd_info)
 		return -EIO;
 
@@ -885,6 +892,10 @@ int diagfwd_channel_close(struct diagfwd_info *fwd_info)
 	if (fwd_info->buf_2 && fwd_info->buf_2->data)
 		atomic_set(&fwd_info->buf_2->in_busy, 0);
 
+	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
+		if (fwd_info->buf_ptr[i])
+			atomic_set(&fwd_info->buf_ptr[i]->in_busy, 1);
+	}
 	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "p: %d t: %d considered closed\n",
 		 fwd_info->peripheral, fwd_info->type);
 
@@ -940,10 +951,11 @@ int diagfwd_write_buffer_done(struct diagfwd_info *fwd_info, const void *ptr)
 
 	int found = 0;
 	int index = 0;
+	unsigned long flags;
 
 	if (!fwd_info || !ptr)
 		return found;
-
+	spin_lock_irqsave(&fwd_info->write_buf_lock, flags);
 	for (index = 0; index < NUM_WRITE_BUFFERS; index++) {
 		if (fwd_info->buf_ptr[index]->data == ptr) {
 			atomic_set(&fwd_info->buf_ptr[index]->in_busy, 0);
@@ -951,6 +963,7 @@ int diagfwd_write_buffer_done(struct diagfwd_info *fwd_info, const void *ptr)
 			break;
 		}
 	}
+	spin_unlock_irqrestore(&fwd_info->write_buf_lock, flags);
 	return found;
 }
 
@@ -1197,7 +1210,7 @@ void diagfwd_write_buffers_init(struct diagfwd_info *fwd_info)
 		return;
 	}
 
-	spin_lock_irqsave(&fwd_info->buf_lock, flags);
+	spin_lock_irqsave(&fwd_info->write_buf_lock, flags);
 	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
 		if (!fwd_info->buf_ptr[i])
 			fwd_info->buf_ptr[i] =
@@ -1215,11 +1228,11 @@ void diagfwd_write_buffers_init(struct diagfwd_info *fwd_info)
 			kmemleak_not_leak(fwd_info->buf_ptr[i]->data);
 		}
 	}
-	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
+	spin_unlock_irqrestore(&fwd_info->write_buf_lock, flags);
 	return;
 
 err:
-	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
+	spin_unlock_irqrestore(&fwd_info->write_buf_lock, flags);
 	pr_err("diag:unable to allocate write buffers\n");
 	diagfwd_write_buffers_exit(fwd_info);
 
@@ -1233,7 +1246,7 @@ static void diagfwd_write_buffers_exit(struct diagfwd_info *fwd_info)
 	if (!fwd_info)
 		return;
 
-	spin_lock_irqsave(&fwd_info->buf_lock, flags);
+	spin_lock_irqsave(&fwd_info->write_buf_lock, flags);
 	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
 		if (fwd_info->buf_ptr[i]) {
 			kfree(fwd_info->buf_ptr[i]->data);
@@ -1242,5 +1255,5 @@ static void diagfwd_write_buffers_exit(struct diagfwd_info *fwd_info)
 			fwd_info->buf_ptr[i] = NULL;
 		}
 	}
-	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
+	spin_unlock_irqrestore(&fwd_info->write_buf_lock, flags);
 }
diff --git a/drivers/char/diag/diagfwd_peripheral.h b/drivers/char/diag/diagfwd_peripheral.h
index b511bf495bc2..cbbab86a9425 100644
--- a/drivers/char/diag/diagfwd_peripheral.h
+++ b/drivers/char/diag/diagfwd_peripheral.h
@@ -71,6 +71,7 @@ struct diagfwd_info {
 	unsigned long read_bytes;
 	unsigned long write_bytes;
 	spinlock_t buf_lock;
+	spinlock_t write_buf_lock;
 	struct mutex data_mutex;
 	void *ctxt;
 	struct diagfwd_buf_t *buf_1;