Some inline notes. OK for all the rest. Il giorno gio 28 ott 2021 alle ore 15:27 Jonathan Cameron <jic23@xxxxxxxxxx> ha scritto: > > On Thu, 28 Oct 2021 12:18:37 +0200 > Andrea Merello <andrea.merello@xxxxxxxxx> 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. > > > > 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 <andrea.merello@xxxxxx> > > --- > > drivers/iio/imu/Kconfig | 1 + > > drivers/iio/imu/Makefile | 1 + > > drivers/iio/imu/bno055/Kconfig | 4 + > > drivers/iio/imu/bno055/Makefile | 3 + > > drivers/iio/imu/bno055/bno055.c | 1480 +++++++++++++++++++++++++++++++ > > drivers/iio/imu/bno055/bno055.h | 12 + > > 6 files changed, 1501 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/bno055/bno055.c b/drivers/iio/imu/bno055/bno055.c > > new file mode 100644 > > index 000000000000..c85cb985f0f1 > > --- /dev/null > > +++ b/drivers/iio/imu/bno055/bno055.c > > @@ -0,0 +1,1480 @@ > > ... > > > + > > +static int bno055_reg_update_bits(struct bno055_priv *priv, unsigned int reg, > > + unsigned int mask, unsigned int val) > > +{ > > + int ret; > > + > > + ret = regmap_update_bits(priv->regmap, reg, mask, val); > > + if (ret && ret != -ERESTARTSYS) { > > + dev_err(priv->dev, "Regmap update_bits error. adr: 0x%x, ret: %d", > > + reg, ret); > > This feels like a wrapper that made sense when developing but probably doesn't > want to still be here now things are 'working'. > > > + } > > + > > + return ret; > > +} > > + > > ... > > > + > > +static void bno055_clk_disable(void *arg) > > Easy to make arg == priv->clk and turn this into a one line function. > I'd expect these cleanup functions to be just above where probe() is defined rather > than all the way up here. > > > +{ > > + struct bno055_priv *priv = arg; > > + > > + clk_disable_unprepare(priv->clk); > > +} > > + > > ... > > > + > > +static int bno055_get_acc_lpf(struct bno055_priv *priv, int *val, int *val2) > > +{ > > + const int shift = __ffs(BNO055_ACC_CONFIG_LPF_MASK); > > + int hwval, idx; > > + int ret; > > + > > + ret = regmap_read(priv->regmap, BNO055_ACC_CONFIG_REG, &hwval); > > + if (ret) > > + return ret; > > + > > + idx = (hwval & BNO055_ACC_CONFIG_LPF_MASK) >> shift; > > Use FIELD_GET() and FIELD_PREP where possible rather than reinventing them. > > > + *val = bno055_acc_lpf_vals[idx * 2]; > > + *val2 = bno055_acc_lpf_vals[idx * 2 + 1]; > > + > > + return IIO_VAL_INT_PLUS_MICRO; > > +} > > + > > +static int bno055_set_acc_lpf(struct bno055_priv *priv, int val, int val2) > > +{ > > + const int shift = __ffs(BNO055_ACC_CONFIG_LPF_MASK); > > + int req_val = val * 1000 + val2 / 1000; > > + bool first = true; > > + int best_delta; > > + int best_idx; > > + int tbl_val; > > + int delta; > > + int ret; > > + int i; > > + > > + for (i = 0; i < ARRAY_SIZE(bno055_acc_lpf_vals) / 2; i++) { > > + tbl_val = bno055_acc_lpf_vals[i * 2] * 1000 + > > + bno055_acc_lpf_vals[i * 2 + 1] / 1000; > > + delta = abs(tbl_val - req_val); > > + if (first || delta < best_delta) { > > + best_delta = delta; > > + best_idx = i; > > + first = false; > > + } > > + } > > + > > + /* > > + * 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; > > Can we do this before the big loop above? > > > > + > > + ret = regmap_write(priv->regmap, BNO055_OPR_MODE_REG, > > + BNO055_OPR_MODE_CONFIG); > > + if (ret) > > + return ret; > > + > > + ret = bno055_reg_update_bits(priv, BNO055_ACC_CONFIG_REG, > > + BNO055_ACC_CONFIG_LPF_MASK, > > + best_idx << shift); > > + > > + if (ret) > > + return ret; > > + > > + return regmap_write(priv->regmap, BNO055_OPR_MODE_REG, > > + BNO055_OPR_MODE_AMG); > > +} > > + > > ... > > > + > > +#define bno055_get_mag_odr(p, v) \ > > + bno055_get_regmask(p, v, \ > > + BNO055_MAG_CONFIG_REG, BNO055_MAG_CONFIG_ODR_MASK, \ > > + bno055_mag_odr_vals) > > I'm not really convinced this is a worthwhile abstraction as these are typically > only used once. > > > + > ... > > > +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, sizeof(raw_val)); > > + if (ret < 0) > > + return ret; > > + *val = (s16)le16_to_cpu(raw_val); > > + 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, sizeof(raw_val)); > > + if (ret < 0) > > + return ret; > > + /* > > + * IMU reports sensor offests; IIO wants correction > > + * offset, thus we need the 'minus' here. > > + */ > > + *val = -(s16)le16_to_cpu(raw_val); > > + } > > + 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; > > default in the middle is a bit unusual. move it to the end. > > > + > > + case IIO_CHAN_INFO_SAMP_FREQ: > > + if (chan->type != IIO_MAGN) > > + return -EINVAL; > > + else > > + return bno055_get_mag_odr(priv, val); > > + > > + case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY: > > + switch (chan->type) { > > + case IIO_ANGL_VEL: > > + return bno055_get_gyr_lpf(priv, val); > > + case IIO_ACCEL: > > + return bno055_get_acc_lpf(priv, val, val2); > > + default: > > + return -EINVAL; > > + } > > + } > > +} > > + > > > > + > > +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; > > IIO_VAL_FRACTIONAL_LOG2? > > > + return IIO_VAL_FRACTIONAL; > > + default: > > + return -EINVAL; > > + } > > +} > > + > > ... > > > + > > +static ssize_t bno055_fusion_enable_store(struct device *dev, > > + struct device_attribute *attr, > > + const char *buf, size_t len) > > +{ > > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > > + int ret = 0; > > + > > + if (sysfs_streq(buf, "0")) { > > + ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_AMG); > > + } else { > > + /* > > + * Coming from AMG means the FMC was off, just switch to fusion > > + * but don't change anything that doesn't belong to us (i.e let. > > + * FMC stay off. > > + * Coming from any other fusion mode means we don't need to do > > + * anything. > > + */ > > + if (priv->operation_mode == BNO055_OPR_MODE_AMG) > > + ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION_FMC_OFF); > > + } > > + > > + return len ?: len; > > return ret?: len; might make sense, though my inclination would be to use an explicit > if (ret) at the various possible error locations. > > > +} > > ... > > > +static ssize_t bno055_fmc_enable_store(struct device *dev, > > + struct device_attribute *attr, > > + const char *buf, size_t len) > > +{ > > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > > + int ret = 0; > > + > > + if (sysfs_streq(buf, "0")) { > > + if (priv->operation_mode == BNO055_OPR_MODE_FUSION) > > + ret = bno055_operation_mode_set(priv, BNO055_OPR_MODE_FUSION_FMC_OFF); > > + } else { > > + if (priv->operation_mode == BNO055_OPR_MODE_AMG) > > + return -EINVAL; > > + } > > + > > + return len ?: ret; > > Don't think that will return ret which is what we want if it's set. > > > +} > > + > > ... > > > +static ssize_t in_calibration_data_show(struct device *dev, > > + struct device_attribute *attr, > > + char *buf) > > +{ > > + struct bno055_priv *priv = iio_priv(dev_to_iio_dev(dev)); > > + u8 data[BNO055_CALDATA_LEN]; > > + int ret; > > + > > + mutex_lock(&priv->lock); > > + ret = regmap_write(priv->regmap, 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 = regmap_write(priv->regmap, BNO055_OPR_MODE_REG, priv->operation_mode); > > + mutex_unlock(&priv->lock); > > + if (ret) > > + return ret; > > This is a case where I'd move the mutex_unlock after the check so that we have > a nice shared error path via the unlock lable. > > > + > > + memcpy(buf, data, BNO055_CALDATA_LEN); > > + > > + return BNO055_CALDATA_LEN; > > +unlock: > > + mutex_unlock(&priv->lock); > > + return ret; > > +} > > + > ... > > > +static ssize_t bno055_show_fw_version(struct file *file, char __user *userbuf, > > + size_t count, loff_t *ppos) > > +{ > > + struct bno055_priv *priv = file->private_data; > > + int rev, ver; > > + char *buf; > > + int ret; > > + > > + ret = regmap_read(priv->regmap, BNO055_SW_REV_LSB_REG, &rev); > > + if (ret) > > + return ret; > > + > > + ret = regmap_read(priv->regmap, BNO055_SW_REV_MSB_REG, &ver); > > + if (ret) > > + return ret; > > + > > + buf = devm_kasprintf(priv->dev, GFP_KERNEL, "ver: 0x%x, rev: 0x%x\n", > > + ver, rev); > > + if (!buf) > > + return -ENOMEM; > > + > > + ret = simple_read_from_buffer(userbuf, count, ppos, buf, strlen(buf)); > > + devm_kfree(priv->dev, buf); > > Why use devm managed allocations if you are just going to free it immediately? > > > + > > + return ret; > > +} > > + > > ... > > > +/* > > + * 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 transferred 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 in spite of how many samples we are > > + * going to cull. > > This is rather complex - I take we can't just fall back to letting the IIO core > demux do all the hard work for us? Hum. I'm not sure.. I admit that I'm not familiar with the demux thing, but as far as I can see it needs to be initialized once with a list containing all allowed scan masks; IIO core will pick one of them and eventually cull extra samples it contains. Is this right? I would say we may precalculate this list at probe time (depending on the burst break threshold) and populate it with all the possible scan masks in which there are no gaps < than the bust break threshold. But this could be a quite high number of combinations.. This way the IIO layer will only request xfers in which gaps are always > than burst break threshold, which the driver in turn will always split in several xfers. Does this make sense to you? > > + */ > > +static int bno055_scan_xfer(struct bno055_priv *priv, > > + int start_ch, int len, unsigned long mask, > > + __le16 *buf, int *buf_idx) > > +{ > > + const int base = BNO055_ACC_DATA_X_LSB_REG; > > + bool quat_in_read = false; > > + int buf_base = *buf_idx; > > + __le16 *dst, *src; > > + int offs_fixup = 0; > > + int xfer_len = len; > > + int ret; > > + int i, n; > > + > > + /* > > + * All chans are made up 1 16-bit sample, except for quaternion that is > > + * made up 4 16-bit values. > > + * For us the quaternion CH is just like 4 regular CHs. > > + * If our 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) > > + memcpy(dst, src, n * sizeof(__le16)); > > + > > + *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); > > + int xfer_start, start, end, prev_end; > > + bool xfer_pending = false; > > + bool first = true; > > + unsigned long mask; > > + int buf_idx = 0; > > + bool thr_hit; > > + int quat; > > + int ret; > > + > > + mutex_lock(&priv->lock); > > + for_each_set_bitrange(start, end, iio_dev->active_scan_mask, > > + iio_dev->masklength) { > > I'm not seeing this function in mainline... I guess this series has a dependency > I missed? I've been pointed to Yuri Norov bitmap series (I mentioned this in the cover letter). Assuming it is close to be merged, I've updated my drv for its API changes, but if you prefer I can revert to the current mainline API. It's a trivial change. > > + if (!xfer_pending) > > + xfer_start = start; > > + xfer_pending = true; > > + > > + if (!first) { > > first == true and we never get in here to set it to false. That's awful. Possibly I've broken this while making changes for V2, and my test program didn't catch it. Maybe it just impacts performances, which, now I realize, I probably didn't rechek :( > Perhaps we would benefit from a state machine diagram for this function? > In general this function is complex enough to need documentation of what > each major part is doing. > > > + quat = ((start > BNO055_SCAN_QUATERNION) && > > + (prev_end <= BNO055_SCAN_QUATERNION)) ? 3 : 0; > > Having quat == 3 for a variable named quat doesn't seem intuitive. > > > + thr_hit = (start - prev_end + quat) > > > + priv->xfer_burst_break_thr; > > + > > + if (thr_hit) { > > + mask = *iio_dev->active_scan_mask >> xfer_start; > > + ret = bno055_scan_xfer(priv, xfer_start, > > + prev_end - xfer_start + 1, > > + mask, priv->buf.chans, &buf_idx); > > + if (ret) > > + goto done; > > + xfer_pending = false; > > + } > > + first = false; > > + } > > + prev_end = end; > > + } > > + > > + if (xfer_pending) { > > + mask = *iio_dev->active_scan_mask >> xfer_start; > > + ret = bno055_scan_xfer(priv, xfer_start, > > + end - xfer_start + 1, > > + mask, priv->buf.chans, &buf_idx); > > + } > > + > > + iio_push_to_buffers_with_timestamp(iio_dev, &priv->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 xfer_burst_break_thr) > > +{ > > + const struct firmware *caldata; > > + struct bno055_priv *priv; > > + struct iio_dev *iio_dev; > > + struct gpio_desc *rst; > > + char *fw_name_buf; > > + unsigned int val; > > + int ret; > > + > > + iio_dev = devm_iio_device_alloc(dev, sizeof(*priv)); > > + if (!iio_dev) > > + return -ENOMEM; > > + > > + iio_dev->name = "bno055"; > > + priv = iio_priv(iio_dev); > > + mutex_init(&priv->lock); > > + priv->regmap = regmap; > > + priv->dev = dev; > > + priv->xfer_burst_break_thr = xfer_burst_break_thr; > > blank line here would hep readability a little I think. > > > + rst = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH); > > + if (IS_ERR(rst)) > > + return dev_err_probe(dev, PTR_ERR(rst), "Failed to get reset GPIO"); > > + > > + priv->clk = devm_clk_get_optional(dev, "clk"); > > + if (IS_ERR(priv->clk)) > > + return dev_err_probe(dev, PTR_ERR(priv->clk), "Failed to get CLK"); > > + > > + ret = clk_prepare_enable(priv->clk); > > + if (ret) > > + return ret; > > + > > + ret = devm_add_action_or_reset(dev, bno055_clk_disable, priv); > > As mentioned above, pass priv->clk into this as we don't need to see anything > else in the callback. > > > + if (ret) > > + return ret; > > + > > + if (rst) { > > + usleep_range(5000, 10000); > > + gpiod_set_value_cansleep(rst, 0); > > + usleep_range(650000, 750000); > > + } > > + > > + ret = regmap_read(priv->regmap, BNO055_CHIP_ID_REG, &val); > > + if (ret) > > + return ret; > > + > > + if (val != BNO055_CHIP_ID_MAGIC) { > > + dev_err(dev, "Unrecognized chip ID 0x%x", val); > > + return -ENODEV; > > + } > > + > > + ret = regmap_bulk_read(priv->regmap, BNO055_UID_LOWER_REG, > > + priv->uid, BNO055_UID_LEN); > > + if (ret) > > + return ret; > > + > > + /* > > + * This has nothing to do with the IMU firmware, this is for sensor > > + * calibration data. > > + */ > > + fw_name_buf = devm_kasprintf(dev, GFP_KERNEL, > > + BNO055_FW_UID_NAME, > > + BNO055_UID_LEN, priv->uid); > > + if (!fw_name_buf) > > + return -ENOMEM; > > + > > + ret = request_firmware(&caldata, fw_name_buf, dev); > > + devm_kfree(dev, fw_name_buf); > > + if (ret) > > + ret = request_firmware(&caldata, BNO055_FW_GENERIC_NAME, dev); > > + > > + if (ret) { > > + dev_notice(dev, "Failed to load calibration data firmware file; this has nothing to do with IMU main firmware.\nYou can calibrate your IMU (look for 'in_autocalibration_status*' files in sysfs) and then copy 'in_calibration_data' to your firmware file"); > > As the notice has multiple lines, you can break at the \n points without any loss of greppability. > It would be good to make this shorter though if we can - I wouldn't way what it isn't for example. > > Calibration file load failed. > Follow instructions in Documentation/ * > > + write some docs on the calibration procedure. I don't think it is a > good plan to give a guide in a kernel log. > > > + caldata = NULL; > > I'd hope that is already the case and it definitely looks like it is from a quick look > at request_firmware(). I'd consider request_firmware buggy if it did anything else > as that would imply it had side effects that weren't cleaned up on error. > > > + } > > + > > + ret = bno055_init(priv, caldata); > > + if (caldata) > > + release_firmware(caldata); > > + if (ret) > > + return ret; > > + > > + ret = devm_add_action_or_reset(dev, bno055_uninit, priv); > > + if (ret) > > + return ret; > > + > > + iio_dev->channels = bno055_channels; > > + iio_dev->num_channels = ARRAY_SIZE(bno055_channels); > > + iio_dev->info = &bno055_info; > > + iio_dev->modes = INDIO_DIRECT_MODE; > > + > > + ret = devm_iio_triggered_buffer_setup(dev, iio_dev, > > + iio_pollfunc_store_time, > > + bno055_trigger_handler, NULL); > > + if (ret) > > + return ret; > > + > > + ret = devm_iio_device_register(dev, iio_dev); > > + if (ret) > > + return ret; > > + > > + bno055_debugfs_init(iio_dev); > > + > > + return 0; > > +} > > +EXPORT_SYMBOL_GPL(bno055_probe); > > + > ... > > Thanks, > > Jonathan