From 108e4d4de2b58011eafd14581b6ea7469f1fc467 Mon Sep 17 00:00:00 2001 From: Stephen Boyd Date: Thu, 24 Mar 2022 15:29:28 -0700 Subject: iio:proximity:sx9324: Fix hardware gain read/write There are four possible gain values according to 'sx9324_gain_vals[]': 1, 2, 4, and 8 The values are off by one when writing and reading the register. The bits should be set according to this equation: ilog2() + 1 so that a gain of 8 is 0x4 in the register field and a gain of 4 is 0x3 in the register field, etc. Note that a gain of 0 is reserved per the datasheet. The default gain (SX9324_REG_PROX_CTRL0_GAIN_1) is also wrong. It should be 0x1 << 3, i.e. 0x8, not 0x80 which is setting the reserved bit 7. Fix this all up to properly handle the hardware gain and return errors for invalid settings. Fixes: 4c18a890dff8 ("iio:proximity:sx9324: Add SX9324 support") Signed-off-by: Stephen Boyd Reviewed-by: Gwendal Grignou Link: https://lore.kernel.org/r/20220324222928.874522-1-swboyd@chromium.org Signed-off-by: Jonathan Cameron --- drivers/iio/proximity/sx9324.c | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/drivers/iio/proximity/sx9324.c b/drivers/iio/proximity/sx9324.c index 0d9bbbb50cb4..6e90917e3e36 100644 --- a/drivers/iio/proximity/sx9324.c +++ b/drivers/iio/proximity/sx9324.c @@ -76,7 +76,10 @@ #define SX9324_REG_PROX_CTRL0 0x30 #define SX9324_REG_PROX_CTRL0_GAIN_MASK GENMASK(5, 3) -#define SX9324_REG_PROX_CTRL0_GAIN_1 0x80 +#define SX9324_REG_PROX_CTRL0_GAIN_SHIFT 3 +#define SX9324_REG_PROX_CTRL0_GAIN_RSVD 0x0 +#define SX9324_REG_PROX_CTRL0_GAIN_1 0x1 +#define SX9324_REG_PROX_CTRL0_GAIN_8 0x4 #define SX9324_REG_PROX_CTRL0_RAWFILT_MASK GENMASK(2, 0) #define SX9324_REG_PROX_CTRL0_RAWFILT_1P50 0x01 #define SX9324_REG_PROX_CTRL1 0x31 @@ -379,7 +382,14 @@ static int sx9324_read_gain(struct sx_common_data *data, if (ret) return ret; - *val = 1 << FIELD_GET(SX9324_REG_PROX_CTRL0_GAIN_MASK, regval); + regval = FIELD_GET(SX9324_REG_PROX_CTRL0_GAIN_MASK, regval); + if (regval) + regval--; + else if (regval == SX9324_REG_PROX_CTRL0_GAIN_RSVD || + regval > SX9324_REG_PROX_CTRL0_GAIN_8) + return -EINVAL; + + *val = 1 << regval; return IIO_VAL_INT; } @@ -725,8 +735,12 @@ static int sx9324_write_gain(struct sx_common_data *data, unsigned int gain, reg; int ret; - gain = ilog2(val); reg = SX9324_REG_PROX_CTRL0 + chan->channel / 2; + + gain = ilog2(val) + 1; + if (val <= 0 || gain > SX9324_REG_PROX_CTRL0_GAIN_8) + return -EINVAL; + gain = FIELD_PREP(SX9324_REG_PROX_CTRL0_GAIN_MASK, gain); mutex_lock(&data->mutex); @@ -784,9 +798,11 @@ static const struct sx_common_reg_default sx9324_default_regs[] = { { SX9324_REG_AFE_CTRL8, SX9324_REG_AFE_CTRL8_RESFILTN_4KOHM }, { SX9324_REG_AFE_CTRL9, SX9324_REG_AFE_CTRL9_AGAIN_1 }, - { SX9324_REG_PROX_CTRL0, SX9324_REG_PROX_CTRL0_GAIN_1 | + { SX9324_REG_PROX_CTRL0, + SX9324_REG_PROX_CTRL0_GAIN_1 << SX9324_REG_PROX_CTRL0_GAIN_SHIFT | SX9324_REG_PROX_CTRL0_RAWFILT_1P50 }, - { SX9324_REG_PROX_CTRL1, SX9324_REG_PROX_CTRL0_GAIN_1 | + { SX9324_REG_PROX_CTRL1, + SX9324_REG_PROX_CTRL0_GAIN_1 << SX9324_REG_PROX_CTRL0_GAIN_SHIFT | SX9324_REG_PROX_CTRL0_RAWFILT_1P50 }, { SX9324_REG_PROX_CTRL2, SX9324_REG_PROX_CTRL2_AVGNEG_THRESH_16K }, { SX9324_REG_PROX_CTRL3, SX9324_REG_PROX_CTRL3_AVGDEB_2SAMPLES | -- cgit v1.2.3