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