Received: by 2002:a05:6a10:1287:0:0:0:0 with SMTP id d7csp1067493pxv; Fri, 16 Jul 2021 00:28:10 -0700 (PDT) X-Google-Smtp-Source: ABdhPJyK+1Bpvjh8z7oEtHojZc7LF5IDCDktcdULiZebc1LVpa4QY+ibliX+NTj7iIW3yLRYk8Ti X-Received: by 2002:a17:906:5d0b:: with SMTP id g11mr10262995ejt.437.1626420490152; Fri, 16 Jul 2021 00:28:10 -0700 (PDT) ARC-Seal: i=1; a=rsa-sha256; t=1626420490; cv=none; d=google.com; s=arc-20160816; b=vTM4FoysUHwlQrtNjQMQa1D51wdNf8fHPgxcxUP0fuWFm4w3n8hKVVkyK1iDeiq9i8 IeRTB7Z6+yOB8dCZd9FZ5Sv8MIxlx/7/lH/N9Sux0/q7f0wyoQOWJiFpcKVDEzvhjk+H 5cFlyZJLjgq5KjxXNokVqpFECPmJyFqdlMUJX1h/mIsPD8fJSIBt+iBmdDHCmJExGpIU eSTjuIILsrK9RwWqcYlYj8H7lp+oPsh2rqbN/F9wtNE6j4prIBT58XPd44uPU5vIqncy 9Hl3/vsnLFlMLD4qx+qg6Iq32YczUK8vUEJY3M3IAhS0KjetqRPmOcHWktV/MZOfF5x7 Lt8A== ARC-Message-Signature: i=1; a=rsa-sha256; c=relaxed/relaxed; d=google.com; s=arc-20160816; h=list-id:precedence:cc:to:subject:message-id:date:from:in-reply-to :references:mime-version:dkim-signature; bh=moYYiHa/l29c9o5hzMphrS0YmtC6yx63D5ojnxNnuv4=; b=GzE+Qacflg2tcKy75HyKBgjK2jYr2G06J1SQ3OxwXaVpPd64xYJmUsrgk3bACk555f cw/bb6971xqT3EsI406kF8opotDAGncn6tvgf9x0OhN4BtEqdksQw0vKYhvkxsybGCo7 ilopvCQuC3yYs3HLU+JXC0pbqpcJsFDlaCfwz4LGKQ6Utg8nE/9EgXeN+UQxuHA0KP0D 02iJMyIkzUn8V/eEJzgh9oGTB0sQgIxRIt/cTREJOwWPF6pLw5/FDNgM6NcZHBMXVehM /InVPQnUBu0uuOiUUdY0pU8s9fe7hH8B3GNcztejf3ZZ8KiB5UocK5Lq+S7VkO8eIrSU W6FA== ARC-Authentication-Results: i=1; mx.google.com; dkim=pass header.i=@gmail.com header.s=20161025 header.b=VK9AI7kA; spf=pass (google.com: domain of linux-kernel-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) smtp.mailfrom=linux-kernel-owner@vger.kernel.org; dmarc=pass (p=NONE sp=QUARANTINE dis=NONE) header.from=gmail.com Return-Path: Received: from vger.kernel.org (vger.kernel.org. [23.128.96.18]) by mx.google.com with ESMTP id z10si9836305eja.338.2021.07.16.00.27.46; Fri, 16 Jul 2021 00:28:10 -0700 (PDT) Received-SPF: pass (google.com: domain of linux-kernel-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) client-ip=23.128.96.18; Authentication-Results: mx.google.com; dkim=pass header.i=@gmail.com header.s=20161025 header.b=VK9AI7kA; spf=pass (google.com: domain of linux-kernel-owner@vger.kernel.org designates 23.128.96.18 as permitted sender) smtp.mailfrom=linux-kernel-owner@vger.kernel.org; dmarc=pass (p=NONE sp=QUARANTINE dis=NONE) header.from=gmail.com Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S235691AbhGPH1L (ORCPT + 99 others); Fri, 16 Jul 2021 03:27:11 -0400 Received: from lindbergh.monkeyblade.net ([23.128.96.19]:57676 "EHLO lindbergh.monkeyblade.net" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S231737AbhGPH1K (ORCPT ); Fri, 16 Jul 2021 03:27:10 -0400 Received: from mail-pj1-x102c.google.com (mail-pj1-x102c.google.com [IPv6:2607:f8b0:4864:20::102c]) by lindbergh.monkeyblade.net (Postfix) with ESMTPS id 1A6ACC06175F; Fri, 16 Jul 2021 00:24:15 -0700 (PDT) Received: by mail-pj1-x102c.google.com with SMTP id v18-20020a17090ac912b0290173b9578f1cso7123169pjt.0; Fri, 16 Jul 2021 00:24:15 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=20161025; h=mime-version:references:in-reply-to:from:date:message-id:subject:to :cc; bh=moYYiHa/l29c9o5hzMphrS0YmtC6yx63D5ojnxNnuv4=; b=VK9AI7kAFWYP3dDsi21QzAk1ZYJB/ku/SEcHYvz3DP70c9jC9vIv3KZuxvG1sO+b2r DQCIvzI599wMubxFKEyhZk2caTOfmi+DOdjsG7yz/TO8HJGJaxTX3OLaxns0aaYoM/oT 7HVdaUqvQfzbVA5/XpFg35C87D00SaV/NetPEDakCVTaIHenMjxv/O2AzExdWmxkzMkm Ks6dIIoHBKr6m34rYp3cUfZPF9HAePZWAgExS24Gg0rcDP41F04HQ8fY4SVsN82pCKry Zeli3lhOa2PUOs8rDlueFQscguDjo3MNWljmuCoSO61st3Ye/TrB9Jv4G73HRJLfZAAm rK3w== X-Google-DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=1e100.net; s=20161025; h=x-gm-message-state:mime-version:references:in-reply-to:from:date :message-id:subject:to:cc; bh=moYYiHa/l29c9o5hzMphrS0YmtC6yx63D5ojnxNnuv4=; b=UxCxrt2wjfTXUfHMoKKZHJBzmT64QNdZFfYI/Iw9Fw7H1pjoawEpPXHFAng9rvdQ6P 0yISTUPb2971wiowW3Fl21nPnbZEJMUcKHTdBkZJTIzAP/CvXZjzwfeeCqdX5vyHqNzO 9b0pPG/jugGPndpjAcDCWhpTbis5QnslQgR49Q0+BeR90Uj0sR8owV+WOD+8j7BkvzoL zyuMxco2PYVwuKRJ5WJK+hl3nc8/IwRd+ZBzkYnvI0ItMB7PFQqodE1R49LNSz4ka5Mw eBx7CEc1JyC5yYrNqed3Hmf+2sBzYDqm2CqvwpXer7YXLyK2USdfo44nARSpbIr0Mrpr j/Eg== X-Gm-Message-State: AOAM531ADlEMTh/6Vc31vWANbmt8GyaMSWateyj0+Lq/IaxNGGpx7iJu JOs5cHq+qKM9rP3WKfLCUeNdsmW0T7sK+Jbvr3Y= X-Received: by 2002:a17:90a:728d:: with SMTP id e13mr8633596pjg.181.1626420254363; Fri, 16 Jul 2021 00:24:14 -0700 (PDT) MIME-Version: 1.0 References: <20210715141742.15072-1-andrea.merello@gmail.com> <20210715141742.15072-3-andrea.merello@gmail.com> In-Reply-To: <20210715141742.15072-3-andrea.merello@gmail.com> From: Alexandru Ardelean Date: Fri, 16 Jul 2021 10:24:02 +0300 Message-ID: Subject: Re: [PATCH 2/4] iio: imu: add Bosch Sensortec BNO055 core driver To: Andrea Merello Cc: Jonathan Cameron , Lars-Peter Clausen , Rob Herring , matt.ranostay@konsulko.com, Andy Shevchenko , vlad.dogaru@intel.com, LKML , linux-iio , Andrea Merello Content-Type: text/plain; charset="UTF-8" Precedence: bulk List-ID: X-Mailing-List: linux-kernel@vger.kernel.org On Thu, Jul 15, 2021 at 5:21 PM Andrea Merello wrote: > > This patch adds a core driver for the BNO055 IMU from Bosch. This IMU > can be connected via both serial and I2C busses; separate patches will > add support for them. > Hey, I tried to comment on the ones that Andy did not touch. I had a little trouble following all the locking. I'm not sure if I missed anything. My notes are inline. > The driver supports "AMG" (Accelerometer, Magnetometer, Gyroscope) mode, > that provides raw data from the said internal sensors, and a couple of > "fusion" modes (i.e. the IMU also do calculations in order to provide > euler angles, quaternions, linear acceleration and gravity measurements). > > In fusion modes the AMG data is still available (with some calibration > refinements done by the IMU), but certain settings such as low pass > filters cut-off frequency and sensors ranges are fixed, while in AMG mode > they can be customized; this is why AMG mode can still be interesting. > > Signed-off-by: Andrea Merello > Cc: Andrea Merello > Cc: Rob Herring > Cc: Matt Ranostay > Cc: Andy Shevchenko > Cc: Vlad Dogaru > Cc: linux-kernel@vger.kernel.org > Cc: linux-iio@vger.kernel.org > --- > drivers/iio/imu/Kconfig | 1 + > drivers/iio/imu/Makefile | 1 + > drivers/iio/imu/bno055/Kconfig | 7 + > drivers/iio/imu/bno055/Makefile | 6 + > drivers/iio/imu/bno055/bno055.c | 1361 +++++++++++++++++++++++++++++++ > drivers/iio/imu/bno055/bno055.h | 12 + > 6 files changed, 1388 insertions(+) > create mode 100644 drivers/iio/imu/bno055/Kconfig > create mode 100644 drivers/iio/imu/bno055/Makefile > create mode 100644 drivers/iio/imu/bno055/bno055.c > create mode 100644 drivers/iio/imu/bno055/bno055.h > > diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig > index 001ca2c3ff95..f1d7d4b5e222 100644 > --- a/drivers/iio/imu/Kconfig > +++ b/drivers/iio/imu/Kconfig > @@ -52,6 +52,7 @@ config ADIS16480 > ADIS16485, ADIS16488 inertial sensors. > > source "drivers/iio/imu/bmi160/Kconfig" > +source "drivers/iio/imu/bno055/Kconfig" > > config FXOS8700 > tristate > diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile > index c82748096c77..6eb612034722 100644 > --- a/drivers/iio/imu/Makefile > +++ b/drivers/iio/imu/Makefile > @@ -15,6 +15,7 @@ adis_lib-$(CONFIG_IIO_ADIS_LIB_BUFFER) += adis_buffer.o > obj-$(CONFIG_IIO_ADIS_LIB) += adis_lib.o > > obj-y += bmi160/ > +obj-y += bno055/ > > obj-$(CONFIG_FXOS8700) += fxos8700_core.o > obj-$(CONFIG_FXOS8700_I2C) += fxos8700_i2c.o > diff --git a/drivers/iio/imu/bno055/Kconfig b/drivers/iio/imu/bno055/Kconfig > new file mode 100644 > index 000000000000..2bfed8df4554 > --- /dev/null > +++ b/drivers/iio/imu/bno055/Kconfig > @@ -0,0 +1,7 @@ > +# SPDX-License-Identifier: GPL-2.0-only > +# > +# driver for Bosh bmo055 > +# > + > +config BOSH_BNO055_IIO > + tristate > diff --git a/drivers/iio/imu/bno055/Makefile b/drivers/iio/imu/bno055/Makefile > new file mode 100644 > index 000000000000..15c5ddf8d648 > --- /dev/null > +++ b/drivers/iio/imu/bno055/Makefile > @@ -0,0 +1,6 @@ > +# SPDX-License-Identifier: GPL-2.0 > +# > +# Makefile for bosh bno055 > +# > + > +obj-$(CONFIG_BOSH_BNO055_IIO) += bno055.o > diff --git a/drivers/iio/imu/bno055/bno055.c b/drivers/iio/imu/bno055/bno055.c > new file mode 100644 > index 000000000000..888a88bb13d5 > --- /dev/null > +++ b/drivers/iio/imu/bno055/bno055.c > @@ -0,0 +1,1361 @@ > +// SPDX-License-Identifier: GPL-2.0-or-later > +/* > + * IIO driver for Bosh BNO055 IMU > + * > + * Copyright (C) 2021 Istituto Italiano di Tecnologia > + * Electronic Design Laboratory > + * Written by Andrea Merello > + * > + * Portions of this driver are taken from the BNO055 driver patch > + * from Vlad Dogaru which is Copyright (c) 2016, Intel Corporation. > + * > + * This driver is also based on BMI160 driver, which is: > + * Copyright (c) 2016, Intel Corporation. > + * Copyright (c) 2019, Martin Kelly. > + */ > + > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > +#include > + > +#include "bno055.h" > + > +#define BNO055_FW_NAME "bno055-caldata" > +#define BNO055_FW_EXT ".dat" > + > +/* common registers */ > +#define BNO055_PAGESEL_REG 0x7 > + > +/* page 0 registers */ > +#define BNO055_CHIP_ID_REG 0x0 > +#define BNO055_CHIP_ID_MAGIC 0xA0 > +#define BNO055_SW_REV_LSB_REG 0x4 > +#define BNO055_SW_REV_MSB_REG 0x5 > +#define BNO055_ACC_DATA_X_LSB_REG 0x8 > +#define BNO055_ACC_DATA_Y_LSB_REG 0xA > +#define BNO055_ACC_DATA_Z_LSB_REG 0xC > +#define BNO055_MAG_DATA_X_LSB_REG 0xE > +#define BNO055_MAG_DATA_Y_LSB_REG 0x10 > +#define BNO055_MAG_DATA_Z_LSB_REG 0x12 > +#define BNO055_GYR_DATA_X_LSB_REG 0x14 > +#define BNO055_GYR_DATA_Y_LSB_REG 0x16 > +#define BNO055_GYR_DATA_Z_LSB_REG 0x18 > +#define BNO055_EUL_DATA_X_LSB_REG 0x1A > +#define BNO055_EUL_DATA_Y_LSB_REG 0x1C > +#define BNO055_EUL_DATA_Z_LSB_REG 0x1E > +#define BNO055_QUAT_DATA_W_LSB_REG 0x20 > +#define BNO055_LIA_DATA_X_LSB_REG 0x28 > +#define BNO055_LIA_DATA_Y_LSB_REG 0x2A > +#define BNO055_LIA_DATA_Z_LSB_REG 0x2C > +#define BNO055_GRAVITY_DATA_X_LSB_REG 0x2E > +#define BNO055_GRAVITY_DATA_Y_LSB_REG 0x30 > +#define BNO055_GRAVITY_DATA_Z_LSB_REG 0x32 > +#define BNO055_TEMP_REG 0x34 > +#define BNO055_CALIB_STAT_REG 0x35 > +#define BNO055_CALIB_STAT_MASK 3 > +#define BNO055_CALIB_STAT_MAGN_SHIFT 0 > +#define BNO055_CALIB_STAT_ACCEL_SHIFT 2 > +#define BNO055_CALIB_STAT_GYRO_SHIFT 4 > +#define BNO055_CALIB_STAT_SYS_SHIFT 6 > +#define BNO055_SYS_TRIGGER_REG 0x3F > +#define BNO055_SYS_TRIGGER_RST_INT BIT(6) > +#define BNO055_SYS_TRIGGER_CLK_SEL BIT(7) > +#define BNO055_OPR_MODE_REG 0x3D > +#define BNO055_OPR_MODE_CONFIG 0x0 > +#define BNO055_OPR_MODE_AMG 0x7 > +#define BNO055_OPR_MODE_FUSION_FMC_OFF 0xB > +#define BNO055_OPR_MODE_FUSION 0xC > +#define BNO055_UNIT_SEL_REG 0x3B > +#define BNO055_UNIT_SEL_ANDROID BIT(7) > +#define BNO055_CALDATA_START 0x55 > +#define BNO055_CALDATA_END 0x6A > +#define BNO055_CALDATA_LEN (BNO055_CALDATA_END - BNO055_CALDATA_START + 1) > + > +/* > + * The difference in address between the register that contains the > + * value and the register that contains the offset. This applies for > + * accel, gyro and magn channels. > + */ > +#define BNO055_REG_OFFSET_ADDR 0x4D > + > +/* page 1 registers */ > +#define PG1(x) ((x) | 0x80) > +#define BNO055_ACC_CONFIG_REG PG1(0x8) > +#define BNO055_ACC_CONFIG_LPF_MASK 0x1C > +#define BNO055_ACC_CONFIG_LPF_SHIFT 0x2 > +#define BNO055_ACC_CONFIG_RANGE_MASK 0x3 > +#define BNO055_ACC_CONFIG_RANGE_SHIFT 0x0 > +#define BNO055_MAG_CONFIG_REG PG1(0x9) > +#define BNO055_MAG_CONFIG_HIGHACCURACY 0x18 > +#define BNO055_MAG_CONFIG_ODR_MASK 0x7 > +#define BNO055_MAG_CONFIG_ODR_SHIFT 0 > +#define BNO055_GYR_CONFIG_REG PG1(0xA) > +#define BNO055_GYR_CONFIG_RANGE_MASK 0x7 > +#define BNO055_GYR_CONFIG_RANGE_SHIFT 0 > +#define BNO055_GYR_CONFIG_LPF_MASK 0x38 > +#define BNO055_GYR_CONFIG_LPF_SHIFT 3 > +#define BNO055_INT_MSK PG1(0xF) > +#define BNO055_INT_EN PG1(0x10) > +#define BNO055_INT_ACC_BSX_DRDY BIT(0) > +#define BNO055_INT_MAG_DRDY BIT(1) > +#define BNO055_INT_GYR_DRDY BIT(4) > +#define BNO055_UID_REG PG1(0x50) > +#define BNO055_UID_LEN (0xF) > + > +static const int bno055_mag_odr_vals[] = {2, 6, 8, 10, 15, 20, 25, 30}; > +static const int bno055_acc_lpf_vals[] = {781, 1563, 3125, 6250, > + 12500, 25000, 50000, 100000}; > +static const int bno055_acc_ranges[] = {2, 4, 8, 16}; > +static const int bno055_gyr_lpf_vals[] = {523, 230, 116, 47, 23, 12, 64, 32}; > +static const int bno055_gyr_ranges[] = {2000, 1000, 500, 250, 125}; > + > +struct bno055_priv { > + struct regmap *regmap; > + struct device *dev; > + struct clk *clk; > + int operation_mode; > + int xfer_burst_break_thr; > + struct mutex lock; > + u8 uid[BNO055_UID_LEN]; > +}; > + > +static int find_closest_unsorted(int val, const int arr[], int len) > +{ > + int i; > + int best_idx, best_delta, delta; > + int first = 1; > + > + for (i = 0; i < len; i++) { > + delta = abs(arr[i] - val); > + if (first || delta < best_delta) { > + best_delta = delta; > + best_idx = i; > + } > + first = 0; > + } > + > + return best_idx; > +} > + > +static bool bno055_regmap_volatile(struct device *dev, unsigned int reg) > +{ > + if ((reg >= 0x8 && reg <= 0x3A) || > + /* when in fusion mode, config is updated by chip */ > + reg == BNO055_MAG_CONFIG_REG || > + reg == BNO055_ACC_CONFIG_REG || > + reg == BNO055_GYR_CONFIG_REG || > + (reg >= BNO055_CALDATA_START && reg <= BNO055_CALDATA_END)) > + return true; > + return false; > +} > + > +static bool bno055_regmap_readable(struct device *dev, unsigned int reg) > +{ > + if ((reg <= 0x7F && reg >= 0x6B) || > + reg == 0x3C || > + (reg <= PG1(0x7F) && reg >= PG1(0x60)) || > + (reg <= PG1(0x4F) && reg >= PG1(0x20)) || > + reg == PG1(0xE) || > + (reg <= PG1(0x6) && reg >= PG1(0x0))) > + return false; > + return true; > +} > + > +static bool bno055_regmap_writeable(struct device *dev, unsigned int reg) > +{ > + if ((!bno055_regmap_readable(dev, reg)) || > + (reg <= 0x3A && reg >= 0x8) || > + reg <= 0x6 || > + (reg <= PG1(0x5F) && reg >= PG1(0x50))) > + return false; > + return true; > +} > + > +static const struct regmap_range_cfg bno055_regmap_ranges[] = { > + { > + .range_min = 0, > + .range_max = 0x7f * 2, > + .selector_reg = BNO055_PAGESEL_REG, > + .selector_mask = 0xff, > + .selector_shift = 0, > + .window_start = 0, > + .window_len = 0x80 > + }, > +}; > + > +const struct regmap_config bno055_regmap_config = { > + .name = "bno055", > + .reg_bits = 8, > + .val_bits = 8, > + .ranges = bno055_regmap_ranges, > + .num_ranges = 1, > + .volatile_reg = bno055_regmap_volatile, > + .max_register = 0x80 * 2, > + .writeable_reg = bno055_regmap_writeable, > + .readable_reg = bno055_regmap_readable, > + .cache_type = REGCACHE_RBTREE, > +}; > +EXPORT_SYMBOL_GPL(bno055_regmap_config); > + > +static int bno055_reg_read(struct bno055_priv *priv, > + unsigned int reg, unsigned int *val) > +{ > + int res = regmap_read(priv->regmap, reg, val); > + > + if (res && res != -ERESTARTSYS) { > + dev_err(priv->dev, "Regmap read error. adr: 0x%x, res: %d", > + reg, res); > + } > + > + return res; > +} > + > +static int bno055_reg_write(struct bno055_priv *priv, > + unsigned int reg, unsigned int val) > +{ > + int res = regmap_write(priv->regmap, reg, val); > + > + if (res && res != -ERESTARTSYS) { > + dev_err(priv->dev, "Regmap write error. adr: 0x%x, res: %d", > + reg, res); > + } > + > + return res; > +} > + > +static int bno055_reg_update_bits(struct bno055_priv *priv, unsigned int reg, > + unsigned int mask, unsigned int val) > +{ > + int res = regmap_update_bits(priv->regmap, reg, mask, val); > + > + if (res && res != -ERESTARTSYS) { > + dev_err(priv->dev, "Regmap update_bits error. adr: 0x%x, res: %d", > + reg, res); > + } > + > + return res; > +} > + > +/* must be called in configuration mode */ > +int bno055_calibration_load(struct bno055_priv *priv, const struct firmware *fw) > +{ > + int i; > + unsigned int tmp; > + u8 cal[BNO055_CALDATA_LEN]; > + int read, tot_read = 0; > + int ret = 0; > + char *buf = kmalloc(fw->size + 1, GFP_KERNEL); > + > + if (!buf) > + return -ENOMEM; > + > + memcpy(buf, fw->data, fw->size); > + buf[fw->size] = '\0'; > + for (i = 0; i < BNO055_CALDATA_LEN; i++) { > + ret = sscanf(buf + tot_read, "%x%n", > + &tmp, &read); > + if (ret != 1 || tmp > 0xff) { > + ret = -EINVAL; > + goto exit; > + } > + cal[i] = tmp; > + tot_read += read; > + } > + dev_dbg(priv->dev, "loading cal data: %*ph", BNO055_CALDATA_LEN, cal); > + ret = regmap_bulk_write(priv->regmap, BNO055_CALDATA_START, > + cal, BNO055_CALDATA_LEN); > +exit: > + kfree(buf); > + return ret; > +} > + > +static int bno055_init(struct bno055_priv *priv, const struct firmware *caldata) > +{ > + int res; > + > + res = bno055_reg_write(priv, BNO055_SYS_TRIGGER_REG, > + (priv->clk ? BNO055_SYS_TRIGGER_CLK_SEL : 0) | > + BNO055_SYS_TRIGGER_RST_INT); > + if (res) > + return res; > + > + msleep(100); > + res = bno055_reg_write(priv, BNO055_OPR_MODE_REG, > + BNO055_OPR_MODE_CONFIG); > + if (res) > + return res; > + > + /* use standard SI units */ > + res = bno055_reg_write(priv, BNO055_UNIT_SEL_REG, > + BNO055_UNIT_SEL_ANDROID); > + if (res) > + return res; > + > + if (caldata) { > + res = bno055_calibration_load(priv, caldata); > + if (res) > + dev_warn(priv->dev, "failed to load calibration data with error %d", > + res); > + } > + > + /* > + * Start in fusion mode (all data available), but with magnetometer auto > + * calibration switched off, in order not to overwrite magnetometer > + * calibration data in case one want to keep it untouched. > + */ > + priv->operation_mode = BNO055_OPR_MODE_FUSION_FMC_OFF; > + return bno055_reg_write(priv, BNO055_OPR_MODE_REG, > + priv->operation_mode); > +} > + > +static void bno055_uninit(void *arg) > +{ > + struct bno055_priv *priv = arg; > + > + bno055_reg_write(priv, BNO055_INT_EN, 0); > + > + clk_disable_unprepare(priv->clk); devm_add_action_or_reset() callbacks should be used as one-per-each uninit; it's one of the rules for their usage; it also took me a while to get this; so, you would do: .................. static void bno055_clk_disable(void *clk) { clk_disable_unprepare(clk) } static void bno055_uninit(void *priv) { bno055_reg_write(priv, BNO055_INT_EN, 0); } ......................... ret = clk_prepare_enable(priv->clk); if (ret) return ret; // also make sure to check return code for clk_prepare_enable ret = devm_add_action_or_reset(dev, bno055_clk_disable, priv->clk) if (ret) return ret; ............ res = bno055_init(priv, caldata); if (res) return res; ret = devm_add_action_or_reset(dev, bno055_uninit, priv) if (ret) return ret; Right now, as bno055_uninit() does both, which is not recommended, as devm_ uninit actions should mirror the init actions (but in reverse). > +} > + > +#define BNO055_CHANNEL(_type, _axis, _index, _address, _sep, _sh) { \ > + .address = _address, \ > + .type = _type, \ > + .modified = 1, \ > + .channel2 = IIO_MOD_##_axis, \ > + .info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | (_sep), \ > + .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) | (_sh), \ > + .scan_index = _index, \ > + .scan_type = { \ > + .sign = 's', \ > + .realbits = 16, \ > + .storagebits = 16, \ > + .endianness = IIO_LE, \ > + .repeat = IIO_MOD_##_axis == IIO_MOD_QUATERNION ? 4 : 0 \ > + }, \ > +} > + > +/* scan indexes follow DATA register order */ > +enum bmi160_scan_axis { > + BNO055_SCAN_ACCEL_X, > + BNO055_SCAN_ACCEL_Y, > + BNO055_SCAN_ACCEL_Z, > + BNO055_SCAN_MAGN_X, > + BNO055_SCAN_MAGN_Y, > + BNO055_SCAN_MAGN_Z, > + BNO055_SCAN_GYRO_X, > + BNO055_SCAN_GYRO_Y, > + BNO055_SCAN_GYRO_Z, > + BNO055_SCAN_HEADING, > + BNO055_SCAN_ROLL, > + BNO055_SCAN_PITCH, > + BNO055_SCAN_QUATERNION, > + BNO055_SCAN_LIA_X, > + BNO055_SCAN_LIA_Y, > + BNO055_SCAN_LIA_Z, > + BNO055_SCAN_GRAVITY_X, > + BNO055_SCAN_GRAVITY_Y, > + BNO055_SCAN_GRAVITY_Z, > + BNO055_SCAN_TIMESTAMP, > +}; > + > +static const struct iio_chan_spec bno055_channels[] = { > + /* accelerometer */ > + BNO055_CHANNEL(IIO_ACCEL, X, BNO055_SCAN_ACCEL_X, > + BNO055_ACC_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), > + BNO055_CHANNEL(IIO_ACCEL, Y, BNO055_SCAN_ACCEL_Y, > + BNO055_ACC_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), > + BNO055_CHANNEL(IIO_ACCEL, Z, BNO055_SCAN_ACCEL_Z, > + BNO055_ACC_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), > + /* gyroscope */ > + BNO055_CHANNEL(IIO_ANGL_VEL, X, BNO055_SCAN_GYRO_X, > + BNO055_GYR_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), > + BNO055_CHANNEL(IIO_ANGL_VEL, Y, BNO055_SCAN_GYRO_Y, > + BNO055_GYR_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), > + BNO055_CHANNEL(IIO_ANGL_VEL, Z, BNO055_SCAN_GYRO_Z, > + BNO055_GYR_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY)), > + /* magnetometer */ > + BNO055_CHANNEL(IIO_MAGN, X, BNO055_SCAN_MAGN_X, > + BNO055_MAG_DATA_X_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_SAMP_FREQ)), > + BNO055_CHANNEL(IIO_MAGN, Y, BNO055_SCAN_MAGN_Y, > + BNO055_MAG_DATA_Y_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_SAMP_FREQ)), > + BNO055_CHANNEL(IIO_MAGN, Z, BNO055_SCAN_MAGN_Z, > + BNO055_MAG_DATA_Z_LSB_REG, BIT(IIO_CHAN_INFO_OFFSET), > + BIT(IIO_CHAN_INFO_SAMP_FREQ)), > + /* euler angle */ > + BNO055_CHANNEL(IIO_ROT, X, BNO055_SCAN_HEADING, > + BNO055_EUL_DATA_X_LSB_REG, 0, 0), > + BNO055_CHANNEL(IIO_ROT, Y, BNO055_SCAN_ROLL, > + BNO055_EUL_DATA_Y_LSB_REG, 0, 0), > + BNO055_CHANNEL(IIO_ROT, Z, BNO055_SCAN_PITCH, > + BNO055_EUL_DATA_Z_LSB_REG, 0, 0), > + /* quaternion */ > + BNO055_CHANNEL(IIO_ROT, QUATERNION, BNO055_SCAN_QUATERNION, > + BNO055_QUAT_DATA_W_LSB_REG, 0, 0), > + > + /* linear acceleration */ > + BNO055_CHANNEL(IIO_ACCEL, ACCEL_LINEAR_X, BNO055_SCAN_LIA_X, > + BNO055_LIA_DATA_X_LSB_REG, 0, 0), > + BNO055_CHANNEL(IIO_ACCEL, ACCEL_LINEAR_Y, BNO055_SCAN_LIA_Y, > + BNO055_LIA_DATA_Y_LSB_REG, 0, 0), > + BNO055_CHANNEL(IIO_ACCEL, ACCEL_LINEAR_Z, BNO055_SCAN_LIA_Z, > + BNO055_LIA_DATA_Z_LSB_REG, 0, 0), > + > + /* gravity vector */ > + BNO055_CHANNEL(IIO_GRAVITY, X, BNO055_SCAN_GRAVITY_X, > + BNO055_GRAVITY_DATA_X_LSB_REG, 0, 0), > + BNO055_CHANNEL(IIO_GRAVITY, Y, BNO055_SCAN_GRAVITY_Y, > + BNO055_GRAVITY_DATA_Y_LSB_REG, 0, 0), > + BNO055_CHANNEL(IIO_GRAVITY, Z, BNO055_SCAN_GRAVITY_Z, > + BNO055_GRAVITY_DATA_Z_LSB_REG, 0, 0), > + > + { > + .type = IIO_TEMP, > + .info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED), > + .scan_index = -1 > + }, > + IIO_CHAN_SOFT_TIMESTAMP(BNO055_SCAN_TIMESTAMP), > +}; > + > +static int bno055_get_regmask(struct bno055_priv *priv, int *val, int *val2, > + int reg, int mask, int shift, > + const int tbl[], int k) > +{ > + int hwval, idx; > + int ret = bno055_reg_read(priv, reg, &hwval); > + > + if (ret) > + return ret; > + if (val2) > + *val2 = 0; > + idx = (hwval & mask) >> shift; > + *val = tbl[idx] / k; > + > + if (k == 1) > + return IIO_VAL_INT; > + > + *val2 = (tbl[idx] % k) * 10000; > + return IIO_VAL_INT_PLUS_MICRO; > +} > + > +static int bno055_set_regmask(struct bno055_priv *priv, int val, int val2, > + int reg, int mask, int shift, > + const int table[], int table_len, int k) > + > +{ > + int ret; > + int hwval = find_closest_unsorted(val * k + val2 / 10000, > + table, table_len); > + /* > + * The closest value the HW supports is only one in fusion mode, > + * and it is autoselected, so don't do anything, just return OK, > + * as the closest possible value has been (virtually) selected > + */ > + if (priv->operation_mode != BNO055_OPR_MODE_AMG) > + return 0; > + > + dev_dbg(priv->dev, "WR config - reg, mask, val: 0x%x, 0x%x, 0x%x", > + reg, mask, hwval); > + > + ret = bno055_reg_write(priv, BNO055_OPR_MODE_REG, > + BNO055_OPR_MODE_CONFIG); > + if (ret) > + return ret; > + > + ret = bno055_reg_update_bits(priv, reg, mask, hwval << shift); > + > + if (ret) > + return ret; > + > + return bno055_reg_write(priv, BNO055_OPR_MODE_REG, > + BNO055_OPR_MODE_AMG); > + return 0; this return 0 statement looks unreachable; i wonder if the compiler would have caught this > +} > + > +#define bno055_get_mag_odr(p, v, v2) \ > + bno055_get_regmask(p, v, v2, \ > + BNO055_MAG_CONFIG_REG, BNO055_MAG_CONFIG_ODR_MASK, \ > + BNO055_MAG_CONFIG_ODR_SHIFT, bno055_mag_odr_vals, 1) > + > +#define bno055_set_mag_odr(p, v, v2) \ > + bno055_set_regmask(p, v, v2, \ > + BNO055_MAG_CONFIG_REG, BNO055_MAG_CONFIG_ODR_MASK, \ > + BNO055_MAG_CONFIG_ODR_SHIFT, \ > + bno055_mag_odr_vals, \ > + ARRAY_SIZE(bno055_mag_odr_vals), 1) > + > +#define bno055_get_acc_lpf(p, v, v2) \ > + bno055_get_regmask(p, v, v2, \ > + BNO055_ACC_CONFIG_REG, BNO055_ACC_CONFIG_LPF_MASK, \ > + BNO055_ACC_CONFIG_LPF_SHIFT, \ > + bno055_acc_lpf_vals, 100) > + > +#define bno055_set_acc_lpf(p, v, v2) \ > + bno055_set_regmask(p, v, v2, \ > + BNO055_ACC_CONFIG_REG, BNO055_ACC_CONFIG_LPF_MASK, \ > + BNO055_ACC_CONFIG_LPF_SHIFT, \ > + bno055_acc_lpf_vals, \ > + ARRAY_SIZE(bno055_acc_lpf_vals), 100) > + > +#define bno055_get_acc_range(p, v, v2) \ > + bno055_get_regmask(priv, v, v2, \ > + BNO055_ACC_CONFIG_REG, \ > + BNO055_ACC_CONFIG_RANGE_MASK, \ > + BNO055_ACC_CONFIG_RANGE_SHIFT, bno055_acc_ranges, 1) > + > +#define bno055_set_acc_range(p, v, v2) \ > + bno055_set_regmask(p, v, v2, \ > + BNO055_ACC_CONFIG_REG, \ > + BNO055_ACC_CONFIG_RANGE_MASK, \ > + BNO055_ACC_CONFIG_RANGE_SHIFT, \ > + bno055_acc_ranges, ARRAY_SIZE(bno055_acc_ranges), 1) > + > +#define bno055_get_gyr_lpf(p, v, v2) \ > + bno055_get_regmask(p, v, v2, \ > + BNO055_GYR_CONFIG_REG, BNO055_GYR_CONFIG_LPF_MASK, \ > + BNO055_GYR_CONFIG_LPF_SHIFT, bno055_gyr_lpf_vals, 1) > + > +#define bno055_set_gyr_lpf(p, v, v2) \ > + bno055_set_regmask(p, v, v2, \ > + BNO055_GYR_CONFIG_REG, BNO055_GYR_CONFIG_LPF_MASK, \ > + BNO055_GYR_CONFIG_LPF_SHIFT, \ > + bno055_gyr_lpf_vals, \ > + ARRAY_SIZE(bno055_gyr_lpf_vals), 1) > + > +#define bno055_get_gyr_range(p, v, v2) \ > + bno055_get_regmask(p, v, v2, \ > + BNO055_GYR_CONFIG_REG, \ > + BNO055_GYR_CONFIG_RANGE_MASK, \ > + BNO055_GYR_CONFIG_RANGE_SHIFT, \ > + bno055_gyr_ranges, 1) > + > +#define bno055_set_gyr_range(p, v, v2) \ > + bno055_set_regmask(p, v, v2, \ > + BNO055_GYR_CONFIG_REG, \ > + BNO055_GYR_CONFIG_RANGE_MASK, \ > + BNO055_GYR_CONFIG_RANGE_SHIFT, \ > + bno055_gyr_ranges, ARRAY_SIZE(bno055_gyr_ranges), 1) > + > +static int bno055_read_simple_chan(struct iio_dev *indio_dev, > + struct iio_chan_spec const *chan, > + int *val, int *val2, long mask) > +{ > + struct bno055_priv *priv = iio_priv(indio_dev); > + __le16 raw_val; > + int ret; > + > + switch (mask) { > + case IIO_CHAN_INFO_RAW: > + ret = regmap_bulk_read(priv->regmap, chan->address, > + &raw_val, 2); > + if (ret < 0) > + return ret; > + *val = (s16)le16_to_cpu(raw_val); > + *val2 = 0; > + return IIO_VAL_INT; > + case IIO_CHAN_INFO_OFFSET: > + if (priv->operation_mode != BNO055_OPR_MODE_AMG) { > + *val = 0; > + } else { > + ret = regmap_bulk_read(priv->regmap, > + chan->address + > + BNO055_REG_OFFSET_ADDR, > + &raw_val, 2); > + if (ret < 0) > + return ret; > + *val = -(s16)le16_to_cpu(raw_val); > + } > + *val2 = 0; > + return IIO_VAL_INT; > + case IIO_CHAN_INFO_SCALE: > + *val = 1; > + switch (chan->type) { > + case IIO_GRAVITY: > + /* Table 3-35: 1 m/s^2 = 100 LSB */ > + case IIO_ACCEL: > + /* Table 3-17: 1 m/s^2 = 100 LSB */ > + *val2 = 100; > + break; > + case IIO_MAGN: > + /* > + * Table 3-19: 1 uT = 16 LSB. But we need > + * Gauss: 1G = 0.1 uT. > + */ > + *val2 = 160; > + break; > + case IIO_ANGL_VEL: > + /* Table 3-22: 1 Rps = 900 LSB */ > + *val2 = 900; > + break; > + case IIO_ROT: > + /* Table 3-28: 1 degree = 16 LSB */ > + *val2 = 16; > + break; > + default: > + return -EINVAL; > + } > + return IIO_VAL_FRACTIONAL; > + default: > + return -EINVAL; > + > + case IIO_CHAN_INFO_SAMP_FREQ: > + if (chan->type == IIO_MAGN) > + return bno055_get_mag_odr(priv, val, val2); > + else > + return -EINVAL; > + > + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: > + switch (chan->type) { > + case IIO_ANGL_VEL: > + return bno055_get_gyr_lpf(priv, val, val2); > + case IIO_ACCEL: > + return bno055_get_acc_lpf(priv, val, val2); > + default: > + return -EINVAL; > + } > + } > +} > + > +static int bno055_read_temp_chan(struct iio_dev *indio_dev, int *val) > +{ > + struct bno055_priv *priv = iio_priv(indio_dev); > + unsigned int raw_val; > + int ret; > + > + ret = regmap_read(priv->regmap, BNO055_TEMP_REG, &raw_val); > + if (ret < 0) > + return ret; > + > + /* > + * Tables 3-36 and 3-37: one byte of priv, signed, 1 LSB = 1C. > + * ABI wants milliC. > + */ > + *val = raw_val * 1000; > + > + return IIO_VAL_INT; > +} > + > +static int bno055_read_quaternion(struct iio_dev *indio_dev, > + struct iio_chan_spec const *chan, > + int size, int *vals, int *val_len, > + long mask) > +{ > + struct bno055_priv *priv = iio_priv(indio_dev); > + __le16 raw_vals[4]; > + int i, ret; > + > + switch (mask) { > + case IIO_CHAN_INFO_RAW: > + if (size < 4) > + return -EINVAL; > + ret = regmap_bulk_read(priv->regmap, > + BNO055_QUAT_DATA_W_LSB_REG, > + raw_vals, sizeof(raw_vals)); > + if (ret < 0) > + return ret; > + for (i = 0; i < 4; i++) > + vals[i] = (s16)le16_to_cpu(raw_vals[i]); > + *val_len = 4; > + return IIO_VAL_INT_MULTIPLE; > + case IIO_CHAN_INFO_SCALE: > + /* Table 3-31: 1 quaternion = 2^14 LSB */ > + if (size < 2) > + return -EINVAL; > + vals[0] = 1; > + vals[1] = 1 << 14; > + return IIO_VAL_FRACTIONAL; > + default: > + return -EINVAL; > + } > +} > + > +static int _bno055_read_raw_multi(struct iio_dev *indio_dev, > + struct iio_chan_spec const *chan, > + int size, int *vals, int *val_len, > + long mask) > +{ > + switch (chan->type) { > + case IIO_MAGN: > + case IIO_ACCEL: > + case IIO_ANGL_VEL: > + case IIO_GRAVITY: > + if (size < 2) > + return -EINVAL; > + *val_len = 2; > + return bno055_read_simple_chan(indio_dev, chan, > + &vals[0], &vals[1], > + mask); > + > + case IIO_TEMP: > + *val_len = 1; > + return bno055_read_temp_chan(indio_dev, &vals[0]); > + > + case IIO_ROT: > + /* > + * Rotation is exposed as either a quaternion or three > + * Euler angles. > + */ > + if (chan->channel2 == IIO_MOD_QUATERNION) > + return bno055_read_quaternion(indio_dev, chan, > + size, vals, > + val_len, mask); > + if (size < 2) > + return -EINVAL; > + *val_len = 2; > + return bno055_read_simple_chan(indio_dev, chan, > + &vals[0], &vals[1], > + mask); > + default: > + return -EINVAL; > + } > +} > + > +static int bno055_read_raw_multi(struct iio_dev *indio_dev, > + struct iio_chan_spec const *chan, > + int size, int *vals, int *val_len, > + long mask) > +{ > + int ret; > + struct bno055_priv *priv = iio_priv(indio_dev); > + > + mutex_lock(&priv->lock); > + ret = _bno055_read_raw_multi(indio_dev, chan, size, > + vals, val_len, mask); > + mutex_unlock(&priv->lock); > + return ret; > +} > + > +static int _bno055_write_raw(struct iio_dev *iio_dev, > + struct iio_chan_spec const *chan, > + int val, int val2, long mask) > +{ > + struct bno055_priv *priv = iio_priv(iio_dev); > + > + switch (chan->type) { > + case IIO_MAGN: > + switch (mask) { > + case IIO_CHAN_INFO_SAMP_FREQ: > + return bno055_set_mag_odr(priv, val, val2); > + > + default: > + return -EINVAL; > + } > + break; This break looks unreachable. > + case IIO_ACCEL: > + switch (mask) { > + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: > + return bno055_set_acc_lpf(priv, val, val2); > + > + default: > + return -EINVAL; > + } > + case IIO_ANGL_VEL: > + switch (mask) { > + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: > + return bno055_set_gyr_lpf(priv, val, val2); > + } this looks like an implicit switch-case fall-through; sometimes the compiler complains about these > + default: > + return -EINVAL; > + } > + > + return 0; This return also looks unreachable. > +} > + > +static int bno055_write_raw(struct iio_dev *iio_dev, > + struct iio_chan_spec const *chan, > + int val, int val2, long mask) > +{ > + int ret; > + struct bno055_priv *priv = iio_priv(iio_dev); > + > + mutex_lock(&priv->lock); > + ret = _bno055_write_raw(iio_dev, chan, val, val2, mask); > + mutex_unlock(&priv->lock); > + > + return ret; > +} > + > +static ssize_t in_magn_sampling_frequency_available_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", > + (priv->operation_mode != BNO055_OPR_MODE_AMG) ? "20" : > + "2 6 8 10 15 20 25 30"); > +} > + > +static ssize_t in_accel_range_available_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", > + (priv->operation_mode != BNO055_OPR_MODE_AMG) ? "4" : > + "2 4 8 16"); > +} > + > +static ssize_t > +in_accel_filter_low_pass_3db_frequency_available_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", > + (priv->operation_mode != BNO055_OPR_MODE_AMG) ? "62.5" : > + "7.81 15.63 31.25 62.5 125 250 500 1000"); > +} > + > +static ssize_t in_anglvel_range_available_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", > + (priv->operation_mode != BNO055_OPR_MODE_AMG) ? "2000" : > + "125 250 500 1000 2000"); > +} > + > +static ssize_t > +in_anglvel_filter_low_pass_3db_frequency_available_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", > + (priv->operation_mode != BNO055_OPR_MODE_AMG) ? "32" : > + "12 23 47 32 64 116 230 523"); > +} > + > +static ssize_t bno055_operation_mode_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", > + (priv->operation_mode == BNO055_OPR_MODE_AMG) ? "amg" : > + (priv->operation_mode == BNO055_OPR_MODE_FUSION) ? > + "fusion" : "fusion_fmc_off"); > +} > + > +static ssize_t bno055_operation_mode_store(struct device *dev, > + struct device_attribute *attr, > + const char *buf, size_t len) > +{ > + int res; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + if (sysfs_streq(buf, "amg")) > + priv->operation_mode = BNO055_OPR_MODE_AMG; > + else if (sysfs_streq(buf, "fusion")) > + priv->operation_mode = BNO055_OPR_MODE_FUSION; > + else if (sysfs_streq(buf, "fusion_fmc_off")) > + priv->operation_mode = BNO055_OPR_MODE_FUSION_FMC_OFF; > + else > + return -EINVAL; > + > + mutex_lock(&priv->lock); > + res = bno055_reg_write(priv, BNO055_OPR_MODE_REG, > + BNO055_OPR_MODE_CONFIG); > + if (res) { > + mutex_unlock(&priv->lock); > + return res; > + } > + > + res = bno055_reg_write(priv, BNO055_OPR_MODE_REG, priv->operation_mode); > + mutex_unlock(&priv->lock); > + > + return res ? res : len; > +} > + > +static ssize_t bno055_in_accel_range_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + int val; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + int res = bno055_get_acc_range(priv, &val, NULL); > + > + if (res < 0) > + return res; > + > + return scnprintf(buf, PAGE_SIZE, "%d\n", val); > +} > + > +static ssize_t bno055_in_accel_range_store(struct device *dev, > + struct device_attribute *attr, > + const char *buf, size_t len) > +{ > + int ret; > + unsigned long val; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + ret = kstrtoul(buf, 10, &val); > + if (ret) > + return ret; > + > + mutex_lock(&priv->lock); > + ret = bno055_set_acc_range(priv, val, 0); > + mutex_unlock(&priv->lock); > + > + return ret ? ret : len; > +} > + > +static ssize_t bno055_in_gyr_range_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + int val; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + int res = bno055_get_gyr_range(priv, &val, NULL); > + > + if (res < 0) > + return res; > + > + return scnprintf(buf, PAGE_SIZE, "%d\n", val); > +} > + > +static ssize_t bno055_in_gyr_range_store(struct device *dev, > + struct device_attribute *attr, > + const char *buf, size_t len) > +{ > + int ret; > + unsigned long val; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + ret = kstrtoul(buf, 10, &val); > + if (ret) > + return ret; > + > + mutex_lock(&priv->lock); > + ret = bno055_set_gyr_range(priv, val, 0); > + mutex_unlock(&priv->lock); > + > + return ret ? ret : len; > +} > + > +static ssize_t bno055_get_calib_status(struct device *dev, char *buf, int which) > +{ > + int val; > + int ret; > + const char *calib_str; > + static const char * const calib_status[] = {"bad", "barely enough", > + "fair", "good"}; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + if (priv->operation_mode == BNO055_OPR_MODE_AMG || > + (priv->operation_mode == BNO055_OPR_MODE_FUSION_FMC_OFF && > + which == BNO055_CALIB_STAT_MAGN_SHIFT)) { > + calib_str = "idle"; > + } else { > + mutex_lock(&priv->lock); > + ret = bno055_reg_read(priv, BNO055_CALIB_STAT_REG, &val); > + mutex_unlock(&priv->lock); > + > + if (ret) > + return -EIO; > + > + val = (val >> which) & BNO055_CALIB_STAT_MASK; > + calib_str = calib_status[val]; > + } > + > + return scnprintf(buf, PAGE_SIZE, "%s\n", calib_str); > +} > + > +static ssize_t in_calibration_data_show(struct device *dev, > + struct device_attribute *attr, > + char *buf) > +{ > + int ret; > + int size; > + int i; > + u8 data[BNO055_CALDATA_LEN]; > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > + > + mutex_lock(&priv->lock); > + ret = bno055_reg_write(priv, BNO055_OPR_MODE_REG, > + BNO055_OPR_MODE_CONFIG); > + if (ret) > + goto unlock; > + > + ret = regmap_bulk_read(priv->regmap, BNO055_CALDATA_START, data, > + BNO055_CALDATA_LEN); > + if (ret) > + goto unlock; > + > + ret = bno055_reg_write(priv, BNO055_OPR_MODE_REG, priv->operation_mode); > + mutex_unlock(&priv->lock); > + if (ret) > + return ret; > + > + for (size = 0, i = 0; i < BNO055_CALDATA_LEN; i++) { > + ret = scnprintf(buf + size, > + PAGE_SIZE - size, "%02x%c", data[i], > + (i + 1 < BNO055_CALDATA_LEN) ? ' ' : '\n'); > + if (ret < 0) > + return ret; > + size += ret; > + } > + > + return size; > +unlock: > + mutex_unlock(&priv->lock); > + return ret; > +} > + > +static ssize_t in_autocalibration_status_sys_show(struct device *dev, > + struct device_attribute *a, > + char *buf) > +{ > + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_SYS_SHIFT); > +} > + > +static ssize_t in_autocalibration_status_accel_show(struct device *dev, > + struct device_attribute *a, > + char *buf) > +{ > + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_ACCEL_SHIFT); > +} > + > +static ssize_t in_autocalibration_status_gyro_show(struct device *dev, > + struct device_attribute *a, > + char *buf) > +{ > + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_GYRO_SHIFT); > +} > + > +static ssize_t in_autocalibration_status_magn_show(struct device *dev, > + struct device_attribute *a, > + char *buf) > +{ > + return bno055_get_calib_status(dev, buf, BNO055_CALIB_STAT_MAGN_SHIFT); > +} > + > +static IIO_DEVICE_ATTR_RO(in_magn_sampling_frequency_available, > + 0); > + > +static IIO_DEVICE_ATTR(operation_mode, 0644, > + bno055_operation_mode_show, > + bno055_operation_mode_store, 0); > + > +static IIO_CONST_ATTR(operation_mode_available, > + "amg fusion fusion_fmc_off"); > + > +static IIO_DEVICE_ATTR(in_accel_range, 0644, > + bno055_in_accel_range_show, > + bno055_in_accel_range_store, 0); > + > +static IIO_DEVICE_ATTR_RO(in_accel_range_available, 0); > +static IIO_DEVICE_ATTR_RO(in_accel_filter_low_pass_3db_frequency_available, 0); > + > +static IIO_DEVICE_ATTR(in_anglvel_range, 0644, > + bno055_in_gyr_range_show, > + bno055_in_gyr_range_store, 0); > + > +static IIO_DEVICE_ATTR_RO(in_anglvel_range_available, 0); > +static IIO_DEVICE_ATTR_RO(in_anglvel_filter_low_pass_3db_frequency_available, 0); > + > +static IIO_DEVICE_ATTR_RO(in_autocalibration_status_sys, 0); > +static IIO_DEVICE_ATTR_RO(in_autocalibration_status_accel, 0); > +static IIO_DEVICE_ATTR_RO(in_autocalibration_status_gyro, 0); > +static IIO_DEVICE_ATTR_RO(in_autocalibration_status_magn, 0); > +static IIO_DEVICE_ATTR_RO(in_calibration_data, 0); > + > +static struct attribute *bno055_attrs[] = { > + &iio_dev_attr_in_magn_sampling_frequency_available.dev_attr.attr, > + &iio_dev_attr_in_accel_range_available.dev_attr.attr, > + &iio_dev_attr_in_accel_range.dev_attr.attr, > + &iio_dev_attr_in_accel_filter_low_pass_3db_frequency_available.dev_attr.attr, > + &iio_dev_attr_in_anglvel_range_available.dev_attr.attr, > + &iio_dev_attr_in_anglvel_range.dev_attr.attr, > + &iio_dev_attr_in_anglvel_filter_low_pass_3db_frequency_available.dev_attr.attr, > + &iio_const_attr_operation_mode_available.dev_attr.attr, > + &iio_dev_attr_operation_mode.dev_attr.attr, > + &iio_dev_attr_in_autocalibration_status_sys.dev_attr.attr, > + &iio_dev_attr_in_autocalibration_status_accel.dev_attr.attr, > + &iio_dev_attr_in_autocalibration_status_gyro.dev_attr.attr, > + &iio_dev_attr_in_autocalibration_status_magn.dev_attr.attr, > + &iio_dev_attr_in_calibration_data.dev_attr.attr, > + NULL, > +}; > + > +static const struct attribute_group bno055_attrs_group = { > + .attrs = bno055_attrs, > +}; > + > +static const struct iio_info bno055_info = { > + .read_raw_multi = bno055_read_raw_multi, > + .write_raw = bno055_write_raw, > + .attrs = &bno055_attrs_group, > +}; > + > +/* > + * Reads len samples from the HW, stores them in buf starting from buf_idx, > + * and applies mask to cull (skip) unneeded samples. > + * Updates buf_idx incrementing with the number of stored samples. > + * Samples from HW are xferred into buf, then in-place copy on buf is > + * performed in order to cull samples that need to be skipped. > + * This avoids copies of the first samples until we hit the 1st sample to skip, > + * and also avoids having an extra bounce buffer. > + * buf must be able to contain len elements inspite of how many samples we are > + * going to cull. > + */ > +static int bno055_scan_xfer(struct bno055_priv *priv, > + int start_ch, int len, unsigned long mask, > + __le16 *buf, int *buf_idx) > +{ > + int buf_base = *buf_idx; > + const int base = BNO055_ACC_DATA_X_LSB_REG; > + int ret; > + int i, j, n; > + __le16 *dst, *src; > + bool quat_in_read = false; > + int offs_fixup = 0; > + int xfer_len = len; > + > + /* All chans are made up 1 16bit sample, except for quaternion > + * that is made up 4 16-bit values. > + * For us the quaternion CH is just like 4 regular CHs. > + * If out read starts past the quaternion make sure to adjust the > + * starting offset; if the quaternion is contained in our scan then > + * make sure to adjust the read len. > + */ > + if (start_ch > BNO055_SCAN_QUATERNION) { > + start_ch += 3; > + } else if ((start_ch <= BNO055_SCAN_QUATERNION) && > + ((start_ch + len) > BNO055_SCAN_QUATERNION)) { > + quat_in_read = true; > + xfer_len += 3; > + } > + > + ret = regmap_bulk_read(priv->regmap, > + base + start_ch * sizeof(__le16), > + buf + buf_base, > + xfer_len * sizeof(__le16)); > + if (ret) > + return ret; > + > + for_each_set_bit(i, &mask, len) { > + if (quat_in_read && ((start_ch + i) > BNO055_SCAN_QUATERNION)) > + offs_fixup = 3; > + > + dst = buf + *buf_idx; > + src = buf + buf_base + offs_fixup + i; > + > + n = ((start_ch + i) == BNO055_SCAN_QUATERNION) ? 4 : 1; > + > + if (dst != src) { > + for (j = 0; j < n; j++) > + dst[j] = src[j]; > + } > + > + *buf_idx += n; > + } > + return 0; > +} > + > +static irqreturn_t bno055_trigger_handler(int irq, void *p) > +{ > + struct iio_poll_func *pf = p; > + struct iio_dev *iio_dev = pf->indio_dev; > + struct bno055_priv *priv = iio_priv(iio_dev); > + struct { > + __le16 chans[(BNO055_GRAVITY_DATA_Z_LSB_REG - > + BNO055_ACC_DATA_X_LSB_REG) / 2]; > + s64 timestamp __aligned(8); > + } buf; > + bool thr_hit; > + int quat; > + int ret; > + int start, end, xfer_start, next = 0; > + int buf_idx = 0; > + bool finish = false; > + unsigned long mask; > + > + /* we have less than 32 chs, all masks fit in an ulong */ > + start = find_first_bit(iio_dev->active_scan_mask, iio_dev->masklength); > + xfer_start = start; > + if (start == iio_dev->masklength) > + goto done; > + > + mutex_lock(&priv->lock); > + while (!finish) { > + end = find_next_zero_bit(iio_dev->active_scan_mask, > + iio_dev->masklength, start); > + if (end == iio_dev->masklength) { > + finish = true; > + } else { > + next = find_next_bit(iio_dev->active_scan_mask, > + iio_dev->masklength, end); > + if (next == iio_dev->masklength) { > + finish = true; > + } else { > + quat = ((next > BNO055_SCAN_QUATERNION) && > + (end <= BNO055_SCAN_QUATERNION)) ? 3 : 0; > + thr_hit = (next - end + quat) > > + priv->xfer_burst_break_thr; > + } > + } > + > + if (thr_hit || finish) { > + mask = *iio_dev->active_scan_mask >> xfer_start; > + ret = bno055_scan_xfer(priv, xfer_start, > + end - xfer_start, > + mask, buf.chans, &buf_idx); > + if (ret) > + goto done; > + xfer_start = next; > + } > + start = next; > + } > + iio_push_to_buffers_with_timestamp(iio_dev, &buf, pf->timestamp); > +done: > + mutex_unlock(&priv->lock); > + iio_trigger_notify_done(iio_dev->trig); > + return IRQ_HANDLED; > +} > + > +int bno055_probe(struct device *dev, struct regmap *regmap, int irq, > + int xfer_burst_break_thr) > +{ > + int ver, rev; > + int res; > + unsigned int val; > + struct gpio_desc *rst; > + struct iio_dev *iio_dev; > + struct bno055_priv *priv; > + /* base name + separator + UID + ext + zero */ > + char fw_name_buf[sizeof(BNO055_FW_NAME BNO055_FW_EXT) + > + BNO055_UID_LEN * 2 + 1 + 1]; > + const struct firmware *caldata; > + > + iio_dev = devm_iio_device_alloc(dev, sizeof(*priv)); > + if (!iio_dev) > + return -ENOMEM; > + > + iio_dev->name = "bno055"; > + priv = iio_priv(iio_dev); > + memset(priv, 0, sizeof(*priv)); > + mutex_init(&priv->lock); > + priv->regmap = regmap; > + priv->dev = dev; > + priv->xfer_burst_break_thr = xfer_burst_break_thr; > + rst = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH); > + if (IS_ERR(rst) && (PTR_ERR(rst) != -EPROBE_DEFER)) { > + dev_err(dev, "Failed to get reset GPIO"); > + return PTR_ERR(rst); > + } > + > + priv->clk = devm_clk_get_optional(dev, "clk"); > + if (IS_ERR(priv->clk) && (PTR_ERR(priv->clk) != -EPROBE_DEFER)) { > + dev_err(dev, "Failed to get CLK"); > + return PTR_ERR(priv->clk); > + } > + > + clk_prepare_enable(priv->clk); > + > + if (rst) { > + usleep_range(5000, 10000); > + gpiod_set_value_cansleep(rst, 0); > + usleep_range(650000, 750000); > + } > + > + res = devm_add_action_or_reset(dev, bno055_uninit, priv); > + if (res) > + return res; > + > + res = bno055_reg_read(priv, BNO055_CHIP_ID_REG, &val); > + if (res) > + return res; > + > + if (val != BNO055_CHIP_ID_MAGIC) { > + dev_err(dev, "Unrecognized chip ID 0x%x", val); > + return -ENODEV; > + } > + dev_dbg(dev, "Found BMO055 chip"); > + > + res = regmap_bulk_read(priv->regmap, BNO055_UID_REG, > + priv->uid, BNO055_UID_LEN); > + if (res) > + return res; > + > + dev_info(dev, "unique ID: %*ph", BNO055_UID_LEN, priv->uid); > + > + /* > + * This has nothing to do with the IMU firmware, this is for sensor > + * calibration data. > + */ > + sprintf(fw_name_buf, BNO055_FW_NAME "-%*phN" BNO055_FW_EXT, > + BNO055_UID_LEN, priv->uid); > + res = request_firmware(&caldata, fw_name_buf, dev); > + if (res) > + res = request_firmware(&caldata, > + BNO055_FW_NAME BNO055_FW_EXT, dev); > + > + if (res) { > + dev_notice(dev, "Failed to load calibration data firmware file; this has nothing to do with IMU main firmware."); > + dev_notice(dev, "You can calibrate your IMU (look for 'in_autocalibration_status*' files in sysfs) and then copy 'in_calibration_data' to your firmware file"); > + caldata = NULL; > + } > + > + res = bno055_init(priv, caldata); > + if (res) > + return res; > + > + if (caldata) > + release_firmware(caldata); > + > + res = regmap_read(priv->regmap, > + BNO055_SW_REV_LSB_REG, &rev); > + if (res) > + return res; > + > + res = regmap_read(priv->regmap, > + BNO055_SW_REV_MSB_REG, &ver); > + if (res) > + return res; > + > + dev_info(dev, "Firmware version %x.%x", ver, rev); > + > + iio_dev->channels = bno055_channels; > + iio_dev->num_channels = ARRAY_SIZE(bno055_channels); > + iio_dev->info = &bno055_info; > + iio_dev->modes = INDIO_DIRECT_MODE; > + > + res = devm_iio_triggered_buffer_setup(dev, iio_dev, > + iio_pollfunc_store_time, > + bno055_trigger_handler, NULL); > + if (res) > + return res; > + > + return devm_iio_device_register(dev, iio_dev); > +} > +EXPORT_SYMBOL_GPL(bno055_probe); > + > +MODULE_AUTHOR("Andrea Merello "); > +MODULE_DESCRIPTION("Bosch BNO055 driver"); > +MODULE_LICENSE("GPL v2"); > diff --git a/drivers/iio/imu/bno055/bno055.h b/drivers/iio/imu/bno055/bno055.h > new file mode 100644 > index 000000000000..163ab8068e7c > --- /dev/null > +++ b/drivers/iio/imu/bno055/bno055.h > @@ -0,0 +1,12 @@ > +/* SPDX-License-Identifier: GPL-2.0-or-later */ > +#ifndef __BNO055_H__ > +#define __BNO055_H__ > + > +#include > +#include > + > +int bno055_probe(struct device *dev, struct regmap *regmap, int irq, > + int xfer_burst_break_thr); > +extern const struct regmap_config bno055_regmap_config; > + > +#endif > -- > 2.17.1 >