龙芯2k0300 - 走马观碑组MPU6050驱动移植

判涔 · 昨天 15:40

在《龙芯2k0300 - 走马观碑组第21届智能汽车竞赛软硬件设计》中我们介绍到我们的开发板使用了MPU6050 陀螺仪，MPU6050是一个六轴姿态传感器，内部集成了三轴加速度计和三轴陀螺仪：

加速度计：检测X、Y、Z三个轴上的加速度（比如前进、后退、倾斜）；
陀螺仪：检测绕X、Y、Z轴的旋转速度（比如左转、右转、翻滚）；

MPU6050陀螺仪采用I2C通信方式：

MPU6050陀螺仪采用I2C通信方式，与龙芯2K0300开发板的连接关系需严格对应，确保I2C通信与GPIO控制正常，连接表如下：
屏幕引脚功能连接的龙芯GPIO 说明 VCC 为MPU6050提供工作电压 3.3V 接3.3V GND 电源共地，确保电压稳定性 GND 必须可靠接地，否则可能出现显示异常 SCL I2C 时钟引脚 I2C1_SCL （GPIO50）提供I2C同步通信时钟 SDA SDA数据引脚 I2C1_SDA（GPIO51）传输I2C命令与显示数据 XDA —— XCL —— AD0 决定 I2C 地址 —— 接 GND 地址为 0x68，接 3.3V 为 0x69 INT 中断引脚 —— 中断引脚，暂时不用接一、MPU6050设备驱动

MPU6050由于采用了I2C通讯协议，所以涉及到了I2C设备驱动。如果对I2C驱动源码移植感兴趣，可参考：

《通信协议-I2C》；
《linux驱动移植-I2C总线设备驱动》；
《linux驱动移植-I2C适配器驱动移植》；
《linux驱动移植-I2C驱动移植（OLED SSD1306）》。

龙邱科技提供了MPU6050设备驱动实现，位于： i2c_mpu6050_driver，只不过这个是基于linux 4.19的，如果移植这个改动的相对较多。
1.1 内核配置

实际上我们下载的内核linux 6.12已经内置了MPU6050的驱动，其使用了IIO框架，这里我们就直接使用内核驱动即可，无需从零开发驱动，需开启内核I2C及IIO相关配置，并添加MPU6050驱动代码。
进入内核配置界面：

zhengyang@ubuntu:~$ cd /opt/2k0300/build-2k0300/workspace/linux-6.12
zhengyang@ubuntu:/opt/2k0300/build-2k0300/workspace/linux-6.12$ source ../set_env.sh && make menuconfig

复制代码

依次进入以下菜单：

Device Drivers →
I2C support →
I2C Hardware Bus support →
<*> Loongson fast speed I2C adapter # 龙芯 2K0300 I2C控制器驱动
<*> I2C device interface
[*] Industrial I/O support →
Inertial measurement units →
<*> Invensense MPU6050 devices (I2C)

复制代码

默认会生成配置：

CONFIG_I2C_LSFS=y # 这个已经配置，不用追加
CONFIG_I2C_CHARDEV=y
CONFIG_IIO=y # 这个已经配置，不用追加
CONFIG_INV_MPU6050_IIO=y
CONFIG_INV_MPU6050_I2C=y

复制代码

我们直接修改arch/loongarch/configs/loongson_2k300_defconfig文件，加入这几个配置。
1.2 MPU6050驱动

驱动源码位于drivers/iio/imu/inv_mpu6050/目录下，基于内核IIO框架开发，无需修改内核核心代码，只需编译进内核即可；

zhengyang@ubuntu:/opt/2k0300/build-2k0300/workspace/linux-6.12$ ll drivers/iio/imu/inv_mpu6050/
-rw-rw-r-- 1 zhengyang zhengyang 4620 3月 19 19:48 inv_mpu_acpi.c
-rw-rw-r-- 1 zhengyang zhengyang 5522 3月 19 19:48 inv_mpu_aux.c
-rw-rw-r-- 1 zhengyang zhengyang 479 3月 19 19:48 inv_mpu_aux.h
-rw-rw-r-- 1 zhengyang zhengyang 62807 3月 19 19:48 inv_mpu_core.c
-rw-rw-r-- 1 zhengyang zhengyang 6467 3月 19 19:48 inv_mpu_i2c.c
-rw-rw-r-- 1 zhengyang zhengyang 16370 3月 19 19:48 inv_mpu_iio.h
-rw-rw-r-- 1 zhengyang zhengyang 8654 3月 19 19:48 inv_mpu_magn.c
-rw-rw-r-- 1 zhengyang zhengyang 931 3月 19 19:48 inv_mpu_magn.h
-rw-rw-r-- 1 zhengyang zhengyang 3830 3月 19 19:48 inv_mpu_ring.c
-rw-rw-r-- 1 zhengyang zhengyang 3883 3月 19 19:48 inv_mpu_spi.c
-rw-rw-r-- 1 zhengyang zhengyang 8786 3月 19 19:48 inv_mpu_trigger.c
-rw-rw-r-- 1 zhengyang zhengyang 1042 3月 19 19:48 Kconfig
-rw-rw-r-- 1 zhengyang zhengyang 417 3月 19 19:48 Makefile

复制代码

我们重点关注inv_mpu_i2c.c和inv_mpu_core.c。
1.2.1 inv_mpu_i2c.c

drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c源码如下：
点击查看详情

// SPDX-License-Identifier: GPL-2.0-only
/*
* Copyright (C) 2012 Invensense, Inc.
*/
#include <linux/delay.h>
#include <linux/err.h>
#include <linux/i2c.h>
#include <linux/iio/iio.h>
#include <linux/mod_devicetable.h>
#include <linux/module.h>
#include <linux/property.h>
#include "inv_mpu_iio.h"
static const struct regmap_config inv_mpu_regmap_config = {
.reg_bits = 8,
.val_bits = 8,
};
static int inv_mpu6050_select_bypass(struct i2c_mux_core *muxc, u32 chan_id)
{
return 0;
}
static bool inv_mpu_i2c_aux_bus(struct device *dev)
{
struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
switch (st->chip_type) {
case INV_ICM20608:
case INV_ICM20608D:
case INV_ICM20609:
case INV_ICM20689:
case INV_ICM20600:
case INV_ICM20602:
case INV_IAM20680:
/* no i2c auxiliary bus on the chip */
return false;
case INV_MPU9150:
case INV_MPU9250:
case INV_MPU9255:
if (st->magn_disabled)
return true;
else
return false;
default:
return true;
}
}
static int inv_mpu_i2c_aux_setup(struct iio_dev *indio_dev)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
struct device *dev = indio_dev->dev.parent;
struct fwnode_handle *mux_node;
int ret;
/*
* MPU9xxx magnetometer support requires to disable i2c auxiliary bus.
* To ensure backward compatibility with existing setups, do not disable
* i2c auxiliary bus if it used.
* Check for i2c-gate node in devicetree and set magnetometer disabled.
* Only MPU6500 is supported by ACPI, no need to check.
*/
switch (st->chip_type) {
case INV_MPU9150:
case INV_MPU9250:
case INV_MPU9255:
mux_node = device_get_named_child_node(dev, "i2c-gate");
if (mux_node != NULL) {
st->magn_disabled = true;
dev_warn(dev, "disable internal use of magnetometer\n");
}
fwnode_handle_put(mux_node);
break;
default:
break;
}
/* enable i2c bypass when using i2c auxiliary bus */
if (inv_mpu_i2c_aux_bus(dev)) {
ret = regmap_write(st->map, st->reg->int_pin_cfg,
st->irq_mask | INV_MPU6050_BIT_BYPASS_EN);
if (ret)
return ret;
}
return 0;
}
/**
* inv_mpu_probe() - probe function.
* @client: i2c client.
*
* Returns 0 on success, a negative error code otherwise.
*/
static int inv_mpu_probe(struct i2c_client *client)
{
const struct i2c_device_id *id = i2c_client_get_device_id(client);
const void *match;
struct inv_mpu6050_state *st;
int result;
enum inv_devices chip_type;
struct regmap *regmap;
const char *name;
if (!i2c_check_functionality(client->adapter,
I2C_FUNC_SMBUS_I2C_BLOCK))
return -EOPNOTSUPP;
match = device_get_match_data(&client->dev);
if (match) {
chip_type = (uintptr_t)match;
name = client->name;
} else if (id) {
chip_type = (enum inv_devices)
id->driver_data;
name = id->name;
} else {
return -ENOSYS;
}
regmap = devm_regmap_init_i2c(client, &inv_mpu_regmap_config);
if (IS_ERR(regmap)) {
dev_err(&client->dev, "Failed to register i2c regmap: %pe\n",
regmap);
return PTR_ERR(regmap);
}
result = inv_mpu_core_probe(regmap, client->irq, name,
inv_mpu_i2c_aux_setup, chip_type);
if (result < 0)
return result;
st = iio_priv(dev_get_drvdata(&client->dev));
if (inv_mpu_i2c_aux_bus(&client->dev)) {
/* declare i2c auxiliary bus */
st->muxc = i2c_mux_alloc(client->adapter, &client->dev,
1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE,
inv_mpu6050_select_bypass, NULL);
if (!st->muxc)
return -ENOMEM;
st->muxc->priv = dev_get_drvdata(&client->dev);
result = i2c_mux_add_adapter(st->muxc, 0, 0);
if (result)
return result;
result = inv_mpu_acpi_create_mux_client(client);
if (result)
goto out_del_mux;
}
return 0;
out_del_mux:
i2c_mux_del_adapters(st->muxc);
return result;
}
static void inv_mpu_remove(struct i2c_client *client)
{
struct iio_dev *indio_dev = i2c_get_clientdata(client);
struct inv_mpu6050_state *st = iio_priv(indio_dev);
if (st->muxc) {
inv_mpu_acpi_delete_mux_client(client);
i2c_mux_del_adapters(st->muxc);
}
}
/*
* device id table is used to identify what device can be
* supported by this driver
*/
static const struct i2c_device_id inv_mpu_id[] = {
{"mpu6050", INV_MPU6050},
{"mpu6500", INV_MPU6500},
{"mpu6515", INV_MPU6515},
{"mpu6880", INV_MPU6880},
{"mpu9150", INV_MPU9150},
{"mpu9250", INV_MPU9250},
{"mpu9255", INV_MPU9255},
{"icm20608", INV_ICM20608},
{"icm20608d", INV_ICM20608D},
{"icm20609", INV_ICM20609},
{"icm20689", INV_ICM20689},
{"icm20600", INV_ICM20600},
{"icm20602", INV_ICM20602},
{"icm20690", INV_ICM20690},
{"iam20680", INV_IAM20680},
{}
};
MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
static const struct of_device_id inv_of_match[] = {
{
.compatible = "invensense,mpu6050",
.data = (void *)INV_MPU6050
},
{
.compatible = "invensense,mpu6500",
.data = (void *)INV_MPU6500
},
{
.compatible = "invensense,mpu6515",
.data = (void *)INV_MPU6515
},
{
.compatible = "invensense,mpu6880",
.data = (void *)INV_MPU6880
},
{
.compatible = "invensense,mpu9150",
.data = (void *)INV_MPU9150
},
{
.compatible = "invensense,mpu9250",
.data = (void *)INV_MPU9250
},
{
.compatible = "invensense,mpu9255",
.data = (void *)INV_MPU9255
},
{
.compatible = "invensense,icm20608",
.data = (void *)INV_ICM20608
},
{
.compatible = "invensense,icm20608d",
.data = (void *)INV_ICM20608D
},
{
.compatible = "invensense,icm20609",
.data = (void *)INV_ICM20609
},
{
.compatible = "invensense,icm20689",
.data = (void *)INV_ICM20689
},
{
.compatible = "invensense,icm20600",
.data = (void *)INV_ICM20600
},
{
.compatible = "invensense,icm20602",
.data = (void *)INV_ICM20602
},
{
.compatible = "invensense,icm20690",
.data = (void *)INV_ICM20690
},
{
.compatible = "invensense,iam20680",
.data = (void *)INV_IAM20680
},
{ }
};
MODULE_DEVICE_TABLE(of, inv_of_match);
static const struct acpi_device_id inv_acpi_match[] = {
{"INVN6500", INV_MPU6500},
{ },
};
MODULE_DEVICE_TABLE(acpi, inv_acpi_match);
static struct i2c_driver inv_mpu_driver = {
.probe = inv_mpu_probe,
.remove = inv_mpu_remove,
.id_table = inv_mpu_id,
.driver = {
.of_match_table = inv_of_match,
.acpi_match_table = inv_acpi_match,
.name = "inv-mpu6050-i2c",
.pm = pm_ptr(&inv_mpu_pmops),
},
};
module_i2c_driver(inv_mpu_driver);
MODULE_AUTHOR("Invensense Corporation");
MODULE_DESCRIPTION("Invensense device MPU6050 driver");
MODULE_LICENSE("GPL");
MODULE_IMPORT_NS(IIO_MPU6050);

复制代码

1.2.2 inv_mpu_core.c

drivers/iio/imu/inv_mpu6050/inv_mpu_core.c源码如下：
点击查看详情

// SPDX-License-Identifier: GPL-2.0-only
/*
* Copyright (C) 2012 Invensense, Inc.
*/
#include <linux/module.h>
#include <linux/slab.h>
#include <linux/i2c.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/sysfs.h>
#include <linux/jiffies.h>
#include <linux/irq.h>
#include <linux/interrupt.h>
#include <linux/acpi.h>
#include <linux/platform_device.h>
#include <linux/regulator/consumer.h>
#include <linux/math64.h>
#include <linux/minmax.h>
#include <linux/pm.h>
#include <linux/pm_runtime.h>
#include <linux/property.h>
#include <linux/iio/common/inv_sensors_timestamp.h>
#include <linux/iio/iio.h>
#include "inv_mpu_iio.h"
#include "inv_mpu_magn.h"
/*
* this is the gyro scale translated from dynamic range plus/minus
* {250, 500, 1000, 2000} to rad/s
*/
static const int gyro_scale_6050[] = {133090, 266181, 532362, 1064724};
/*
* this is the accel scale translated from dynamic range plus/minus
* {2, 4, 8, 16} to m/s^2
*/
static const int accel_scale[] = {598, 1196, 2392, 4785};
static const struct inv_mpu6050_reg_map reg_set_icm20602 = {
.sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV,
.lpf = INV_MPU6050_REG_CONFIG,
.accel_lpf = INV_MPU6500_REG_ACCEL_CONFIG_2,
.user_ctrl = INV_MPU6050_REG_USER_CTRL,
.fifo_en = INV_MPU6050_REG_FIFO_EN,
.gyro_config = INV_MPU6050_REG_GYRO_CONFIG,
.accl_config = INV_MPU6050_REG_ACCEL_CONFIG,
.fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H,
.fifo_r_w = INV_MPU6050_REG_FIFO_R_W,
.raw_gyro = INV_MPU6050_REG_RAW_GYRO,
.raw_accl = INV_MPU6050_REG_RAW_ACCEL,
.temperature = INV_MPU6050_REG_TEMPERATURE,
.int_enable = INV_MPU6050_REG_INT_ENABLE,
.int_status = INV_MPU6050_REG_INT_STATUS,
.pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1,
.pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2,
.int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG,
.accl_offset = INV_MPU6500_REG_ACCEL_OFFSET,
.gyro_offset = INV_MPU6050_REG_GYRO_OFFSET,
.i2c_if = INV_ICM20602_REG_I2C_IF,
};
static const struct inv_mpu6050_reg_map reg_set_6500 = {
.sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV,
.lpf = INV_MPU6050_REG_CONFIG,
.accel_lpf = INV_MPU6500_REG_ACCEL_CONFIG_2,
.user_ctrl = INV_MPU6050_REG_USER_CTRL,
.fifo_en = INV_MPU6050_REG_FIFO_EN,
.gyro_config = INV_MPU6050_REG_GYRO_CONFIG,
.accl_config = INV_MPU6050_REG_ACCEL_CONFIG,
.fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H,
.fifo_r_w = INV_MPU6050_REG_FIFO_R_W,
.raw_gyro = INV_MPU6050_REG_RAW_GYRO,
.raw_accl = INV_MPU6050_REG_RAW_ACCEL,
.temperature = INV_MPU6050_REG_TEMPERATURE,
.int_enable = INV_MPU6050_REG_INT_ENABLE,
.int_status = INV_MPU6050_REG_INT_STATUS,
.pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1,
.pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2,
.int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG,
.accl_offset = INV_MPU6500_REG_ACCEL_OFFSET,
.gyro_offset = INV_MPU6050_REG_GYRO_OFFSET,
.i2c_if = 0,
};
static const struct inv_mpu6050_reg_map reg_set_6050 = {
.sample_rate_div = INV_MPU6050_REG_SAMPLE_RATE_DIV,
.lpf = INV_MPU6050_REG_CONFIG,
.user_ctrl = INV_MPU6050_REG_USER_CTRL,
.fifo_en = INV_MPU6050_REG_FIFO_EN,
.gyro_config = INV_MPU6050_REG_GYRO_CONFIG,
.accl_config = INV_MPU6050_REG_ACCEL_CONFIG,
.fifo_count_h = INV_MPU6050_REG_FIFO_COUNT_H,
.fifo_r_w = INV_MPU6050_REG_FIFO_R_W,
.raw_gyro = INV_MPU6050_REG_RAW_GYRO,
.raw_accl = INV_MPU6050_REG_RAW_ACCEL,
.temperature = INV_MPU6050_REG_TEMPERATURE,
.int_enable = INV_MPU6050_REG_INT_ENABLE,
.pwr_mgmt_1 = INV_MPU6050_REG_PWR_MGMT_1,
.pwr_mgmt_2 = INV_MPU6050_REG_PWR_MGMT_2,
.int_pin_cfg = INV_MPU6050_REG_INT_PIN_CFG,
.accl_offset = INV_MPU6050_REG_ACCEL_OFFSET,
.gyro_offset = INV_MPU6050_REG_GYRO_OFFSET,
.i2c_if = 0,
};
static const struct inv_mpu6050_chip_config chip_config_6050 = {
.clk = INV_CLK_INTERNAL,
.fsr = INV_MPU6050_FSR_2000DPS,
.lpf = INV_MPU6050_FILTER_20HZ,
.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50),
.gyro_en = true,
.accl_en = true,
.temp_en = true,
.magn_en = false,
.gyro_fifo_enable = false,
.accl_fifo_enable = false,
.temp_fifo_enable = false,
.magn_fifo_enable = false,
.accl_fs = INV_MPU6050_FS_02G,
.user_ctrl = 0,
};
static const struct inv_mpu6050_chip_config chip_config_6500 = {
.clk = INV_CLK_PLL,
.fsr = INV_MPU6050_FSR_2000DPS,
.lpf = INV_MPU6050_FILTER_20HZ,
.divider = INV_MPU6050_FIFO_RATE_TO_DIVIDER(50),
.gyro_en = true,
.accl_en = true,
.temp_en = true,
.magn_en = false,
.gyro_fifo_enable = false,
.accl_fifo_enable = false,
.temp_fifo_enable = false,
.magn_fifo_enable = false,
.accl_fs = INV_MPU6050_FS_02G,
.user_ctrl = 0,
};
/* Indexed by enum inv_devices */
static const struct inv_mpu6050_hw hw_info[] = {
{
.whoami = INV_MPU6050_WHOAMI_VALUE,
.name = "MPU6050",
.reg = &reg_set_6050,
.config = &chip_config_6050,
.fifo_size = 1024,
.temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE},
.startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU6500_WHOAMI_VALUE,
.name = "MPU6500",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU6515_WHOAMI_VALUE,
.name = "MPU6515",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU6880_WHOAMI_VALUE,
.name = "MPU6880",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 4096,
.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU6000_WHOAMI_VALUE,
.name = "MPU6000",
.reg = &reg_set_6050,
.config = &chip_config_6050,
.fifo_size = 1024,
.temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE},
.startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU9150_WHOAMI_VALUE,
.name = "MPU9150",
.reg = &reg_set_6050,
.config = &chip_config_6050,
.fifo_size = 1024,
.temp = {INV_MPU6050_TEMP_OFFSET, INV_MPU6050_TEMP_SCALE},
.startup_time = {INV_MPU6050_GYRO_STARTUP_TIME, INV_MPU6050_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU9250_WHOAMI_VALUE,
.name = "MPU9250",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_MPU9255_WHOAMI_VALUE,
.name = "MPU9255",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_MPU6500_TEMP_OFFSET, INV_MPU6500_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20608_WHOAMI_VALUE,
.name = "ICM20608",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20608D_WHOAMI_VALUE,
.name = "ICM20608D",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20609_WHOAMI_VALUE,
.name = "ICM20609",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 4 * 1024,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20689_WHOAMI_VALUE,
.name = "ICM20689",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 4 * 1024,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20600_WHOAMI_VALUE,
.name = "ICM20600",
.reg = &reg_set_icm20602,
.config = &chip_config_6500,
.fifo_size = 1008,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_ICM20602_GYRO_STARTUP_TIME, INV_ICM20602_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20602_WHOAMI_VALUE,
.name = "ICM20602",
.reg = &reg_set_icm20602,
.config = &chip_config_6500,
.fifo_size = 1008,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_ICM20602_GYRO_STARTUP_TIME, INV_ICM20602_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_ICM20690_WHOAMI_VALUE,
.name = "ICM20690",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 1024,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_ICM20690_GYRO_STARTUP_TIME, INV_ICM20690_ACCEL_STARTUP_TIME},
},
{
.whoami = INV_IAM20680_WHOAMI_VALUE,
.name = "IAM20680",
.reg = &reg_set_6500,
.config = &chip_config_6500,
.fifo_size = 512,
.temp = {INV_ICM20608_TEMP_OFFSET, INV_ICM20608_TEMP_SCALE},
.startup_time = {INV_MPU6500_GYRO_STARTUP_TIME, INV_MPU6500_ACCEL_STARTUP_TIME},
},
};
static int inv_mpu6050_pwr_mgmt_1_write(struct inv_mpu6050_state *st, bool sleep,
bool cycle, int clock, int temp_dis)
{
u8 val;
if (clock < 0)
clock = st->chip_config.clk;
if (temp_dis < 0)
temp_dis = !st->chip_config.temp_en;
val = clock & INV_MPU6050_BIT_CLK_MASK;
if (temp_dis)
val |= INV_MPU6050_BIT_TEMP_DIS;
if (sleep)
val |= INV_MPU6050_BIT_SLEEP;
if (cycle)
val |= INV_MPU6050_BIT_CYCLE;
dev_dbg(regmap_get_device(st->map), "pwr_mgmt_1: 0x%x\n", val);
return regmap_write(st->map, st->reg->pwr_mgmt_1, val);
}
static int inv_mpu6050_clock_switch(struct inv_mpu6050_state *st,
unsigned int clock)
{
int ret;
switch (st->chip_type) {
case INV_MPU6050:
case INV_MPU6000:
case INV_MPU9150:
/* old chips: switch clock manually */
ret = inv_mpu6050_pwr_mgmt_1_write(st, false, false, clock, -1);
if (ret)
return ret;
st->chip_config.clk = clock;
break;
default:
/* automatic clock switching, nothing to do */
break;
}
return 0;
}
int inv_mpu6050_switch_engine(struct inv_mpu6050_state *st, bool en,
unsigned int mask)
{
unsigned int sleep, val;
u8 pwr_mgmt2, user_ctrl;
int ret;
/* delete useless requests */
if (mask & INV_MPU6050_SENSOR_ACCL && en == st->chip_config.accl_en)
mask &= ~INV_MPU6050_SENSOR_ACCL;
if (mask & INV_MPU6050_SENSOR_GYRO && en == st->chip_config.gyro_en)
mask &= ~INV_MPU6050_SENSOR_GYRO;
if (mask & INV_MPU6050_SENSOR_TEMP && en == st->chip_config.temp_en)
mask &= ~INV_MPU6050_SENSOR_TEMP;
if (mask & INV_MPU6050_SENSOR_MAGN && en == st->chip_config.magn_en)
mask &= ~INV_MPU6050_SENSOR_MAGN;
if (mask & INV_MPU6050_SENSOR_WOM && en == st->chip_config.wom_en)
mask &= ~INV_MPU6050_SENSOR_WOM;
/* force accel on if WoM is on and not going off */
if (!en && (mask & INV_MPU6050_SENSOR_ACCL) && st->chip_config.wom_en &&
!(mask & INV_MPU6050_SENSOR_WOM))
mask &= ~INV_MPU6050_SENSOR_ACCL;
if (mask == 0)
return 0;
/* turn on/off temperature sensor */
if (mask & INV_MPU6050_SENSOR_TEMP) {
ret = inv_mpu6050_pwr_mgmt_1_write(st, false, false, -1, !en);
if (ret)
return ret;
st->chip_config.temp_en = en;
}
/* update user_crtl for driving magnetometer */
if (mask & INV_MPU6050_SENSOR_MAGN) {
user_ctrl = st->chip_config.user_ctrl;
if (en)
user_ctrl |= INV_MPU6050_BIT_I2C_MST_EN;
else
user_ctrl &= ~INV_MPU6050_BIT_I2C_MST_EN;
ret = regmap_write(st->map, st->reg->user_ctrl, user_ctrl);
if (ret)
return ret;
st->chip_config.user_ctrl = user_ctrl;
st->chip_config.magn_en = en;
}
/* manage accel & gyro engines */
if (mask & (INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO)) {
/* compute power management 2 current value */
pwr_mgmt2 = 0;
if (!st->chip_config.accl_en)
pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY;
if (!st->chip_config.gyro_en)
pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY;
/* update to new requested value */
if (mask & INV_MPU6050_SENSOR_ACCL) {
if (en)
pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_ACCL_STBY;
else
pwr_mgmt2 |= INV_MPU6050_BIT_PWR_ACCL_STBY;
}
if (mask & INV_MPU6050_SENSOR_GYRO) {
if (en)
pwr_mgmt2 &= ~INV_MPU6050_BIT_PWR_GYRO_STBY;
else
pwr_mgmt2 |= INV_MPU6050_BIT_PWR_GYRO_STBY;
}
/* switch clock to internal when turning gyro off */
if (mask & INV_MPU6050_SENSOR_GYRO && !en) {
ret = inv_mpu6050_clock_switch(st, INV_CLK_INTERNAL);
if (ret)
return ret;
}
/* update sensors engine */
dev_dbg(regmap_get_device(st->map), "pwr_mgmt_2: 0x%x\n",
pwr_mgmt2);
ret = regmap_write(st->map, st->reg->pwr_mgmt_2, pwr_mgmt2);
if (ret)
return ret;
if (mask & INV_MPU6050_SENSOR_ACCL)
st->chip_config.accl_en = en;
if (mask & INV_MPU6050_SENSOR_GYRO)
st->chip_config.gyro_en = en;
/* compute required time to have sensors stabilized */
sleep = 0;
if (en) {
if (mask & INV_MPU6050_SENSOR_ACCL) {
if (sleep < st->hw->startup_time.accel)
sleep = st->hw->startup_time.accel;
}
if (mask & INV_MPU6050_SENSOR_GYRO) {
if (sleep < st->hw->startup_time.gyro)
sleep = st->hw->startup_time.gyro;
}
} else {
if (mask & INV_MPU6050_SENSOR_GYRO) {
if (sleep < INV_MPU6050_GYRO_DOWN_TIME)
sleep = INV_MPU6050_GYRO_DOWN_TIME;
}
}
if (sleep)
msleep(sleep);
/* switch clock to PLL when turning gyro on */
if (mask & INV_MPU6050_SENSOR_GYRO && en) {
ret = inv_mpu6050_clock_switch(st, INV_CLK_PLL);
if (ret)
return ret;
}
}
/* enable/disable accel intelligence control */
if (mask & INV_MPU6050_SENSOR_WOM) {
val = en ? INV_MPU6500_BIT_ACCEL_INTEL_EN |
INV_MPU6500_BIT_ACCEL_INTEL_MODE : 0;
ret = regmap_write(st->map, INV_MPU6500_REG_ACCEL_INTEL_CTRL, val);
if (ret)
return ret;
st->chip_config.wom_en = en;
}
return 0;
}
static int inv_mpu6050_set_power_itg(struct inv_mpu6050_state *st,
bool power_on)
{
int result;
result = inv_mpu6050_pwr_mgmt_1_write(st, !power_on, false, -1, -1);
if (result)
return result;
if (power_on)
usleep_range(INV_MPU6050_REG_UP_TIME_MIN,
INV_MPU6050_REG_UP_TIME_MAX);
return 0;
}
static int inv_mpu6050_set_gyro_fsr(struct inv_mpu6050_state *st,
enum inv_mpu6050_fsr_e val)
{
unsigned int gyro_shift;
u8 data;
switch (st->chip_type) {
case INV_ICM20690:
gyro_shift = INV_ICM20690_GYRO_CONFIG_FSR_SHIFT;
break;
default:
gyro_shift = INV_MPU6050_GYRO_CONFIG_FSR_SHIFT;
break;
}
data = val << gyro_shift;
return regmap_write(st->map, st->reg->gyro_config, data);
}
static int inv_mpu6050_set_accel_lpf_regs(struct inv_mpu6050_state *st,
enum inv_mpu6050_filter_e val)
{
switch (st->chip_type) {
case INV_MPU6050:
case INV_MPU6000:
case INV_MPU9150:
/* old chips, nothing to do */
return 0;
case INV_ICM20689:
case INV_ICM20690:
/* set FIFO size to maximum value */
val |= INV_ICM20689_BITS_FIFO_SIZE_MAX;
break;
default:
break;
}
return regmap_write(st->map, st->reg->accel_lpf, val);
}
/*
* inv_mpu6050_set_lpf_regs() - set low pass filter registers, chip dependent
*
* MPU60xx/MPU9150 use only 1 register for accelerometer + gyroscope
* MPU6500 and above have a dedicated register for accelerometer
*/
static int inv_mpu6050_set_lpf_regs(struct inv_mpu6050_state *st,
enum inv_mpu6050_filter_e val)
{
int result;
result = regmap_write(st->map, st->reg->lpf, val);
if (result)
return result;
/* set accel lpf */
return inv_mpu6050_set_accel_lpf_regs(st, val);
}
/*
* inv_mpu6050_init_config() - Initialize hardware, disable FIFO.
*
* Initial configuration:
* FSR: ± 2000DPS
* DLPF: 20Hz
* FIFO rate: 50Hz
* Clock source: Gyro PLL
*/
static int inv_mpu6050_init_config(struct iio_dev *indio_dev)
{
int result;
u8 d;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
struct inv_sensors_timestamp_chip timestamp;
result = inv_mpu6050_set_gyro_fsr(st, st->chip_config.fsr);
if (result)
return result;
result = inv_mpu6050_set_lpf_regs(st, st->chip_config.lpf);
if (result)
return result;
d = st->chip_config.divider;
result = regmap_write(st->map, st->reg->sample_rate_div, d);
if (result)
return result;
d = (st->chip_config.accl_fs << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
result = regmap_write(st->map, st->reg->accl_config, d);
if (result)
return result;
result = regmap_write(st->map, st->reg->int_pin_cfg, st->irq_mask);
if (result)
return result;
/* clock jitter is +/- 2% */
timestamp.clock_period = NSEC_PER_SEC / INV_MPU6050_INTERNAL_FREQ_HZ;
timestamp.jitter = 20;
timestamp.init_period =
NSEC_PER_SEC / INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
inv_sensors_timestamp_init(&st->timestamp, &timestamp);
/* magn chip init, noop if not present in the chip */
result = inv_mpu_magn_probe(st);
if (result)
return result;
return 0;
}
static int inv_mpu6050_sensor_set(struct inv_mpu6050_state *st, int reg,
int axis, int val)
{
int ind;
__be16 d = cpu_to_be16(val);
ind = (axis - IIO_MOD_X) * 2;
return regmap_bulk_write(st->map, reg + ind, &d, sizeof(d));
}
static int inv_mpu6050_sensor_show(struct inv_mpu6050_state *st, int reg,
int axis, int *val)
{
int ind, result;
__be16 d;
ind = (axis - IIO_MOD_X) * 2;
result = regmap_bulk_read(st->map, reg + ind, &d, sizeof(d));
if (result)
return result;
*val = (short)be16_to_cpup(&d);
return IIO_VAL_INT;
}
static int inv_mpu6050_read_channel_data(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int *val)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
struct device *pdev = regmap_get_device(st->map);
unsigned int freq_hz, period_us, min_sleep_us, max_sleep_us;
int result;
int ret;
/* compute sample period */
freq_hz = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
period_us = 1000000 / freq_hz;
result = pm_runtime_resume_and_get(pdev);
if (result)
return result;
switch (chan->type) {
case IIO_ANGL_VEL:
if (!st->chip_config.gyro_en) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_SENSOR_GYRO);
if (result)
goto error_power_off;
/* need to wait 2 periods to have first valid sample */
min_sleep_us = 2 * period_us;
max_sleep_us = 2 * (period_us + period_us / 2);
usleep_range(min_sleep_us, max_sleep_us);
}
ret = inv_mpu6050_sensor_show(st, st->reg->raw_gyro,
chan->channel2, val);
break;
case IIO_ACCEL:
if (!st->chip_config.accl_en) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_SENSOR_ACCL);
if (result)
goto error_power_off;
/* wait 1 period for first sample availability */
min_sleep_us = period_us;
max_sleep_us = period_us + period_us / 2;
usleep_range(min_sleep_us, max_sleep_us);
}
ret = inv_mpu6050_sensor_show(st, st->reg->raw_accl,
chan->channel2, val);
break;
case IIO_TEMP:
/* temperature sensor work only with accel and/or gyro */
if (!st->chip_config.accl_en && !st->chip_config.gyro_en) {
result = -EBUSY;
goto error_power_off;
}
if (!st->chip_config.temp_en) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_SENSOR_TEMP);
if (result)
goto error_power_off;
/* wait 1 period for first sample availability */
min_sleep_us = period_us;
max_sleep_us = period_us + period_us / 2;
usleep_range(min_sleep_us, max_sleep_us);
}
ret = inv_mpu6050_sensor_show(st, st->reg->temperature,
IIO_MOD_X, val);
break;
case IIO_MAGN:
if (!st->chip_config.magn_en) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_SENSOR_MAGN);
if (result)
goto error_power_off;
/* frequency is limited for magnetometer */
if (freq_hz > INV_MPU_MAGN_FREQ_HZ_MAX) {
freq_hz = INV_MPU_MAGN_FREQ_HZ_MAX;
period_us = 1000000 / freq_hz;
}
/* need to wait 2 periods to have first valid sample */
min_sleep_us = 2 * period_us;
max_sleep_us = 2 * (period_us + period_us / 2);
usleep_range(min_sleep_us, max_sleep_us);
}
ret = inv_mpu_magn_read(st, chan->channel2, val);
break;
default:
ret = -EINVAL;
break;
}
pm_runtime_mark_last_busy(pdev);
pm_runtime_put_autosuspend(pdev);
return ret;
error_power_off:
pm_runtime_put_autosuspend(pdev);
return result;
}
static int
inv_mpu6050_read_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int *val, int *val2, long mask)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
int ret = 0;
switch (mask) {
case IIO_CHAN_INFO_RAW:
ret = iio_device_claim_direct_mode(indio_dev);
if (ret)
return ret;
mutex_lock(&st->lock);
ret = inv_mpu6050_read_channel_data(indio_dev, chan, val);
mutex_unlock(&st->lock);
iio_device_release_direct_mode(indio_dev);
return ret;
case IIO_CHAN_INFO_SCALE:
switch (chan->type) {
case IIO_ANGL_VEL:
mutex_lock(&st->lock);
*val = 0;
*val2 = gyro_scale_6050[st->chip_config.fsr];
mutex_unlock(&st->lock);
return IIO_VAL_INT_PLUS_NANO;
case IIO_ACCEL:
mutex_lock(&st->lock);
*val = 0;
*val2 = accel_scale[st->chip_config.accl_fs];
mutex_unlock(&st->lock);
return IIO_VAL_INT_PLUS_MICRO;
case IIO_TEMP:
*val = st->hw->temp.scale / 1000000;
*val2 = st->hw->temp.scale % 1000000;
return IIO_VAL_INT_PLUS_MICRO;
case IIO_MAGN:
return inv_mpu_magn_get_scale(st, chan, val, val2);
default:
return -EINVAL;
}
case IIO_CHAN_INFO_OFFSET:
switch (chan->type) {
case IIO_TEMP:
*val = st->hw->temp.offset;
return IIO_VAL_INT;
default:
return -EINVAL;
}
case IIO_CHAN_INFO_CALIBBIAS:
switch (chan->type) {
case IIO_ANGL_VEL:
mutex_lock(&st->lock);
ret = inv_mpu6050_sensor_show(st, st->reg->gyro_offset,
chan->channel2, val);
mutex_unlock(&st->lock);
return ret;
case IIO_ACCEL:
mutex_lock(&st->lock);
ret = inv_mpu6050_sensor_show(st, st->reg->accl_offset,
chan->channel2, val);
mutex_unlock(&st->lock);
return ret;
default:
return -EINVAL;
}
default:
return -EINVAL;
}
}
static int inv_mpu6050_write_gyro_scale(struct inv_mpu6050_state *st, int val,
int val2)
{
int result, i;
if (val != 0)
return -EINVAL;
for (i = 0; i < ARRAY_SIZE(gyro_scale_6050); ++i) {
if (gyro_scale_6050[i] == val2) {
result = inv_mpu6050_set_gyro_fsr(st, i);
if (result)
return result;
st->chip_config.fsr = i;
return 0;
}
}
return -EINVAL;
}
static int inv_write_raw_get_fmt(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan, long mask)
{
switch (mask) {
case IIO_CHAN_INFO_SCALE:
switch (chan->type) {
case IIO_ANGL_VEL:
return IIO_VAL_INT_PLUS_NANO;
default:
return IIO_VAL_INT_PLUS_MICRO;
}
default:
return IIO_VAL_INT_PLUS_MICRO;
}
return -EINVAL;
}
static int inv_mpu6050_write_accel_scale(struct inv_mpu6050_state *st, int val,
int val2)
{
int result, i;
u8 d;
if (val != 0)
return -EINVAL;
for (i = 0; i < ARRAY_SIZE(accel_scale); ++i) {
if (accel_scale[i] == val2) {
d = (i << INV_MPU6050_ACCL_CONFIG_FSR_SHIFT);
result = regmap_write(st->map, st->reg->accl_config, d);
if (result)
return result;
st->chip_config.accl_fs = i;
return 0;
}
}
return -EINVAL;
}
static int inv_mpu6050_write_raw(struct iio_dev *indio_dev,
struct iio_chan_spec const *chan,
int val, int val2, long mask)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
struct device *pdev = regmap_get_device(st->map);
int result;
/*
* we should only update scale when the chip is disabled, i.e.
* not running
*/
result = iio_device_claim_direct_mode(indio_dev);
if (result)
return result;
mutex_lock(&st->lock);
result = pm_runtime_resume_and_get(pdev);
if (result)
goto error_write_raw_unlock;
switch (mask) {
case IIO_CHAN_INFO_SCALE:
switch (chan->type) {
case IIO_ANGL_VEL:
result = inv_mpu6050_write_gyro_scale(st, val, val2);
break;
case IIO_ACCEL:
result = inv_mpu6050_write_accel_scale(st, val, val2);
break;
default:
result = -EINVAL;
break;
}
break;
case IIO_CHAN_INFO_CALIBBIAS:
switch (chan->type) {
case IIO_ANGL_VEL:
result = inv_mpu6050_sensor_set(st,
st->reg->gyro_offset,
chan->channel2, val);
break;
case IIO_ACCEL:
result = inv_mpu6050_sensor_set(st,
st->reg->accl_offset,
chan->channel2, val);
break;
default:
result = -EINVAL;
break;
}
break;
default:
result = -EINVAL;
break;
}
pm_runtime_mark_last_busy(pdev);
pm_runtime_put_autosuspend(pdev);
error_write_raw_unlock:
mutex_unlock(&st->lock);
iio_device_release_direct_mode(indio_dev);
return result;
}
static u64 inv_mpu6050_convert_wom_to_roc(unsigned int threshold, unsigned int freq_div)
{
/* 4mg per LSB converted in m/s² in micro (1000000) */
const unsigned int convert = 4U * 9807U;
u64 value;
value = threshold * convert;
/* compute the differential by multiplying by the frequency */
return div_u64(value * INV_MPU6050_INTERNAL_FREQ_HZ, freq_div);
}
static unsigned int inv_mpu6050_convert_roc_to_wom(u64 roc, unsigned int freq_div)
{
/* 4mg per LSB converted in m/s² in micro (1000000) */
const unsigned int convert = 4U * 9807U;
u64 value;
/* return 0 only if roc is 0 */
if (roc == 0)
return 0;
value = div_u64(roc * freq_div, convert * INV_MPU6050_INTERNAL_FREQ_HZ);
/* limit value to 8 bits and prevent 0 */
return min(255, max(1, value));
}
static int inv_mpu6050_set_wom_int(struct inv_mpu6050_state *st, bool on)
{
unsigned int reg_val, val;
switch (st->chip_type) {
case INV_MPU6050:
case INV_MPU6500:
case INV_MPU6515:
case INV_MPU6880:
case INV_MPU6000:
case INV_MPU9150:
case INV_MPU9250:
case INV_MPU9255:
reg_val = INV_MPU6500_BIT_WOM_INT_EN;
break;
default:
reg_val = INV_ICM20608_BIT_WOM_INT_EN;
break;
}
val = on ? reg_val : 0;
return regmap_update_bits(st->map, st->reg->int_enable, reg_val, val);
}
static int inv_mpu6050_set_wom_threshold(struct inv_mpu6050_state *st, u64 value,
unsigned int freq_div)
{
unsigned int threshold;
int result;
/* convert roc to wom threshold and convert back to handle clipping */
threshold = inv_mpu6050_convert_roc_to_wom(value, freq_div);
value = inv_mpu6050_convert_wom_to_roc(threshold, freq_div);
dev_dbg(regmap_get_device(st->map), "wom_threshold: 0x%x\n", threshold);
switch (st->chip_type) {
case INV_ICM20609:
case INV_ICM20689:
case INV_ICM20600:
case INV_ICM20602:
case INV_ICM20690:
st->data[0] = threshold;
st->data[1] = threshold;
st->data[2] = threshold;
result = regmap_bulk_write(st->map, INV_ICM20609_REG_ACCEL_WOM_X_THR,
st->data, 3);
break;
default:
result = regmap_write(st->map, INV_MPU6500_REG_WOM_THRESHOLD, threshold);
break;
}
if (result)
return result;
st->chip_config.roc_threshold = value;
return 0;
}
static int inv_mpu6050_set_lp_odr(struct inv_mpu6050_state *st, unsigned int freq_div,
unsigned int *lp_div)
{
static const unsigned int freq_dividers[] = {2, 4, 8, 16, 32, 64, 128, 256};
static const unsigned int reg_values[] = {
INV_MPU6050_LPOSC_500HZ, INV_MPU6050_LPOSC_250HZ,
INV_MPU6050_LPOSC_125HZ, INV_MPU6050_LPOSC_62HZ,
INV_MPU6050_LPOSC_31HZ, INV_MPU6050_LPOSC_16HZ,
INV_MPU6050_LPOSC_8HZ, INV_MPU6050_LPOSC_4HZ,
};
unsigned int val, i;
switch (st->chip_type) {
case INV_ICM20609:
case INV_ICM20689:
case INV_ICM20600:
case INV_ICM20602:
case INV_ICM20690:
/* nothing to do */
*lp_div = INV_MPU6050_FREQ_DIVIDER(st);
return 0;
default:
break;
}
/* found the nearest superior frequency divider */
i = ARRAY_SIZE(reg_values) - 1;
val = reg_values[i];
*lp_div = freq_dividers[i];
for (i = 0; i < ARRAY_SIZE(freq_dividers); ++i) {
if (freq_div <= freq_dividers[i]) {
val = reg_values[i];
*lp_div = freq_dividers[i];
break;
}
}
dev_dbg(regmap_get_device(st->map), "lp_odr: 0x%x\n", val);
return regmap_write(st->map, INV_MPU6500_REG_LP_ODR, val);
}
static int inv_mpu6050_set_wom_lp(struct inv_mpu6050_state *st, bool on)
{
unsigned int lp_div;
int result;
if (on) {
/* set low power ODR */
result = inv_mpu6050_set_lp_odr(st, INV_MPU6050_FREQ_DIVIDER(st), &lp_div);
if (result)
return result;
/* disable accel low pass filter */
result = inv_mpu6050_set_accel_lpf_regs(st, INV_MPU6050_FILTER_NOLPF);
if (result)
return result;
/* update wom threshold with new low-power frequency divider */
result = inv_mpu6050_set_wom_threshold(st, st->chip_config.roc_threshold, lp_div);
if (result)
return result;
/* set cycle mode */
result = inv_mpu6050_pwr_mgmt_1_write(st, false, true, -1, -1);
} else {
/* disable cycle mode */
result = inv_mpu6050_pwr_mgmt_1_write(st, false, false, -1, -1);
if (result)
return result;
/* restore wom threshold */
result = inv_mpu6050_set_wom_threshold(st, st->chip_config.roc_threshold,
INV_MPU6050_FREQ_DIVIDER(st));
if (result)
return result;
/* restore accel low pass filter */
result = inv_mpu6050_set_accel_lpf_regs(st, st->chip_config.lpf);
}
return result;
}
static int inv_mpu6050_enable_wom(struct inv_mpu6050_state *st, bool en)
{
struct device *pdev = regmap_get_device(st->map);
unsigned int mask;
int result;
if (en) {
result = pm_runtime_resume_and_get(pdev);
if (result)
return result;
mask = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_WOM;
result = inv_mpu6050_switch_engine(st, true, mask);
if (result)
goto error_suspend;
result = inv_mpu6050_set_wom_int(st, true);
if (result)
goto error_suspend;
} else {
result = inv_mpu6050_set_wom_int(st, false);
if (result)
dev_err(pdev, "error %d disabling WoM interrupt bit", result);
/* disable only WoM and let accel be disabled by autosuspend */
result = inv_mpu6050_switch_engine(st, false, INV_MPU6050_SENSOR_WOM);
if (result) {
dev_err(pdev, "error %d disabling WoM force off", result);
/* force WoM off */
st->chip_config.wom_en = false;
}
pm_runtime_mark_last_busy(pdev);
pm_runtime_put_autosuspend(pdev);
}
return result;
error_suspend:
pm_runtime_mark_last_busy(pdev);
pm_runtime_put_autosuspend(pdev);
return result;
}
static int inv_mpu6050_read_event_config(struct iio_dev *indio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
/* support only WoM (accel roc rising) event */
if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||
dir != IIO_EV_DIR_RISING)
return -EINVAL;
guard(mutex)(&st->lock);
return st->chip_config.wom_en ? 1 : 0;
}
static int inv_mpu6050_write_event_config(struct iio_dev *indio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir,
int state)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
int enable;
/* support only WoM (accel roc rising) event */
if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||
dir != IIO_EV_DIR_RISING)
return -EINVAL;
enable = !!state;
guard(mutex)(&st->lock);
if (st->chip_config.wom_en == enable)
return 0;
return inv_mpu6050_enable_wom(st, enable);
}
static int inv_mpu6050_read_event_value(struct iio_dev *indio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir,
enum iio_event_info info,
int *val, int *val2)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
u32 rem;
/* support only WoM (accel roc rising) event value */
if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||
dir != IIO_EV_DIR_RISING || info != IIO_EV_INFO_VALUE)
return -EINVAL;
guard(mutex)(&st->lock);
/* return value in micro */
*val = div_u64_rem(st->chip_config.roc_threshold, 1000000U, &rem);
*val2 = rem;
return IIO_VAL_INT_PLUS_MICRO;
}
static int inv_mpu6050_write_event_value(struct iio_dev *indio_dev,
const struct iio_chan_spec *chan,
enum iio_event_type type,
enum iio_event_direction dir,
enum iio_event_info info,
int val, int val2)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
struct device *pdev = regmap_get_device(st->map);
u64 value;
int result;
/* support only WoM (accel roc rising) event value */
if (chan->type != IIO_ACCEL || type != IIO_EV_TYPE_ROC ||
dir != IIO_EV_DIR_RISING || info != IIO_EV_INFO_VALUE)
return -EINVAL;
if (val < 0 || val2 < 0)
return -EINVAL;
guard(mutex)(&st->lock);
result = pm_runtime_resume_and_get(pdev);
if (result)
return result;
value = (u64)val * 1000000ULL + (u64)val2;
result = inv_mpu6050_set_wom_threshold(st, value, INV_MPU6050_FREQ_DIVIDER(st));
pm_runtime_mark_last_busy(pdev);
pm_runtime_put_autosuspend(pdev);
return result;
}
/*
* inv_mpu6050_set_lpf() - set low pass filer based on fifo rate.
*
* Based on the Nyquist principle, the bandwidth of the low
* pass filter must not exceed the signal sampling rate divided
* by 2, or there would be aliasing.
* This function basically search for the correct low pass
* parameters based on the fifo rate, e.g, sampling frequency.
*
* lpf is set automatically when setting sampling rate to avoid any aliases.
*/
static int inv_mpu6050_set_lpf(struct inv_mpu6050_state *st, int rate)
{
static const int hz[] = {400, 200, 90, 40, 20, 10};
static const int d[] = {
INV_MPU6050_FILTER_200HZ, INV_MPU6050_FILTER_100HZ,
INV_MPU6050_FILTER_45HZ, INV_MPU6050_FILTER_20HZ,
INV_MPU6050_FILTER_10HZ, INV_MPU6050_FILTER_5HZ
};
int i, result;
u8 data;
data = INV_MPU6050_FILTER_5HZ;
for (i = 0; i < ARRAY_SIZE(hz); ++i) {
if (rate >= hz[i]) {
data = d[i];
break;
}
}
result = inv_mpu6050_set_lpf_regs(st, data);
if (result)
return result;
st->chip_config.lpf = data;
return 0;
}
/*
* inv_mpu6050_fifo_rate_store() - Set fifo rate.
*/
static ssize_t
inv_mpu6050_fifo_rate_store(struct device *dev, struct device_attribute *attr,
const char *buf, size_t count)
{
int fifo_rate;
u32 fifo_period;
bool fifo_on;
u8 d;
int result;
struct iio_dev *indio_dev = dev_to_iio_dev(dev);
struct inv_mpu6050_state *st = iio_priv(indio_dev);
struct device *pdev = regmap_get_device(st->map);
if (kstrtoint(buf, 10, &fifo_rate))
return -EINVAL;
if (fifo_rate < INV_MPU6050_MIN_FIFO_RATE ||
fifo_rate > INV_MPU6050_MAX_FIFO_RATE)
return -EINVAL;
/* compute the chip sample rate divider */
d = INV_MPU6050_FIFO_RATE_TO_DIVIDER(fifo_rate);
/* compute back the fifo rate to handle truncation cases */
fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(d);
fifo_period = NSEC_PER_SEC / fifo_rate;
mutex_lock(&st->lock);
if (d == st->chip_config.divider) {
result = 0;
goto fifo_rate_fail_unlock;
}
fifo_on = st->chip_config.accl_fifo_enable ||
st->chip_config.gyro_fifo_enable ||
st->chip_config.magn_fifo_enable;
result = inv_sensors_timestamp_update_odr(&st->timestamp, fifo_period, fifo_on);
if (result)
goto fifo_rate_fail_unlock;
result = pm_runtime_resume_and_get(pdev);
if (result)
goto fifo_rate_fail_unlock;
result = regmap_write(st->map, st->reg->sample_rate_div, d);
if (result)
goto fifo_rate_fail_power_off;
st->chip_config.divider = d;
result = inv_mpu6050_set_lpf(st, fifo_rate);
if (result)
goto fifo_rate_fail_power_off;
/* update rate for magn, noop if not present in chip */
result = inv_mpu_magn_set_rate(st, fifo_rate);
if (result)
goto fifo_rate_fail_power_off;
/* update wom threshold since roc is dependent on sampling frequency */
result = inv_mpu6050_set_wom_threshold(st, st->chip_config.roc_threshold,
INV_MPU6050_FREQ_DIVIDER(st));
if (result)
goto fifo_rate_fail_power_off;
pm_runtime_mark_last_busy(pdev);
fifo_rate_fail_power_off:
pm_runtime_put_autosuspend(pdev);
fifo_rate_fail_unlock:
mutex_unlock(&st->lock);
if (result)
return result;
return count;
}
/*
* inv_fifo_rate_show() - Get the current sampling rate.
*/
static ssize_t
inv_fifo_rate_show(struct device *dev, struct device_attribute *attr,
char *buf)
{
struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
unsigned fifo_rate;
mutex_lock(&st->lock);
fifo_rate = INV_MPU6050_DIVIDER_TO_FIFO_RATE(st->chip_config.divider);
mutex_unlock(&st->lock);
return scnprintf(buf, PAGE_SIZE, "%u\n", fifo_rate);
}
/*
* inv_attr_show() - calling this function will show current
* parameters.
*
* Deprecated in favor of IIO mounting matrix API.
*
* See inv_get_mount_matrix()
*/
static ssize_t inv_attr_show(struct device *dev, struct device_attribute *attr,
char *buf)
{
struct inv_mpu6050_state *st = iio_priv(dev_to_iio_dev(dev));
struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
s8 *m;
switch (this_attr->address) {
/*
* In MPU6050, the two matrix are the same because gyro and accel
* are integrated in one chip
*/
case ATTR_GYRO_MATRIX:
case ATTR_ACCL_MATRIX:
m = st->plat_data.orientation;
return scnprintf(buf, PAGE_SIZE,
"%d, %d, %d; %d, %d, %d; %d, %d, %d\n",
m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8]);
default:
return -EINVAL;
}
}
/**
* inv_mpu6050_validate_trigger() - validate_trigger callback for invensense
* MPU6050 device.
* @indio_dev: The IIO device
* @trig: The new trigger
*
* Returns: 0 if the 'trig' matches the trigger registered by the MPU6050
* device, -EINVAL otherwise.
*/
static int inv_mpu6050_validate_trigger(struct iio_dev *indio_dev,
struct iio_trigger *trig)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
if (st->trig != trig)
return -EINVAL;
return 0;
}
static const struct iio_mount_matrix *
inv_get_mount_matrix(const struct iio_dev *indio_dev,
const struct iio_chan_spec *chan)
{
struct inv_mpu6050_state *data = iio_priv(indio_dev);
const struct iio_mount_matrix *matrix;
if (chan->type == IIO_MAGN)
matrix = &data->magn_orient;
else
matrix = &data->orientation;
return matrix;
}
static const struct iio_chan_spec_ext_info inv_ext_info[] = {
IIO_MOUNT_MATRIX(IIO_SHARED_BY_TYPE, inv_get_mount_matrix),
{ }
};
static const struct iio_event_spec inv_wom_events[] = {
{
.type = IIO_EV_TYPE_ROC,
.dir = IIO_EV_DIR_RISING,
.mask_separate = BIT(IIO_EV_INFO_ENABLE) |
BIT(IIO_EV_INFO_VALUE),
},
};
#define INV_MPU6050_CHAN(_type, _channel2, _index) \
{ \
.type = _type, \
.modified = 1, \
.channel2 = _channel2, \
.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
BIT(IIO_CHAN_INFO_CALIBBIAS), \
.scan_index = _index, \
.scan_type = { \
.sign = 's', \
.realbits = 16, \
.storagebits = 16, \
.shift = 0, \
.endianness = IIO_BE, \
}, \
.ext_info = inv_ext_info, \
}
#define INV_MPU6050_TEMP_CHAN(_index) \
{ \
.type = IIO_TEMP, \
.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) \
| BIT(IIO_CHAN_INFO_OFFSET) \
| BIT(IIO_CHAN_INFO_SCALE), \
.scan_index = _index, \
.scan_type = { \
.sign = 's', \
.realbits = 16, \
.storagebits = 16, \
.shift = 0, \
.endianness = IIO_BE, \
}, \
}
#define INV_MPU6050_EVENT_CHAN(_type, _channel2, _events, _events_nb) \
{ \
.type = _type, \
.modified = 1, \
.channel2 = _channel2, \
.event_spec = _events, \
.num_event_specs = _events_nb, \
.scan_index = -1, \
}
static const struct iio_chan_spec inv_mpu6050_channels[] = {
IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP),
INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
};
static const struct iio_chan_spec inv_mpu6500_channels[] = {
IIO_CHAN_SOFT_TIMESTAMP(INV_MPU6050_SCAN_TIMESTAMP),
INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
INV_MPU6050_EVENT_CHAN(IIO_ACCEL, IIO_MOD_X_OR_Y_OR_Z,
inv_wom_events, ARRAY_SIZE(inv_wom_events)),
};
#define INV_MPU6050_SCAN_MASK_3AXIS_ACCEL \
(BIT(INV_MPU6050_SCAN_ACCL_X) \
| BIT(INV_MPU6050_SCAN_ACCL_Y) \
| BIT(INV_MPU6050_SCAN_ACCL_Z))
#define INV_MPU6050_SCAN_MASK_3AXIS_GYRO \
(BIT(INV_MPU6050_SCAN_GYRO_X) \
| BIT(INV_MPU6050_SCAN_GYRO_Y) \
| BIT(INV_MPU6050_SCAN_GYRO_Z))
#define INV_MPU6050_SCAN_MASK_TEMP (BIT(INV_MPU6050_SCAN_TEMP))
static const unsigned long inv_mpu_scan_masks[] = {
/* 3-axis accel */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL,
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_TEMP,
/* 3-axis gyro */
INV_MPU6050_SCAN_MASK_3AXIS_GYRO,
INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU6050_SCAN_MASK_TEMP,
/* 6-axis accel + gyro */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO,
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO
| INV_MPU6050_SCAN_MASK_TEMP,
0,
};
#define INV_MPU9X50_MAGN_CHAN(_chan2, _bits, _index) \
{ \
.type = IIO_MAGN, \
.modified = 1, \
.channel2 = _chan2, \
.info_mask_separate = BIT(IIO_CHAN_INFO_SCALE) | \
BIT(IIO_CHAN_INFO_RAW), \
.scan_index = _index, \
.scan_type = { \
.sign = 's', \
.realbits = _bits, \
.storagebits = 16, \
.shift = 0, \
.endianness = IIO_BE, \
}, \
.ext_info = inv_ext_info, \
}
static const struct iio_chan_spec inv_mpu9150_channels[] = {
IIO_CHAN_SOFT_TIMESTAMP(INV_MPU9X50_SCAN_TIMESTAMP),
INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
/* Magnetometer resolution is 13 bits */
INV_MPU9X50_MAGN_CHAN(IIO_MOD_X, 13, INV_MPU9X50_SCAN_MAGN_X),
INV_MPU9X50_MAGN_CHAN(IIO_MOD_Y, 13, INV_MPU9X50_SCAN_MAGN_Y),
INV_MPU9X50_MAGN_CHAN(IIO_MOD_Z, 13, INV_MPU9X50_SCAN_MAGN_Z),
};
static const struct iio_chan_spec inv_mpu9250_channels[] = {
IIO_CHAN_SOFT_TIMESTAMP(INV_MPU9X50_SCAN_TIMESTAMP),
INV_MPU6050_TEMP_CHAN(INV_MPU6050_SCAN_TEMP),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_X, INV_MPU6050_SCAN_GYRO_X),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Y, INV_MPU6050_SCAN_GYRO_Y),
INV_MPU6050_CHAN(IIO_ANGL_VEL, IIO_MOD_Z, INV_MPU6050_SCAN_GYRO_Z),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_X, INV_MPU6050_SCAN_ACCL_X),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Y, INV_MPU6050_SCAN_ACCL_Y),
INV_MPU6050_CHAN(IIO_ACCEL, IIO_MOD_Z, INV_MPU6050_SCAN_ACCL_Z),
/* Magnetometer resolution is 16 bits */
INV_MPU9X50_MAGN_CHAN(IIO_MOD_X, 16, INV_MPU9X50_SCAN_MAGN_X),
INV_MPU9X50_MAGN_CHAN(IIO_MOD_Y, 16, INV_MPU9X50_SCAN_MAGN_Y),
INV_MPU9X50_MAGN_CHAN(IIO_MOD_Z, 16, INV_MPU9X50_SCAN_MAGN_Z),
};
#define INV_MPU9X50_SCAN_MASK_3AXIS_MAGN \
(BIT(INV_MPU9X50_SCAN_MAGN_X) \
| BIT(INV_MPU9X50_SCAN_MAGN_Y) \
| BIT(INV_MPU9X50_SCAN_MAGN_Z))
static const unsigned long inv_mpu9x50_scan_masks[] = {
/* 3-axis accel */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL,
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_TEMP,
/* 3-axis gyro */
INV_MPU6050_SCAN_MASK_3AXIS_GYRO,
INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU6050_SCAN_MASK_TEMP,
/* 3-axis magn */
INV_MPU9X50_SCAN_MASK_3AXIS_MAGN,
INV_MPU9X50_SCAN_MASK_3AXIS_MAGN | INV_MPU6050_SCAN_MASK_TEMP,
/* 6-axis accel + gyro */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO,
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO
| INV_MPU6050_SCAN_MASK_TEMP,
/* 6-axis accel + magn */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN,
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN
| INV_MPU6050_SCAN_MASK_TEMP,
/* 6-axis gyro + magn */
INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN,
INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU9X50_SCAN_MASK_3AXIS_MAGN
| INV_MPU6050_SCAN_MASK_TEMP,
/* 9-axis accel + gyro + magn */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO
| INV_MPU9X50_SCAN_MASK_3AXIS_MAGN,
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO
| INV_MPU9X50_SCAN_MASK_3AXIS_MAGN
| INV_MPU6050_SCAN_MASK_TEMP,
0,
};
static const unsigned long inv_icm20602_scan_masks[] = {
/* 3-axis accel + temp (mandatory) */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_TEMP,
/* 3-axis gyro + temp (mandatory) */
INV_MPU6050_SCAN_MASK_3AXIS_GYRO | INV_MPU6050_SCAN_MASK_TEMP,
/* 6-axis accel + gyro + temp (mandatory) */
INV_MPU6050_SCAN_MASK_3AXIS_ACCEL | INV_MPU6050_SCAN_MASK_3AXIS_GYRO
| INV_MPU6050_SCAN_MASK_TEMP,
0,
};
/*
* The user can choose any frequency between INV_MPU6050_MIN_FIFO_RATE and
* INV_MPU6050_MAX_FIFO_RATE, but only these frequencies are matched by the
* low-pass filter. Specifically, each of these sampling rates are about twice
* the bandwidth of a corresponding low-pass filter, which should eliminate
* aliasing following the Nyquist principle. By picking a frequency different
* from these, the user risks aliasing effects.
*/
static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("10 20 50 100 200 500");
static IIO_CONST_ATTR(in_anglvel_scale_available,
"0.000133090 0.000266181 0.000532362 0.001064724");
static IIO_CONST_ATTR(in_accel_scale_available,
"0.000598 0.001196 0.002392 0.004785");
static IIO_DEV_ATTR_SAMP_FREQ(S_IRUGO | S_IWUSR, inv_fifo_rate_show,
inv_mpu6050_fifo_rate_store);
/* Deprecated: kept for userspace backward compatibility. */
static IIO_DEVICE_ATTR(in_gyro_matrix, S_IRUGO, inv_attr_show, NULL,
ATTR_GYRO_MATRIX);
static IIO_DEVICE_ATTR(in_accel_matrix, S_IRUGO, inv_attr_show, NULL,
ATTR_ACCL_MATRIX);
static struct attribute *inv_attributes[] = {
&iio_dev_attr_in_gyro_matrix.dev_attr.attr, /* deprecated */
&iio_dev_attr_in_accel_matrix.dev_attr.attr, /* deprecated */
&iio_dev_attr_sampling_frequency.dev_attr.attr,
&iio_const_attr_sampling_frequency_available.dev_attr.attr,
&iio_const_attr_in_accel_scale_available.dev_attr.attr,
&iio_const_attr_in_anglvel_scale_available.dev_attr.attr,
NULL,
};
static const struct attribute_group inv_attribute_group = {
.attrs = inv_attributes
};
static int inv_mpu6050_reg_access(struct iio_dev *indio_dev,
unsigned int reg,
unsigned int writeval,
unsigned int *readval)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
int ret;
mutex_lock(&st->lock);
if (readval)
ret = regmap_read(st->map, reg, readval);
else
ret = regmap_write(st->map, reg, writeval);
mutex_unlock(&st->lock);
return ret;
}
static const struct iio_info mpu_info = {
.read_raw = &inv_mpu6050_read_raw,
.write_raw = &inv_mpu6050_write_raw,
.write_raw_get_fmt = &inv_write_raw_get_fmt,
.attrs = &inv_attribute_group,
.read_event_config = inv_mpu6050_read_event_config,
.write_event_config = inv_mpu6050_write_event_config,
.read_event_value = inv_mpu6050_read_event_value,
.write_event_value = inv_mpu6050_write_event_value,
.validate_trigger = inv_mpu6050_validate_trigger,
.debugfs_reg_access = &inv_mpu6050_reg_access,
};
/*
* inv_check_and_setup_chip() - check and setup chip.
*/
static int inv_check_and_setup_chip(struct inv_mpu6050_state *st)
{
int result;
unsigned int regval, mask;
int i;
st->hw = &hw_info[st->chip_type];
st->reg = hw_info[st->chip_type].reg;
memcpy(&st->chip_config, hw_info[st->chip_type].config,
sizeof(st->chip_config));
st->data = devm_kzalloc(regmap_get_device(st->map), st->hw->fifo_size, GFP_KERNEL);
if (st->data == NULL)
return -ENOMEM;
/* check chip self-identification */
result = regmap_read(st->map, INV_MPU6050_REG_WHOAMI, &regval);
if (result)
return result;
if (regval != st->hw->whoami) {
/* check whoami against all possible values */
for (i = 0; i < INV_NUM_PARTS; ++i) {
if (regval == hw_info[i].whoami) {
dev_warn(regmap_get_device(st->map),
"whoami mismatch got 0x%02x (%s) expected 0x%02x (%s)\n",
regval, hw_info[i].name,
st->hw->whoami, st->hw->name);
break;
}
}
if (i >= INV_NUM_PARTS) {
dev_err(regmap_get_device(st->map),
"invalid whoami 0x%02x expected 0x%02x (%s)\n",
regval, st->hw->whoami, st->hw->name);
return -ENODEV;
}
}
/* reset to make sure previous state are not there */
result = regmap_write(st->map, st->reg->pwr_mgmt_1,
INV_MPU6050_BIT_H_RESET);
if (result)
return result;
msleep(INV_MPU6050_POWER_UP_TIME);
switch (st->chip_type) {
case INV_MPU6000:
case INV_MPU6500:
case INV_MPU6515:
case INV_MPU6880:
case INV_MPU9250:
case INV_MPU9255:
/* reset signal path (required for spi connection) */
regval = INV_MPU6050_BIT_TEMP_RST | INV_MPU6050_BIT_ACCEL_RST |
INV_MPU6050_BIT_GYRO_RST;
result = regmap_write(st->map, INV_MPU6050_REG_SIGNAL_PATH_RESET,
regval);
if (result)
return result;
msleep(INV_MPU6050_POWER_UP_TIME);
break;
default:
break;
}
/*
* Turn power on. After reset, the sleep bit could be on
* or off depending on the OTP settings. Turning power on
* make it in a definite state as well as making the hardware
* state align with the software state
*/
result = inv_mpu6050_set_power_itg(st, true);
if (result)
return result;
mask = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO |
INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN;
result = inv_mpu6050_switch_engine(st, false, mask);
if (result)
goto error_power_off;
return 0;
error_power_off:
inv_mpu6050_set_power_itg(st, false);
return result;
}
static int inv_mpu_core_enable_regulator_vddio(struct inv_mpu6050_state *st)
{
int result;
result = regulator_enable(st->vddio_supply);
if (result) {
dev_err(regmap_get_device(st->map),
"Failed to enable vddio regulator: %d\n", result);
} else {
/* Give the device a little bit of time to start up. */
usleep_range(3000, 5000);
}
return result;
}
static int inv_mpu_core_disable_regulator_vddio(struct inv_mpu6050_state *st)
{
int result;
result = regulator_disable(st->vddio_supply);
if (result)
dev_err(regmap_get_device(st->map),
"Failed to disable vddio regulator: %d\n", result);
return result;
}
static void inv_mpu_core_disable_regulator_action(void *_data)
{
struct inv_mpu6050_state *st = _data;
int result;
result = regulator_disable(st->vdd_supply);
if (result)
dev_err(regmap_get_device(st->map),
"Failed to disable vdd regulator: %d\n", result);
inv_mpu_core_disable_regulator_vddio(st);
}
static void inv_mpu_pm_disable(void *data)
{
struct device *dev = data;
pm_runtime_disable(dev);
}
int inv_mpu_core_probe(struct regmap *regmap, int irq, const char *name,
int (*inv_mpu_bus_setup)(struct iio_dev *), int chip_type)
{
struct inv_mpu6050_state *st;
struct iio_dev *indio_dev;
struct inv_mpu6050_platform_data *pdata;
struct device *dev = regmap_get_device(regmap);
int result;
struct irq_data *desc;
int irq_type;
indio_dev = devm_iio_device_alloc(dev, sizeof(*st));
if (!indio_dev)
return -ENOMEM;
BUILD_BUG_ON(ARRAY_SIZE(hw_info) != INV_NUM_PARTS);
if (chip_type < 0 || chip_type >= INV_NUM_PARTS) {
dev_err(dev, "Bad invensense chip_type=%d name=%s\n",
chip_type, name);
return -ENODEV;
}
st = iio_priv(indio_dev);
mutex_init(&st->lock);
st->chip_type = chip_type;
st->irq = irq;
st->map = regmap;
st->level_shifter = device_property_read_bool(dev,
"invensense,level-shifter");
pdata = dev_get_platdata(dev);
if (!pdata) {
result = iio_read_mount_matrix(dev, &st->orientation);
if (result) {
dev_err(dev, "Failed to retrieve mounting matrix %d\n",
result);
return result;
}
} else {
st->plat_data = *pdata;
}
if (irq > 0) {
desc = irq_get_irq_data(irq);
if (!desc) {
dev_err(dev, "Could not find IRQ %d\n", irq);
return -EINVAL;
}
irq_type = irqd_get_trigger_type(desc);
if (!irq_type)
irq_type = IRQF_TRIGGER_RISING;
} else {
/* Doesn't really matter, use the default */
irq_type = IRQF_TRIGGER_RISING;
}
if (irq_type & IRQF_TRIGGER_RISING) // rising or both-edge
st->irq_mask = INV_MPU6050_ACTIVE_HIGH;
else if (irq_type == IRQF_TRIGGER_FALLING)
st->irq_mask = INV_MPU6050_ACTIVE_LOW;
else if (irq_type == IRQF_TRIGGER_HIGH)
st->irq_mask = INV_MPU6050_ACTIVE_HIGH |
INV_MPU6050_LATCH_INT_EN;
else if (irq_type == IRQF_TRIGGER_LOW)
st->irq_mask = INV_MPU6050_ACTIVE_LOW |
INV_MPU6050_LATCH_INT_EN;
else {
dev_err(dev, "Invalid interrupt type 0x%x specified\n",
irq_type);
return -EINVAL;
}
device_set_wakeup_capable(dev, true);
st->vdd_supply = devm_regulator_get(dev, "vdd");
if (IS_ERR(st->vdd_supply))
return dev_err_probe(dev, PTR_ERR(st->vdd_supply),
"Failed to get vdd regulator\n");
st->vddio_supply = devm_regulator_get(dev, "vddio");
if (IS_ERR(st->vddio_supply))
return dev_err_probe(dev, PTR_ERR(st->vddio_supply),
"Failed to get vddio regulator\n");
result = regulator_enable(st->vdd_supply);
if (result) {
dev_err(dev, "Failed to enable vdd regulator: %d\n", result);
return result;
}
msleep(INV_MPU6050_POWER_UP_TIME);
result = inv_mpu_core_enable_regulator_vddio(st);
if (result) {
regulator_disable(st->vdd_supply);
return result;
}
result = devm_add_action_or_reset(dev, inv_mpu_core_disable_regulator_action,
st);
if (result) {
dev_err(dev, "Failed to setup regulator cleanup action %d\n",
result);
return result;
}
/* fill magnetometer orientation */
result = inv_mpu_magn_set_orient(st);
if (result)
return result;
/* power is turned on inside check chip type*/
result = inv_check_and_setup_chip(st);
if (result)
return result;
result = inv_mpu6050_init_config(indio_dev);
if (result) {
dev_err(dev, "Could not initialize device.\n");
goto error_power_off;
}
dev_set_drvdata(dev, indio_dev);
/* name will be NULL when enumerated via ACPI */
if (name)
indio_dev->name = name;
else
indio_dev->name = dev_name(dev);
/* requires parent device set in indio_dev */
if (inv_mpu_bus_setup) {
result = inv_mpu_bus_setup(indio_dev);
if (result)
goto error_power_off;
}
/* chip init is done, turning on runtime power management */
result = pm_runtime_set_active(dev);
if (result)
goto error_power_off;
pm_runtime_get_noresume(dev);
pm_runtime_enable(dev);
pm_runtime_set_autosuspend_delay(dev, INV_MPU6050_SUSPEND_DELAY_MS);
pm_runtime_use_autosuspend(dev);
pm_runtime_put(dev);
result = devm_add_action_or_reset(dev, inv_mpu_pm_disable, dev);
if (result)
return result;
switch (chip_type) {
case INV_MPU6000:
case INV_MPU6050:
indio_dev->channels = inv_mpu6050_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu6050_channels);
indio_dev->available_scan_masks = inv_mpu_scan_masks;
break;
case INV_MPU9150:
indio_dev->channels = inv_mpu9150_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu9150_channels);
indio_dev->available_scan_masks = inv_mpu9x50_scan_masks;
break;
case INV_MPU9250:
case INV_MPU9255:
indio_dev->channels = inv_mpu9250_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu9250_channels);
indio_dev->available_scan_masks = inv_mpu9x50_scan_masks;
break;
case INV_ICM20600:
case INV_ICM20602:
indio_dev->channels = inv_mpu6500_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu6500_channels);
indio_dev->available_scan_masks = inv_icm20602_scan_masks;
break;
default:
indio_dev->channels = inv_mpu6500_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu6500_channels);
indio_dev->available_scan_masks = inv_mpu_scan_masks;
break;
}
/*
* Use magnetometer inside the chip only if there is no i2c
* auxiliary device in use. Otherwise Going back to 6-axis only.
*/
if (st->magn_disabled) {
switch (chip_type) {
case INV_MPU9150:
indio_dev->channels = inv_mpu6050_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu6050_channels);
indio_dev->available_scan_masks = inv_mpu_scan_masks;
break;
default:
indio_dev->channels = inv_mpu6500_channels;
indio_dev->num_channels = ARRAY_SIZE(inv_mpu6500_channels);
indio_dev->available_scan_masks = inv_mpu_scan_masks;
break;
}
}
indio_dev->info = &mpu_info;
if (irq > 0) {
/*
* The driver currently only supports buffered capture with its
* own trigger. So no IRQ, no trigger, no buffer
*/
result = devm_iio_triggered_buffer_setup(dev, indio_dev,
iio_pollfunc_store_time,
inv_mpu6050_read_fifo,
NULL);
if (result) {
dev_err(dev, "configure buffer fail %d\n", result);
return result;
}
result = inv_mpu6050_probe_trigger(indio_dev, irq_type);
if (result) {
dev_err(dev, "trigger probe fail %d\n", result);
return result;
}
}
result = devm_iio_device_register(dev, indio_dev);
if (result) {
dev_err(dev, "IIO register fail %d\n", result);
return result;
}
return 0;
error_power_off:
inv_mpu6050_set_power_itg(st, false);
return result;
}
EXPORT_SYMBOL_NS_GPL(inv_mpu_core_probe, IIO_MPU6050);
static int inv_mpu_resume(struct device *dev)
{
struct iio_dev *indio_dev = dev_get_drvdata(dev);
struct inv_mpu6050_state *st = iio_priv(indio_dev);
bool wakeup;
int result;
guard(mutex)(&st->lock);
wakeup = device_may_wakeup(dev) && st->chip_config.wom_en;
if (wakeup) {
enable_irq(st->irq);
disable_irq_wake(st->irq);
result = inv_mpu6050_set_wom_lp(st, false);
if (result)
return result;
} else {
result = inv_mpu_core_enable_regulator_vddio(st);
if (result)
return result;
result = inv_mpu6050_set_power_itg(st, true);
if (result)
return result;
}
pm_runtime_disable(dev);
pm_runtime_set_active(dev);
pm_runtime_enable(dev);
result = inv_mpu6050_switch_engine(st, true, st->suspended_sensors);
if (result)
return result;
if (st->chip_config.wom_en && !wakeup) {
result = inv_mpu6050_set_wom_int(st, true);
if (result)
return result;
}
if (iio_buffer_enabled(indio_dev))
result = inv_mpu6050_prepare_fifo(st, true);
return result;
}
static int inv_mpu_suspend(struct device *dev)
{
struct iio_dev *indio_dev = dev_get_drvdata(dev);
struct inv_mpu6050_state *st = iio_priv(indio_dev);
bool wakeup;
int result;
guard(mutex)(&st->lock);
st->suspended_sensors = 0;
if (pm_runtime_suspended(dev))
return 0;
if (iio_buffer_enabled(indio_dev)) {
result = inv_mpu6050_prepare_fifo(st, false);
if (result)
return result;
}
wakeup = device_may_wakeup(dev) && st->chip_config.wom_en;
if (st->chip_config.wom_en && !wakeup) {
result = inv_mpu6050_set_wom_int(st, false);
if (result)
return result;
}
if (st->chip_config.accl_en && !wakeup)
st->suspended_sensors |= INV_MPU6050_SENSOR_ACCL;
if (st->chip_config.gyro_en)
st->suspended_sensors |= INV_MPU6050_SENSOR_GYRO;
if (st->chip_config.temp_en)
st->suspended_sensors |= INV_MPU6050_SENSOR_TEMP;
if (st->chip_config.magn_en)
st->suspended_sensors |= INV_MPU6050_SENSOR_MAGN;
if (st->chip_config.wom_en && !wakeup)
st->suspended_sensors |= INV_MPU6050_SENSOR_WOM;
result = inv_mpu6050_switch_engine(st, false, st->suspended_sensors);
if (result)
return result;
if (wakeup) {
result = inv_mpu6050_set_wom_lp(st, true);
if (result)
return result;
enable_irq_wake(st->irq);
disable_irq(st->irq);
} else {
result = inv_mpu6050_set_power_itg(st, false);
if (result)
return result;
inv_mpu_core_disable_regulator_vddio(st);
}
return 0;
}
static int inv_mpu_runtime_suspend(struct device *dev)
{
struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
unsigned int sensors;
int ret;
mutex_lock(&st->lock);
sensors = INV_MPU6050_SENSOR_ACCL | INV_MPU6050_SENSOR_GYRO |
INV_MPU6050_SENSOR_TEMP | INV_MPU6050_SENSOR_MAGN |
INV_MPU6050_SENSOR_WOM;
ret = inv_mpu6050_switch_engine(st, false, sensors);
if (ret)
goto out_unlock;
ret = inv_mpu6050_set_power_itg(st, false);
if (ret)
goto out_unlock;
inv_mpu_core_disable_regulator_vddio(st);
out_unlock:
mutex_unlock(&st->lock);
return ret;
}
static int inv_mpu_runtime_resume(struct device *dev)
{
struct inv_mpu6050_state *st = iio_priv(dev_get_drvdata(dev));
int ret;
ret = inv_mpu_core_enable_regulator_vddio(st);
if (ret)
return ret;
return inv_mpu6050_set_power_itg(st, true);
}
EXPORT_NS_GPL_DEV_PM_OPS(inv_mpu_pmops, IIO_MPU6050) = {
SYSTEM_SLEEP_PM_OPS(inv_mpu_suspend, inv_mpu_resume)
RUNTIME_PM_OPS(inv_mpu_runtime_suspend, inv_mpu_runtime_resume, NULL)
};
MODULE_AUTHOR("Invensense Corporation");
MODULE_DESCRIPTION("Invensense device MPU6050 driver");
MODULE_LICENSE("GPL");
MODULE_IMPORT_NS(IIO_INV_SENSORS_TIMESTAMP);

复制代码

1.3 新增设备节点

这里我们将MPU6050接到i2c1接口，因此需要适当调整设备树。
1.3.1 i2c1

i2c1节点定义在arch/loongarch/boot/dts/loongson-2k0300.dtsi：

i2c1: i2c1@0x16109000 {
compatible = "loongson,lsfs-i2c";
reg = <0 0x16109000 0 0x1000>;
#address-cells = <1>;
#size-cells = <0>;
interrupt-parent = <&liointc0>;
interrupts = <4 IRQ_TYPE_LEVEL_HIGH>;
pinctrl-0 = <&i2c1_pins>;
pinctrl-names = "default";
clock-frequency = <200000000>;
status = "disabled";
};

复制代码

其中：

i2c1:：标签，用于在设备树中其他地方通过&i2c1引用这个节点，方便添加属性或修改状态；
i2c1@0x16109000：节点名，格式为设备名@寄存器基地址。这里i2c1是功能名，0x16109000是该I2C控制器的物理寄存器基地址。
compatible：驱动匹配字符串。内核通过该属性寻找能驱动此设备的驱动程序；
reg：寄存器地址范围。格式为，由于龙芯采用64位寻址，这里用两个32位数表示64位地址；
#address-cells和#size-cells：定义该节点下子节点（即挂载的I2C从设备）的地址和长度格式。
- #address-cells = ：子节点的 reg 属性中，地址部分占用1个32位单元。对于I2C设备，这通常是7位从设备地址；
- #size-cells = ：子节点的 reg 属性中没有长度字段（因为I2C设备地址不涉及地址范围）；
interrupt-parent：指定该设备的中断路由到哪个中断控制器。&liointc0 是龙芯2K0300内部的中断控制器节点（即龙芯I/O中断控制器）的标签；
interrupts：描述中断线的具体信息；
- ：硬件中断编号（对应I2C1控制器在中断控制器中的编号）；
- IRQ_TYPE_LEVEL_HIGH：中断触发类型，这里定义为高电平触发。该宏在中定义。
pinctrl-0：指定设备使用的第一组引脚配置，&i2c1_pins 是另一个设备树节点的标签，该节点描述了I2C1的SCL和SDA引脚应复用为I2C功能，并可能包含上拉等电气属性；
pinctrl-names：为引脚的配置状态命名，与 pinctrl-0 对应。"default" 是默认状态，驱动在probe时会自动应用 pinctrl-0 的配置，引脚配置设置为&i2c1_pins；
clock-frequency：I2C控制器的输入时钟频率（单位Hz）；
status：设备状态，此时处于禁用状态。

更多有关设备树相关的内容可以参考：《linux设备树-基础介绍》。
1.3.2 mpu6050@68

zhengyang@ubuntu:~$ cd /opt/2k0300/build-2k0300/workspace/linux-6.12
zhengyang@ubuntu:/opt/2k0300/build-2k0300/workspace/linux-6.12$ vim arch/loongarch/boot/dts/ls2k300_99pi.dtsi

复制代码

修改arch/loongarch/boot/dts/ls2k300_99pi.dtsi：

// 在根节点或合适位置（通常是 &soc 内）引用并启用 I2C1
&i2c1 {
status = "okay";
......
// MPU6050 传感器
mpu6050@68 {
compatible = "invensense,mpu6050";
reg = <0x68>;
status = "okay";
};
};

复制代码

通过 &i2c1 引用这个节点，将 status 改为 "okay"，同时添加了I2C从设备节点。
其中mpu6050@68设备节点下的compatible名称要和inv_mpu_i2c.c驱动中的inv_mpu_driver.driver.of_match_table里的名称匹配。

/*
* device id table is used to identify what device can be
* supported by this driver
*/
static const struct i2c_device_id inv_mpu_id[] = {
{"mpu6050", INV_MPU6050},
{"mpu6500", INV_MPU6500},
{"mpu6515", INV_MPU6515},
{"mpu6880", INV_MPU6880},
{"mpu9150", INV_MPU9150},
{"mpu9250", INV_MPU9250},
{"mpu9255", INV_MPU9255},
{"icm20608", INV_ICM20608},
{"icm20608d", INV_ICM20608D},
{"icm20609", INV_ICM20609},
{"icm20689", INV_ICM20689},
{"icm20600", INV_ICM20600},
{"icm20602", INV_ICM20602},
{"icm20690", INV_ICM20690},
{"iam20680", INV_IAM20680},
{}
};
MODULE_DEVICE_TABLE(i2c, inv_mpu_id);
static const struct of_device_id inv_of_match[] = {
{
.compatible = "invensense,mpu6050",
.data = (void *)INV_MPU6050
},
{
.compatible = "invensense,mpu6500",
.data = (void *)INV_MPU6500
},
{
.compatible = "invensense,mpu6515",
.data = (void *)INV_MPU6515
},
......
}
MODULE_DEVICE_TABLE(of, inv_of_match);
static const struct acpi_device_id inv_acpi_match[] = {
{"INVN6500", INV_MPU6500},
{ },
};
MODULE_DEVICE_TABLE(acpi, inv_acpi_match);
static struct i2c_driver inv_mpu_driver = {
.probe = inv_mpu_probe,
.remove = inv_mpu_remove,
.id_table = inv_mpu_id,
.driver = {
.of_match_table = inv_of_match,
.acpi_match_table = inv_acpi_match,
.name = "inv-mpu6050-i2c",
.pm = pm_ptr(&inv_mpu_pmops),
},
};

复制代码

这样在适配器注册的时候会遍历i2c设备树节点下的所有子设备节点，从而实现注册该控制器下的所有I2C从设备，具体可以参考《I2C适配器注册》源码部分。
1.3.3 i2c1_pins

i2c1_pins定义在arch/loongarch/boot/dts/loongson-2k0300.dtsi：

pinmux: pinmux@16000490 {
......
i2c1_pins: pinmux_G50_G51_as_i2c1 {
pinctrl-single,bits = <0xc 0x000000f0 0x000000f0>;
};
......
}

复制代码

这个设备树节点是使用pinctrl-single驱动来配置引脚复用功能的典型写法，这行代码会在寄存器0x1600049c的7:4位写入 0xf，而其他位保持不变，用于将芯片的GPIO50和GPIO51这两个引脚设置为I2C1功能。

注：pinctrl-single 是一个通用的引脚控制驱动，适用于那些引脚复用寄存器是“单寄存器位域”的芯片。当SoC没有提供复杂的pinctrl框架时，可以直接用这种方式“裸写”寄存器。
二、应用程序

接下来我们在example目录下创建子目录mpu6050_app；

zhengyang@ubuntu:~$ cd /opt/2k0300/loongson_2k300_lib/example
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/example$ mkdir mpu6050_app

复制代码

目录结构如下：

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/example/mpu6050_app$ tree .
.
├── main.c
└── Makefile

复制代码

2.1 main.c

内核自带驱动把MPU6050注册成IIO传感器，应用层直接读文件即可获取距离。下面编写一个简单的测试应用程序，用于读取数据并打印；

#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
// 根据你的实际设备路径修改，通常是 iio:device0 或 iio:device1
#define IIO_DEVICE_PATH "/sys/bus/iio/devices/iio:device1"
// 灵敏度配置 (MPU6050 默认配置)
#define ACC_SENSITIVITY 16384.0f
#define GYRO_SENSITIVITY 16.4f
#define GRAVITY 9.8f
#define PI 3.1415926f
// 互补滤波系数
#define ALPHA 0.98f
// --- 全局变量 ---
float gyro_zero_offset_x = 0.0f;
float gyro_zero_offset_y = 0.0f;
float gyro_zero_offset_z = 0.0f;
// 姿态角 (弧度)
float pitch = 0.0f; // 俯仰角
float roll = 0.0f; // 横滚角
// --- 辅助函数：读取 sysfs 文件 ---
int read_sysfs_int(const char *path)
{
int fd;
char buf[32];
int val;
// 拼接完整路径
char full_path[256];
snprintf(full_path, sizeof(full_path), "%s/%s", IIO_DEVICE_PATH, path);
fd = open(full_path, O_RDONLY);
if (fd < 0) return 0; // 读取失败返回0
memset(buf, 0, sizeof(buf));
read(fd, buf, sizeof(buf) - 1);
close(fd);
val = atoi(buf);
return val;
}
// --- 辅助函数：获取时间差 (秒) ---
double get_time_diff(struct timeval *last, struct timeval *now)
{
return (now->tv_sec - last->tv_sec) + (now->tv_usec - last->tv_usec) / 1000000.0;
}
// --- 1. 校准陀螺仪 ---
void calibrate_gyro()
{
printf("正在校准陀螺仪，请保持开发板绝对静止...\n");
int samples = 500; // 采样次数
long sum_x = 0, sum_y = 0, sum_z = 0;
for (int i = 0; i < samples; i++) {
sum_x += read_sysfs_int("in_anglvel_x_raw");
sum_y += read_sysfs_int("in_anglvel_y_raw");
sum_z += read_sysfs_int("in_anglvel_z_raw");
usleep(2000); // 2ms 间隔
}
// 计算平均值作为零偏 (转换为 rad/s)
gyro_zero_offset_x = (sum_x / (float)samples) / GYRO_SENSITIVITY * (PI / 180.0f);
gyro_zero_offset_y = (sum_y / (float)samples) / GYRO_SENSITIVITY * (PI / 180.0f);
gyro_zero_offset_z = (sum_z / (float)samples) / GYRO_SENSITIVITY * (PI / 180.0f);
printf("校准完成！零偏: X=%.4f, Y=%.4f, Z=%.4f (rad/s)\n",
gyro_zero_offset_x, gyro_zero_offset_y, gyro_zero_offset_z);
}
// --- 主函数 ---
int main()
{
struct timeval last_time, current_time;
double dt;
// 1. 初始化时间
gettimeofday(&last_time, NULL);
// 2. 执行校准
calibrate_gyro();
printf("开始输出姿态数据 (Pitch: 前后倾, Roll: 左右倾)\n");
printf("单位: 度 (°)\n");
while (1) {
// --- A. 读取原始数据 ---
int acc_x_raw = read_sysfs_int("in_accel_x_raw");
int acc_y_raw = read_sysfs_int("in_accel_y_raw");
int acc_z_raw = read_sysfs_int("in_accel_z_raw");
int gyro_x_raw = read_sysfs_int("in_anglvel_x_raw");
int gyro_y_raw = read_sysfs_int("in_anglvel_y_raw");
int gyro_z_raw = read_sysfs_int("in_anglvel_z_raw");
// --- B. 单位转换 ---
// 加速度转 m/s^2
float acc_x = (acc_x_raw / ACC_SENSITIVITY) * GRAVITY;
float acc_y = (acc_y_raw / ACC_SENSITIVITY) * GRAVITY;
float acc_z = (acc_z_raw / ACC_SENSITIVITY) * GRAVITY;
// 角速度转 rad/s 并减去零偏
float gyro_x = (gyro_x_raw / GYRO_SENSITIVITY) * (PI / 180.0f) - gyro_zero_offset_x;
float gyro_y = (gyro_y_raw / GYRO_SENSITIVITY) * (PI / 180.0f) - gyro_zero_offset_y;
float gyro_z = (gyro_z_raw / GYRO_SENSITIVITY) * (PI / 180.0f) - gyro_zero_offset_z;
// --- C. 计算时间间隔 dt ---
gettimeofday(&current_time, NULL);
dt = get_time_diff(&last_time, &current_time);
last_time = current_time;
// 防止 dt 过大导致积分发散（比如调试暂停过）
if (dt > 0.1) dt = 0.01;
// --- D. 姿态解算 ---
// 1. 加速度计计算角度 (参考角度)
// Pitch (绕Y轴旋转)
float acc_pitch = atan2(acc_x, sqrt(acc_y*acc_y + acc_z*acc_z)) * (180.0f / PI);
// Roll (绕X轴旋转)
float acc_roll = atan2(acc_y, sqrt(acc_x*acc_x + acc_z*acc_z)) * (180.0f / PI);
// 2. 陀螺仪积分 (变化量)
// 注意：这里简化了坐标系转换，仅适用于小角度或特定安装方向
float gyro_pitch = gyro_y * (180.0f / PI) * dt;
float gyro_roll = gyro_x * (180.0f / PI) * dt;
// 3. 互补滤波融合
// 新角度 = 0.98 * (旧角度 + 陀螺仪变化) + 0.02 * 加速度计角度
pitch = ALPHA * (pitch + gyro_pitch) + (1.0f - ALPHA) * acc_pitch;
roll = ALPHA * (roll + gyro_roll) + (1.0f - ALPHA) * acc_roll;
// --- E. 打印结果 ---
// \r 用于覆盖当前行，实现刷新效果
printf("\rPitch: %6.2f° | Roll: %6.2f° | AccZ: %.2f m/s^2", pitch, roll, acc_z);
fflush(stdout);
usleep(10000); // 10ms 刷新率 (100Hz)
}
return 0;
}

复制代码

2.2 Makefile

all:
loongarch64-linux-gnu-gcc -o main main.c
clean:
rm -rf *.o main

复制代码

2.3 编译应用程序

zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/example/mpu6050_app$ make
loongarch64-linux-gnu-gcc -o main main.c
zhengyang@ubuntu:/opt/2k0300/loongson_2k300_lib/example/mpu6050_app$ ll

复制代码

三、测试

3.1 烧录设备树

3.1.1 编译设备树

如果需要单独编译设备树，在linux内核根目录执行如下命令：

zhengyang@ubuntu:~$ cd /opt/2k0300/build-2k0300/workspace/linux-6.12
zhengyang@ubuntu:/opt/2k0300/build-2k0300/workspace/linux-6.12$ source ../set_env.sh && make arch/loongarch/boot/dts/ls2k300_99pi_wifi.dts dtbs V=1

复制代码

3.1.2 更新设备树

将设备树拷贝到久久派的/opt目录；

zhengyang@ubuntu:/opt/2k0300/build-2k0300/workspace/linux-6.12$ scp arch/loongarch/boot/dts/ls2k300_99pi_wifi.dtb root@172.23.34.188:/opt

复制代码

在久久派使用dd命令烧写设备树到SPI Nor Flash的dtb分区；

[root@LS-GD opt]# dd if=/opt/ls2k300_99pi_wifi.dtb of=/dev/mtdblock3 bs=1
22124+0 records in
22124+0 records out
22124 bytes (22 kB, 22 KiB) copied, 0.500342 s, 44.2 kB/s

复制代码

3.2 安装驱动

由于我们在内核配置环节将驱动配置到内核中了，因此需要重新编译内核并烧录内核。
3.2.1 编译内核

ubuntu宿主接重新编译内核：

zhengyang@ubuntu:/opt/2k0300/build-2k0300/workspace/linux-6.12$ source ../set_env.sh && make uImage -j$(nproc)

复制代码

3.2.2 烧录内核

久久派烧录内核，即将编译生成的uImage（内核镜像）拷贝到/boot目录；

[root@LS-GD ~]# scp zhengyang@172.23.34.187:/opt/2k0300/build-2k0300/workspace/linux-6.12/arch/loongarch/boot/uImage /boot/
[root@LS-GD ~]# reboot

复制代码

3.2.3 查看i2c信息

我们将龙邱MPU6050陀螺仪接入开发板的I2C1接口处，进入I2C总线的控制目录：

# I2C 总线的控制目录
[root@LS-GD ~]# cd /sys/bus/i2c/
[root@LS-GD i2c]# ls -l
drwxr-xr-x 2 root root 0 Jul 24 20:49 devices
drwxr-xr-x 10 root root 0 Jul 24 20:49 drivers
-rw-r--r-- 1 root root 16384 Jul 24 20:51 drivers_autoprobe
--w------- 1 root root 16384 Jul 24 20:51 drivers_probe
--w------- 1 root root 16384 Jul 24 20:49 uevent

复制代码

其中：

devices：列出了所有连接在I2C总线上的设备；
drivers：列出了所有已注册的I2C驱动程序；
drivers_autoprobe、drivers_probe：这些是控制文件，用于手动触发驱动探测或设置自动探测。

进入设备列表目录，查看具体挂载了哪些硬件：

[root@LS-GD i2c]# cd devices/
[root@LS-GD devices]# ls -l
# i2c-1：代表I2C控制器 1-0068：代表连接在 i2c-1 总线上的设备。
lrwxrwxrwx 1 root root 0 Jul 24 20:49 1-0029 -> ../../../devices/platform/2k300-soc/16109000.i2c1/i2c-1/1-0029
lrwxrwxrwx 1 root root 0 Jul 24 20:49 1-0068 -> ../../../devices/platform/2k300-soc/16109000.i2c1/i2c-1/1-0068
lrwxrwxrwx 1 root root 0 Jul 24 20:49 i2c-1 -> ../../../devices/platform/2k300-soc/16109000.i2c1/i2c-1
lrwxrwxrwx 1 root root 0 Jul 24 20:49 i2c-4 -> ../../../devices/platform/2k300-soc/16109000.i2c1/i2c-1/i2c-4

复制代码

进入了这个地址为0x68的设备目录：

[root@LS-GD devices]# cd 1-0068
[root@LS-GD 1-0068]# ls -l
lrwxrwxrwx 1 root root 0 Jul 24 20:54 channel-0 -> ../i2c-4
lrwxrwxrwx 1 root root 0 Jul 24 20:54 driver -> ../../../../../../bus/i2c/drivers/inv-mpu6050-i2c
drwxr-xr-x 3 root root 0 Jul 24 20:49 iio:device1
-r--r--r-- 1 root root 16384 Jul 24 20:49 modalias
-r--r--r-- 1 root root 16384 Jul 24 20:49 name
lrwxrwxrwx 1 root root 0 Jul 24 20:54 of_node -> ../../../../../../firmware/devicetree/base/2k300-soc/i2c1@0x16109000/mpu6050@68
drwxr-xr-x 2 root root 0 Jul 24 20:54 power
lrwxrwxrwx 1 root root 0 Jul 24 20:49 subsystem -> ../../../../../../bus/i2c
-rw-r--r-- 1 root root 16384 Jul 24 20:49 uevent

复制代码

其中：

driver：绑定的驱动：内核已经设备和inv-mpu6050-i2c驱动绑定在了一起；
iio:device1：生成的输入设备节点；
of_node：设备树节点匹配。

3.3 查看iio信息

我们将龙邱MPU6050陀螺仪接入开发板的I2C1接口处，查看IIO设备相关文件：

[root@LS-GD i2c]# ls /sys/bus/iio/devices/iio\:device1/ -l
-rw-r--r-- 1 root root 16384 Jul 24 20:56 current_timestamp_clock
-r--r--r-- 1 root root 16384 Jul 24 20:56 in_accel_matrix
-r--r--r-- 1 root root 16384 Jul 24 20:56 in_accel_mount_matrix
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_scale
-r--r--r-- 1 root root 16384 Jul 24 20:56 in_accel_scale_available
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_x_calibbias
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_x_raw
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_y_calibbias
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_y_raw
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_z_calibbias
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_accel_z_raw
-r--r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_mount_matrix
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_scale
-r--r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_scale_available
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_x_calibbias
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_x_raw
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_y_calibbias
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_y_raw
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_z_calibbias
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_anglvel_z_raw
-r--r--r-- 1 root root 16384 Jul 24 20:56 in_gyro_matrix
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_temp_offset
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_temp_raw
-rw-r--r-- 1 root root 16384 Jul 24 20:56 in_temp_scale
-r--r--r-- 1 root root 16384 Jul 24 20:49 name
lrwxrwxrwx 1 root root 0 Jul 24 20:56 of_node -> ../../../../../../../firmware/devicetree/base/2k300-soc/i2c1@0x16109000/mpu6050@68
drwxr-xr-x 2 root root 0 Jul 24 20:56 power
-rw-r--r-- 1 root root 16384 Jul 24 20:56 sampling_frequency
-r--r--r-- 1 root root 16384 Jul 24 20:56 sampling_frequency_available
lrwxrwxrwx 1 root root 0 Jul 24 20:49 subsystem -> ../../../../../../../bus/iio
-rw-r--r-- 1 root root 16384 Jul 24 20:49 uevent
-r--r--r-- 1 root root 16384 Jul 24 20:56 waiting_for_supplier

复制代码

查看传感器名称：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/name
mpu6050

复制代码

3.3.1 加速度计相关文件

文件说明示例值 in_accel_x_raw X轴加速度原始值 16384 in_accel_y_raw Y轴加速度原始值 -2048 in_accel_z_raw Z轴加速度原始值 16384 in_accel_scale 缩放系数（将原始值转换为g/s^2） 0.000598 in_accel_scale_available 支持的缩放系数 -- in_accel_x_calibbias X轴加速度校准偏置（可写，用于零点校准） 0 in_accel_y_calibbias Y轴加速度校准偏置（可写，用于零点校准） 0 in_accel_z_calibbias Z轴加速度校准偏置（可写，用于零点校准） 0 读取加速度计x轴原始值：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_accel_x_raw
-788

复制代码

读取加速度计y轴原始值：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_accel_y_raw
-206

复制代码

读取加速度计z轴原始值：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_accel_z_raw
15616

复制代码

查看缩放系数（每一个原始数据LSB代表多少$m/s^2$））：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_accel_scale
0.000598

复制代码

查看支持的缩放系数：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_accel_scale_available
0.000598 0.001196 0.002392 0.004785

复制代码

如何将将读取的原始值转换为实际值呢？

\[Accel(m/s^2)=\frac{Raw_{Value}}{16384.0}×9.8 = Raw_Value \times 0.000598 \]
注意：静止时，Z轴应该约为9.8，X/Y轴约为0。
3.3.2 陀螺仪相关文件

文件说明示例值 in_anglvel_x_raw X轴角速度原始值 -256 in_anglvel_y_raw Y轴角速度原始值 128 in_anglvel_z_raw Z轴角速度原始值 64 in_anglvel_scale 缩放系数（原始值→rad/s） 0.001064724 in_anglvel_scale_available 支持的缩放系数 -- in_anglvel_x_calibbias X轴角速度校准偏置（可写） 0 in_anglvel_y_calibbias Y轴角速度校准偏置（可写） 0 in_anglvel_z_calibbias Z轴角速度校准偏置（可写） 0 读取角速度计x轴原始值：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_anglvel_x_raw
-62

复制代码

读取角速度计y轴原始值：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_anglvel_y_raw
6

复制代码

读取角速度计z轴原始值：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_anglvel_z_raw
-20

复制代码

查看缩放系数（每一个原始数据LSB代表多少弧度/秒（rad/s））：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_anglvel_scale
0.001064724

复制代码

查看支持的缩放系数：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/in_anglvel_scale_available
0.000133090 0.000266181 0.000532362 0.001064724

复制代码

如何将将读取的原始值转换为实际值呢？

\[Accel(m/s2) = \frac {Raw_{Value}} {16.4} × \frac{\pi}{180} = Raw_Value \times 0.001064724 \]
3.3.3 温度传感器文件

文件说明示例值 in_temp_raw 温度原始值 1638 in_temp_offset 温度偏移 0 in_temp_scale 温度缩放系数 1 3.3.4 采样频率控制

文件说明 sampling_frequency 当前采样频率（Hz） sampling_frequency_available 支持的采样频率列表当前采样频率：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/sampling_frequency
50

复制代码

支持的采样频率列表：

[root@LS-GD i2c]# cat /sys/bus/iio/devices/iio\:device1/sampling_frequency_available
10 20 50 100 200 500

复制代码

3.3 应用程序测试

3.3.1 校准

校准的核心目的是算出零偏。加速度计校准：

把MPU6050水平静止放在桌面上；
理论上Z轴应该是1g (9.8 m/s² )，X/Y轴是0；
但在代码中，我们通常先不校准加速度计，因为它受重力影响，计算偏置比较麻烦。对于简单的倾角计算，直接用原始数据即可。

陀螺仪校准：

陀螺仪非常容易“漂移”，即使不动，它也会输出比如 10 或 -5 这种小数值，积分后角度会乱飞；
方法：程序启动时，保持开发板绝对静止1-2秒。程序采集几百次数据取平均值，这个平均值就是零偏。以后每次读取数据都要减去这个零偏。

校准程序在我们的main.c文件中已经实现。
3.3.2 测试

我们将龙邱MPU6050陀螺仪接入开发板的I2C1接口处。
在久久派开发板执行如下命令：

[root@LS-GD opt]# scp zhengyang@172.23.34.187:/opt/2k0300/loongson_2k300_lib/example/mpu6050_app/main ./
zhengyang@172.23.34.187's password:
main 100% 20KB 1.4MB/s 00:00
[root@LS-GD opt]# ./main

复制代码

参考文章
[1] LS2K0300久久派_V1.1板卡使用手册v1.2_20240705.pdf
[2] [Linux IIO驱动
[3] Linux下IIO子系统的使用与源码分析
[4] Linux下IIO子系统驱动
[5] linux IIO驱动框架开发流程说明

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！

账号		自动登录	找回密码
密码			立即注册

龙芯2k0300 - 走马观碑组MPU6050驱动移植

相关帖子

签约作者

龙芯2k0300 - 走马观碑组MPU6050驱动移植

相关帖子

相关推荐

龙芯2k0300 - 走马观碑组vl53l0x驱动移植

龙芯2k0300 - 走马观碑组ST7735驱动移植

龙芯2k0300 - 久久派开发环境搭建及内核升级（下）

龙芯2k0300 - 久久派开发环境搭建及内核升级（下）

签约作者