Aksal/nuttx
3
1
Fork 1
mirror of https://github.com/apache/nuttx.git synced 2026-08-01 20:28:58 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions 2
nuttx/drivers/sensors/mpu60x0.c
Catalin Visinescu 081e4c478a drivers/: Multiple Drivers Are Registered With World Writable - Part 2 
Permissions (Part 2)

Description:

In kernel builds, any unprivileged process running on the NuttX
device can open /dev/efuse and attempt to read/write fuse content.
Reading the fuses may provide valuable information to an attacker
controlling the user process. The write operation, in extreme cases
where the fuse blocks are not locked, may brick the device.

DISCLAIMER: I tried to be strict with the settings, better to relax them
later if it's needed.

This is part of https://github.com/apache/nuttx/issues/19410

See https://github.com/apache/nuttx/issues/19410

Compiles ok.

Signed-off-by: Catalin Visinescu <catalin_visinescu@yahoo.com>
2026-07-15 15:27:28 +08:00

1201 lines
33 KiB
C
Raw Permalink Blame History

/****************************************************************************
* drivers/sensors/mpu60x0.c
*
* SPDX-License-Identifier: Apache-2.0
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/* WARNING for developers:
*
* This driver uses the legacy style of writing sensor drivers for NuttX. The
* project has since decided to adopt a new sensor framework in order to
* have a consistent API and feature-set.
*
* Sensors which use the uORB framework are typically suffixed "_uorb". You
* can also visit the documentation about the new sensor framework to learn
* more.
*/
#warning "This is a deprecated legacy sensor driver."
/****************************************************************************
* TODO: Theory of Operation
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <errno.h>
#include <nuttx/debug.h>
#include <string.h>
#include <limits.h>
#include <nuttx/bits.h>
#include <nuttx/mutex.h>
#include <nuttx/signal.h>
#include <nuttx/compiler.h>
#include <nuttx/kmalloc.h>
#ifdef CONFIG_MPU60X0_SPI
#include <nuttx/spi/spi.h>
#else
#include <nuttx/i2c/i2c_master.h>
#endif
#include <nuttx/fs/fs.h>
#include <nuttx/sensors/mpu60x0.h>
#include <nuttx/sensors/ioctl.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
/* Creates a mask of @m bits, i.e. MASK(2) -> 00000011 */
#define MASK(m) (BIT(m) - 1)
/* Masks and shifts @v into bit field @m */
#define TO_BITFIELD(m,v) (((v) & MASK(m ##__WIDTH)) << (m ##__SHIFT))
/* Un-masks and un-shifts bit field @m from @v */
#define FROM_BITFIELD(m,v) (((v) >> (m ##__SHIFT)) & MASK(m ##__WIDTH))
/* SPI read/write codes */
#define MPU_REG_READ 0x80
#define MPU_REG_WRITE 0
/****************************************************************************
* Private Types
****************************************************************************/
enum mpu_regaddr_e
{
SELF_TEST_X = 0x0d,
SELF_TEST_Y = 0x0e,
SELF_TEST_Z = 0x0f,
SELF_TEST_A = 0x10,
SMPLRT_DIV = 0x19,
/* __SHIFT : number of empty bits to the right of the field
* __WIDTH : width of the field, in bits
*
* single-bit fields don't have __SHIFT or __mask
*/
CONFIG = 0x1a,
CONFIG__EXT_SYNC_SET__SHIFT = 3,
CONFIG__EXT_SYNC_SET__WIDTH = 3,
CONFIG__DLPF_CFG__SHIFT = 0,
CONFIG__DLPF_CFG__WIDTH = 3,
GYRO_CONFIG = 0x1b,
GYRO_CONFIG__FS_SEL__SHIFT = 3,
GYRO_CONFIG__FS_SEL__WIDTH = 2,
ACCEL_CONFIG = 0x1c,
ACCEL_CONFIG__XA_ST = BIT(7),
ACCEL_CONFIG__YA_ST = BIT(6),
ACCEL_CONFIG__ZA_ST = BIT(5),
ACCEL_CONFIG__AFS_SEL__SHIFT = 3,
ACCEL_CONFIG__AFS_SEL__WIDTH = 2,
MOT_THR = 0x1f,
FIFO_EN = 0x23,
FIFO_EN__TEMP = BIT(7),
FIFO_EN__XG = BIT(6),
FIFO_EN__YG = BIT(5),
FIFO_EN__ZG = BIT(4),
FIFO_EN__ACCEL = BIT(3),
I2C_MST_CTRL = 0x24,
I2C_SLV0_ADDR = 0x25,
I2C_SLV0_REG = 0x26,
I2C_SLV0_CTRL = 0x27,
I2C_SLV1_ADDR = 0x28,
I2C_SLV1_REG = 0x29,
I2C_SLV1_CTRL = 0x2a,
I2C_SLV2_ADDR = 0x2b,
I2C_SLV2_REG = 0x2c,
I2C_SLV2_CTRL = 0x2d,
I2C_SLV3_ADDR = 0x2e,
I2C_SLV3_REG = 0x2f,
I2C_SLV3_CTRL = 0x30,
I2C_SLV4_ADDR = 0x31,
I2C_SLV4_REG = 0x32,
I2C_SLV4_DO = 0x33,
I2C_SLV4_CTRL = 0x34,
I2C_SLV4_DI = 0x35, /* RO */
I2C_MST_STATUS = 0x36, /* RO */
INT_PIN_CFG = 0x37,
INT_PIN_CFG__INT_LEVEL = BIT(7),
INT_PIN_CFG__INT_OPEN = BIT(6),
INT_PIN_CFG__LATCH_INT_EN = BIT(5),
INT_PIN_CFG__INT_RD_CLEAR = BIT(4),
INT_PIN_CFG__FSYNC_INT_LEVEL = BIT(3),
INT_PIN_CFG__FSYNC_INT_EN = BIT(2),
INT_PIN_CFG__I2C_BYPASS_EN = BIT(1),
INT_ENABLE = 0x38,
INT_STATUS = 0x3a, /* RO */
ACCEL_XOUT_H = 0x3b, /* RO */
ACCEL_XOUT_L = 0x3c, /* RO */
ACCEL_YOUT_H = 0x3d, /* RO */
ACCEL_YOUT_L = 0x3e, /* RO */
ACCEL_ZOUT_H = 0x3f, /* RO */
ACCEL_ZOUT_L = 0x40, /* RO */
TEMP_OUT_H = 0x41, /* RO */
TEMP_OUT_L = 0x42, /* RO */
GYRO_XOUT_H = 0x43, /* RO */
GYRO_XOUT_L = 0x44, /* RO */
GYRO_YOUT_H = 0x45, /* RO */
GYRO_YOUT_L = 0x46, /* RO */
GYRO_ZOUT_H = 0x47, /* RO */
GYRO_ZOUT_L = 0x48, /* RO */
EXT_SENS_DATA_00 = 0x49, /* RO */
EXT_SENS_DATA_01 = 0x4a, /* RO */
EXT_SENS_DATA_02 = 0x4b, /* RO */
EXT_SENS_DATA_03 = 0x4c, /* RO */
EXT_SENS_DATA_04 = 0x4d, /* RO */
EXT_SENS_DATA_05 = 0x4e, /* RO */
EXT_SENS_DATA_06 = 0x4f, /* RO */
EXT_SENS_DATA_07 = 0x50, /* RO */
EXT_SENS_DATA_08 = 0x51, /* RO */
EXT_SENS_DATA_09 = 0x52, /* RO */
EXT_SENS_DATA_10 = 0x53, /* RO */
EXT_SENS_DATA_11 = 0x54, /* RO */
EXT_SENS_DATA_12 = 0x55, /* RO */
EXT_SENS_DATA_13 = 0x56, /* RO */
EXT_SENS_DATA_14 = 0x57, /* RO */
EXT_SENS_DATA_15 = 0x58, /* RO */
EXT_SENS_DATA_16 = 0x59, /* RO */
EXT_SENS_DATA_17 = 0x5a, /* RO */
EXT_SENS_DATA_18 = 0x5b, /* RO */
EXT_SENS_DATA_19 = 0x5c, /* RO */
EXT_SENS_DATA_20 = 0x5d, /* RO */
EXT_SENS_DATA_21 = 0x5e, /* RO */
EXT_SENS_DATA_22 = 0x5f, /* RO */
EXT_SENS_DATA_23 = 0x60, /* RO */
I2C_SLV0_DO = 0x63,
I2C_SLV1_DO = 0x64,
I2C_SLV2_DO = 0x65,
I2C_SLV3_DO = 0x66,
I2C_MST_DELAY_CTRL = 0x67,
SIGNAL_PATH_RESET = 0x68,
SIGNAL_PATH_RESET__GYRO_RESET = BIT(2),
SIGNAL_PATH_RESET__ACCEL_RESET = BIT(1),
SIGNAL_PATH_RESET__TEMP_RESET = BIT(0),
SIGNAL_PATH_RESET__ALL_RESET = BIT(3) - 1,
MOT_DETECT_CTRL = 0x69,
USER_CTRL = 0x6a,
USER_CTRL__FIFO_EN = BIT(6),
USER_CTRL__I2C_MST_EN = BIT(5),
USER_CTRL__I2C_IF_DIS = BIT(4),
USER_CTRL__FIFO_RESET = BIT(2),
USER_CTRL__I2C_MST_RESET = BIT(1),
USER_CTRL__SIG_COND_RESET = BIT(0),
PWR_MGMT_1 = 0x6b, /* Reset: 0x40 */
PWR_MGMT_1__DEVICE_RESET = BIT(7),
PWR_MGMT_1__SLEEP = BIT(6),
PWR_MGMT_1__CYCLE = BIT(5),
PWR_MGMT_1__TEMP_DIS = BIT(3),
PWR_MGMT_1__CLK_SEL__SHIFT = 0,
PWR_MGMT_1__CLK_SEL__WIDTH = 3,
PWR_MGMT_2 = 0x6c,
FIFO_COUNTH = 0x72,
FIFO_COUNTL = 0x73,
FIFO_R_W = 0x74,
WHO_AM_I = 0x75, /* RO reset: 0x68 */
};
/* Describes the mpu60x0 sensor register file. This structure reflects
* the underlying hardware, so don't change it!
*/
begin_packed_struct struct sensor_data_s
{
int16_t x_accel;
int16_t y_accel;
int16_t z_accel;
int16_t temp;
int16_t x_gyro;
int16_t y_gyro;
int16_t z_gyro;
} end_packed_struct;
/* Used by the driver to manage the device */
struct mpu_dev_s
{
mutex_t lock; /* mutex for this structure */
struct mpu_config_s config; /* board-specific information */
struct sensor_data_s buf; /* temporary buffer (for read(), etc.) */
size_t bufpos; /* cursor into @buf, in bytes (!) */
uint8_t smplrt_div; /* divider to control sample rate */
uint8_t afs_sel; /* full scale range of the accelerometer */
uint8_t dlpf_config; /* digital low pass filter configuration */
bool fifo_enabled; /* current enable state of FIFO buffer */
float sample_rate; /* current sample rate */
};
/****************************************************************************
* Private Function Function Prototypes
****************************************************************************/
static int mpu_open(FAR struct file *filep);
static int mpu_close(FAR struct file *filep);
static ssize_t mpu_read(FAR struct file *filep, FAR char *buf, size_t len);
static ssize_t mpu_write(FAR struct file *filep, FAR const char *buf,
size_t len);
static off_t mpu_seek(FAR struct file *filep, off_t offset, int whence);
static int mpu_ioctl(FAR struct file *filep, int cmd, unsigned long arg);
/****************************************************************************
* Private Data
****************************************************************************/
static const struct file_operations g_mpu_fops =
{
mpu_open, /* open */
mpu_close, /* close */
mpu_read, /* read */
mpu_write, /* write */
mpu_seek, /* seek */
mpu_ioctl, /* ioctl */
};
/****************************************************************************
* Private Functions
****************************************************************************/
/* NOTE :
*
* In all of the following code, functions named with a double leading
* underscore '__' must be invoked ONLY if the mpu_dev_s lock is
* already held. Failure to do this might cause the transaction to get
* interrupted, which will likely confuse the data you get back.
*
* The mpu_dev_s lock is NOT the same thing as, i.e. the SPI master
* interface lock: the latter protects the bus interface hardware
* (which may have other SPI devices attached), the former protects
* the chip and its associated data.
*/
#ifdef CONFIG_MPU60X0_SPI
/* __mpu_read_reg(), but for spi-connected devices. See that function
* for documentation.
*/
static int __mpu_read_reg_spi(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR uint8_t *buf, uint8_t len)
{
int ret;
FAR struct spi_dev_s *spi = dev->config.spi;
int id = dev->config.spi_devid;
/* We'll probably return the number of bytes asked for. */
ret = len;
/* Grab and configure the SPI master device: always mode 0, 20MHz if it's a
* data register, 1MHz otherwise (per datasheet).
*/
SPI_LOCK(spi, true);
SPI_SETMODE(spi, SPIDEV_MODE0);
if ((reg_addr >= ACCEL_XOUT_H) && ((reg_addr + len) <= I2C_SLV0_DO))
{
SPI_SETFREQUENCY(spi, 20000000);
}
else
{
SPI_SETFREQUENCY(spi, 1000000);
}
/* Select the chip. */
SPI_SELECT(spi, id, true);
/* Send the read request. */
SPI_SEND(spi, reg_addr | MPU_REG_READ);
/* Clock in the data. */
while (0 != len--)
{
*buf++ = (uint8_t) (SPI_SEND(spi, 0xff));
}
/* Deselect the chip, release the SPI master. */
SPI_SELECT(spi, id, false);
SPI_LOCK(spi, false);
return ret;
}
/* __mpu_write_reg(), but for SPI connections. */
static int __mpu_write_reg_spi(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR const uint8_t * buf, uint8_t len)
{
int ret;
FAR struct spi_dev_s *spi = dev->config.spi;
int id = dev->config.spi_devid;
/* Hopefully, we'll return all the bytes they're asking for. */
ret = len;
/* Grab and configure the SPI master device. */
SPI_LOCK(spi, true);
SPI_SETMODE(spi, SPIDEV_MODE0);
SPI_SETFREQUENCY(spi, 1000000);
/* Select the chip. */
SPI_SELECT(spi, id, true);
/* Send the write request. */
SPI_SEND(spi, reg_addr | MPU_REG_WRITE);
/* Send the data. */
while (0 != len--)
{
SPI_SEND(spi, *buf++);
}
/* Release the chip and SPI master. */
SPI_SELECT(spi, id, false);
SPI_LOCK(spi, false);
return ret;
}
#else
/* __mpu_read_reg(), but for i2c-connected devices. */
static int __mpu_read_reg_i2c(FAR struct mpu_dev_s *dev,
uint8_t reg_addr,
FAR uint8_t *buf, uint8_t len)
{
int ret;
struct i2c_msg_s msg[2];
msg[0].frequency = CONFIG_MPU60X0_I2C_FREQ;
msg[0].addr = dev->config.addr;
msg[0].flags = I2C_M_NOSTOP;
msg[0].buffer = &reg_addr;
msg[0].length = 1;
msg[1].frequency = CONFIG_MPU60X0_I2C_FREQ;
msg[1].addr = dev->config.addr;
msg[1].flags = I2C_M_READ;
msg[1].buffer = buf;
msg[1].length = len;
ret = I2C_TRANSFER(dev->config.i2c, msg, 2);
if (ret < 0)
{
snerr("ERROR: I2C_TRANSFER(read) failed: %d\n", ret);
return ret;
}
return OK;
}
static int __mpu_write_reg_i2c(FAR struct mpu_dev_s *dev,
uint8_t reg_addr,
FAR const uint8_t *buf, uint8_t len)
{
int ret;
struct i2c_msg_s msg[2];
msg[0].frequency = CONFIG_MPU60X0_I2C_FREQ;
msg[0].addr = dev->config.addr;
msg[0].flags = I2C_M_NOSTOP;
msg[0].buffer = &reg_addr;
msg[0].length = 1;
msg[1].frequency = CONFIG_MPU60X0_I2C_FREQ;
msg[1].addr = dev->config.addr;
msg[1].flags = I2C_M_NOSTART;
msg[1].buffer = (FAR uint8_t *)buf;
msg[1].length = len;
ret = I2C_TRANSFER(dev->config.i2c, msg, 2);
if (ret < 0)
{
snerr("ERROR: I2C_TRANSFER(write) failed: %d\n", ret);
return ret;
}
return OK;
}
#endif /* CONFIG_MPU60X0_SPI */
/* __mpu_read_reg()
*
* Reads a block of @len byte-wide registers, starting at @reg_addr,
* from the device connected to @dev. Bytes are returned in @buf,
* which must have a capacity of at least @len bytes.
*
* Note: The caller must hold @dev->lock before calling this function.
*
* Returns number of bytes read, or a negative errno.
*/
static inline int __mpu_read_reg(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR uint8_t *buf, uint8_t len)
{
#ifdef CONFIG_MPU60X0_SPI
/* If we're wired to SPI, use that function. */
if (dev->config.spi != NULL)
{
return __mpu_read_reg_spi(dev, reg_addr, buf, len);
}
#else
/* If we're wired to I2C, use that function. */
if (dev->config.i2c != NULL)
{
return __mpu_read_reg_i2c(dev, reg_addr, buf, len);
}
#endif
/* If we get this far, it's because we can't "find" our device. */
return -ENODEV;
}
/* __mpu_write_reg()
*
* Writes a block of @len byte-wide registers, starting at @reg_addr,
* using the values in @buf to the device connected to @dev. Register
* values are taken in numerical order from @buf, i.e.:
*
* buf[0] -> register[@reg_addr]
* buf[1] -> register[@reg_addr + 1]
* ...
*
* Note: The caller must hold @dev->lock before calling this function.
*
* Returns number of bytes written, or a negative errno.
*/
static inline int __mpu_write_reg(FAR struct mpu_dev_s *dev,
enum mpu_regaddr_e reg_addr,
FAR const uint8_t *buf, uint8_t len)
{
#ifdef CONFIG_MPU60X0_SPI
/* If we're connected to SPI, use that function. */
if (dev->config.spi != NULL)
{
return __mpu_write_reg_spi(dev, reg_addr, buf, len);
}
#else
if (dev->config.i2c != NULL)
{
return __mpu_write_reg_i2c(dev, reg_addr, buf, len);
}
#endif
/* If we get this far, it's because we can't "find" our device. */
return -ENODEV;
}
/* __mpu_read_imu()
*
* Reads the whole IMU data file from @dev in one uninterrupted pass,
* placing the sampled values into @buf. This function is the only way
* to guarantee that the measured values are sampled as closely-spaced
* in time as the hardware permits, which is almost always what you
* want.
*/
static inline int __mpu_read_imu(FAR struct mpu_dev_s *dev,
FAR struct sensor_data_s *buf)
{
if (dev->fifo_enabled)
{
return __mpu_read_reg(dev, FIFO_R_W, (FAR uint8_t *)buf, sizeof(*buf));
}
return __mpu_read_reg(dev, ACCEL_XOUT_H, (FAR uint8_t *)buf, sizeof(*buf));
}
/* __mpu_read_pwr_mgmt_1()
*
* Returns the value of the PWR_MGMT_1 register from @dev.
*/
static inline uint8_t __mpu_read_pwr_mgmt_1(FAR struct mpu_dev_s *dev)
{
uint8_t buf = 0xff;
__mpu_read_reg(dev, PWR_MGMT_1, &buf, sizeof(buf));
return buf;
}
static inline int __mpu_write_signal_path_reset(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, SIGNAL_PATH_RESET, &val, sizeof(val));
}
static inline int __mpu_write_int_pin_cfg(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, INT_PIN_CFG, &val, sizeof(val));
}
static inline int __mpu_write_pwr_mgmt_1(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, PWR_MGMT_1, &val, sizeof(val));
}
static inline int __mpu_write_pwr_mgmt_2(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, PWR_MGMT_2, &val, sizeof(val));
}
#ifdef CONFIG_MPU60X0_SPI
static inline int __mpu_write_user_ctrl(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, USER_CTRL, &val, sizeof(val));
}
#endif
/* __mpu_write_gyro_config() :
*
* Sets the @fs_sel bit in GYRO_CONFIG to the value provided. Per the
* datasheet, the meaning of @fs_sel is as follows:
*
* GYRO_CONFIG(0x1b) : XG_ST YG_ST ZG_ST FS_SEL1 FS_SEL0 x x x
*
* XG_ST, YG_ST, ZG_ST : self-test (unsupported in this driver)
* 1 -> activate self-test on X, Y, and/or Z gyros
*
* FS_SEL[10] : full-scale range select
* 0 -> ± 250 deg/sec
* 1 -> ± 500 deg/sec
* 2 -> ± 1000 deg/sec
* 3 -> ± 2000 deg/sec
*/
static inline int __mpu_write_gyro_config(FAR struct mpu_dev_s *dev,
uint8_t fs_sel)
{
uint8_t val = TO_BITFIELD(GYRO_CONFIG__FS_SEL, fs_sel);
return __mpu_write_reg(dev, GYRO_CONFIG, &val, sizeof(val));
}
/* __mpu_write_accel_config() :
*
* Sets the @afs_sel bit in ACCEL_CONFIG to the value provided. Per
* the datasheet, the meaning of @afs_sel is as follows:
*
* ACCEL_CONFIG(0x1c) : XA_ST YA_ST ZA_ST AFS_SEL1 AFS_SEL0 x x x
*
* XA_ST, YA_ST, ZA_ST : self-test (unsupported in this driver)
* 1 -> activate self-test on X, Y, and/or Z accelerometers
*
* AFS_SEL[10] : full-scale range select
* 0 -> ± 2 g
* 1 -> ± 4 g
* 2 -> ± 8 g
* 3 -> ± 16 g
*/
static inline int __mpu_write_accel_config(FAR struct mpu_dev_s *dev,
uint8_t afs_sel)
{
uint8_t val;
if (afs_sel > 3)
{
snerr("ERROR: Invalid AFS_SEL value\n");
return -EINVAL;
}
val = TO_BITFIELD(ACCEL_CONFIG__AFS_SEL, afs_sel);
return __mpu_write_reg(dev, ACCEL_CONFIG, &val, sizeof(val));
}
/* CONFIG (0x1a) : x x EXT_SYNC_SET[2..0] DLPF_CFG[2..0]
*
* EXT_SYNC_SET : frame sync bit position
* DLPF_CFG : digital low-pass filter bandwidth
* (see datasheet, it's ... complicated)
*/
static inline int __mpu_write_config(FAR struct mpu_dev_s *dev,
uint8_t ext_sync_set, uint8_t dlpf_cfg)
{
uint8_t val = TO_BITFIELD(CONFIG__EXT_SYNC_SET, ext_sync_set) |
TO_BITFIELD(CONFIG__DLPF_CFG, dlpf_cfg);
return __mpu_write_reg(dev, CONFIG, &val, sizeof(val));
}
/* Sets the SMPLRT_DIV that controls the sample rate. */
static inline int __mpu_set_sample_rate_divider(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, SMPLRT_DIV, &val, sizeof(val));
}
/* Reads current sample rate. Value is updated to mpu_dev_s->sample_rate. */
static inline int __mpu_read_sample_rate(FAR struct mpu_dev_s *dev)
{
int ret;
float gyro_output_rate = 1000.0f;
ret = __mpu_read_reg(dev, SMPLRT_DIV, &dev->smplrt_div,
sizeof(dev->smplrt_div));
if (ret < 0)
{
return ret;
}
ret = __mpu_read_reg(dev, CONFIG, &dev->dlpf_config,
sizeof(dev->dlpf_config));
if (ret < 0)
{
return ret;
}
dev->dlpf_config = TO_BITFIELD(CONFIG__DLPF_CFG, dev->dlpf_config);
/* This condition verifies if DLPF is disabled */
if ((dev->dlpf_config == 0) || (dev->dlpf_config == 7))
{
gyro_output_rate = 8000.0f;
}
dev->sample_rate = gyro_output_rate / (float)(1 + dev->smplrt_div);
return OK;
}
/* Read the number of bytes currently in FIFO buffer. */
static inline int __mpu_read_fifo_count(FAR struct mpu_dev_s *dev,
uint16_t *buf)
{
int ret;
uint8_t fifo_counter[2];
ret = __mpu_read_reg(dev, FIFO_COUNTH, fifo_counter, sizeof(fifo_counter));
if (ret < 0)
{
snerr("ERROR: Failed to read FIFO counter\n");
*buf = 0;
}
else
{
*buf = (fifo_counter[0] << 8) | fifo_counter[1];
}
return ret;
}
/* Enables or disables FIFO loading a specific sensor.
* It may receive a OR combination of multiple sensors.
* Example:
* __mpu_set_fifo(priv, FIFO_EN__TEMP | FIFO_EN__YG | FIFO_EN__ACCEL);
*/
static inline int __mpu_set_fifo(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, FIFO_EN, &val, sizeof(val));
}
/* Sets USER CONTROL register. It may receive an OR combination of multiple
* bitfields.
* Example:
* __mpu_user_control(priv, USER_CTRL__FIFO_EN | USER_CTRL__I2C_MST_RESET);
*/
static inline int __mpu_user_control(FAR struct mpu_dev_s *dev,
uint8_t val)
{
return __mpu_write_reg(dev, USER_CTRL, &val, sizeof(val));
}
/* Resets the mpu60x0, sets it to a default configuration. */
static int mpu_reset(FAR struct mpu_dev_s *dev)
{
int ret;
#ifdef CONFIG_MPU60X0_SPI
if (dev->config.spi == NULL)
{
return -EINVAL;
}
#else
if (dev->config.i2c == NULL)
{
return -EINVAL;
}
#endif
nxmutex_lock(&dev->lock);
/* Awaken chip, issue hardware reset */
ret = __mpu_write_pwr_mgmt_1(dev, PWR_MGMT_1__DEVICE_RESET);
if (ret < 0)
{
nxmutex_unlock(&dev->lock);
snerr("Could not find mpu60x0!\n");
return ret;
}
/* Wait for reset cycle to finish (note: per the datasheet, we don't need
* to hold NSS for this)
*/
do
{
nxsched_usleep(50000); /* usecs (arbitrary) */
}
while (__mpu_read_pwr_mgmt_1(dev) & PWR_MGMT_1__DEVICE_RESET);
/* Reset signal paths */
__mpu_write_signal_path_reset(dev, SIGNAL_PATH_RESET__ALL_RESET);
nxsched_usleep(2000);
/* Disable SLEEP, use PLL with z-axis clock source */
__mpu_write_pwr_mgmt_1(dev, 3);
nxsched_usleep(2000);
/* Disable i2c if we're on spi. */
#ifdef CONFIG_MPU60X0_SPI
if (dev->config.spi)
{
__mpu_write_user_ctrl(dev, USER_CTRL__I2C_IF_DIS);
}
#endif
/* Disable low-power mode, enable all gyros and accelerometers */
__mpu_write_pwr_mgmt_2(dev, 0);
/* default No FSYNC, set accel LPF at 184 Hz, gyro LPF at 188 Hz in
* menuconfig
*/
__mpu_write_config(dev, CONFIG_MPU60X0_EXT_SYNC_SET,
CONFIG_MPU60X0_DLPF_CFG);
dev->dlpf_config = CONFIG_MPU60X0_DLPF_CFG;
/* default ± 1000 deg/sec in menuconfig */
__mpu_write_gyro_config(dev, CONFIG_MPU60X0_GYRO_FS_SEL);
/* default ± 8g in menuconfig */
__mpu_write_accel_config(dev, CONFIG_MPU60X0_ACCEL_AFS_SEL);
dev->afs_sel = CONFIG_MPU60X0_ACCEL_AFS_SEL;
/* clear INT on any read (we aren't using that pin right now) */
__mpu_write_int_pin_cfg(dev, INT_PIN_CFG__INT_RD_CLEAR);
/* Disable use of FIFO buffer */
__mpu_set_fifo(dev, 0);
dev->fifo_enabled = false;
nxmutex_unlock(&dev->lock);
return 0;
}
/****************************************************************************
* Name: mpu_open
*
* Note: we don't deal with multiple users trying to access this interface at
* the same time. Until further notice, don't do that.
*
* And no, it's not as simple as just prohibiting concurrent opens or
* reads with a mutex: there are legit reasons for truy concurrent
* access, but they must be treated carefully in this interface lest a
* partial reader end up with a mixture of old and new samples. This
* will make some users unhappy.
*
****************************************************************************/
static int mpu_open(FAR struct file *filep)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
/* Reset the register cache */
nxmutex_lock(&dev->lock);
dev->bufpos = 0;
nxmutex_unlock(&dev->lock);
return 0;
}
/****************************************************************************
* Name: mpu_close
****************************************************************************/
static int mpu_close(FAR struct file *filep)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
/* Reset (clear) the register cache. */
nxmutex_lock(&dev->lock);
dev->bufpos = 0;
nxmutex_unlock(&dev->lock);
return 0;
}
/****************************************************************************
* Name: mpu_read
*
* Returns a snapshot of the accelerometer, temperature, and gyro registers.
*
* Note: the chip uses traditional, twos-complement notation, i.e. "0"
* is encoded as 0, and full-scale-negative is 0x8000, and
* full-scale-positive is 0x7fff. If we read the registers
* sequentially and directly into memory (as we do), the measurements
* from each sensor are captured as big endian words.
*
* In contrast, ASN.1 maps "0" to 0x8000, full-scale-negative to 0,
* and full-scale-positive to 0xffff. So if we want to send in a
* format that an ASN.1 PER-decoder would recognize, must:
*
* 1. Treat the register data/measurements as unsigned,
* 2. Add 0x8000 to each measurement, and then,
* 3. Send each word in big-endian order.
*
* The result of the above will be something you could neatly describe
* like this (confirmed with asn1scc):
*
* Sint16 ::= INTEGER(-32768..32767)
*
* Mpu60x0Sample ::= SEQUENCE
* {
* accel-X Sint16,
* accel-Y Sint16,
* accel-Z Sint16,
* temp Sint16,
* gyro-X Sint16,
* gyro-Y Sint16,
* gyro-Z Sint16
* }
*
****************************************************************************/
static ssize_t mpu_read(FAR struct file *filep, FAR char *buf, size_t len)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
size_t send_len = 0;
nxmutex_lock(&dev->lock);
/* Populate the register cache if it seems empty. */
if (!dev->bufpos)
{
__mpu_read_imu(dev, &dev->buf);
}
/* Send the lesser of: available bytes, or amount requested. */
send_len = sizeof(dev->buf) - dev->bufpos;
if (send_len > len)
{
send_len = len;
}
if (send_len)
{
memcpy(buf, ((FAR uint8_t *)&dev->buf) + dev->bufpos, send_len);
}
/* Move the cursor, to mark them as sent. */
dev->bufpos += send_len;
/* If we've sent the last byte, reset the buffer. */
if (dev->bufpos >= sizeof(dev->buf))
{
dev->bufpos = 0;
}
nxmutex_unlock(&dev->lock);
return send_len;
}
/****************************************************************************
* Name: mpu_write
****************************************************************************/
static ssize_t mpu_write(FAR struct file *filep, FAR const char *buf,
size_t len)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
UNUSED(inode);
UNUSED(dev);
snerr("ERROR: %p %p %zu\n", inode, dev, len);
return len;
}
/****************************************************************************
* Name: mpu60x0_seek
****************************************************************************/
static off_t mpu_seek(FAR struct file *filep, off_t offset, int whence)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *dev = inode->i_private;
UNUSED(inode);
UNUSED(dev);
snerr("ERROR: %p %p\n", inode, dev);
return 0;
}
/****************************************************************************
* Name: mpu60x0_ioctl
****************************************************************************/
static int mpu_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
{
FAR struct inode *inode = filep->f_inode;
FAR struct mpu_dev_s *priv = inode->i_private;
uint8_t write_data = (uint8_t)arg;
int ret = OK;
switch (cmd)
{
/* Sets the accelerometer full scale range. Arg: uin8_t value */
case SNIOC_SET_AFS_SEL:
ret = __mpu_write_accel_config(priv, write_data);
if (ret < 0)
{
snerr("ERROR: SNIOC_SET_AFS_SEL fails. Returns: %d\n", ret);
}
else
{
priv->afs_sel = write_data;
sninfo("SNIOC_SET_AFS_SEL: %d Returns: %d\n", priv->afs_sel,
ret);
}
break;
/* Sets the sample rate divider. Arg: uin8_t value */
case SNIOC_SMPLRT_DIV:
ret = __mpu_set_sample_rate_divider(priv, write_data);
priv->smplrt_div = write_data;
sninfo("SNIOC_SMPLRT_DIV: %d Returns: %d\n", priv->smplrt_div, ret);
break;
/* Read current sample rate. Arg: uin32_t* pointer */
case SNIOC_READ_SAMPLE_RATE:
{
FAR uint32_t *ptr = (FAR uint32_t *)((uintptr_t)arg);
ret = __mpu_read_sample_rate(priv);
sninfo("SNIOC_READ_SAMPLE_RATE: Returns: %d. Read: %f\n",
ret, priv->sample_rate);
*ptr = (uint32_t)priv->sample_rate;
break;
}
/* Read current number of bytes in FIFO buffer. Arg: uin16_t* */
case SNIOC_READ_FIFO_COUNT:
{
FAR uint16_t *ptr = (FAR uint16_t *)((uintptr_t)arg);
uint16_t fifo_count = 0;
ret = __mpu_read_fifo_count(priv, &fifo_count);
*ptr = fifo_count;
sninfo("SNIOC_READ_FIFO_COUNT: Returns: %d. Read: 0x%x\n",
ret, fifo_count);
break;
}
/* Enable or disable the use of FIFO buffer. Arg: bool* */
case SNIOC_ENABLE_FIFO:
if (!write_data)
{
ret = __mpu_set_fifo(priv, 0);
if (ret < 0)
{
sninfo("SNIOC_ENABLE_FIFO failed. Returns: %d\n", ret);
}
ret = __mpu_user_control(priv, 0);
priv->fifo_enabled = false;
}
else
{
ret = __mpu_user_control(priv, USER_CTRL__FIFO_EN);
if (ret < 0)
{
sninfo("SNIOC_ENABLE_FIFO failed. Returns: %d\n", ret);
}
/* This configuration enables temperature, accelerometer and
* gyro on all three axis. Each read requires 14 bytes, allowing
* the FIFO to store 1024/14 = 73 samples.
*/
ret = __mpu_set_fifo(priv, FIFO_EN__TEMP | FIFO_EN__XG |
FIFO_EN__YG | FIFO_EN__ZG | FIFO_EN__ACCEL);
priv->fifo_enabled = true;
}
sninfo("SNIOC_ENABLE_FIFO: %d Returns: %d\n", write_data, ret);
break;
default:
sninfo("Unrecognized IOCTL command: 0x%04x\n", cmd);
ret = -ENOTTY;
break;
}
return ret;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: mpu60x0_register
*
* Description:
* Registers the mpu60x0 interface as 'devpath'
*
* Input Parameters:
* devpath - The full path to the interface to register. E.g., "/dev/imu0"
* spi - SPI interface for chip communications
* config - Configuration information
*
* Returned Value:
* Zero (OK) on success; a negated errno value on failure.
*
****************************************************************************/
int mpu60x0_register(FAR const char *path, FAR struct mpu_config_s *config)
{
FAR struct mpu_dev_s *priv;
int ret;
/* Without config info, we can't do anything. */
if (config == NULL)
{
return -EINVAL;
}
/* Initialize the device structure. */
priv = kmm_malloc(sizeof(struct mpu_dev_s));
if (priv == NULL)
{
snerr("ERROR: Failed to allocate mpu60x0 device instance\n");
return -ENOMEM;
}
memset(priv, 0, sizeof(*priv));
nxmutex_init(&priv->lock);
/* Keep a copy of the config structure, in case the caller discards
* theirs.
*/
priv->config = *config;
/* Reset the chip, to give it an initial configuration. */
ret = mpu_reset(priv);
if (ret < 0)
{
snerr("ERROR: Failed to configure mpu60x0: %d\n", ret);
nxmutex_destroy(&priv->lock);
kmm_free(priv);
return ret;
}
/* Register the device node. */
ret = register_driver(path, &g_mpu_fops, 0600, priv);
if (ret < 0)
{
snerr("ERROR: Failed to register mpu60x0 interface: %d\n", ret);
nxmutex_destroy(&priv->lock);
kmm_free(priv);
return ret;
}
return OK;
}
Reference in a new issue View git blame Copy permalink