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
35 static int32_t sensorStatusList[SENSOR_TYPE_MAX] = { 0 };
GetSensorStatus(void)36 int32_t *GetSensorStatus(void)
37 {
38 return sensorStatusList;
39 }
40
ReleaseAllSensorInfo(void)41 void 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
SetSensorIdClassification(void)58 static 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
GetSensorInfoFromReply(struct HdfSBuf * reply)92 static 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
139 ERROR:
140 ReleaseAllSensorInfo();
141 return SENSOR_FAILURE;
142 }
143
GetSensorNumByManagerType(struct HdfSBuf * reply)144 static 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
GetSensorInfo(struct SensorInformation ** sensor,int32_t * count)152 static 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
GetSensorServiceBySensorId(int32_t sensorId)213 static 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
SendSensorMsg(int32_t sensorId,struct HdfSBuf * msg,struct HdfSBuf * reply)226 static 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
EnableSensor(int32_t sensorId)243 static 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
DisableSensor(int32_t sensorId)276 static 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
SetSensorBatch(int32_t sensorId,int64_t samplingInterval,int64_t interval)309 static 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
SetSensorMode(int32_t sensorId,int32_t mode)346 static 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
SetSensorOption(int32_t sensorId,uint32_t option)372 static 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
GetSensorEvent(struct HdfSBuf * reply,struct SensorEvents * sensorEvent)398 static 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
ReadData(int32_t sensorId,struct SensorEvents * event)432 static 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 if (reply == NULL) {
440 HDF_LOGE("%s: Sensor write id fail!", __func__);
441 HdfSbufRecycle(msg);
442 return SENSOR_FAILURE;
443 }
444 CHECK_NULL_PTR_RETURN_VALUE(reply, SENSOR_NULL_PTR);
445
446 if (!HdfSbufWriteInt32(msg, sensorId)) {
447 HDF_LOGE("%{public}s: Sensor write id failed", __func__);
448 goto ERROR;
449 }
450
451 if (!HdfSbufWriteInt32(msg, SENSOR_OPS_IO_CMD_READ_DATA)) {
452 HDF_LOGE("%{public}s: Sensor write readData failed", __func__);
453 goto ERROR;
454 }
455
456 int32_t ret = SendSensorMsg(sensorId, msg, reply);
457 if (ret != SENSOR_SUCCESS) {
458 HDF_LOGE("%{public}s: Sensor read data failed, ret[%{public}d]", __func__, ret);
459 HdfSbufRecycle(reply);
460 HdfSbufRecycle(msg);
461 return ret;
462 }
463
464 ret = GetSensorEvent(reply, event);
465 if (ret != SENSOR_SUCCESS) {
466 HDF_LOGE("%{public}s: Sensor get data from reply failed", __func__);
467 goto ERROR;
468 }
469
470 HdfSbufRecycle(reply);
471 HdfSbufRecycle(msg);
472 return HDF_SUCCESS;
473
474 ERROR:
475 HdfSbufRecycle(reply);
476 HdfSbufRecycle(msg);
477 return HDF_FAILURE;
478 }
479
GetSdcSensorInfo(struct SdcSensorInfo sdcSensorInfo[])480 static int32_t GetSdcSensorInfo(struct SdcSensorInfo sdcSensorInfo[])
481 {
482 (void)sdcSensorInfo;
483 return HDF_SUCCESS;
484 }
485
GetSensorDeviceMethods(struct SensorInterface * device)486 void GetSensorDeviceMethods(struct SensorInterface *device)
487 {
488 CHECK_NULL_PTR_RETURN(device);
489 device->GetAllSensors = GetSensorInfo;
490 device->Enable = EnableSensor;
491 device->Disable = DisableSensor;
492 device->SetBatch = SetSensorBatch;
493 device->SetMode = SetSensorMode;
494 device->SetOption = SetSensorOption;
495 device->ReadData = ReadData;
496 device->Register = Register;
497 device->Unregister = Unregister;
498 device->GetSdcSensorInfo = GetSdcSensorInfo;
499 }
500