From b4b7ff4b08e691656c9d77c758fc355833128ac0 Mon Sep 17 00:00:00 2001 From: AndrĂ© Fabian Silva Delgado Date: Wed, 20 Jan 2016 14:01:31 -0300 Subject: Linux-libre 4.4-gnu --- drivers/iio/proximity/pulsedlight-lidar-lite-v2.c | 289 ++++++++++++++++++++++ 1 file changed, 289 insertions(+) create mode 100644 drivers/iio/proximity/pulsedlight-lidar-lite-v2.c (limited to 'drivers/iio/proximity/pulsedlight-lidar-lite-v2.c') diff --git a/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c b/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c new file mode 100644 index 000000000..e544fcfd5 --- /dev/null +++ b/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c @@ -0,0 +1,289 @@ +/* + * pulsedlight-lidar-lite-v2.c - Support for PulsedLight LIDAR sensor + * + * Copyright (C) 2015 Matt Ranostay + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * TODO: runtime pm, interrupt mode, and signal strength reporting + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define LIDAR_REG_CONTROL 0x00 +#define LIDAR_REG_CONTROL_ACQUIRE BIT(2) + +#define LIDAR_REG_STATUS 0x01 +#define LIDAR_REG_STATUS_INVALID BIT(3) +#define LIDAR_REG_STATUS_READY BIT(0) + +#define LIDAR_REG_DATA_HBYTE 0x0f +#define LIDAR_REG_DATA_LBYTE 0x10 + +#define LIDAR_DRV_NAME "lidar" + +struct lidar_data { + struct iio_dev *indio_dev; + struct i2c_client *client; + + u16 buffer[8]; /* 2 byte distance + 8 byte timestamp */ +}; + +static const struct iio_chan_spec lidar_channels[] = { + { + .type = IIO_DISTANCE, + .info_mask_separate = + BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), + .scan_index = 0, + .scan_type = { + .sign = 'u', + .realbits = 16, + .storagebits = 16, + }, + }, + IIO_CHAN_SOFT_TIMESTAMP(1), +}; + +static int lidar_read_byte(struct lidar_data *data, int reg) +{ + struct i2c_client *client = data->client; + int ret; + + /* + * Device needs a STOP condition between address write, and data read + * so in turn i2c_smbus_read_byte_data cannot be used + */ + + ret = i2c_smbus_write_byte(client, reg); + if (ret < 0) { + dev_err(&client->dev, "cannot write addr value"); + return ret; + } + + ret = i2c_smbus_read_byte(client); + if (ret < 0) + dev_err(&client->dev, "cannot read data value"); + + return ret; +} + +static inline int lidar_write_control(struct lidar_data *data, int val) +{ + return i2c_smbus_write_byte_data(data->client, LIDAR_REG_CONTROL, val); +} + +static int lidar_read_measurement(struct lidar_data *data, u16 *reg) +{ + int ret; + int val; + + ret = lidar_read_byte(data, LIDAR_REG_DATA_HBYTE); + if (ret < 0) + return ret; + val = ret << 8; + + ret = lidar_read_byte(data, LIDAR_REG_DATA_LBYTE); + if (ret < 0) + return ret; + + val |= ret; + *reg = val; + + return 0; +} + +static int lidar_get_measurement(struct lidar_data *data, u16 *reg) +{ + struct i2c_client *client = data->client; + int tries = 10; + int ret; + + /* start sample */ + ret = lidar_write_control(data, LIDAR_REG_CONTROL_ACQUIRE); + if (ret < 0) { + dev_err(&client->dev, "cannot send start measurement command"); + return ret; + } + + while (tries--) { + usleep_range(1000, 2000); + + ret = lidar_read_byte(data, LIDAR_REG_STATUS); + if (ret < 0) + break; + + /* return -EINVAL since laser is likely pointed out of range */ + if (ret & LIDAR_REG_STATUS_INVALID) { + *reg = 0; + ret = -EINVAL; + break; + } + + /* sample ready to read */ + if (!(ret & LIDAR_REG_STATUS_READY)) { + ret = lidar_read_measurement(data, reg); + break; + } + ret = -EIO; + } + + return ret; +} + +static int lidar_read_raw(struct iio_dev *indio_dev, + struct iio_chan_spec const *chan, + int *val, int *val2, long mask) +{ + struct lidar_data *data = iio_priv(indio_dev); + int ret = -EINVAL; + + mutex_lock(&indio_dev->mlock); + + if (iio_buffer_enabled(indio_dev) && mask == IIO_CHAN_INFO_RAW) { + ret = -EBUSY; + goto error_busy; + } + + switch (mask) { + case IIO_CHAN_INFO_RAW: { + u16 reg; + + ret = lidar_get_measurement(data, ®); + if (!ret) { + *val = reg; + ret = IIO_VAL_INT; + } + break; + } + case IIO_CHAN_INFO_SCALE: + *val = 0; + *val2 = 10000; + ret = IIO_VAL_INT_PLUS_MICRO; + break; + } + +error_busy: + mutex_unlock(&indio_dev->mlock); + + return ret; +} + +static irqreturn_t lidar_trigger_handler(int irq, void *private) +{ + struct iio_poll_func *pf = private; + struct iio_dev *indio_dev = pf->indio_dev; + struct lidar_data *data = iio_priv(indio_dev); + int ret; + + ret = lidar_get_measurement(data, data->buffer); + if (!ret) { + iio_push_to_buffers_with_timestamp(indio_dev, data->buffer, + iio_get_time_ns()); + } else if (ret != -EINVAL) { + dev_err(&data->client->dev, "cannot read LIDAR measurement"); + } + + iio_trigger_notify_done(indio_dev->trig); + + return IRQ_HANDLED; +} + +static const struct iio_info lidar_info = { + .driver_module = THIS_MODULE, + .read_raw = lidar_read_raw, +}; + +static int lidar_probe(struct i2c_client *client, + const struct i2c_device_id *id) +{ + struct lidar_data *data; + struct iio_dev *indio_dev; + int ret; + + indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data)); + if (!indio_dev) + return -ENOMEM; + + indio_dev->info = &lidar_info; + indio_dev->name = LIDAR_DRV_NAME; + indio_dev->channels = lidar_channels; + indio_dev->num_channels = ARRAY_SIZE(lidar_channels); + indio_dev->modes = INDIO_DIRECT_MODE; + + data = iio_priv(indio_dev); + i2c_set_clientdata(client, indio_dev); + + data->client = client; + data->indio_dev = indio_dev; + + ret = iio_triggered_buffer_setup(indio_dev, NULL, + lidar_trigger_handler, NULL); + if (ret) + return ret; + + ret = iio_device_register(indio_dev); + if (ret) + goto error_unreg_buffer; + + return 0; + +error_unreg_buffer: + iio_triggered_buffer_cleanup(indio_dev); + + return ret; +} + +static int lidar_remove(struct i2c_client *client) +{ + struct iio_dev *indio_dev = i2c_get_clientdata(client); + + iio_device_unregister(indio_dev); + iio_triggered_buffer_cleanup(indio_dev); + + return 0; +} + +static const struct i2c_device_id lidar_id[] = { + {"lidar-lite-v2", 0}, + { }, +}; +MODULE_DEVICE_TABLE(i2c, lidar_id); + +static const struct of_device_id lidar_dt_ids[] = { + { .compatible = "pulsedlight,lidar-lite-v2" }, + { } +}; +MODULE_DEVICE_TABLE(of, lidar_dt_ids); + +static struct i2c_driver lidar_driver = { + .driver = { + .name = LIDAR_DRV_NAME, + .of_match_table = of_match_ptr(lidar_dt_ids), + }, + .probe = lidar_probe, + .remove = lidar_remove, + .id_table = lidar_id, +}; +module_i2c_driver(lidar_driver); + +MODULE_AUTHOR("Matt Ranostay "); +MODULE_DESCRIPTION("PulsedLight LIDAR sensor"); +MODULE_LICENSE("GPL"); -- cgit v1.2.3-54-g00ecf