You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
kernel_samsung_sm7125/drivers/regulator/virtio_regulator.c

623 lines
15 KiB

/* Copyright (c) 2019, 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
* only version 2 as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#define pr_fmt(fmt) "%s: " fmt, __func__
#include <linux/spinlock.h>
#include <linux/slab.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/interrupt.h>
#include <linux/virtio.h>
#include <linux/virtio_regulator.h>
#include <linux/scatterlist.h>
#include <linux/delay.h>
#include <linux/completion.h>
#include <linux/mutex.h>
#include <linux/regulator/driver.h>
#include <linux/regulator/machine.h>
#include <linux/regulator/of_regulator.h>
#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;
strlcpy(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;
strlcpy(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;
strlcpy(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;
strlcpy(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;
strlcpy(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;
strlcpy(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;
strlcpy(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 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;
int i, ret;
vreg->regs_count = 0;
parent_node = vreg->vdev->dev.parent->of_node;
for_each_available_child_of_node(parent_node, node) {
/* Skip child nodes handled by other drivers. */
if (of_find_property(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) {
/* Skip child nodes handled by other drivers. */
if (of_find_property(node, "compatible", NULL))
continue;
vreg->regs[i].of_node = node;
vreg->regs[i].vreg = vreg;
ret = of_property_read_string(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, &reg->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;
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, &reg->rdesc, &reg_config);
if (IS_ERR(reg->rdev)) {
ret = PTR_ERR(reg->rdev);
reg->rdev = NULL;
dev_err(&reg->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;
}
}
dev_dbg(&vdev->dev, "virtio regulator probe successfully\n");
return 0;
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 v2");