[PATCHv2] gnss: motmdm: Add support for Motorola Mapphone MDM6600 modem

[Pavel Machek]

Motorola is using a custom TS 27.010 based multiplexer protocol
for various devices on the modem. These devices can be accessed on
dedicated channels using Linux kernel serdev-ngsm driver.

For the GNSS on these devices, we need to kick the GNSS device at a
desired rate. Otherwise the GNSS device stops sending data after a
few minutes. The rate we poll data defaults to 1000 ms, and can be
specified with a module option rate_ms between 1 to 16 seconds.

[Tony Lindgren did most of the work here, I just converted it to be
normal serdev.]

Signed-off-by: Pavel Machek <pavel@xxxxxx>

diff --git a/drivers/gnss/Kconfig b/drivers/gnss/Kconfig
index bd12e3d57baa..9fac72eba726 100644
--- a/drivers/gnss/Kconfig
+++ b/drivers/gnss/Kconfig
@@ -13,6 +13,14 @@ menuconfig GNSS
 
 if GNSS
 
+config GNSS_MOTMDM
+	tristate "Motorola Modem TS 27.010 serdev GNSS receiver support"
+	depends on SERIAL_DEV_BUS
+	help
+	  Say Y here if you have a Motorola modem using TS 27.010
+	  multiplexer protocol for GNSS such as a Motorola Mapphone
+	  series device like Droid 4.
+
 config GNSS_SERIAL
 	tristate
 
diff --git a/drivers/gnss/Makefile b/drivers/gnss/Makefile
index 451f11401ecc..f5afc2c22a3b 100644
--- a/drivers/gnss/Makefile
+++ b/drivers/gnss/Makefile
@@ -6,6 +6,9 @@
 obj-$(CONFIG_GNSS)			+= gnss.o
 gnss-y := core.o
 
+obj-$(CONFIG_GNSS_MOTMDM)		+= gnss-motmdm.o
+gnss-motmdm-y := motmdm.o
+
 obj-$(CONFIG_GNSS_SERIAL)		+= gnss-serial.o
 gnss-serial-y := serial.o
 
diff --git a/drivers/gnss/motmdm.c b/drivers/gnss/motmdm.c
new file mode 100644
index 000000000000..00cddddab10b
--- /dev/null
+++ b/drivers/gnss/motmdm.c
@@ -0,0 +1,406 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Motorola Modem TS 27.010 serdev GNSS driver
+ *
+ * Copyright (C) 2018 - 2020 Tony Lindgren <tony@xxxxxxxxxxx>
+ * Copyright (C) 2020 - 2021 Pavel Machek <pavel@xxxxxx>
+ *
+ * Based on drivers/gnss/sirf.c driver example:
+ * Copyright (C) 2018 Johan Hovold <johan@xxxxxxxxxx>
+ */
+
+#include <linux/errno.h>
+#include <linux/gnss.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/serdev.h>
+
+#define MOTMDM_GNSS_TIMEOUT	1000
+#define MOTMDM_GNSS_RATE	1000
+
+/*
+ * Motorola MDM GNSS device communicates over a dedicated TS 27.010 channel
+ * using custom data packets. The packets look like AT commands embedded into
+ * a Motorola invented packet using format like "U1234AT+MPDSTART=0,1,100,0".
+ * But it's not an AT compatible serial interface, it's a packet interface
+ * using AT like commands.
+ */
+#define MOTMDM_GNSS_HEADER_LEN	5				/* U1234 */
+#define MOTMDM_GNSS_RESP_LEN	(MOTMDM_GNSS_HEADER_LEN + 4)	/* U1234+MPD */
+#define MOTMDM_GNSS_DATA_LEN	(MOTMDM_GNSS_RESP_LEN + 1)	/* U1234~+MPD */
+#define MOTMDM_GNSS_STATUS_LEN	(MOTMDM_GNSS_DATA_LEN + 7) /* U1234~+MPDSTATUS= */
+#define MOTMDM_GNSS_NMEA_LEN	(MOTMDM_GNSS_DATA_LEN + 8) /* U1234~+MPDNMEA=NN, */
+
+enum motmdm_gnss_status {
+	MOTMDM_GNSS_UNKNOWN,
+	MOTMDM_GNSS_INITIALIZED,
+	MOTMDM_GNSS_DATA_OR_TIMEOUT,
+	MOTMDM_GNSS_STARTED,
+	MOTMDM_GNSS_STOPPED,
+};
+
+struct motmdm_gnss_data {
+	struct gnss_device *gdev;
+	struct device *modem;
+	struct serdev_device *serdev;
+	struct delayed_work restart_work;
+	struct mutex mutex;	/* For modem commands */
+	ktime_t last_update;
+	int status;
+	unsigned char *buf;
+	size_t len;
+	wait_queue_head_t read_queue;
+	unsigned int parsed:1;
+};
+
+static unsigned int rate_ms = MOTMDM_GNSS_RATE;
+module_param(rate_ms, uint, 0644);
+MODULE_PARM_DESC(rate_ms, "GNSS refresh rate between 1000 and 16000 ms (default 1000 ms)");
+
+/*
+ * Note that multiple commands can be sent in series with responses coming
+ * out-of-order. For GNSS, we don't need to care about the out-of-order
+ * responses, and can assume we have at most one command active at a time.
+ * For the commands, can use just a jiffies base packet ID and let the modem
+ * sort out the ID conflicts with the modem's unsolicited message ID
+ * numbering.
+ */
+int motmdm_gnss_send_command(struct motmdm_gnss_data *ddata,
+			     const u8 *buf, int len)
+{
+	struct gnss_device *gdev = ddata->gdev;
+	const int timeout_ms = 1000;
+	unsigned char cmd[128];
+	int ret, cmdlen;
+
+	cmdlen = len + MOTMDM_GNSS_HEADER_LEN + 1;
+	if (cmdlen > 128)
+		return -EINVAL;
+
+	mutex_lock(&ddata->mutex);
+	memset(ddata->buf, 0, ddata->len);
+	ddata->parsed = false;
+	snprintf(cmd, cmdlen, "U%04li%s", jiffies % 10000, buf);
+
+	ret = serdev_device_write(ddata->serdev, cmd, cmdlen, MAX_SCHEDULE_TIMEOUT);
+	if (ret < 0)
+		goto out_unlock;
+
+	serdev_device_wait_until_sent(ddata->serdev, 0);
+
+	ret = wait_event_timeout(ddata->read_queue, ddata->parsed,
+				 msecs_to_jiffies(timeout_ms));
+	if (ret == 0) {
+		ret = -ETIMEDOUT;
+		goto out_unlock;
+	} else if (ret < 0) {
+		goto out_unlock;
+	}
+
+	if (!strstr(ddata->buf, ":OK")) {
+		dev_err(&gdev->dev, "command %s error %s\n",
+			cmd, ddata->buf);
+		ret = -EPIPE;
+	}
+
+	ret = len;
+
+out_unlock:
+	mutex_unlock(&ddata->mutex);
+
+	return ret;
+}
+
+/*
+ * Android uses AT+MPDSTART=0,1,100,0 which starts GNSS for a while,
+ * and then GNSS needs to be kicked with an AT command based on a
+ * status message.
+ */
+static void motmdm_gnss_restart(struct work_struct *work)
+{
+	struct motmdm_gnss_data *ddata =
+		container_of(work, struct motmdm_gnss_data,
+			     restart_work.work);
+	struct gnss_device *gdev = ddata->gdev;
+	const unsigned char *cmd = "AT+MPDSTART=0,1,100,0";
+	int error;
+
+	ddata->last_update = ktime_get();
+
+	error = motmdm_gnss_send_command(ddata, cmd, strlen(cmd));
+	if (error < 0) {
+		/* Timeouts can happen, don't warn and try again */
+		if (error != -ETIMEDOUT)
+			dev_warn(&gdev->dev, "%s: could not start: %i\n",
+				 __func__, error);
+
+		schedule_delayed_work(&ddata->restart_work,
+				      msecs_to_jiffies(MOTMDM_GNSS_RATE));
+	}
+}
+
+static void motmdm_gnss_start(struct gnss_device *gdev, int delay_ms)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+	ktime_t now, next, delta;
+	int next_ms;
+
+	now = ktime_get();
+	next = ktime_add_ms(ddata->last_update, delay_ms);
+	delta = ktime_sub(next, now);
+	next_ms = ktime_to_ms(delta);
+
+	if (next_ms < 0)
+		next_ms = 0;
+	if (next_ms > delay_ms)
+		next_ms = delay_ms;
+
+	schedule_delayed_work(&ddata->restart_work, msecs_to_jiffies(next_ms));
+}
+
+static int motmdm_gnss_stop(struct gnss_device *gdev)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+	const unsigned char *cmd = "AT+MPDSTOP";
+
+	cancel_delayed_work_sync(&ddata->restart_work);
+
+	return motmdm_gnss_send_command(ddata, cmd, strlen(cmd));
+}
+
+static int motmdm_gnss_init(struct gnss_device *gdev)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+	const unsigned char *cmd = "AT+MPDINIT=1";
+	int error;
+
+	error = motmdm_gnss_send_command(ddata, cmd, strlen(cmd));
+	if (error < 0)
+		return error;
+
+	motmdm_gnss_start(gdev, 0);
+
+	return 0;
+}
+
+static int motmdm_gnss_finish(struct gnss_device *gdev)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+	const unsigned char *cmd = "AT+MPDINIT=0";
+	int error;
+
+	error = motmdm_gnss_stop(gdev);
+	if (error < 0)
+		return error;
+
+	return motmdm_gnss_send_command(ddata, cmd, strlen(cmd));
+}
+
+static int motmdm_gnss_receive_data(struct serdev_device *serdev,
+					const unsigned char *buf, size_t len)
+{
+	struct motmdm_gnss_data *ddata = serdev_device_get_drvdata(serdev);
+	struct gnss_device *gdev = ddata->gdev;
+	const unsigned char *msg;
+	size_t msglen;
+	int ret = 0;
+
+	if (len <= MOTMDM_GNSS_RESP_LEN)
+		return 0;
+
+	/* Handle U1234+MPD style command response */
+	if (buf[MOTMDM_GNSS_HEADER_LEN] != '~') {
+		msg = buf + MOTMDM_GNSS_RESP_LEN;
+		strncpy(ddata->buf, msg, len - MOTMDM_GNSS_RESP_LEN);
+		ddata->parsed = true;
+		wake_up(&ddata->read_queue);
+
+		return len;
+	}
+
+	if (len <= MOTMDM_GNSS_DATA_LEN)
+		return 0;
+
+	/* Handle U1234~+MPD style unsolicted message */
+	switch (buf[MOTMDM_GNSS_DATA_LEN]) {
+	case 'N':	/* UNNNN~+MPDNMEA=NN, */
+		msg = buf + MOTMDM_GNSS_NMEA_LEN;
+		msglen = len - MOTMDM_GNSS_NMEA_LEN;
+
+		/*
+		 * Firmware bug: Strip out extra duplicate line break always
+		 * in the data
+		 */
+		msglen--;
+
+		/*
+		 * Firmware bug: Strip out extra data based on an
+		 * earlier line break in the data
+		 */
+		if (msg[msglen - 5 - 1] == 0x0a)
+			msglen -= 5;
+
+		ret = gnss_insert_raw(gdev, msg, msglen);
+		WARN_ON(ret != msglen);
+		break;
+	case 'S':	/* UNNNN~+MPDSTATUS=N,NN */
+		msg = buf + MOTMDM_GNSS_STATUS_LEN;
+		msglen = len - MOTMDM_GNSS_STATUS_LEN;
+
+		switch (msg[0]) {
+		case '1':
+			ddata->status = MOTMDM_GNSS_INITIALIZED;
+			break;
+		case '2':
+			ddata->status = MOTMDM_GNSS_DATA_OR_TIMEOUT;
+			if (rate_ms < MOTMDM_GNSS_RATE)
+				rate_ms = MOTMDM_GNSS_RATE;
+			if (rate_ms > 16 * MOTMDM_GNSS_RATE)
+				rate_ms = 16 * MOTMDM_GNSS_RATE;
+			motmdm_gnss_start(gdev, rate_ms);
+			break;
+		case '3':
+			ddata->status = MOTMDM_GNSS_STARTED;
+			break;
+		case '4':
+			ddata->status = MOTMDM_GNSS_STOPPED;
+			break;
+		default:
+			ddata->status = MOTMDM_GNSS_UNKNOWN;
+			break;
+		}
+		break;
+	case 'X':	/* UNNNN~+MPDXREQ=N for updated xtra2.bin needed */
+	default:
+		break;
+	}
+
+	return len;
+}
+
+static const struct serdev_device_ops gnss_serdev_ops = {
+	.receive_buf    = motmdm_gnss_receive_data,
+	.write_wakeup   = serdev_device_write_wakeup,
+};
+
+static int motmdm_gnss_open(struct gnss_device *gdev)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+	int error;
+
+	serdev_device_set_client_ops(ddata->serdev, &gnss_serdev_ops);
+
+	error = serdev_device_open(ddata->serdev);
+	if (error)
+		return error;
+
+	error = motmdm_gnss_init(gdev);
+	if (error) {
+		serdev_device_close(ddata->serdev);
+		return error;
+	}
+
+	return 0;
+}
+
+static void motmdm_gnss_close(struct gnss_device *gdev)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+	int error;
+
+	error = motmdm_gnss_finish(gdev);
+	if (error < 0)
+		dev_warn(&gdev->dev, "%s: close failed: %i\n",
+			 __func__, error);
+
+	serdev_device_close(ddata->serdev);
+}
+
+static int motmdm_gnss_write_raw(struct gnss_device *gdev,
+				 const unsigned char *buf,
+				 size_t count)
+{
+	struct motmdm_gnss_data *ddata = gnss_get_drvdata(gdev);
+
+	return serdev_device_write(ddata->serdev, buf, count, MAX_SCHEDULE_TIMEOUT);
+}
+
+static const struct gnss_operations motmdm_gnss_ops = {
+	.open		= motmdm_gnss_open,
+	.close		= motmdm_gnss_close,
+	.write_raw	= motmdm_gnss_write_raw,
+};
+
+static int motmdm_gnss_probe(struct serdev_device *serdev)
+{
+	struct device *dev = &serdev->dev;
+	struct motmdm_gnss_data *ddata;
+	struct gnss_device *gdev;
+	int ret;
+
+	ddata = devm_kzalloc(dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	ddata->serdev = serdev;
+	ddata->modem = dev->parent;
+	ddata->len = PAGE_SIZE;
+	mutex_init(&ddata->mutex);
+	INIT_DELAYED_WORK(&ddata->restart_work, motmdm_gnss_restart);
+	init_waitqueue_head(&ddata->read_queue);
+
+	ddata->buf = devm_kzalloc(dev, ddata->len, GFP_KERNEL);
+	if (!ddata->buf)
+		return -ENOMEM;
+
+	serdev_device_set_drvdata(serdev, ddata);
+
+	gdev = gnss_allocate_device(dev);
+	if (!gdev)
+		return -ENOMEM;
+
+	gdev->type = GNSS_TYPE_NMEA;
+	gdev->ops = &motmdm_gnss_ops;
+	gnss_set_drvdata(gdev, ddata);
+	ddata->gdev = gdev;
+
+	ret = gnss_register_device(gdev);
+	if (ret)
+		gnss_put_device(ddata->gdev);
+
+	return ret;
+}
+
+static void motmdm_gnss_remove(struct serdev_device *serdev)
+{
+	struct motmdm_gnss_data *data = serdev_device_get_drvdata(serdev);
+
+	gnss_deregister_device(data->gdev);
+	gnss_put_device(data->gdev);
+};
+
+#ifdef CONFIG_OF
+static const struct of_device_id motmdm_gnss_of_match[] = {
+	{ .compatible = "motorola,mapphone-mdm6600-gnss" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, motmdm_gnss_of_match);
+#endif
+
+static struct serdev_device_driver motmdm_gnss_driver = {
+	.driver	= {
+		.name		= "gnss-mot-mdm6600",
+		.of_match_table	= of_match_ptr(motmdm_gnss_of_match),
+	},
+	.probe	= motmdm_gnss_probe,
+	.remove	= motmdm_gnss_remove,
+};
+module_serdev_device_driver(motmdm_gnss_driver);
+
+MODULE_AUTHOR("Tony Lindgren <tony@xxxxxxxxxxx>");
+MODULE_DESCRIPTION("Motorola Mapphone MDM6600 GNSS receiver driver");
+MODULE_LICENSE("GPL v2");


-- 
http://www.livejournal.com/~pavelmachek

Attachment: signature.asc
Description: PGP signature