drivers/iio/imu/st_lsm9ds0/st_lsm9ds0_core.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * STMicroelectronics LSM9DS0 IMU driver
   4  *
   5  * Copyright (C) 2021, Intel Corporation
   6  *
   7  * Author: Andy Shevchenko <andriy.shevchenko@linux.intel.com>
   8  */
   9
  10 #include <linux/device.h>
  11 #include <linux/err.h>
  12 #include <linux/module.h>
  13 #include <linux/regmap.h>
  14 #include <linux/regulator/consumer.h>
  15
  16 #include <linux/iio/common/st_sensors.h>
  17 #include <linux/iio/iio.h>
  18
  19 #include "st_lsm9ds0.h"
  20
  21 static int st_lsm9ds0_power_enable(struct device *dev, struct st_lsm9ds0 *lsm9ds0)
  22 {
  23         int ret;
  24
  25         /* Regulators not mandatory, but if requested we should enable them. */
  26         lsm9ds0->vdd = devm_regulator_get(dev, "vdd");
  27         if (IS_ERR(lsm9ds0->vdd)) {
  28                 dev_err(dev, "unable to get Vdd supply\n");
  29                 return PTR_ERR(lsm9ds0->vdd);
  30         }
  31         ret = regulator_enable(lsm9ds0->vdd);
  32         if (ret) {
  33                 dev_warn(dev, "Failed to enable specified Vdd supply\n");
  34                 return ret;
  35         }
  36
  37         lsm9ds0->vdd_io = devm_regulator_get(dev, "vddio");
  38         if (IS_ERR(lsm9ds0->vdd_io)) {
  39                 dev_err(dev, "unable to get Vdd_IO supply\n");
  40                 regulator_disable(lsm9ds0->vdd);
  41                 return PTR_ERR(lsm9ds0->vdd_io);
  42         }
  43         ret = regulator_enable(lsm9ds0->vdd_io);
  44         if (ret) {
  45                 dev_warn(dev, "Failed to enable specified Vdd_IO supply\n");
  46                 regulator_disable(lsm9ds0->vdd);
  47                 return ret;
  48         }
  49
  50         return 0;
  51 }
  52
  53 static void st_lsm9ds0_power_disable(void *data)
  54 {
  55         struct st_lsm9ds0 *lsm9ds0 = data;
  56
  57         regulator_disable(lsm9ds0->vdd_io);
  58         regulator_disable(lsm9ds0->vdd);
  59 }
  60
  61 static int devm_st_lsm9ds0_power_enable(struct st_lsm9ds0 *lsm9ds0)
  62 {
  63         struct device *dev = lsm9ds0->dev;
  64         int ret;
  65
  66         ret = st_lsm9ds0_power_enable(dev, lsm9ds0);
  67         if (ret)
  68                 return ret;
  69
  70         return devm_add_action_or_reset(dev, st_lsm9ds0_power_disable, lsm9ds0);
  71 }
  72
  73 static int st_lsm9ds0_probe_accel(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap)
  74 {
  75         const struct st_sensor_settings *settings;
  76         struct device *dev = lsm9ds0->dev;
  77         struct st_sensor_data *data;
  78
  79         settings = st_accel_get_settings(lsm9ds0->name);
  80         if (!settings) {
  81                 dev_err(dev, "device name %s not recognized.\n", lsm9ds0->name);
  82                 return -ENODEV;
  83         }
  84
  85         lsm9ds0->accel = devm_iio_device_alloc(dev, sizeof(*data));
  86         if (!lsm9ds0->accel)
  87                 return -ENOMEM;
  88
  89         lsm9ds0->accel->name = lsm9ds0->name;
  90
  91         data = iio_priv(lsm9ds0->accel);
  92         data->sensor_settings = (struct st_sensor_settings *)settings;
  93         data->dev = dev;
  94         data->irq = lsm9ds0->irq;
  95         data->regmap = regmap;
  96         data->vdd = lsm9ds0->vdd;
  97         data->vdd_io = lsm9ds0->vdd_io;
  98
  99         return st_accel_common_probe(lsm9ds0->accel);
 100 }
 101
 102 static int st_lsm9ds0_probe_magn(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap)
 103 {
 104         const struct st_sensor_settings *settings;
 105         struct device *dev = lsm9ds0->dev;
 106         struct st_sensor_data *data;
 107
 108         settings = st_magn_get_settings(lsm9ds0->name);
 109         if (!settings) {
 110                 dev_err(dev, "device name %s not recognized.\n", lsm9ds0->name);
 111                 return -ENODEV;
 112         }
 113
 114         lsm9ds0->magn = devm_iio_device_alloc(dev, sizeof(*data));
 115         if (!lsm9ds0->magn)
 116                 return -ENOMEM;
 117
 118         lsm9ds0->magn->name = lsm9ds0->name;
 119
 120         data = iio_priv(lsm9ds0->magn);
 121         data->sensor_settings = (struct st_sensor_settings *)settings;
 122         data->dev = dev;
 123         data->irq = lsm9ds0->irq;
 124         data->regmap = regmap;
 125         data->vdd = lsm9ds0->vdd;
 126         data->vdd_io = lsm9ds0->vdd_io;
 127
 128         return st_magn_common_probe(lsm9ds0->magn);
 129 }
 130
 131 int st_lsm9ds0_probe(struct st_lsm9ds0 *lsm9ds0, struct regmap *regmap)
 132 {
 133         int ret;
 134
 135         ret = devm_st_lsm9ds0_power_enable(lsm9ds0);
 136         if (ret)
 137                 return ret;
 138
 139         /* Setup accelerometer device */
 140         ret = st_lsm9ds0_probe_accel(lsm9ds0, regmap);
 141         if (ret)
 142                 return ret;
 143
 144         /* Setup magnetometer device */
 145         ret = st_lsm9ds0_probe_magn(lsm9ds0, regmap);
 146         if (ret)
 147                 st_accel_common_remove(lsm9ds0->accel);
 148
 149         return ret;
 150 }
 151 EXPORT_SYMBOL_GPL(st_lsm9ds0_probe);
 152
 153 int st_lsm9ds0_remove(struct st_lsm9ds0 *lsm9ds0)
 154 {
 155         st_magn_common_remove(lsm9ds0->magn);
 156         st_accel_common_remove(lsm9ds0->accel);
 157
 158         return 0;
 159 }
 160 EXPORT_SYMBOL_GPL(st_lsm9ds0_remove);
 161
 162 MODULE_AUTHOR("Andy Shevchenko <andriy.shevchenko@linux.intel.com>");
 163 MODULE_DESCRIPTION("STMicroelectronics LSM9DS0 IMU core driver");
 164 MODULE_LICENSE("GPL v2");