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 "sensor_gyro_driver.h"
10 #include <securec.h>
11 #include "gyro_bmi160.h"
12 #include "hdf_base.h"
13 #include "hdf_device_desc.h"
14 #include "osal_math.h"
15 #include "osal_mem.h"
16 #include "sensor_config_controller.h"
17 #include "sensor_device_manager.h"
18 #include "sensor_platform_if.h"
19
20 #define HDF_LOG_TAG sensor_gyro_driver_c
21
22 #define HDF_GYRO_WORK_QUEUE_NAME "hdf_gyro_work_queue"
23
24 static struct GyroDetectIfList g_gyroDetectIfList[] = {
25 {GYRO_CHIP_NAME_BMI160, DetectGyroBim160Chip},
26 };
27
28 static struct GyroDrvData *g_gyroDrvData = NULL;
29
GyroGetDrvData(void)30 static struct GyroDrvData *GyroGetDrvData(void)
31 {
32 return g_gyroDrvData;
33 }
34
35 static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL };
36
RegisterGyroChipOps(const struct GyroOpsCall * ops)37 int32_t RegisterGyroChipOps(const struct GyroOpsCall *ops)
38 {
39 struct GyroDrvData *drvData = NULL;
40
41 CHECK_NULL_PTR_RETURN_VALUE(ops, HDF_ERR_INVALID_PARAM);
42
43 drvData = GyroGetDrvData();
44 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
45 drvData->ops.Init = ops->Init;
46 drvData->ops.ReadData = ops->ReadData;
47 return HDF_SUCCESS;
48 }
49
GyroDataWorkEntry(void * arg)50 static void GyroDataWorkEntry(void *arg)
51 {
52 int32_t ret;
53 struct GyroDrvData *drvData = (struct GyroDrvData *)arg;
54 CHECK_NULL_PTR_RETURN(drvData);
55 CHECK_NULL_PTR_RETURN(drvData->ops.ReadData);
56
57 ret = drvData->ops.ReadData(drvData->gyroCfg);
58 if (ret != HDF_SUCCESS) {
59 HDF_LOGE("%s: gyro read data failed", __func__);
60 return;
61 }
62 }
63
GyroTimerEntry(uintptr_t arg)64 static void GyroTimerEntry(uintptr_t arg)
65 {
66 int64_t interval;
67 int32_t ret;
68 struct GyroDrvData *drvData = (struct GyroDrvData *)arg;
69 CHECK_NULL_PTR_RETURN(drvData);
70
71 if (!HdfAddWork(&drvData->gyroWorkQueue, &drvData->gyroWork)) {
72 HDF_LOGE("%s: gyro add work queue failed", __func__);
73 }
74
75 interval = OsalDivS64(drvData->interval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT));
76 interval = (interval < SENSOR_TIMER_MIN_TIME) ? SENSOR_TIMER_MIN_TIME : interval;
77 ret = OsalTimerSetTimeout(&drvData->gyroTimer, interval);
78 if (ret != HDF_SUCCESS) {
79 HDF_LOGE("%s: gyro modify time failed", __func__);
80 }
81 }
82
InitGyroData(void)83 static int32_t InitGyroData(void)
84 {
85 struct GyroDrvData *drvData = GyroGetDrvData();
86 int32_t ret;
87 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
88
89 if (drvData->initStatus) {
90 return HDF_SUCCESS;
91 }
92
93 if (HdfWorkQueueInit(&drvData->gyroWorkQueue, HDF_GYRO_WORK_QUEUE_NAME) != HDF_SUCCESS) {
94 HDF_LOGE("%s: gyro init work queue failed", __func__);
95 return HDF_FAILURE;
96 }
97
98 if (HdfWorkInit(&drvData->gyroWork, GyroDataWorkEntry, drvData) != HDF_SUCCESS) {
99 HDF_LOGE("%s: gyro create thread failed", __func__);
100 return HDF_FAILURE;
101 }
102
103 CHECK_NULL_PTR_RETURN_VALUE(drvData->ops.Init, HDF_ERR_INVALID_PARAM);
104
105 ret = drvData->ops.Init(drvData->gyroCfg);
106 if (ret != HDF_SUCCESS) {
107 HDF_LOGE("%s: gyro create thread failed", __func__);
108 return HDF_FAILURE;
109 }
110
111 drvData->interval = SENSOR_TIMER_MIN_TIME;
112 drvData->initStatus = true;
113 drvData->enable = false;
114
115 return HDF_SUCCESS;
116 }
117
SetGyroEnable(void)118 static int32_t SetGyroEnable(void)
119 {
120 int32_t ret;
121 struct GyroDrvData *drvData = GyroGetDrvData();
122
123 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
124 CHECK_NULL_PTR_RETURN_VALUE(drvData->gyroCfg, HDF_ERR_INVALID_PARAM);
125
126 if (drvData->enable) {
127 HDF_LOGE("%s: gyro sensor is enabled", __func__);
128 return HDF_SUCCESS;
129 }
130
131 ret = SetSensorRegCfgArray(&drvData->gyroCfg->busCfg, drvData->gyroCfg->regCfgGroup[SENSOR_ENABLE_GROUP]);
132 if (ret != HDF_SUCCESS) {
133 HDF_LOGE("%s: gyro sensor enable config failed", __func__);
134 return ret;
135 }
136
137 ret = OsalTimerCreate(&drvData->gyroTimer, SENSOR_TIMER_MIN_TIME, GyroTimerEntry, (uintptr_t)drvData);
138 if (ret != HDF_SUCCESS) {
139 HDF_LOGE("%s: gyro create timer failed[%d]", __func__, ret);
140 return ret;
141 }
142
143 ret = OsalTimerStartLoop(&drvData->gyroTimer);
144 if (ret != HDF_SUCCESS) {
145 HDF_LOGE("%s: gyro start timer failed[%d]", __func__, ret);
146 return ret;
147 }
148 drvData->enable = true;
149
150 return HDF_SUCCESS;
151 }
152
SetGyroDisable(void)153 static int32_t SetGyroDisable(void)
154 {
155 int32_t ret;
156 struct GyroDrvData *drvData = GyroGetDrvData();
157
158 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
159 CHECK_NULL_PTR_RETURN_VALUE(drvData->gyroCfg, HDF_ERR_INVALID_PARAM);
160
161 if (!drvData->enable) {
162 HDF_LOGE("%s: gyro sensor had disable", __func__);
163 return HDF_SUCCESS;
164 }
165
166 ret = SetSensorRegCfgArray(&drvData->gyroCfg->busCfg, drvData->gyroCfg->regCfgGroup[SENSOR_DISABLE_GROUP]);
167 if (ret != HDF_SUCCESS) {
168 HDF_LOGE("%s: gyro sensor disable config failed", __func__);
169 return ret;
170 }
171
172 ret = OsalTimerDelete(&drvData->gyroTimer);
173 if (ret != HDF_SUCCESS) {
174 HDF_LOGE("%s: gyro delete timer failed", __func__);
175 return ret;
176 }
177 drvData->enable = false;
178 return HDF_SUCCESS;
179 }
180
SetGyroBatch(int64_t samplingInterval,int64_t interval)181 static int32_t SetGyroBatch(int64_t samplingInterval, int64_t interval)
182 {
183 (void)interval;
184
185 struct GyroDrvData *drvData = NULL;
186
187 drvData = GyroGetDrvData();
188 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
189
190 drvData->interval = samplingInterval;
191
192 return HDF_SUCCESS;
193 }
194
SetGyroMode(int32_t mode)195 static int32_t SetGyroMode(int32_t mode)
196 {
197 return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE;
198 }
199
SetGyroOption(uint32_t option)200 static int32_t SetGyroOption(uint32_t option)
201 {
202 (void)option;
203 return HDF_SUCCESS;
204 }
205
DispatchGyro(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)206 static int32_t DispatchGyro(struct HdfDeviceIoClient *client,
207 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
208 {
209 (void)client;
210 (void)cmd;
211 (void)data;
212 (void)reply;
213
214 return HDF_SUCCESS;
215 }
216
BindGyroDriver(struct HdfDeviceObject * device)217 int32_t BindGyroDriver(struct HdfDeviceObject *device)
218 {
219 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
220
221 struct GyroDrvData *drvData = (struct GyroDrvData *)OsalMemCalloc(sizeof(*drvData));
222 if (drvData == NULL) {
223 HDF_LOGE("%s: malloc gyro drv data fail!", __func__);
224 return HDF_ERR_MALLOC_FAIL;
225 }
226
227 drvData->ioService.Dispatch = DispatchGyro;
228 drvData->device = device;
229 device->service = &drvData->ioService;
230 g_gyroDrvData = drvData;
231 return HDF_SUCCESS;
232 }
233
InitGyroOps(struct SensorDeviceInfo * deviceInfo)234 static int32_t InitGyroOps(struct SensorDeviceInfo *deviceInfo)
235 {
236 struct GyroDrvData *drvData = GyroGetDrvData();
237
238 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
239
240 deviceInfo->ops.Enable = SetGyroEnable;
241 deviceInfo->ops.Disable = SetGyroDisable;
242 deviceInfo->ops.SetBatch = SetGyroBatch;
243 deviceInfo->ops.SetMode = SetGyroMode;
244 deviceInfo->ops.SetOption = SetGyroOption;
245
246 if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo),
247 &drvData->gyroCfg->sensorInfo, sizeof(drvData->gyroCfg->sensorInfo)) != EOK) {
248 HDF_LOGE("%s: copy sensor info failed", __func__);
249 return HDF_FAILURE;
250 }
251
252 return HDF_SUCCESS;
253 }
254
InitGyroAfterConfig(void)255 static int32_t InitGyroAfterConfig(void)
256 {
257 struct SensorDeviceInfo deviceInfo;
258
259 if (InitGyroData() != HDF_SUCCESS) {
260 HDF_LOGE("%s: init gyro config failed", __func__);
261 return HDF_FAILURE;
262 }
263
264 if (InitGyroOps(&deviceInfo) != HDF_SUCCESS) {
265 HDF_LOGE("%s: init gyro ops failed", __func__);
266 return HDF_FAILURE;
267 }
268
269 if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) {
270 HDF_LOGE("%s: add gyro device failed", __func__);
271 return HDF_FAILURE;
272 }
273
274 return HDF_SUCCESS;
275 }
276
DetectGyroChip(void)277 static int32_t DetectGyroChip(void)
278 {
279 int32_t num;
280 int32_t ret;
281 int32_t loop;
282 struct GyroDrvData *drvData = GyroGetDrvData();
283
284 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
285 CHECK_NULL_PTR_RETURN_VALUE(drvData->gyroCfg, HDF_ERR_INVALID_PARAM);
286
287 num = sizeof(g_gyroDetectIfList) / sizeof(g_gyroDetectIfList[0]);
288 for (loop = 0; loop < num; ++loop) {
289 if (g_gyroDetectIfList[loop].DetectChip != NULL) {
290 ret = g_gyroDetectIfList[loop].DetectChip(drvData->gyroCfg);
291 if (ret == HDF_SUCCESS) {
292 drvData->detectFlag = true;
293 return HDF_SUCCESS;
294 }
295 }
296 }
297
298 HDF_LOGE("%s: detect gyro device failed", __func__);
299 drvData->detectFlag = false;
300 return HDF_FAILURE;
301 }
302
InitGyroDriver(struct HdfDeviceObject * device)303 int32_t InitGyroDriver(struct HdfDeviceObject *device)
304 {
305 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
306 struct GyroDrvData *drvData = (struct GyroDrvData *)device->service;
307 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
308
309 if (drvData->detectFlag) {
310 HDF_LOGE("%s: gyro sensor have detected", __func__);
311 return HDF_SUCCESS;
312 }
313
314 drvData->gyroCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->gyroCfg));
315 if (drvData->gyroCfg == NULL) {
316 HDF_LOGE("%s: malloc sensor config data failed", __func__);
317 return HDF_FAILURE;
318 }
319
320 drvData->gyroCfg->regCfgGroup = &g_regCfgGroup[0];
321
322 if (GetSensorBaseConfigData(device->property, drvData->gyroCfg) != HDF_SUCCESS) {
323 HDF_LOGE("%s: get sensor base config failed", __func__);
324 goto BASE_CONFIG_EXIT;
325 }
326
327 // if return failure, hdf framework go to next detect sensor
328 if (DetectGyroChip() != HDF_SUCCESS) {
329 HDF_LOGE("%s: gyro sensor detect device no exist", __func__);
330 goto DETECT_CHIP_EXIT;
331 }
332 drvData->detectFlag = true;
333
334 if (ParseSensorRegConfig(drvData->gyroCfg) != HDF_SUCCESS) {
335 HDF_LOGE("%s: detect sensor device failed", __func__);
336 goto REG_CONFIG_EXIT;
337 }
338
339 if (InitGyroAfterConfig() != HDF_SUCCESS) {
340 HDF_LOGE("%s: init gyro after config failed", __func__);
341 goto INIT_EXIT;
342 }
343
344 HDF_LOGI("%s: init gyro driver success", __func__);
345 return HDF_SUCCESS;
346
347 INIT_EXIT:
348 (void)DeleteSensorDevice(&drvData->gyroCfg->sensorInfo);
349 REG_CONFIG_EXIT:
350 ReleaseSensorAllRegConfig(drvData->gyroCfg);
351 (void)ReleaseSensorBusHandle(&drvData->gyroCfg->busCfg);
352 DETECT_CHIP_EXIT:
353 drvData->detectFlag = false;
354 BASE_CONFIG_EXIT:
355 drvData->gyroCfg->root = NULL;
356 drvData->gyroCfg->regCfgGroup = NULL;
357 OsalMemFree(drvData->gyroCfg);
358 drvData->gyroCfg = NULL;
359 return HDF_FAILURE;
360 }
361
ReleaseGyroDriver(struct HdfDeviceObject * device)362 void ReleaseGyroDriver(struct HdfDeviceObject *device)
363 {
364 CHECK_NULL_PTR_RETURN(device);
365
366 struct GyroDrvData *drvData = (struct GyroDrvData *)device->service;
367 CHECK_NULL_PTR_RETURN(drvData);
368
369 (void)DeleteSensorDevice(&drvData->gyroCfg->sensorInfo);
370 drvData->detectFlag = false;
371
372 if (drvData->gyroCfg != NULL) {
373 drvData->gyroCfg->root = NULL;
374 drvData->gyroCfg->regCfgGroup = NULL;
375 ReleaseSensorAllRegConfig(drvData->gyroCfg);
376 (void)ReleaseSensorBusHandle(&drvData->gyroCfg->busCfg);
377 OsalMemFree(drvData->gyroCfg);
378 drvData->gyroCfg = NULL;
379 }
380
381 drvData->initStatus = false;
382 }
383
384 struct HdfDriverEntry g_sensorGyroDevEntry = {
385 .moduleVersion = 1,
386 .moduleName = "HDF_SENSOR_GYRO",
387 .Bind = BindGyroDriver,
388 .Init = InitGyroDriver,
389 .Release = ReleaseGyroDriver,
390 };
391
392 HDF_INIT(g_sensorGyroDevEntry);
393