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