BeagleV-Fire-ubuntu/patches/linux/0010-BeagleV-Fire-Add-MPFS-TVS-auxiliary-driver.patch

From 413071dc2f13f581eb844a0009d571dfa0bb2f02 Mon Sep 17 00:00:00 2001
From: Lars Randers <lranders@mail.dk>
Date: Thu, 20 Jun 2024 13:55:40 +0000
Subject: [PATCH] BeagleV-Fire: Add MPFS TVS auxiliary driver

Signed-off-by: Lars Randers <lranders@mail.dk>
---
 drivers/hwmon/Kconfig          |  11 +
 drivers/hwmon/Makefile         |   1 +
 drivers/hwmon/tvs-mpfs.c       | 371 +++++++++++++++++++++++++++++++++
 drivers/mailbox/mailbox-mpfs.c |  62 ++++++
 4 files changed, 445 insertions(+)
 create mode 100644 drivers/hwmon/tvs-mpfs.c

diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index a5143d01b..e3564df18 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -2108,6 +2108,17 @@ config SENSORS_TMP513
 	  This driver can also be built as a module. If so, the module
 	  will be called tmp513.
 
+config SENSORS_TVS_MPFS
+	tristate "PolarFire SoC (MPFS) TVS"
+	depends on POLARFIRE_SOC_MAILBOX
+	help
+	  This driver adds support for the PolarFire SoC (MPFS) Temperature Voltage Sensor.
+
+	  To compile this driver as a module, choose M here. the
+	  module will be called tvs-mpfs.
+
+	  If unsure, say N.
+
 config SENSORS_VEXPRESS
 	tristate "Versatile Express"
 	depends on VEXPRESS_CONFIG
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 11d076cad..1cebd0e73 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -203,6 +203,7 @@ obj-$(CONFIG_SENSORS_TMP401)	+= tmp401.o
 obj-$(CONFIG_SENSORS_TMP421)	+= tmp421.o
 obj-$(CONFIG_SENSORS_TMP464)	+= tmp464.o
 obj-$(CONFIG_SENSORS_TMP513)	+= tmp513.o
+obj-$(CONFIG_SENSORS_TVS_MPFS)  += tvs-mpfs.o
 obj-$(CONFIG_SENSORS_VEXPRESS)	+= vexpress-hwmon.o
 obj-$(CONFIG_SENSORS_VIA_CPUTEMP)+= via-cputemp.o
 obj-$(CONFIG_SENSORS_VIA686A)	+= via686a.o
diff --git a/drivers/hwmon/tvs-mpfs.c b/drivers/hwmon/tvs-mpfs.c
new file mode 100644
index 000000000..087a8b33f
--- /dev/null
+++ b/drivers/hwmon/tvs-mpfs.c
@@ -0,0 +1,371 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ *
+ * Author: Lars Randers <lranders@mail.dk>
+ *
+ */
+
+#include <linux/auxiliary_bus.h>
+#include <linux/err.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+#include <linux/hwmon.h>
+#include <linux/io.h>
+#include <linux/module.h>
+
+#define PFSOC_CONTROL_SCB_TVS_CONTROL 0x08
+#define PFSOC_CONTROL_SCB_TVS_OUTPUT0 0x24
+#define PFSOC_CONTROL_SCB_TVS_OUTPUT1 0x28
+
+#define CTRL_POWEROFF BIT(5)
+#define CTRL_ABORT    BIT(4)
+#define CTRL_TEMP     BIT(3)
+#define CTRL_2P5      BIT(2)
+#define CTRL_1P8      BIT(1)
+#define CTRL_1P05     BIT(0)
+
+#define OUTPUT0_U1P8_MASK 0x7FFF0000
+#define OUTPUT0_U1P8_OFF  16
+#define OUTPUT0_U1P0_MASK 0x00007FFF
+#define OUTPUT0_U1P0_OFF  0
+#define OUTPUT1_TEMP_MASK 0xFFFF0000
+#define OUTPUT1_TEMP_OFF  16
+#define OUTPUT1_U2P5_MASK 0x00007FFF
+#define OUTPUT1_U2P5_OFF  0
+
+/*
+ * Poll intervals (in milliseconds)
+ */
+#define MPFS_TVS_MIN_POLL_INTERVAL 2000
+
+// The following constant is 273.5 in (16.4) fixedpoint notation
+#define MPFS_TVS_MIN_TEMP_IN_K 0x1112
+
+typedef struct {
+	long min;
+	long actual;
+	long max;
+} mpfs_tvs_sensor_t;
+
+typedef enum {
+	SN_V1P05 = 0,
+	SN_V1P8,
+	SN_V2P5,
+	SN_TEMP,
+
+	SN_MAX
+} mpfs_tvs_sn_t;
+
+static const char *mpfs_tvs_voltage_labels[] = { "U1P05", "U1P8", "U2P5" };
+
+struct mpfs_tvs {
+	struct device *dev;
+	struct device *hwmon_dev;
+	struct task_struct *poll_task;
+	bool kthread_running;
+	long update_interval;	/* in milli-seconds */
+	mpfs_tvs_sensor_t sensors[SN_MAX];
+	void __iomem *ctrl_base;
+};
+
+static int mpfs_tvs_update_sensors(struct mpfs_tvs *data) {
+	u32 temp;
+	u32 work;
+
+	/* read measurement */
+	temp = readl(data->ctrl_base + PFSOC_CONTROL_SCB_TVS_OUTPUT1);
+	work = temp;
+
+	/* compute temperature */
+	temp = (temp & OUTPUT1_TEMP_MASK) >> OUTPUT1_TEMP_OFF;
+	temp = clamp_val(temp, MPFS_TVS_MIN_TEMP_IN_K, INT_MAX);
+	temp = temp - MPFS_TVS_MIN_TEMP_IN_K; /* Kelvin to Celsius */
+	temp = (temp * 1000) >> 4;
+	data->sensors[SN_TEMP].actual = temp;
+	data->sensors[SN_TEMP].max =
+		max(data->sensors[SN_TEMP].actual, data->sensors[SN_TEMP].max);
+	data->sensors[SN_TEMP].min =
+		min(data->sensors[SN_TEMP].min, data->sensors[SN_TEMP].actual);
+
+	/* compute voltage */
+	work &= OUTPUT1_U2P5_MASK;
+	work  = (1 * work) >> 3;
+	data->sensors[SN_V2P5].actual = work;
+	data->sensors[SN_V2P5].max =
+		max(data->sensors[SN_V2P5].actual, data->sensors[SN_V2P5].max);
+	data->sensors[SN_V2P5].min =
+		min(data->sensors[SN_V2P5].min, data->sensors[SN_V2P5].actual);
+
+	temp = readl(data->ctrl_base + PFSOC_CONTROL_SCB_TVS_OUTPUT0);
+	work = temp;
+	temp = (OUTPUT0_U1P8_MASK & temp) >> OUTPUT0_U1P8_OFF;
+	temp = (1 * temp) >> 3;
+	data->sensors[SN_V1P8].actual = temp;
+	data->sensors[SN_V1P8].max =
+		max(data->sensors[SN_V1P8].actual, data->sensors[SN_V1P8].max);
+	data->sensors[SN_V2P5].min =
+		min(data->sensors[SN_V1P8].min, data->sensors[SN_V1P8].actual);
+
+	work &= OUTPUT0_U1P0_MASK;
+	work  = (1 * work) >> 3;
+	data->sensors[SN_V1P05].actual = work;
+	data->sensors[SN_V1P05].max =
+		max(data->sensors[SN_V1P05].actual, data->sensors[SN_V1P05].max);
+	data->sensors[SN_V1P05].min =
+		min(data->sensors[SN_V1P05].min, data->sensors[SN_V1P05].actual);
+
+	return 0;
+}
+
+
+static int mpfs_tvs_chip_read(struct mpfs_tvs *data, long *val)
+{
+	*val = data->update_interval;
+	return 0;
+}
+
+static int mpfs_tvs_temp_read(struct mpfs_tvs *data, u32 attr,
+							  int channel, long *val)
+{
+	switch(attr) {
+	case hwmon_temp_input:
+		*val = data->sensors[SN_TEMP].actual;
+		break;
+
+	case hwmon_temp_max:
+		*val = data->sensors[SN_TEMP].max;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static int mpfs_tvs_voltage_read(struct mpfs_tvs *data, u32 attr,
+								 int channel, long *val)
+{
+	dev_dbg(data->dev, "read voltage chan %d\n", channel);
+	switch(attr) {
+	case hwmon_in_input:
+		*val = data->sensors[channel].actual;
+		break;
+
+	case hwmon_in_max:
+		*val = data->sensors[channel].max;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+
+static ssize_t mpfs_tvs_interval_write(struct mpfs_tvs *data, long val)
+{
+	data->update_interval = clamp_val(val,
+									  MPFS_TVS_MIN_POLL_INTERVAL, INT_MAX);
+	return 0;
+}
+
+
+static umode_t mpfs_tvs_is_visible(const void *data,
+								   enum hwmon_sensor_types type,
+								   u32 attr, int channel)
+{
+	if(type == hwmon_chip && attr == hwmon_chip_update_interval)
+		return 0644;
+
+	if(type == hwmon_temp) {
+		switch(attr) {
+		case hwmon_temp_input:
+		case hwmon_temp_max:
+		case hwmon_temp_label:
+			return 0444;
+
+		default:
+			return 0;
+		}
+	} else if (type == hwmon_in) {
+		switch (attr) {
+		case hwmon_in_input:
+		case hwmon_in_label:
+			return 0444;
+
+		default:
+			return 0;
+		}
+	}
+	return 0;
+}
+
+static int mpfs_tvs_read(struct device *dev, enum hwmon_sensor_types type,
+						 u32 attr, int channel, long *val)
+{
+	struct mpfs_tvs *data = dev_get_drvdata(dev);
+
+	switch(type) {
+	case hwmon_temp:
+		return mpfs_tvs_temp_read(data, attr, channel, val);
+	case hwmon_in:
+		return mpfs_tvs_voltage_read(data, attr, channel, val);
+	case hwmon_chip:
+		return mpfs_tvs_chip_read(data, val);
+
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int mpfs_tvs_write(struct device *dev, enum hwmon_sensor_types type,
+						  u32 attr, int channel, long val)
+{
+	struct mpfs_tvs *data = dev_get_drvdata(dev);
+
+	switch(type) {
+	case hwmon_chip:
+		return mpfs_tvs_interval_write(data, val);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int mpfs_tvs_read_labels(struct device *dev,
+								enum hwmon_sensor_types type,
+								u32 attr, int channel,
+								const char **str)
+{
+	switch(type) {
+	case hwmon_temp:
+		*str = "CPU Temp";
+		return 0;
+	case hwmon_in:
+		*str = mpfs_tvs_voltage_labels[channel];
+		return 0;
+	default:
+		return -ENOTSUPP;
+	}
+}
+
+
+static const struct hwmon_ops mpfs_tvs_ops = {
+	.is_visible = mpfs_tvs_is_visible,
+	.read_string = mpfs_tvs_read_labels,
+	.read = mpfs_tvs_read,
+	.write = mpfs_tvs_write,
+};
+
+static const struct hwmon_channel_info *mpfs_tvs_info[] = {
+	HWMON_CHANNEL_INFO(chip,
+					   HWMON_C_REGISTER_TZ|HWMON_C_UPDATE_INTERVAL),
+	HWMON_CHANNEL_INFO(temp,
+					   HWMON_T_INPUT|HWMON_T_MIN|HWMON_T_MAX|HWMON_T_LABEL),
+	HWMON_CHANNEL_INFO(in,
+					   HWMON_I_INPUT|HWMON_I_LABEL,
+					   HWMON_I_INPUT|HWMON_I_LABEL,
+					   HWMON_I_INPUT|HWMON_I_LABEL),
+	NULL
+};
+
+static const struct hwmon_chip_info mpfs_tvs_chip_info = {
+	.ops = &mpfs_tvs_ops,
+	.info = mpfs_tvs_info,
+};
+
+
+static int mpfs_tvs_poll_task(void *ptr)
+{
+	struct mpfs_tvs *data = ptr;
+	int ret = 0;
+
+	data->kthread_running = true;
+
+	set_freezable();
+
+	while(!kthread_should_stop()) {
+		schedule_timeout_interruptible(data->update_interval);
+		try_to_freeze();
+		ret = mpfs_tvs_update_sensors(data);
+		if(ret)
+			break;
+	}
+
+	data->kthread_running = false;
+	return ret;
+}
+
+static int mpfs_tvs_probe(struct auxiliary_device *auxdev,
+						  const struct auxiliary_device_id *id)
+{
+	struct device *dev = &auxdev->dev;
+	struct device *hwmon_dev;
+	struct mpfs_tvs *data;
+	struct task_struct *task;
+	int err;
+
+	data = devm_kzalloc(dev, sizeof(*data), GFP_KERNEL);
+	if(!data)
+		return -ENOMEM;
+
+	data->ctrl_base = (void __iomem *)auxdev->dev.platform_data;
+
+	data->dev = dev;
+	data->kthread_running = false;
+
+	hwmon_dev = devm_hwmon_device_register_with_info(dev, "mpfs_tvs",
+													 data,
+													 &mpfs_tvs_chip_info,
+													 NULL);
+
+	if(IS_ERR(hwmon_dev)) {
+		err = PTR_ERR(hwmon_dev);
+		dev_err(dev, "Class registration failed (%d)\n", err);
+		return err;
+	}
+
+	/* enable HW sensor */
+	writel(CTRL_1P05 | CTRL_1P8 | CTRL_2P5 | CTRL_TEMP,
+		   data->ctrl_base + PFSOC_CONTROL_SCB_TVS_CONTROL);
+
+	data->hwmon_dev = hwmon_dev;
+	data->sensors[SN_TEMP].max = 0;
+	data->sensors[SN_V1P05].min =
+		data->sensors[SN_V1P8].min =
+		data->sensors[SN_V2P5].min = 20000;
+	data->sensors[SN_V1P05].max =
+		data->sensors[SN_V1P8].max =
+		data->sensors[SN_V2P5].max = 0;
+	data->update_interval = MPFS_TVS_MIN_POLL_INTERVAL;
+	mpfs_tvs_update_sensors(data);
+
+	task = kthread_run(mpfs_tvs_poll_task, data, "tvs-mpfs-kthread");
+	if (IS_ERR(task)) {
+		err = PTR_ERR(task);
+		dev_err(dev, "Unable to run kthread err %d\n", err);
+		return err;
+	}
+
+	data->poll_task = task;
+
+	dev_info(dev, "Registered MPFS TVS auxiliary driver\n");
+	return 0;
+}
+
+static const struct auxiliary_device_id mpfs_tvs_ids[] = {
+	{
+		.name = "mailbox_mpfs.tvs-mpfs",
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(auxiliary, mpfs_reset_ids);
+
+static struct auxiliary_driver mpfs_tvs_driver = {
+	.probe		= mpfs_tvs_probe,
+	.id_table	= mpfs_tvs_ids,
+};
+module_auxiliary_driver(mpfs_tvs_driver);
+
+MODULE_AUTHOR("Lars Randers <lranders@mail.dk>");
+MODULE_DESCRIPTION("MPFS temperature voltage sensor driver");
+MODULE_LICENSE("GPL");
\ No newline at end of file
diff --git a/drivers/mailbox/mailbox-mpfs.c b/drivers/mailbox/mailbox-mpfs.c
index 162df4965..198e6b37d 100644
--- a/drivers/mailbox/mailbox-mpfs.c
+++ b/drivers/mailbox/mailbox-mpfs.c
@@ -8,6 +8,7 @@
  *
  */
 
+#include <linux/auxiliary_bus.h>
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/init.h>
@@ -220,6 +221,64 @@ static const struct mbox_chan_ops mpfs_mbox_ops = {
 	.last_tx_done = mpfs_mbox_last_tx_done,
 };
 
+static void mpfs_mbox_auxdev_release(struct device *dev)
+{
+	struct auxiliary_device *auxdev = to_auxiliary_dev(dev);
+
+	kfree(auxdev);
+}
+
+static struct auxiliary_device *mpfs_mbox_adev_alloc(struct device *parent_dev)
+{
+	struct auxiliary_device *auxdev;
+	int ret;
+
+	auxdev = kzalloc(sizeof(*auxdev), GFP_KERNEL);
+	if(!auxdev)
+		return ERR_PTR(-ENOMEM);
+
+	auxdev->name = "tvs-mpfs";
+	auxdev->dev.parent = parent_dev;
+	auxdev->dev.release = mpfs_mbox_auxdev_release;
+	auxdev->id = 10u;
+
+	ret = auxiliary_device_init(auxdev);
+	if(ret) {
+		kfree(auxdev);
+		return ERR_PTR(ret);
+	}
+
+	return auxdev;
+}
+
+static void mpfs_mbox_unregister_auxdev(void *dev)
+{
+	struct auxiliary_device *auxdev = dev;
+
+	auxiliary_device_delete(auxdev);
+	auxiliary_device_uninit(auxdev);
+}
+
+int mpfs_mbox_auxdev_register(struct device *parent_dev, void __iomem *base)
+{
+	struct auxiliary_device *auxdev;
+	int ret;
+
+	auxdev = mpfs_mbox_adev_alloc(parent_dev);
+	if(IS_ERR(auxdev))
+		return PTR_ERR(auxdev);
+
+	auxdev->dev.platform_data = base;
+
+	ret = auxiliary_device_add(auxdev);
+	if(ret) {
+		auxiliary_device_uninit(auxdev);
+		return ret;
+	}
+
+	return devm_add_action_or_reset(parent_dev, mpfs_mbox_unregister_auxdev, auxdev);
+}
+
 static int mpfs_mbox_probe(struct platform_device *pdev)
 {
 	struct mpfs_mbox *mbox;
@@ -261,6 +320,9 @@ static int mpfs_mbox_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Registering MPFS mailbox controller failed\n");
 		return ret;
 	}
+
+	mpfs_mbox_auxdev_register(&pdev->dev, mbox->ctrl_base);
+
 	dev_info(&pdev->dev, "Registered MPFS mailbox controller driver\n");
 
 	return 0;
-- 
2.39.2