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 sensor_magnetic_driver_c
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
280 return NULL;
281 }
282
283 if (drvData->detectFlag) {
284 HDF_LOGE("%s: Magnetic sensor have detected", __func__);
285
286 return NULL;
287 }
288
289 if (drvData->magneticCfg == NULL) {
290 HDF_LOGE("%s: Magnetic magneticCfg pointer NULL", __func__);
291
292 return NULL;
293 }
294
295 if (GetSensorBaseConfigData(node, drvData->magneticCfg) != HDF_SUCCESS) {
296 HDF_LOGE("%s: Get sensor base config failed", __func__);
297 goto BASE_CONFIG_EXIT;
298 }
299
300 if (DetectSensorDevice(drvData->magneticCfg) != HDF_SUCCESS) {
301 HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__);
302 drvData->detectFlag = false;
303 goto BASE_CONFIG_EXIT;
304 }
305
306 drvData->detectFlag = true;
307 if (InitMagneticAfterDetected(drvData->magneticCfg) != HDF_SUCCESS) {
308 HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__);
309
310 goto INIT_EXIT;
311 }
312 return drvData->magneticCfg;
313
314 INIT_EXIT:
315 (void)ReleaseSensorBusHandle(&drvData->magneticCfg->busCfg);
316 BASE_CONFIG_EXIT:
317 drvData->magneticCfg->root = NULL;
318 (void)memset_s(&drvData->magneticCfg->sensorInfo,
319 sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo));
320 (void)memset_s(&drvData->magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg));
321 (void)memset_s(&drvData->magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr));
322
323 return drvData->magneticCfg;
324 }
325
MagneticReleaseCfgData(struct SensorCfgData * magneticCfg)326 void MagneticReleaseCfgData(struct SensorCfgData *magneticCfg)
327 {
328 CHECK_NULL_PTR_RETURN(magneticCfg);
329
330 (void)DeleteSensorDevice(&magneticCfg->sensorInfo);
331 ReleaseSensorAllRegConfig(magneticCfg);
332 (void)ReleaseSensorBusHandle(&magneticCfg->busCfg);
333 ReleaseSensorDirectionConfig(magneticCfg);
334
335 magneticCfg->root = NULL;
336 (void)memset_s(&magneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo));
337 (void)memset_s(&magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg));
338 (void)memset_s(&magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr));
339 }
340
MagneticInitDriver(struct HdfDeviceObject * device)341 int32_t MagneticInitDriver(struct HdfDeviceObject *device)
342 {
343 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
344 struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service;
345 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
346
347 if (InitMagneticData(drvData) != HDF_SUCCESS) {
348 HDF_LOGE("%s: Init magnetic config failed", __func__);
349 return HDF_FAILURE;
350 }
351
352 drvData->magneticCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->magneticCfg));
353 if (drvData->magneticCfg == NULL) {
354 HDF_LOGE("%s: Malloc magnetic config data failed", __func__);
355 return HDF_FAILURE;
356 }
357
358 drvData->magneticCfg->regCfgGroup = &g_regCfgGroup[0];
359
360 HDF_LOGI("%s: Init magnetic driver success", __func__);
361 return HDF_SUCCESS;
362 }
363
MagneticReleaseDriver(struct HdfDeviceObject * device)364 void MagneticReleaseDriver(struct HdfDeviceObject *device)
365 {
366 CHECK_NULL_PTR_RETURN(device);
367
368 struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service;
369 CHECK_NULL_PTR_RETURN(drvData);
370
371 if (drvData->detectFlag && drvData->magneticCfg != NULL) {
372 MagneticReleaseCfgData(drvData->magneticCfg);
373 }
374
375 OsalMemFree(drvData->magneticCfg);
376 drvData->magneticCfg = NULL;
377
378 HdfWorkDestroy(&drvData->magneticWork);
379 HdfWorkQueueDestroy(&drvData->magneticWorkQueue);
380 OsalMemFree(drvData);
381 }
382
383 struct HdfDriverEntry g_sensorMagneticDevEntry = {
384 .moduleVersion = 1,
385 .moduleName = "HDF_SENSOR_MAGNETIC",
386 .Bind = MagneticBindDriver,
387 .Init = MagneticInitDriver,
388 .Release = MagneticReleaseDriver,
389 };
390
391 HDF_INIT(g_sensorMagneticDevEntry);