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