From 9261d6f52b8ce052dff101b1b66ceb239cf8d3e8 Mon Sep 17 00:00:00 2001 From: Abdul Salam Date: Wed, 15 Mar 2023 08:39:09 +0530 Subject: [PATCH] regulator: Add snapshot of virtio_regulator driver Add snapshot for virtio_regulator driver from msm-5.15 at commit 2993484d0d65 ("drivers: virtio: Remove Backward compatibility of VIRTIO_ID's"). Change-Id: I17200a54d6aa90862b50108b9d16073b53d68fd1 Signed-off-by: Abdul Salam --- drivers/regulator/Kconfig | 9 + drivers/regulator/Makefile | 1 + drivers/regulator/virtio_regulator.c | 637 +++++++++++++++++++++++++++ include/linux/virtio_regulator.h | 34 ++ 4 files changed, 681 insertions(+) create mode 100644 drivers/regulator/virtio_regulator.c create mode 100644 include/linux/virtio_regulator.h diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig index b642ab7edb5c..a09962b37766 100644 --- a/drivers/regulator/Kconfig +++ b/drivers/regulator/Kconfig @@ -1591,4 +1591,13 @@ config REGULATOR_QTI_OCP_NOTIFIER providing a more graceful recovery mechanism than resetting the entire system. +config VIRTIO_REGULATOR + tristate "Virtio regulator driver" + depends on VIRTIO + help + This is the virtual regulator driver for virtio. It can be used on + Qualcomm Technologies Inc. virtual machine. Virtio regulator can vote + regulator for pass through peripheral devices via regulator backend in + host. + endif diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile index a0885d64db6d..f27fa36d59bb 100644 --- a/drivers/regulator/Makefile +++ b/drivers/regulator/Makefile @@ -189,5 +189,6 @@ obj-$(CONFIG_REGULATOR_RPMH) += rpmh-regulator.o obj-$(CONFIG_REGULATOR_STUB) += stub-regulator.o obj-$(CONFIG_REGULATOR_DEBUG_CONTROL) += debug-regulator.o obj-$(CONFIG_REGULATOR_QTI_OCP_NOTIFIER) += qti-ocp-notifier.o +obj-$(CONFIG_VIRTIO_REGULATOR) += virtio_regulator.o ccflags-$(CONFIG_REGULATOR_DEBUG) += -DDEBUG diff --git a/drivers/regulator/virtio_regulator.c b/drivers/regulator/virtio_regulator.c new file mode 100644 index 000000000000..77d7fc846e05 --- /dev/null +++ b/drivers/regulator/virtio_regulator.c @@ -0,0 +1,637 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved. + */ +#define pr_fmt(fmt) "%s: " fmt, __func__ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define VIRTIO_REGULATOR_MAX_NAME 20 +#define VIRTIO_REGULATOR_VOLTAGE_UNKNOWN 1 + +struct reg_virtio; + +struct virtio_regulator { + struct virtio_device *vdev; + struct virtqueue *vq; + struct completion rsp_avail; + struct mutex lock; + struct reg_virtio *regs; + int regs_count; +}; + +struct reg_virtio { + struct device_node *of_node; + struct regulator_desc rdesc; + struct regulator_dev *rdev; + struct virtio_regulator *vreg; + char name[VIRTIO_REGULATOR_MAX_NAME]; + bool enabled; +}; + +static int virtio_regulator_enable(struct regulator_dev *rdev) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_ENABLE); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + ret = virtio32_to_cpu(vreg->vdev, rsp->result); + + if (!ret) + reg->enabled = true; +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static int virtio_regulator_disable(struct regulator_dev *rdev) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_DISABLE); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + ret = virtio32_to_cpu(vreg->vdev, rsp->result); + + if (!ret) + reg->enabled = false; +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static int virtio_regulator_is_enabled(struct regulator_dev *rdev) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + + pr_debug("%s return %d\n", reg->rdesc.name, reg->enabled); + + return reg->enabled; +} + +static int virtio_regulator_set_voltage(struct regulator_dev *rdev, int min_uV, + int max_uV, unsigned int *selector) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_SET_VOLTAGE); + req->data[0] = cpu_to_virtio32(vreg->vdev, DIV_ROUND_UP(min_uV, 1000)); + req->data[1] = cpu_to_virtio32(vreg->vdev, max_uV / 1000); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + ret = virtio32_to_cpu(vreg->vdev, rsp->result); + +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static int virtio_regulator_get_voltage(struct regulator_dev *rdev) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_GET_VOLTAGE); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + if (rsp->result) { + pr_debug("%s: error response (%d)\n", reg->rdesc.name, + virtio32_to_cpu(vreg->vdev, rsp->result)); + ret = VIRTIO_REGULATOR_VOLTAGE_UNKNOWN; + } else + ret = virtio32_to_cpu(vreg->vdev, rsp->data[0]) * 1000; + +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static int virtio_regulator_set_mode(struct regulator_dev *rdev, + unsigned int mode) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_SET_MODE); + req->data[0] = cpu_to_virtio32(vreg->vdev, mode); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + ret = virtio32_to_cpu(vreg->vdev, rsp->result); + +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static unsigned int virtio_regulator_get_mode(struct regulator_dev *rdev) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_GET_MODE); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + if (rsp->result) { + pr_err("%s: error response (%d)\n", reg->rdesc.name, + virtio32_to_cpu(vreg->vdev, rsp->result)); + ret = 0; + } else + ret = virtio32_to_cpu(vreg->vdev, rsp->data[0]); + +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static int virtio_regulator_set_load(struct regulator_dev *rdev, int load_ua) +{ + struct reg_virtio *reg = rdev_get_drvdata(rdev); + struct virtio_regulator *vreg = reg->vreg; + struct virtio_regulator_msg *req, *rsp; + struct scatterlist sg[1]; + unsigned int len; + int ret = 0; + + req = kzalloc(sizeof(struct virtio_regulator_msg), GFP_KERNEL); + if (!req) + return -ENOMEM; + + strscpy(req->name, reg->rdesc.name, sizeof(req->name)); + req->type = cpu_to_virtio32(vreg->vdev, VIRTIO_REGULATOR_T_SET_LOAD); + req->data[0] = cpu_to_virtio32(vreg->vdev, load_ua); + sg_init_one(sg, req, sizeof(*req)); + + mutex_lock(&vreg->lock); + + ret = virtqueue_add_outbuf(vreg->vq, sg, 1, req, GFP_KERNEL); + if (ret) { + pr_err("%s: fail to add output buffer\n", reg->rdesc.name); + goto out; + } + + virtqueue_kick(vreg->vq); + + wait_for_completion(&vreg->rsp_avail); + + rsp = virtqueue_get_buf(vreg->vq, &len); + if (!rsp) { + pr_err("%s: fail to get virtqueue buffer\n", reg->rdesc.name); + ret = -EIO; + goto out; + } + + ret = virtio32_to_cpu(vreg->vdev, rsp->result); + +out: + mutex_unlock(&vreg->lock); + kfree(req); + + pr_debug("%s return %d\n", reg->rdesc.name, ret); + + return ret; +} + +static const struct regulator_ops virtio_regulator_ops = { + .enable = virtio_regulator_enable, + .disable = virtio_regulator_disable, + .is_enabled = virtio_regulator_is_enabled, + .set_voltage = virtio_regulator_set_voltage, + .get_voltage = virtio_regulator_get_voltage, + .set_mode = virtio_regulator_set_mode, + .get_mode = virtio_regulator_get_mode, + .set_load = virtio_regulator_set_load, +}; + +static void virtio_regulator_isr(struct virtqueue *vq) +{ + struct virtio_regulator *vregulator = vq->vdev->priv; + + complete(&vregulator->rsp_avail); +} + +static int virtio_regulator_init_vqs(struct virtio_regulator *vreg) +{ + struct virtqueue *vqs[1]; + vq_callback_t *cbs[] = { virtio_regulator_isr }; + static const char * const names[] = { "regulator" }; + int ret; + + ret = virtio_find_vqs(vreg->vdev, 1, vqs, cbs, names, NULL); + if (ret) + return ret; + + vreg->vq = vqs[0]; + + return 0; +} + +static int virtio_regulator_allocate_reg(struct virtio_regulator *vreg) +{ + struct device_node *parent_node, *node, *child_node; + int i, ret; + + vreg->regs_count = 0; + parent_node = vreg->vdev->dev.parent->of_node; + + for_each_available_child_of_node(parent_node, node) { + for_each_available_child_of_node(node, child_node) { + /* Skip child nodes handled by other drivers. */ + if (of_find_property(child_node, "compatible", NULL)) + continue; + vreg->regs_count++; + } + } + + if (vreg->regs_count == 0) { + dev_err(&vreg->vdev->dev, + "could not find any regulator subnodes\n"); + return -ENODEV; + } + + vreg->regs = devm_kcalloc(&vreg->vdev->dev, vreg->regs_count, + sizeof(*vreg->regs), GFP_KERNEL); + if (!vreg->regs) + return -ENOMEM; + + i = 0; + for_each_available_child_of_node(parent_node, node) { + for_each_available_child_of_node(node, child_node) { + /* Skip child nodes handled by other drivers. */ + if (of_find_property(child_node, "compatible", NULL)) + continue; + + vreg->regs[i].of_node = child_node; + vreg->regs[i].vreg = vreg; + + ret = of_property_read_string(child_node, "regulator-name", + &vreg->regs[i].rdesc.name); + if (ret) { + dev_err(&vreg->vdev->dev, + "could not read regulator-name\n"); + return ret; + } + + i++; + } + } + + return 0; +} + +static int virtio_regulator_init_reg(struct reg_virtio *reg) +{ + struct device *dev = reg->vreg->vdev->dev.parent; + struct regulator_config reg_config = {}; + struct regulator_init_data *init_data; + int ret = 0; + + reg->rdesc.owner = THIS_MODULE; + reg->rdesc.type = REGULATOR_VOLTAGE; + reg->rdesc.ops = &virtio_regulator_ops; + + init_data = of_get_regulator_init_data(dev, reg->of_node, ®->rdesc); + if (init_data == NULL) + return -ENOMEM; + + init_data->constraints.input_uV = init_data->constraints.max_uV; + init_data->constraints.valid_ops_mask |= REGULATOR_CHANGE_VOLTAGE; + + if (init_data->constraints.min_uV == 0 && + init_data->constraints.max_uV == 0) + reg->rdesc.n_voltages = 0; + else if (init_data->constraints.min_uV == init_data->constraints.max_uV) + reg->rdesc.n_voltages = 1; + else + reg->rdesc.n_voltages = 2; + + reg_config.dev = dev; + reg_config.init_data = init_data; + reg_config.of_node = reg->of_node; + reg_config.driver_data = reg; + + reg->rdev = devm_regulator_register(dev, ®->rdesc, ®_config); + if (IS_ERR(reg->rdev)) { + ret = PTR_ERR(reg->rdev); + reg->rdev = NULL; + dev_err(®->vreg->vdev->dev, "fail to register regulator\n"); + return ret; + } + + return ret; +} + +static int virtio_regulator_probe(struct virtio_device *vdev) +{ + struct virtio_regulator *vreg; + unsigned int i; + int ret; + + if (!virtio_has_feature(vdev, VIRTIO_F_VERSION_1)) + return -ENODEV; + + vreg = devm_kzalloc(&vdev->dev, sizeof(struct virtio_regulator), + GFP_KERNEL); + if (!vreg) + return -ENOMEM; + + vdev->priv = vreg; + vreg->vdev = vdev; + mutex_init(&vreg->lock); + init_completion(&vreg->rsp_avail); + + ret = virtio_regulator_init_vqs(vreg); + if (ret) { + dev_err(&vdev->dev, "fail to initialize virtqueue\n"); + return ret; + } + + virtio_device_ready(vdev); + + ret = virtio_regulator_allocate_reg(vreg); + if (ret) { + dev_err(&vdev->dev, "fail to allocate regulators\n"); + goto err_allocate_reg; + } + + for (i = 0; i < vreg->regs_count; i++) { + ret = virtio_regulator_init_reg(&vreg->regs[i]); + if (ret) { + dev_err(&vdev->dev, "fail to initialize regulator %s\n", + vreg->regs[i].rdesc.name); + goto err_init_reg; + } + + ret = devm_regulator_debug_register(&vdev->dev, vreg->regs[i].rdev); + if (ret) { + dev_err(&vdev->dev, "fail to register regulator %s debugfs\n", + vreg->regs[i].rdesc.name); + goto err_register_reg_debug; + } + } + + dev_dbg(&vdev->dev, "virtio regulator probe successfully\n"); + + return 0; + +err_register_reg_debug: +err_init_reg: +err_allocate_reg: + vdev->config->del_vqs(vdev); + return ret; +} + +static void virtio_regulator_remove(struct virtio_device *vdev) +{ + struct virtio_regulator *vreg = vdev->priv; + void *buf; + + vdev->config->reset(vdev); + while ((buf = virtqueue_detach_unused_buf(vreg->vq)) != NULL) + kfree(buf); + vdev->config->del_vqs(vdev); +} + +static const struct virtio_device_id id_table[] = { + { VIRTIO_ID_REGULATOR, VIRTIO_DEV_ANY_ID }, + { 0 }, +}; + +static unsigned int features[] = { +}; + +static struct virtio_driver virtio_regulator_driver = { + .feature_table = features, + .feature_table_size = ARRAY_SIZE(features), + .driver.name = KBUILD_MODNAME, + .driver.owner = THIS_MODULE, + .id_table = id_table, + .probe = virtio_regulator_probe, + .remove = virtio_regulator_remove, +}; + +static int __init virtio_regulator_init(void) +{ + return register_virtio_driver(&virtio_regulator_driver); +} + +static void __exit virtio_regulator_exit(void) +{ + unregister_virtio_driver(&virtio_regulator_driver); +} +subsys_initcall(virtio_regulator_init); +module_exit(virtio_regulator_exit); + +MODULE_DEVICE_TABLE(virtio, id_table); +MODULE_DESCRIPTION("Virtio regulator driver"); +MODULE_LICENSE("GPL"); diff --git a/include/linux/virtio_regulator.h b/include/linux/virtio_regulator.h new file mode 100644 index 000000000000..40797bcf08bb --- /dev/null +++ b/include/linux/virtio_regulator.h @@ -0,0 +1,34 @@ +/* SPDX-License-Identifier: GPL-2.0-only */ +/* Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved. */ + +#ifndef _LINUX_VIRTIO_REGULATOR_H +#define _LINUX_VIRTIO_REGULATOR_H + +#include +#include +#include +#include + +/* Request/response message format */ +struct virtio_regulator_msg { + u8 name[20]; + __virtio32 type; + __virtio32 result; + __virtio32 data[4]; +}; + +/* Virtio ID of regulator : 0xC001 */ +#define VIRTIO_ID_REGULATOR 49153 + +/* Request type */ +#define VIRTIO_REGULATOR_T_ENABLE 0 +#define VIRTIO_REGULATOR_T_DISABLE 1 +#define VIRTIO_REGULATOR_T_SET_VOLTAGE 2 +#define VIRTIO_REGULATOR_T_GET_VOLTAGE 3 +#define VIRTIO_REGULATOR_T_SET_CURRENT_LIMIT 4 +#define VIRTIO_REGULATOR_T_GET_CURRENT_LIMIT 5 +#define VIRTIO_REGULATOR_T_SET_MODE 6 +#define VIRTIO_REGULATOR_T_GET_MODE 7 +#define VIRTIO_REGULATOR_T_SET_LOAD 8 + +#endif /* _LINUX_VIRTIO_REGULATOR_H */