1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Copyright (C) 2026 InvenSense, Inc. 4 */ 5 6 #include <linux/array_size.h> 7 #include <linux/bits.h> 8 #include <linux/cleanup.h> 9 #include <linux/device/devres.h> 10 #include <linux/err.h> 11 #include <linux/iio/iio.h> 12 #include <linux/mutex.h> 13 #include <linux/pm_runtime.h> 14 #include <linux/regmap.h> 15 #include <linux/types.h> 16 17 #include "inv_icm42607.h" 18 #include "inv_icm42607_temp.h" 19 20 #define INV_ICM42607_GYRO_CHAN(_modifier, _index, _ext_info) \ 21 { \ 22 .type = IIO_ANGL_VEL, \ 23 .modified = 1, \ 24 .channel2 = _modifier, \ 25 .info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \ 26 .info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \ 27 .info_mask_shared_by_type_available = BIT(IIO_CHAN_INFO_SCALE), \ 28 .info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 29 .info_mask_shared_by_all_available = BIT(IIO_CHAN_INFO_SAMP_FREQ), \ 30 .scan_index = _index, \ 31 .scan_type = { \ 32 .sign = 's', \ 33 .realbits = 16, \ 34 .storagebits = 16, \ 35 .endianness = IIO_BE, \ 36 }, \ 37 .ext_info = _ext_info, \ 38 } 39 40 enum inv_icm42607_gyro_scan { 41 INV_ICM42607_GYRO_SCAN_X, 42 INV_ICM42607_GYRO_SCAN_Y, 43 INV_ICM42607_GYRO_SCAN_Z, 44 INV_ICM42607_GYRO_SCAN_TEMP, 45 }; 46 47 static const struct iio_chan_spec_ext_info inv_icm42607_gyro_ext_infos[] = { 48 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, inv_icm42607_get_mount_matrix), 49 { } 50 }; 51 52 static const struct iio_chan_spec inv_icm42607_gyro_channels[] = { 53 INV_ICM42607_GYRO_CHAN(IIO_MOD_X, INV_ICM42607_GYRO_SCAN_X, 54 inv_icm42607_gyro_ext_infos), 55 INV_ICM42607_GYRO_CHAN(IIO_MOD_Y, INV_ICM42607_GYRO_SCAN_Y, 56 inv_icm42607_gyro_ext_infos), 57 INV_ICM42607_GYRO_CHAN(IIO_MOD_Z, INV_ICM42607_GYRO_SCAN_Z, 58 inv_icm42607_gyro_ext_infos), 59 INV_ICM42607_TEMP_CHAN(INV_ICM42607_GYRO_SCAN_TEMP), 60 }; 61 62 static const int inv_icm42607_gyro_scale_nano[][2] = { 63 [INV_ICM42607_GYRO_FS_2000DPS] = { 0, 1065264 }, 64 [INV_ICM42607_GYRO_FS_1000DPS] = { 0, 532632 }, 65 [INV_ICM42607_GYRO_FS_500DPS] = { 0, 266316 }, 66 [INV_ICM42607_GYRO_FS_250DPS] = { 0, 133158 }, 67 }; 68 69 static int inv_icm42607_gyro_read_scale(struct iio_dev *indio_dev, 70 int *val, int *val2) 71 { 72 struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev); 73 unsigned int idx; 74 75 guard(mutex)(&st->lock); 76 77 idx = st->conf.gyro.fs; 78 79 *val = inv_icm42607_gyro_scale_nano[idx][0]; 80 *val2 = inv_icm42607_gyro_scale_nano[idx][1]; 81 return IIO_VAL_INT_PLUS_NANO; 82 } 83 84 static int inv_icm42607_gyro_write_scale(struct iio_dev *indio_dev, 85 int val, int val2) 86 { 87 struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev); 88 struct device *dev = regmap_get_device(st->map); 89 unsigned int idx; 90 struct inv_icm42607_sensor_conf conf = INV_ICM42607_SENSOR_CONF_INIT; 91 size_t scales_len = ARRAY_SIZE(inv_icm42607_gyro_scale_nano); 92 int ret; 93 94 for (idx = 0; idx < scales_len; idx++) { 95 if (val == inv_icm42607_gyro_scale_nano[idx][0] && 96 val2 == inv_icm42607_gyro_scale_nano[idx][1]) 97 break; 98 } 99 if (idx == scales_len) 100 return -EINVAL; 101 102 conf.fs = idx; 103 104 PM_RUNTIME_ACQUIRE_AUTOSUSPEND(dev, pm); 105 ret = PM_RUNTIME_ACQUIRE_ERR(&pm); 106 if (ret) 107 return ret; 108 109 guard(mutex)(&st->lock); 110 111 return inv_icm42607_set_sensor_conf(st, &conf, IIO_ANGL_VEL); 112 } 113 114 /* IIO format int + micro , values 0-4 reserved. */ 115 static const int inv_icm42607_gyro_odr[][2] = { 116 [INV_ICM42607_ODR_1600HZ] = { 1600, 0 }, 117 [INV_ICM42607_ODR_800HZ] = { 800, 0 }, 118 [INV_ICM42607_ODR_400HZ] = { 400, 0 }, 119 [INV_ICM42607_ODR_200HZ] = { 200, 0 }, 120 [INV_ICM42607_ODR_100HZ] = { 100, 0 }, 121 [INV_ICM42607_ODR_50HZ] = { 50, 0 }, 122 [INV_ICM42607_ODR_25HZ] = { 25, 0 }, 123 [INV_ICM42607_ODR_12_5HZ] = { 12, 500000 }, 124 }; 125 126 static int inv_icm42607_gyro_read_odr(struct inv_icm42607_state *st, 127 int *val, int *val2) 128 { 129 unsigned int odr; 130 unsigned int i; 131 132 guard(mutex)(&st->lock); 133 134 odr = st->conf.gyro.odr; 135 136 for (i = INV_ICM42607_ODR_1600HZ; i < ARRAY_SIZE(inv_icm42607_gyro_odr); i++) { 137 if (i == odr) 138 break; 139 } 140 if (i == ARRAY_SIZE(inv_icm42607_gyro_odr)) 141 return -EINVAL; 142 143 *val = inv_icm42607_gyro_odr[i][0]; 144 *val2 = inv_icm42607_gyro_odr[i][1]; 145 146 return IIO_VAL_INT_PLUS_MICRO; 147 } 148 149 static int inv_icm42607_gyro_write_odr(struct iio_dev *indio_dev, 150 int val, int val2) 151 { 152 struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev); 153 struct device *dev = regmap_get_device(st->map); 154 unsigned int idx; 155 struct inv_icm42607_sensor_conf conf = INV_ICM42607_SENSOR_CONF_INIT; 156 int ret; 157 158 for (idx = INV_ICM42607_ODR_1600HZ; 159 idx < ARRAY_SIZE(inv_icm42607_gyro_odr); idx++) { 160 if (val == inv_icm42607_gyro_odr[idx][0] && 161 val2 == inv_icm42607_gyro_odr[idx][1]) 162 break; 163 } 164 if (idx == ARRAY_SIZE(inv_icm42607_gyro_odr)) 165 return -EINVAL; 166 167 conf.odr = idx; 168 169 PM_RUNTIME_ACQUIRE_AUTOSUSPEND(dev, pm); 170 ret = PM_RUNTIME_ACQUIRE_ERR(&pm); 171 if (ret) 172 return ret; 173 174 guard(mutex)(&st->lock); 175 176 return inv_icm42607_set_sensor_conf(st, &conf, IIO_ANGL_VEL); 177 } 178 179 static int inv_icm42607_gyro_read_raw(struct iio_dev *indio_dev, 180 struct iio_chan_spec const *chan, 181 int *val, int *val2, long mask) 182 { 183 struct inv_icm42607_state *st = iio_device_get_drvdata(indio_dev); 184 s16 data; 185 int ret; 186 187 switch (chan->type) { 188 case IIO_ANGL_VEL: 189 break; 190 case IIO_TEMP: 191 if (mask != IIO_CHAN_INFO_SAMP_FREQ) 192 return inv_icm42607_temp_read_raw(indio_dev, chan, 193 val, val2, mask); 194 break; 195 default: 196 return -EINVAL; 197 } 198 199 switch (mask) { 200 case IIO_CHAN_INFO_RAW: 201 ret = inv_icm42607_read_sensor(indio_dev, chan, &data); 202 if (ret) 203 return ret; 204 *val = data; 205 return IIO_VAL_INT; 206 case IIO_CHAN_INFO_SCALE: 207 return inv_icm42607_gyro_read_scale(indio_dev, val, val2); 208 case IIO_CHAN_INFO_SAMP_FREQ: 209 return inv_icm42607_gyro_read_odr(st, val, val2); 210 default: 211 return -EINVAL; 212 } 213 } 214 215 static int inv_icm42607_gyro_read_avail(struct iio_dev *indio_dev, 216 struct iio_chan_spec const *chan, 217 const int **vals, 218 int *type, int *length, long mask) 219 { 220 switch (mask) { 221 case IIO_CHAN_INFO_SCALE: 222 if (chan->type != IIO_ANGL_VEL) 223 return -EINVAL; 224 *vals = (const int *)inv_icm42607_gyro_scale_nano; 225 *type = IIO_VAL_INT_PLUS_NANO; 226 *length = ARRAY_SIZE(inv_icm42607_gyro_scale_nano) * 2; 227 return IIO_AVAIL_LIST; 228 case IIO_CHAN_INFO_SAMP_FREQ: 229 *vals = (const int *)inv_icm42607_gyro_odr[INV_ICM42607_ODR_1600HZ]; 230 *type = IIO_VAL_INT_PLUS_MICRO; 231 *length = (ARRAY_SIZE(inv_icm42607_gyro_odr) - 232 INV_ICM42607_ODR_1600HZ) * 2; 233 return IIO_AVAIL_LIST; 234 default: 235 return -EINVAL; 236 } 237 } 238 239 static int inv_icm42607_gyro_write_raw(struct iio_dev *indio_dev, 240 struct iio_chan_spec const *chan, 241 int val, int val2, long mask) 242 { 243 int ret; 244 245 switch (mask) { 246 case IIO_CHAN_INFO_SCALE: 247 if (chan->type != IIO_ANGL_VEL) 248 return -EINVAL; 249 ret = inv_icm42607_gyro_write_scale(indio_dev, val, val2); 250 return ret; 251 case IIO_CHAN_INFO_SAMP_FREQ: 252 return inv_icm42607_gyro_write_odr(indio_dev, val, val2); 253 default: 254 return -EINVAL; 255 } 256 } 257 258 static int inv_icm42607_gyro_write_raw_get_fmt(struct iio_dev *indio_dev, 259 struct iio_chan_spec const *chan, 260 long mask) 261 { 262 switch (mask) { 263 case IIO_CHAN_INFO_SCALE: 264 if (chan->type != IIO_ANGL_VEL) 265 return -EINVAL; 266 return IIO_VAL_INT_PLUS_NANO; 267 case IIO_CHAN_INFO_SAMP_FREQ: 268 return IIO_VAL_INT_PLUS_MICRO; 269 default: 270 return -EINVAL; 271 } 272 } 273 274 static const struct iio_info inv_icm42607_gyro_info = { 275 .read_raw = inv_icm42607_gyro_read_raw, 276 .read_avail = inv_icm42607_gyro_read_avail, 277 .write_raw = inv_icm42607_gyro_write_raw, 278 .write_raw_get_fmt = inv_icm42607_gyro_write_raw_get_fmt, 279 }; 280 281 struct iio_dev *inv_icm42607_gyro_init(struct inv_icm42607_state *st) 282 { 283 struct device *dev = regmap_get_device(st->map); 284 const char *name; 285 struct inv_icm42607_sensor_state *gyro_st; 286 struct iio_dev *indio_dev; 287 int ret; 288 289 name = devm_kasprintf(dev, GFP_KERNEL, "%s-gyro", st->hw->name); 290 if (!name) 291 return ERR_PTR(-ENOMEM); 292 293 indio_dev = devm_iio_device_alloc(dev, sizeof(*gyro_st)); 294 if (!indio_dev) 295 return ERR_PTR(-ENOMEM); 296 297 gyro_st = iio_priv(indio_dev); 298 gyro_st->power_mode = INV_ICM42607_SENSOR_MODE_LOW_NOISE; 299 gyro_st->filter = INV_ICM42607_FILTER_BW_73HZ; 300 301 indio_dev->name = name; 302 indio_dev->info = &inv_icm42607_gyro_info; 303 indio_dev->modes = INDIO_DIRECT_MODE; 304 indio_dev->channels = inv_icm42607_gyro_channels; 305 indio_dev->num_channels = ARRAY_SIZE(inv_icm42607_gyro_channels); 306 iio_device_set_drvdata(indio_dev, st); 307 308 ret = devm_iio_device_register(dev, indio_dev); 309 if (ret) 310 return ERR_PTR(ret); 311 312 return indio_dev; 313 } 314