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