diff options
| -rw-r--r-- | drivers/soc/qcom/subsystem_notif_virt.c | 175 |
1 files changed, 129 insertions, 46 deletions
diff --git a/drivers/soc/qcom/subsystem_notif_virt.c b/drivers/soc/qcom/subsystem_notif_virt.c index cf794f249d3d..fb6697b25f05 100644 --- a/drivers/soc/qcom/subsystem_notif_virt.c +++ b/drivers/soc/qcom/subsystem_notif_virt.c @@ -21,38 +21,77 @@ #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 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 subsys_state_callback(struct notifier_block *this, +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) { @@ -69,65 +108,109 @@ static int subsys_notif_virt_probe(struct platform_device *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; - - notif_block->subsystem = - of_get_property(child, "subsys-name", NULL); - if (IS_ERR_OR_NULL(notif_block->subsystem)) { - dev_err(&pdev->dev, "Could not find subsystem name\n"); - ret = -EINVAL; - goto err_nb; + if (!subsystem) { + ret = -ENOMEM; + goto err; } - notif_block->nb.notifier_call = subsys_state_callback; - - notif_block->handle = - subsys_notif_register_notifier(notif_block->subsystem, - ¬if_block->nb); - if (IS_ERR_OR_NULL(notif_block->handle)) { - dev_err(&pdev->dev, "Could not register SSR notifier cb\n"); + 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"); ret = -EINVAL; - goto err_nb; + goto err; } ret = of_property_read_u32(child, "offset", - ¬if_block->offset); + &subsystem->offset); if (ret) { dev_err(&pdev->dev, "offset reading for %s failed\n", - notif_block->subsystem); + subsystem->name); ret = -EINVAL; - goto err_offset; + goto err; } - list_add_tail(¬if_block->notifier_list, - ¬ifier_block_list); + ret = of_property_read_string(child, "type", + &ss_type); + if (ret) { + dev_err(&pdev->dev, "type reading for %s failed\n", + subsystem->name); + ret = -EINVAL; + goto err; + } + 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, - ¬if_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, ¬ifier_block_list, - notifier_list) { - subsys_notif_unregister_notifier(notif_block->handle, - ¬if_block->nb); - list_del(¬if_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; } |