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_config_parser.h"
10 #include <securec.h>
11 #include "device_resource_if.h"
12 #include "osal_mem.h"
13 #include "sensor_platform_if.h"
14 
15 #define HDF_LOG_TAG    khdf_sensor_common_driver
16 
17 static char *g_sensorRegGroupName[SENSOR_GROUP_MAX] = {
18     "initSeqConfig",
19     "enableSeqConfig",
20     "disableSeqConfig",
21 };
22 
GetSensorRegGroupNameIndex(const char * name)23 static uint32_t GetSensorRegGroupNameIndex(const char *name)
24 {
25     uint32_t index;
26 
27     if (name == NULL) {
28         return SENSOR_GROUP_MAX;
29     }
30 
31     for (index = 0; index < SENSOR_GROUP_MAX; ++index) {
32         if ((g_sensorRegGroupName[index] != NULL) && (strcmp(name, g_sensorRegGroupName[index]) == 0)) {
33             break;
34         }
35     }
36 
37     return index;
38 }
39 
ReleaseSensorAllRegConfig(struct SensorCfgData * config)40 void ReleaseSensorAllRegConfig(struct SensorCfgData *config)
41 {
42     int32_t index;
43 
44     if (config == NULL || config->regCfgGroup == NULL) {
45         return;
46     }
47 
48     for (index = 0; index < SENSOR_GROUP_MAX; ++index) {
49         if (config->regCfgGroup[index] != NULL) {
50             if (config->regCfgGroup[index]->regCfgItem != NULL) {
51                 OsalMemFree(config->regCfgGroup[index]->regCfgItem);
52                 config->regCfgGroup[index]->regCfgItem = NULL;
53             }
54             OsalMemFree(config->regCfgGroup[index]);
55             config->regCfgGroup[index] = NULL;
56         }
57     }
58 }
59 
ParseSensorRegItem(struct DeviceResourceIface * parser,const struct DeviceResourceNode * regNode,const char * groupName,struct SensorRegCfgGroupNode * group)60 static int32_t ParseSensorRegItem(struct DeviceResourceIface *parser, const struct DeviceResourceNode *regNode,
61     const char *groupName, struct SensorRegCfgGroupNode *group)
62 {
63     int32_t ret;
64     int32_t step;
65     uint32_t index;
66     int32_t num;
67     uint32_t itemNum = group->itemNum;
68     uint16_t *buf = NULL;
69 
70     CHECK_NULL_PTR_RETURN_VALUE(group->regCfgItem, HDF_ERR_INVALID_PARAM);
71     CHECK_NULL_PTR_RETURN_VALUE(groupName, HDF_ERR_INVALID_PARAM);
72 
73     num = parser->GetElemNum(regNode, groupName);
74     if (num <= 0 || num > SENSOR_CONFIG_MAX_ITEM) {
75         HDF_LOGE("%s: parser %s element num failed", __func__, groupName);
76         return HDF_SUCCESS;
77     }
78 
79     buf = (uint16_t *)OsalMemCalloc(sizeof(uint16_t) * num);
80     CHECK_NULL_PTR_RETURN_VALUE(buf, HDF_ERR_MALLOC_FAIL);
81 
82     ret = parser->GetUint16Array(regNode, groupName, buf, num, 0);
83     if (ret != HDF_SUCCESS) {
84         HDF_LOGE("%s: parser %s reg array failed", __func__, groupName);
85         OsalMemFree(buf);
86         return HDF_SUCCESS;
87     }
88 
89     for (index = 0; index < itemNum; ++index) {
90         step = SENSOR_REG_CFG_INDEX_MAX * index;
91         if (step + SENSOR_REG_CFG_SAVE_INDEX >= num) {
92             break;
93         }
94         group->regCfgItem[index].regAddr = buf[step + SENSOR_REG_CFG_ADDR_INDEX];
95         group->regCfgItem[index].value = buf[step + SENSOR_REG_CFG_VALUE_INDEX];
96         group->regCfgItem[index].mask = buf[step + SENSOR_REG_CFG_MASK_INDEX];
97         group->regCfgItem[index].len = buf[step + SENSOR_REG_CFG_LEN_INDEX];
98         group->regCfgItem[index].delay = buf[step + SENSOR_REG_CFG_DELAY_INDEX];
99         group->regCfgItem[index].opsType = buf[step + SENSOR_REG_CFG_OPS_INDEX];
100         group->regCfgItem[index].calType = buf[step + SENSOR_REG_CFG_CAL_INDEX];
101         group->regCfgItem[index].shiftNum = buf[step + SENSOR_REG_CFG_SHIFT_INDEX];
102         group->regCfgItem[index].debug = buf[step + SENSOR_REG_CFG_DEBUG_INDEX];
103         group->regCfgItem[index].save = buf[step + SENSOR_REG_CFG_SAVE_INDEX];
104     }
105     OsalMemFree(buf);
106 
107     return HDF_SUCCESS;
108 }
109 
ParseSensorRegGroup(struct DeviceResourceIface * parser,const struct DeviceResourceNode * regCfgNode,const char * groupName,struct SensorRegCfgGroupNode ** groupNode)110 int32_t ParseSensorRegGroup(struct DeviceResourceIface *parser, const struct DeviceResourceNode *regCfgNode,
111     const char *groupName, struct SensorRegCfgGroupNode **groupNode)
112 {
113     int32_t num;
114     struct SensorRegCfgGroupNode *group = NULL;
115 
116     CHECK_NULL_PTR_RETURN_VALUE(parser, HDF_ERR_INVALID_PARAM);
117     CHECK_NULL_PTR_RETURN_VALUE(regCfgNode, HDF_ERR_INVALID_PARAM);
118     CHECK_NULL_PTR_RETURN_VALUE(groupName, HDF_ERR_INVALID_PARAM);
119     CHECK_NULL_PTR_RETURN_VALUE(groupNode, HDF_ERR_INVALID_PARAM);
120 
121     num = parser->GetElemNum(regCfgNode, groupName);
122     group = *groupNode;
123 
124     if (num > 0) {
125         if (group != NULL) {
126             if (group->regCfgItem != NULL) {
127                 OsalMemFree(group->regCfgItem);
128             }
129             OsalMemFree(group);
130         }
131 
132         group = (struct SensorRegCfgGroupNode*)OsalMemCalloc(sizeof(*group));
133         if (group == NULL) {
134             HDF_LOGE("%s: malloc sensor reg config group failed", __func__);
135             return HDF_ERR_MALLOC_FAIL;
136         }
137 
138         *groupNode = group;
139         group->itemNum = (uint32_t)(num / SENSOR_REG_CFG_INDEX_MAX);
140         group->itemNum = ((SENSOR_REG_CFG_INDEX_MAX * group->itemNum) < (uint32_t)num) ?
141             (group->itemNum + 1) : group->itemNum;
142 
143         group->regCfgItem = (struct SensorRegCfg*)OsalMemCalloc(group->itemNum * sizeof(*(group->regCfgItem)));
144         if (group->regCfgItem == NULL) {
145             HDF_LOGE("%s: malloc sensor reg config item failed", __func__);
146             return HDF_ERR_MALLOC_FAIL;
147         }
148 
149         if (ParseSensorRegItem(parser, regCfgNode, groupName, group) != HDF_SUCCESS) {
150             HDF_LOGE("%s: malloc sensor reg config item data failed", __func__);
151             return HDF_FAILURE;
152         }
153     }
154 
155     return HDF_SUCCESS;
156 }
157 
ParseSensorRegConfig(struct SensorCfgData * config)158 int32_t ParseSensorRegConfig(struct SensorCfgData *config)
159 {
160     uint32_t index;
161     const struct DeviceResourceNode *regCfgNode = NULL;
162     struct DeviceResourceIface *parser = NULL;
163     const struct DeviceResourceAttr *regAttr = NULL;
164 
165     CHECK_NULL_PTR_RETURN_VALUE(config->root, HDF_ERR_INVALID_PARAM);
166     parser = DeviceResourceGetIfaceInstance(HDF_CONFIG_SOURCE);
167     CHECK_NULL_PTR_RETURN_VALUE(parser, HDF_ERR_INVALID_PARAM);
168 
169     regCfgNode = parser->GetChildNode(config->root, "sensorRegConfig");
170     CHECK_NULL_PTR_RETURN_VALUE(regCfgNode, HDF_ERR_INVALID_PARAM);
171 
172     DEV_RES_NODE_FOR_EACH_ATTR(regCfgNode, regAttr) {
173         if (regAttr == NULL || regAttr->name == NULL) {
174             HDF_LOGE("%s:sensor reg node attr is null", __func__);
175             break;
176         }
177 
178         index = GetSensorRegGroupNameIndex(regAttr->name);
179         if (index >= SENSOR_GROUP_MAX) {
180             HDF_LOGE("%s: get sensor register group index failed", __func__);
181             goto error;
182         }
183 
184         if (ParseSensorRegGroup(parser, regCfgNode, regAttr->name, &config->regCfgGroup[index]) != HDF_SUCCESS) {
185             HDF_LOGE("%s: parse sensor register group failed", __func__);
186             goto error;
187         }
188     }
189     return HDF_SUCCESS;
190 
191 error:
192     ReleaseSensorAllRegConfig(config);
193     HDF_LOGE("%s: parse sensor reg config failed", __func__);
194     return HDF_FAILURE;
195 }
196 
GetSensorI2cHandle(struct SensorBusCfg * busCfg)197 static int32_t GetSensorI2cHandle(struct SensorBusCfg *busCfg)
198 {
199     CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_ERR_INVALID_PARAM);
200     int16_t busNum = busCfg->i2cCfg.busNum;
201     busCfg->i2cCfg.handle = I2cOpen(busNum);
202     if (busCfg->i2cCfg.handle == NULL) {
203         HDF_LOGE("%s: sensor i2c Handle invalid", __func__);
204         return HDF_FAILURE;
205     }
206 
207     return HDF_SUCCESS;
208 }
209 
210 #if defined(LOSCFG_DRIVERS_HDF_PLATFORM_SPI) || defined(CONFIG_DRIVERS_HDF_PLATFORM_SPI)
GetSensorSpiHandle(struct SensorBusCfg * busCfg)211 static int32_t GetSensorSpiHandle(struct SensorBusCfg *busCfg)
212 {
213     int32_t ret;
214     struct SpiDevInfo spiDevinfo;
215     CHECK_NULL_PTR_RETURN_VALUE	(busCfg, HDF_ERR_INVALID_PARAM);
216 
217     spiDevinfo.busNum = busCfg->spiCfg.busNum;
218     spiDevinfo.csNum = busCfg->spiCfg.csNum;
219     busCfg->spiCfg.handle = SpiOpen(&spiDevinfo);
220 
221     ret = SpiSetCfg(busCfg->spiCfg.handle, &busCfg->spiCfg.spi);
222     if (ret != HDF_SUCCESS) {
223         HDF_LOGE("%s: SpiSetCfg failed", __func__);
224         SpiClose(busCfg->spiCfg.handle);
225         return ret;
226     }
227 
228     return HDF_SUCCESS;
229 }
230 #endif // LOSCFG_DRIVERS_HDF_PLATFORM_SPI || CONFIG_DRIVERS_HDF_PLATFORM_SPI
231 
GetSensorBusHandle(struct SensorBusCfg * busCfg)232 int32_t GetSensorBusHandle(struct SensorBusCfg *busCfg)
233 {
234     CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_ERR_INVALID_PARAM);
235 
236     if (busCfg->busType == SENSOR_BUS_I2C) {
237         return GetSensorI2cHandle(busCfg);
238 #if defined(LOSCFG_DRIVERS_HDF_PLATFORM_SPI) || defined(CONFIG_DRIVERS_HDF_PLATFORM_SPI)
239     } else if (busCfg->busType == SENSOR_BUS_SPI) {
240         return GetSensorSpiHandle(busCfg);
241 #endif // LOSCFG_DRIVERS_HDF_PLATFORM_SPI || CONFIG_DRIVERS_HDF_PLATFORM_SPI
242     }
243 
244     return HDF_SUCCESS;
245 }
246 
ReleaseSensorBusHandle(struct SensorBusCfg * busCfg)247 int32_t ReleaseSensorBusHandle(struct SensorBusCfg *busCfg)
248 {
249     if (busCfg == NULL) {
250         return HDF_SUCCESS;
251     }
252 
253     if (busCfg->busType == SENSOR_BUS_I2C && busCfg->i2cCfg.handle != NULL) {
254         I2cClose(busCfg->i2cCfg.handle);
255         busCfg->i2cCfg.handle = NULL;
256 
257 #if defined(LOSCFG_DRIVERS_HDF_PLATFORM_SPI) || defined(CONFIG_DRIVERS_HDF_PLATFORM_SPI)
258     } else if (busCfg->busType == SENSOR_BUS_SPI) {
259         SpiClose(busCfg->spiCfg.handle);
260         busCfg->spiCfg.handle = NULL;
261 #endif
262     }
263 
264     return HDF_SUCCESS;
265 }
266 
DetectSensorDevice(struct SensorCfgData * config)267 int32_t DetectSensorDevice(struct SensorCfgData *config)
268 {
269     uint8_t value = 0;
270     uint16_t chipIdReg;
271     uint16_t chipIdValue;
272     int32_t ret;
273 
274     CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
275 
276     chipIdReg = config->sensorAttr.chipIdReg;
277     chipIdValue = config->sensorAttr.chipIdValue;
278 
279     ret = GetSensorBusHandle(&config->busCfg);
280     if (ret != HDF_SUCCESS) {
281         HDF_LOGE("%s: get sensor bus handle failed", __func__);
282         (void)ReleaseSensorBusHandle(&config->busCfg);
283         return HDF_FAILURE;
284     }
285 
286     ret = ReadSensor(&config->busCfg, chipIdReg, &value, sizeof(value));
287     if (ret != HDF_SUCCESS) {
288         HDF_LOGE("%s: i2c read chip id failed", __func__);
289         (void)ReleaseSensorBusHandle(&config->busCfg);
290         return HDF_FAILURE;
291     }
292 
293     if (value != chipIdValue) {
294         HDF_LOGE("%s: sensor chip[0x%x] id [0x%x] detect value[%hhu]", __func__, chipIdReg, chipIdValue, value);
295         (void)ReleaseSensorBusHandle(&config->busCfg);
296         return HDF_FAILURE;
297     }
298 
299     HDF_LOGD("%s: sensor [%s] detect chip success", __func__, config->sensorInfo.sensorName);
300     return HDF_SUCCESS;
301 }
302 
ParseSensorString(struct DeviceResourceIface * parser,const struct DeviceResourceNode * infoNode,struct SensorCfgData * config)303 static int32_t ParseSensorString(struct DeviceResourceIface *parser, const struct DeviceResourceNode *infoNode,
304     struct SensorCfgData *config)
305 {
306     const char *name = NULL;
307 
308     int32_t ret = parser->GetString(infoNode, "sensorName", &name, NULL);
309     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorName");
310     if (strcpy_s(config->sensorInfo.sensorName, SENSOR_INFO_NAME_MAX_LEN, name) != EOK) {
311         HDF_LOGE("%s:copy sensorName failed!", __func__);
312         return HDF_FAILURE;
313     }
314 
315     ret = parser->GetString(infoNode, "vendorName", &name, NULL);
316     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "vendorName");
317     if (strcpy_s(config->sensorInfo.vendorName, SENSOR_INFO_NAME_MAX_LEN, name) != EOK) {
318         HDF_LOGE("%s:copy vendorName failed!", __func__);
319         return HDF_FAILURE;
320     }
321 
322     ret = parser->GetString(infoNode, "firmwareVersion", &name, NULL);
323     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "firmwareVersion");
324     if (strcpy_s(config->sensorInfo.firmwareVersion, SENSOR_INFO_NAME_MAX_LEN, name) != EOK) {
325         HDF_LOGE("%s:copy firmwareVersion failed!", __func__);
326         return HDF_FAILURE;
327     }
328 
329     ret = parser->GetString(infoNode, "hardwareVersion", &name, NULL);
330     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "hardwareVersion");
331     if (strcpy_s(config->sensorInfo.hardwareVersion, SENSOR_INFO_NAME_MAX_LEN, name) != EOK) {
332         HDF_LOGE("%s:copy hardwareVersion failed!", __func__);
333         return HDF_FAILURE;
334     }
335 
336     return ret;
337 }
338 
ParseSensorValue(struct DeviceResourceIface * parser,const struct DeviceResourceNode * infoNode,struct SensorCfgData * config)339 static int32_t ParseSensorValue(struct DeviceResourceIface *parser, const struct DeviceResourceNode *infoNode,
340     struct SensorCfgData *config)
341 {
342     uint16_t id;
343     int32_t value;
344     int32_t ret = parser->GetUint16(infoNode, "sensorTypeId", &id, 0);
345     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorTypeId");
346     config->sensorInfo.sensorTypeId = id;
347     ret = parser->GetUint16(infoNode, "sensorId", &id, 0);
348     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorId");
349     config->sensorInfo.sensorId = id;
350 
351     ret = parser->GetUint32(infoNode, "maxRange", (uint32_t *)&value, 0);
352     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "maxRange");
353     config->sensorInfo.maxRange = value;
354     ret = parser->GetUint32(infoNode, "accuracy", (uint32_t *)&value, 0);
355     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "accuracy");
356     config->sensorInfo.accuracy = value;
357     ret = parser->GetUint32(infoNode, "power", (uint32_t *)&value, 0);
358     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "power");
359     config->sensorInfo.power = value;
360     ret = parser->GetUint64(infoNode, "minDelay", (uint64_t *)&value, 0);
361     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "minDelay");
362     config->sensorInfo.minDelay = value;
363     ret = parser->GetUint64(infoNode, "maxDelay", (uint64_t *)&value, 0);
364     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "maxDelay");
365     config->sensorInfo.maxDelay = value;
366 
367     return ret;
368 }
369 
ParseSensorInfo(struct DeviceResourceIface * parser,const struct DeviceResourceNode * infoNode,struct SensorCfgData * config)370 static int32_t ParseSensorInfo(struct DeviceResourceIface *parser, const struct DeviceResourceNode *infoNode,
371     struct SensorCfgData *config)
372 {
373     int32_t ret = ParseSensorString(parser, infoNode, config);
374     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "ParseSensorString");
375 
376     ret = ParseSensorValue(parser, infoNode, config);
377     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "ParseSensorValue");
378     return ret;
379 }
380 
ParseSensorSpiBus(struct DeviceResourceIface * parser,const struct DeviceResourceNode * spiBusNode,struct SensorBusCfg * busConfig)381 static int32_t ParseSensorSpiBus(struct DeviceResourceIface *parser, const struct DeviceResourceNode *spiBusNode,
382     struct SensorBusCfg *busConfig)
383 {
384     int32_t ret;
385 
386     ret = parser->GetUint32(spiBusNode, "maxSpeedHz", &busConfig->spiCfg.spi.maxSpeedHz, 0);
387     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "maxSpeedHz");
388     ret = parser->GetUint16(spiBusNode, "mode", &busConfig->spiCfg.spi.mode, 0);
389     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "mode");
390     ret = parser->GetUint8(spiBusNode, "transferMode", &busConfig->spiCfg.spi.transferMode, 0);
391     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "transferMode");
392     ret = parser->GetUint8(spiBusNode, "bitsPerWord", &busConfig->spiCfg.spi.bitsPerWord, 0);
393     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "bitsPerWord");
394 
395     return HDF_SUCCESS;
396 }
397 
ParseSensorBus(struct DeviceResourceIface * parser,const struct DeviceResourceNode * busNode,struct SensorCfgData * config)398 static int32_t ParseSensorBus(struct DeviceResourceIface *parser, const struct DeviceResourceNode *busNode,
399     struct SensorCfgData *config)
400 {
401     int32_t ret;
402     const struct DeviceResourceNode *spiBusNode = NULL;
403 
404     ret = parser->GetUint8(busNode, "busType", &config->busCfg.busType, 0);
405     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "busType");
406     ret = parser->GetUint8(busNode, "regBigEndian", &config->busCfg.regBigEndian, 0);
407     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "regBigEndian");
408 
409     if (config->busCfg.busType == SENSOR_BUS_I2C) {
410         ret = parser->GetUint16(busNode, "busNum", &config->busCfg.i2cCfg.busNum, 0);
411         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "busNum");
412         ret = parser->GetUint16(busNode, "busAddr", &config->busCfg.i2cCfg.devAddr, 0);
413         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "busAddr");
414         ret = parser->GetUint16(busNode, "regWidth", &config->busCfg.i2cCfg.regWidth, 0);
415         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "regWidth");
416     } else if (config->busCfg.busType == SENSOR_BUS_SPI) {
417         ret = parser->GetUint32(busNode, "busNum", &config->busCfg.spiCfg.busNum, 0);
418         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "busNum");
419         ret = parser->GetUint32(busNode, "busAddr", &config->busCfg.spiCfg.csNum, 0);
420         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "busAddr");
421         spiBusNode = parser->GetChildNode(busNode, "spiBusCfg");
422         if (spiBusNode != NULL) {
423             ret = ParseSensorSpiBus(parser, spiBusNode, &config->busCfg);
424             CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorBusConfig.spiBusCfg");
425         }
426     } else if (config->busCfg.busType == SENSOR_BUS_GPIO) {
427         ret = parser->GetUint32(busNode, "gpioIrq1", &config->busCfg.GpioNum[SENSOR_GPIO_NUM1], 0);
428         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "gpioIrq1");
429         ret = parser->GetUint32(busNode, "gpioIrq2", &config->busCfg.GpioNum[SENSOR_GPIO_NUM2], 0);
430         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "gpioIrq2");
431     }
432 
433     return HDF_SUCCESS;
434 }
435 
ParseSensorAttr(struct DeviceResourceIface * parser,const struct DeviceResourceNode * attrNode,struct SensorCfgData * config)436 static int32_t ParseSensorAttr(struct DeviceResourceIface *parser, const struct DeviceResourceNode *attrNode,
437     struct SensorCfgData *config)
438 {
439     int32_t ret;
440     ret = parser->GetString(attrNode, "chipName", &config->sensorAttr.chipName, NULL);
441     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "chipName");
442     ret = parser->GetUint16(attrNode, "chipIdRegister", &config->sensorAttr.chipIdReg, 0);
443     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "chipIdRegister");
444     ret = parser->GetUint16(attrNode, "chipIdValue", &config->sensorAttr.chipIdValue, 0);
445     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "chipIdValue");
446 
447     return ret;
448 }
449 
ReleaseSensorDirectionConfig(struct SensorCfgData * config)450 void ReleaseSensorDirectionConfig(struct SensorCfgData *config)
451 {
452     CHECK_NULL_PTR_RETURN(config);
453 
454     if (config->direction != NULL) {
455         OsalMemFree(config->direction);
456         config->direction = NULL;
457     }
458 }
459 
ParseSensorDirection(struct SensorCfgData * config)460 int32_t ParseSensorDirection(struct SensorCfgData *config)
461 {
462     int32_t num;
463     int32_t ret;
464     uint32_t index;
465     uint32_t *buf = NULL;
466     const struct DeviceResourceNode *directionNode = NULL;
467     struct DeviceResourceIface *parser = NULL;
468 
469     CHECK_NULL_PTR_RETURN_VALUE(config->root, HDF_ERR_INVALID_PARAM);
470     parser = DeviceResourceGetIfaceInstance(HDF_CONFIG_SOURCE);
471     CHECK_NULL_PTR_RETURN_VALUE(parser, HDF_ERR_INVALID_PARAM);
472 
473     directionNode = parser->GetChildNode(config->root, "sensorDirection");
474     CHECK_NULL_PTR_RETURN_VALUE(directionNode, HDF_ERR_INVALID_PARAM);
475 
476     num = parser->GetElemNum(directionNode, "convert");
477     ret = parser->GetUint32(directionNode, "direction", &index, 0);
478     CHECK_PARSER_RESULT_RETURN_VALUE(ret, "direction");
479     if ((num <= 0 || num > MAX_SENSOR_INDEX_NUM) || (index > num / AXIS_INDEX_MAX)) {
480         HDF_LOGE("%s: parser %d element num failed", __func__, num);
481         return HDF_FAILURE;
482     }
483 
484     buf = (uint32_t *)OsalMemCalloc(sizeof(uint32_t) * num);
485     CHECK_NULL_PTR_RETURN_VALUE(buf, HDF_ERR_MALLOC_FAIL);
486 
487     ret = parser->GetUint32Array(directionNode, "convert", buf, num, 0);
488     if (ret != HDF_SUCCESS) {
489         HDF_LOGE("%s: parser %s convert failed", __func__, "convert");
490         OsalMemFree(buf);
491         return HDF_FAILURE;
492     }
493 
494     config->direction = (struct SensorDirection*)OsalMemCalloc(sizeof(struct SensorDirection));
495     if (config->direction == NULL) {
496         HDF_LOGE("%s: malloc sensor direction config item failed", __func__);
497         OsalMemFree(buf);
498         return HDF_ERR_MALLOC_FAIL;
499     }
500 
501     index = index * AXIS_INDEX_MAX;
502     config->direction->sign[AXIS_X] = buf[index + SIGN_X_INDEX];
503     config->direction->sign[AXIS_Y] = buf[index + SIGN_Y_INDEX];
504     config->direction->sign[AXIS_Z] = buf[index + SIGN_Z_INDEX];
505     config->direction->map[AXIS_X] = buf[index + AXIS_X_INDEX];
506     config->direction->map[AXIS_Y] = buf[index + AXIS_Y_INDEX];
507     config->direction->map[AXIS_Z] = buf[index + AXIS_Z_INDEX];
508 
509     OsalMemFree(buf);
510     return HDF_SUCCESS;
511 }
512 
SensorRawDataToRemapData(struct SensorDirection * direction,int32_t * remapData,uint32_t num)513 int32_t SensorRawDataToRemapData(struct SensorDirection *direction, int32_t *remapData, uint32_t num)
514 {
515     uint32_t axis;
516     int32_t directionSign[MAX_SENSOR_AXIS_NUM];
517     int32_t newData[MAX_SENSOR_AXIS_NUM];
518 
519     CHECK_NULL_PTR_RETURN_VALUE(direction, HDF_ERR_INVALID_PARAM);
520     CHECK_NULL_PTR_RETURN_VALUE(remapData, HDF_ERR_INVALID_PARAM);
521     if (num != MAX_SENSOR_AXIS_NUM) {
522         HDF_LOGE("%s: afferent num failed", __func__);
523         return HDF_FAILURE;
524     }
525 
526     for (axis = 0; axis < num; axis++) {
527         if (direction->sign[axis] == 0) {
528             directionSign[axis] = 1;
529         } else {
530             directionSign[axis] = -1;
531         }
532     }
533 
534     newData[direction->map[AXIS_X]] = directionSign[AXIS_X] * remapData[AXIS_X];
535     newData[direction->map[AXIS_Y]] = directionSign[AXIS_Y] * remapData[AXIS_Y];
536     newData[direction->map[AXIS_Z]] = directionSign[AXIS_Z] * remapData[AXIS_Z];
537 
538     remapData[AXIS_X] = newData[direction->map[AXIS_X]];
539     remapData[AXIS_Y] = newData[direction->map[AXIS_Y]];
540     remapData[AXIS_Z] = newData[direction->map[AXIS_Z]];
541 
542     return HDF_SUCCESS;
543 }
544 
GetSensorBaseConfigData(const struct DeviceResourceNode * node,struct SensorCfgData * config)545 int32_t GetSensorBaseConfigData(const struct DeviceResourceNode *node, struct SensorCfgData *config)
546 {
547     int32_t ret;
548     struct DeviceResourceIface *parser = NULL;
549     const struct DeviceResourceNode *infoNode = NULL;
550     const struct DeviceResourceNode *busNode = NULL;
551     const struct DeviceResourceNode *attrNode = NULL;
552 
553     CHECK_NULL_PTR_RETURN_VALUE(node, HDF_ERR_INVALID_PARAM);
554     CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
555 
556     parser = DeviceResourceGetIfaceInstance(HDF_CONFIG_SOURCE);
557     CHECK_NULL_PTR_RETURN_VALUE(parser, HDF_ERR_INVALID_PARAM);
558 
559     config->root = node;
560     CHECK_NULL_PTR_RETURN_VALUE(parser->GetChildNode, HDF_ERR_INVALID_PARAM);
561 
562     infoNode = parser->GetChildNode(node, "sensorInfo");
563     if (infoNode != NULL) {
564         ret = ParseSensorInfo(parser, infoNode, config);
565         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorInfo");
566     }
567 
568     busNode = parser->GetChildNode(node, "sensorBusConfig");
569     if (busNode != NULL) {
570         ret = ParseSensorBus(parser, busNode, config);
571         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorBusConfig");
572     }
573 
574     attrNode = parser->GetChildNode(node, "sensorIdAttr");
575     if (attrNode != NULL) {
576         ret = ParseSensorAttr(parser, attrNode, config);
577         CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorIdAttr");
578     }
579 
580     return HDF_SUCCESS;
581 }
582