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