diag: Fix error response during SSR

This patch addresses the scheduling conflicts of
control channel removal work by listening
to peripheral events.

CRs-Fixed: 1081265
Change-Id: Ib9f2448e564c4800535bd80d13c9bda5cc283c3c
Signed-off-by: Manoj Prabhu B <bmanoj@codeaurora.org>

drivers/char/diag/diagchar.h

@@ -473,6 +473,7 @@ struct diagchar_dev {
 	int ref_count;
 	int mask_clear;
 	struct mutex diag_maskclear_mutex;
+	struct mutex diag_notifier_mutex;
 	struct mutex diagchar_mutex;
 	struct mutex diag_file_mutex;
 	wait_queue_head_t wait_q;

drivers/char/diag/diagchar_core.c

@@ -3400,6 +3400,7 @@ static int __init diagchar_init(void)
 	mutex_init(&driver->hdlc_disable_mutex);
 	mutex_init(&driver->diagchar_mutex);
 	mutex_init(&driver->diag_maskclear_mutex);
+	mutex_init(&driver->diag_notifier_mutex);
 	mutex_init(&driver->diag_file_mutex);
 	mutex_init(&driver->delayed_rsp_mutex);
 	mutex_init(&apps_data_mutex);

drivers/char/diag/diagfwd_cntl.c

@@ -358,6 +358,8 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
 		feature_mask_len = FEATURE_MASK_LEN;
 	}
 
+	diag_cmd_remove_reg_by_proc(peripheral);
+
 	driver->feature[peripheral].rcvd_feature_mask = 1;
 
 	for (i = 0; i < feature_mask_len && read_len < len; i++) {

drivers/char/diag/diagfwd_socket.c

@@ -34,6 +34,9 @@
 #include "diagfwd_socket.h"
 #include "diag_ipc_logging.h"
 
+#include <soc/qcom/subsystem_notif.h>
+#include <soc/qcom/subsystem_restart.h>
+
 #define DIAG_SVC_ID		0x1001
 
 #define MODEM_INST_BASE		0
@@ -50,6 +53,7 @@
 #define INST_ID_DCI		4
 
 struct diag_cntl_socket_info *cntl_socket;
+static uint64_t bootup_req[NUM_SOCKET_SUBSYSTEMS];
 
 struct diag_socket_info socket_data[NUM_PERIPHERALS] = {
 	{
@@ -416,7 +420,7 @@ static void socket_open_client(struct diag_socket_info *info)
 		return;
 	}
 	__socket_open_channel(info);
-	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s opened client\n", info->name);
 }
 
 static void socket_open_server(struct diag_socket_info *info)
@@ -492,6 +496,13 @@ static void __socket_close_channel(struct diag_socket_info *info)
 	if (!atomic_read(&info->opened))
 		return;
 
+	if (bootup_req[info->peripheral] == PEPIPHERAL_SSR_UP) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: %s is up, stopping cleanup: bootup_req = %d\n",
+		info->name, (int)bootup_req[info->peripheral]);
+		return;
+	}
+
 	memset(&info->remote_addr, 0, sizeof(struct sockaddr_msm_ipc));
 	diagfwd_channel_close(info->fwd_ctxt);
 
@@ -610,7 +621,9 @@ static int cntl_socket_process_msg_client(uint32_t cmd, uint32_t node_id,
 	case CNTL_CMD_REMOVE_CLIENT:
 		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received remove client\n",
 			 info->name);
+		mutex_lock(&driver->diag_notifier_mutex);
 		socket_close_channel(info);
+		mutex_unlock(&driver->diag_notifier_mutex);
 		break;
 	default:
 		return -EINVAL;
@@ -619,6 +632,25 @@ static int cntl_socket_process_msg_client(uint32_t cmd, uint32_t node_id,
 	return 0;
 }
 
+static int restart_notifier_cb(struct notifier_block *this,
+				  unsigned long code,
+				  void *data);
+
+struct restart_notifier_block {
+	unsigned processor;
+	char *name;
+	struct notifier_block nb;
+};
+
+static struct restart_notifier_block restart_notifiers[] = {
+	{SOCKET_MODEM, "modem", .nb.notifier_call = restart_notifier_cb},
+	{SOCKET_ADSP, "adsp", .nb.notifier_call = restart_notifier_cb},
+	{SOCKET_WCNSS, "wcnss", .nb.notifier_call = restart_notifier_cb},
+	{SOCKET_SLPI, "slpi", .nb.notifier_call = restart_notifier_cb},
+	{SOCKET_CDSP, "cdsp", .nb.notifier_call = restart_notifier_cb},
+};
+
+
 static void cntl_socket_read_work_fn(struct work_struct *work)
 {
 	union cntl_port_msg msg;
@@ -626,7 +658,6 @@ static void cntl_socket_read_work_fn(struct work_struct *work)
 	struct kvec iov = { 0 };
 	struct msghdr read_msg = { 0 };
 
-
 	if (!cntl_socket)
 		return;
 
@@ -845,8 +876,11 @@ static int __diag_cntl_socket_init(void)
 int diag_socket_init(void)
 {
 	int err = 0;
+	int i;
 	int peripheral = 0;
+	void *handle;
 	struct diag_socket_info *info = NULL;
+	struct restart_notifier_block *nb;
 
 	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
 		info = &socket_cntl[peripheral];
@@ -867,6 +901,17 @@ int diag_socket_init(void)
 		goto fail;
 	}
 
+	for (i = 0; i < ARRAY_SIZE(restart_notifiers); i++) {
+		nb = &restart_notifiers[i];
+		if (nb) {
+			handle = subsys_notif_register_notifier(nb->name,
+				&nb->nb);
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s: registering notifier for '%s', handle=%p\n",
+			__func__, nb->name, handle);
+		}
+	}
+
 	register_ipcrtr_af_init_notifier(&socket_notify);
 fail:
 	return err;
@@ -902,6 +947,65 @@ static int socket_ready_notify(struct notifier_block *nb,
 	return 0;
 }
 
+static int restart_notifier_cb(struct notifier_block *this, unsigned long code,
+	void *_cmd)
+{
+	struct restart_notifier_block *notifier;
+
+	notifier = container_of(this,
+			struct restart_notifier_block, nb);
+	if (!notifier) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: %s: invalid notifier block\n", __func__);
+		return NOTIFY_DONE;
+	}
+
+	mutex_lock(&driver->diag_notifier_mutex);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+	"%s: ssr for processor %d ('%s')\n",
+	__func__, notifier->processor, notifier->name);
+
+	switch (code) {
+
+	case SUBSYS_BEFORE_SHUTDOWN:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: %s: SUBSYS_BEFORE_SHUTDOWN\n", __func__);
+		bootup_req[notifier->processor] = PEPIPHERAL_SSR_DOWN;
+		break;
+
+	case SUBSYS_AFTER_SHUTDOWN:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: %s: SUBSYS_AFTER_SHUTDOWN\n", __func__);
+		break;
+
+	case SUBSYS_BEFORE_POWERUP:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: %s: SUBSYS_BEFORE_POWERUP\n", __func__);
+		break;
+
+	case SUBSYS_AFTER_POWERUP:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: %s: SUBSYS_AFTER_POWERUP\n", __func__);
+		if (!bootup_req[notifier->processor]) {
+			bootup_req[notifier->processor] = PEPIPHERAL_SSR_DOWN;
+			break;
+		}
+		bootup_req[notifier->processor] = PEPIPHERAL_SSR_UP;
+		break;
+
+	default:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: code: %lu\n", code);
+		break;
+	}
+	mutex_unlock(&driver->diag_notifier_mutex);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+	"diag: bootup_req[%s] = %d\n",
+	notifier->name, (int)bootup_req[notifier->processor]);
+
+	return NOTIFY_DONE;
+}
+
 int diag_socket_init_peripheral(uint8_t peripheral)
 {
 	struct diag_socket_info *info = NULL;

drivers/char/diag/diagfwd_socket.h

@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-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
@@ -24,10 +24,24 @@
 #define PORT_TYPE_SERVER		0
 #define PORT_TYPE_CLIENT		1
 
+#define PEPIPHERAL_AFTER_BOOT		0
+#define PEPIPHERAL_SSR_DOWN		1
+#define PEPIPHERAL_SSR_UP		2
+
 #define CNTL_CMD_NEW_SERVER		4
 #define CNTL_CMD_REMOVE_SERVER		5
 #define CNTL_CMD_REMOVE_CLIENT		6
 
+enum {
+	SOCKET_MODEM,
+	SOCKET_ADSP,
+	SOCKET_WCNSS,
+	SOCKET_SLPI,
+	SOCKET_CDSP,
+	SOCKET_APPS,
+	NUM_SOCKET_SUBSYSTEMS,
+};
+
 struct diag_socket_info {
 	uint8_t peripheral;
 	uint8_t type;