drivers/iio/common/st_sensors/st_sensors_core.c

   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 int st_sensors_init_sensor(struct iio_dev *indio_dev,
 202                                         struct st_sensors_platform_data *pdata)
 203 {
 204         int err;
 205         struct st_sensor_data *sdata = iio_priv(indio_dev);
 206
 207         mutex_init(&sdata->tb.buf_lock);
 208
 209         switch (pdata->drdy_int_pin) {
 210         case 1:
 211                 if (sdata->sensor->drdy_irq.mask_int1 == 0) {
 212                         dev_err(&indio_dev->dev,
 213                                         "DRDY on INT1 not available.\n");
 214                         err = -EINVAL;
 215                         goto init_error;
 216                 }
 217                 sdata->drdy_int_pin = 1;
 218                 break;
 219         case 2:
 220                 if (sdata->sensor->drdy_irq.mask_int2 == 0) {
 221                         dev_err(&indio_dev->dev,
 222                                         "DRDY on INT2 not available.\n");
 223                         err = -EINVAL;
 224                         goto init_error;
 225                 }
 226                 sdata->drdy_int_pin = 2;
 227                 break;
 228         default:
 229                 dev_err(&indio_dev->dev, "DRDY on pdata not valid.\n");
 230                 err = -EINVAL;
 231                 goto init_error;
 232         }
 233
 234         err = st_sensors_set_enable(indio_dev, false);
 235         if (err < 0)
 236                 goto init_error;
 237
 238         if (sdata->current_fullscale) {
 239                 err = st_sensors_set_fullscale(indio_dev,
 240                                                sdata->current_fullscale->num);
 241                 if (err < 0)
 242                         goto init_error;
 243         } else
 244                 dev_info(&indio_dev->dev, "Full-scale not possible\n");
 245
 246         err = st_sensors_set_odr(indio_dev, sdata->odr);
 247         if (err < 0)
 248                 goto init_error;
 249
 250         /* set BDU */
 251         err = st_sensors_write_data_with_mask(indio_dev,
 252                         sdata->sensor->bdu.addr, sdata->sensor->bdu.mask, true);
 253         if (err < 0)
 254                 goto init_error;
 255
 256         err = st_sensors_set_axis_enable(indio_dev, ST_SENSORS_ENABLE_ALL_AXIS);
 257
 258 init_error:
 259         return err;
 260 }
 261 EXPORT_SYMBOL(st_sensors_init_sensor);
 262
 263 int st_sensors_set_dataready_irq(struct iio_dev *indio_dev, bool enable)
 264 {
 265         int err;
 266         u8 drdy_mask;
 267         struct st_sensor_data *sdata = iio_priv(indio_dev);
 268
 269         /* Enable/Disable the interrupt generator 1. */
 270         if (sdata->sensor->drdy_irq.ig1.en_addr > 0) {
 271                 err = st_sensors_write_data_with_mask(indio_dev,
 272                         sdata->sensor->drdy_irq.ig1.en_addr,
 273                         sdata->sensor->drdy_irq.ig1.en_mask, (int)enable);
 274                 if (err < 0)
 275                         goto st_accel_set_dataready_irq_error;
 276         }
 277
 278         if (sdata->drdy_int_pin == 1)
 279                 drdy_mask = sdata->sensor->drdy_irq.mask_int1;
 280         else
 281                 drdy_mask = sdata->sensor->drdy_irq.mask_int2;
 282
 283         /* Enable/Disable the interrupt generator for data ready. */
 284         err = st_sensors_write_data_with_mask(indio_dev,
 285                         sdata->sensor->drdy_irq.addr, drdy_mask, (int)enable);
 286
 287 st_accel_set_dataready_irq_error:
 288         return err;
 289 }
 290 EXPORT_SYMBOL(st_sensors_set_dataready_irq);
 291
 292 int st_sensors_set_fullscale_by_gain(struct iio_dev *indio_dev, int scale)
 293 {
 294         int err = -EINVAL, i;
 295         struct st_sensor_data *sdata = iio_priv(indio_dev);
 296
 297         for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
 298                 if ((sdata->sensor->fs.fs_avl[i].gain == scale) &&
 299                                 (sdata->sensor->fs.fs_avl[i].gain != 0)) {
 300                         err = 0;
 301                         break;
 302                 }
 303         }
 304         if (err < 0)
 305                 goto st_sensors_match_scale_error;
 306
 307         err = st_sensors_set_fullscale(indio_dev,
 308                                         sdata->sensor->fs.fs_avl[i].num);
 309
 310 st_sensors_match_scale_error:
 311         return err;
 312 }
 313 EXPORT_SYMBOL(st_sensors_set_fullscale_by_gain);
 314
 315 static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
 316                                 struct iio_chan_spec const *ch, int *data)
 317 {
 318         int err;
 319         u8 *outdata;
 320         struct st_sensor_data *sdata = iio_priv(indio_dev);
 321         unsigned int byte_for_channel = ch->scan_type.storagebits >> 3;
 322
 323         outdata = kmalloc(byte_for_channel, GFP_KERNEL);
 324         if (!outdata) {
 325                 err = -EINVAL;
 326                 goto st_sensors_read_axis_data_error;
 327         }
 328
 329         err = sdata->tf->read_multiple_byte(&sdata->tb, sdata->dev,
 330                                 ch->address, byte_for_channel,
 331                                 outdata, sdata->multiread_bit);
 332         if (err < 0)
 333                 goto st_sensors_free_memory;
 334
 335         if (byte_for_channel == 2)
 336                 *data = (s16)get_unaligned_le16(outdata);
 337         else if (byte_for_channel == 3)
 338                 *data = (s32)st_sensors_get_unaligned_le24(outdata);
 339
 340 st_sensors_free_memory:
 341         kfree(outdata);
 342 st_sensors_read_axis_data_error:
 343         return err;
 344 }
 345
 346 int st_sensors_read_info_raw(struct iio_dev *indio_dev,
 347                                 struct iio_chan_spec const *ch, int *val)
 348 {
 349         int err;
 350         struct st_sensor_data *sdata = iio_priv(indio_dev);
 351
 352         mutex_lock(&indio_dev->mlock);
 353         if (indio_dev->currentmode == INDIO_BUFFER_TRIGGERED) {
 354                 err = -EBUSY;
 355                 goto read_error;
 356         } else {
 357                 err = st_sensors_set_enable(indio_dev, true);
 358                 if (err < 0)
 359                         goto read_error;
 360
 361                 msleep((sdata->sensor->bootime * 1000) / sdata->odr);
 362                 err = st_sensors_read_axis_data(indio_dev, ch, val);
 363                 if (err < 0)
 364                         goto read_error;
 365
 366                 *val = *val >> ch->scan_type.shift;
 367
 368                 err = st_sensors_set_enable(indio_dev, false);
 369         }
 370         mutex_unlock(&indio_dev->mlock);
 371
 372         return err;
 373
 374 read_error:
 375         mutex_unlock(&indio_dev->mlock);
 376         return err;
 377 }
 378 EXPORT_SYMBOL(st_sensors_read_info_raw);
 379
 380 int st_sensors_check_device_support(struct iio_dev *indio_dev,
 381                         int num_sensors_list, const struct st_sensors *sensors)
 382 {
 383         u8 wai;
 384         int i, n, err;
 385         struct st_sensor_data *sdata = iio_priv(indio_dev);
 386
 387         err = sdata->tf->read_byte(&sdata->tb, sdata->dev,
 388                                         ST_SENSORS_DEFAULT_WAI_ADDRESS, &wai);
 389         if (err < 0) {
 390                 dev_err(&indio_dev->dev, "failed to read Who-Am-I register.\n");
 391                 goto read_wai_error;
 392         }
 393
 394         for (i = 0; i < num_sensors_list; i++) {
 395                 if (sensors[i].wai == wai)
 396                         break;
 397         }
 398         if (i == num_sensors_list)
 399                 goto device_not_supported;
 400
 401         for (n = 0; n < ARRAY_SIZE(sensors[i].sensors_supported); n++) {
 402                 if (strcmp(indio_dev->name,
 403                                 &sensors[i].sensors_supported[n][0]) == 0)
 404                         break;
 405         }
 406         if (n == ARRAY_SIZE(sensors[i].sensors_supported)) {
 407                 dev_err(&indio_dev->dev, "device name and WhoAmI mismatch.\n");
 408                 goto sensor_name_mismatch;
 409         }
 410
 411         sdata->sensor = (struct st_sensors *)&sensors[i];
 412
 413         return i;
 414
 415 device_not_supported:
 416         dev_err(&indio_dev->dev, "device not supported: WhoAmI (0x%x).\n", wai);
 417 sensor_name_mismatch:
 418         err = -ENODEV;
 419 read_wai_error:
 420         return err;
 421 }
 422 EXPORT_SYMBOL(st_sensors_check_device_support);
 423
 424 ssize_t st_sensors_sysfs_get_sampling_frequency(struct device *dev,
 425                                 struct device_attribute *attr, char *buf)
 426 {
 427         struct st_sensor_data *adata = iio_priv(dev_get_drvdata(dev));
 428
 429         return sprintf(buf, "%d\n", adata->odr);
 430 }
 431 EXPORT_SYMBOL(st_sensors_sysfs_get_sampling_frequency);
 432
 433 ssize_t st_sensors_sysfs_set_sampling_frequency(struct device *dev,
 434                 struct device_attribute *attr, const char *buf, size_t size)
 435 {
 436         int err;
 437         unsigned int odr;
 438         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 439
 440         err = kstrtoint(buf, 10, &odr);
 441         if (err < 0)
 442                 goto conversion_error;
 443
 444         mutex_lock(&indio_dev->mlock);
 445         err = st_sensors_set_odr(indio_dev, odr);
 446         mutex_unlock(&indio_dev->mlock);
 447
 448 conversion_error:
 449         return err < 0 ? err : size;
 450 }
 451 EXPORT_SYMBOL(st_sensors_sysfs_set_sampling_frequency);
 452
 453 ssize_t st_sensors_sysfs_sampling_frequency_avail(struct device *dev,
 454                                 struct device_attribute *attr, char *buf)
 455 {
 456         int i, len = 0;
 457         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 458         struct st_sensor_data *sdata = iio_priv(indio_dev);
 459
 460         mutex_lock(&indio_dev->mlock);
 461         for (i = 0; i < ST_SENSORS_ODR_LIST_MAX; i++) {
 462                 if (sdata->sensor->odr.odr_avl[i].hz == 0)
 463                         break;
 464
 465                 len += scnprintf(buf + len, PAGE_SIZE - len, "%d ",
 466                                         sdata->sensor->odr.odr_avl[i].hz);
 467         }
 468         mutex_unlock(&indio_dev->mlock);
 469         buf[len - 1] = '\n';
 470
 471         return len;
 472 }
 473 EXPORT_SYMBOL(st_sensors_sysfs_sampling_frequency_avail);
 474
 475 ssize_t st_sensors_sysfs_scale_avail(struct device *dev,
 476                                 struct device_attribute *attr, char *buf)
 477 {
 478         int i, len = 0;
 479         struct iio_dev *indio_dev = dev_get_drvdata(dev);
 480         struct st_sensor_data *sdata = iio_priv(indio_dev);
 481
 482         mutex_lock(&indio_dev->mlock);
 483         for (i = 0; i < ST_SENSORS_FULLSCALE_AVL_MAX; i++) {
 484                 if (sdata->sensor->fs.fs_avl[i].num == 0)
 485                         break;
 486
 487                 len += scnprintf(buf + len, PAGE_SIZE - len, "0.%06u ",
 488                                         sdata->sensor->fs.fs_avl[i].gain);
 489         }
 490         mutex_unlock(&indio_dev->mlock);
 491         buf[len - 1] = '\n';
 492
 493         return len;
 494 }
 495 EXPORT_SYMBOL(st_sensors_sysfs_scale_avail);
 496
 497 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>");
 498 MODULE_DESCRIPTION("STMicroelectronics ST-sensors core");
 499 MODULE_LICENSE("GPL v2");