1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * STMicroelectronics magnetometers driver 4 * 5 * Copyright 2012-2013 STMicroelectronics Inc. 6 * 7 * Denis Ciocca <denis.ciocca@st.com> 8 */ 9 10 #include <linux/kernel.h> 11 #include <linux/module.h> 12 #include <linux/slab.h> 13 #include <linux/errno.h> 14 #include <linux/types.h> 15 #include <linux/interrupt.h> 16 #include <linux/i2c.h> 17 #include <linux/irq.h> 18 #include <linux/delay.h> 19 #include <linux/iio/iio.h> 20 #include <linux/iio/sysfs.h> 21 #include <linux/iio/buffer.h> 22 23 #include <linux/iio/common/st_sensors.h> 24 #include "st_magn.h" 25 26 #define ST_MAGN_NUMBER_DATA_CHANNELS 3 27 28 /* DEFAULT VALUE FOR SENSORS */ 29 #define ST_MAGN_DEFAULT_OUT_X_H_ADDR 0x03 30 #define ST_MAGN_DEFAULT_OUT_Y_H_ADDR 0x07 31 #define ST_MAGN_DEFAULT_OUT_Z_H_ADDR 0x05 32 33 /* FULLSCALE */ 34 #define ST_MAGN_FS_AVL_1300MG 1300 35 #define ST_MAGN_FS_AVL_1900MG 1900 36 #define ST_MAGN_FS_AVL_2000MG 2000 37 #define ST_MAGN_FS_AVL_2500MG 2500 38 #define ST_MAGN_FS_AVL_4000MG 4000 39 #define ST_MAGN_FS_AVL_4700MG 4700 40 #define ST_MAGN_FS_AVL_5600MG 5600 41 #define ST_MAGN_FS_AVL_8000MG 8000 42 #define ST_MAGN_FS_AVL_8100MG 8100 43 #define ST_MAGN_FS_AVL_12000MG 12000 44 #define ST_MAGN_FS_AVL_15000MG 15000 45 #define ST_MAGN_FS_AVL_16000MG 16000 46 47 /* Special L addresses for Sensor 2 */ 48 #define ST_MAGN_2_OUT_X_L_ADDR 0x28 49 #define ST_MAGN_2_OUT_Y_L_ADDR 0x2a 50 #define ST_MAGN_2_OUT_Z_L_ADDR 0x2c 51 52 /* Special L addresses for sensor 3 */ 53 #define ST_MAGN_3_OUT_X_L_ADDR 0x68 54 #define ST_MAGN_3_OUT_Y_L_ADDR 0x6a 55 #define ST_MAGN_3_OUT_Z_L_ADDR 0x6c 56 57 /* Special L addresses for sensor 4 */ 58 #define ST_MAGN_4_OUT_X_L_ADDR 0x08 59 #define ST_MAGN_4_OUT_Y_L_ADDR 0x0a 60 #define ST_MAGN_4_OUT_Z_L_ADDR 0x0c 61 62 static const struct iio_mount_matrix * 63 st_magn_get_mount_matrix(const struct iio_dev *indio_dev, 64 const struct iio_chan_spec *chan) 65 { 66 struct st_sensor_data *mdata = iio_priv(indio_dev); 67 68 return &mdata->mount_matrix; 69 } 70 71 static const struct iio_chan_spec_ext_info st_magn_mount_matrix_ext_info[] = { 72 IIO_MOUNT_MATRIX(IIO_SHARED_BY_ALL, st_magn_get_mount_matrix), 73 { } 74 }; 75 76 static const struct iio_chan_spec st_magn_16bit_channels[] = { 77 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 78 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 79 ST_SENSORS_SCAN_X, 1, IIO_MOD_X, 's', IIO_BE, 16, 16, 80 ST_MAGN_DEFAULT_OUT_X_H_ADDR, 81 st_magn_mount_matrix_ext_info), 82 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 83 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 84 ST_SENSORS_SCAN_Y, 1, IIO_MOD_Y, 's', IIO_BE, 16, 16, 85 ST_MAGN_DEFAULT_OUT_Y_H_ADDR, 86 st_magn_mount_matrix_ext_info), 87 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 88 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 89 ST_SENSORS_SCAN_Z, 1, IIO_MOD_Z, 's', IIO_BE, 16, 16, 90 ST_MAGN_DEFAULT_OUT_Z_H_ADDR, 91 st_magn_mount_matrix_ext_info), 92 IIO_CHAN_SOFT_TIMESTAMP(3) 93 }; 94 95 static const struct iio_chan_spec st_magn_2_16bit_channels[] = { 96 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 97 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 98 ST_SENSORS_SCAN_X, 1, IIO_MOD_X, 's', IIO_LE, 16, 16, 99 ST_MAGN_2_OUT_X_L_ADDR, 100 st_magn_mount_matrix_ext_info), 101 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 102 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 103 ST_SENSORS_SCAN_Y, 1, IIO_MOD_Y, 's', IIO_LE, 16, 16, 104 ST_MAGN_2_OUT_Y_L_ADDR, 105 st_magn_mount_matrix_ext_info), 106 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 107 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 108 ST_SENSORS_SCAN_Z, 1, IIO_MOD_Z, 's', IIO_LE, 16, 16, 109 ST_MAGN_2_OUT_Z_L_ADDR, 110 st_magn_mount_matrix_ext_info), 111 IIO_CHAN_SOFT_TIMESTAMP(3) 112 }; 113 114 static const struct iio_chan_spec st_magn_3_16bit_channels[] = { 115 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 116 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 117 ST_SENSORS_SCAN_X, 1, IIO_MOD_X, 's', IIO_LE, 16, 16, 118 ST_MAGN_3_OUT_X_L_ADDR, 119 st_magn_mount_matrix_ext_info), 120 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 121 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 122 ST_SENSORS_SCAN_Y, 1, IIO_MOD_Y, 's', IIO_LE, 16, 16, 123 ST_MAGN_3_OUT_Y_L_ADDR, 124 st_magn_mount_matrix_ext_info), 125 ST_SENSORS_LSM_CHANNELS_EXT(IIO_MAGN, 126 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 127 ST_SENSORS_SCAN_Z, 1, IIO_MOD_Z, 's', IIO_LE, 16, 16, 128 ST_MAGN_3_OUT_Z_L_ADDR, 129 st_magn_mount_matrix_ext_info), 130 IIO_CHAN_SOFT_TIMESTAMP(3) 131 }; 132 133 static const struct iio_chan_spec st_magn_4_16bit_channels[] = { 134 ST_SENSORS_LSM_CHANNELS(IIO_MAGN, 135 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 136 ST_SENSORS_SCAN_X, 1, IIO_MOD_X, 's', IIO_LE, 16, 16, 137 ST_MAGN_4_OUT_X_L_ADDR), 138 ST_SENSORS_LSM_CHANNELS(IIO_MAGN, 139 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 140 ST_SENSORS_SCAN_Y, 1, IIO_MOD_Y, 's', IIO_LE, 16, 16, 141 ST_MAGN_4_OUT_Y_L_ADDR), 142 ST_SENSORS_LSM_CHANNELS(IIO_MAGN, 143 BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE), 144 ST_SENSORS_SCAN_Z, 1, IIO_MOD_Z, 's', IIO_LE, 16, 16, 145 ST_MAGN_4_OUT_Z_L_ADDR), 146 IIO_CHAN_SOFT_TIMESTAMP(3) 147 }; 148 149 static const struct st_sensor_settings st_magn_sensors_settings[] = { 150 { 151 .wai = 0, /* This sensor has no valid WhoAmI report 0 */ 152 .wai_addr = ST_SENSORS_DEFAULT_WAI_ADDRESS, 153 .sensors_supported = { 154 [0] = LSM303DLH_MAGN_DEV_NAME, 155 }, 156 .ch = (struct iio_chan_spec *)st_magn_16bit_channels, 157 .odr = { 158 .addr = 0x00, 159 .mask = 0x1c, 160 .odr_avl = { 161 { .hz = 1, .value = 0x00 }, 162 { .hz = 2, .value = 0x01 }, 163 { .hz = 3, .value = 0x02 }, 164 { .hz = 8, .value = 0x03 }, 165 { .hz = 15, .value = 0x04 }, 166 { .hz = 30, .value = 0x05 }, 167 { .hz = 75, .value = 0x06 }, 168 /* 220 Hz, 0x07 reportedly exist */ 169 }, 170 }, 171 .pw = { 172 .addr = 0x02, 173 .mask = 0x03, 174 .value_on = 0x00, 175 .value_off = 0x03, 176 }, 177 .fs = { 178 .addr = 0x01, 179 .mask = 0xe0, 180 .fs_avl = { 181 [0] = { 182 .num = ST_MAGN_FS_AVL_1300MG, 183 .value = 0x01, 184 .gain = 1100, 185 .gain2 = 980, 186 }, 187 [1] = { 188 .num = ST_MAGN_FS_AVL_1900MG, 189 .value = 0x02, 190 .gain = 855, 191 .gain2 = 760, 192 }, 193 [2] = { 194 .num = ST_MAGN_FS_AVL_2500MG, 195 .value = 0x03, 196 .gain = 670, 197 .gain2 = 600, 198 }, 199 [3] = { 200 .num = ST_MAGN_FS_AVL_4000MG, 201 .value = 0x04, 202 .gain = 450, 203 .gain2 = 400, 204 }, 205 [4] = { 206 .num = ST_MAGN_FS_AVL_4700MG, 207 .value = 0x05, 208 .gain = 400, 209 .gain2 = 355, 210 }, 211 [5] = { 212 .num = ST_MAGN_FS_AVL_5600MG, 213 .value = 0x06, 214 .gain = 330, 215 .gain2 = 295, 216 }, 217 [6] = { 218 .num = ST_MAGN_FS_AVL_8100MG, 219 .value = 0x07, 220 .gain = 230, 221 .gain2 = 205, 222 }, 223 }, 224 }, 225 .multi_read_bit = false, 226 .bootime = 2, 227 }, 228 { 229 .wai = 0x3c, 230 .wai_addr = ST_SENSORS_DEFAULT_WAI_ADDRESS, 231 .sensors_supported = { 232 [0] = LSM303DLHC_MAGN_DEV_NAME, 233 [1] = LSM303DLM_MAGN_DEV_NAME, 234 }, 235 .ch = (struct iio_chan_spec *)st_magn_16bit_channels, 236 .odr = { 237 .addr = 0x00, 238 .mask = 0x1c, 239 .odr_avl = { 240 { .hz = 1, .value = 0x00 }, 241 { .hz = 2, .value = 0x01 }, 242 { .hz = 3, .value = 0x02 }, 243 { .hz = 8, .value = 0x03 }, 244 { .hz = 15, .value = 0x04 }, 245 { .hz = 30, .value = 0x05 }, 246 { .hz = 75, .value = 0x06 }, 247 { .hz = 220, .value = 0x07 }, 248 }, 249 }, 250 .pw = { 251 .addr = 0x02, 252 .mask = 0x03, 253 .value_on = 0x00, 254 .value_off = 0x03, 255 }, 256 .fs = { 257 .addr = 0x01, 258 .mask = 0xe0, 259 .fs_avl = { 260 [0] = { 261 .num = ST_MAGN_FS_AVL_1300MG, 262 .value = 0x01, 263 .gain = 909, 264 .gain2 = 1020, 265 }, 266 [1] = { 267 .num = ST_MAGN_FS_AVL_1900MG, 268 .value = 0x02, 269 .gain = 1169, 270 .gain2 = 1315, 271 }, 272 [2] = { 273 .num = ST_MAGN_FS_AVL_2500MG, 274 .value = 0x03, 275 .gain = 1492, 276 .gain2 = 1666, 277 }, 278 [3] = { 279 .num = ST_MAGN_FS_AVL_4000MG, 280 .value = 0x04, 281 .gain = 2222, 282 .gain2 = 2500, 283 }, 284 [4] = { 285 .num = ST_MAGN_FS_AVL_4700MG, 286 .value = 0x05, 287 .gain = 2500, 288 .gain2 = 2816, 289 }, 290 [5] = { 291 .num = ST_MAGN_FS_AVL_5600MG, 292 .value = 0x06, 293 .gain = 3030, 294 .gain2 = 3389, 295 }, 296 [6] = { 297 .num = ST_MAGN_FS_AVL_8100MG, 298 .value = 0x07, 299 .gain = 4347, 300 .gain2 = 4878, 301 }, 302 }, 303 }, 304 .multi_read_bit = false, 305 .bootime = 2, 306 }, 307 { 308 .wai = 0x3d, 309 .wai_addr = ST_SENSORS_DEFAULT_WAI_ADDRESS, 310 .sensors_supported = { 311 [0] = LIS3MDL_MAGN_DEV_NAME, 312 [1] = LSM9DS1_MAGN_DEV_NAME, 313 }, 314 .ch = (struct iio_chan_spec *)st_magn_2_16bit_channels, 315 .odr = { 316 .addr = 0x20, 317 .mask = 0x1c, 318 .odr_avl = { 319 { .hz = 1, .value = 0x00 }, 320 { .hz = 2, .value = 0x01 }, 321 { .hz = 3, .value = 0x02 }, 322 { .hz = 5, .value = 0x03 }, 323 { .hz = 10, .value = 0x04 }, 324 { .hz = 20, .value = 0x05 }, 325 { .hz = 40, .value = 0x06 }, 326 { .hz = 80, .value = 0x07 }, 327 }, 328 }, 329 .pw = { 330 .addr = 0x22, 331 .mask = 0x03, 332 .value_on = 0x00, 333 .value_off = 0x03, 334 }, 335 .fs = { 336 .addr = 0x21, 337 .mask = 0x60, 338 .fs_avl = { 339 [0] = { 340 .num = ST_MAGN_FS_AVL_4000MG, 341 .value = 0x00, 342 .gain = 146, 343 }, 344 [1] = { 345 .num = ST_MAGN_FS_AVL_8000MG, 346 .value = 0x01, 347 .gain = 292, 348 }, 349 [2] = { 350 .num = ST_MAGN_FS_AVL_12000MG, 351 .value = 0x02, 352 .gain = 438, 353 }, 354 [3] = { 355 .num = ST_MAGN_FS_AVL_16000MG, 356 .value = 0x03, 357 .gain = 584, 358 }, 359 }, 360 }, 361 .bdu = { 362 .addr = 0x24, 363 .mask = 0x40, 364 }, 365 .drdy_irq = { 366 /* drdy line is routed drdy pin */ 367 .stat_drdy = { 368 .addr = ST_SENSORS_DEFAULT_STAT_ADDR, 369 .mask = 0x07, 370 }, 371 }, 372 .sim = { 373 .addr = 0x22, 374 .value = BIT(2), 375 }, 376 .multi_read_bit = true, 377 .bootime = 2, 378 }, 379 { 380 .wai = 0x40, 381 .wai_addr = 0x4f, 382 .sensors_supported = { 383 [0] = LSM303AGR_MAGN_DEV_NAME, 384 [1] = LIS2MDL_MAGN_DEV_NAME, 385 [2] = IIS2MDC_MAGN_DEV_NAME, 386 }, 387 .ch = (struct iio_chan_spec *)st_magn_3_16bit_channels, 388 .odr = { 389 .addr = 0x60, 390 .mask = 0x0c, 391 .odr_avl = { 392 { .hz = 10, .value = 0x00 }, 393 { .hz = 20, .value = 0x01 }, 394 { .hz = 50, .value = 0x02 }, 395 { .hz = 100, .value = 0x03 }, 396 }, 397 }, 398 .pw = { 399 .addr = 0x60, 400 .mask = 0x03, 401 .value_on = 0x00, 402 .value_off = 0x03, 403 }, 404 .fs = { 405 .fs_avl = { 406 [0] = { 407 .num = ST_MAGN_FS_AVL_15000MG, 408 .gain = 1500, 409 }, 410 }, 411 }, 412 .bdu = { 413 .addr = 0x62, 414 .mask = 0x10, 415 }, 416 .drdy_irq = { 417 .int1 = { 418 .addr = 0x62, 419 .mask = 0x01, 420 }, 421 .stat_drdy = { 422 .addr = 0x67, 423 .mask = 0x07, 424 }, 425 }, 426 .multi_read_bit = false, 427 .bootime = 2, 428 }, 429 { 430 .wai = 0x49, 431 .wai_addr = ST_SENSORS_DEFAULT_WAI_ADDRESS, 432 .sensors_supported = { 433 [0] = LSM9DS0_IMU_DEV_NAME, 434 }, 435 .ch = (struct iio_chan_spec *)st_magn_4_16bit_channels, 436 .odr = { 437 .addr = 0x24, 438 .mask = GENMASK(4, 2), 439 .odr_avl = { 440 { 3, 0x00, }, 441 { 6, 0x01, }, 442 { 12, 0x02, }, 443 { 25, 0x03, }, 444 { 50, 0x04, }, 445 { 100, 0x05, }, 446 }, 447 }, 448 .pw = { 449 .addr = 0x26, 450 .mask = GENMASK(1, 0), 451 .value_on = 0x00, 452 .value_off = 0x03, 453 }, 454 .fs = { 455 .addr = 0x25, 456 .mask = GENMASK(6, 5), 457 .fs_avl = { 458 [0] = { 459 .num = ST_MAGN_FS_AVL_2000MG, 460 .value = 0x00, 461 .gain = 73, 462 }, 463 [1] = { 464 .num = ST_MAGN_FS_AVL_4000MG, 465 .value = 0x01, 466 .gain = 146, 467 }, 468 [2] = { 469 .num = ST_MAGN_FS_AVL_8000MG, 470 .value = 0x02, 471 .gain = 292, 472 }, 473 [3] = { 474 .num = ST_MAGN_FS_AVL_12000MG, 475 .value = 0x03, 476 .gain = 438, 477 }, 478 }, 479 }, 480 .bdu = { 481 .addr = 0x20, 482 .mask = BIT(3), 483 }, 484 .drdy_irq = { 485 .int1 = { 486 .addr = 0x22, 487 .mask = BIT(1), 488 }, 489 .int2 = { 490 .addr = 0x23, 491 .mask = BIT(2), 492 }, 493 .stat_drdy = { 494 .addr = 0x07, 495 .mask = GENMASK(2, 0), 496 }, 497 }, 498 .sim = { 499 .addr = 0x21, 500 .value = BIT(0), 501 }, 502 .multi_read_bit = true, 503 .bootime = 2, 504 }, 505 }; 506 507 /* Default magn DRDY is available on INT2 pin */ 508 static const struct st_sensors_platform_data default_magn_pdata = { 509 .drdy_int_pin = 2, 510 }; 511 512 static int st_magn_read_raw(struct iio_dev *indio_dev, 513 struct iio_chan_spec const *ch, int *val, 514 int *val2, long mask) 515 { 516 int err; 517 struct st_sensor_data *mdata = iio_priv(indio_dev); 518 519 switch (mask) { 520 case IIO_CHAN_INFO_RAW: 521 err = st_sensors_read_info_raw(indio_dev, ch, val); 522 if (err < 0) 523 goto read_error; 524 525 return IIO_VAL_INT; 526 case IIO_CHAN_INFO_SCALE: 527 *val = 0; 528 if ((ch->scan_index == ST_SENSORS_SCAN_Z) && 529 (mdata->current_fullscale->gain2 != 0)) 530 *val2 = mdata->current_fullscale->gain2; 531 else 532 *val2 = mdata->current_fullscale->gain; 533 return IIO_VAL_INT_PLUS_MICRO; 534 case IIO_CHAN_INFO_SAMP_FREQ: 535 *val = mdata->odr; 536 return IIO_VAL_INT; 537 default: 538 return -EINVAL; 539 } 540 541 read_error: 542 return err; 543 } 544 545 static int st_magn_write_raw(struct iio_dev *indio_dev, 546 struct iio_chan_spec const *chan, int val, int val2, long mask) 547 { 548 int err; 549 550 switch (mask) { 551 case IIO_CHAN_INFO_SCALE: 552 err = st_sensors_set_fullscale_by_gain(indio_dev, val2); 553 break; 554 case IIO_CHAN_INFO_SAMP_FREQ: 555 if (val2) 556 return -EINVAL; 557 mutex_lock(&indio_dev->mlock); 558 err = st_sensors_set_odr(indio_dev, val); 559 mutex_unlock(&indio_dev->mlock); 560 return err; 561 default: 562 err = -EINVAL; 563 } 564 565 return err; 566 } 567 568 static ST_SENSORS_DEV_ATTR_SAMP_FREQ_AVAIL(); 569 static ST_SENSORS_DEV_ATTR_SCALE_AVAIL(in_magn_scale_available); 570 571 static struct attribute *st_magn_attributes[] = { 572 &iio_dev_attr_sampling_frequency_available.dev_attr.attr, 573 &iio_dev_attr_in_magn_scale_available.dev_attr.attr, 574 NULL, 575 }; 576 577 static const struct attribute_group st_magn_attribute_group = { 578 .attrs = st_magn_attributes, 579 }; 580 581 static const struct iio_info magn_info = { 582 .attrs = &st_magn_attribute_group, 583 .read_raw = &st_magn_read_raw, 584 .write_raw = &st_magn_write_raw, 585 .debugfs_reg_access = &st_sensors_debugfs_reg_access, 586 }; 587 588 #ifdef CONFIG_IIO_TRIGGER 589 static const struct iio_trigger_ops st_magn_trigger_ops = { 590 .set_trigger_state = ST_MAGN_TRIGGER_SET_STATE, 591 .validate_device = st_sensors_validate_device, 592 }; 593 #define ST_MAGN_TRIGGER_OPS (&st_magn_trigger_ops) 594 #else 595 #define ST_MAGN_TRIGGER_OPS NULL 596 #endif 597 598 /* 599 * st_magn_get_settings() - get sensor settings from device name 600 * @name: device name buffer reference. 601 * 602 * Return: valid reference on success, NULL otherwise. 603 */ 604 const struct st_sensor_settings *st_magn_get_settings(const char *name) 605 { 606 int index = st_sensors_get_settings_index(name, 607 st_magn_sensors_settings, 608 ARRAY_SIZE(st_magn_sensors_settings)); 609 if (index < 0) 610 return NULL; 611 612 return &st_magn_sensors_settings[index]; 613 } 614 EXPORT_SYMBOL(st_magn_get_settings); 615 616 int st_magn_common_probe(struct iio_dev *indio_dev) 617 { 618 struct st_sensor_data *mdata = iio_priv(indio_dev); 619 struct st_sensors_platform_data *pdata = dev_get_platdata(mdata->dev); 620 int err; 621 622 indio_dev->modes = INDIO_DIRECT_MODE; 623 indio_dev->info = &magn_info; 624 625 err = st_sensors_verify_id(indio_dev); 626 if (err < 0) 627 return err; 628 629 mdata->num_data_channels = ST_MAGN_NUMBER_DATA_CHANNELS; 630 indio_dev->channels = mdata->sensor_settings->ch; 631 indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS; 632 633 err = iio_read_mount_matrix(mdata->dev, &mdata->mount_matrix); 634 if (err) 635 return err; 636 637 mdata->current_fullscale = &mdata->sensor_settings->fs.fs_avl[0]; 638 mdata->odr = mdata->sensor_settings->odr.odr_avl[0].hz; 639 640 if (!pdata) 641 pdata = (struct st_sensors_platform_data *)&default_magn_pdata; 642 643 err = st_sensors_init_sensor(indio_dev, pdata); 644 if (err < 0) 645 return err; 646 647 err = st_magn_allocate_ring(indio_dev); 648 if (err < 0) 649 return err; 650 651 if (mdata->irq > 0) { 652 err = st_sensors_allocate_trigger(indio_dev, 653 ST_MAGN_TRIGGER_OPS); 654 if (err < 0) 655 goto st_magn_probe_trigger_error; 656 } 657 658 err = iio_device_register(indio_dev); 659 if (err) 660 goto st_magn_device_register_error; 661 662 dev_info(&indio_dev->dev, "registered magnetometer %s\n", 663 indio_dev->name); 664 665 return 0; 666 667 st_magn_device_register_error: 668 if (mdata->irq > 0) 669 st_sensors_deallocate_trigger(indio_dev); 670 st_magn_probe_trigger_error: 671 st_magn_deallocate_ring(indio_dev); 672 return err; 673 } 674 EXPORT_SYMBOL(st_magn_common_probe); 675 676 void st_magn_common_remove(struct iio_dev *indio_dev) 677 { 678 struct st_sensor_data *mdata = iio_priv(indio_dev); 679 680 iio_device_unregister(indio_dev); 681 if (mdata->irq > 0) 682 st_sensors_deallocate_trigger(indio_dev); 683 684 st_magn_deallocate_ring(indio_dev); 685 } 686 EXPORT_SYMBOL(st_magn_common_remove); 687 688 MODULE_AUTHOR("Denis Ciocca <denis.ciocca@st.com>"); 689 MODULE_DESCRIPTION("STMicroelectronics magnetometers driver"); 690 MODULE_LICENSE("GPL v2"); 691