1 /* 2 * STMicroelectronics sensors core library driver 3 * 4 * Copyright 2012-2013 STMicroelectronics Inc. 5 * 6 * Denis Ciocca <denis.ciocca@st.com> 7 * 8 * Licensed under the GPL-2. 9 */ 10 11 #include <linux/kernel.h> 12 #include <linux/module.h> 13 #include <linux/slab.h> 14 #include <linux/delay.h> 15 #include <linux/iio/iio.h> 16 #include <asm/unaligned.h> 17 18 #include <linux/iio/common/st_sensors.h> 19 20 21 #define ST_SENSORS_WAI_ADDRESS 0x0f 22 23 static inline u32 st_sensors_get_unaligned_le24(const u8 *p) 24 { 25 return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8; 26 } 27 28 static int st_sensors_write_data_with_mask(struct iio_dev *indio_dev, 29 u8 reg_addr, u8 mask, u8 data) 30 { 31 int err; 32 u8 new_data; 33 struct st_sensor_data *sdata = iio_priv(indio_dev); 34 35 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, reg_addr, &new_data); 36 if (err < 0) 37 goto st_sensors_write_data_with_mask_error; 38 39 new_data = ((new_data & (~mask)) | ((data << __ffs(mask)) & mask)); 40 err = sdata->tf->write_byte(&sdata->tb, sdata->dev, reg_addr, new_data); 41 42 st_sensors_write_data_with_mask_error: 43 return err; 44 } 45 46 static int st_sensors_match_odr(struct st_sensors *sensor, 47 unsigned int odr, struct st_sensor_odr_avl *odr_out) 48 { 49 int i, ret = -EINVAL; 50 51 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 52 if (sensor->odr.odr_avl[i].hz == 0) 53 goto st_sensors_match_odr_error; 54 55 if (sensor->odr.odr_avl[i].hz == odr) { 56 odr_out->hz = sensor->odr.odr_avl[i].hz; 57 odr_out->value = sensor->odr.odr_avl[i].value; 58 ret = 0; 59 break; 60 } 61 } 62 63 st_sensors_match_odr_error: 64 return ret; 65 } 66 67 int st_sensors_set_odr(struct iio_dev *indio_dev, unsigned int odr) 68 { 69 int err; 70 struct st_sensor_odr_avl odr_out = {0, 0}; 71 struct st_sensor_data *sdata = iio_priv(indio_dev); 72 73 err = st_sensors_match_odr(sdata->sensor, odr, &odr_out); 74 if (err < 0) 75 goto st_sensors_match_odr_error; 76 77 if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) && 78 (sdata->sensor->odr.mask == sdata->sensor->pw.mask)) { 79 if (sdata->enabled == true) { 80 err = st_sensors_write_data_with_mask(indio_dev, 81 sdata->sensor->odr.addr, 82 sdata->sensor->odr.mask, 83 odr_out.value); 84 } else { 85 err = 0; 86 } 87 } else { 88 err = st_sensors_write_data_with_mask(indio_dev, 89 sdata->sensor->odr.addr, sdata->sensor->odr.mask, 90 odr_out.value); 91 } 92 if (err >= 0) 93 sdata->odr = odr_out.hz; 94 95 st_sensors_match_odr_error: 96 return err; 97 } 98 EXPORT_SYMBOL(st_sensors_set_odr); 99 100 static int st_sensors_match_fs(struct st_sensors *sensor, 101 unsigned int fs, int *index_fs_avl) 102 { 103 int i, ret = -EINVAL; 104 105 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 106 if (sensor->fs.fs_avl[i].num == 0) 107 goto st_sensors_match_odr_error; 108 109 if (sensor->fs.fs_avl[i].num == fs) { 110 *index_fs_avl = i; 111 ret = 0; 112 break; 113 } 114 } 115 116 st_sensors_match_odr_error: 117 return ret; 118 } 119 120 static int st_sensors_set_fullscale(struct iio_dev *indio_dev, 121 unsigned int fs) 122 { 123 int err, i = 0; 124 struct st_sensor_data *sdata = iio_priv(indio_dev); 125 126 err = st_sensors_match_fs(sdata->sensor, fs, &i); 127 if (err < 0) 128 goto st_accel_set_fullscale_error; 129 130 err = st_sensors_write_data_with_mask(indio_dev, 131 sdata->sensor->fs.addr, 132 sdata->sensor->fs.mask, 133 sdata->sensor->fs.fs_avl[i].value); 134 if (err < 0) 135 goto st_accel_set_fullscale_error; 136 137 sdata->current_fullscale = (struct st_sensor_fullscale_avl *) 138 &sdata->sensor->fs.fs_avl[i]; 139 return err; 140 141 st_accel_set_fullscale_error: 142 dev_err(&indio_dev->dev, "failed to set new fullscale.\n"); 143 return err; 144 } 145 146 int st_sensors_set_enable(struct iio_dev *indio_dev, bool enable) 147 { 148 u8 tmp_value; 149 int err = -EINVAL; 150 bool found = false; 151 struct st_sensor_odr_avl odr_out = {0, 0}; 152 struct st_sensor_data *sdata = iio_priv(indio_dev); 153 154 if (enable) { 155 tmp_value = sdata->sensor->pw.value_on; 156 if ((sdata->sensor->odr.addr == sdata->sensor->pw.addr) && 157 (sdata->sensor->odr.mask == sdata->sensor->pw.mask)) { 158 err = st_sensors_match_odr(sdata->sensor, 159 sdata->odr, &odr_out); 160 if (err < 0) 161 goto set_enable_error; 162 tmp_value = odr_out.value; 163 found = true; 164 } 165 err = st_sensors_write_data_with_mask(indio_dev, 166 sdata->sensor->pw.addr, 167 sdata->sensor->pw.mask, tmp_value); 168 if (err < 0) 169 goto set_enable_error; 170 171 sdata->enabled = true; 172 173 if (found) 174 sdata->odr = odr_out.hz; 175 } else { 176 err = st_sensors_write_data_with_mask(indio_dev, 177 sdata->sensor->pw.addr, 178 sdata->sensor->pw.mask, 179 sdata->sensor->pw.value_off); 180 if (err < 0) 181 goto set_enable_error; 182 183 sdata->enabled = false; 184 } 185 186 set_enable_error: 187 return err; 188 } 189 EXPORT_SYMBOL(st_sensors_set_enable); 190 191 int st_sensors_set_axis_enable(struct iio_dev *indio_dev, u8 axis_enable) 192 { 193 struct st_sensor_data *sdata = iio_priv(indio_dev); 194 195 return st_sensors_write_data_with_mask(indio_dev, 196 sdata->sensor->enable_axis.addr, 197 sdata->sensor->enable_axis.mask, axis_enable); 198 } 199 EXPORT_SYMBOL(st_sensors_set_axis_enable); 200 201 static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev, 202 struct st_sensors_platform_data *pdata) 203 { 204 struct st_sensor_data *sdata = iio_priv(indio_dev); 205 206 switch (pdata->drdy_int_pin) { 207 case 1: 208 if (sdata->sensor->drdy_irq.mask_int1 == 0) { 209 dev_err(&indio_dev->dev, 210 "DRDY on INT1 not available.\n"); 211 return -EINVAL; 212 } 213 sdata->drdy_int_pin = 1; 214 break; 215 case 2: 216 if (sdata->sensor->drdy_irq.mask_int2 == 0) { 217 dev_err(&indio_dev->dev, 218 "DRDY on INT2 not available.\n"); 219 return -EINVAL; 220 } 221 sdata->drdy_int_pin = 2; 222 break; 223 default: 224 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n"); 225 return -EINVAL; 226 } 227 228 return 0; 229 } 230 231 int st_sensors_init_sensor(struct iio_dev *indio_dev, 232 struct st_sensors_platform_data *pdata) 233 { 234 struct st_sensor_data *sdata = iio_priv(indio_dev); 235 int err = 0; 236 237 mutex_init(&sdata->tb.buf_lock); 238 239 if (pdata) 240 err = st_sensors_set_drdy_int_pin(indio_dev, pdata); 241 242 err = st_sensors_set_enable(indio_dev, false); 243 if (err < 0) 244 return err; 245 246 if (sdata->current_fullscale) { 247 err = st_sensors_set_fullscale(indio_dev, 248 sdata->current_fullscale->num); 249 if (err < 0) 250 return err; 251 } else 252 dev_info(&indio_dev->dev, "Full-scale not possible\n"); 253 254 err = st_sensors_set_odr(indio_dev, sdata->odr); 255 if (err < 0) 256 return err; 257 258 /* set BDU */ 259 err = st_sensors_write_data_with_mask(indio_dev, 260 sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true); 261 if (err < 0) 262 return err; 263 264 err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS); 265 266 return err; 267 } 268 EXPORT_SYMBOL(st_sensors_init_sensor); 269 270 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable) 271 { 272 int err; 273 u8 drdy_mask; 274 struct st_sensor_data *sdata = iio_priv(indio_dev); 275 276 if (!sdata->sensor->drdy_irq.addr) 277 return 0; 278 279 /* Enable/Disable the interrupt generator 1. */ 280 if (sdata->sensor->drdy_irq.ig1.en_addr > 0) { 281 err = st_sensors_write_data_with_mask(indio_dev, 282 sdata->sensor->drdy_irq.ig1.en_addr, 283 sdata->sensor->drdy_irq.ig1.en_mask, (int)enable); 284 if (err < 0) 285 goto st_accel_set_dataready_irq_error; 286 } 287 288 if (sdata->drdy_int_pin == 1) 289 drdy_mask = sdata->sensor->drdy_irq.mask_int1; 290 else 291 drdy_mask = sdata->sensor->drdy_irq.mask_int2; 292 293 /* Enable/Disable the interrupt generator for data ready. */ 294 err = st_sensors_write_data_with_mask(indio_dev, 295 sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable); 296 297 st_accel_set_dataready_irq_error: 298 return err; 299 } 300 EXPORT_SYMBOL(st_sensors_set_dataready_irq); 301 302 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale) 303 { 304 int err = -EINVAL, i; 305 struct st_sensor_data *sdata = iio_priv(indio_dev); 306 307 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 308 if ((sdata->sensor->fs.fs_avl[i].gain == scale) && 309 (sdata->sensor->fs.fs_avl[i].gain != 0)) { 310 err = 0; 311 break; 312 } 313 } 314 if (err < 0) 315 goto st_sensors_match_scale_error; 316 317 err = st_sensors_set_fullscale(indio_dev, 318 sdata->sensor->fs.fs_avl[i].num); 319 320 st_sensors_match_scale_error: 321 return err; 322 } 323 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain); 324 325 static int st_sensors_read_axis_data(struct iio_dev *indio_dev, 326 struct iio_chan_spec const *ch, int *data) 327 { 328 int err; 329 u8 *outdata; 330 struct st_sensor_data *sdata = iio_priv(indio_dev); 331 unsigned int byte_for_channel = ch->scan_type.storagebits >> 3; 332 333 outdata = kmalloc(byte_for_channel, GFP_KERNEL); 334 if (!outdata) { 335 err = -EINVAL; 336 goto st_sensors_read_axis_data_error; 337 } 338 339 err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev, 340 ch->address, byte_for_channel, 341 outdata, sdata->multiread_bit); 342 if (err < 0) 343 goto st_sensors_free_memory; 344 345 if (byte_for_channel == 2) 346 *data = (s16)get_unaligned_le16(outdata); 347 else if (byte_for_channel == 3) 348 *data = (s32)st_sensors_get_unaligned_le24(outdata); 349 350 st_sensors_free_memory: 351 kfree(outdata); 352 st_sensors_read_axis_data_error: 353 return err; 354 } 355 356 int st_sensors_read_info_raw(struct iio_dev *indio_dev, 357 struct iio_chan_spec const *ch, int *val) 358 { 359 int err; 360 struct st_sensor_data *sdata = iio_priv(indio_dev); 361 362 mutex_lock(&indio_dev->mlock); 363 if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) { 364 err = -EBUSY; 365 goto out; 366 } else { 367 err = st_sensors_set_enable(indio_dev, true); 368 if (err < 0) 369 goto out; 370 371 msleep((sdata->sensor->bootime * 1000) / sdata->odr); 372 err = st_sensors_read_axis_data(indio_dev, ch, val); 373 if (err < 0) 374 goto out; 375 376 *val = *val >> ch->scan_type.shift; 377 378 err = st_sensors_set_enable(indio_dev, false); 379 } 380 out: 381 mutex_unlock(&indio_dev->mlock); 382 383 return err; 384 } 385 EXPORT_SYMBOL(st_sensors_read_info_raw); 386 387 int st_sensors_check_device_support(struct iio_dev *indio_dev, 388 int num_sensors_list, const struct st_sensors *sensors) 389 { 390 u8 wai; 391 int i, n, err; 392 struct st_sensor_data *sdata = iio_priv(indio_dev); 393 394 err = sdata->tf->read_byte(&sdata->tb, sdata->dev, 395 ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai); 396 if (err < 0) { 397 dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n"); 398 goto read_wai_error; 399 } 400 401 for (i = 0; i < num_sensors_list; i++) { 402 if (sensors[i].wai == wai) 403 break; 404 } 405 if (i == num_sensors_list) 406 goto device_not_supported; 407 408 for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) { 409 if (strcmp(indio_dev->name, 410 &sensors[i].sensors_supported[n][0]) == 0) 411 break; 412 } 413 if (n == ARRAY_SIZE(sensors[i].sensors_supported)) { 414 dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n"); 415 goto sensor_name_mismatch; 416 } 417 418 sdata->sensor = (struct st_sensors *)&sensors[i]; 419 420 return i; 421 422 device_not_supported: 423 dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai); 424 sensor_name_mismatch: 425 err = -ENODEV; 426 read_wai_error: 427 return err; 428 } 429 EXPORT_SYMBOL(st_sensors_check_device_support); 430 431 ssize_t st_sensors_sysfs_get_sampling_frequency(struct device *dev, 432 struct device_attribute *attr, char *buf) 433 { 434 struct st_sensor_data *adata = iio_priv(dev_get_drvdata(dev)); 435 436 return sprintf(buf, "%d\n", adata->odr); 437 } 438 EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency); 439 440 ssize_t st_sensors_sysfs_set_sampling_frequency(struct device *dev, 441 struct device_attribute *attr, const char *buf, size_t size) 442 { 443 int err; 444 unsigned int odr; 445 struct iio_dev *indio_dev = dev_get_drvdata(dev); 446 447 err = kstrtoint(buf, 10, &odr); 448 if (err < 0) 449 goto conversion_error; 450 451 mutex_lock(&indio_dev->mlock); 452 err = st_sensors_set_odr(indio_dev, odr); 453 mutex_unlock(&indio_dev->mlock); 454 455 conversion_error: 456 return err < 0 ? err : size; 457 } 458 EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency); 459 460 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev, 461 struct device_attribute *attr, char *buf) 462 { 463 int i, len = 0; 464 struct iio_dev *indio_dev = dev_get_drvdata(dev); 465 struct st_sensor_data *sdata = iio_priv(indio_dev); 466 467 mutex_lock(&indio_dev->mlock); 468 for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) { 469 if (sdata->sensor->odr.odr_avl[i].hz == 0) 470 break; 471 472 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", 473 sdata->sensor->odr.odr_avl[i].hz); 474 } 475 mutex_unlock(&indio_dev->mlock); 476 buf[len - 1] = '\n'; 477 478 return len; 479 } 480 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail); 481 482 ssize_t st_sensors_sysfs_scale_avail(struct device *dev, 483 struct device_attribute *attr, char *buf) 484 { 485 int i, len = 0; 486 struct iio_dev *indio_dev = dev_get_drvdata(dev); 487 struct st_sensor_data *sdata = iio_priv(indio_dev); 488 489 mutex_lock(&indio_dev->mlock); 490 for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) { 491 if (sdata->sensor->fs.fs_avl[i].num == 0) 492 break; 493 494 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ", 495 sdata->sensor->fs.fs_avl[i].gain); 496 } 497 mutex_unlock(&indio_dev->mlock); 498 buf[len - 1] = '\n'; 499 500 return len; 501 } 502 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail); 503 504 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); 505 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core"); 506 MODULE_LICENSE("GPL v2"); 507