BPI-CM9-Linux/aml-comp/kernel/aml-6.12/common_drivers/drivers/thermal/vir_tsens.c

// SPDX-License-Identifier: (GPL-2.0+ OR MIT)
/*
 * Copyright (c) 2019 Amlogic, Inc. All rights reserved.
 */

//#define DEBUG
#include <linux/reboot.h>
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/types.h>
#include <linux/printk.h>
#include <linux/platform_device.h>
#include <linux/of.h>
#include <linux/io.h>
#include <linux/thermal.h>
#include <linux/bitfield.h>
#include <linux/of_device.h>
#include <linux/amlogic/vir_tsens.h>
#include <linux/of_platform.h>
#include <linux/amlogic/meson_cooldev.h>
#include "thermal_core.h"

static int vir_get_temp(struct thermal_zone_device *p, int *temp)
{
	const struct vir_tsens *vir_tsens = p->devdata;
	int ret;

	if (!vir_tsens || !vir_tsens->soc_tzd)
		return -EINVAL;

	ret = thermal_zone_get_temp(vir_tsens->soc_tzd, temp);
	if (ret) {
		dev_err(vir_tsens->dev, "fail to read temperature %d\n", ret);
		return ret;
	}

	return 0;
}

static struct thermal_zone_device_ops vir_tsens_ops = {
	.get_temp = vir_get_temp,
};

int vir_register_device(struct vir_tsens *vir_tsens)
{
	int id;

	for (id = 0; id < vir_tsens->num_vtsens; id++) {
		vir_tsens->tzd_array[id] = devm_thermal_of_zone_register(vir_tsens->dev, id,
									 vir_tsens,
									 &vir_tsens_ops);
		if (IS_ERR(vir_tsens->tzd_array[id])) {
			for (id--; id >= 0; id--) {
				devm_thermal_of_zone_unregister(vir_tsens->dev,
								vir_tsens->tzd_array[id]);
			}
			return -EINVAL;
		}
	}

	return 0;
}

static struct platform_device *get_pdev_of_soc_sensor(const char *zone_name)
{
	struct device_node *soc_np, *sensor_np;
	struct platform_device *pdev;
	struct of_phandle_args args;
	int ret;

	soc_np = of_find_node_by_name(NULL, zone_name);
	if (!soc_np) {
		pr_err("Failed to find thermal zone node '%s'\n", zone_name);
		return NULL;
	}

	ret = of_parse_phandle_with_args(soc_np, "thermal-sensors",
					 "#thermal-sensor-cells", 0, &args);
	of_node_put(soc_np);
	if (ret) {
		pr_err("Failed to parse thermal-sensors: %d\n", ret);
		return NULL;
	}

	sensor_np = args.np;
	pdev = of_find_device_by_node(sensor_np);
	of_node_put(sensor_np);
	if (!pdev) {
		pr_err("Failed to find platform_device for tsensor\n");
		return NULL;
	}

	return pdev;
}

static int vir_params_init(struct vir_tsens *vir_tsens)
{
	const char *name;
	int ret;

	ret = of_property_read_u32(vir_tsens->dev->of_node, "num-vtsens", &vir_tsens->num_vtsens);
	if (ret) {
		dev_err(vir_tsens->dev, "get num-vtsensor failed\n");
		return ret;
	}

	vir_tsens->tzd_array = devm_kcalloc(vir_tsens->dev, vir_tsens->num_vtsens,
					    sizeof(struct thermal_zone_device *), GFP_KERNEL);
	if (!vir_tsens->tzd_array)
		return -ENOMEM;

	ret = of_property_read_string(vir_tsens->dev->of_node, "zone-name", &name);
	if (ret)
		return ret;
	/*for get soc_thermal temp*/
	vir_tsens->soc_tzd = thermal_zone_get_zone_by_name(name);
	if (IS_ERR(vir_tsens->soc_tzd)) {
		dev_err(vir_tsens->dev, "fail to get thermal zone %ld\n",
			PTR_ERR(vir_tsens->soc_tzd));
		return PTR_ERR(vir_tsens->soc_tzd);
	}

	vir_tsens->soc_pdev = get_pdev_of_soc_sensor(name);
	if (!vir_tsens->soc_pdev)
		return -EPROBE_DEFER;

	return ret;
}

static int vir_tsens_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct vir_tsens *vir_tsens;
	int ret;

	vir_tsens = devm_kzalloc(dev, sizeof(*vir_tsens), GFP_KERNEL);
	if (!vir_tsens)
		return -ENOMEM;

	vir_tsens->dev = dev;
	ret = vir_params_init(vir_tsens);
	if (ret)
		return ret;

	/*
	 * For status tracking only; not involved in probe order management;
	 * must be deleted manually.
	 */
	vir_tsens->soc_pdev_link = device_link_add(&pdev->dev,
						   &vir_tsens->soc_pdev->dev,
						   DL_FLAG_STATELESS);
	if (!vir_tsens->soc_pdev_link) {
		dev_err(dev, "failed to add device link to soc sensor\n");
		put_device(&vir_tsens->soc_pdev->dev);
		vir_tsens->soc_pdev = NULL;
		return -ENOMEM;
	}

	ret = vir_register_device(vir_tsens);
	if (ret) {
		device_link_del(vir_tsens->soc_pdev_link);
		vir_tsens->soc_pdev_link = NULL;
		put_device(&vir_tsens->soc_pdev->dev);
		vir_tsens->soc_pdev = NULL;
		return ret;
	}
	dev_set_drvdata(dev, vir_tsens);
	dev_dbg(dev, "virtual sensor probe done\n");

	return 0;
}

static void vir_tsens_remove(struct platform_device *pdev)
{
	struct vir_tsens *vir_tsens = dev_get_drvdata(&pdev->dev);
	int id;

	if (!vir_tsens)
		return;
	for (id = 0; id < vir_tsens->num_vtsens; id++) {
		if (vir_tsens->tzd_array[id])
			devm_thermal_of_zone_unregister(&pdev->dev, vir_tsens->tzd_array[id]);
	}

	if (vir_tsens->soc_pdev_link) {
		device_link_del(vir_tsens->soc_pdev_link);
		vir_tsens->soc_pdev_link = NULL;
	}
	if (vir_tsens->soc_pdev) {
		put_device(&vir_tsens->soc_pdev->dev);
		vir_tsens->soc_pdev = NULL;
	}
}

static const struct of_device_id control_of_match[] = {
	{
		.compatible = "amlogic, vir-tsens",
	},
	{},
};

struct platform_driver vir_tsens_platdrv = {
	.driver = {
		.name		= "vir-tsens",
		.owner		= THIS_MODULE,
		.of_match_table = control_of_match,
	},
	.probe	= vir_tsens_probe,
	.remove	= vir_tsens_remove,
	.shutdown = vir_tsens_remove,
};