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_controller.h"
10 #include <securec.h>
11 #include "osal_mem.h"
12 #include "osal_time.h"
13 #include "sensor_platform_if.h"
14
15 #define HDF_LOG_TAG hdf_sensor_commom
16
SensorOpsNop(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)17 static int32_t SensorOpsNop(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
18 {
19 (void)busCfg;
20 (void)cfgItem;
21 return HDF_SUCCESS;
22 }
23
SensorOpsRead(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)24 static int32_t SensorOpsRead(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
25 {
26 uint16_t value = 0;
27 int32_t ret;
28
29 ret = ReadSensor(busCfg, cfgItem->regAddr, (uint8_t *)&value, sizeof(value));
30 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read i2c reg");
31
32 return value;
33 }
34
GetSensorRegRealValueMask(struct SensorRegCfg * cfgItem,uint32_t * value,uint32_t busMask)35 static uint32_t GetSensorRegRealValueMask(struct SensorRegCfg *cfgItem, uint32_t *value, uint32_t busMask)
36 {
37 uint32_t mask = cfgItem->mask & busMask;
38 *value = cfgItem->value & busMask;
39
40 if (cfgItem->shiftNum != 0) {
41 if (cfgItem->calType == SENSOR_CFG_CALC_TYPE_RIGHT_SHIFT) {
42 *value = *value >> cfgItem->shiftNum;
43 mask = mask >> cfgItem->shiftNum;
44 } else {
45 *value = *value << cfgItem->shiftNum;
46 mask = mask << cfgItem->shiftNum;
47 }
48 }
49
50 return mask;
51 }
52
SensorOpsWrite(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)53 static int32_t SensorOpsWrite(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
54 {
55 uint8_t value[SENSOR_VALUE_BUTT];
56 int32_t ret;
57 uint32_t originValue;
58 uint32_t busMask;
59 uint32_t mask;
60
61 busMask = (busCfg->i2cCfg.regWidth == SENSOR_ADDR_WIDTH_1_BYTE) ? 0x00ff : 0xffff;
62 mask = GetSensorRegRealValueMask(cfgItem, &originValue, busMask);
63
64 value[SENSOR_ADDR_INDEX] = cfgItem->regAddr;
65 value[SENSOR_VALUE_INDEX] = originValue & mask;
66
67 ret = WriteSensor(busCfg, value, sizeof(value));
68 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "write i2c reg");
69
70 return ret;
71 }
72
SensorOpsReadCheck(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)73 static int32_t SensorOpsReadCheck(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
74 {
75 uint32_t value = 0;
76 uint32_t originValue;
77 uint32_t mask;
78 uint32_t busMask = 0xffff;
79 int32_t ret;
80
81 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
82
83 if (busCfg->busType == SENSOR_BUS_I2C) {
84 ret = ReadSensor(busCfg, cfgItem->regAddr, (uint8_t *)&value, sizeof(value));
85 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read i2c reg");
86 busMask = (busCfg->i2cCfg.regWidth == SENSOR_ADDR_WIDTH_1_BYTE) ? 0x00ff : 0xffff;
87 }
88
89 mask = GetSensorRegRealValueMask(cfgItem, &originValue, busMask);
90 if ((value & mask) != (originValue & mask)) {
91 return HDF_FAILURE;
92 }
93
94 return HDF_SUCCESS;
95 }
96
SensorBitwiseCalculate(struct SensorRegCfg * cfgItem,uint32_t * value,uint32_t valueMask)97 static int32_t SensorBitwiseCalculate(struct SensorRegCfg *cfgItem, uint32_t *value, uint32_t valueMask)
98 {
99 uint32_t originValue;
100 uint32_t mask;
101 uint32_t tmp;
102
103 mask = GetSensorRegRealValueMask(cfgItem, &originValue, valueMask);
104 switch ((enum SensorCalculateType)cfgItem->calType) {
105 case SENSOR_CFG_CALC_TYPE_NONE:
106 break;
107 case SENSOR_CFG_CALC_TYPE_SET:
108 *value &= ~mask;
109 *value |= (originValue & mask);
110 break;
111 case SENSOR_CFG_CALC_TYPE_REVERT:
112 tmp = *value & (~mask);
113 *value = ~(*value & mask);
114 *value = tmp | (*value & mask);
115 break;
116 case SENSOR_CFG_CALC_TYPE_XOR:
117 tmp = *value & (~mask);
118 originValue = originValue & mask;
119 *value = *value & mask;
120 *value ^= originValue;
121 *value = tmp | (*value);
122 break;
123 default:
124 HDF_LOGE("%s: unsupported cal type", __func__);
125 break;
126 }
127
128 return 0;
129 }
130
SensorOpsUpdateBitwise(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)131 static int32_t SensorOpsUpdateBitwise(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
132 {
133 uint32_t value = 0;
134 uint32_t busMask = 0x000000ff;
135 int32_t ret;
136 uint8_t valueArray[SENSOR_VALUE_BUTT];
137
138 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
139
140 if (busCfg->busType == SENSOR_BUS_I2C) {
141 ret = ReadSensor(busCfg, cfgItem->regAddr, (uint8_t *)&value, busCfg->i2cCfg.regWidth);
142 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read i2c reg");
143 busMask = (busCfg->i2cCfg.regWidth == SENSOR_ADDR_WIDTH_1_BYTE) ? 0x000000ff : 0x0000ffff;
144 }
145
146 ret = SensorBitwiseCalculate(cfgItem, &value, busMask);
147 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "update bitwise failed");
148
149 valueArray[SENSOR_ADDR_INDEX] = cfgItem->regAddr;
150 valueArray[SENSOR_VALUE_INDEX] = (uint8_t)value;
151
152 ret = WriteSensor(busCfg, valueArray, sizeof(valueArray));
153 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "update bitewise write failed");
154
155 return HDF_SUCCESS;
156 }
157
SensorOpsExtBuffRead(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)158 static int32_t SensorOpsExtBuffRead(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
159 {
160 int32_t ret;
161
162 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
163 CHECK_NULL_PTR_RETURN_VALUE(cfgItem, HDF_FAILURE);
164 CHECK_NULL_PTR_RETURN_VALUE(cfgItem->buff, HDF_FAILURE);
165
166 if (busCfg->busType == SENSOR_BUS_I2C) {
167 ret = ReadSensor(busCfg, cfgItem->regAddr, cfgItem->buff, cfgItem->len);
168 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read i2c reg");
169 }
170
171 return HDF_SUCCESS;
172 }
173
SensorOpsExtBuffWrite(struct SensorBusCfg * busCfg,struct SensorRegCfg * cfgItem)174 static int32_t SensorOpsExtBuffWrite(struct SensorBusCfg *busCfg, struct SensorRegCfg *cfgItem)
175 {
176 int32_t ret;
177 uint8_t *value = NULL;
178
179 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
180 CHECK_NULL_PTR_RETURN_VALUE(cfgItem, HDF_FAILURE);
181
182 if (cfgItem->len > LENGTH_NUMBER) {
183 return HDF_FAILURE;
184 }
185 value = (uint8_t *)OsalMemCalloc(sizeof(uint8_t) * (cfgItem->len + 1));
186 CHECK_NULL_PTR_RETURN_VALUE(value, HDF_FAILURE);
187
188 value[SENSOR_ADDR_INDEX] = cfgItem->regAddr;
189 if (memcpy_s(&value[SENSOR_VALUE_INDEX], cfgItem->len, cfgItem->buff, cfgItem->len) != EOK) {
190 HDF_LOGE("%s: Cpy value failed", __func__);
191 OsalMemFree(value);
192 return HDF_FAILURE;
193 }
194
195 if (busCfg->busType == SENSOR_BUS_I2C) {
196 ret = WriteSensor(busCfg, value, cfgItem->len);
197 if (ret != HDF_SUCCESS) {
198 HDF_LOGE("%s: write ext register failed", __func__);
199 OsalMemFree(value);
200 return HDF_FAILURE;
201 }
202 }
203 OsalMemFree(value);
204 return HDF_SUCCESS;
205 }
206
207 static struct SensorOpsCall g_doOpsCall[] = {
208 { SENSOR_OPS_TYPE_NOP, SensorOpsNop },
209 { SENSOR_OPS_TYPE_READ, SensorOpsRead },
210 { SENSOR_OPS_TYPE_WRITE, SensorOpsWrite },
211 { SENSOR_OPS_TYPE_READ_CHECK, SensorOpsReadCheck },
212 { SENSOR_OPS_TYPE_UPDATE_BITWISE, SensorOpsUpdateBitwise },
213 { SENSOR_OPS_TYPE_EXTBUFF_READ, SensorOpsExtBuffRead },
214 { SENSOR_OPS_TYPE_EXTBUFF_WRITE, SensorOpsExtBuffWrite },
215 };
216
SetSensorRegCfgArray(struct SensorBusCfg * busCfg,const struct SensorRegCfgGroupNode * group)217 int32_t SetSensorRegCfgArray(struct SensorBusCfg *busCfg, const struct SensorRegCfgGroupNode *group)
218 {
219 int32_t num = 0;
220 uint32_t count;
221 struct SensorRegCfg *cfgItem = NULL;
222
223 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
224
225 if (group == NULL) {
226 HDF_LOGI("%s: Pointer group is null", __func__);
227 return HDF_SUCCESS;
228 }
229
230 CHECK_NULL_PTR_RETURN_VALUE(group->regCfgItem, HDF_FAILURE);
231
232 count = sizeof(g_doOpsCall) / sizeof(g_doOpsCall[0]);
233
234 while (num < group->itemNum) {
235 cfgItem = (group->regCfgItem + num);
236 if (cfgItem->opsType >= count) {
237 HDF_LOGE("%s: cfg item para invalid", __func__);
238 break;
239 }
240 if ((g_doOpsCall[cfgItem->opsType].ops != NULL) && (cfgItem->opsType <= SENSOR_OPS_TYPE_UPDATE_BITWISE)) {
241 if (g_doOpsCall[cfgItem->opsType].ops(busCfg, cfgItem) != HDF_SUCCESS) {
242 HDF_LOGE("%s: malloc sensor reg config item data failed", __func__);
243 return HDF_FAILURE;
244 }
245 }
246 if (cfgItem->delay != 0) {
247 OsalMDelay(cfgItem->delay);
248 }
249 num++;
250 }
251
252 return HDF_SUCCESS;
253 }
254
SetSensorRegCfgArrayByBuff(struct SensorBusCfg * busCfg,const struct SensorRegCfgGroupNode * group,uint8_t * buff,int16_t len)255 int32_t SetSensorRegCfgArrayByBuff(struct SensorBusCfg *busCfg, const struct SensorRegCfgGroupNode *group,
256 uint8_t *buff, int16_t len)
257 {
258 int32_t num = 0;
259 uint32_t count;
260 uint8_t buffOffset = 0;
261 struct SensorRegCfg *cfgItem = NULL;
262
263 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
264 CHECK_NULL_PTR_RETURN_VALUE(group, HDF_FAILURE);
265 CHECK_NULL_PTR_RETURN_VALUE(group->regCfgItem, HDF_FAILURE);
266 CHECK_NULL_PTR_RETURN_VALUE(buff, HDF_FAILURE);
267
268 count = sizeof(g_doOpsCall) / sizeof(g_doOpsCall[0]);
269
270 while (num < group->itemNum) {
271 cfgItem = (group->regCfgItem + num);
272 if (cfgItem->opsType >= count) {
273 HDF_LOGE("%s: cfg item para invalid", __func__);
274 return HDF_FAILURE;
275 }
276
277 if ((g_doOpsCall[cfgItem->opsType].ops != NULL) && (cfgItem->opsType >= SENSOR_OPS_TYPE_EXTBUFF_READ)) {
278 cfgItem->buff = buff + buffOffset;
279 len -= (int16_t)cfgItem->len;
280 if (len < 0) {
281 HDF_LOGE("%s: cfg item para invalid", __func__);
282 return HDF_FAILURE;
283 }
284 if (g_doOpsCall[cfgItem->opsType].ops(busCfg, cfgItem) != HDF_SUCCESS) {
285 HDF_LOGE("%s: extbuff is read and write failed", __func__);
286 return HDF_FAILURE;
287 }
288 buffOffset += cfgItem->len;
289 if (cfgItem->delay != 0) {
290 OsalMDelay(cfgItem->delay);
291 }
292 }
293
294 num++;
295 }
296
297 return HDF_SUCCESS;
298 }
299
ReadSensorRegCfgArray(struct SensorBusCfg * busCfg,const struct SensorRegCfgGroupNode * group,int32_t index,uint8_t * buf,int32_t len)300 int32_t ReadSensorRegCfgArray(struct SensorBusCfg *busCfg, const struct SensorRegCfgGroupNode *group,
301 int32_t index, uint8_t *buf, int32_t len)
302 {
303 int32_t ret;
304 struct SensorRegCfg *cfgItem = NULL;
305
306 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
307 CHECK_NULL_PTR_RETURN_VALUE(group, HDF_FAILURE);
308 CHECK_NULL_PTR_RETURN_VALUE(group->regCfgItem, HDF_FAILURE);
309
310 if ((index >= group->itemNum) || index < 0) {
311 HDF_LOGE("%s: Index is invalid parameter", __func__);
312 return HDF_FAILURE;
313 }
314
315 cfgItem = group->regCfgItem + index;
316 len = (cfgItem->len > len) ? len : cfgItem->len;
317
318 ret = ReadSensor(busCfg, cfgItem->regAddr, buf, len);
319 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read i2c reg");
320
321 return HDF_SUCCESS;
322 }
323
WriteSensorRegCfgArray(struct SensorBusCfg * busCfg,const struct SensorRegCfgGroupNode * group,int32_t index,int32_t len)324 int32_t WriteSensorRegCfgArray(struct SensorBusCfg *busCfg, const struct SensorRegCfgGroupNode *group,
325 int32_t index, int32_t len)
326 {
327 struct SensorRegCfg *cfgItem = NULL;
328
329 CHECK_NULL_PTR_RETURN_VALUE(busCfg, HDF_FAILURE);
330 CHECK_NULL_PTR_RETURN_VALUE(group, HDF_FAILURE);
331 CHECK_NULL_PTR_RETURN_VALUE(group->regCfgItem, HDF_FAILURE);
332
333 if ((index >= group->itemNum) || index < 0) {
334 HDF_LOGE("%s: Index is invalid parameter", __func__);
335 return HDF_FAILURE;
336 }
337
338 cfgItem = group->regCfgItem + index;
339
340 int32_t ret = SensorOpsUpdateBitwise(busCfg, cfgItem);
341 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "Write i2c reg");
342
343 return HDF_SUCCESS;
344 }