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_accel_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 sensor_accel_driver_c
20
21 #define HDF_ACCEL_WORK_QUEUE_NAME "hdf_accel_work_queue"
22
23 static struct AccelDrvData *g_accelDrvData = NULL;
24
AccelGetDrvData(void)25 static struct AccelDrvData *AccelGetDrvData(void)
26 {
27 return g_accelDrvData;
28 }
29
30 static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL };
31
AccelRegisterChipOps(const struct AccelOpsCall * ops)32 int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops)
33 {
34 struct AccelDrvData *drvData = AccelGetDrvData();
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
AccelDataWorkEntry(void * arg)44 static void AccelDataWorkEntry(void *arg)
45 {
46 struct AccelDrvData *drvData = NULL;
47
48 drvData = (struct AccelDrvData *)arg;
49 CHECK_NULL_PTR_RETURN(drvData);
50
51 if (drvData->ops.ReadData == NULL) {
52 HDF_LOGI("%s: Accel ReadData function NULl", __func__);
53 return;
54 }
55 if (drvData->ops.ReadData(drvData->accelCfg) != HDF_SUCCESS) {
56 HDF_LOGE("%s: Accel read data failed", __func__);
57 }
58 }
59
AccelTimerEntry(uintptr_t arg)60 static void AccelTimerEntry(uintptr_t arg)
61 {
62 int64_t interval;
63 int32_t ret;
64 struct AccelDrvData *drvData = (struct AccelDrvData *)arg;
65 CHECK_NULL_PTR_RETURN(drvData);
66
67 if (!HdfAddWork(&drvData->accelWorkQueue, &drvData->accelWork)) {
68 HDF_LOGE("%s: Accel 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->accelTimer, interval);
74 if (ret != HDF_SUCCESS) {
75 HDF_LOGE("%s: Accel modify time failed", __func__);
76 }
77 }
78
InitAccelData(struct AccelDrvData * drvData)79 static int32_t InitAccelData(struct AccelDrvData *drvData)
80 {
81 if (HdfWorkQueueInit(&drvData->accelWorkQueue, HDF_ACCEL_WORK_QUEUE_NAME) != HDF_SUCCESS) {
82 HDF_LOGE("%s: Accel init work queue failed", __func__);
83 return HDF_FAILURE;
84 }
85
86 if (HdfWorkInit(&drvData->accelWork, AccelDataWorkEntry, drvData) != HDF_SUCCESS) {
87 HDF_LOGE("%s: Accel 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
SetAccelEnable(void)98 static int32_t SetAccelEnable(void)
99 {
100 int32_t ret;
101 struct AccelDrvData *drvData = AccelGetDrvData();
102
103 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
104 CHECK_NULL_PTR_RETURN_VALUE(drvData->accelCfg, HDF_ERR_INVALID_PARAM);
105
106 if (drvData->enable) {
107 HDF_LOGE("%s: Accel sensor is enabled", __func__);
108 return HDF_SUCCESS;
109 }
110
111 ret = SetSensorRegCfgArray(&drvData->accelCfg->busCfg, drvData->accelCfg->regCfgGroup[SENSOR_ENABLE_GROUP]);
112 if (ret != HDF_SUCCESS) {
113 HDF_LOGE("%s: Accel sensor enable config failed", __func__);
114 return ret;
115 }
116
117 ret = OsalTimerCreate(&drvData->accelTimer, SENSOR_TIMER_MIN_TIME, AccelTimerEntry, (uintptr_t)drvData);
118 if (ret != HDF_SUCCESS) {
119 HDF_LOGE("%s: Accel create timer failed[%d]", __func__, ret);
120 return ret;
121 }
122
123 ret = OsalTimerStartLoop(&drvData->accelTimer);
124 if (ret != HDF_SUCCESS) {
125 HDF_LOGE("%s: Accel start timer failed[%d]", __func__, ret);
126 return ret;
127 }
128 drvData->enable = true;
129
130 return HDF_SUCCESS;
131 }
132
SetAccelDisable(void)133 static int32_t SetAccelDisable(void)
134 {
135 int32_t ret;
136 struct AccelDrvData *drvData = AccelGetDrvData();
137
138 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
139 CHECK_NULL_PTR_RETURN_VALUE(drvData->accelCfg, HDF_ERR_INVALID_PARAM);
140
141 if (!drvData->enable) {
142 HDF_LOGE("%s: Accel sensor had disable", __func__);
143 return HDF_SUCCESS;
144 }
145
146 ret = SetSensorRegCfgArray(&drvData->accelCfg->busCfg, drvData->accelCfg->regCfgGroup[SENSOR_DISABLE_GROUP]);
147 if (ret != HDF_SUCCESS) {
148 HDF_LOGE("%s: Accel sensor disable config failed", __func__);
149 return ret;
150 }
151
152 ret = OsalTimerDelete(&drvData->accelTimer);
153 if (ret != HDF_SUCCESS) {
154 HDF_LOGE("%s: Accel delete timer failed", __func__);
155 return ret;
156 }
157 drvData->enable = false;
158
159 return HDF_SUCCESS;
160 }
161
SetAccelBatch(int64_t samplingInterval,int64_t interval)162 static int32_t SetAccelBatch(int64_t samplingInterval, int64_t interval)
163 {
164 (void)interval;
165
166 struct AccelDrvData *drvData = NULL;
167
168 drvData = AccelGetDrvData();
169 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
170
171 drvData->interval = samplingInterval;
172
173 return HDF_SUCCESS;
174 }
175
SetAccelMode(int32_t mode)176 static int32_t SetAccelMode(int32_t mode)
177 {
178 return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE;
179 }
180
SetAccelOption(uint32_t option)181 static int32_t SetAccelOption(uint32_t option)
182 {
183 (void)option;
184 return HDF_SUCCESS;
185 }
186
DispatchAccel(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)187 static int32_t DispatchAccel(struct HdfDeviceIoClient *client,
188 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
189 {
190 (void)client;
191 (void)cmd;
192 (void)data;
193 (void)reply;
194
195 return HDF_SUCCESS;
196 }
197
AccelBindDriver(struct HdfDeviceObject * device)198 int32_t AccelBindDriver(struct HdfDeviceObject *device)
199 {
200 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
201
202 struct AccelDrvData *drvData = (struct AccelDrvData *)OsalMemCalloc(sizeof(*drvData));
203 if (drvData == NULL) {
204 HDF_LOGE("%s: Malloc accel drv data fail!", __func__);
205 return HDF_ERR_MALLOC_FAIL;
206 }
207
208 drvData->ioService.Dispatch = DispatchAccel;
209 drvData->device = device;
210 device->service = &drvData->ioService;
211 g_accelDrvData = drvData;
212 return HDF_SUCCESS;
213 }
214
InitAccelOps(struct SensorCfgData * config,struct SensorDeviceInfo * deviceInfo)215 static int32_t InitAccelOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo)
216 {
217 CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
218
219 deviceInfo->ops.Enable = SetAccelEnable;
220 deviceInfo->ops.Disable = SetAccelDisable;
221 deviceInfo->ops.SetBatch = SetAccelBatch;
222 deviceInfo->ops.SetMode = SetAccelMode;
223 deviceInfo->ops.SetOption = SetAccelOption;
224
225 if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo),
226 &config->sensorInfo, sizeof(config->sensorInfo)) != EOK) {
227 HDF_LOGE("%s: Copy sensor info failed", __func__);
228 return HDF_FAILURE;
229 }
230
231 return HDF_SUCCESS;
232 }
233
InitAccelAfterDetected(struct SensorCfgData * config)234 static int32_t InitAccelAfterDetected(struct SensorCfgData *config)
235 {
236 struct SensorDeviceInfo deviceInfo;
237 CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
238
239 if (InitAccelOps(config, &deviceInfo) != HDF_SUCCESS) {
240 HDF_LOGE("%s: Init accel ops failed", __func__);
241 return HDF_FAILURE;
242 }
243
244 if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) {
245 HDF_LOGE("%s: Add accel device failed", __func__);
246 return HDF_FAILURE;
247 }
248
249 if (ParseSensorRegConfig(config) != HDF_SUCCESS) {
250 HDF_LOGE("%s: Parse sensor register failed", __func__);
251 (void)DeleteSensorDevice(&config->sensorInfo);
252 ReleaseSensorAllRegConfig(config);
253 return HDF_FAILURE;
254 }
255 return HDF_SUCCESS;
256 }
257
AccelCreateCfgData(const struct DeviceResourceNode * node)258 struct SensorCfgData *AccelCreateCfgData(const struct DeviceResourceNode *node)
259 {
260 struct AccelDrvData *drvData = AccelGetDrvData();
261
262 if (drvData == NULL || node == NULL) {
263 HDF_LOGE("%s: Accel node pointer NULL", __func__);
264 return NULL;
265 }
266
267 if (drvData->detectFlag) {
268 HDF_LOGE("%s: Accel sensor have detected", __func__);
269 return NULL;
270 }
271
272 if (drvData->accelCfg == NULL) {
273 HDF_LOGE("%s: Accel accelCfg pointer NULL", __func__);
274 return NULL;
275 }
276
277 if (GetSensorBaseConfigData(node, drvData->accelCfg) != HDF_SUCCESS) {
278 HDF_LOGE("%s: Get sensor base config failed", __func__);
279 goto BASE_CONFIG_EXIT;
280 }
281
282 if (DetectSensorDevice(drvData->accelCfg) != HDF_SUCCESS) {
283 HDF_LOGI("%s: Accel sensor detect device no exist", __func__);
284 drvData->detectFlag = false;
285 goto BASE_CONFIG_EXIT;
286 }
287
288 drvData->detectFlag = true;
289 if (InitAccelAfterDetected(drvData->accelCfg) != HDF_SUCCESS) {
290 HDF_LOGE("%s: Accel sensor detect device no exist", __func__);
291 goto INIT_EXIT;
292 }
293 return drvData->accelCfg;
294
295 INIT_EXIT:
296 (void)ReleaseSensorBusHandle(&drvData->accelCfg->busCfg);
297 BASE_CONFIG_EXIT:
298 drvData->accelCfg->root = NULL;
299 (void)memset_s(&drvData->accelCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo));
300 (void)memset_s(&drvData->accelCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg));
301 (void)memset_s(&drvData->accelCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr));
302 return drvData->accelCfg;
303 }
304
AccelReleaseCfgData(struct SensorCfgData * accelCfg)305 void AccelReleaseCfgData(struct SensorCfgData *accelCfg)
306 {
307 CHECK_NULL_PTR_RETURN(accelCfg);
308
309 (void)DeleteSensorDevice(&accelCfg->sensorInfo);
310 ReleaseSensorAllRegConfig(accelCfg);
311 (void)ReleaseSensorBusHandle(&accelCfg->busCfg);
312
313 accelCfg->root = NULL;
314 (void)memset_s(&accelCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo));
315 (void)memset_s(&accelCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg));
316 (void)memset_s(&accelCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr));
317 }
318
AccelInitDriver(struct HdfDeviceObject * device)319 int32_t AccelInitDriver(struct HdfDeviceObject *device)
320 {
321 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
322 struct AccelDrvData *drvData = (struct AccelDrvData *)device->service;
323 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
324
325 if (InitAccelData(drvData) != HDF_SUCCESS) {
326 HDF_LOGE("%s: Init accel config failed", __func__);
327 return HDF_FAILURE;
328 }
329
330 drvData->accelCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->accelCfg));
331 if (drvData->accelCfg == NULL) {
332 HDF_LOGE("%s: Malloc accel config data failed", __func__);
333 return HDF_FAILURE;
334 }
335
336 drvData->accelCfg->regCfgGroup = &g_regCfgGroup[0];
337
338 HDF_LOGI("%s: Init accel driver success", __func__);
339 return HDF_SUCCESS;
340 }
341
AccelReleaseDriver(struct HdfDeviceObject * device)342 void AccelReleaseDriver(struct HdfDeviceObject *device)
343 {
344 CHECK_NULL_PTR_RETURN(device);
345
346 struct AccelDrvData *drvData = (struct AccelDrvData *)device->service;
347 CHECK_NULL_PTR_RETURN(drvData);
348
349 if (drvData->detectFlag && drvData->accelCfg != NULL) {
350 AccelReleaseCfgData(drvData->accelCfg);
351 }
352
353 OsalMemFree(drvData->accelCfg);
354 drvData->accelCfg = NULL;
355
356 HdfWorkDestroy(&drvData->accelWork);
357 HdfWorkQueueDestroy(&drvData->accelWorkQueue);
358 OsalMemFree(drvData);
359 }
360
361 struct HdfDriverEntry g_sensorAccelDevEntry = {
362 .moduleVersion = 1,
363 .moduleName = "HDF_SENSOR_ACCEL",
364 .Bind = AccelBindDriver,
365 .Init = AccelInitDriver,
366 .Release = AccelReleaseDriver,
367 };
368
369 HDF_INIT(g_sensorAccelDevEntry);
370