1 /*
2 * Copyright (c) 2021 Huawei Device Co., Ltd.
3 *
4 * HDF is dual licensed: you can use it either under the terms of
5 * the GPL, or the BSD license, at your option.
6 * See the LICENSE file in the root of this repository for complete details.
7 */
8
9 #include "gyro_bmi160.h"
10 #include <securec.h>
11 #include "osal_time.h"
12 #include "sensor_gyro_driver.h"
13 #include "sensor_config_controller.h"
14 #include "sensor_device_manager.h"
15
16 /* IO config for int-pin and I2C-pin */
17 #define SENSOR_I2C6_DATA_REG_ADDR 0x114f004c
18 #define SENSOR_I2C6_CLK_REG_ADDR 0x114f0048
19 #define SENSOR_I2C_REG_CFG 0x403
20
ReadBmi160GyroRawData(struct SensorCfgData * data,struct GyroData * rawData,int64_t * timestamp)21 static int32_t ReadBmi160GyroRawData(struct SensorCfgData *data, struct GyroData *rawData, int64_t *timestamp)
22 {
23 uint8_t status = 0;
24 uint8_t reg[GYRO_AXIS_BUTT];
25 OsalTimespec time;
26
27 (void)memset_s(&time, sizeof(time), 0, sizeof(time));
28 (void)memset_s(reg, sizeof(reg), 0, sizeof(reg));
29
30 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
31
32 if (OsalGetTime(&time) != HDF_SUCCESS) {
33 HDF_LOGE("%s: Get time failed", __func__);
34 return HDF_FAILURE;
35 }
36 *timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */
37
38 int32_t ret = ReadSensor(&data->busCfg, BMI160_STATUS_ADDR, &status, sizeof(uint8_t));
39 if (!(status & BMI160_GYRO_DATA_READY_MASK) || (ret != HDF_SUCCESS)) {
40 return HDF_FAILURE;
41 }
42
43 ret = ReadSensor(&data->busCfg, BMI160_GYRO_X_LSB_ADDR, ®[GYRO_X_AXIS_LSB], sizeof(uint8_t));
44 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
45
46 ret = ReadSensor(&data->busCfg, BMI160_GYRO_X_MSB_ADDR, ®[GYRO_X_AXIS_MSB], sizeof(uint8_t));
47 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
48
49 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Y_LSB_ADDR, ®[GYRO_Y_AXIS_LSB], sizeof(uint8_t));
50 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
51
52 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Y_MSB_ADDR, ®[GYRO_Y_AXIS_MSB], sizeof(uint8_t));
53 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
54
55 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Z_LSB_ADDR, ®[GYRO_Z_AXIS_LSB], sizeof(uint8_t));
56 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
57
58 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Z_MSB_ADDR, ®[GYRO_Z_AXIS_MSB], sizeof(uint8_t));
59 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
60
61 rawData->x = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_X_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
62 reg[GYRO_X_AXIS_LSB]);
63 rawData->y = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_Y_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
64 reg[GYRO_Y_AXIS_LSB]);
65 rawData->z = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_Z_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
66 reg[GYRO_Z_AXIS_LSB]);
67
68 return ret;
69 }
70
ReadBmi160GyroData(struct SensorCfgData * data)71 int32_t ReadBmi160GyroData(struct SensorCfgData *data)
72 {
73 int32_t ret;
74 struct GyroData rawData = { 0, 0, 0 };
75 int32_t tmp[GYRO_AXIS_NUM];
76 struct SensorReportEvent event;
77
78 (void)memset_s(&event, sizeof(event), 0, sizeof(event));
79
80 ret = ReadBmi160GyroRawData(data, &rawData, &event.timestamp);
81 if (ret != HDF_SUCCESS) {
82 return HDF_FAILURE;
83 }
84
85 event.sensorId = SENSOR_TAG_GYROSCOPE;
86 event.option = 0;
87 event.mode = SENSOR_WORK_MODE_REALTIME;
88
89 tmp[GYRO_X_AXIS] = rawData.x * BMI160_GYRO_SENSITIVITY_2000DPS;
90 tmp[GYRO_Y_AXIS] = rawData.y * BMI160_GYRO_SENSITIVITY_2000DPS;
91 tmp[GYRO_Z_AXIS] = rawData.z * BMI160_GYRO_SENSITIVITY_2000DPS;
92
93 event.dataLen = sizeof(tmp);
94 event.data = (uint8_t *)&tmp;
95 ret = ReportSensorEvent(&event);
96 return ret;
97 }
98
InitBmi160Gyro(struct SensorCfgData * data)99 static int32_t InitBmi160Gyro(struct SensorCfgData *data)
100 {
101 int32_t ret;
102
103 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
104 ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]);
105 if (ret != HDF_SUCCESS) {
106 HDF_LOGE("%s: BMI160 sensor init config failed", __func__);
107 return HDF_FAILURE;
108 }
109 return HDF_SUCCESS;
110 }
111
InitGyroPreConfig(void)112 static int32_t InitGyroPreConfig(void)
113 {
114 if (SetSensorPinMux(SENSOR_I2C6_DATA_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) {
115 HDF_LOGE("%s: Data write mux pin failed", __func__);
116 return HDF_FAILURE;
117 }
118 if (SetSensorPinMux(SENSOR_I2C6_CLK_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) {
119 HDF_LOGE("%s: Clk write mux pin failed", __func__);
120 return HDF_FAILURE;
121 }
122
123 return HDF_SUCCESS;
124 }
125
DetectGyroBim160Chip(struct SensorCfgData * data)126 int32_t DetectGyroBim160Chip(struct SensorCfgData *data)
127 {
128 int32_t ret;
129 struct GyroOpsCall ops;
130 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
131
132 if (strcmp(GYRO_CHIP_NAME_BMI160, data->sensorAttr.chipName) != 0) {
133 return HDF_SUCCESS;
134 }
135 ret = InitGyroPreConfig();
136 if (ret != HDF_SUCCESS) {
137 HDF_LOGE("%s: init BMI160 bus mux config", __func__);
138 return HDF_FAILURE;
139 }
140 if (DetectSensorDevice(data) != HDF_SUCCESS) {
141 return HDF_FAILURE;
142 }
143 ops.Init = InitBmi160Gyro;
144 ops.ReadData = ReadBmi160GyroData;
145 ret = RegisterGyroChipOps(&ops);
146 if (ret != HDF_SUCCESS) {
147 HDF_LOGE("%s: register BMI160 gyro failed", __func__);
148 (void)ReleaseSensorBusHandle(&data->busCfg);
149 return HDF_FAILURE;
150 }
151 return HDF_SUCCESS;
152 }
153