summaryrefslogtreecommitdiff
path: root/driver/sensorhub_lsm6dsm.c
blob: 711294fb9806537f903aded65b64e7d885081156 (plain)
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
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
/* Copyright 2018 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

/*
 * Sensor Hub Driver for LSM6DSM acce/gyro module to enable connecting
 * external sensors like magnetometer
 */

#include "console.h"
#include "driver/accelgyro_lsm6dsm.h"
#include "driver/sensorhub_lsm6dsm.h"
#include "driver/stm_mems_common.h"

#define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)

static int set_reg_bit_field(const struct motion_sensor_t *s,
					uint8_t reg, uint8_t bit_field)
{
	int tmp;
	int ret;

	ret = st_raw_read8__7bf(s->port, s->i2c_spi_addr__7bf, reg, &tmp);
	if (ret != EC_SUCCESS)
		return ret;

	tmp |= bit_field;
	return st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf, reg, tmp);
}

static int clear_reg_bit_field(const struct motion_sensor_t *s,
					uint8_t reg, uint8_t bit_field)
{
	int tmp;
	int ret;

	ret = st_raw_read8__7bf(s->port, s->i2c_spi_addr__7bf, reg, &tmp);
	if (ret != EC_SUCCESS)
		return ret;

	tmp &= ~(bit_field);
	return st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf, reg, tmp);
}

static inline int enable_sensorhub_func(const struct motion_sensor_t *s)
{
	return set_reg_bit_field(s, LSM6DSM_CTRL10_ADDR,
						LSM6DSM_EMBED_FUNC_EN);
}

static inline int disable_sensorhub_func(const struct motion_sensor_t *s)
{
	return clear_reg_bit_field(s, LSM6DSM_CTRL10_ADDR,
						LSM6DSM_EMBED_FUNC_EN);
}

/*
 * Sensor hub includes embedded register banks associated with external
 * sensors. 4 external sensor slaves can be attached to the sensor hub
 * and hence 4 such register banks exist. The access to them are disabled
 * by default. Below 2 helper functions help enable/disable access to those
 * register banks.
 */
static inline int enable_ereg_bank_acc(const struct motion_sensor_t *s)
{
	return set_reg_bit_field(s, LSM6DSM_FUNC_CFG_ACC_ADDR,
							LSM6DSM_FUNC_CFG_EN);
}

static inline int disable_ereg_bank_acc(const struct motion_sensor_t *s)
{
	return clear_reg_bit_field(s, LSM6DSM_FUNC_CFG_ACC_ADDR,
							LSM6DSM_FUNC_CFG_EN);
}

static inline int enable_aux_i2c_master(const struct motion_sensor_t *s)
{
	return set_reg_bit_field(s, LSM6DSM_MASTER_CFG_ADDR,
							LSM6DSM_I2C_MASTER_ON);
}

static inline int disable_aux_i2c_master(const struct motion_sensor_t *s)
{
	return clear_reg_bit_field(s, LSM6DSM_MASTER_CFG_ADDR,
							LSM6DSM_I2C_MASTER_ON);
}

static inline int restore_master_cfg(const struct motion_sensor_t *s,
								int cache)
{
	return st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			     LSM6DSM_MASTER_CFG_ADDR, cache);
}

static int enable_i2c_pass_through(const struct motion_sensor_t *s,
							int *cache)
{
	int ret;

	ret = st_raw_read8__7bf(s->port, s->i2c_spi_addr__7bf,
			   LSM6DSM_MASTER_CFG_ADDR, cache);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x MCR error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	/*
	 * Fake set sensor hub to external trigger event and wait for 10ms.
	 * Wait is for any pending bus activity(probably read) to settle down
	 * so that there is no bus contention.
	 */
	ret = st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			    LSM6DSM_MASTER_CFG_ADDR,
			    *cache | LSM6DSM_EXT_TRIGGER_EN);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x MCETEN error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}
	msleep(10);

	ret = st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			    LSM6DSM_MASTER_CFG_ADDR,
				 *cache & ~(LSM6DSM_EXT_TRIGGER_EN
					    | LSM6DSM_I2C_MASTER_ON));
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x MCC error ret: %d\n",
			__func__, s->name, s->type, ret);
		restore_master_cfg(s, *cache);
		return ret;
	}

	return st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			LSM6DSM_MASTER_CFG_ADDR, LSM6DSM_I2C_PASS_THRU_MODE);
}

static inline int power_down_accel(const struct motion_sensor_t *s,
						int *cache)
{
	int ret;

	ret = st_raw_read8__7bf(s->port, s->i2c_spi_addr__7bf,
			   LSM6DSM_CTRL1_ADDR, cache);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x CTRL1R error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	return st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			     LSM6DSM_CTRL1_ADDR,
			     *cache & ~LSM6DSM_XL_ODR_MASK);
}

static inline int restore_ctrl1(const struct motion_sensor_t *s, int cache)
{
	return st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			     LSM6DSM_CTRL1_ADDR, cache);
}

static int config_slv0_read__7bf(const struct motion_sensor_t *s,
			    const uint16_t slv_addr__7bf,
			    uint16_t reg, uint8_t len)
{
	int ret;
	uint16_t addr__8b = I2C_GET_ADDR__7b(slv_addr__7bf) << 1;

	ret = st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			    LSM6DSM_SLV0_ADD_ADDR,
			    (addr__8b | LSM6DSM_SLV0_RD_BIT));
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x SA error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	ret = st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			    LSM6DSM_SLV0_SUBADD_ADDR, reg);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x RA error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	/*
	 * No decimation for external sensor 0,
	 * Number of sensors connected to external sensor hub 1
	 */
	ret = st_raw_write8__7bf(s->port, s->i2c_spi_addr__7bf,
			    LSM6DSM_SLV0_CONFIG_ADDR,
			    (len & LSM6DSM_SLV0_NUM_OPS_MASK));
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x CFG error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	return EC_SUCCESS;
}

int sensorhub_config_ext_reg__7bf(const struct motion_sensor_t *s,
			     const uint16_t slv_addr__7bf,
			     uint8_t reg, uint8_t val)
{
	int ret;
	int tmp;

	ret = enable_i2c_pass_through(s, &tmp);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x ENI2C error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	ret = st_raw_write8__7bf(s->port, slv_addr__7bf, reg, val);
	restore_master_cfg(s, tmp);
	return ret;
}

int sensorhub_config_slv0_read__7bf(const struct motion_sensor_t *s,
			       uint16_t slv_addr__7bf, uint8_t reg, int len)
{
	int tmp_xl_cfg;
	int ret;

	if (len <= 0 || len > OUT_XYZ_SIZE) {
		CPRINTF("%s: %s type:0x%x Invalid length: %d\n",
			__func__, s->name, s->type, len);
		return EC_ERROR_INVAL;
	}

	ret = power_down_accel(s, &tmp_xl_cfg);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x PDXL error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	ret = enable_ereg_bank_acc(s);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x ENERB error ret: %d\n",
			__func__, s->name, s->type, ret);
		goto out_restore_ctrl1;
	}

	ret = config_slv0_read__7bf(s, slv_addr__7bf, reg, len);
	disable_ereg_bank_acc(s);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x CS0R error ret: %d\n",
			__func__, s->name, s->type, ret);
		goto out_restore_ctrl1;
	}

	ret = enable_sensorhub_func(s);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x ENSH error ret: %d\n",
			__func__, s->name, s->type, ret);
		goto out_restore_ctrl1;
	}

	ret = enable_aux_i2c_master(s);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x ENI2CM error ret: %d\n",
			__func__, s->name, s->type, ret);
		disable_sensorhub_func(s);
	}
out_restore_ctrl1:
	restore_ctrl1(s, tmp_xl_cfg);
	return ret;
}

int sensorhub_slv0_data_read(const struct motion_sensor_t *s, uint8_t *raw)
{
	int ret;

	/*
	 * Accel/Gyro is already reading slave 0 data into the sensorhub1
	 * register as soon as the accel is in power-up mode. So return the
	 * contents of that register.
	 */
	ret = st_raw_read_n_noinc__7bf(s->port, s->i2c_spi_addr__7bf,
				  LSM6DSM_SENSORHUB1_REG,
				  raw, OUT_XYZ_SIZE);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x SH1R error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}
	return EC_SUCCESS;
}

int sensorhub_check_and_rst__7bf(const struct motion_sensor_t *s,
			    const uint16_t slv_addr__7bf,
			    uint8_t whoami_reg, uint8_t whoami_val,
			    uint8_t rst_reg, uint8_t rst_val)
{
	int ret, tmp;
	int tmp_master_cfg;

	ret = enable_i2c_pass_through(s, &tmp_master_cfg);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x ENI2C error ret: %d\n",
			__func__, s->name, s->type, ret);
		return ret;
	}

	ret = st_raw_read8__7bf(s->port, slv_addr__7bf, whoami_reg, &tmp);
	if (ret != EC_SUCCESS) {
		CPRINTF("%s: %s type:0x%x WAIR error ret: %d\n",
			__func__, s->name, s->type, ret);
		goto err_restore_master_cfg;
	}

	if (tmp != whoami_val) {
		CPRINTF("%s: %s type:0x%x WAIC error ret: %d\n",
			__func__, s->name, s->type, ret);
		ret = EC_ERROR_UNKNOWN;
		goto err_restore_master_cfg;
	}

	ret = st_raw_write8__7bf(s->port, slv_addr__7bf, rst_reg, rst_val);
err_restore_master_cfg:
	restore_master_cfg(s, tmp_master_cfg);
	return ret;
}