diff --git a/drivers/char/diag/diag_mux.c b/drivers/char/diag/diag_mux.c
index e016acf4e12e..6586f5e0cf86 100644
--- a/drivers/char/diag/diag_mux.c
+++ b/drivers/char/diag/diag_mux.c
@@ -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
@@ -23,11 +23,13 @@
 #include <linux/spinlock.h>
 #include <linux/ratelimit.h>
 #include "diagchar.h"
+#include "diagfwd.h"
 #include "diag_mux.h"
 #include "diag_usb.h"
 #include "diag_memorydevice.h"
 
-struct diag_logger_t *logger;
+
+struct diag_mux_state_t *diag_mux;
 static struct diag_logger_t usb_logger;
 static struct diag_logger_t md_logger;
 
@@ -49,11 +51,11 @@ static struct diag_logger_ops md_log_ops = {
 
 int diag_mux_init()
 {
-	logger = kzalloc(NUM_MUX_PROC * sizeof(struct diag_logger_t),
+	diag_mux = kzalloc(sizeof(struct diag_mux_state_t),
 			 GFP_KERNEL);
-	if (!logger)
+	if (!diag_mux)
 		return -ENOMEM;
-	kmemleak_not_leak(logger);
+	kmemleak_not_leak(diag_mux);
 
 	usb_logger.mode = DIAG_USB_MODE;
 	usb_logger.log_ops = &usb_log_ops;
@@ -66,13 +68,17 @@ int diag_mux_init()
 	 * Set USB logging as the default logger. This is the mode
 	 * Diag should be in when it initializes.
 	 */
-	logger = &usb_logger;
+	diag_mux->usb_ptr = &usb_logger;
+	diag_mux->md_ptr = &md_logger;
+	diag_mux->logger = &usb_logger;
+	diag_mux->mux_mask = 0;
+	diag_mux->mode = DIAG_USB_MODE;
 	return 0;
 }
 
 void diag_mux_exit()
 {
-	kfree(logger);
+	kfree(diag_mux);
 }
 
 int diag_mux_register(int proc, int ctx, struct diag_mux_ops *ops)
@@ -106,19 +112,43 @@ int diag_mux_register(int proc, int ctx, struct diag_mux_ops *ops)
 
 int diag_mux_queue_read(int proc)
 {
+	struct diag_logger_t *logger = NULL;
+
 	if (proc < 0 || proc >= NUM_MUX_PROC)
 		return -EINVAL;
-	if (!logger)
+	if (!diag_mux)
 		return -EIO;
-	if (logger->log_ops && logger->log_ops->queue_read)
+
+	if (diag_mux->mode == DIAG_MULTI_MODE)
+		logger = diag_mux->usb_ptr;
+	else
+		logger = diag_mux->logger;
+
+	if (logger && logger->log_ops && logger->log_ops->queue_read)
 		return logger->log_ops->queue_read(proc);
+
 	return 0;
 }
 
 int diag_mux_write(int proc, unsigned char *buf, int len, int ctx)
 {
+	struct diag_logger_t *logger = NULL;
+	int peripheral;
+
 	if (proc < 0 || proc >= NUM_MUX_PROC)
 		return -EINVAL;
+	if (!diag_mux)
+		return -EIO;
+
+	peripheral = GET_BUF_PERIPHERAL(ctx);
+	if (peripheral > NUM_PERIPHERALS)
+		return -EINVAL;
+
+	if (MD_PERIPHERAL_MASK(peripheral) & diag_mux->mux_mask)
+		logger = diag_mux->md_ptr;
+	else
+		logger = diag_mux->usb_ptr;
+
 	if (logger && logger->log_ops && logger->log_ops->write)
 		return logger->log_ops->write(proc, buf, len, ctx);
 	return 0;
@@ -126,38 +156,86 @@ int diag_mux_write(int proc, unsigned char *buf, int len, int ctx)
 
 int diag_mux_close_peripheral(int proc, uint8_t peripheral)
 {
+	struct diag_logger_t *logger = NULL;
 	if (proc < 0 || proc >= NUM_MUX_PROC)
 		return -EINVAL;
 	/* Peripheral should account for Apps data as well */
 	if (peripheral > NUM_PERIPHERALS)
 		return -EINVAL;
+	if (!diag_mux)
+		return -EIO;
+
+	if (MD_PERIPHERAL_MASK(peripheral) & diag_mux->mux_mask)
+		logger = diag_mux->md_ptr;
+	else
+		logger = diag_mux->logger;
+
 	if (logger && logger->log_ops && logger->log_ops->close_peripheral)
 		return logger->log_ops->close_peripheral(proc, peripheral);
 	return 0;
 }
 
-int diag_mux_switch_logging(int new_mode)
+int diag_mux_switch_logging(int *req_mode, int *peripheral_mask)
 {
-	struct diag_logger_t *new_logger = NULL;
+	unsigned int new_mask = 0;
 
-	switch (new_mode) {
-	case DIAG_USB_MODE:
-		new_logger = &usb_logger;
-		break;
-	case DIAG_MEMORY_DEVICE_MODE:
-		new_logger = &md_logger;
-		break;
-	default:
-		pr_err("diag: Invalid mode %d in %s\n", new_mode, __func__);
+	if (!req_mode)
+		return -EINVAL;
+
+	if (*peripheral_mask <= 0 || *peripheral_mask > DIAG_CON_ALL) {
+		pr_err("diag: mask %d in %s\n", *peripheral_mask, __func__);
 		return -EINVAL;
 	}
 
-	if (logger) {
-		logger->log_ops->close();
-		logger = new_logger;
-		logger->log_ops->open();
+	switch (*req_mode) {
+	case DIAG_USB_MODE:
+		new_mask = ~(*peripheral_mask) & diag_mux->mux_mask;
+		if (new_mask != DIAG_CON_NONE)
+			*req_mode = DIAG_MULTI_MODE;
+		break;
+	case DIAG_MEMORY_DEVICE_MODE:
+		new_mask = (*peripheral_mask) | diag_mux->mux_mask;
+		if (new_mask != DIAG_CON_ALL)
+			*req_mode = DIAG_MULTI_MODE;
+		break;
+	default:
+		pr_err("diag: Invalid mode %d in %s\n", *req_mode, __func__);
+		return -EINVAL;
 	}
 
+	switch (diag_mux->mode) {
+	case DIAG_USB_MODE:
+		if (*req_mode == DIAG_MEMORY_DEVICE_MODE) {
+			diag_mux->usb_ptr->log_ops->close();
+			diag_mux->logger = diag_mux->md_ptr;
+			diag_mux->md_ptr->log_ops->open();
+		} else if (*req_mode == DIAG_MULTI_MODE) {
+			diag_mux->md_ptr->log_ops->open();
+			diag_mux->logger = NULL;
+		}
+		break;
+	case DIAG_MEMORY_DEVICE_MODE:
+		if (*req_mode == DIAG_USB_MODE) {
+			diag_mux->md_ptr->log_ops->close();
+			diag_mux->logger = diag_mux->usb_ptr;
+			diag_mux->usb_ptr->log_ops->open();
+		} else if (*req_mode == DIAG_MULTI_MODE) {
+			diag_mux->usb_ptr->log_ops->open();
+			diag_mux->logger = NULL;
+		}
+		break;
+	case DIAG_MULTI_MODE:
+		if (*req_mode == DIAG_USB_MODE) {
+			diag_mux->md_ptr->log_ops->close();
+			diag_mux->logger = diag_mux->usb_ptr;
+		} else if (*req_mode == DIAG_MEMORY_DEVICE_MODE) {
+			diag_mux->usb_ptr->log_ops->close();
+			diag_mux->logger = diag_mux->md_ptr;
+		}
+		break;
+	}
+	diag_mux->mode = *req_mode;
+	diag_mux->mux_mask = new_mask;
+	*peripheral_mask = new_mask;
 	return 0;
 }
-
diff --git a/drivers/char/diag/diag_mux.h b/drivers/char/diag/diag_mux.h
index 97632d198494..e1fcebbe6fd1 100644
--- a/drivers/char/diag/diag_mux.h
+++ b/drivers/char/diag/diag_mux.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
@@ -13,6 +13,14 @@
 #define DIAG_MUX_H
 #include "diagchar.h"
 
+struct diag_mux_state_t {
+	struct diag_logger_t *logger;
+	struct diag_logger_t *usb_ptr;
+	struct diag_logger_t *md_ptr;
+	unsigned int mux_mask;
+	unsigned int mode;
+};
+
 struct diag_mux_ops {
 	int (*open)(int id, int mode);
 	int (*close)(int id, int mode);
@@ -24,6 +32,7 @@ struct diag_mux_ops {
 #define DIAG_USB_MODE			0
 #define DIAG_MEMORY_DEVICE_MODE		1
 #define DIAG_NO_LOGGING_MODE		2
+#define DIAG_MULTI_MODE			3
 
 #define DIAG_MUX_LOCAL		0
 #define DIAG_MUX_LOCAL_LAST	1
@@ -53,7 +62,7 @@ struct diag_logger_t {
 	struct diag_logger_ops *log_ops;
 };
 
-extern struct diag_logger_t *logger;
+extern struct diag_mux_state_t *diag_mux;
 
 int diag_mux_init(void);
 void diag_mux_exit(void);
@@ -63,5 +72,5 @@ int diag_mux_write(int proc, unsigned char *buf, int len, int ctx);
 int diag_mux_close_peripheral(int proc, uint8_t peripheral);
 int diag_mux_open_all(struct diag_logger_t *logger);
 int diag_mux_close_all(void);
-int diag_mux_switch_logging(int new_mode);
+int diag_mux_switch_logging(int *new_mode, int *peripheral_mask);
 #endif
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index b021b99c6e9c..5d7b1e7fe757 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2008-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2008-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
@@ -59,11 +59,15 @@
 #define DIAG_CTRL_MSG_F3_MASK	11
 #define CONTROL_CHAR	0x7E
 
-#define DIAG_CON_APSS (0x0001)	/* Bit mask for APSS */
-#define DIAG_CON_MPSS (0x0002)	/* Bit mask for MPSS */
-#define DIAG_CON_LPASS (0x0004)	/* Bit mask for LPASS */
-#define DIAG_CON_WCNSS (0x0008)	/* Bit mask for WCNSS */
-#define DIAG_CON_SENSORS (0x0010) /* Bit mask for Sensors */
+#define DIAG_CON_APSS		(0x0001)	/* Bit mask for APSS */
+#define DIAG_CON_MPSS		(0x0002)	/* Bit mask for MPSS */
+#define DIAG_CON_LPASS		(0x0004)	/* Bit mask for LPASS */
+#define DIAG_CON_WCNSS		(0x0008)	/* Bit mask for WCNSS */
+#define DIAG_CON_SENSORS	(0x0010)	/* Bit mask for Sensors */
+#define DIAG_CON_NONE		(0x0000)	/* Bit mask for No SS*/
+#define DIAG_CON_ALL		(DIAG_CON_APSS | DIAG_CON_MPSS \
+				| DIAG_CON_LPASS | DIAG_CON_WCNSS \
+				| DIAG_CON_SENSORS)
 
 #define DIAG_STM_MODEM	0x01
 #define DIAG_STM_LPASS	0x02
@@ -158,8 +162,7 @@
 #define FEATURE_MASK_LEN	2
 
 #define DIAG_MD_NONE			0
-#define DIAG_MD_NORMAL			1
-#define DIAG_MD_PERIPHERAL		2
+#define DIAG_MD_PERIPHERAL		1
 
 /*
  * The status bit masks when received in a signal handler are to be
@@ -561,6 +564,7 @@ struct diagchar_dev {
 	uint32_t dci_pkt_length;
 	int in_busy_dcipktdata;
 	int logging_mode;
+	int logging_mask;
 	int mask_check;
 	uint32_t md_session_mask;
 	uint8_t md_session_mode;
diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index edc104acb777..ecdbf9f9480e 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -369,6 +369,24 @@ fail:
 	return -ENOMEM;
 }
 
+static uint32_t diag_translate_kernel_to_user_mask(uint32_t peripheral_mask)
+{
+	uint32_t ret = 0;
+
+	if (peripheral_mask & MD_PERIPHERAL_MASK(APPS_DATA))
+		ret |= DIAG_CON_APSS;
+	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_MODEM))
+		ret |= DIAG_CON_MPSS;
+	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_LPASS))
+		ret |= DIAG_CON_LPASS;
+	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_WCNSS))
+		ret |= DIAG_CON_WCNSS;
+	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_SENSORS))
+		ret |= DIAG_CON_SENSORS;
+
+	return ret;
+}
+
 static void diag_close_logging_process(const int pid)
 {
 	int i;
@@ -388,7 +406,8 @@ static void diag_close_logging_process(const int pid)
 
 	params.req_mode = USB_MODE;
 	params.mode_param = 0;
-	params.peripheral_mask = 0;
+	params.peripheral_mask =
+		diag_translate_kernel_to_user_mask(session_peripheral_mask);
 	mutex_lock(&driver->diagchar_mutex);
 	diag_switch_logging(&params);
 	mutex_unlock(&driver->diagchar_mutex);
@@ -417,10 +436,11 @@ static int diag_remove_client_entry(struct file *file)
 
 	diagpriv_data = file->private_data;
 
-	/* clean up any DCI registrations, if this is a DCI client
-	* This will specially help in case of ungraceful exit of any DCI client
-	* This call will remove any pending registrations of such client
-	*/
+	/*
+	 * clean up any DCI registrations, if this is a DCI client
+	 * This will specially help in case of ungraceful exit of any DCI client
+	 * This call will remove any pending registrations of such client
+	 */
 	mutex_lock(&driver->dci_mutex);
 	dci_entry = dci_lookup_client_entry_pid(current->tgid);
 	if (dci_entry)
@@ -1148,19 +1168,10 @@ int diag_md_session_create(int mode, int peripheral_mask, int proc)
 	struct diag_md_session_t *new_session = NULL;
 
 	/*
-	 * If there is any session running in Normal mode
-	 * we cannot start a new  session . If there is a
-	 * session running in Peripheral mode we cannot
-	 * start a new session in  NORMAL mode. If a session is
-	 * running with a peripheral mask and a new session
-	 * request comes in with same peripheral mask value
-	 * then return invalid param
+	 * If a session is running with a peripheral mask and a new session
+	 * request comes in with same peripheral mask value then return
+	 * invalid param
 	 */
-	if (driver->md_session_mode == DIAG_MD_NORMAL)
-		return -EINVAL;
-	if (driver->md_session_mode == DIAG_MD_PERIPHERAL
-	    && mode == DIAG_MD_NORMAL)
-		return -EINVAL;
 	if (driver->md_session_mode == DIAG_MD_PERIPHERAL &&
 	    (driver->md_session_mask & peripheral_mask) != 0)
 		return -EINVAL;
@@ -1176,32 +1187,6 @@ int diag_md_session_create(int mode, int peripheral_mask, int proc)
 	new_session->pid = current->tgid;
 	new_session->task = current;
 
-	if (mode == DIAG_MD_NORMAL) {
-		new_session->log_mask = &log_mask;
-		new_session->event_mask = &event_mask;
-		new_session->msg_mask = &msg_mask;
-		for (i = 0; i < NUM_MD_SESSIONS; i++) {
-			if (driver->md_session_map[i] != NULL) {
-				DIAG_LOG(DIAG_DEBUG_USERSPACE,
-					 "another instance present for %d\n",
-					 i);
-				err = -EEXIST;
-				goto fail_normal;
-			}
-			new_session->peripheral_mask |= MD_PERIPHERAL_MASK(i);
-			driver->md_session_mask |= MD_PERIPHERAL_MASK(i);
-			driver->md_session_map[i] = new_session;
-		}
-		driver->md_session_mode = DIAG_MD_NORMAL;
-		setup_timer(&new_session->hdlc_reset_timer,
-			diag_md_hdlc_reset_timer_func,
-			new_session->pid);
-		DIAG_LOG(DIAG_DEBUG_USERSPACE,
-			 "created session in normal mode\n");
-		mutex_unlock(&driver->md_session_lock);
-		return 0;
-	}
-
 	new_session->log_mask = kzalloc(sizeof(struct diag_mask_info),
 					GFP_KERNEL);
 	if (!new_session->log_mask) {
@@ -1251,10 +1236,11 @@ int diag_md_session_create(int mode, int peripheral_mask, int proc)
 		new_session->peripheral_mask |= MD_PERIPHERAL_MASK(i);
 		driver->md_session_map[i] = new_session;
 		driver->md_session_mask |= MD_PERIPHERAL_MASK(i);
-		setup_timer(&new_session->hdlc_reset_timer,
-			diag_md_hdlc_reset_timer_func,
-			new_session->pid);
 	}
+	setup_timer(&new_session->hdlc_reset_timer,
+		diag_md_hdlc_reset_timer_func,
+		new_session->pid);
+
 	driver->md_session_mode = DIAG_MD_PERIPHERAL;
 	mutex_unlock(&driver->md_session_lock);
 	DIAG_LOG(DIAG_DEBUG_USERSPACE,
@@ -1271,7 +1257,6 @@ fail_peripheral:
 	diag_msg_mask_free(new_session->msg_mask);
 	kfree(new_session->msg_mask);
 	new_session->msg_mask = NULL;
-fail_normal:
 	kfree(new_session);
 	new_session = NULL;
 	mutex_unlock(&driver->md_session_lock);
@@ -1292,19 +1277,17 @@ static void diag_md_session_close(struct diag_md_session_t *session_info)
 			continue;
 		driver->md_session_map[i] = NULL;
 		driver->md_session_mask &= ~session_info->peripheral_mask;
-		if (driver->md_session_mode == DIAG_MD_NORMAL)
-			continue;
-		diag_log_mask_free(session_info->log_mask);
-		kfree(session_info->log_mask);
-		session_info->log_mask = NULL;
-		diag_msg_mask_free(session_info->msg_mask);
-		kfree(session_info->msg_mask);
-		session_info->msg_mask = NULL;
-		diag_event_mask_free(session_info->event_mask);
-		kfree(session_info->event_mask);
-		session_info->event_mask = NULL;
-		del_timer(&session_info->hdlc_reset_timer);
 	}
+	diag_log_mask_free(session_info->log_mask);
+	kfree(session_info->log_mask);
+	session_info->log_mask = NULL;
+	diag_msg_mask_free(session_info->msg_mask);
+	kfree(session_info->msg_mask);
+	session_info->msg_mask = NULL;
+	diag_event_mask_free(session_info->event_mask);
+	kfree(session_info->event_mask);
+	session_info->event_mask = NULL;
+	del_timer(&session_info->hdlc_reset_timer);
 
 	for (i = 0; i < NUM_MD_SESSIONS && !found; i++) {
 		if (driver->md_session_map[i] != NULL)
@@ -1337,113 +1320,157 @@ struct diag_md_session_t *diag_md_session_get_peripheral(uint8_t peripheral)
 	return driver->md_session_map[peripheral];
 }
 
+static int diag_md_peripheral_switch(struct diag_md_session_t *session_info,
+				int peripheral_mask, int req_mode) {
+	int i, bit = 0;
+
+	if (!session_info)
+		return -EINVAL;
+	if (req_mode != DIAG_USB_MODE || req_mode != DIAG_MEMORY_DEVICE_MODE)
+		return -EINVAL;
+
+	/*
+	 * check that md_session_map for i == session_info,
+	 * if not then race condition occurred and bail
+	 */
+	mutex_lock(&driver->md_session_lock);
+	for (i = 0; i < NUM_MD_SESSIONS; i++) {
+		bit = MD_PERIPHERAL_MASK(i) & peripheral_mask;
+		if (!bit)
+			continue;
+		if (req_mode == DIAG_USB_MODE) {
+			if (driver->md_session_map[i] != session_info) {
+				mutex_unlock(&driver->md_session_lock);
+				return -EINVAL;
+			}
+			driver->md_session_map[i] = NULL;
+			driver->md_session_mask &= ~bit;
+			session_info->peripheral_mask &= ~bit;
+
+		} else {
+			if (driver->md_session_map[i] != NULL) {
+				mutex_unlock(&driver->md_session_lock);
+				return -EINVAL;
+			}
+			driver->md_session_map[i] = session_info;
+			driver->md_session_mask |= bit;
+			session_info->peripheral_mask |= bit;
+
+		}
+	}
+
+	driver->md_session_mode = DIAG_MD_PERIPHERAL;
+	mutex_unlock(&driver->md_session_lock);
+	DIAG_LOG(DIAG_DEBUG_USERSPACE, "Changed Peripherals:0x%x to mode:%d\n",
+		peripheral_mask, req_mode);
+}
+
 static int diag_md_session_check(int curr_mode, int req_mode,
 				 const struct diag_logging_mode_param_t *param,
 				 uint8_t *change_mode)
 {
-	int err = 0;
+	int i, bit = 0, err = 0;
+	int change_mask = 0;
 	struct diag_md_session_t *session_info = NULL;
 
 	if (!param || !change_mode)
 		return -EIO;
 
-	*change_mode = 1;
+	*change_mode = 0;
 
 	switch (curr_mode) {
 	case DIAG_USB_MODE:
 	case DIAG_MEMORY_DEVICE_MODE:
+	case DIAG_MULTI_MODE:
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	switch (req_mode) {
-	case DIAG_USB_MODE:
-	case DIAG_MEMORY_DEVICE_MODE:
-		break;
-	default:
+	if (req_mode != DIAG_USB_MODE && req_mode != DIAG_MEMORY_DEVICE_MODE)
 		return -EINVAL;
-	}
 
-	if (curr_mode == DIAG_USB_MODE) {
-		if (req_mode == DIAG_USB_MODE) {
-			/*
-			 * This case tries to change from USB mode to USB mode.
-			 * There is no change required. Return success.
-			 */
-			*change_mode = 0;
+	if (req_mode == DIAG_USB_MODE) {
+		if (curr_mode == DIAG_USB_MODE)
+			return 0;
+		if (driver->md_session_mode == DIAG_MD_NONE
+		    && driver->md_session_mask == 0 && driver->logging_mask) {
+			*change_mode = 1;
 			return 0;
 		}
 
 		/*
-		 * If there is no other mdlog process, return success.
-		 * Check if the peripheral interested in is active.
+		 * curr_mode is either DIAG_MULTI_MODE or DIAG_MD_MODE
+		 * Check if requested peripherals are already in usb mode
 		 */
-		if (param->mode_param == DIAG_MD_NORMAL) {
-			err = diag_md_session_create(DIAG_MD_NORMAL, 0,
-						     DIAG_LOCAL_PROC);
-			return err;
-		} else if (param->mode_param == DIAG_MD_PERIPHERAL &&
-			   (!(driver->md_session_mask &
-			      param->peripheral_mask))) {
-			err = diag_md_session_create(DIAG_MD_PERIPHERAL,
-						     param->peripheral_mask,
-						     DIAG_LOCAL_PROC);
-			return err;
+		for (i = 0; i < NUM_MD_SESSIONS; i++) {
+			bit = MD_PERIPHERAL_MASK(i) & param->peripheral_mask;
+			if (!bit)
+				continue;
+			if (bit & driver->logging_mask)
+				change_mask |= bit;
 		}
-		DIAG_LOG(DIAG_DEBUG_USERSPACE,
-			 "an instance of mdlog is active\n");
-		*change_mode = 0;
-		return -EINVAL;
-	} else if (curr_mode == DIAG_MEMORY_DEVICE_MODE) {
-		if (req_mode == DIAG_USB_MODE) {
-			if (driver->md_session_mask != 0 &&
-				driver->md_session_mode == DIAG_MD_PERIPHERAL) {
-				/*
-				 * An instance of mdlog is still running, Return
-				 * error.
-				 */
-				DIAG_LOG(DIAG_DEBUG_USERSPACE,
-					 "another instance running\n");
-				*change_mode = 0;
-				return -EINVAL;
-			}
-			session_info = diag_md_session_get_pid(current->tgid);
-			diag_md_session_close(session_info);
+		if (!change_mask)
+			return 0;
+
+		/*
+		 * Change is needed. Check if this md_session has set all the
+		 * requested peripherals. If another md session set a requested
+		 * peripheral then we cannot switch that peripheral to USB.
+		 * If this session owns all the requested peripherals, then
+		 * call function to switch the modes/masks for the md_session
+		 */
+		session_info = diag_md_session_get_pid(current->tgid);
+		if (!session_info) {
+			*change_mode = 1;
 			return 0;
 		}
-
-		if (param->mode_param == DIAG_MD_NORMAL) {
-			/*
-			 * The new client is asking for MD_NORMAL. We're
-			 * already in memory device mode - this must be
-			 * set by another active process. Return error
-			 * for this new client.
-			 */
+		if ((change_mask & session_info->peripheral_mask)
+							!= change_mask) {
 			DIAG_LOG(DIAG_DEBUG_USERSPACE,
-				 "unable to switch logging mode\n");
-			*change_mode = 0;
+			    "Another MD Session owns a requested peripheral\n");
 			return -EINVAL;
-		} else if (param->mode_param == DIAG_MD_PERIPHERAL) {
-			if (driver->md_session_mask & param->peripheral_mask) {
-				/*
-				 * The new client is asking for a
-				 * specific peripheral. This case checks
-				 * if a client is exercising this
-				 * peripheral already. Return error
-				 * if the peripheral is already in use.
-				 */
+		}
+		*change_mode = 1;
+
+		/* If all peripherals are being set to USB Mode, call close */
+		if (~change_mask & session_info->peripheral_mask) {
+			err = diag_md_peripheral_switch(session_info,
+					change_mask, DIAG_USB_MODE);
+		} else
+			diag_md_session_close(session_info);
+
+		return err;
+
+	} else if (req_mode == DIAG_MEMORY_DEVICE_MODE) {
+		/*
+		 * Get bit mask that represents what peripherals already have
+		 * been set. Check that requested peripherals already set are
+		 * owned by this md session
+		 */
+		change_mask = driver->md_session_mask & param->peripheral_mask;
+		session_info = diag_md_session_get_pid(current->tgid);
+
+		if (session_info) {
+			if ((session_info->peripheral_mask & change_mask)
+							!= change_mask) {
 				DIAG_LOG(DIAG_DEBUG_USERSPACE,
-					 "another instance running\n");
-				*change_mode = 0;
+				    "Another MD Session owns a requested peripheral\n");
+				return -EINVAL;
+			}
+			err = diag_md_peripheral_switch(session_info,
+					change_mask, DIAG_USB_MODE);
+		} else {
+			if (change_mask) {
+				DIAG_LOG(DIAG_DEBUG_USERSPACE,
+				    "Another MD Session owns a requested peripheral\n");
 				return -EINVAL;
 			}
 			err = diag_md_session_create(DIAG_MD_PERIPHERAL,
-						     param->peripheral_mask,
-						     DIAG_LOCAL_PROC);
-			*change_mode = 0;
-			return err;
+				param->peripheral_mask, DIAG_LOCAL_PROC);
 		}
+		*change_mode = 1;
+		return err;
 	}
 	return -EINVAL;
 }
@@ -1477,17 +1504,14 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
 	if (!param)
 		return -EINVAL;
 
-	if (param->mode_param == DIAG_MD_PERIPHERAL &&
-	    param->peripheral_mask == 0) {
+	if (!param->peripheral_mask) {
 		DIAG_LOG(DIAG_DEBUG_USERSPACE,
-			 "asking for peripehral mode with no mask being set\n");
+			"asking for mode switch with no peripheral mask set\n");
 		return -EINVAL;
 	}
 
-	if (param->mode_param == DIAG_MD_PERIPHERAL) {
-		peripheral_mask = diag_translate_mask(param->peripheral_mask);
-		param->peripheral_mask = peripheral_mask;
-	}
+	peripheral_mask = diag_translate_mask(param->peripheral_mask);
+	param->peripheral_mask = peripheral_mask;
 
 	switch (param->req_mode) {
 	case CALLBACK_MODE:
@@ -1507,8 +1531,8 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
 
 	curr_mode = driver->logging_mode;
 	DIAG_LOG(DIAG_DEBUG_USERSPACE,
-		 "request to switch logging from: %d to %d\n",
-		 curr_mode, new_mode);
+		"request to switch logging from %d mask:%0x to %d mask:%0x\n",
+		curr_mode, driver->md_session_mask, new_mode, peripheral_mask);
 
 	err = diag_md_session_check(curr_mode, new_mode, param, &do_switch);
 	if (err) {
@@ -1525,7 +1549,7 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
 	}
 
 	diag_ws_reset(DIAG_WS_MUX);
-	err = diag_mux_switch_logging(new_mode);
+	err = diag_mux_switch_logging(&new_mode, &peripheral_mask);
 	if (err) {
 		pr_err("diag: In %s, unable to switch mode from %d to %d, err: %d\n",
 		       __func__, curr_mode, new_mode, err);
@@ -1533,7 +1557,11 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
 		goto fail;
 	}
 	driver->logging_mode = new_mode;
+	driver->logging_mask = peripheral_mask;
+	DIAG_LOG(DIAG_DEBUG_USERSPACE,
+		"Switch logging to %d mask:%0x\n", new_mode, peripheral_mask);
 
+	/* Update to take peripheral_mask */
 	if (new_mode != DIAG_MEMORY_DEVICE_MODE) {
 		diag_update_real_time_vote(DIAG_PROC_MEMORY_DEVICE,
 					   MODE_REALTIME, ALL_PROC);
@@ -1839,10 +1867,8 @@ static int diag_ioctl_register_callback(unsigned long ioarg)
 		return -EINVAL;
 	}
 
-	if (driver->md_session_mode == DIAG_MD_NORMAL ||
-	    driver->md_session_mode == DIAG_MD_PERIPHERAL) {
+	if (driver->md_session_mode == DIAG_MD_PERIPHERAL)
 		return -EIO;
-	}
 
 	return err;
 }
@@ -2737,7 +2763,8 @@ static ssize_t diagchar_read(struct file *file, char __user *buf, size_t count,
 	mutex_lock(&driver->diagchar_mutex);
 
 	if ((driver->data_ready[index] & USER_SPACE_DATA_TYPE) &&
-	    (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE)) {
+	    (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE ||
+	     driver->logging_mode == DIAG_MULTI_MODE)) {
 		pr_debug("diag: process woken up\n");
 		/*Copy the type of data being passed*/
 		data_type = driver->data_ready[index] & USER_SPACE_DATA_TYPE;
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index aa394f517163..aec4f965b13e 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -234,7 +234,7 @@ void chk_logging_wakeup(void)
 		 * index as all the indices point to the same session
 		 * structure.
 		 */
-		if (driver->md_session_mode == DIAG_MD_NORMAL && j == 0)
+		if ((driver->md_session_mask == DIAG_CON_ALL) && (j == 0))
 			break;
 	}
 }
@@ -278,7 +278,8 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
 		 * for responses. Make sure we don't miss previous wakeups for
 		 * draining responses when we are in Memory Device Mode.
 		 */
-		if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE)
+		if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE ||
+				driver->logging_mode == DIAG_MULTI_MODE)
 			chk_logging_wakeup();
 	}
 	if (driver->rsp_buf_busy) {
@@ -346,7 +347,8 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
 		 * for responses. Make sure we don't miss previous wakeups for
 		 * draining responses when we are in Memory Device Mode.
 		 */
-		if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE)
+		if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE ||
+				driver->logging_mode == DIAG_MULTI_MODE)
 			chk_logging_wakeup();
 	}
 
@@ -919,8 +921,13 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 			if (MD_PERIPHERAL_MASK(reg_item->proc) &
 				info->peripheral_mask)
 				write_len = diag_send_data(reg_item, buf, len);
-		} else
-			write_len = diag_send_data(reg_item, buf, len);
+		} else {
+			if (MD_PERIPHERAL_MASK(reg_item->proc) &
+				driver->logging_mask)
+				diag_send_error_rsp(buf, len);
+			else
+				write_len = diag_send_data(reg_item, buf, len);
+		}
 		mutex_unlock(&driver->cmd_reg_mutex);
 		return write_len;
 	}
@@ -1228,10 +1235,9 @@ static int diagfwd_mux_close(int id, int mode)
 		return -EINVAL;
 	}
 
-	if ((mode == DIAG_USB_MODE &&
-	     driver->logging_mode == DIAG_MEMORY_DEVICE_MODE) ||
-	    (mode == DIAG_MEMORY_DEVICE_MODE &&
-	     driver->logging_mode == DIAG_USB_MODE)) {
+	if ((driver->logging_mode == DIAG_MULTI_MODE &&
+		driver->md_session_mode == DIAG_MD_NONE) ||
+		(driver->md_session_mode == DIAG_MD_PERIPHERAL)) {
 		/*
 		 * In this case the channel must not be closed. This case
 		 * indicates that the USB is removed but there is a client