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 }