drivers: soc: qcom: Add snapshot of virtual subsystem notification driver

Add snapshot of virtual subsystem notification driver
from msm-4.14. This snapshot is taken as of msm-4.14
commit 035eb1f641f7 ("drivers: soc: qcom: Add snapshot of
virtual subsystem notification driver").

Change-Id: I8006342906e80e10ce2b2579f0dd0d89a072a6c5
Signed-off-by: Anant Goel <anantg@codeaurora.org>

drivers/soc/qcom/Kconfig

@@ -299,6 +299,17 @@ config MSM_SUBSYSTEM_RESTART
 	  Moreover, the framework also notifies userspace of transitions
 	  between these states via sysfs.
 
+config MSM_QUIN_SUBSYSTEM_NOTIF_VIRT
+	tristate "MSM QUIN Subsystem Notif Virt"
+	select MSM_SUBSYSTEM_RESTART
+	help
+	  This option enables the virtual MSM subsystem notification
+	  framework for the QUIN platform.
+
+	  The virtual MSM subsystem notif framework provides support for
+	  a subsystem to notify its clients of its state, when the clients
+	  are in a different virtual machine domain than the subsystem.
+
 config SETUP_SSR_NOTIF_TIMEOUTS
 	bool "Set timeouts on SSR sysmon notifications and notifier callbacks"
 	help

drivers/soc/qcom/Makefile

@@ -11,6 +11,7 @@ obj-$(CONFIG_MSM_PIL)   +=      peripheral-loader.o
 obj-$(CONFIG_MSM_SUBSYSTEM_RESTART) += subsystem_restart.o
 obj-$(CONFIG_MSM_CDSP_LOADER) += qdsp6v2/
 subsystem_restart-y := msm_subsystem_restart.o subsystem_notif.o ramdump.o sysmon-qmi.o
+obj-$(CONFIG_MSM_QUIN_SUBSYSTEM_NOTIF_VIRT) += subsystem_notif_virt.o
 obj-$(CONFIG_MSM_PIL_SSR_GENERIC) += subsys-pil-tz.o
 obj-$(CONFIG_MSM_PIL_MSS_QDSP6V5) += pil-q6v5.o pil-msa.o pil-q6v5-mss.o
 obj-$(CONFIG_MSM_SERVICE_NOTIFIER) += service-notifier.o

drivers/soc/qcom/subsystem_notif_virt.c

@@ -0,0 +1,243 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2019-2020, The Linux Foundation. All rights reserved.
+ */
+
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/notifier.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/io.h>
+#include <linux/of_platform.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <soc/qcom/subsystem_notif.h>
+
+#define CLIENT_STATE_OFFSET 4
+#define SUBSYS_STATE_OFFSET 8
+
+static void __iomem *base_reg;
+
+enum subsystem_type {
+	VIRTUAL,
+	NATIVE,
+};
+
+struct subsystem_descriptor {
+	const char *name;
+	u32 offset;
+	enum subsystem_type type;
+	struct notifier_block nb;
+	void *handle;
+	int ssr_irq;
+	struct list_head subsystem_list;
+	struct work_struct work;
+};
+
+static LIST_HEAD(subsystem_descriptor_list);
+static struct workqueue_struct *ssr_wq;
+
+static void subsystem_notif_wq_func(struct work_struct *work)
+{
+	struct subsystem_descriptor *subsystem =
+		container_of(work, struct subsystem_descriptor, work);
+	void *subsystem_handle;
+	int state, ret;
+
+	state = readl_relaxed(base_reg + subsystem->offset);
+	subsystem_handle = subsys_notif_add_subsys(subsystem->name);
+	ret = subsys_notif_queue_notification(subsystem_handle, state, NULL);
+	writel_relaxed(ret, base_reg + subsystem->offset + CLIENT_STATE_OFFSET);
+}
+
+static int subsystem_state_callback(struct notifier_block *this,
+		unsigned long value, void *priv)
+{
+	struct subsystem_descriptor *subsystem =
+		container_of(this, struct subsystem_descriptor, nb);
+
+	writel_relaxed(value, base_reg + subsystem->offset +
+			SUBSYS_STATE_OFFSET);
+
+	return NOTIFY_OK;
+}
+
+static irqreturn_t subsystem_restart_irq_handler(int irq, void *dev_id)
+{
+	struct subsystem_descriptor *subsystem = dev_id;
+
+	queue_work(ssr_wq, &subsystem->work);
+
+	return IRQ_HANDLED;
+}
+
+static int get_resources(struct platform_device *pdev)
+{
+	struct device_node *node;
+	struct device_node *child = NULL;
+	const char *ss_type;
+	struct resource *res;
+	struct subsystem_descriptor *subsystem = NULL;
+	int ret = 0;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "vdev_base");
+	base_reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR_OR_NULL(base_reg)) {
+		dev_err(&pdev->dev, "Memory mapping failed\n");
+		return -ENOMEM;
+	}
+
+	node = pdev->dev.of_node;
+	for_each_child_of_node(node, child) {
+
+		subsystem = devm_kmalloc(&pdev->dev,
+				sizeof(struct subsystem_descriptor),
+				GFP_KERNEL);
+		if (!subsystem)
+			return -ENOMEM;
+
+		subsystem->name =
+			of_get_property(child, "subsys-name", NULL);
+		if (IS_ERR_OR_NULL(subsystem->name)) {
+			dev_err(&pdev->dev, "Could not find subsystem name\n");
+			return -EINVAL;
+		}
+
+		ret = of_property_read_u32(child, "offset",
+				&subsystem->offset);
+		if (ret) {
+			dev_err(&pdev->dev, "offset reading for %s failed\n",
+					subsystem->name);
+			return -EINVAL;
+		}
+
+		ret = of_property_read_string(child, "type",
+				&ss_type);
+		if (ret) {
+			dev_err(&pdev->dev, "type reading for %s failed\n",
+					subsystem->name);
+			return -EINVAL;
+		}
+
+		if (!strcmp(ss_type, "virtual"))
+			subsystem->type = VIRTUAL;
+
+		if (!strcmp(ss_type, "native"))
+			subsystem->type = NATIVE;
+
+		switch (subsystem->type) {
+		case NATIVE:
+			subsystem->nb.notifier_call =
+				subsystem_state_callback;
+
+			subsystem->handle =
+				subsys_notif_register_notifier(
+					subsystem->name, &subsystem->nb);
+			if (IS_ERR_OR_NULL(subsystem->handle)) {
+				dev_err(&pdev->dev,
+					"Could not register SSR notifier cb\n");
+				return -EINVAL;
+			}
+			list_add_tail(&subsystem->subsystem_list,
+					&subsystem_descriptor_list);
+			break;
+		case VIRTUAL:
+			subsystem->ssr_irq =
+				of_irq_get_byname(child, "state-irq");
+			if (subsystem->ssr_irq < 0) {
+				dev_err(&pdev->dev, "Could not find IRQ\n");
+				return -EINVAL;
+			}
+			ret = devm_request_threaded_irq(&pdev->dev,
+					subsystem->ssr_irq, NULL,
+					subsystem_restart_irq_handler,
+					IRQF_ONESHOT | IRQF_TRIGGER_RISING,
+					subsystem->name, subsystem);
+			INIT_WORK(&subsystem->work, subsystem_notif_wq_func);
+			break;
+		default:
+			dev_err(&pdev->dev, "Unsupported type %d\n",
+				subsystem->type);
+		}
+	}
+
+	return 0;
+}
+
+static void release_resources(void)
+{
+	struct subsystem_descriptor *subsystem, *node;
+
+	list_for_each_entry_safe(subsystem, node, &subsystem_descriptor_list,
+			subsystem_list) {
+		subsys_notif_unregister_notifier(subsystem->handle,
+				&subsystem->nb);
+		list_del(&subsystem->subsystem_list);
+	}
+}
+
+static int subsys_notif_virt_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+
+	if (!pdev) {
+		pr_err("%s: pdev is NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	ssr_wq = create_singlethread_workqueue("ssr_wq");
+	if (!ssr_wq) {
+		dev_err(&pdev->dev, "Workqueue creation failed\n");
+		return -ENOMEM;
+	}
+
+	ret = get_resources(pdev);
+	if (ret)
+		destroy_workqueue(ssr_wq);
+
+	return ret;
+}
+
+static int subsys_notif_virt_remove(struct platform_device *pdev)
+{
+	destroy_workqueue(ssr_wq);
+	release_resources();
+
+	return 0;
+}
+
+static const struct of_device_id match_table[] = {
+	{ .compatible = "qcom,subsys-notif-virt" },
+	{},
+};
+
+static struct platform_driver subsys_notif_virt_driver = {
+	.probe = subsys_notif_virt_probe,
+	.remove = subsys_notif_virt_remove,
+	.driver = {
+		.name = "subsys_notif_virt",
+		.of_match_table = match_table,
+	},
+};
+
+static int __init subsys_notif_virt_init(void)
+{
+	return platform_driver_register(&subsys_notif_virt_driver);
+}
+module_init(subsys_notif_virt_init);
+
+static void __exit subsys_notif_virt_exit(void)
+{
+	platform_driver_unregister(&subsys_notif_virt_driver);
+}
+module_exit(subsys_notif_virt_exit);
+
+MODULE_DESCRIPTION("Subsystem Notification Virtual Driver");
+MODULE_LICENSE("GPL v2");