Hi! > Motorola is using a custom TS 27.010 based serial port line discipline > 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 <pa...@ucw.cz>
20 days... ping...?
Pavel
> diff --git a/drivers/gnss/Kconfig b/drivers/gnss/Kconfig
> index bd12e3d57baa..a7c449d8428c 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_N_GSM
> + help
> + Say Y here if you have a Motorola modem using TS 27.010 line
> + discipline 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..bfd50e130631
> --- /dev/null
> +++ b/drivers/gnss/motmdm.c
> @@ -0,0 +1,409 @@
> +// SPDX-License-Identifier: GPL-2.0
> +/*
> + * Motorola Modem TS 27.010 serdev GNSS driver
> + *
> + * Copyright (C) 2018 - 2020 Tony Lindgren <t...@atomide.com>
> + * Copyright (C) 2020 - 2021 Pavel Machek <pa...@ucw.cz>
> + *
> + * Based on drivers/gnss/sirf.c driver example:
> + * Copyright (C) 2018 Johan Hovold <jo...@kernel.org>
> + */
> +
> +#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/serdev-gsm.h>
> +#include <linux/slab.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) /*
> STATUS= */
> +#define MOTMDM_GNSS_NMEA_LEN (MOTMDM_GNSS_DATA_LEN + 8) /* NMEA=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 error = 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;
> +
> + error = gnss_insert_raw(gdev, msg, msglen);
> + WARN_ON(error);
> + 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) {
> + 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);
> + serdev_device_wait_until_sent(ddata->serdev, 0);
> +
> + return count;
> +}
> +
> +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 <t...@atomide.com>");
> +MODULE_DESCRIPTION("Motorola Mapphone MDM6600 GNSS receiver driver");
> +MODULE_LICENSE("GPL v2");
>
>
> --
> http://www.livejournal.com/~pavelmachek
--
http://www.livejournal.com/~pavelmachek
signature.asc
Description: PGP signature
