1/*
2 * Copyright (c) 2021-2023 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 *     http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16#include "sensor_controller.h"
17#include <fcntl.h>
18#include <inttypes.h>
19#include <securec.h>
20#include "osal_mem.h"
21#include "osal_mutex.h"
22#include "sensor_channel.h"
23#include "sensor_common.h"
24#include "sensor_dump.h"
25#include "sensor_if.h"
26#include "sensor_manager.h"
27
28#define HDF_LOG_TAG    uhdf_sensor_service
29
30#define HDF_SENSOR_INFO_MAX_SIZE (4 * 1024) // 4kB
31#define SENSOR_STATUS_ENABLE 1
32#define SENSOR_STATUS_DISENABLE 0
33#define HDF_SENSOR_EVENT_MAX_SIZE (4 * 1024) // 4kB
34
35static int32_t sensorStatusList[SENSOR_TYPE_MAX] = { 0 };
36int32_t *GetSensorStatus(void)
37{
38    return sensorStatusList;
39}
40
41void ReleaseAllSensorInfo(void)
42{
43    struct SensorDevManager *manager = GetSensorDevManager();
44    struct SensorIdListNode *pos = NULL;
45    struct SensorIdListNode *tmp = NULL;
46
47    DLIST_FOR_EACH_ENTRY_SAFE(pos, tmp, &manager->sensorIdListHead, struct SensorIdListNode, node) {
48        DListRemove(&(pos->node));
49        OsalMemFree(pos);
50    }
51    if (manager->sensorInfoEntry != NULL) {
52        OsalMemFree(manager->sensorInfoEntry);
53        manager->sensorInfoEntry = NULL;
54    }
55    manager->sensorSum = 0;
56}
57
58static int32_t SetSensorIdClassification(void)
59{
60    struct SensorDevManager *manager = GetSensorDevManager();
61    struct SensorManagerNode *pos = NULL;
62    int32_t begin = 0;
63    int32_t end;
64    struct SensorIdListNode *sensorIdNode = NULL;
65    CHECK_NULL_PTR_RETURN_VALUE(manager->sensorInfoEntry, SENSOR_NULL_PTR);
66
67    DLIST_FOR_EACH_ENTRY(pos, &manager->managerHead, struct SensorManagerNode, node) {
68        end = begin + pos->sensorCount;
69        if (end > manager->sensorSum) {
70            break;
71        }
72
73        for (int i = begin; i < end; i++) {
74            sensorIdNode = (struct SensorIdListNode*)OsalMemCalloc(sizeof(*sensorIdNode));
75            CHECK_NULL_PTR_RETURN_VALUE(sensorIdNode, SENSOR_NULL_PTR);
76            sensorIdNode->ioService = pos->ioService;
77            sensorIdNode->sensorId = manager->sensorInfoEntry[i].sensorId;
78            SetSensorIdBySensorType(manager->sensorInfoEntry[i].sensorTypeId, manager->sensorInfoEntry[i].sensorId);
79            if (sensorIdNode->sensorId == SENSOR_TYPE_ACCELEROMETER) {
80                manager->sensorInfoEntry[i].maxRange = manager->sensorInfoEntry[i].maxRange * HDI_SENSOR_GRAVITY;
81                manager->sensorInfoEntry[i].accuracy = HDI_SENSOR_GRAVITY / HDI_SENSOR_ACCEL_LSB / HDI_SENSOR_UNITS;
82                manager->sensorInfoEntry[i].power = manager->sensorInfoEntry[i].power / HDI_SENSOR_UNITS;
83            }
84            DListInsertTail(&sensorIdNode->node, &manager->sensorIdListHead);
85        }
86        begin = end;
87    }
88
89    return HDF_SUCCESS;
90}
91
92static int32_t GetSensorInfoFromReply(struct HdfSBuf *reply)
93{
94    struct SensorDevManager *manager = GetSensorDevManager();
95    struct SensorInformation *pos = NULL;
96    struct SensorBasicInformation *buf = NULL;
97    int32_t count = manager->sensorSum;
98    uint32_t len;
99
100    if (manager->sensorInfoEntry != NULL) {
101        OsalMemFree(manager->sensorInfoEntry);
102        manager->sensorInfoEntry = NULL;
103    }
104
105    manager->sensorInfoEntry = (struct SensorInformation *)OsalMemCalloc(sizeof(*manager->sensorInfoEntry) * count);
106    if (manager->sensorInfoEntry == NULL) {
107        HDF_LOGE("%{public}s: Sensor info malloc failed", __func__);
108        return SENSOR_FAILURE;
109    }
110
111    pos = manager->sensorInfoEntry;
112    size_t preLen = sizeof(*manager->sensorInfoEntry) -
113        (sizeof(pos->maxRange) + sizeof(pos->accuracy) + sizeof(pos->power));
114
115    for (int32_t i = 0; i < count; i++) {
116        if (!HdfSbufReadBuffer(reply, (const void **)&buf, &len) || buf == NULL) {
117            HDF_LOGE("%{public}s: Sensor read reply info failed", __func__);
118            break;
119        }
120
121        if (memcpy_s(pos, sizeof(*pos), (void *)buf, preLen) != EOK) {
122            HDF_LOGE("%{public}s: Sensor copy reply info failed", __func__);
123            goto ERROR;
124        }
125        pos->maxRange = (float)(buf->maxRange);
126        pos->accuracy = (float)(buf->accuracy);
127        pos->power = (float)(buf->power);
128        pos->minDelay = (int64_t)(buf->minDelay);
129        pos->maxDelay = (int64_t)(buf->maxDelay);
130        pos++;
131    }
132
133    if (SetSensorIdClassification() != SENSOR_SUCCESS) {
134        HDF_LOGE("%{public}s: Sensor id Classification failed", __func__);
135        goto ERROR;
136    }
137    return SENSOR_SUCCESS;
138
139ERROR:
140    ReleaseAllSensorInfo();
141    return SENSOR_FAILURE;
142}
143
144static int32_t GetSensorNumByManagerType(struct HdfSBuf *reply)
145{
146    struct SensorDevManager *manager = GetSensorDevManager();
147    int32_t count = (int32_t)(HdfSbufGetDataSize(reply) / sizeof(struct SensorBasicInformation));
148
149    return ((count > manager->sensorSum) ? (count - manager->sensorSum) : 0);
150}
151
152static int32_t GetSensorInfo(struct SensorInformation **sensor, int32_t *count)
153{
154    struct SensorDevManager *manager = GetSensorDevManager();
155    CHECK_NULL_PTR_RETURN_VALUE(sensor, SENSOR_NULL_PTR);
156    CHECK_NULL_PTR_RETURN_VALUE(count, SENSOR_NULL_PTR);
157
158    if (manager->sensorSum > 0) {
159        *count = manager->sensorSum;
160        *sensor = manager->sensorInfoEntry;
161        return SENSOR_SUCCESS;
162    }
163
164    struct HdfSBuf *reply = HdfSbufObtain(HDF_SENSOR_INFO_MAX_SIZE);
165    CHECK_NULL_PTR_RETURN_VALUE(reply, SENSOR_NULL_PTR);
166
167    (void)OsalMutexLock(&manager->mutex);
168    manager->sensorSum = 0;
169    struct SensorManagerNode *pos = NULL;
170    DLIST_FOR_EACH_ENTRY(pos, &manager->managerHead, struct SensorManagerNode, node) {
171        if (manager->sensorSum >= SENSOR_TYPE_MAX) {
172            break;
173        }
174        pos->sensorCount = 0;
175        if ((pos->ioService == NULL) || (pos->ioService->dispatcher == NULL) ||
176            (pos->ioService->dispatcher->Dispatch == NULL)) {
177            HDF_LOGE("%{public}s: Sensor pos para failed", __func__);
178            continue;
179        }
180
181        int32_t ret = pos->ioService->dispatcher->Dispatch(&pos->ioService->object,
182            SENSOR_IO_CMD_GET_INFO_LIST, NULL, reply);
183        if (ret != SENSOR_SUCCESS) {
184            HDF_LOGE("%{public}s: Sensor dispatch info failed[%{public}d]", __func__, ret);
185            break;
186        }
187
188        pos->sensorCount = GetSensorNumByManagerType(reply);
189        manager->sensorSum += pos->sensorCount;
190    }
191
192    if (manager->sensorSum == 0) {
193        HdfSbufRecycle(reply);
194        (void)OsalMutexUnlock(&manager->mutex);
195        return SENSOR_FAILURE;
196    }
197
198    if (GetSensorInfoFromReply(reply) != SENSOR_SUCCESS) {
199        HdfSbufRecycle(reply);
200        (void)OsalMutexUnlock(&manager->mutex);
201        HDF_LOGE("%{public}s: Sensor get info from reply failed", __func__);
202        return SENSOR_FAILURE;
203    }
204
205    HdfSbufRecycle(reply);
206    (void)OsalMutexUnlock(&manager->mutex);
207
208    *count = manager->sensorSum;
209    *sensor = manager->sensorInfoEntry;
210    return SENSOR_SUCCESS;
211}
212
213static struct HdfIoService *GetSensorServiceBySensorId(int32_t sensorId)
214{
215    struct SensorDevManager *manager = GetSensorDevManager();
216    struct SensorIdListNode *sensorIdPos = NULL;
217
218    DLIST_FOR_EACH_ENTRY(sensorIdPos, &manager->sensorIdListHead, struct SensorIdListNode, node) {
219        if (sensorIdPos->sensorId == sensorId) {
220            return sensorIdPos->ioService;
221        }
222    }
223    return NULL;
224}
225
226static int32_t SendSensorMsg(int32_t sensorId, struct HdfSBuf *msg, struct HdfSBuf *reply)
227{
228    struct HdfIoService *ioService = NULL;
229
230    ioService = GetSensorServiceBySensorId(sensorId);
231    CHECK_NULL_PTR_RETURN_VALUE(ioService, SENSOR_NOT_SUPPORT);
232    CHECK_NULL_PTR_RETURN_VALUE(ioService->dispatcher, SENSOR_NULL_PTR);
233    CHECK_NULL_PTR_RETURN_VALUE(ioService->dispatcher->Dispatch, SENSOR_NULL_PTR);
234
235    int32_t ret = ioService->dispatcher->Dispatch(&ioService->object, SENSOR_IO_CMD_OPS, msg, reply);
236    if (ret != SENSOR_SUCCESS) {
237        HDF_LOGE("%{public}s: Sensor dispatch failed", __func__);
238        return ret;
239    }
240    return SENSOR_SUCCESS;
241}
242
243static int32_t EnableSensor(int32_t sensorId)
244{
245    struct HdfSBuf *msg = HdfSbufObtainDefaultSize();
246    if (msg == NULL) {
247        HDF_LOGE("%{public}s: Failed to obtain sBuf size", __func__);
248        return SENSOR_FAILURE;
249    }
250
251    if (!HdfSbufWriteInt32(msg, sensorId)) {
252        HDF_LOGE("%{public}s: Sensor write id failed", __func__);
253        HdfSbufRecycle(msg);
254        return SENSOR_FAILURE;
255    }
256
257    if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_ENABLE)) {
258        HDF_LOGE("%{public}s: Sensor write enable failed", __func__);
259        HdfSbufRecycle(msg);
260        return SENSOR_FAILURE;
261    }
262
263    int32_t ret = SendSensorMsg(sensorId, msg, NULL);
264    if (ret != SENSOR_SUCCESS) {
265        HDF_LOGE("%{public}s: Sensor enable failed, ret[%{public}d]", __func__, ret);
266    }
267    HdfSbufRecycle(msg);
268
269    if (sensorId < SENSOR_TYPE_MAX && sensorId >= 0) {
270        sensorStatusList[sensorId] = SENSOR_STATUS_ENABLE;
271    }
272
273    return ret;
274}
275
276static int32_t DisableSensor(int32_t sensorId)
277{
278    struct HdfSBuf *msg = HdfSbufObtainDefaultSize();
279    if (msg == NULL) {
280        HDF_LOGE("%{public}s: Failed to obtain sBuf", __func__);
281        return SENSOR_FAILURE;
282    }
283
284    if (!HdfSbufWriteInt32(msg, sensorId)) {
285        HDF_LOGE("%{public}s: Sensor write id failed", __func__);
286        HdfSbufRecycle(msg);
287        return SENSOR_FAILURE;
288    }
289
290    if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_DISABLE)) {
291        HDF_LOGE("%{public}s: Sensor write disable failed", __func__);
292        HdfSbufRecycle(msg);
293        return SENSOR_FAILURE;
294    }
295
296    int32_t ret = SendSensorMsg(sensorId, msg, NULL);
297    if (ret != SENSOR_SUCCESS) {
298        HDF_LOGE("%{public}s: Sensor disable failed, ret[%{public}d]", __func__, ret);
299    }
300    HdfSbufRecycle(msg);
301
302    if (sensorId < SENSOR_TYPE_MAX && sensorId >= 0) {
303        sensorStatusList[sensorId] = SENSOR_STATUS_DISENABLE;
304    }
305
306    return ret;
307}
308
309static int32_t SetSensorBatch(int32_t sensorId, int64_t samplingInterval, int64_t interval)
310{
311    if (interval < 0) {
312        HDF_LOGE("%{public}s: invalid param , interval is [%{public}" PRId64 "]", __func__, interval);
313        return SENSOR_INVALID_PARAM;
314    }
315
316    if (samplingInterval < 0) {
317        HDF_LOGE("%{public}s: invalid param , samplingInterval is [%{public}" PRId64 "]", __func__, samplingInterval);
318        return SENSOR_INVALID_PARAM;
319    }
320
321    struct HdfSBuf *msg = HdfSbufObtainDefaultSize();
322    CHECK_NULL_PTR_RETURN_VALUE(msg, SENSOR_FAILURE);
323
324    if (!HdfSbufWriteInt32(msg, sensorId)) {
325        HDF_LOGE("%{public}s: Sensor write id failed", __func__);
326        HdfSbufRecycle(msg);
327        return SENSOR_FAILURE;
328    }
329
330    if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_SET_BATCH) || !HdfSbufWriteInt64(msg, samplingInterval) ||
331        !HdfSbufWriteInt64(msg, interval)) {
332        HDF_LOGE("%{public}s: Sensor write failed", __func__);
333        HdfSbufRecycle(msg);
334        return SENSOR_FAILURE;
335    }
336
337    int32_t ret = SendSensorMsg(sensorId, msg, NULL);
338    if (ret != SENSOR_SUCCESS) {
339        HDF_LOGE("%{public}s: Sensor set batch failed, ret[%{public}d]", __func__, ret);
340    }
341    HdfSbufRecycle(msg);
342
343    return ret;
344}
345
346static int32_t SetSensorMode(int32_t sensorId, int32_t mode)
347{
348    struct HdfSBuf *msg = HdfSbufObtainDefaultSize();
349    CHECK_NULL_PTR_RETURN_VALUE(msg, SENSOR_FAILURE);
350
351    if (!HdfSbufWriteInt32(msg, sensorId)) {
352        HDF_LOGE("%{public}s: Sensor write id failed", __func__);
353        HdfSbufRecycle(msg);
354        return SENSOR_FAILURE;
355    }
356
357    if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_SET_MODE) || !HdfSbufWriteInt32(msg, mode)) {
358        HDF_LOGE("%{public}s: Sensor write failed", __func__);
359        HdfSbufRecycle(msg);
360        return SENSOR_FAILURE;
361    }
362
363    int32_t ret = SendSensorMsg(sensorId, msg, NULL);
364    if (ret != SENSOR_SUCCESS) {
365        HDF_LOGE("%{public}s: Sensor set mode failed, ret[%{public}d]", __func__, ret);
366    }
367    HdfSbufRecycle(msg);
368
369    return ret;
370}
371
372static int32_t SetSensorOption(int32_t sensorId, uint32_t option)
373{
374    struct HdfSBuf *msg = HdfSbufObtainDefaultSize();
375    CHECK_NULL_PTR_RETURN_VALUE(msg, SENSOR_FAILURE);
376
377    if (!HdfSbufWriteInt32(msg, sensorId)) {
378        HDF_LOGE("%{public}s: Sensor write id failed", __func__);
379        HdfSbufRecycle(msg);
380        return SENSOR_FAILURE;
381    }
382
383    if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_SET_OPTION) || !HdfSbufWriteUint32(msg, option)) {
384        HDF_LOGE("%{public}s: Sensor write failed", __func__);
385        HdfSbufRecycle(msg);
386        return SENSOR_FAILURE;
387    }
388
389    int32_t ret = SendSensorMsg(sensorId, msg, NULL);
390    if (ret != SENSOR_SUCCESS) {
391        HDF_LOGE("%{public}s: Sensor set option failed, ret[%{public}d]", __func__, ret);
392    }
393    HdfSbufRecycle(msg);
394
395    return ret;
396}
397
398static int32_t GetSensorEvent(struct HdfSBuf *reply, struct SensorEvents *sensorEvent)
399{
400    struct SensorEvents *events = NULL;
401    uint8_t *buf = NULL;
402    uint32_t len = 0;
403    uint32_t length = 0;
404
405    if (!HdfSbufReadBuffer(reply, (const void **)&events, &len) || sensorEvent == NULL) {
406        HDF_LOGE("%{public}s: Read sensor event fail!", __func__);
407        return SENSOR_FAILURE;
408    }
409
410    if (!HdfSbufReadBuffer(reply, (const void **)&buf, &len) || buf == NULL) {
411        HDF_LOGE("%{public}s: Read sensor data fail!", __func__);
412        return SENSOR_FAILURE;
413    }
414
415    length = sensorEvent->dataLen > len ? len : sensorEvent->dataLen;
416    if (memcpy_s(sensorEvent->data, sensorEvent->dataLen, buf, length) != EOK) {
417        HDF_LOGE("%{public}s: sensor memcpy data fail!", __func__);
418        return SENSOR_FAILURE;
419    }
420
421    sensorEvent->dataLen = length;
422    sensorEvent->sensorId = events->sensorId;
423    sensorEvent->version = events->version;
424    sensorEvent->timestamp = events->timestamp;
425    sensorEvent->option = events->option;
426    sensorEvent->mode = events->mode;
427
428    ConvertSensorData(sensorEvent);
429    return SENSOR_SUCCESS;
430}
431
432static int32_t ReadData(int32_t sensorId, struct SensorEvents *event)
433{
434    CHECK_NULL_PTR_RETURN_VALUE(event, SENSOR_NULL_PTR);
435    CHECK_NULL_PTR_RETURN_VALUE(event->data, SENSOR_NULL_PTR);
436    struct HdfSBuf *msg = HdfSbufObtainDefaultSize();
437    CHECK_NULL_PTR_RETURN_VALUE(msg, SENSOR_NULL_PTR);
438    struct HdfSBuf *reply = HdfSbufObtain(HDF_SENSOR_EVENT_MAX_SIZE);
439    CHECK_NULL_PTR_RETURN_VALUE(reply, SENSOR_NULL_PTR);
440
441    if (!HdfSbufWriteInt32(msg, sensorId)) {
442        HDF_LOGE("%{public}s: Sensor write id failed", __func__);
443        goto ERROR;
444    }
445
446    if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_READ_DATA)) {
447        HDF_LOGE("%{public}s: Sensor write readData failed", __func__);
448        goto ERROR;
449    }
450
451    int32_t ret = SendSensorMsg(sensorId, msg, reply);
452    if (ret != SENSOR_SUCCESS) {
453        HDF_LOGE("%{public}s: Sensor read data failed, ret[%{public}d]", __func__, ret);
454        HdfSbufRecycle(reply);
455        HdfSbufRecycle(msg);
456        return ret;
457    }
458
459    ret = GetSensorEvent(reply, event);
460    if (ret != SENSOR_SUCCESS) {
461        HDF_LOGE("%{public}s: Sensor get data from reply failed", __func__);
462        goto ERROR;
463    }
464
465    HdfSbufRecycle(reply);
466    HdfSbufRecycle(msg);
467    return HDF_SUCCESS;
468
469ERROR:
470    HdfSbufRecycle(reply);
471    HdfSbufRecycle(msg);
472    return HDF_FAILURE;
473}
474
475static int32_t GetSdcSensorInfo(struct SdcSensorInfo sdcSensorInfo[])
476{
477    (void)sdcSensorInfo;
478    return HDF_SUCCESS;
479}
480
481void GetSensorDeviceMethods(struct SensorInterface *device)
482{
483    CHECK_NULL_PTR_RETURN(device);
484    device->GetAllSensors = GetSensorInfo;
485    device->Enable = EnableSensor;
486    device->Disable = DisableSensor;
487    device->SetBatch = SetSensorBatch;
488    device->SetMode = SetSensorMode;
489    device->SetOption = SetSensorOption;
490    device->ReadData = ReadData;
491    device->Register = Register;
492    device->Unregister = Unregister;
493    device->GetSdcSensorInfo = GetSdcSensorInfo;
494}
495