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_gravity_driver.h"
10 #include <securec.h>
11 #include "sensor_accel_driver.h"
12 #include "hdf_base.h"
13 #include "hdf_device_desc.h"
14 #include "osal_math.h"
15 #include "osal_mem.h"
16 #include "osal_time.h"
17 #include "sensor_device_manager.h"
18 #include "sensor_platform_if.h"
19
20 #define HDF_LOG_TAG hdf_sensor_gravity_driver
21
22 #define HDF_GRAVITY_WORK_QUEUE_NAME "hdf_gravity_work_queue"
23
24 static struct GravityDrvData *g_gravityDrvData = NULL;
25 static int32_t g_accelRawData[GRAVITY_AXIS_NUM];
26
GravityGetDrvData(void)27 static struct GravityDrvData *GravityGetDrvData(void)
28 {
29 return g_gravityDrvData;
30 }
31
GravitySubscribeAccelData(int32_t * accelData,int32_t size)32 static int32_t GravitySubscribeAccelData(int32_t *accelData, int32_t size)
33 {
34 CHECK_NULL_PTR_RETURN_VALUE(accelData, HDF_ERR_INVALID_PARAM);
35
36 if (size > GRAVITY_AXIS_NUM * sizeof(int32_t)) {
37 HDF_LOGE("%s: size exceed max value", __func__);
38 return HDF_FAILURE;
39 }
40
41 g_accelRawData[GRAVITY_X_AXIS] = accelData[GRAVITY_X_AXIS];
42 g_accelRawData[GRAVITY_Y_AXIS] = accelData[GRAVITY_Y_AXIS];
43 g_accelRawData[GRAVITY_Z_AXIS] = accelData[GRAVITY_Z_AXIS];
44
45 return HDF_SUCCESS;
46 }
47
GravityDataWorkEntry(void * arg)48 static void GravityDataWorkEntry(void *arg)
49 {
50 struct GravityDrvData *drvData = NULL;
51 static int32_t GravityRawData[GRAVITY_AXIS_NUM];
52 struct SensorReportEvent event;
53 drvData = (struct GravityDrvData *)arg;
54 CHECK_NULL_PTR_RETURN(drvData);
55 OsalTimespec time;
56
57 GravityRawData[GRAVITY_X_AXIS] = (g_accelRawData[GRAVITY_X_AXIS] * GRAVITY_UNITS +
58 GRAVITY_FILTER_UNITS * (GravityRawData[GRAVITY_X_AXIS] - g_accelRawData[GRAVITY_X_AXIS])) / GRAVITY_UNITS;
59 GravityRawData[GRAVITY_Y_AXIS] = (g_accelRawData[GRAVITY_Y_AXIS] * GRAVITY_UNITS +
60 GRAVITY_FILTER_UNITS * (GravityRawData[GRAVITY_Y_AXIS] - g_accelRawData[GRAVITY_Y_AXIS])) / GRAVITY_UNITS;
61 GravityRawData[GRAVITY_Z_AXIS] = (g_accelRawData[GRAVITY_Z_AXIS] * GRAVITY_UNITS +
62 GRAVITY_FILTER_UNITS * (GravityRawData[GRAVITY_Z_AXIS] - g_accelRawData[GRAVITY_Z_AXIS])) / GRAVITY_UNITS;
63
64 if (OsalGetTime(&time) != HDF_SUCCESS) {
65 HDF_LOGE("%s: Get time failed", __func__);
66 return;
67 }
68 event.timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT;
69 event.sensorId = SENSOR_TAG_GRAVITY;
70 event.option = 0;
71 event.mode = SENSOR_WORK_MODE_REALTIME;
72 event.dataLen = sizeof(GravityRawData);
73 event.data = (uint8_t *)&GravityRawData;
74
75 if (ReportSensorEvent(&event) != HDF_SUCCESS) {
76 HDF_LOGE("%s: Report gravity data failed", __func__);
77 }
78 }
79
GravityTimerEntry(uintptr_t arg)80 static void GravityTimerEntry(uintptr_t arg)
81 {
82 int32_t ret;
83 struct GravityDrvData *drvData = (struct GravityDrvData *)arg;
84 CHECK_NULL_PTR_RETURN(drvData);
85
86 if (!HdfAddWork(&drvData->gravityWorkQueue, &drvData->gravityWork)) {
87 HDF_LOGE("%s: Gravity add work queue failed", __func__);
88 }
89
90 ret = OsalTimerSetTimeout(&drvData->gravityTimer, drvData->interval);
91 if (ret != HDF_SUCCESS) {
92 HDF_LOGE("%s: Gravity modify time failed", __func__);
93 }
94 }
95
InitGravityData(struct GravityDrvData * drvData)96 static int32_t InitGravityData(struct GravityDrvData *drvData)
97 {
98 if (HdfWorkQueueInit(&drvData->gravityWorkQueue, HDF_GRAVITY_WORK_QUEUE_NAME) != HDF_SUCCESS) {
99 HDF_LOGE("%s: Gravity init work queue failed", __func__);
100 return HDF_FAILURE;
101 }
102
103 if (HdfWorkInit(&drvData->gravityWork, GravityDataWorkEntry, drvData) != HDF_SUCCESS) {
104 HDF_LOGE("%s: Gravity create thread failed", __func__);
105 return HDF_FAILURE;
106 }
107
108 drvData->interval = GRAVITY_TIMER_MAX_TIME;
109 drvData->enable = false;
110
111 return HDF_SUCCESS;
112 }
113
SetGravityEnable(void)114 static int32_t SetGravityEnable(void)
115 {
116 int32_t ret;
117 struct GravityDrvData *drvData = GravityGetDrvData();
118
119 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
120 CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM);
121
122 if (drvData->enable) {
123 HDF_LOGE("%s: Gravity sensor is enabled", __func__);
124 return HDF_SUCCESS;
125 }
126
127 ret = OsalTimerCreate(&drvData->gravityTimer, GRAVITY_TIMER_MAX_TIME, GravityTimerEntry, (uintptr_t)drvData);
128 if (ret != HDF_SUCCESS) {
129 HDF_LOGE("%s: Gravity create timer failed[%d]", __func__, ret);
130 return ret;
131 }
132
133 ret = OsalTimerStartLoop(&drvData->gravityTimer);
134 if (ret != HDF_SUCCESS) {
135 HDF_LOGE("%s: Gravity start timer failed[%d]", __func__, ret);
136 return ret;
137 }
138 drvData->enable = true;
139
140 return HDF_SUCCESS;
141 }
142
SetGravityDisable(void)143 static int32_t SetGravityDisable(void)
144 {
145 int32_t ret;
146 struct GravityDrvData *drvData = GravityGetDrvData();
147
148 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
149 CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM);
150
151 if (!drvData->enable) {
152 HDF_LOGE("%s: Gravity sensor had disable", __func__);
153 return HDF_SUCCESS;
154 }
155
156 ret = OsalTimerDelete(&drvData->gravityTimer);
157 if (ret != HDF_SUCCESS) {
158 HDF_LOGE("%s: Gravity delete timer failed", __func__);
159 return ret;
160 }
161 drvData->enable = false;
162
163 return HDF_SUCCESS;
164 }
165
SetGravityBatch(int64_t samplingInterval,int64_t interval)166 static int32_t SetGravityBatch(int64_t samplingInterval, int64_t interval)
167 {
168 (void)interval;
169
170 int64_t ms;
171 struct GravityDrvData *drvData = NULL;
172
173 drvData = GravityGetDrvData();
174 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
175
176 ms = OsalDivS64(samplingInterval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT));
177
178 drvData->interval = (ms <= GRAVITY_TIMER_MIN_TIME) ? GRAVITY_TIMER_MIN_TIME : GRAVITY_TIMER_MAX_TIME;
179
180 return HDF_SUCCESS;
181 }
182
SetGravityMode(int32_t mode)183 static int32_t SetGravityMode(int32_t mode)
184 {
185 if (mode <= SENSOR_WORK_MODE_DEFAULT || mode >= SENSOR_WORK_MODE_MAX) {
186 HDF_LOGE("%s: The current mode is not supported", __func__);
187 return HDF_FAILURE;
188 }
189
190 return HDF_SUCCESS;
191 }
192
SetGravityOption(uint32_t option)193 static int32_t SetGravityOption(uint32_t option)
194 {
195 (void)option;
196 return HDF_SUCCESS;
197 }
198
DispatchGravity(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)199 static int32_t DispatchGravity(struct HdfDeviceIoClient *client,
200 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
201 {
202 (void)client;
203 (void)cmd;
204 (void)data;
205 (void)reply;
206
207 return HDF_SUCCESS;
208 }
209
GravityBindDriver(struct HdfDeviceObject * device)210 int32_t GravityBindDriver(struct HdfDeviceObject *device)
211 {
212 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
213
214 struct GravityDrvData *drvData = (struct GravityDrvData *)OsalMemCalloc(sizeof(*drvData));
215 if (drvData == NULL) {
216 HDF_LOGE("%s: Malloc gravity drv data fail!", __func__);
217 return HDF_ERR_MALLOC_FAIL;
218 }
219
220 drvData->ioService.Dispatch = DispatchGravity;
221 drvData->device = device;
222 device->service = &drvData->ioService;
223 g_gravityDrvData = drvData;
224 return HDF_SUCCESS;
225 }
226
InitGravityOps(struct SensorCfgData * config,struct SensorDeviceInfo * deviceInfo)227 static int32_t InitGravityOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo)
228 {
229 CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
230
231 deviceInfo->ops.Enable = SetGravityEnable;
232 deviceInfo->ops.Disable = SetGravityDisable;
233 deviceInfo->ops.SetBatch = SetGravityBatch;
234 deviceInfo->ops.SetMode = SetGravityMode;
235 deviceInfo->ops.SetOption = SetGravityOption;
236
237 if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo),
238 &config->sensorInfo, sizeof(config->sensorInfo)) != EOK) {
239 HDF_LOGE("%s: Copy sensor info failed", __func__);
240 return HDF_FAILURE;
241 }
242
243 if (SubscribeAccelDataCallbackFunc(GravitySubscribeAccelData) != HDF_SUCCESS) {
244 HDF_LOGE("%s: Gravity register accel failed", __func__);
245 return HDF_FAILURE;
246 }
247
248 return HDF_SUCCESS;
249 }
250
InitGravityAfterBase(struct SensorCfgData * config)251 static int32_t InitGravityAfterBase(struct SensorCfgData *config)
252 {
253 struct SensorDeviceInfo deviceInfo;
254 CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
255
256 if (InitGravityOps(config, &deviceInfo) != HDF_SUCCESS) {
257 HDF_LOGE("%s: Init gravity ops failed", __func__);
258 return HDF_FAILURE;
259 }
260
261 if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) {
262 HDF_LOGE("%s: Add gravity device failed", __func__);
263 return HDF_FAILURE;
264 }
265 return HDF_SUCCESS;
266 }
267
GravityCreateCfgData(const struct DeviceResourceNode * node)268 static int32_t GravityCreateCfgData(const struct DeviceResourceNode *node)
269 {
270 struct GravityDrvData *drvData = GravityGetDrvData();
271
272 CHECK_NULL_PTR_RETURN_VALUE(node, HDF_ERR_INVALID_PARAM);
273 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
274 CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM);
275
276 if (GetSensorBaseConfigData(node, drvData->gravityCfg) != HDF_SUCCESS) {
277 HDF_LOGE("%s: Get sensor base config failed", __func__);
278 return HDF_FAILURE;
279 }
280
281 if (InitGravityAfterBase(drvData->gravityCfg) != HDF_SUCCESS) {
282 HDF_LOGE("%s: Init gravity sensor device failed", __func__);
283 return HDF_FAILURE;
284 }
285 return HDF_SUCCESS;
286 }
287
GravityInitDriver(struct HdfDeviceObject * device)288 int32_t GravityInitDriver(struct HdfDeviceObject *device)
289 {
290 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
291 struct GravityDrvData *drvData = (struct GravityDrvData *)device->service;
292 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
293
294 if (InitGravityData(drvData) != HDF_SUCCESS) {
295 HDF_LOGE("%s: Init gravity config failed", __func__);
296 return HDF_FAILURE;
297 }
298
299 drvData->gravityCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->gravityCfg));
300 if (drvData->gravityCfg == NULL) {
301 HDF_LOGE("%s: Malloc gravity config data failed", __func__);
302 return HDF_FAILURE;
303 }
304
305 if (GravityCreateCfgData(device->property) != HDF_SUCCESS) {
306 HDF_LOGE("%s: create gravity device failed", __func__);
307 OsalMemFree(drvData->gravityCfg);
308 return HDF_FAILURE;
309 }
310 HDF_LOGI("%s: Init gravity driver success", __func__);
311 return HDF_SUCCESS;
312 }
313
GravityReleaseDriver(struct HdfDeviceObject * device)314 void GravityReleaseDriver(struct HdfDeviceObject *device)
315 {
316 CHECK_NULL_PTR_RETURN(device);
317
318 struct GravityDrvData *drvData = (struct GravityDrvData *)device->service;
319 CHECK_NULL_PTR_RETURN(drvData);
320
321 if (drvData->gravityCfg != NULL) {
322 (void)DeleteSensorDevice(&drvData->gravityCfg->sensorInfo);
323 OsalMemFree(drvData->gravityCfg);
324 drvData->gravityCfg = NULL;
325 }
326
327 HdfWorkDestroy(&drvData->gravityWork);
328 HdfWorkQueueDestroy(&drvData->gravityWorkQueue);
329 OsalMemFree(drvData);
330 }
331
332 struct HdfDriverEntry g_sensorGravityDevEntry = {
333 .moduleVersion = 1,
334 .moduleName = "HDF_SENSOR_GRAVITY",
335 .Bind = GravityBindDriver,
336 .Init = GravityInitDriver,
337 .Release = GravityReleaseDriver,
338 };
339
340 HDF_INIT(g_sensorGravityDevEntry);
341