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