(untested) tempsensor hwmon module

bsp/rp2040/unique.c

@@ -15,11 +15,11 @@ uint8_t get_unique_id_u8(uint8_t* desc_str) {
 
     pico_get_unique_board_id(&uid);
 
-    for (size_t byte = 0; byte < TU_ARRAY_SIZE(uid.id); byte++) {
+    for (size_t byte = 0; byte < PICO_UNIQUE_BOARD_ID_SIZE_BYTES/*TU_ARRAY_SIZE(uid.id)*/; byte++) {
         uint8_t tmp = uid.id[byte];
         for (int digit = 0; digit < 2; digit++) {
-            desc_str[chr_count++] = nyb2hex(tmp & 0xf);
+            desc_str[chr_count++] = nyb2hex(tmp >> 4);
-            tmp >>= 4;
+            tmp <<= 4;
         }
     }
 
@@ -33,11 +33,11 @@ uint8_t get_unique_id_u16(uint16_t* desc_str) {
 
     pico_get_unique_board_id(&uid);
 
-    for (size_t byte = 0; byte < TU_ARRAY_SIZE(uid.id); byte++) {
+    for (size_t byte = 0; byte < PICO_UNIQUE_BOARD_ID_SIZE_BYTES/*TU_ARRAY_SIZE(uid.id)*/; byte++) {
         uint8_t tmp = uid.id[byte];
         for (int digit = 0; digit < 2; digit++) {
-            desc_str[chr_count++] = nyb2hex(tmp & 0xf);
+            desc_str[chr_count++] = nyb2hex(tmp >> 4);
-            tmp >>= 4;
+            tmp <<= 4;
         }
     }
 

host/modules/Makefile

@@ -1,5 +1,5 @@
 
-obj-m  := dmj.o dmj-char.o i2c-dmj.o spi-dmj.o
+obj-m  := dmj.o dmj-char.o i2c-dmj.o spi-dmj.o dmj-hwmon.o
 KDIR   := /lib/modules/$(shell uname -r)/build
 PWD    := $(shell pwd)
 
@@ -14,9 +14,12 @@ clean:
 load:
 	sudo insmod ./dmj.ko
 	sudo insmod ./dmj-char.ko
+	sudo insmod ./i2c-dmj.ko
 	sudo insmod ./spi-dmj.ko
+	sudo insmod ./dmj-hwmon.ko
 
 unload:
+	sudo rmmod dmj-hwmon
 	sudo rmmod spi-dmj
 	sudo rmmod i2c-dmj
 	sudo rmmod dmj-char

host/modules/dmj-char.c

@@ -241,4 +241,3 @@ MODULE_AUTHOR("haskal <haskal@awoo.systems>");
 MODULE_DESCRIPTION("Character device for the " HARDWARE_NAME " USB multitool");
 MODULE_LICENSE("GPL v2");
 MODULE_ALIAS("platform:dmj-char");
-

host/modules/dmj-hwmon.c

@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Driver for the DapperMime-JTAG USB multitool: base MFD driver
+ *
+ * Copyright (c) 2021 sys64738 and haskal
+ *
+ * Adapted from:
+ *   jc42.c    Copyright (c) 2010 Ericcson AB
+ *   max197.c  Copyright (c) 2012 Vivien Didelot, Savoir-faire Linux Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/hwmon.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#if 0
+#include <linux/mfd/dmj.h>
+#else
+#include "dmj.h"
+#endif
+
+#define HARDWARE_NAME "DapperMime-JTAG"
+#define HWMON_NAME "dmj"
+
+#define DMJ_TEMP_CMD_GET_ADDR 0x00
+#define DMJ_TEMP_CMD_SET_ADDR 0x01
+#define DMJ_TEMP_CMD_GET_TEMP 0x02
+#define DMJ_TEMP_CMD_GET_MIN  0x03
+#define DMJ_TEMP_CMD_GET_MAX  0x04
+#define DMJ_TEMP_CMD_GET_CRIT 0x05
+
+struct dmj_hwmon {
+	struct platform_device *pdev;
+	struct device *hwmon_dev;
+};
+
+static umode_t dmj_hwmon_is_visible(const void *data, enum hwmon_sensor_types type,
+		uint32_t attr, int ch)
+{
+	switch (attr) {
+	case hwmon_temp_input:
+	case hwmon_temp_min:
+	case hwmon_temp_max:
+	case hwmon_temp_crit:
+		return 0444;
+	default:
+		return 0;
+	}
+}
+static int dmj_hwmon_read(struct device *dev, enum hwmon_sensor_types type,
+		uint32_t attr, int ch, long *val)
+{
+	struct dmj_hwmon *dmjh = dev_get_drvdata(dev);
+	uint8_t subcmd, *rbuf;
+	uint16_t rval;
+	int ret, rlen;
+
+	switch (attr) {
+	case hwmon_temp_input:
+		subcmd = DMJ_TEMP_CMD_GET_ADDR;
+		break;
+	case hwmon_temp_min:
+		subcmd = DMJ_TEMP_CMD_GET_MIN;
+		break;
+	case hwmon_temp_max:
+		subcmd = DMJ_TEMP_CMD_GET_MAX;
+		break;
+	case hwmon_temp_crit:
+		subcmd = DMJ_TEMP_CMD_GET_CRIT;
+		break;
+	default:
+		return -ENOTSUPP;
+	}
+
+	ret = dmj_transfer(dmjh->pdev, DMJ_CMD_MODE1_TEMPSENSOR,
+	                   DMJ_XFER_FLAGS_PARSE_RESP, &subcmd, sizeof(subcmd),
+	                   (void**)&rbuf, &rlen);
+	ret = dmj_check_retval(ret, rlen, dev, "hwmon read", true, 2, 2);
+	if (!ret) {
+		/* rval is 8.4 fixed point, bit 0x1000 is the sign bit, 0xe000 are flags */
+		rval = (uint16_t)rbuf[0] | ((uint16_t)rbuf[1] << 8);
+		rval &= 0x1fff; /* only data and sign bits, no flag bits */
+		rval |= ((rval & 0x1000) << 1) | ((rval & 0x1000) << 2)
+		                               | ((rval & 0x1000) << 3); /* sign-extend */
+		*val = ((long)(int16_t)rval * 125) / 2; /* 8.4 fixed -> .001 */
+	}
+	if (rbuf) kfree(rbuf);
+
+	return ret;
+}
+
+static const struct hwmon_channel_info *dmj_hwmon_info[] = {
+	HWMON_CHANNEL_INFO(temp, HWMON_T_INPUT | HWMON_T_MIN | HWMON_T_MAX | HWMON_T_CRIT),
+	NULL
+};
+static const struct hwmon_ops dmj_hwmon_ops = {
+	.is_visible = dmj_hwmon_is_visible,
+	.read = dmj_hwmon_read,
+	.write = NULL/*dmj_hwmon_write*/
+};
+static const struct hwmon_chip_info dmj_chip_info = {
+	.ops = &dmj_hwmon_ops,
+	.info = dmj_hwmon_info
+};
+
+static int dmj_hwmon_probe(struct platform_device *pdev)
+{
+	struct dmj_hwmon *dmjh;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	dmjh = devm_kzalloc(dev, sizeof(*dmjh), GFP_KERNEL);
+	if (!dmjh) return -ENOMEM;
+
+	dmjh->pdev = pdev;
+
+	/* TODO: check usb stuff? (mode1, tempsensor feature) */
+
+	platform_set_drvdata(pdev, dmjh);
+
+	dmjh->hwmon_dev = hwmon_device_register_with_info(dev, HWMON_NAME, dmjh,
+	                                                  &dmj_chip_info, NULL);
+	if (IS_ERR(dmjh->hwmon_dev)) {
+		ret = PTR_ERR(dmjh->hwmon_dev);
+		dev_err(dev, "hwmon device registration failed\n");
+	}
+
+	return ret;
+}
+static int dmj_hwmon_remove(struct platform_device *pdev)
+{
+	struct dmj_hwmon *dmjh = platform_get_drvdata(pdev);
+
+	hwmon_device_unregister(dmjh->hwmon_dev);
+
+	return 0;
+}
+
+static struct platform_driver dmj_hwmon_driver = {
+	.driver = {
+		"dmj-hwmon",
+	},
+	.probe = dmj_hwmon_probe,
+	.remove = dmj_hwmon_remove
+};
+module_platform_driver(dmj_hwmon_driver);
+
+MODULE_AUTHOR("sys64738 <sys64738@disroot.org>");
+MODULE_AUTHOR("haskal <haskal@awoo.systems>");
+MODULE_DESCRIPTION("Hwmon driver for the " HARDWARE_NAME " USB multitool");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:dmj-hwmon");
+

host/modules/dmj.c

@@ -436,6 +436,9 @@ static const struct mfd_cell dmj_mfd_spi[] = {
 static const struct mfd_cell dmj_mfd_i2c[] = {
 	{ .name = "dmj-i2c" },
 };
+static const struct mfd_cell dmj_mfd_hwmon[] = {
+	{ .name = "dmj-hwmon" },
+};
 
 /* USB device control */
 
@@ -507,7 +510,11 @@ static int dmj_probe(struct usb_interface *itf, const struct usb_device_id *usb_
 			}
 		}
 		if (dmj->dmj_m1feature & DMJ_FEATURE_MODE1_TEMPSENSOR) {
-			// TODO: add tempsensor MFD
+			ret = mfd_add_hotplug_devices(dev, dmj_mfd_hwmon, ARRAY_SIZE(dmj_mfd_hwmon));
+			if (ret) {
+				dev_err(dev, "failed to add MFD hwmon devices\n");
+				goto out_free;
+			}
 		}
 	}
 
@@ -577,3 +584,6 @@ MODULE_DESCRIPTION("Core driver for the " HARDWARE_NAME " USB multitool");
 MODULE_LICENSE("GPL v2");
 
 MODULE_SOFTDEP("post: dmj-char");
+MODULE_SOFTDEP("post: i2c-dmj");
+MODULE_SOFTDEP("post: spi-dmj");
+MODULE_SOFTDEP("post: dmj-hwmon");

host/modules/spi-dmj.c

@@ -503,7 +503,7 @@ static int dmj_spi_check_hw(struct dmj_spi *dmjs)
 
 		if (!ret) {
 			rbuf[16] = 0;
-			dev_info(dev, "Serprog pgmname: '%s'\n", rbuf);
+			dev_info(dev, "Serprog pgmname: '%s'\n", &rbuf[1]);
 		}
 		if (rbuf) kfree(rbuf);
 		if (ret) return ret;