diff --git a/drivers/staging/iio/imu/adis16400.h b/drivers/staging/iio/imu/adis16400.h index 77c601da1846..7a105e966464 100644 --- a/drivers/staging/iio/imu/adis16400.h +++ b/drivers/staging/iio/imu/adis16400.h @@ -123,6 +123,9 @@ /* SLP_CNT */ #define ADIS16400_SLP_CNT_POWER_OFF (1<<8) +#define ADIS16334_RATE_DIV_SHIFT 8 +#define ADIS16334_RATE_INT_CLK BIT(0) + #define ADIS16400_MAX_TX 24 #define ADIS16400_MAX_RX 24 @@ -130,18 +133,21 @@ #define ADIS16400_SPI_BURST (u32)(1000 * 1000) #define ADIS16400_SPI_FAST (u32)(2000 * 1000) -#define ADIS16400_HAS_PROD_ID 1 -#define ADIS16400_NO_BURST 2 +#define ADIS16400_HAS_PROD_ID BIT(0) +#define ADIS16400_NO_BURST BIT(1) +#define ADIS16400_HAS_SLOW_MODE BIT(2) + struct adis16400_chip_info { const struct iio_chan_spec *channels; const int num_channels; - const int product_id; const long flags; unsigned int gyro_scale_micro; unsigned int accel_scale_micro; int temp_scale_nano; int temp_offset; unsigned long default_scan_mask; + int (*set_freq)(struct iio_dev *indio_dev, unsigned int freq); + int (*get_freq)(struct iio_dev *indio_dev); }; /** diff --git a/drivers/staging/iio/imu/adis16400_core.c b/drivers/staging/iio/imu/adis16400_core.c index baec6a1f23a0..c3f9bb95c7b3 100644 --- a/drivers/staging/iio/imu/adis16400_core.c +++ b/drivers/staging/iio/imu/adis16400_core.c @@ -38,7 +38,6 @@ enum adis16400_chip_variant { ADIS16360, ADIS16362, ADIS16364, - ADIS16365, ADIS16400, }; @@ -161,10 +160,39 @@ static int adis16400_spi_read_reg_16(struct iio_dev *indio_dev, return ret; } +static int adis16334_get_freq(struct iio_dev *indio_dev) +{ + int ret; + u16 t; + + ret = adis16400_spi_read_reg_16(indio_dev, ADIS16400_SMPL_PRD, &t); + if (ret < 0) + return ret; + + t >>= ADIS16334_RATE_DIV_SHIFT; + + return (8192 >> t) / 10; +} + +static int adis16334_set_freq(struct iio_dev *indio_dev, unsigned int freq) +{ + unsigned int t; + + t = ilog2(8192 / (freq * 10)); + + if (t > 0x31) + t = 0x31; + + t <<= ADIS16334_RATE_DIV_SHIFT; + t |= ADIS16334_RATE_INT_CLK; + + return adis16400_spi_write_reg_16(indio_dev, ADIS16400_SMPL_PRD, t); +} + static int adis16400_get_freq(struct iio_dev *indio_dev) { - u16 t; int sps, ret; + u16 t; ret = adis16400_spi_read_reg_16(indio_dev, ADIS16400_SMPL_PRD, &t); if (ret < 0) @@ -175,13 +203,33 @@ static int adis16400_get_freq(struct iio_dev *indio_dev) return sps; } +static int adis16400_set_freq(struct iio_dev *indio_dev, unsigned int freq) +{ + struct adis16400_state *st = iio_priv(indio_dev); + unsigned int t; + + t = 1638 / freq; + if (t > 0) + t--; + t &= ADIS16400_SMPL_PRD_DIV_MASK; + if ((t & ADIS16400_SMPL_PRD_DIV_MASK) >= 0x0A) + st->us->max_speed_hz = ADIS16400_SPI_SLOW; + else + st->us->max_speed_hz = ADIS16400_SPI_FAST; + + return adis16400_spi_write_reg_8(indio_dev, + ADIS16400_SMPL_PRD, t); +} + static ssize_t adis16400_read_frequency(struct device *dev, struct device_attribute *attr, char *buf) { struct iio_dev *indio_dev = dev_to_iio_dev(dev); + struct adis16400_state *st = iio_priv(indio_dev); int ret, len = 0; - ret = adis16400_get_freq(indio_dev); + + ret = st->variant->get_freq(indio_dev); if (ret < 0) return ret; len = sprintf(buf, "%d SPS\n", ret); @@ -229,7 +277,6 @@ static ssize_t adis16400_write_frequency(struct device *dev, struct adis16400_state *st = iio_priv(indio_dev); long val; int ret; - u8 t; ret = strict_strtol(buf, 10, &val); if (ret) @@ -239,18 +286,7 @@ static ssize_t adis16400_write_frequency(struct device *dev, mutex_lock(&indio_dev->mlock); - t = (1638 / val); - if (t > 0) - t--; - t &= ADIS16400_SMPL_PRD_DIV_MASK; - if ((t & ADIS16400_SMPL_PRD_DIV_MASK) >= 0x0A) - st->us->max_speed_hz = ADIS16400_SPI_SLOW; - else - st->us->max_speed_hz = ADIS16400_SPI_FAST; - - ret = adis16400_spi_write_reg_8(indio_dev, - ADIS16400_SMPL_PRD, - t); + st->variant->set_freq(indio_dev, val); /* Also update the filter */ mutex_unlock(&indio_dev->mlock); @@ -378,10 +414,14 @@ static int adis16400_initial_setup(struct iio_dev *indio_dev) { int ret; u16 prod_id, smp_prd; + unsigned int device_id; struct adis16400_state *st = iio_priv(indio_dev); - /* use low spi speed for init */ - st->us->max_speed_hz = ADIS16400_SPI_SLOW; + /* use low spi speed for init if the device has a slow mode */ + if (st->variant->flags & ADIS16400_HAS_SLOW_MODE) + st->us->max_speed_hz = ADIS16400_SPI_SLOW; + else + st->us->max_speed_hz = ADIS16400_SPI_FAST; st->us->mode = SPI_MODE_3; spi_setup(st->us); @@ -414,19 +454,27 @@ static int adis16400_initial_setup(struct iio_dev *indio_dev) if (ret) goto err_ret; - if ((prod_id & 0xF000) != st->variant->product_id) - dev_warn(&indio_dev->dev, "incorrect id"); + sscanf(indio_dev->name, "adis%u\n", &device_id); + + if (prod_id != device_id) + dev_warn(&indio_dev->dev, "Device ID(%u) and product ID(%u) do not match.", + device_id, prod_id); dev_info(&indio_dev->dev, "%s: prod_id 0x%04x at CS%d (irq %d)\n", indio_dev->name, prod_id, st->us->chip_select, st->us->irq); } /* use high spi speed if possible */ - ret = adis16400_spi_read_reg_16(indio_dev, - ADIS16400_SMPL_PRD, &smp_prd); - if (!ret && (smp_prd & ADIS16400_SMPL_PRD_DIV_MASK) < 0x0A) { - st->us->max_speed_hz = ADIS16400_SPI_SLOW; - spi_setup(st->us); + if (st->variant->flags & ADIS16400_HAS_SLOW_MODE) { + ret = adis16400_spi_read_reg_16(indio_dev, + ADIS16400_SMPL_PRD, &smp_prd); + if (ret) + goto err_ret; + + if ((smp_prd & ADIS16400_SMPL_PRD_DIV_MASK) < 0x0A) { + st->us->max_speed_hz = ADIS16400_SPI_FAST; + spi_setup(st->us); + } } err_ret: @@ -503,7 +551,7 @@ static int adis16400_write_raw(struct iio_dev *indio_dev, mutex_lock(&indio_dev->mlock); st->filt_int = val; /* Work out update to current value */ - sps = adis16400_get_freq(indio_dev); + sps = st->variant->get_freq(indio_dev); if (sps < 0) { mutex_unlock(&indio_dev->mlock); return sps; @@ -601,7 +649,7 @@ static int adis16400_read_raw(struct iio_dev *indio_dev, mutex_unlock(&indio_dev->mlock); return ret; } - ret = adis16400_get_freq(indio_dev); + val16 = st->variant->get_freq(indio_dev); if (ret > 0) *val = ret/adis16400_3db_divisors[val16 & 0x03]; *val2 = 0; @@ -1060,6 +1108,7 @@ static struct adis16400_chip_info adis16400_chips[] = { [ADIS16300] = { .channels = adis16300_channels, .num_channels = ARRAY_SIZE(adis16300_channels), + .flags = ADIS16400_HAS_SLOW_MODE, .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ .accel_scale_micro = 5884, .temp_scale_nano = 140000000, /* 0.14 C */ @@ -1070,10 +1119,13 @@ static struct adis16400_chip_info adis16400_chips[] = { (1 << ADIS16400_SCAN_TEMP) | (1 << ADIS16400_SCAN_ADC_0) | (1 << ADIS16300_SCAN_INCLI_X) | (1 << ADIS16300_SCAN_INCLI_Y) | (1 << 14), + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, }, [ADIS16334] = { .channels = adis16334_channels, .num_channels = ARRAY_SIZE(adis16334_channels), + .flags = ADIS16400_HAS_PROD_ID, .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */ .temp_scale_nano = 67850000, /* 0.06785 C */ @@ -1082,6 +1134,8 @@ static struct adis16400_chip_info adis16400_chips[] = { (1 << ADIS16400_SCAN_GYRO_Y) | (1 << ADIS16400_SCAN_GYRO_Z) | (1 << ADIS16400_SCAN_ACC_X) | (1 << ADIS16400_SCAN_ACC_Y) | (1 << ADIS16400_SCAN_ACC_Z), + .set_freq = adis16334_set_freq, + .get_freq = adis16334_get_freq, }, [ADIS16350] = { .channels = adis16350_channels, @@ -1091,62 +1145,57 @@ static struct adis16400_chip_info adis16400_chips[] = { .temp_scale_nano = 145300000, /* 0.1453 C */ .temp_offset = 25000000 / 145300, /* 25 C = 0x00 */ .default_scan_mask = 0x7FF, - .flags = ADIS16400_NO_BURST, + .flags = ADIS16400_NO_BURST | ADIS16400_HAS_SLOW_MODE, + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, }, [ADIS16360] = { .channels = adis16350_channels, .num_channels = ARRAY_SIZE(adis16350_channels), - .flags = ADIS16400_HAS_PROD_ID, - .product_id = 0x3FE8, + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE, .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */ .temp_scale_nano = 136000000, /* 0.136 C */ .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ .default_scan_mask = 0x7FF, + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, }, [ADIS16362] = { .channels = adis16350_channels, .num_channels = ARRAY_SIZE(adis16350_channels), - .flags = ADIS16400_HAS_PROD_ID, - .product_id = 0x3FEA, + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE, .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ .accel_scale_micro = IIO_G_TO_M_S_2(333), /* 0.333 mg */ .temp_scale_nano = 136000000, /* 0.136 C */ .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ .default_scan_mask = 0x7FF, + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, }, [ADIS16364] = { .channels = adis16350_channels, .num_channels = ARRAY_SIZE(adis16350_channels), - .flags = ADIS16400_HAS_PROD_ID, - .product_id = 0x3FEC, - .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ - .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */ - .temp_scale_nano = 136000000, /* 0.136 C */ - .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ - .default_scan_mask = 0x7FF, - }, - [ADIS16365] = { - .channels = adis16350_channels, - .num_channels = ARRAY_SIZE(adis16350_channels), - .flags = ADIS16400_HAS_PROD_ID, - .product_id = 0x3FED, + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE, .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ .accel_scale_micro = IIO_G_TO_M_S_2(1000), /* 1 mg */ .temp_scale_nano = 136000000, /* 0.136 C */ .temp_offset = 25000000 / 136000, /* 25 C = 0x00 */ .default_scan_mask = 0x7FF, + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, }, [ADIS16400] = { .channels = adis16400_channels, .num_channels = ARRAY_SIZE(adis16400_channels), - .flags = ADIS16400_HAS_PROD_ID, - .product_id = 0x4015, + .flags = ADIS16400_HAS_PROD_ID | ADIS16400_HAS_SLOW_MODE, .gyro_scale_micro = IIO_DEGREE_TO_RAD(50000), /* 0.05 deg/s */ .accel_scale_micro = IIO_G_TO_M_S_2(3333), /* 3.333 mg */ .default_scan_mask = 0xFFF, .temp_scale_nano = 140000000, /* 0.14 C */ .temp_offset = 25000000 / 140000, /* 25 C = 0x00 */ + .set_freq = adis16400_set_freq, + .get_freq = adis16400_get_freq, } }; @@ -1248,7 +1297,7 @@ static const struct spi_device_id adis16400_id[] = { {"adis16360", ADIS16360}, {"adis16362", ADIS16362}, {"adis16364", ADIS16364}, - {"adis16365", ADIS16365}, + {"adis16365", ADIS16360}, {"adis16400", ADIS16400}, {"adis16405", ADIS16400}, {}