1 /*
2 * Copyright (c) 2021-2022 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_mem.h"
12 #include "osal_time.h"
13 #include "sensor_gyro_driver.h"
14 #include "sensor_config_controller.h"
15 #include "sensor_device_manager.h"
16
17 #define HDF_LOG_TAG hdf_sensor_gyro
18
19 static struct Bmi160DrvData *g_bmi160DrvData = NULL;
20
GyroBmi160GetDrvData(void)21 static struct Bmi160DrvData *GyroBmi160GetDrvData(void)
22 {
23 return g_bmi160DrvData;
24 }
25
26 /* IO config for int-pin and I2C-pin */
27 #define SENSOR_I2C6_DATA_REG_ADDR 0x114f004c
28 #define SENSOR_I2C6_CLK_REG_ADDR 0x114f0048
29 #define SENSOR_I2C_REG_CFG 0x403
30
ReadBmi160GyroRawData(struct SensorCfgData * data,struct GyroData * rawData,uint64_t * timestamp)31 static int32_t ReadBmi160GyroRawData(struct SensorCfgData *data, struct GyroData *rawData, uint64_t *timestamp)
32 {
33 uint8_t status = 0;
34 uint8_t reg[GYRO_AXIS_BUTT];
35 OsalTimespec time;
36
37 (void)memset_s(&time, sizeof(time), 0, sizeof(time));
38 (void)memset_s(reg, sizeof(reg), 0, sizeof(reg));
39
40 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
41
42 if (OsalGetTime(&time) != HDF_SUCCESS) {
43 HDF_LOGE("%s: Get time failed", __func__);
44 return HDF_FAILURE;
45 }
46 *timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */
47
48 int32_t ret = ReadSensor(&data->busCfg, BMI160_STATUS_ADDR, &status, sizeof(uint8_t));
49 if (!(status & BMI160_GYRO_DATA_READY_MASK) || (ret != HDF_SUCCESS)) {
50 return HDF_FAILURE;
51 }
52
53 ret = ReadSensor(&data->busCfg, BMI160_GYRO_X_LSB_ADDR, ®[GYRO_X_AXIS_LSB], sizeof(uint8_t));
54 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
55
56 ret = ReadSensor(&data->busCfg, BMI160_GYRO_X_MSB_ADDR, ®[GYRO_X_AXIS_MSB], sizeof(uint8_t));
57 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
58
59 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Y_LSB_ADDR, ®[GYRO_Y_AXIS_LSB], sizeof(uint8_t));
60 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
61
62 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Y_MSB_ADDR, ®[GYRO_Y_AXIS_MSB], sizeof(uint8_t));
63 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
64
65 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Z_LSB_ADDR, ®[GYRO_Z_AXIS_LSB], sizeof(uint8_t));
66 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
67
68 ret = ReadSensor(&data->busCfg, BMI160_GYRO_Z_MSB_ADDR, ®[GYRO_Z_AXIS_MSB], sizeof(uint8_t));
69 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
70
71 rawData->x = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_X_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
72 reg[GYRO_X_AXIS_LSB]);
73 rawData->y = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_Y_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
74 reg[GYRO_Y_AXIS_LSB]);
75 rawData->z = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_Z_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
76 reg[GYRO_Z_AXIS_LSB]);
77
78 return ret;
79 }
80
ReadBmi160GyroData(struct SensorCfgData * data)81 int32_t ReadBmi160GyroData(struct SensorCfgData *data)
82 {
83 int32_t ret;
84 struct GyroData rawData = { 0, 0, 0 };
85 int32_t tmp[GYRO_AXIS_NUM];
86 struct SensorReportEvent event;
87
88 (void)memset_s(&event, sizeof(event), 0, sizeof(event));
89
90 ret = ReadBmi160GyroRawData(data, &rawData, &event.timestamp);
91 if (ret != HDF_SUCCESS) {
92 return HDF_FAILURE;
93 }
94
95 event.sensorId = SENSOR_TAG_GYROSCOPE;
96 event.option = 0;
97 event.mode = SENSOR_WORK_MODE_REALTIME;
98
99 tmp[GYRO_X_AXIS] = rawData.x * BMI160_GYRO_SENSITIVITY_2000DPS;
100 tmp[GYRO_Y_AXIS] = rawData.y * BMI160_GYRO_SENSITIVITY_2000DPS;
101 tmp[GYRO_Z_AXIS] = rawData.z * BMI160_GYRO_SENSITIVITY_2000DPS;
102
103 ret = SensorRawDataToRemapData(data->direction, tmp, sizeof(tmp) / sizeof(tmp[0]));
104 if (ret != HDF_SUCCESS) {
105 HDF_LOGE("%s: BMI160 convert raw data failed", __func__);
106 return HDF_FAILURE;
107 }
108
109 event.dataLen = sizeof(tmp);
110 event.data = (uint8_t *)&tmp;
111 ret = ReportSensorEvent(&event);
112 return ret;
113 }
InitBmi160(struct SensorCfgData * data)114 static int32_t InitBmi160(struct SensorCfgData *data)
115 {
116 int32_t ret;
117
118 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
119 ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]);
120 if (ret != HDF_SUCCESS) {
121 HDF_LOGE("%s: BMI160 sensor init config failed", __func__);
122 return HDF_FAILURE;
123 }
124 return HDF_SUCCESS;
125 }
126
InitGyroPreConfig(void)127 static int32_t InitGyroPreConfig(void)
128 {
129 if (SetSensorPinMux(SENSOR_I2C6_DATA_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) {
130 HDF_LOGE("%s: Data write mux pin failed", __func__);
131 return HDF_FAILURE;
132 }
133 if (SetSensorPinMux(SENSOR_I2C6_CLK_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) {
134 HDF_LOGE("%s: Clk write mux pin failed", __func__);
135 return HDF_FAILURE;
136 }
137
138 return HDF_SUCCESS;
139 }
140
DispatchBMI160(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)141 static int32_t DispatchBMI160(struct HdfDeviceIoClient *client,
142 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
143 {
144 (void)client;
145 (void)cmd;
146 (void)data;
147 (void)reply;
148
149 return HDF_SUCCESS;
150 }
151
GyroBmi160BindDriver(struct HdfDeviceObject * device)152 int32_t GyroBmi160BindDriver(struct HdfDeviceObject *device)
153 {
154 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
155
156 struct Bmi160DrvData *drvData = (struct Bmi160DrvData *)OsalMemCalloc(sizeof(*drvData));
157 if (drvData == NULL) {
158 HDF_LOGE("%s: Malloc Bmi160 drv data fail", __func__);
159 return HDF_ERR_MALLOC_FAIL;
160 }
161
162 drvData->ioService.Dispatch = DispatchBMI160;
163 drvData->device = device;
164 device->service = &drvData->ioService;
165 g_bmi160DrvData = drvData;
166
167 return HDF_SUCCESS;
168 }
169
GyroBmi160InitDriver(struct HdfDeviceObject * device)170 int32_t GyroBmi160InitDriver(struct HdfDeviceObject *device)
171 {
172 int32_t ret;
173 struct GyroOpsCall ops;
174
175 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
176 struct Bmi160DrvData *drvData = (struct Bmi160DrvData *)device->service;
177 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
178
179 ret = InitGyroPreConfig();
180 if (ret != HDF_SUCCESS) {
181 HDF_LOGE("%s: Init BMI160 bus mux config", __func__);
182 return HDF_FAILURE;
183 }
184
185 drvData->sensorCfg = GyroCreateCfgData(device->property);
186 if (drvData->sensorCfg == NULL || drvData->sensorCfg->root == NULL) {
187 HDF_LOGD("%s: Creating gyrocfg failed because detection failed", __func__);
188 return HDF_ERR_NOT_SUPPORT;
189 }
190
191 ops.Init = NULL;
192 ops.ReadData = ReadBmi160GyroData;
193 ret = GyroRegisterChipOps(&ops);
194 if (ret != HDF_SUCCESS) {
195 HDF_LOGE("%s: Register BMI160 gyro failed", __func__);
196 return HDF_FAILURE;
197 }
198
199 ret = InitBmi160(drvData->sensorCfg);
200 if (ret != HDF_SUCCESS) {
201 HDF_LOGE("%s: Init BMI160 gyro failed", __func__);
202 return HDF_FAILURE;
203 }
204
205 return HDF_SUCCESS;
206 }
207
GyroBmi160ReleaseDriver(struct HdfDeviceObject * device)208 void GyroBmi160ReleaseDriver(struct HdfDeviceObject *device)
209 {
210 CHECK_NULL_PTR_RETURN(device);
211
212 struct Bmi160DrvData *drvData = (struct Bmi160DrvData *)device->service;
213 CHECK_NULL_PTR_RETURN(drvData);
214
215 if (drvData->sensorCfg != NULL) {
216 GyroReleaseCfgData(drvData->sensorCfg);
217 drvData->sensorCfg = NULL;
218 }
219
220 OsalMemFree(drvData);
221 }
222
223 struct HdfDriverEntry g_gyroBmi160DevEntry = {
224 .moduleVersion = 1,
225 .moduleName = "HDF_SENSOR_GYRO_BMI160",
226 .Bind = GyroBmi160BindDriver,
227 .Init = GyroBmi160InitDriver,
228 .Release = GyroBmi160ReleaseDriver,
229 };
230
231 HDF_INIT(g_gyroBmi160DevEntry);
232