DragonProbe/host/modules/dragonprobe.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * Driver for the Dragon Probe USB multitool: base MFD driver
 *
 * Copyright (c) 2021 sys64738 and haskal
 *
 * Adapted from:
 *   dln2.c   Copyright (c) 2014 Intel Corporation
 */

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/types.h>
#include <linux/slab.h>
#include <linux/usb.h>
#include <linux/i2c.h>
#include <linux/mutex.h>
#include <linux/platform_device.h>
#include <linux/mfd/core.h>
#include <linux/rculist.h>

#if 0
#include <linux/mfd/dragonprobe.h>
#else
#include "dragonprobe.h"
#endif

#define HARDWARE_NAME "Dragon Probe"
#define HARDWARE_NAME_SYMBOLIC "dragonprobe"

#define DP_USB_TIMEOUT 500

#define DP_RESP_HDR_SIZE 4

/* endpoint indices, not addresses */
#define DP_VND_CFG_EP_OUT 0
#define DP_VND_CFG_EP_IN  1

struct dp_dev {
	struct usb_device *usb_dev;
	struct usb_interface *interface;
	uint8_t ep_in;
	uint8_t ep_out;

	spinlock_t disconnect_lock;
	bool disconnect;

	uint8_t dp_mode, dp_m1feature;
};

/* USB transfers */

static void *dp_prep_buf(int cmd, const void *wbuf, int *wbufsize, gfp_t gfp)
{
	int len;
	uint8_t *buf;

	if (cmd >= 0 && cmd <= 0xff) {
		/* send extra cmd byte, and optionally a payload */
		if (wbufsize && wbuf) len = *wbufsize + 1;
		else len = 1;

		buf = kmalloc(len, gfp);
		if (!buf) return NULL;

		buf[0] = (uint8_t)cmd;
		if (wbuf && *wbufsize)
			memcpy(&buf[1], wbuf, *wbufsize);
	} else {
		/* assuming we have a payload to send, but no (explicit) cmd */
		len = *wbufsize;
		buf = kmalloc(len, gfp);
		if (!buf) return NULL;

		memcpy(buf, wbuf, len);
	}

	*wbufsize = len;

	return buf;
}
static int dp_send_wait(struct dp_dev *dp, int cmd, const void *wbuf, int wbufsize)
{
	int ret = 0;
	int len = wbufsize, actual;
	void *buf;

	buf = dp_prep_buf(cmd, wbuf, &len, GFP_KERNEL);
	if (!buf) return -ENOMEM;

	ret = usb_bulk_msg(dp->usb_dev, usb_sndbulkpipe(dp->usb_dev, dp->ep_out),
			buf, len, &actual, DP_USB_TIMEOUT);

	kfree(buf);

	return ret;
}

int dp_xfer_internal(struct dp_dev *dp, int cmd, int recvflags,
		const void *wbuf, int wbufsize, void **rbuf, int *rbufsize)
{
	int ret = 0, actual, pl_off, todo;
	struct device *dev = &dp->interface->dev;
	uint32_t pl_len;
	void *tmpbuf = NULL, *longbuf = NULL;
	uint8_t *bbuf;
	uint8_t respstat;

	spin_lock(&dp->disconnect_lock);
	if (dp->disconnect) ret = -ENODEV;
	spin_unlock(&dp->disconnect_lock);

	if (ret) return ret;

	if ((cmd >= 0 && cmd <= 0xff) || (wbufsize && wbuf)) {
		ret = dp_send_wait(dp, cmd, wbuf, wbufsize);
		if (ret < 0) {
			dev_err(dev, "USB write failed: %d\n", ret);
			return ret;
		}
	}

	if (rbuf) *rbuf = NULL;

	if (recvflags & DP_XFER_FLAGS_PARSE_RESP) {
		/*
		 * if we do want to parse the response, we'll split the reads into
		 * blocks of 64 bytes, first to read the response header, and then
		 * allocate a big reponse buffer, and then keep doing 64b reads
		 * to fill that buffer. result length will be put in rbufsize, its
		 * value when passed to the function will not matter
		 */

		if (rbufsize) *rbufsize = -1;

		tmpbuf = kmalloc(64, GFP_KERNEL);
		if (!tmpbuf) return -ENOMEM;
		/* first read: 64b, with header data to parse */
		ret = usb_bulk_msg(dp->usb_dev, usb_rcvbulkpipe(dp->usb_dev, dp->ep_in),
				tmpbuf, 64, &actual, DP_USB_TIMEOUT);
		if (ret < 0) goto err_freetmp;
		if (actual < 0) { ret = -EREMOTEIO; goto err_freetmp; }

		bbuf = tmpbuf;

		if (bbuf[1] & 0x80) {
			if (actual < 3) {
				dev_err(dev, "short response (%d, expected at least 3)\n", actual);
				ret = -EREMOTEIO;
				goto err_freetmp;
			}

			pl_len = (uint32_t)(bbuf[1] & 0x7f);

			if (bbuf[2] & 0x80) {
				if (actual < 4) {
					dev_err(dev, "short response (%d, expected at least 4)\n", actual);
					ret = -EREMOTEIO;
					goto err_freetmp;
				}

				pl_len |= (uint32_t)(bbuf[2] & 0x7f) << 7;
				pl_len |= (uint32_t)bbuf[3] << 14;
				pl_off = 4;
			} else {
				pl_len |= (uint32_t)bbuf[2] << 7;
				pl_off = 3;
			}
		} else {
			if (actual < 2) {
				dev_err(dev, "short response (%d, expected at least 2)\n", actual);
				ret = -EREMOTEIO;
				goto err_freetmp;
			}

			pl_len = (uint32_t)bbuf[1];
			pl_off = 2;
		}

		respstat = bbuf[0];

		dev_dbg(dev, "got packet hdr: status %02x, payload len 0x%x, off %u\n",
				respstat, pl_len, pl_off);

		/* now that the header has been parsed, we can start filling in the
		 * actual response buffer */
		longbuf = kmalloc(pl_len, GFP_KERNEL);
		todo = (int)pl_len;
		/* rest of the data of the 1st read */
		memcpy(longbuf, tmpbuf + pl_off, actual - pl_off);
		todo -= (actual - pl_off);
		pl_off = actual - pl_off;

		while (todo) {
			actual = 64;
			if (todo < actual) actual = todo;

			ret = usb_bulk_msg(dp->usb_dev, usb_rcvbulkpipe(dp->usb_dev, dp->ep_in),
					tmpbuf, actual, &actual, DP_USB_TIMEOUT);
			if (ret < 0) goto err_freelong;
			if (actual < 0) { ret = -EREMOTEIO; goto err_freelong; }
			if (actual > todo) {
				dev_err(dev, "USB protocol violation! bulk reply longer than payload header!\n");
				ret = -EMSGSIZE;
				goto err_freelong;
			}

			memcpy(longbuf + pl_off, tmpbuf, actual);

			todo   -= actual;
			pl_off += actual;
		}

		/* we're done! */

		/* response maybe not always wanted in this case, maybe was just called
		 * to check the status */
		if (rbuf) *rbuf = longbuf;
		else kfree(longbuf);
		if (rbufsize) *rbufsize = (int)pl_len;

		ret = respstat;
	} else {
		/*
		 * otherwise, read max. rbufsize bytes (if using
		 * DP_XFER_FLAGS_FILL_RECVBUF, will try to fill it exactly, but it
		 * will error when going beyond!). also done in 64b chunks
		 */

		if (!rbufsize || *rbufsize <= 0) {
			//dev_warn(dev, "no rbuf\n");
			return 0;
		}

		if (recvflags & DP_XFER_FLAGS_FILL_RECVBUF) {
			tmpbuf = kmalloc(64, GFP_KERNEL);
			if (!tmpbuf) return -ENOMEM;
			longbuf = kmalloc(*rbufsize, GFP_KERNEL);
			if (!longbuf) { ret = -ENOMEM; goto err_freetmp; }

			todo = *rbufsize;
			pl_off = 0;
			while (todo) {
				actual = 64;
				if (todo < actual) actual = todo;

				ret = usb_bulk_msg(dp->usb_dev, usb_rcvbulkpipe(dp->usb_dev, dp->ep_in),
						tmpbuf, actual, &actual, DP_USB_TIMEOUT);
				if (ret < 0) goto err_freelong;
				if (actual < 0) { ret = -EREMOTEIO; goto err_freelong; }
				if (actual > todo) { ret = -EMSGSIZE; goto err_freelong; }

				memcpy(longbuf + pl_off, tmpbuf, actual);

				todo   -= actual;
				pl_off += actual;
			}

			ret = 0;
			*rbuf = longbuf;
			*rbufsize = pl_off;
		} else {
			/* just try it at once & see what happens */
			tmpbuf = NULL;
			longbuf = kmalloc(*rbufsize, GFP_KERNEL);
			if (!longbuf) return -ENOMEM;

			ret = usb_bulk_msg(dp->usb_dev, usb_rcvbulkpipe(dp->usb_dev, dp->ep_in),
					longbuf, *rbufsize, rbufsize, DP_USB_TIMEOUT);
			if (ret < 0) goto err_freelong;
			if (*rbufsize < 0) {
				//dev_warn(dev, "remoteio\n");
				ret = -EREMOTEIO; goto err_freelong;
			}

			ret = 0;
			*rbuf = longbuf;
		}
	}

	if (tmpbuf) kfree(tmpbuf);
	return ret;


err_freelong:
	if (longbuf) kfree(longbuf);
	if (rbuf) *rbuf = NULL;
err_freetmp:
	if (tmpbuf) kfree(tmpbuf);
	return ret;
}

int dp_transfer(struct platform_device *pdev, int cmd, int recvflags,
		const void *wbuf, int wbufsize, void **rbuf, int *rbufsize)
{
	struct dp_dev *dp;

	dp = dev_get_drvdata(pdev->dev.parent);

	return dp_xfer_internal(dp, cmd, recvflags, wbuf, wbufsize, rbuf, rbufsize);
}
EXPORT_SYMBOL(dp_transfer);

/* stuff on init */

static int dp_check_hw(struct dp_dev *dp)
{
	struct device *dev = &dp->interface->dev;

	int ret, len;
	uint16_t protover;
	uint8_t *buf = NULL;

	ret = dp_xfer_internal(dp, DP_CMD_CFG_GET_VERSION,
			DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
	ret = dp_check_retval(ret, len, dev, "version check", true, sizeof(protover), sizeof(protover));
	if (ret < 0 || !buf) goto out;

	protover = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);

	if (protover != DP_USB_CFG_PROTO_VER) {
		dev_err(&dp->interface->dev, HARDWARE_NAME " config protocol version 0x%04x too %s\n",
				protover, (protover > DP_USB_CFG_PROTO_VER) ? "new" : "old");

		ret = -ENODEV;
	} else
		ret = 0;

out:
	if (buf) kfree(buf);
	return ret;
}
static int dp_print_info(struct dp_dev *dp)
{
	int ret, i, j, len;
	uint16_t modes, mversion;
	uint8_t curmode, features;
	struct device *dev = &dp->interface->dev;
	uint8_t *buf;
	char modeinfo[16], namebuf[64];

	/* info string */
	ret = dp_xfer_internal(dp, DP_CMD_CFG_GET_INFOSTR,
			DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
	ret = dp_check_retval(ret, len, dev, "get info", true, -1, sizeof(namebuf)-1);
	if (ret < 0 || !buf) goto out;
	memcpy(namebuf, buf, len);
	namebuf[len] = 0;
	dev_info(dev, HARDWARE_NAME " '%s'\n", namebuf);
	kfree(buf); buf = NULL;

	/* cur mode */
	ret = dp_xfer_internal(dp, DP_CMD_CFG_GET_CUR_MODE,
			DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
	ret = dp_check_retval(ret, len, dev, "get info", true, sizeof(curmode), sizeof(curmode));
	if (ret < 0 || !buf) goto out;
	dp->dp_mode = curmode = buf[0];
	kfree(buf); buf = NULL;

	/* map of available modes */
	ret = dp_xfer_internal(dp, DP_CMD_CFG_GET_MODES,
			DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
	ret = dp_check_retval(ret, len, dev, "get info", true, sizeof(modes), sizeof(modes));
	if (ret < 0 || !buf) goto out;
	modes = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);
	kfree(buf); buf = NULL;

	for (i = 1; i < 16; ++i) { /* build the string, uglily */
		if (modes & (1<<i)) {
			if (i < 0xa) modeinfo[i - 1] = '0'+i-0;
			else modeinfo[i - 1] = 'a'+i-0xa;
		} else modeinfo[i - 1] = '-';
	}
	modeinfo[15] = 0;
	dev_dbg(dev, "available modes: x%s, currently %d\n", modeinfo, (int)curmode);

	/* for each available mode print name, version and features */
	for (i = 1; i < 16; ++i) {
		if (!(modes & (1<<i))) continue; /* not available */

		/* name */
		ret = dp_xfer_internal(dp, (i<<4) | DP_CMD_MODE_GET_NAME,
				DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
		ret = dp_check_retval(ret, len, dev, "get info", true, -1, sizeof(namebuf)-1);
		if (ret < 0 || !buf) goto out;
		memcpy(namebuf, buf, len);
		namebuf[len] = 0;
		kfree(buf); buf = NULL;

		/* version */
		ret = dp_xfer_internal(dp, (i<<4) | DP_CMD_MODE_GET_VERSION,
				DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
		ret = dp_check_retval(ret, len, dev, "get info", true, sizeof(mversion), sizeof(mversion));
		if (ret < 0 || !buf) goto out;
		mversion = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);
		kfree(buf); buf = NULL;

		/* features */
		ret = dp_xfer_internal(dp, (i<<4) | DP_CMD_MODE_GET_FEATURES,
				DP_XFER_FLAGS_PARSE_RESP, NULL, 0, (void**)&buf, &len);
		ret = dp_check_retval(ret, len, dev, "get info", true, sizeof(features), sizeof(features));
		if (ret < 0 || !buf) goto out;
		features = (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);
		kfree(buf); buf = NULL;

		if (i == 1) dp->dp_m1feature = features;

		for (j = 0; j < 8; ++j) {
			if (features & (1<<j)) modeinfo[j] = '0'+j;
			else modeinfo[j] = '-';
		}
		modeinfo[8] = 0;

		dev_dbg(dev, "Mode %d: '%s' version 0x%04x, features: %s\n",
				i, namebuf, mversion, modeinfo);
	}

	return 0;

out:
	if (buf) kfree(buf);
	return ret;
}
static int dp_hw_init(struct dp_dev *dp)
{
	int ret;

	ret = dp_check_hw(dp);
	if (ret < 0) return ret;

	return dp_print_info(dp);
}

/* MFD stuff */

static const struct mfd_cell dp_mfd_char[] = {
	{ .name = "dragonprobe-char" },
};
static const struct mfd_cell dp_mfd_spi[] = {
	{ .name = "dragonprobe-spi" },
};
static const struct mfd_cell dp_mfd_i2c[] = {
	{ .name = "dragonprobe-i2c" },
};
static const struct mfd_cell dp_mfd_hwmon[] = {
	{ .name = "dragonprobe-hwmon" },
};

/* USB device control */

static int dp_probe(struct usb_interface *itf, const struct usb_device_id *usb_id)
{
	struct usb_host_interface *hostitf = itf->cur_altsetting;
	struct usb_endpoint_descriptor *epin = NULL, *epout = NULL, *curep;
	struct device *dev = &itf->dev;
	struct dp_dev *dp;
	int ret, i;

	if (hostitf->desc.bNumEndpoints < 2) {
		dev_warn(dev, "not enough endpoints!\n");
		return -ENODEV;
	}

	for (i = 0; i < hostitf->desc.bNumEndpoints; ++i) {
		curep = &hostitf->endpoint[i].desc;

		if (!epin  && usb_endpoint_is_bulk_in (curep)) epin  = curep;
		if (!epout && usb_endpoint_is_bulk_out(curep)) epout = curep;

		if (epin && epout) break;
	}

	if (!epin) {
		dev_warn(dev, "found suitable device but no ep in\n");
		return -ENODEV;
	}
	if (!epout) {
		dev_warn(dev, "found suitable device but no ep out\n");
		return -ENODEV;
	}

	dp = kzalloc(sizeof(*dp), GFP_KERNEL);
	if (!dp) return -ENOMEM;

	dp->ep_out = epout->bEndpointAddress;
	dp->ep_in = epin->bEndpointAddress;
	dp->usb_dev = usb_get_dev(interface_to_usbdev(itf));
	dp->interface = itf;
	usb_set_intfdata(itf, dp);

	spin_lock_init(&dp->disconnect_lock);

	ret = dp_hw_init(dp);
	if (ret < 0) {
		dev_err(dev, "failed to initialize hardware\n");
		goto out_free;
	}

	ret = mfd_add_hotplug_devices(dev, dp_mfd_char, ARRAY_SIZE(dp_mfd_char));
	if (ret) {
		dev_err(dev, "failed to add MFD character devices\n");
		goto out_free;
	}

	if (dp->dp_mode == 1) {
		if (dp->dp_m1feature & DP_FEATURE_MODE1_SPI) {
			ret = mfd_add_hotplug_devices(dev, dp_mfd_spi, ARRAY_SIZE(dp_mfd_spi));
			if (ret) {
				dev_err(dev, "failed to add MFD SPI devices\n");
				goto out_free;
			}
		}
		if (dp->dp_m1feature & DP_FEATURE_MODE1_I2C) {
			ret = mfd_add_hotplug_devices(dev, dp_mfd_i2c, ARRAY_SIZE(dp_mfd_i2c));
			if (ret) {
				dev_err(dev, "failed to add MFD I2C devices\n");
				goto out_free;
			}
		}
		if (dp->dp_m1feature & DP_FEATURE_MODE1_TEMPSENSOR) {
			ret = mfd_add_hotplug_devices(dev, dp_mfd_hwmon, ARRAY_SIZE(dp_mfd_hwmon));
			if (ret) {
				dev_err(dev, "failed to add MFD hwmon devices\n");
				goto out_free;
			}
		}
	}

	return 0;

out_free:
	usb_put_dev(dp->usb_dev);
	kfree(dp);

	return ret;
}
static void dp_disconnect(struct usb_interface *itf)
{
	struct dp_dev *dp = usb_get_intfdata(itf);

	spin_lock(&dp->disconnect_lock);
	dp->disconnect = true;
	spin_unlock(&dp->disconnect_lock);

	mfd_remove_devices(&itf->dev);

	usb_put_dev(dp->usb_dev);

	kfree(dp);
}

static int dp_suspend(struct usb_interface *itf, pm_message_t message)
{
	struct dp_dev *dp = usb_get_intfdata(itf);

	(void)message;

	spin_lock(&dp->disconnect_lock);
	dp->disconnect = true;
	spin_unlock(&dp->disconnect_lock);

	return 0;
}
static int dp_resume(struct usb_interface *itf)
{
	struct dp_dev *dp = usb_get_intfdata(itf);

	dp->disconnect = false;

	return 0;
}

static const struct usb_device_id dp_table[] = {
	{ USB_DEVICE_AND_INTERFACE_INFO(0xcafe, 0x1312, USB_CLASS_VENDOR_SPEC, 'D', 'P') },
	{ }
};
MODULE_DEVICE_TABLE(usb, dp_table);

static struct usb_driver dp_driver = {
	.name = HARDWARE_NAME_SYMBOLIC,
	.probe = dp_probe,
	.disconnect = dp_disconnect,
	.id_table = dp_table,
	.suspend = dp_suspend,
	.resume = dp_resume,
};
module_usb_driver(dp_driver);

MODULE_AUTHOR("sys64738 <sys64738@disroot.org>");
MODULE_AUTHOR("haskal <haskal@awoo.systems>");
MODULE_DESCRIPTION("Core driver for the " HARDWARE_NAME " USB multitool");
MODULE_LICENSE("GPL v2");

MODULE_SOFTDEP("post: dragonprobe-char");
MODULE_SOFTDEP("post: dragonprobe-i2c");
MODULE_SOFTDEP("post: dragonprobe-spi");
MODULE_SOFTDEP("post: dragonprobe-hwmon");