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_device_manager.h"
10 #include <securec.h>
11 #include "asm/io.h"
12 #include "osal_mem.h"
13 #include "sensor_platform_if.h"
14 
15 #define HDF_LOG_TAG    khdf_sensor_common_driver
16 
17 #define HDF_SENSOR_INFO_MAX_BUF (4 * 1024) // 4kB for all sensor info
18 #define HDF_SENSOR_EVENT_MAX_BUF (4 * 1024) // 4kB
19 
20 struct SensorDevMgrData *g_sensorDeviceManager = NULL;
21 
GetSensorDeviceManager(void)22 static struct SensorDevMgrData *GetSensorDeviceManager(void)
23 {
24     return g_sensorDeviceManager;
25 }
26 
AddSensorDevice(const struct SensorDeviceInfo * deviceInfo)27 int32_t AddSensorDevice(const struct SensorDeviceInfo *deviceInfo)
28 {
29     struct SensorDevInfoNode *pos = NULL;
30     struct SensorDevInfoNode *devInfoNode = NULL;
31     struct SensorDevMgrData *manager = GetSensorDeviceManager();
32 
33     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
34     CHECK_NULL_PTR_RETURN_VALUE(manager, HDF_ERR_INVALID_PARAM);
35 
36     DLIST_FOR_EACH_ENTRY(pos, &manager->sensorDevInfoHead, struct SensorDevInfoNode, node) {
37         if ((deviceInfo->sensorInfo.sensorId == pos->devInfo.sensorInfo.sensorId) &&
38             (strcmp(deviceInfo->sensorInfo.sensorName, pos->devInfo.sensorInfo.sensorName) == 0)) {
39             HDF_LOGE("%s: sensor id[%d] had existed", __func__, deviceInfo->sensorInfo.sensorId);
40             return HDF_FAILURE;
41         }
42     }
43 
44     (void)OsalMutexLock(&manager->mutex);
45     devInfoNode = (struct SensorDevInfoNode*)OsalMemCalloc(sizeof(*devInfoNode));
46     if (devInfoNode == NULL) {
47         (void)OsalMutexUnlock(&manager->mutex);
48         return HDF_FAILURE;
49     }
50     if (memcpy_s(&devInfoNode->devInfo, sizeof(devInfoNode->devInfo),
51         (void *)deviceInfo, sizeof(*deviceInfo)) != EOK) {
52         HDF_LOGE("%s: copy sensor info failed", __func__);
53         OsalMemFree(devInfoNode);
54         (void)OsalMutexUnlock(&manager->mutex);
55         return HDF_FAILURE;
56     }
57     DListInsertTail(&devInfoNode->node, &manager->sensorDevInfoHead);
58     (void)OsalMutexUnlock(&manager->mutex);
59     HDF_LOGI("%s: register sensor name[%s] success", __func__, deviceInfo->sensorInfo.sensorName);
60 
61     return HDF_SUCCESS;
62 }
63 
DeleteSensorDevice(const struct SensorBasicInfo * sensorBaseInfo)64 int32_t DeleteSensorDevice(const struct SensorBasicInfo *sensorBaseInfo)
65 {
66     struct SensorDevInfoNode *pos = NULL;
67     struct SensorDevInfoNode *tmp = NULL;
68     struct SensorDevMgrData *manager = GetSensorDeviceManager();
69 
70     CHECK_NULL_PTR_RETURN_VALUE(sensorBaseInfo, HDF_ERR_INVALID_PARAM);
71     CHECK_NULL_PTR_RETURN_VALUE(manager, HDF_ERR_INVALID_PARAM);
72 
73     (void)OsalMutexLock(&manager->mutex);
74     DLIST_FOR_EACH_ENTRY_SAFE(pos, tmp, &manager->sensorDevInfoHead, struct SensorDevInfoNode, node) {
75         if ((sensorBaseInfo->sensorId == pos->devInfo.sensorInfo.sensorId) &&
76             (strcmp(sensorBaseInfo->sensorName, pos->devInfo.sensorInfo.sensorName) == 0)) {
77             DListRemove(&pos->node);
78             OsalMemFree(pos);
79             (void)OsalMutexUnlock(&manager->mutex);
80             return HDF_SUCCESS;
81         }
82     }
83     (void)OsalMutexUnlock(&manager->mutex);
84     HDF_LOGE("%s: delete sensor id invalid para", __func__);
85     return HDF_FAILURE;
86 }
87 
ReportSensorEvent(const struct SensorReportEvent * events)88 int32_t ReportSensorEvent(const struct SensorReportEvent *events)
89 {
90     int32_t ret;
91     struct SensorDevMgrData *manager = NULL;
92     struct HdfSBuf *msg = NULL;
93 
94     CHECK_NULL_PTR_RETURN_VALUE(events, HDF_ERR_INVALID_PARAM);
95 
96     manager = GetSensorDeviceManager();
97     CHECK_NULL_PTR_RETURN_VALUE(manager, HDF_ERR_INVALID_PARAM);
98 
99     (void)OsalMutexLock(&manager->eventMutex);
100     msg = HdfSbufObtain(HDF_SENSOR_EVENT_MAX_BUF);
101     if (msg == NULL) {
102         (void)OsalMutexUnlock(&manager->eventMutex);
103         return HDF_ERR_INVALID_PARAM;
104     }
105 
106     if (!HdfSbufWriteBuffer(msg, events, sizeof(*events))) {
107         HDF_LOGE("%s: sbuf write event failed", __func__);
108         ret = HDF_FAILURE;
109         goto EXIT;
110     }
111 
112     if (!HdfSbufWriteBuffer(msg, events->data, events->dataLen)) {
113         HDF_LOGE("%s: sbuf write event data failed", __func__);
114         ret = HDF_FAILURE;
115         goto EXIT;
116     }
117 
118     if (HdfDeviceSendEvent(manager->device, 0, msg) != HDF_SUCCESS) {
119         HDF_LOGE("%s: send sensor data event failed", __func__);
120         ret = HDF_FAILURE;
121         goto EXIT;
122     }
123     ret = HDF_SUCCESS;
124 
125 EXIT:
126     HdfSbufRecycle(msg);
127     (void)OsalMutexUnlock(&manager->eventMutex);
128     return ret;
129 }
130 
GetAllSensorInfo(struct HdfSBuf * data,struct HdfSBuf * reply)131 static int32_t GetAllSensorInfo(struct HdfSBuf *data, struct HdfSBuf *reply)
132 {
133     int32_t count = 0;
134     struct SensorDevInfoNode *pos = NULL;
135     struct SensorBasicInfo *sensorInfo = NULL;
136     struct SensorDevMgrData *manager = NULL;
137 
138     (void)data;
139 
140     manager = GetSensorDeviceManager();
141 
142     CHECK_NULL_PTR_RETURN_VALUE(reply, HDF_ERR_INVALID_PARAM);
143     CHECK_NULL_PTR_RETURN_VALUE(manager, HDF_ERR_INVALID_PARAM);
144 
145     DLIST_FOR_EACH_ENTRY(pos, &manager->sensorDevInfoHead, struct SensorDevInfoNode, node) {
146         sensorInfo = &(pos->devInfo.sensorInfo);
147         if (!HdfSbufWriteBuffer(reply, sensorInfo, sizeof(*sensorInfo))) {
148             HDF_LOGE("%s: write sbuf failed", __func__);
149             return HDF_FAILURE;
150         }
151 
152         count++;
153         if ((count + 1) * sizeof(*sensorInfo) > HDF_SENSOR_INFO_MAX_BUF) {
154             HDF_LOGE("%s: write sbuf exceed max buf, sensor count[%d]", __func__, count);
155             break;
156         }
157     }
158 
159     return HDF_SUCCESS;
160 }
161 
Enable(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)162 static int32_t Enable(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
163 {
164     (void)data;
165     (void)reply;
166     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
167     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo->ops.Enable, HDF_ERR_INVALID_PARAM);
168     return deviceInfo->ops.Enable();
169 }
170 
Disable(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)171 static int32_t Disable(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
172 {
173     (void)data;
174     (void)reply;
175     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
176     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo->ops.Disable, HDF_ERR_INVALID_PARAM);
177 
178     return deviceInfo->ops.Disable();
179 }
180 
SetBatch(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)181 static int32_t SetBatch(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
182 {
183     int64_t samplingInterval;
184     int64_t reportInterval;
185     (void)reply;
186 
187     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
188     CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
189     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo->ops.SetBatch, HDF_ERR_INVALID_PARAM);
190 
191     if (!HdfSbufReadInt64(data, &samplingInterval) || !HdfSbufReadInt64(data, &reportInterval)) {
192         HDF_LOGE("%s: sbuf read interval failed", __func__);
193         return HDF_FAILURE;
194     }
195 
196     return deviceInfo->ops.SetBatch(samplingInterval, reportInterval);
197 }
198 
SetMode(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)199 static int32_t SetMode(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
200 {
201     int32_t mode;
202     (void)reply;
203 
204     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
205     CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
206     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo->ops.SetMode, HDF_ERR_INVALID_PARAM);
207 
208     if (!HdfSbufReadInt32(data, &mode)) {
209         HDF_LOGE("%s: sbuf read mode failed", __func__);
210         return HDF_FAILURE;
211     }
212 
213     return deviceInfo->ops.SetMode(mode);
214 }
215 
SetOption(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)216 static int32_t SetOption(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
217 {
218     uint32_t option;
219     (void)reply;
220 
221     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
222     CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
223     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo->ops.SetOption, HDF_ERR_INVALID_PARAM);
224 
225     if (!HdfSbufReadUint32(data, &option)) {
226         HDF_LOGE("%s: sbuf read option failed", __func__);
227         return HDF_FAILURE;
228     }
229 
230     return deviceInfo->ops.SetOption(option);
231 }
232 
ReadData(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)233 static int32_t ReadData(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
234 {
235     struct SensorReportEvent events;
236 
237     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo, HDF_ERR_INVALID_PARAM);
238     CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
239     CHECK_NULL_PTR_RETURN_VALUE(reply, HDF_ERR_INVALID_PARAM);
240     CHECK_NULL_PTR_RETURN_VALUE(deviceInfo->ops.ReadSensorData, HDF_ERR_INVALID_PARAM);
241     int32_t ret = deviceInfo->ops.ReadSensorData(&events);
242     if (ret != HDF_SUCCESS) {
243         HDF_LOGE("%s: ReadSensorData failed", __func__);
244         return HDF_FAILURE;
245     }
246 
247     if (!HdfSbufWriteBuffer(reply, &events, sizeof(events))) {
248         HDF_LOGE("%s: sbuf write event failed", __func__);
249         return HDF_FAILURE;
250     }
251 
252     if (!HdfSbufWriteBuffer(reply, events.data, events.dataLen)) {
253         HDF_LOGE("%s: sbuf write event data failed", __func__);
254         return HDF_FAILURE;
255     }
256 
257     return HDF_SUCCESS;
258 }
259 
260 static struct SensorCmdHandleList g_sensorCmdHandle[] = {
261     {SENSOR_OPS_CMD_ENABLE, Enable},        // SENSOR_CMD_ENABLE
262     {SENSOR_OPS_CMD_DISABLE, Disable},      // SENSOR_CMD_DISABLE
263     {SENSOR_OPS_CMD_SET_BATCH, SetBatch},   // SENSOR_CMD_SET_BATCH
264     {SENSOR_OPS_CMD_SET_MODE, SetMode},     // SENSOR_CMD_SET_MODE
265     {SENSOR_OPS_CMD_SET_OPTION, SetOption}, // SENSOR_CMD_SET_OPTION
266     {SENSOR_OPS_CMD_READ_DATA, ReadData}    // SENSOR_CMD_READ_DATA
267 };
268 
DispatchCmdHandle(struct SensorDeviceInfo * deviceInfo,struct HdfSBuf * data,struct HdfSBuf * reply)269 static int32_t DispatchCmdHandle(struct SensorDeviceInfo *deviceInfo, struct HdfSBuf *data, struct HdfSBuf *reply)
270 {
271     int32_t opsCmd;
272     int32_t loop;
273     int32_t count;
274 
275     CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
276 
277     if (!HdfSbufReadInt32(data, &opsCmd)) {
278         HDF_LOGE("%s: sbuf read opsCmd failed", __func__);
279         return HDF_FAILURE;
280     }
281 
282     if ((opsCmd >= SENSOR_OPS_CMD_BUTT) || (opsCmd < SENSOR_OPS_CMD_ENABLE)) {
283         HDF_LOGE("%s: invalid cmd = %d", __func__, opsCmd);
284         return HDF_FAILURE;
285     }
286 
287     count = sizeof(g_sensorCmdHandle) / sizeof(g_sensorCmdHandle[0]);
288     for (loop = 0; loop < count; ++loop) {
289         if ((opsCmd == g_sensorCmdHandle[loop].cmd) && (g_sensorCmdHandle[loop].func != NULL)) {
290             return g_sensorCmdHandle[loop].func(deviceInfo, data, reply);
291         }
292     }
293 
294     return HDF_FAILURE;
295 }
296 
DispatchSensor(struct HdfDeviceIoClient * client,int32_t cmd,struct HdfSBuf * data,struct HdfSBuf * reply)297 static int32_t DispatchSensor(struct HdfDeviceIoClient *client,
298     int32_t cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
299 {
300     struct SensorDevMgrData *manager = GetSensorDeviceManager();
301     struct SensorDevInfoNode *pos = NULL;
302     int32_t sensorId;
303     int32_t ret;
304 
305     CHECK_NULL_PTR_RETURN_VALUE(manager, HDF_ERR_INVALID_PARAM);
306     CHECK_NULL_PTR_RETURN_VALUE(client, HDF_ERR_INVALID_PARAM);
307 
308     if (cmd >= SENSOR_CMD_END) {
309         HDF_LOGE("%s: sensor cmd invalid para", __func__);
310         return HDF_ERR_INVALID_PARAM;
311     }
312 
313     if (cmd == SENSOR_CMD_GET_INFO_LIST) {
314         return GetAllSensorInfo(data, reply);
315     }
316 
317     (void)OsalMutexLock(&manager->mutex);
318     if (!HdfSbufReadInt32(data, &sensorId)) {
319         HDF_LOGE("%s: sbuf read sensorId failed", __func__);
320         (void)OsalMutexUnlock(&manager->mutex);
321         return HDF_ERR_INVALID_PARAM;
322     }
323 
324     DLIST_FOR_EACH_ENTRY(pos, &manager->sensorDevInfoHead, struct SensorDevInfoNode, node) {
325         if (sensorId == pos->devInfo.sensorInfo.sensorId) {
326             ret = DispatchCmdHandle(&pos->devInfo, data, reply);
327             (void)OsalMutexUnlock(&manager->mutex);
328             return ret;
329         }
330     }
331     (void)OsalMutexUnlock(&manager->mutex);
332 
333     HDF_LOGE("%s: not find sensor[%d] handle function", __func__, sensorId);
334     return HDF_FAILURE;
335 }
336 
BindSensorDevManager(struct HdfDeviceObject * device)337 int32_t BindSensorDevManager(struct HdfDeviceObject *device)
338 {
339     struct SensorDevMgrData *manager = NULL;
340 
341     CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
342 
343     manager = (struct SensorDevMgrData *)OsalMemCalloc(sizeof(*manager));
344     if (manager == NULL) {
345         HDF_LOGE("%s: malloc manager fail!", __func__);
346         return HDF_ERR_MALLOC_FAIL;
347     }
348 
349     manager->ioService.Dispatch = DispatchSensor;
350     manager->device = device;
351     device->service = &manager->ioService;
352     g_sensorDeviceManager = manager;
353 
354     return HDF_SUCCESS;
355 }
356 
InitSensorDevManager(struct HdfDeviceObject * device)357 int32_t InitSensorDevManager(struct HdfDeviceObject *device)
358 {
359     struct SensorDevMgrData *manager = NULL;
360 
361     CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
362     manager = (struct SensorDevMgrData *)device->service;
363     CHECK_NULL_PTR_RETURN_VALUE(manager, HDF_ERR_INVALID_PARAM);
364 
365     DListHeadInit(&manager->sensorDevInfoHead);
366     if (OsalMutexInit(&manager->mutex) != HDF_SUCCESS) {
367         HDF_LOGE("%s: init mutex failed", __func__);
368         return HDF_FAILURE;
369     }
370 
371     if (OsalMutexInit(&manager->eventMutex) != HDF_SUCCESS) {
372         HDF_LOGE("%s: init eventMutex failed", __func__);
373         return HDF_FAILURE;
374     }
375 
376     if (!HdfDeviceSetClass(device, DEVICE_CLASS_SENSOR)) {
377         HDF_LOGE("%s: init sensor set class failed", __func__);
378         return HDF_FAILURE;
379     }
380 
381     HDF_LOGI("%s: init sensor manager successfully", __func__);
382     return HDF_SUCCESS;
383 }
384 
ReleaseSensorDevManager(struct HdfDeviceObject * device)385 void ReleaseSensorDevManager(struct HdfDeviceObject *device)
386 {
387     struct SensorDevInfoNode *pos = NULL;
388     struct SensorDevInfoNode *tmp = NULL;
389     struct SensorDevMgrData *manager = NULL;
390 
391     CHECK_NULL_PTR_RETURN(device);
392 
393     manager = (struct SensorDevMgrData *)device->service;
394     CHECK_NULL_PTR_RETURN(manager);
395 
396     DLIST_FOR_EACH_ENTRY_SAFE(pos, tmp, &manager->sensorDevInfoHead, struct SensorDevInfoNode, node) {
397         DListRemove(&pos->node);
398         OsalMemFree(pos);
399     }
400 
401     OsalMutexDestroy(&manager->mutex);
402     OsalMutexDestroy(&manager->eventMutex);
403     OsalMemFree(manager);
404     g_sensorDeviceManager = NULL;
405 }
406 
407 struct HdfDriverEntry g_sensorDevManagerEntry = {
408     .moduleVersion = 1,
409     .moduleName = "HDF_SENSOR_MGR_AP",
410     .Bind = BindSensorDevManager,
411     .Init = InitSensorDevManager,
412     .Release = ReleaseSensorDevManager,
413 };
414 
415 HDF_INIT(g_sensorDevManagerEntry);
416