drivers:iio:stm:magnetometer: add support to ST MEMS MAG 3D
This driver adds support to the family of magnetometric sensors having the common whoami 0x3d. The following devices belong to this family: LIS3MDL - 3 axis Magnetometer LSM9DS1 - 6 axis IMU + 3 axis Magnetometer Tested on Kernel <= 5.4 and 5.10 where trigger ops structure differs. Signed-off-by: Mario Tesi <mario.tesi@st.com> Change-Id: If7d7b82333592fdb5a888bf53dfafc8382cce9a6 Reviewed-on: https://gerrit.st.com/c/linuxandroidopen/stm-ldd-iio/+/265990 Reviewed-by: Denis CIOCCA <denis.ciocca@st.com> Tested-by: CITOOLS <MDG-smet-aci-reviews@list.st.com>
This commit is contained in:
Mario Tesi
Denis CIOCCA
parent
106dbf45da
commit
6efdd65453
@ -29,3 +29,13 @@ lis3mdl-magn@0 {
|
||||
interrupt-parent = <&gpio0>;
|
||||
interrupts = <0 IRQ_TYPE_LEVEL_HIGH>;
|
||||
};
|
||||
|
||||
Example for an i2c device node:
|
||||
|
||||
lis3mdl-magn@0x1e {
|
||||
compatible = "st,lis3mdl_magn";
|
||||
reg = <0x1e>;
|
||||
interrupt-parent = <&gpio0>;
|
||||
interrupts = <0 IRQ_TYPE_LEVEL_HIGH>;
|
||||
};
|
||||
|
||||
|
||||
@ -16,6 +16,7 @@
|
||||
#include <linux/iio/trigger.h>
|
||||
#include <linux/iio/trigger_consumer.h>
|
||||
#include <linux/iio/triggered_buffer.h>
|
||||
#include <linux/version.h>
|
||||
|
||||
#include "st_imu68.h"
|
||||
|
||||
@ -141,6 +142,10 @@ static int st_imu68_buffer_postdisable(struct iio_dev *iio_dev)
|
||||
|
||||
static const struct iio_buffer_setup_ops st_imu68_buffer_setup_ops = {
|
||||
.preenable = st_imu68_buffer_preenable,
|
||||
#if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0)
|
||||
.postenable = iio_triggered_buffer_postenable,
|
||||
.predisable = iio_triggered_buffer_predisable,
|
||||
#endif /* LINUX_VERSION_CODE */
|
||||
.postdisable = st_imu68_buffer_postdisable,
|
||||
};
|
||||
|
||||
|
||||
@ -6,6 +6,30 @@
|
||||
|
||||
menu "Magnetometer sensors"
|
||||
|
||||
config ST_MAG3D_IIO
|
||||
tristate "STMicroelectronics LIS3MDL/LSM9DS1 mag sensor"
|
||||
depends on (I2C || SPI) && SYSFS
|
||||
select IIO_BUFFER
|
||||
select IIO_TRIGGERED_BUFFER
|
||||
select ST_MAG3D_I2C_IIO if (I2C)
|
||||
select ST_MAG3D_SPI_IIO if (SPI)
|
||||
help
|
||||
Say yes here to build support for STMicroelectronics magnetometers:
|
||||
LIS3MDL, LSM9DS1 mag sensor
|
||||
|
||||
To compile this driver as a module, choose M here. The module
|
||||
will be called st_mag3d
|
||||
|
||||
config ST_MAG3D_I2C_IIO
|
||||
tristate
|
||||
depends on ST_MAG3D_IIO
|
||||
depends on I2C
|
||||
|
||||
config ST_MAG3D_SPI_IIO
|
||||
tristate
|
||||
depends on ST_MAG3D_IIO
|
||||
depends on SPI
|
||||
|
||||
config ST_MAG40_IIO
|
||||
tristate "STMicroelectronics LIS2MDL/LSM303AH/LSM303AGR/ISM303DAC/IIS2MDC sensor"
|
||||
depends on (I2C || SPI) && SYSFS
|
||||
|
||||
@ -4,6 +4,11 @@
|
||||
#
|
||||
|
||||
# When adding new entries keep the list in alphabetical order
|
||||
st_mag3d-y += st_mag3d_core.o st_mag3d_buffer.o
|
||||
obj-$(CONFIG_ST_MAG3D_IIO) += st_mag3d.o
|
||||
obj-$(CONFIG_ST_MAG3D_I2C_IIO) += st_mag3d_i2c.o
|
||||
obj-$(CONFIG_ST_MAG3D_SPI_IIO) += st_mag3d_spi.o
|
||||
|
||||
st_mag40-y += st_mag40_buffer.o st_mag40_core.o
|
||||
obj-$(CONFIG_ST_MAG40_IIO) += st_mag40.o
|
||||
obj-$(CONFIG_ST_MAG40_I2C_IIO) += st_mag40_i2c.o
|
||||
|
||||
72
drivers/iio/stm/magnetometer/st_mag3d.h
Normal file
72
drivers/iio/stm/magnetometer/st_mag3d.h
Normal file
@ -0,0 +1,72 @@
|
||||
/* SPDX-License-Identifier: GPL-2.0-only */
|
||||
/*
|
||||
* STMicroelectronics st_mag3d driver
|
||||
*
|
||||
* MEMS Software Solutions Team
|
||||
*
|
||||
* Copyright 2016 STMicroelectronics Inc.
|
||||
*/
|
||||
|
||||
#ifndef __ST_MAG3D_H
|
||||
#define __ST_MAG3D_H
|
||||
|
||||
#include <linux/iio/iio.h>
|
||||
|
||||
#define LIS3MDL_DEV_NAME "lis3mdl_magn"
|
||||
#define LSM9DS1_DEV_NAME "lsm9ds1_magn"
|
||||
|
||||
#define ST_MAG3D_TX_MAX_LENGTH 16
|
||||
#define ST_MAG3D_RX_MAX_LENGTH 16
|
||||
|
||||
#define ST_MAG3D_SAMPLE_SIZE 6
|
||||
|
||||
#define ST_MAG3D_EWMA_DIV 128
|
||||
#define ST_MAG3D_EWMA_WEIGHT 96
|
||||
|
||||
struct iio_dev;
|
||||
|
||||
struct st_mag3d_transfer_buffer {
|
||||
u8 rx_buf[ST_MAG3D_RX_MAX_LENGTH];
|
||||
u8 tx_buf[ST_MAG3D_TX_MAX_LENGTH] ____cacheline_aligned;
|
||||
};
|
||||
|
||||
struct st_mag3d_transfer_function {
|
||||
int (*write)(struct device *dev, u8 addr, int len, u8 *data);
|
||||
int (*read)(struct device *dev, u8 addr, int len, u8 *data);
|
||||
};
|
||||
|
||||
struct st_mag3d_hw {
|
||||
struct device *dev;
|
||||
struct mutex lock;
|
||||
u8 buffer[ALIGN(ST_MAG3D_SAMPLE_SIZE, sizeof(s64)) + sizeof(s64)];
|
||||
u16 odr;
|
||||
u16 gain;
|
||||
u8 stodis;
|
||||
|
||||
s64 timestamp;
|
||||
s64 delta_ts;
|
||||
s64 mag_ts;
|
||||
|
||||
struct iio_trigger *trig;
|
||||
int irq;
|
||||
|
||||
const struct st_mag3d_transfer_function *tf;
|
||||
struct st_mag3d_transfer_buffer tb;
|
||||
struct iio_dev *iio_dev;
|
||||
};
|
||||
|
||||
static inline s64 st_mag3d_get_time_ns(struct iio_dev *iio_dev)
|
||||
{
|
||||
return iio_get_time_ns(iio_dev);
|
||||
}
|
||||
|
||||
int st_mag3d_probe(struct device *dev, int irq, const char *name,
|
||||
const struct st_mag3d_transfer_function *tf_ops);
|
||||
void st_mag3d_remove(struct iio_dev *iio_dev);
|
||||
int st_mag3d_allocate_buffer(struct iio_dev *iio_dev);
|
||||
void st_mag3d_deallocate_buffer(struct iio_dev *iio_dev);
|
||||
int st_mag3d_allocate_trigger(struct iio_dev *iio_dev);
|
||||
void st_mag3d_deallocate_trigger(struct iio_dev *iio_dev);
|
||||
int st_mag3d_enable_sensor(struct st_mag3d_hw *hw, bool enable);
|
||||
|
||||
#endif /* __ST_MAG3D_H */
|
||||
169
drivers/iio/stm/magnetometer/st_mag3d_buffer.c
Normal file
169
drivers/iio/stm/magnetometer/st_mag3d_buffer.c
Normal file
@ -0,0 +1,169 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* STMicroelectronics st_mag3d buffer library driver
|
||||
*
|
||||
* MEMS Software Solutions Team
|
||||
*
|
||||
* Copyright 2016 STMicroelectronics Inc.
|
||||
*/
|
||||
|
||||
#include <linux/module.h>
|
||||
#include <linux/kernel.h>
|
||||
#include <linux/device.h>
|
||||
#include <linux/iio/iio.h>
|
||||
#include <linux/iio/buffer.h>
|
||||
#include <linux/iio/trigger.h>
|
||||
#include <linux/iio/triggered_buffer.h>
|
||||
#include <linux/iio/trigger_consumer.h>
|
||||
#include <linux/version.h>
|
||||
|
||||
#include "st_mag3d.h"
|
||||
|
||||
#define ST_MAG3D_STATUS_ADDR 0x27
|
||||
#define ST_MAG3D_DA_MASK 0x07
|
||||
|
||||
static const struct iio_trigger_ops st_mag3d_trigger_ops = {
|
||||
NULL
|
||||
};
|
||||
|
||||
static inline s64 st_mag3d_ewma(s64 old, s64 new)
|
||||
{
|
||||
s64 diff, incr;
|
||||
|
||||
diff = new - old;
|
||||
incr = div_s64((ST_MAG3D_EWMA_DIV - ST_MAG3D_EWMA_WEIGHT) * diff,
|
||||
ST_MAG3D_EWMA_DIV);
|
||||
|
||||
return old + incr;
|
||||
}
|
||||
|
||||
static irqreturn_t st_mag3d_trigger_handler_irq(int irq, void *p)
|
||||
{
|
||||
struct st_mag3d_hw *hw = (struct st_mag3d_hw *)p;
|
||||
s64 irq_ts;
|
||||
|
||||
irq_ts = st_mag3d_get_time_ns(hw->iio_dev);
|
||||
|
||||
if (hw->stodis == 0)
|
||||
hw->delta_ts = st_mag3d_ewma(hw->delta_ts,
|
||||
irq_ts - hw->timestamp);
|
||||
|
||||
hw->timestamp = irq_ts;
|
||||
|
||||
return IRQ_WAKE_THREAD;
|
||||
}
|
||||
|
||||
static irqreturn_t st_mag3d_trigger_handler_thread(int irq, void *p)
|
||||
{
|
||||
struct st_mag3d_hw *hw = (struct st_mag3d_hw *)p;
|
||||
struct iio_dev *iio_dev = hw->iio_dev;
|
||||
struct iio_chan_spec const *ch = iio_dev->channels;
|
||||
u8 status;
|
||||
int err;
|
||||
|
||||
err = hw->tf->read(hw->dev, ST_MAG3D_STATUS_ADDR, sizeof(status),
|
||||
&status);
|
||||
if (err < 0)
|
||||
goto out;
|
||||
|
||||
if (!(status & ST_MAG3D_DA_MASK))
|
||||
return IRQ_NONE;
|
||||
|
||||
err = hw->tf->read(hw->dev, ch->address, ST_MAG3D_SAMPLE_SIZE,
|
||||
hw->buffer);
|
||||
if (err < 0)
|
||||
goto out;
|
||||
|
||||
iio_trigger_poll_chained(hw->trig);
|
||||
|
||||
out:
|
||||
return IRQ_HANDLED;
|
||||
}
|
||||
|
||||
int st_mag3d_allocate_trigger(struct iio_dev *iio_dev)
|
||||
{
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
int err;
|
||||
|
||||
err = devm_request_threaded_irq(hw->dev, hw->irq,
|
||||
st_mag3d_trigger_handler_irq,
|
||||
st_mag3d_trigger_handler_thread,
|
||||
IRQF_TRIGGER_HIGH | IRQF_ONESHOT,
|
||||
"st_mag3d", hw);
|
||||
if (err) {
|
||||
dev_err(hw->dev, "failed to request trigger irq %d\n",
|
||||
hw->irq);
|
||||
return err;
|
||||
}
|
||||
|
||||
hw->trig = devm_iio_trigger_alloc(hw->dev, "%s-trigger",
|
||||
iio_dev->name);
|
||||
if (!hw->trig)
|
||||
return -ENOMEM;
|
||||
|
||||
iio_trigger_set_drvdata(hw->trig, iio_dev);
|
||||
hw->trig->ops = &st_mag3d_trigger_ops,
|
||||
hw->trig->dev.parent = hw->dev;
|
||||
iio_dev->trig = iio_trigger_get(hw->trig);
|
||||
|
||||
return iio_trigger_register(hw->trig);
|
||||
}
|
||||
|
||||
void st_mag3d_deallocate_trigger(struct iio_dev *iio_dev)
|
||||
{
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
|
||||
iio_trigger_unregister(hw->trig);
|
||||
}
|
||||
|
||||
static int st_mag3d_buffer_preenable(struct iio_dev *iio_dev)
|
||||
{
|
||||
return st_mag3d_enable_sensor(iio_priv(iio_dev), true);
|
||||
}
|
||||
|
||||
static int st_mag3d_buffer_postdisable(struct iio_dev *iio_dev)
|
||||
{
|
||||
return st_mag3d_enable_sensor(iio_priv(iio_dev), false);
|
||||
}
|
||||
|
||||
static const struct iio_buffer_setup_ops st_mag3d_buffer_ops = {
|
||||
.preenable = st_mag3d_buffer_preenable,
|
||||
#if LINUX_VERSION_CODE < KERNEL_VERSION(5,10,0)
|
||||
.postenable = iio_triggered_buffer_postenable,
|
||||
.predisable = iio_triggered_buffer_predisable,
|
||||
#endif /* LINUX_VERSION_CODE */
|
||||
.postdisable = st_mag3d_buffer_postdisable,
|
||||
};
|
||||
|
||||
static irqreturn_t st_mag3d_buffer_handler_thread(int irq, void *p)
|
||||
{
|
||||
struct iio_poll_func *pf = p;
|
||||
struct st_mag3d_hw *hw = iio_priv(pf->indio_dev);
|
||||
|
||||
if (hw->stodis == 0)
|
||||
iio_push_to_buffers_with_timestamp(pf->indio_dev, hw->buffer,
|
||||
hw->mag_ts);
|
||||
else
|
||||
hw->stodis--;
|
||||
|
||||
hw->mag_ts += hw->delta_ts;
|
||||
iio_trigger_notify_done(hw->trig);
|
||||
|
||||
return IRQ_HANDLED;
|
||||
}
|
||||
|
||||
int st_mag3d_allocate_buffer(struct iio_dev *iio_dev)
|
||||
{
|
||||
return iio_triggered_buffer_setup(iio_dev, NULL,
|
||||
st_mag3d_buffer_handler_thread,
|
||||
&st_mag3d_buffer_ops);
|
||||
}
|
||||
|
||||
void st_mag3d_deallocate_buffer(struct iio_dev *iio_dev)
|
||||
{
|
||||
iio_triggered_buffer_cleanup(iio_dev);
|
||||
}
|
||||
|
||||
MODULE_DESCRIPTION("STMicroelectronics mag3d buffer driver");
|
||||
MODULE_AUTHOR("MEMS Software Solutions Team");
|
||||
MODULE_LICENSE("GPL v2");
|
||||
465
drivers/iio/stm/magnetometer/st_mag3d_core.c
Normal file
465
drivers/iio/stm/magnetometer/st_mag3d_core.c
Normal file
@ -0,0 +1,465 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* STMicroelectronics mag3d driver
|
||||
*
|
||||
* MEMS Software Solutions Team
|
||||
*
|
||||
* Copyright 2016 STMicroelectronics Inc.
|
||||
*/
|
||||
|
||||
#include <linux/kernel.h>
|
||||
#include <linux/module.h>
|
||||
#include <linux/delay.h>
|
||||
#include <linux/iio/iio.h>
|
||||
#include <linux/iio/sysfs.h>
|
||||
|
||||
#include "st_mag3d.h"
|
||||
|
||||
#define ST_MAG3D_WHOAMI_ADDR 0x0f
|
||||
#define ST_MAG3D_DEF_WHOAMI 0x3d
|
||||
|
||||
#define ST_MAG3D_CTRL1_ADDR 0x20
|
||||
#define ST_MAG3D_ODR_MASK 0x1c
|
||||
#define ST_MAG3D_OM_XY_MASK 0x60
|
||||
#define ST_MAG3D_CTRL2_ADDR 0x21
|
||||
#define ST_MAG3D_FS_MASK 0x60
|
||||
#define ST_MAG3D_CTRL3_ADDR 0x22
|
||||
#define ST_MAG3D_PWR_MASK 0x03
|
||||
#define ST_MAG3D_PWR_ON 0x00
|
||||
#define ST_MAG3D_PWR_OFF 0x03
|
||||
#define ST_MAG3D_CTRL4_ADDR 0x23
|
||||
#define ST_MAG3D_OM_Z_MASK 0x0c
|
||||
#define ST_MAG3D_CTRL5_ADDR 0x24
|
||||
#define ST_MAG3D_BDU_MASK 0x40
|
||||
|
||||
#define ST_MAG3D_OUT_X_L_ADDR 0x28
|
||||
#define ST_MAG3D_OUT_Y_L_ADDR 0x2a
|
||||
#define ST_MAG3D_OUT_Z_L_ADDR 0x2c
|
||||
|
||||
struct st_mag3d_odr {
|
||||
unsigned int hz;
|
||||
u8 val;
|
||||
};
|
||||
|
||||
const struct st_mag3d_odr st_mag3d_odr_table[] = {
|
||||
{ 1, 0x01 },
|
||||
{ 3, 0x02 },
|
||||
{ 5, 0x03 },
|
||||
{ 10, 0x04 },
|
||||
{ 20, 0x05 },
|
||||
{ 40, 0x06 },
|
||||
{ 80, 0x07 },
|
||||
};
|
||||
|
||||
const u8 st_mag3d_stodis_table[] = {
|
||||
0, /* 1Hz */
|
||||
1, /* 3Hz */
|
||||
2, /* 5Hz */
|
||||
5, /* 10Hz */
|
||||
6, /* 20Hz */
|
||||
6, /* 40Hz */
|
||||
7, /* 80Hz */
|
||||
};
|
||||
|
||||
struct st_mag3d_fs {
|
||||
unsigned int gain;
|
||||
u8 val;
|
||||
};
|
||||
|
||||
const struct st_mag3d_fs st_mag3d_fs_table[] = {
|
||||
{ 146, 0x0 }, /* 4000 */
|
||||
{ 292, 0x1 }, /* 8000 */
|
||||
{ 438, 0x2 }, /* 12000 */
|
||||
{ 584, 0x3 }, /* 16000 */
|
||||
};
|
||||
|
||||
#define ST_MAG3D_CHANNEL(addr, mod, scan_idx) \
|
||||
{ \
|
||||
.type = IIO_MAGN, \
|
||||
.address = addr, \
|
||||
.modified = 1, \
|
||||
.channel2 = mod, \
|
||||
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
|
||||
BIT(IIO_CHAN_INFO_SCALE), \
|
||||
.scan_index = scan_idx, \
|
||||
.scan_type = { \
|
||||
.sign = 's', \
|
||||
.realbits = 16, \
|
||||
.storagebits = 16, \
|
||||
.endianness = IIO_LE, \
|
||||
}, \
|
||||
}
|
||||
|
||||
static const struct iio_chan_spec st_mag3d_channels[] = {
|
||||
ST_MAG3D_CHANNEL(ST_MAG3D_OUT_X_L_ADDR, IIO_MOD_X, 0),
|
||||
ST_MAG3D_CHANNEL(ST_MAG3D_OUT_Y_L_ADDR, IIO_MOD_Y, 1),
|
||||
ST_MAG3D_CHANNEL(ST_MAG3D_OUT_Z_L_ADDR, IIO_MOD_Z, 2),
|
||||
IIO_CHAN_SOFT_TIMESTAMP(3),
|
||||
};
|
||||
|
||||
static inline int st_mag3d_claim_direct_mode(struct iio_dev *iio_dev)
|
||||
{
|
||||
mutex_lock(&iio_dev->mlock);
|
||||
|
||||
if (iio_buffer_enabled(iio_dev)) {
|
||||
mutex_unlock(&iio_dev->mlock);
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void st_mag3d_release_direct_mode(struct iio_dev *iio_dev)
|
||||
{
|
||||
mutex_unlock(&iio_dev->mlock);
|
||||
}
|
||||
|
||||
static int st_mag3d_write_with_mask(struct st_mag3d_hw *hw, u8 addr,
|
||||
u8 mask, u8 val)
|
||||
{
|
||||
u8 data;
|
||||
int err;
|
||||
|
||||
mutex_lock(&hw->lock);
|
||||
|
||||
err = hw->tf->read(hw->dev, addr, sizeof(data), &data);
|
||||
if (err < 0) {
|
||||
dev_err(hw->dev, "failed to read %02x register\n", addr);
|
||||
goto out;
|
||||
}
|
||||
|
||||
data = (data & ~mask) | ((val << __ffs(mask)) & mask);
|
||||
|
||||
err = hw->tf->write(hw->dev, addr, sizeof(data), &data);
|
||||
if (err < 0)
|
||||
dev_err(hw->dev, "failed to write %02x register\n", addr);
|
||||
|
||||
out:
|
||||
mutex_unlock(&hw->lock);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int st_mag3d_set_odr(struct st_mag3d_hw *hw, unsigned int odr)
|
||||
{
|
||||
int i, err;
|
||||
|
||||
for (i = 0; i < ARRAY_SIZE(st_mag3d_odr_table); i++) {
|
||||
if (odr == st_mag3d_odr_table[i].hz)
|
||||
break;
|
||||
}
|
||||
|
||||
if (i == ARRAY_SIZE(st_mag3d_odr_table))
|
||||
return -EINVAL;
|
||||
|
||||
err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL1_ADDR,
|
||||
ST_MAG3D_ODR_MASK,
|
||||
st_mag3d_odr_table[i].val);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
hw->odr = odr;
|
||||
hw->stodis = st_mag3d_stodis_table[i];
|
||||
hw->delta_ts = div_s64(1000000000LL, odr);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int st_mag3d_set_fullscale(struct st_mag3d_hw *hw, unsigned int gain)
|
||||
{
|
||||
int i, err;
|
||||
|
||||
for (i = 0; i < ARRAY_SIZE(st_mag3d_fs_table); i++) {
|
||||
if (gain == st_mag3d_fs_table[i].gain)
|
||||
break;
|
||||
}
|
||||
|
||||
if (i == ARRAY_SIZE(st_mag3d_fs_table))
|
||||
return -EINVAL;
|
||||
|
||||
err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL2_ADDR,
|
||||
ST_MAG3D_FS_MASK,
|
||||
st_mag3d_fs_table[i].val);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
hw->gain = gain;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int st_mag3d_enable_sensor(struct st_mag3d_hw *hw, bool enable)
|
||||
{
|
||||
u8 val = enable ? ST_MAG3D_PWR_ON : ST_MAG3D_PWR_OFF;
|
||||
int err;
|
||||
|
||||
err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL3_ADDR,
|
||||
ST_MAG3D_PWR_MASK, val);
|
||||
if (enable) {
|
||||
hw->timestamp = st_mag3d_get_time_ns(hw->iio_dev);
|
||||
hw->mag_ts = hw->timestamp;
|
||||
}
|
||||
|
||||
return err < 0 ? err : 0;
|
||||
}
|
||||
|
||||
static int st_mag3d_check_whoami(struct st_mag3d_hw *hw)
|
||||
{
|
||||
int err;
|
||||
u8 data;
|
||||
|
||||
err = hw->tf->read(hw->dev, ST_MAG3D_WHOAMI_ADDR, sizeof(data),
|
||||
&data);
|
||||
if (err < 0) {
|
||||
dev_err(hw->dev, "failed to read whoami register\n");
|
||||
return err;
|
||||
}
|
||||
|
||||
if (data != ST_MAG3D_DEF_WHOAMI) {
|
||||
dev_err(hw->dev, "unsupported whoami [%02x]\n", data);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int st_mag3d_read_oneshot(struct st_mag3d_hw *hw,
|
||||
u8 addr, int *val)
|
||||
{
|
||||
int err, delay;
|
||||
__le16 data;
|
||||
|
||||
err = st_mag3d_enable_sensor(hw, true);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
delay = 1000000 / hw->odr;
|
||||
usleep_range(delay, 2 * delay);
|
||||
|
||||
err = hw->tf->read(hw->dev, addr, sizeof(data), (u8 *)&data);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
err = st_mag3d_enable_sensor(hw, false);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
*val = (s16)data;
|
||||
|
||||
return IIO_VAL_INT;
|
||||
}
|
||||
|
||||
static int st_mag3d_read_raw(struct iio_dev *iio_dev,
|
||||
struct iio_chan_spec const *ch,
|
||||
int *val, int *val2, long mask)
|
||||
{
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
int ret;
|
||||
|
||||
switch (mask) {
|
||||
case IIO_CHAN_INFO_RAW:
|
||||
ret = st_mag3d_claim_direct_mode(iio_dev);
|
||||
if (ret)
|
||||
break;
|
||||
|
||||
ret = st_mag3d_read_oneshot(hw, ch->address, val);
|
||||
st_mag3d_release_direct_mode(iio_dev);
|
||||
break;
|
||||
case IIO_CHAN_INFO_SCALE:
|
||||
*val = 0;
|
||||
*val2 = hw->gain;
|
||||
ret = IIO_VAL_INT_PLUS_MICRO;
|
||||
break;
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int st_mag3d_write_raw(struct iio_dev *iio_dev,
|
||||
struct iio_chan_spec const *chan,
|
||||
int val, int val2, long mask)
|
||||
{
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
int ret;
|
||||
|
||||
ret = st_mag3d_claim_direct_mode(iio_dev);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
||||
switch (mask) {
|
||||
case IIO_CHAN_INFO_SCALE:
|
||||
ret = st_mag3d_set_fullscale(hw, val2);
|
||||
break;
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
}
|
||||
|
||||
st_mag3d_release_direct_mode(iio_dev);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static ssize_t st_mag3d_get_sampling_frequency(struct device *dev,
|
||||
struct device_attribute *attr,
|
||||
char *buf)
|
||||
{
|
||||
struct st_mag3d_hw *hw = iio_priv(dev_get_drvdata(dev));
|
||||
|
||||
return sprintf(buf, "%d\n", hw->odr);
|
||||
}
|
||||
|
||||
static ssize_t st_mag3d_set_sampling_frequency(struct device *dev,
|
||||
struct device_attribute *attr,
|
||||
const char *buf,
|
||||
size_t count)
|
||||
{
|
||||
struct iio_dev *iio_dev = dev_get_drvdata(dev);
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
unsigned int odr;
|
||||
int err;
|
||||
|
||||
err = kstrtoint(buf, 10, &odr);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
err = st_mag3d_set_odr(hw, odr);
|
||||
|
||||
return err < 0 ? err : count;
|
||||
}
|
||||
|
||||
static ssize_t
|
||||
st_mag3d_get_sampling_frequency_avail(struct device *dev,
|
||||
struct device_attribute *attr,
|
||||
char *buf)
|
||||
{
|
||||
int i, len = 0;
|
||||
|
||||
for (i = 0; i < ARRAY_SIZE(st_mag3d_odr_table); i++)
|
||||
len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
|
||||
st_mag3d_odr_table[i].hz);
|
||||
buf[len - 1] = '\n';
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
static IIO_DEV_ATTR_SAMP_FREQ(0644,
|
||||
st_mag3d_get_sampling_frequency,
|
||||
st_mag3d_set_sampling_frequency);
|
||||
static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(st_mag3d_get_sampling_frequency_avail);
|
||||
|
||||
static struct attribute *st_mag3d_attributes[] = {
|
||||
&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
|
||||
&iio_dev_attr_sampling_frequency.dev_attr.attr,
|
||||
NULL,
|
||||
};
|
||||
|
||||
static const struct attribute_group st_mag3d_attribute_group = {
|
||||
.attrs = st_mag3d_attributes,
|
||||
};
|
||||
|
||||
static const struct iio_info st_mag3d_info = {
|
||||
.read_raw = st_mag3d_read_raw,
|
||||
.write_raw = st_mag3d_write_raw,
|
||||
.attrs = &st_mag3d_attribute_group,
|
||||
};
|
||||
|
||||
static const unsigned long st_mag3d_available_scan_masks[] = {0x7, 0x0};
|
||||
|
||||
int st_mag3d_probe(struct device *dev, int irq, const char *name,
|
||||
const struct st_mag3d_transfer_function *tf_ops)
|
||||
{
|
||||
struct st_mag3d_hw *hw;
|
||||
struct iio_dev *iio_dev;
|
||||
int err;
|
||||
|
||||
iio_dev = devm_iio_device_alloc(dev, sizeof(*hw));
|
||||
if (!iio_dev)
|
||||
return -ENOMEM;
|
||||
|
||||
dev_set_drvdata(dev, iio_dev);
|
||||
|
||||
iio_dev->dev.parent = dev;
|
||||
iio_dev->name = name;
|
||||
iio_dev->modes = INDIO_DIRECT_MODE;
|
||||
iio_dev->available_scan_masks = st_mag3d_available_scan_masks;
|
||||
iio_dev->channels = st_mag3d_channels;
|
||||
iio_dev->num_channels = ARRAY_SIZE(st_mag3d_channels);
|
||||
iio_dev->info = &st_mag3d_info;
|
||||
|
||||
hw = iio_priv(iio_dev);
|
||||
mutex_init(&hw->lock);
|
||||
hw->dev = dev;
|
||||
hw->tf = tf_ops;
|
||||
hw->irq = irq;
|
||||
hw->iio_dev = iio_dev;
|
||||
|
||||
err = st_mag3d_check_whoami(hw);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
err = st_mag3d_set_odr(hw, st_mag3d_odr_table[0].hz);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
err = st_mag3d_set_fullscale(hw, st_mag3d_fs_table[0].gain);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
/* enable BDU */
|
||||
err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL5_ADDR,
|
||||
ST_MAG3D_BDU_MASK, 1);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
/* enable OM mode */
|
||||
err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL1_ADDR,
|
||||
ST_MAG3D_OM_XY_MASK, 3);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
err = st_mag3d_write_with_mask(hw, ST_MAG3D_CTRL4_ADDR,
|
||||
ST_MAG3D_OM_Z_MASK, 3);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
if (irq > 0) {
|
||||
err = st_mag3d_allocate_buffer(iio_dev);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
||||
err = st_mag3d_allocate_trigger(iio_dev);
|
||||
if (err < 0) {
|
||||
st_mag3d_deallocate_buffer(iio_dev);
|
||||
return err;
|
||||
}
|
||||
}
|
||||
|
||||
err = devm_iio_device_register(dev, iio_dev);
|
||||
if (err < 0 && irq > 0) {
|
||||
st_mag3d_deallocate_trigger(iio_dev);
|
||||
st_mag3d_deallocate_buffer(iio_dev);
|
||||
return err;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
EXPORT_SYMBOL(st_mag3d_probe);
|
||||
|
||||
void st_mag3d_remove(struct iio_dev *iio_dev)
|
||||
{
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
|
||||
if (hw->irq > 0) {
|
||||
st_mag3d_deallocate_trigger(iio_dev);
|
||||
st_mag3d_deallocate_buffer(iio_dev);
|
||||
}
|
||||
}
|
||||
EXPORT_SYMBOL(st_mag3d_remove);
|
||||
|
||||
MODULE_DESCRIPTION("STMicroelectronics mag3d driver");
|
||||
MODULE_AUTHOR("MEMS Software Solutions Team");
|
||||
MODULE_LICENSE("GPL v2");
|
||||
90
drivers/iio/stm/magnetometer/st_mag3d_i2c.c
Normal file
90
drivers/iio/stm/magnetometer/st_mag3d_i2c.c
Normal file
@ -0,0 +1,90 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* STMicroelectronics mag3d i2c driver
|
||||
*
|
||||
* MEMS Software Solutions Team
|
||||
*
|
||||
* Copyright 2016 STMicroelectronics Inc.
|
||||
*/
|
||||
|
||||
#include <linux/module.h>
|
||||
#include <linux/types.h>
|
||||
#include <linux/i2c.h>
|
||||
|
||||
#include "st_mag3d.h"
|
||||
|
||||
#define I2C_AUTO_INCREMENT BIT(7)
|
||||
|
||||
static int st_mag3d_i2c_read(struct device *dev, u8 addr, int len, u8 *data)
|
||||
{
|
||||
if (len > 1)
|
||||
addr |= I2C_AUTO_INCREMENT;
|
||||
|
||||
return i2c_smbus_read_i2c_block_data_or_emulated(to_i2c_client(dev),
|
||||
addr, len, data);
|
||||
}
|
||||
|
||||
static int st_mag3d_i2c_write(struct device *dev, u8 addr, int len, u8 *data)
|
||||
{
|
||||
return i2c_smbus_write_i2c_block_data(to_i2c_client(dev), addr,
|
||||
len, data);
|
||||
}
|
||||
|
||||
static const struct st_mag3d_transfer_function st_mag3d_tf_i2c = {
|
||||
.write = st_mag3d_i2c_write,
|
||||
.read = st_mag3d_i2c_read,
|
||||
};
|
||||
|
||||
static int st_mag3d_i2c_probe(struct i2c_client *client,
|
||||
const struct i2c_device_id *id)
|
||||
{
|
||||
return st_mag3d_probe(&client->dev, client->irq, client->name,
|
||||
&st_mag3d_tf_i2c);
|
||||
}
|
||||
|
||||
static int st_mag3d_i2c_remove(struct i2c_client *client)
|
||||
{
|
||||
struct iio_dev *iio_dev = i2c_get_clientdata(client);
|
||||
|
||||
st_mag3d_remove(iio_dev);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct i2c_device_id st_mag3d_ids[] = {
|
||||
{ LIS3MDL_DEV_NAME },
|
||||
{ LSM9DS1_DEV_NAME },
|
||||
{}
|
||||
};
|
||||
MODULE_DEVICE_TABLE(i2c, st_mag3d_ids);
|
||||
|
||||
#ifdef CONFIG_OF
|
||||
static const struct of_device_id st_mag3d_id_table[] = {
|
||||
{
|
||||
.compatible = "st,lis3mdl_magn",
|
||||
},
|
||||
{
|
||||
.compatible = "st,lsm9ds1_magn",
|
||||
},
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(of, st_mag3d_id_table);
|
||||
#endif /* CONFIG_OF */
|
||||
|
||||
static struct i2c_driver st_mag3d_i2c_driver = {
|
||||
.driver = {
|
||||
.owner = THIS_MODULE,
|
||||
.name = "st_mag3d_i2c",
|
||||
#ifdef CONFIG_OF
|
||||
.of_match_table = st_mag3d_id_table,
|
||||
#endif /* CONFIG_OF */
|
||||
},
|
||||
.probe = st_mag3d_i2c_probe,
|
||||
.remove = st_mag3d_i2c_remove,
|
||||
.id_table = st_mag3d_ids,
|
||||
};
|
||||
module_i2c_driver(st_mag3d_i2c_driver);
|
||||
|
||||
MODULE_DESCRIPTION("STMicroelectronics mag3d i2c driver");
|
||||
MODULE_AUTHOR("MEMS Software Solutions Team");
|
||||
MODULE_LICENSE("GPL v2");
|
||||
124
drivers/iio/stm/magnetometer/st_mag3d_spi.c
Normal file
124
drivers/iio/stm/magnetometer/st_mag3d_spi.c
Normal file
@ -0,0 +1,124 @@
|
||||
// SPDX-License-Identifier: GPL-2.0-only
|
||||
/*
|
||||
* STMicroelectronics mag3d spi driver
|
||||
*
|
||||
* MEMS Software Solutions Team
|
||||
*
|
||||
* Copyright 2016 STMicroelectronics Inc.
|
||||
*/
|
||||
|
||||
#include <linux/module.h>
|
||||
#include <linux/types.h>
|
||||
#include <linux/spi/spi.h>
|
||||
#include <linux/iio/iio.h>
|
||||
|
||||
#include "st_mag3d.h"
|
||||
|
||||
#define ST_SENSORS_SPI_READ BIT(7)
|
||||
|
||||
static int st_mag3d_spi_read(struct device *dev, u8 addr, int len, u8 *data)
|
||||
{
|
||||
struct iio_dev *iio_dev = dev_get_drvdata(dev);
|
||||
struct st_mag3d_hw *hw = iio_priv(iio_dev);
|
||||
int err;
|
||||
|
||||
struct spi_transfer xfers[] = {
|
||||
{
|
||||
.tx_buf = hw->tb.tx_buf,
|
||||
.bits_per_word = 8,
|
||||
.len = 1,
|
||||
},
|
||||
{
|
||||
.rx_buf = hw->tb.rx_buf,
|
||||
.bits_per_word = 8,
|
||||
.len = len,
|
||||
}
|
||||
};
|
||||
|
||||
hw->tb.tx_buf[0] = addr | ST_SENSORS_SPI_READ;
|
||||
|
||||
err = spi_sync_transfer(to_spi_device(hw->dev), xfers,
|
||||
ARRAY_SIZE(xfers));
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
memcpy(data, hw->tb.rx_buf, len*sizeof(u8));
|
||||
|
||||
return len;
|
||||
}
|
||||
|
||||
static int st_mag3d_spi_write(struct device *dev, u8 addr, int len, u8 *data)
|
||||
{
|
||||
|
||||
struct st_mag3d_hw *hw;
|
||||
struct iio_dev *iio_dev;
|
||||
|
||||
if (len >= ST_MAG3D_TX_MAX_LENGTH)
|
||||
return -ENOMEM;
|
||||
|
||||
iio_dev = dev_get_drvdata(dev);
|
||||
hw = iio_priv(iio_dev);
|
||||
|
||||
hw->tb.tx_buf[0] = addr;
|
||||
memcpy(&hw->tb.tx_buf[1], data, len);
|
||||
|
||||
return spi_write(to_spi_device(hw->dev), hw->tb.tx_buf, len + 1);
|
||||
}
|
||||
|
||||
static const struct st_mag3d_transfer_function st_mag3d_tf_spi = {
|
||||
.write = st_mag3d_spi_write,
|
||||
.read = st_mag3d_spi_read,
|
||||
};
|
||||
|
||||
static int st_mag3d_spi_probe(struct spi_device *spi)
|
||||
{
|
||||
return st_mag3d_probe(&spi->dev, spi->irq, spi->modalias,
|
||||
&st_mag3d_tf_spi);
|
||||
}
|
||||
|
||||
static int st_mag3d_spi_remove(struct spi_device *spi)
|
||||
{
|
||||
struct iio_dev *iio_dev = spi_get_drvdata(spi);
|
||||
|
||||
st_mag3d_remove(iio_dev);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct spi_device_id st_mag3d_ids[] = {
|
||||
{ LIS3MDL_DEV_NAME },
|
||||
{ LSM9DS1_DEV_NAME },
|
||||
{}
|
||||
};
|
||||
MODULE_DEVICE_TABLE(spi, st_mag3d_ids);
|
||||
|
||||
#ifdef CONFIG_OF
|
||||
static const struct of_device_id st_mag3d_id_table[] = {
|
||||
{
|
||||
.compatible = "st,lis3mdl_magn",
|
||||
},
|
||||
{
|
||||
.compatible = "st,lsm9ds1_magn",
|
||||
},
|
||||
{},
|
||||
};
|
||||
MODULE_DEVICE_TABLE(of, st_mag3d_id_table);
|
||||
#endif /* CONFIG_OF */
|
||||
|
||||
static struct spi_driver st_mag3d_spi_driver = {
|
||||
.driver = {
|
||||
.owner = THIS_MODULE,
|
||||
.name = "st_mag3d_spi",
|
||||
#ifdef CONFIG_OF
|
||||
.of_match_table = st_mag3d_id_table,
|
||||
#endif /* CONFIG_OF */
|
||||
},
|
||||
.probe = st_mag3d_spi_probe,
|
||||
.remove = st_mag3d_spi_remove,
|
||||
.id_table = st_mag3d_ids,
|
||||
};
|
||||
module_spi_driver(st_mag3d_spi_driver);
|
||||
|
||||
MODULE_DESCRIPTION("STMicroelectronics mag3d spi driver");
|
||||
MODULE_AUTHOR("MEMS Software Solutions Team");
|
||||
MODULE_LICENSE("GPL v2");
|
||||
3
stm_iio_configs/mag3d_defconfig
Normal file
3
stm_iio_configs/mag3d_defconfig
Normal file
@ -0,0 +1,3 @@
|
||||
CONFIG_ST_MAG3D_IIO=m
|
||||
CONFIG_ST_MAG3D_I2C_IIO=m
|
||||
CONFIG_ST_MAG3D_SPI_IIO=m
|
||||
Loading…
Reference in New Issue
Block a user