Merge "soc: qcom: subsystem_notif_virt: Added support for virtual subsystems"

#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;

struct state_notifier_block {

	const char *subsystem;

	struct notifier_block nb;

enum subsystem_type {

	VIRTUAL,

	NATIVE,

};

struct subsystem_descriptor {

	const char *name;

	u32 offset;

	enum subsystem_type type;

	struct notifier_block nb;

	void *handle;

	struct list_head notifier_list;

	unsigned int ssr_irq;

	struct list_head subsystem_list;

	struct work_struct work;

};

static LIST_HEAD(notifier_block_list);

static LIST_HEAD(subsystem_descriptor_list);

static struct workqueue_struct *ssr_wq;

static int subsys_state_callback(struct notifier_block *this,

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 state_notifier_block *notifier =

		container_of(this, struct state_notifier_block, nb);

	struct subsystem_descriptor *subsystem =

		container_of(this, struct subsystem_descriptor, nb);

	writel_relaxed(value, base_reg + notifier->offset);

	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 subsys_notif_virt_probe(struct platform_device *pdev)

{

	struct device_node *node;

	struct device_node *child = NULL;

	const char *ss_type;

	struct resource *res;

	struct state_notifier_block *notif_block;

	struct subsystem_descriptor *subsystem;

	int ret = 0;

	if (!pdev) {

		return -ENOMEM;

	}

	ssr_wq = create_singlethread_workqueue("ssr_wq");

	if (!ssr_wq) {

		dev_err(&pdev->dev, "Workqueue creation failed\n");

		return -ENOMEM;

	}

	for_each_child_of_node(node, child) {

		notif_block = devm_kmalloc(&pdev->dev,

				sizeof(struct state_notifier_block),

		subsystem = devm_kmalloc(&pdev->dev,

				sizeof(struct subsystem_descriptor),

				GFP_KERNEL);

		if (!notif_block)

			return -ENOMEM;

		if (!subsystem) {

			ret = -ENOMEM;

			goto err;

		}

		notif_block->subsystem =

		subsystem->name =

			of_get_property(child, "subsys-name", NULL);

		if (IS_ERR_OR_NULL(notif_block->subsystem)) {

		if (IS_ERR_OR_NULL(subsystem->name)) {

			dev_err(&pdev->dev, "Could not find subsystem name\n");

			ret = -EINVAL;

			goto err_nb;

			goto err;

		}

		notif_block->nb.notifier_call = subsys_state_callback;

		notif_block->handle =

			subsys_notif_register_notifier(notif_block->subsystem,

				&notif_block->nb);

		if (IS_ERR_OR_NULL(notif_block->handle)) {

			dev_err(&pdev->dev, "Could not register SSR notifier cb\n");

		ret = of_property_read_u32(child, "offset",

				&subsystem->offset);

		if (ret) {

			dev_err(&pdev->dev, "offset reading for %s failed\n",

					subsystem->name);

			ret = -EINVAL;

			goto err_nb;

			goto err;

		}

		ret = of_property_read_u32(child, "offset",

				&notif_block->offset);

		ret = of_property_read_string(child, "type",

				&ss_type);

		if (ret) {

			dev_err(&pdev->dev, "offset reading for %s failed\n",

				notif_block->subsystem);

			dev_err(&pdev->dev, "type reading for %s failed\n",

					subsystem->name);

			ret = -EINVAL;

			goto err_offset;

			goto err;

		}

		list_add_tail(&notif_block->notifier_list,

			&notifier_block_list);

		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");

				ret = -EINVAL;

				goto err;

			}

			list_add_tail(&subsystem->subsystem_list,

					&subsystem_descriptor_list);

			break;

		case VIRTUAL:

			subsystem->ssr_irq =

				of_irq_get_byname(child, "state-irq");

			if (IS_ERR_OR_NULL(subsystem->ssr_irq)) {

				dev_err(&pdev->dev, "Could not find IRQ\n");

				ret = -EINVAL;

				goto err;

			}

			ret = devm_request_threaded_irq(&pdev->dev,

					subsystem->ssr_irq, NULL,

					subsystem_restart_irq_handler,

					IRQF_ONESHOT | IRQF_TRIGGER_RISING,

					subsystem->name, subsystem);

			break;

		default:

			dev_err(&pdev->dev, "Unsupported type %d\n",

				subsystem->type);

		}

	}

	return 0;

err_offset:

	subsys_notif_unregister_notifier(notif_block->handle,

		&notif_block->nb);

err_nb:

	kfree(notif_block);

	INIT_WORK(&subsystem->work, subsystem_notif_wq_func);

	return 0;

err:

	destroy_workqueue(ssr_wq);

	return ret;

}

static int subsys_notif_virt_remove(struct platform_device *pdev)

{

	struct state_notifier_block *notif_block;

	struct subsystem_descriptor *subsystem, *node;

	destroy_workqueue(ssr_wq);

	list_for_each_entry(notif_block, &notifier_block_list,

			notifier_list) {

		subsys_notif_unregister_notifier(notif_block->handle,

			&notif_block->nb);

		list_del(&notif_block->notifier_list);

	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);

	}

	return 0;

}