1 /**
2 * Copyright (c) 2024 Bosch Sensortec GmbH. All rights reserved.
3 *
4 * gyro_bmi270.c as part of the * /chipsets subdirectory
5 * is dual licensed: you can use it either under the terms of
6 * the GPL, or the BSD license, at your option.
7 * See the LICENSE file in the root of this repository for complete details.
8 */
9
10 #include "gyro_bmi270.h"
11 #include <securec.h>
12 #include "osal_mem.h"
13 #include "osal_time.h"
14 #include "sensor_gyro_driver.h"
15 #include "sensor_config_controller.h"
16 #include "sensor_device_manager.h"
17
18 #define HDF_LOG_TAG khdf_sensor_gyro_driver
19 static struct Bmi270DrvData *g_bmi270DrvData = NULL;
20 static uint8_t g_regw_buffer[512];
21
22 /* IO config for int-pin and I2C-pin */
23 #define SENSOR_I2C6_DATA_REG_ADDR 0x114f004c
24 #define SENSOR_I2C6_CLK_REG_ADDR 0x114f0048
25 #define SENSOR_I2C_REG_CFG 0x403
26
27 static uint8_t bmi270_cfg_buffer[] = {
28 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e,
29 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e,
30 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e,
31 0x59, 0xf5, 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00,
32 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, 0x00, 0x37, 0x00,
33 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1,
34 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
35 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
36 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1,
37 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, 0x24,
38 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24,
39 0xeb, 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f,
40 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, 0xe0, 0x3c, 0x48, 0x00,
41 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86,
42 0xf0, 0x7f, 0x41, 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30,
43 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, 0x00, 0xfc,
44 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00,
45 0x64, 0xbb, 0xd1, 0xbe, 0x94, 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd,
46 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f,
47 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e,
48 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, 0x26, 0xe0, 0x6f, 0x02, 0x31,
49 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7,
50 0x10, 0x08, 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f,
51 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, 0x6f, 0x00, 0x2e,
52 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40,
53 0x52, 0x42, 0x3e, 0x84, 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f,
54 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, 0xf5,
55 0xeb, 0x2c, 0xe1, 0x6f
56 };
57 //328 bytes
58
Bmi270GetDrvData(void)59 static struct Bmi270DrvData *Bmi270GetDrvData(void)
60 {
61 return g_bmi270DrvData;
62 };
63
64 /// @brief basic register write function
65 /// @param data Sensor configuration data structre pointer
66 /// @param rega register address
67 /// @param buffer value to write
68 /// @param len write len
69 /// @return HDF_SUCCESS if success, failed any error
GyroBmi270HalRegWrite(struct SensorCfgData *data, uint16_t rega, uint8_t* buffer, uint32_t len)70 static int32_t GyroBmi270HalRegWrite(struct SensorCfgData *data, uint16_t rega, uint8_t* buffer, uint32_t len)
71 {
72 int32_t rc = HDF_SUCCESS;
73 uint32_t idx = 0;
74 (void)memset_s(g_regw_buffer, sizeof(g_regw_buffer), 0, sizeof(g_regw_buffer));
75
76 g_regw_buffer[0] = (rega & 0xFF);
77 do {
78 g_regw_buffer[idx + 1] = buffer[idx];
79 idx ++;
80 } while (idx < len);
81
82 rc = WriteSensor(&data->busCfg, g_regw_buffer, (len + 1));
83 OsalUDelay(BMI270_LP_MODE_WRITE_DELAY_IN_US);
84
85 if (rc != HDF_SUCCESS) {
86 HDF_LOGE("%s: [BMI270] w reg:%d err", __func__, rega);
87 }
88
89 return rc;
90 }
91
92 /// @brief basic register read function
93 /// @param data Sensor configuration data structre pointer
94 /// @param rega register address to read
95 /// @param buffer read data buffer
96 /// @param len read len
97 /// @return HDF_SUCCESS if success, failed any error
GyroBmi270HalRegRead(struct SensorCfgData *data, uint16_t rega, uint8_t* buffer, uint32_t len)98 static int32_t GyroBmi270HalRegRead(struct SensorCfgData *data, uint16_t rega, uint8_t* buffer, uint32_t len)
99 {
100 int32_t rc = HDF_SUCCESS;
101 rc = ReadSensor(&data->busCfg, rega, buffer, len);
102 OsalUDelay(BMI270_NORMAL_MODE_WRITE_DELAY_IN_US);
103
104 if (rc != HDF_SUCCESS) {
105 HDF_LOGE("%s: [BMI270] r reg:%d err", __func__, rega);
106 }
107
108 return rc;
109 }
110
GyroReadBmi270RawData(struct SensorCfgData *data, struct GyroData *rawData, int64_t *timestamp)111 static int32_t GyroReadBmi270RawData(struct SensorCfgData *data, struct GyroData *rawData, int64_t *timestamp)
112 {
113 OsalTimespec time;
114 uint8_t status = 0;
115 uint8_t regv[GYRO_AXIS_BUTT] = {0};
116 int32_t ret = HDF_SUCCESS;
117
118 (void)memset_s(&time, sizeof(time), 0, sizeof(time));
119 (void)memset_s(regv, sizeof(regv), 0, sizeof(regv));
120
121 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
122
123 if (OsalGetTime(&time) != HDF_SUCCESS) {
124 HDF_LOGE("%s: Get time failed", __func__);
125 return HDF_FAILURE;
126 }
127 *timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */
128
129 ret = GyroBmi270HalRegRead(data, BMI270_GYRO_REGA_STATUS, &status, sizeof(uint8_t));
130 /* any new data? */
131 if (!(status & BMI270_GYRO_DATA_READY_MASK) || (ret != HDF_SUCCESS)) {
132 return HDF_FAILURE;
133 }
134
135 ret = GyroBmi270HalRegRead(data, BMI270_GYRO_REGA_X_LSB_ADDR, regv, BMI270_GYR_DATA_FRAME_SIZE);
136
137 rawData->x = (int16_t)(SENSOR_DATA_SHIFT_LEFT(regv[GYRO_X_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
138 regv[GYRO_X_AXIS_LSB]);
139 rawData->y = (int16_t)(SENSOR_DATA_SHIFT_LEFT(regv[GYRO_Y_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
140 regv[GYRO_Y_AXIS_LSB]);
141 rawData->z = (int16_t)(SENSOR_DATA_SHIFT_LEFT(regv[GYRO_Z_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
142 regv[GYRO_Z_AXIS_LSB]);
143
144 return ret;
145 }
146
GyroReadBmi270Data(struct SensorCfgData *cfg)147 static int32_t GyroReadBmi270Data(struct SensorCfgData *cfg)
148 {
149 int32_t ret;
150 struct GyroData rawData = { 0, 0, 0 };
151 static int32_t tmp[GYRO_AXIS_NUM];
152 struct SensorReportEvent event;
153
154 CHECK_NULL_PTR_RETURN_VALUE(cfg, HDF_ERR_INVALID_PARAM);
155
156 ret = GyroReadBmi270RawData(cfg, &rawData, &event.timestamp);
157 if (ret != HDF_SUCCESS) {
158 HDF_LOGE("%s: BMI270 read raw data failed", __func__);
159 return HDF_FAILURE;
160 }
161
162 event.sensorId = SENSOR_TAG_GYROSCOPE;
163 event.option = 0;
164 event.mode = SENSOR_WORK_MODE_REALTIME;
165
166 rawData.x = rawData.x * BMI270_GYRO_SENSITIVITY_2000DPS;
167 rawData.y = rawData.y * BMI270_GYRO_SENSITIVITY_2000DPS;
168 rawData.z = rawData.z * BMI270_GYRO_SENSITIVITY_2000DPS;
169
170 tmp[GYRO_X_AXIS] = (rawData.x * SENSOR_CONVERT_UNIT) / SENSOR_CONVERT_UNIT;
171 tmp[GYRO_Y_AXIS] = (rawData.y * SENSOR_CONVERT_UNIT) / SENSOR_CONVERT_UNIT;
172 tmp[GYRO_Z_AXIS] = (rawData.z * SENSOR_CONVERT_UNIT) / SENSOR_CONVERT_UNIT;
173
174 ret = SensorRawDataToRemapData(cfg->direction, tmp, sizeof(tmp) / sizeof(tmp[0]));
175 if (ret != HDF_SUCCESS) {
176 HDF_LOGE("%s: BMI270 convert raw data failed", __func__);
177 return HDF_FAILURE;
178 }
179
180 event.dataLen = sizeof(tmp);
181 event.data = (uint8_t *)&tmp;
182 ret = ReportSensorEvent(&event);
183
184 return ret;
185 }
186
187 // success
Bmi270IMUSwReset(struct SensorCfgData* data)188 static int32_t Bmi270IMUSwReset(struct SensorCfgData* data)
189 {
190 uint8_t regv = BMI26X_REGV_CMD_SOFT_RESET;
191 int32_t rc = GyroBmi270HalRegWrite(data, BMI26X_REGA_USR_CMD, ®v, BMI270_ONE_BYTE);
192 // delay 5ms after reset
193 OsalMDelay(BMI270_RESET_DELAY_IN_MS);
194
195 return rc;
196 }
197
Bmi270HalIMUInited(struct SensorCfgData *data)198 static bool Bmi270HalIMUInited(struct SensorCfgData *data)
199 {
200 bool rc = false;
201 uint8_t regv = 0;
202 rc = GyroBmi270HalRegRead(data, BMI270_REGA_INTERNAL_STATUS, ®v, BMI270_ONE_BYTE);
203 if (BST_GET_VAL_BIT(regv, 0)) {
204 HDF_LOGI("[BMI270] %s, IMU inited", __LINE__);
205 rc = true;
206 }
207
208 return rc;
209 }
210
Bmi270CfgGyroPwr(struct SensorCfgData *data, bool en)211 static int32_t Bmi270CfgGyroPwr(struct SensorCfgData *data, bool en)
212 {
213 uint8_t regv = 0;
214 int32_t rc = HDF_SUCCESS;
215 rc = GyroBmi270HalRegRead(data, BMI270_REGA_PWR_CTRL, ®v, BMI270_ONE_BYTE);
216
217 if (en) {
218 regv = BST_SET_VAL_BIT(regv, BMI270_GYR_POWER_BIT_POS_IN_PWR_CTRL_REG);
219 } else {
220 regv = BST_CLR_VAL_BIT(regv, BMI270_GYR_POWER_BIT_POS_IN_PWR_CTRL_REG);
221 }
222
223 rc = GyroBmi270HalRegWrite(data, BMI270_REGA_PWR_CTRL, ®v, BMI270_ONE_BYTE);
224
225 return rc;
226 }
227
228 // @load configuration
Bmi270LoadCfg(struct SensorCfgData *data, uint8_t* cfg_data_buffer, uint32_t cfg_size, uint32_t in_burst_write_size)229 static int32_t Bmi270LoadCfg(struct SensorCfgData *data, uint8_t* cfg_data_buffer,
230 uint32_t cfg_size, uint32_t in_burst_write_size)
231 {
232 int32_t rc = HDF_SUCCESS;
233 uint8_t regv = 0;
234 /* Variable to update the configuration file index */
235 uint16_t index = 0;
236 /* Array to store address */
237 uint8_t addr_array[2] = {0};
238 uint32_t size_bw = in_burst_write_size; //burst write size in byte
239
240 /* Disable loading of the configuration */
241 rc = GyroBmi270HalRegWrite(data, BMI26X_REGA_USR_TITAN_CTRL, ®v, BMI270_ONE_BYTE);
242
243 for (index = 0; index < cfg_size; index += size_bw) {
244 //1. len
245 if (index + in_burst_write_size > cfg_size) {
246 size_bw = cfg_size - index;
247 }
248
249 // index low in word
250 addr_array[0] = (uint8_t)((index / 2) & 0x0F);
251 // index high in word
252 addr_array[1] = (uint8_t)((index / 2) >> 4);
253 rc = GyroBmi270HalRegWrite(data, BMI26X_REGA_USR_CONF_STREAM_IDX_LSB, addr_array, BMI270_TWO_BYTE);
254
255 //2. data
256 rc = GyroBmi270HalRegWrite(data, BMI26X_REGA_USR_CONF_STREAM_IN,
257 cfg_data_buffer + index, size_bw);
258 }
259
260 return rc;
261 }
262
Bmi270HalLoadCfg(struct SensorCfgData *data)263 static int32_t Bmi270HalLoadCfg(struct SensorCfgData *data)
264 {
265 int32_t rc = HDF_SUCCESS;
266 uint8_t regv = 0;
267 uint8_t try_num_to_check_cfg = 0;
268 bool cfg_is_avaiblabe = Bmi270HalIMUInited(data);
269 if (cfg_is_avaiblabe) {
270 return HDF_SUCCESS;
271 }
272
273 rc = Bmi270IMUSwReset(data);
274 if (rc != HDF_SUCCESS) {
275 return rc;
276 }
277
278 //disable advance power saving
279 rc = GyroBmi270HalRegWrite(data, BMI270_REGA_PWR_CFG, ®v, BMI270_ONE_BYTE);
280
281 // load configuration now
282 rc = Bmi270LoadCfg(data, bmi270_cfg_buffer,
283 HDF_ARRAY_SIZE(bmi270_cfg_buffer),
284 HDF_ARRAY_SIZE(bmi270_cfg_buffer));
285
286 regv = 0x01;
287 rc = GyroBmi270HalRegWrite(data, BMI26X_REGA_USR_TITAN_CTRL, ®v, BMI270_ONE_BYTE);
288
289 cfg_is_avaiblabe = false;
290 rc = HDF_DEV_ERR_DEV_INIT_FAIL;
291 do {
292 OsalMDelay(BMI270_LOAD_RAM_PATCH_DELAY_IN_MS); //10ms
293 cfg_is_avaiblabe = Bmi270HalIMUInited(data);
294 try_num_to_check_cfg ++;
295 if (cfg_is_avaiblabe) {
296 rc = HDF_SUCCESS;
297 break;
298 }
299 } while ((try_num_to_check_cfg < BMI26X_CHECK_CONFIGURE_STATUS_TIMES) &&
300 (!cfg_is_avaiblabe));
301
302 return rc;
303 }
304
Bmi270GetWhoAmI(struct SensorCfgData *data)305 static int32_t Bmi270GetWhoAmI(struct SensorCfgData *data)
306 {
307 uint8_t regv = 0;
308 int32_t rc = HDF_SUCCESS;
309
310 rc = GyroBmi270HalRegRead(data, 0x00, ®v, BMI270_ONE_BYTE);
311 if (rc != HDF_SUCCESS) {
312 HDF_LOGE("%s: [BMI270] WARN!!, NO Sensor", __func__);
313 return HDF_FAILURE;
314 }
315 if (regv != BMI26X_REGV_WHOAMI) {
316 rc = HDF_DEV_ERR_NO_DEVICE;
317 }
318
319 return rc;
320 }
321
322
InitBmi270(struct SensorCfgData *data)323 static int32_t InitBmi270(struct SensorCfgData *data)
324 {
325 int32_t ret = HDF_SUCCESS;
326 uint8_t regv = 0x00;
327
328 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
329 ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]);
330 if (ret != HDF_SUCCESS) {
331 HDF_LOGE("%s: BMI270 sensor init config failed", __func__);
332 return HDF_FAILURE;
333 }
334
335 ret = Bmi270CfgGyroPwr(data, true);
336 //defualt odr:200hz
337 // range:+/- 2000dps, 16.4LSB/dps
338 regv = 0;
339 ret = GyroBmi270HalRegWrite(data, BMI26X_REGA_GYRO_RANGE, ®v, BMI270_ONE_BYTE);
340
341 return ret;
342 }
343
DispatchBMI270(struct HdfDeviceIoClient *client, int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)344 static int32_t DispatchBMI270(struct HdfDeviceIoClient *client,
345 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
346 {
347 (void)client;
348 (void)cmd;
349 (void)data;
350 (void)reply;
351
352 return HDF_SUCCESS;
353 }
354
GyroBmi270BindDriver(struct HdfDeviceObject *device)355 static int32_t GyroBmi270BindDriver(struct HdfDeviceObject *device)
356 {
357 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
358
359 struct Bmi270DrvData *drvData = (struct Bmi270DrvData *)OsalMemCalloc(sizeof(*drvData));
360 if (drvData == NULL) {
361 HDF_LOGE("%s: Malloc Bmi270 drv data fail", __func__);
362 return HDF_ERR_MALLOC_FAIL;
363 }
364
365 drvData->ioService.Dispatch = DispatchBMI270;
366 drvData->device = device;
367 device->service = &drvData->ioService;
368 g_bmi270DrvData = drvData;
369
370 return HDF_SUCCESS;
371 }
372
GyroBmi270InitDriver(struct HdfDeviceObject *device)373 static int32_t GyroBmi270InitDriver(struct HdfDeviceObject *device)
374 {
375 int32_t ret;
376 struct GyroOpsCall ops;
377
378 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
379 struct Bmi270DrvData *drvData = (struct Bmi270DrvData *)device->service;
380 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
381
382 drvData->sensorCfg = GyroCreateCfgData(device->property);
383 if (drvData->sensorCfg == NULL || drvData->sensorCfg->root == NULL) {
384 HDF_LOGD("%s: Creating gyrocfg failed because detection failed", __func__);
385 return HDF_ERR_NOT_SUPPORT;
386 }
387
388 ops.Init = NULL;
389 ops.ReadData = GyroReadBmi270Data;
390 ret = GyroRegisterChipOps(&ops);
391 if (ret != HDF_SUCCESS) {
392 HDF_LOGE("%s: Register BMI270 gyro failed", __func__);
393 return HDF_FAILURE;
394 }
395
396 // check whoami
397 ret = Bmi270GetWhoAmI(drvData->sensorCfg);
398 if (ret != HDF_SUCCESS) {
399 return ret;
400 }
401
402 do {
403 ret = Bmi270HalLoadCfg(drvData->sensorCfg);
404 } while (0);
405
406 //init hw
407 ret = InitBmi270(drvData->sensorCfg);
408 if (ret != HDF_SUCCESS) {
409 HDF_LOGE("%s: Init BMI270 gyro failed", __func__);
410 return HDF_FAILURE;
411 }
412
413 return HDF_SUCCESS;
414 }
415
GyroBmi270ReleaseDriver(struct HdfDeviceObject *device)416 static void GyroBmi270ReleaseDriver(struct HdfDeviceObject *device)
417 {
418 CHECK_NULL_PTR_RETURN(device);
419
420 struct Bmi270DrvData *drvData = (struct Bmi270DrvData *)device->service;
421 CHECK_NULL_PTR_RETURN(drvData);
422
423 if (drvData->sensorCfg != NULL) {
424 GyroReleaseCfgData(drvData->sensorCfg);
425 drvData->sensorCfg = NULL;
426 }
427
428 OsalMemFree(drvData);
429 }
430
431 struct HdfDriverEntry g_gyroBmi270DevEntry = {
432 .moduleVersion = 1,
433 .moduleName = "HDF_SENSOR_GYRO_BMI270",
434 .Bind = GyroBmi270BindDriver,
435 .Init = GyroBmi270InitDriver,
436 .Release = GyroBmi270ReleaseDriver,
437 };
438
439 HDF_INIT(g_gyroBmi270DevEntry);
440