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