diff options
Diffstat (limited to 'drivers/aiodev/st_gyro.c')
-rw-r--r-- | drivers/aiodev/st_gyro.c | 123 |
1 files changed, 123 insertions, 0 deletions
diff --git a/drivers/aiodev/st_gyro.c b/drivers/aiodev/st_gyro.c new file mode 100644 index 0000000000..3a8b6a7761 --- /dev/null +++ b/drivers/aiodev/st_gyro.c @@ -0,0 +1,123 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +// SPDX-FileCopyrightText: 2022 Ahmad Fatoum + +#include <common.h> +#include <driver.h> +#include <xfuncs.h> +#include <spi/spi.h> +#include <aiodev.h> + +#define ST_GYRO_WHO_AM_I 0x0F +#define ST_GYRO_CTRL_REG1 0x20 + +#define ST_GYRO_DEFAULT_OUT_TEMP_ADDR 0x26 +#define ST_GYRO_DEFAULT_OUT_X_L_ADDR 0x28 +#define ST_GYRO_DEFAULT_OUT_Y_L_ADDR 0x2a +#define ST_GYRO_DEFAULT_OUT_Z_L_ADDR 0x2c + +#define ST_GYRO_OUT_L_ADDR(idx) \ + (ST_GYRO_DEFAULT_OUT_X_L_ADDR + 2 * (idx)) + +#define ST_GYRO_OUT_H_ADDR(idx) \ + (ST_GYRO_OUT_L_ADDR(idx) + 1) + +#define ST_GYRO_READ 0x80 +#define ST_GYRO_WRITE 0x00 +#define ST_GYRO_MULTI 0x40 + +struct st_gyro { + struct aiodevice aiodev; + struct aiochannel aiochan[4]; + struct spi_device *spi; +}; +#define to_st_gyro(chan) container_of(chan->aiodev, struct st_gyro, aiodev) + +static int st_gyro_read(struct aiochannel *chan, int *val) +{ + struct st_gyro *gyro = to_st_gyro(chan); + int ret; + u8 tx; + u8 rx_h, rx_l; + + if (chan->index == 3) { + tx = ST_GYRO_DEFAULT_OUT_TEMP_ADDR | ST_GYRO_READ; + ret = spi_write_then_read(gyro->spi, &tx, 1, &rx_l, 1); + if (ret) + return ret; + + *val = (s8)rx_l; + return 0; + } + + tx = ST_GYRO_OUT_H_ADDR(chan->index) | ST_GYRO_READ; + ret = spi_write_then_read(gyro->spi, &tx, 1, &rx_h, 1); + if (ret) + return ret; + + tx = ST_GYRO_OUT_L_ADDR(chan->index) | ST_GYRO_READ; + ret = spi_write_then_read(gyro->spi, &tx, 1, &rx_l, 1); + if (ret) + return ret; + + *val = (s16)((rx_h << 8) | rx_l); + *val *= 250; + *val >>= 16; + + return 0; +} + +static int st_gyro_probe(struct device *dev) +{ + u8 tx[2], rx[2]; + struct st_gyro *gyro; + int ret, i; + + gyro = xzalloc(sizeof(*gyro)); + gyro->spi = to_spi_device(dev); + + tx[0] = ST_GYRO_WHO_AM_I | ST_GYRO_READ; + ret = spi_write_then_read(gyro->spi, tx, 1, rx, 1); + if (ret) + return -EIO; + if (rx[0] != 0xD4) + return dev_err_probe(dev, -ENODEV, "unexpected device WAI: %02x\n", rx[0]); + + /* initialize device */ + tx[0] = ST_GYRO_CTRL_REG1 | ST_GYRO_WRITE; + tx[1] = 0xF; /* normal mode, 3 channels */ + ret = spi_write(gyro->spi, tx, 2); + if (ret) + return -EIO; + + gyro->aiodev.num_channels = 4; + gyro->aiodev.hwdev = dev; + gyro->aiodev.read = st_gyro_read; + gyro->aiodev.name = "gyroscope"; + gyro->aiodev.channels = + xmalloc(gyro->aiodev.num_channels * + sizeof(gyro->aiodev.channels[0])); + for (i = 0; i < 3; i++) { + gyro->aiodev.channels[i] = &gyro->aiochan[i]; + gyro->aiochan[i].unit = "dps"; + gyro->aiochan[i].index = i; + } + + gyro->aiodev.channels[3] = &gyro->aiochan[3]; + gyro->aiochan[3].unit = "C"; + gyro->aiochan[3].index = 3; + + return aiodevice_register(&gyro->aiodev); +} + +static const struct of_device_id st_gyro_match[] = { + { .compatible = "st,l3gd20-gyro" }, + { /* sentinel */ } +}; +MODULE_DEVICE_TABLE(of, st_gyro_match); + +static struct driver st_gyro_driver = { + .name = "st_gyro", + .probe = st_gyro_probe, + .of_compatible = st_gyro_match, +}; +device_spi_driver(st_gyro_driver); |