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