1 /*
2 * Copyright (c) 2021-2022 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 "accel_mxc6655xa.h"
10 #include <securec.h>
11 #include "osal_mem.h"
12 #include "osal_time.h"
13 #include "sensor_accel_driver.h"
14 #include "sensor_config_controller.h"
15 #include "sensor_device_manager.h"
16
17 #define HDF_LOG_TAG khdf_sensor_accel_driver
18 #define MXC6655_RANGE (16384 * 2)
19 #define MXC6655_PRECISION 12
20 #define MXC6655_BOUNDARY (0x1 << (MXC6655_PRECISION - 1))
21 #define MXC6655_GRAVITY_STEP (MXC6655_RANGE / MXC6655_BOUNDARY)
22 #define MXC6655_MASK 0x7fff
23 #define MXC6655_ACCEL_OUTPUT_16BIT 16
24 #define MXC6655_ACCEL_OUTPUT_MSB 8
25
26 static struct Mxc6655xaDrvData *g_mxc6655xaDrvData = NULL;
27
Mxc6655xaGetDrvData(void)28 struct Mxc6655xaDrvData *Mxc6655xaGetDrvData(void)
29 {
30 return g_mxc6655xaDrvData;
31 }
32
SensorConvertData(char highByte,char lowByte)33 static int SensorConvertData(char highByte, char lowByte)
34 {
35 int32_t result;
36
37 result = ((uint32_t)highByte << (MXC6655_PRECISION - MXC6655_ACCEL_OUTPUT_MSB)) |
38 ((uint32_t)lowByte >> (MXC6655_ACCEL_OUTPUT_16BIT - MXC6655_PRECISION));
39
40 if (result < MXC6655_BOUNDARY) {
41 result = result * MXC6655_GRAVITY_STEP;
42 } else {
43 result = ~(((~result & (MXC6655_MASK >> (MXC6655_ACCEL_OUTPUT_16BIT - MXC6655_PRECISION))) + 1) *
44 MXC6655_GRAVITY_STEP) + 1;
45 }
46
47 return result;
48 }
49
ReadMxc6655xaRawData(struct SensorCfgData * data,struct AccelData * rawData,uint64_t * timestamp)50 static int32_t ReadMxc6655xaRawData(struct SensorCfgData *data, struct AccelData *rawData, uint64_t *timestamp)
51 {
52 uint8_t status = 0;
53 uint8_t reg[ACCEL_AXIS_BUTT];
54 OsalTimespec time;
55 int32_t x;
56 int32_t y;
57 int32_t z;
58
59 (void)memset_s(&time, sizeof(time), 0, sizeof(time));
60 (void)memset_s(reg, sizeof(reg), 0, sizeof(reg));
61
62 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
63
64 if (OsalGetTime(&time) != HDF_SUCCESS) {
65 HDF_LOGE("%s: Get time failed", __func__);
66 return HDF_FAILURE;
67 }
68 *timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */
69
70 int32_t ret = ReadSensor(&data->busCfg, MXC6655XA_STATUS_ADDR, &status, sizeof(uint8_t));
71 if (ret != HDF_SUCCESS) {
72 HDF_LOGE("%s: data status [%u] ret [%d]", __func__, status, ret);
73 return HDF_FAILURE;
74 }
75
76 ret = ReadSensor(&data->busCfg, MXC6655XA_ACCEL_X_LSB_ADDR, ®[ACCEL_X_AXIS_LSB], sizeof(uint8_t));
77 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
78
79 ret = ReadSensor(&data->busCfg, MXC6655XA_ACCEL_X_MSB_ADDR, ®[ACCEL_X_AXIS_MSB], sizeof(uint8_t));
80 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
81
82 ret = ReadSensor(&data->busCfg, MXC6655XA_ACCEL_Y_LSB_ADDR, ®[ACCEL_Y_AXIS_LSB], sizeof(uint8_t));
83 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
84
85 ret = ReadSensor(&data->busCfg, MXC6655XA_ACCEL_Y_MSB_ADDR, ®[ACCEL_Y_AXIS_MSB], sizeof(uint8_t));
86 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
87
88 ret = ReadSensor(&data->busCfg, MXC6655XA_ACCEL_Z_LSB_ADDR, ®[ACCEL_Z_AXIS_LSB], sizeof(uint8_t));
89 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
90
91 ret = ReadSensor(&data->busCfg, MXC6655XA_ACCEL_Z_MSB_ADDR, ®[ACCEL_Z_AXIS_MSB], sizeof(uint8_t));
92 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
93
94 x = SensorConvertData(reg[ACCEL_X_AXIS_MSB], reg[ACCEL_X_AXIS_LSB]);
95 y = SensorConvertData(reg[ACCEL_Y_AXIS_MSB], reg[ACCEL_Y_AXIS_LSB]);
96 z = SensorConvertData(reg[ACCEL_Z_AXIS_MSB], reg[ACCEL_Z_AXIS_LSB]);
97 rawData->x = x;
98 rawData->y = y;
99 rawData->z = z;
100
101 return HDF_SUCCESS;
102 }
103
ReadMxc6655xaData(struct SensorCfgData * cfg,struct SensorReportEvent * event)104 int32_t ReadMxc6655xaData(struct SensorCfgData *cfg, struct SensorReportEvent *event)
105 {
106 int32_t ret;
107 struct AccelData rawData = { 0, 0, 0 };
108 static int32_t tmp[ACCEL_AXIS_NUM];
109
110 CHECK_NULL_PTR_RETURN_VALUE(cfg, HDF_ERR_INVALID_PARAM);
111 CHECK_NULL_PTR_RETURN_VALUE(event, HDF_ERR_INVALID_PARAM);
112
113 ret = ReadMxc6655xaRawData(cfg, &rawData, &event->timestamp);
114 if (ret != HDF_SUCCESS) {
115 HDF_LOGE("%s: MXC6655XA read raw data failed", __func__);
116 return HDF_FAILURE;
117 }
118
119 event->sensorId = SENSOR_TAG_ACCELEROMETER;
120 event->option = 0;
121 event->mode = SENSOR_WORK_MODE_REALTIME;
122
123 rawData.x = rawData.x * MXC6655XA_ACC_SENSITIVITY_2G;
124 rawData.y = rawData.y * MXC6655XA_ACC_SENSITIVITY_2G;
125 rawData.z = rawData.z * MXC6655XA_ACC_SENSITIVITY_2G;
126
127 tmp[ACCEL_X_AXIS] = (rawData.x * SENSOR_CONVERT_UNIT) / SENSOR_CONVERT_UNIT;
128 tmp[ACCEL_Y_AXIS] = (rawData.y * SENSOR_CONVERT_UNIT) / SENSOR_CONVERT_UNIT;
129 tmp[ACCEL_Z_AXIS] = (rawData.z * SENSOR_CONVERT_UNIT) / SENSOR_CONVERT_UNIT;
130
131 ret = SensorRawDataToRemapData(cfg->direction, tmp, sizeof(tmp) / sizeof(tmp[0]));
132 if (ret != HDF_SUCCESS) {
133 HDF_LOGE("%s: MXC6655XA convert raw data failed", __func__);
134 return HDF_FAILURE;
135 }
136
137 event->dataLen = sizeof(tmp);
138 event->data = (uint8_t *)&tmp;
139
140 return ret;
141 }
142
InitMxc6655xa(struct SensorCfgData * data)143 static int32_t InitMxc6655xa(struct SensorCfgData *data)
144 {
145 int32_t ret;
146
147 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
148 ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]);
149 if (ret != HDF_SUCCESS) {
150 HDF_LOGE("%s: MXC6655XA sensor init config failed", __func__);
151 return HDF_FAILURE;
152 }
153 return HDF_SUCCESS;
154 }
155
DispatchMXC6655xa(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)156 static int32_t DispatchMXC6655xa(struct HdfDeviceIoClient *client,
157 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
158 {
159 (void)client;
160 (void)cmd;
161 (void)data;
162 (void)reply;
163
164 return HDF_SUCCESS;
165 }
166
Mxc6655xaBindDriver(struct HdfDeviceObject * device)167 int32_t Mxc6655xaBindDriver(struct HdfDeviceObject *device)
168 {
169 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
170
171 struct Mxc6655xaDrvData *drvData = (struct Mxc6655xaDrvData *)OsalMemCalloc(sizeof(*drvData));
172 if (drvData == NULL) {
173 HDF_LOGE("%s: Malloc MXC6655XA drv data fail", __func__);
174 return HDF_ERR_MALLOC_FAIL;
175 }
176
177 drvData->ioService.Dispatch = DispatchMXC6655xa;
178 drvData->device = device;
179 device->service = &drvData->ioService;
180 g_mxc6655xaDrvData = drvData;
181
182 return HDF_SUCCESS;
183 }
184
Mxc6655xaInitDriver(struct HdfDeviceObject * device)185 int32_t Mxc6655xaInitDriver(struct HdfDeviceObject *device)
186 {
187 int32_t ret;
188 struct AccelOpsCall ops;
189
190 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
191 struct Mxc6655xaDrvData *drvData = (struct Mxc6655xaDrvData *)device->service;
192 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
193
194 drvData->sensorCfg = AccelCreateCfgData(device->property);
195 if (drvData->sensorCfg == NULL || drvData->sensorCfg->root == NULL) {
196 HDF_LOGD("%s: Creating accelcfg failed because detection failed", __func__);
197 return HDF_ERR_NOT_SUPPORT;
198 }
199
200 ops.Init = NULL;
201 ops.ReadData = ReadMxc6655xaData;
202 ret = AccelRegisterChipOps(&ops);
203 if (ret != HDF_SUCCESS) {
204 HDF_LOGE("%s: Register MXC6655XA accel failed", __func__);
205 return HDF_FAILURE;
206 }
207
208 ret = InitMxc6655xa(drvData->sensorCfg);
209 if (ret != HDF_SUCCESS) {
210 HDF_LOGE("%s: Init MXC6655XA accel failed", __func__);
211 return HDF_FAILURE;
212 }
213
214 return HDF_SUCCESS;
215 }
216
Mxc6655xaReleaseDriver(struct HdfDeviceObject * device)217 void Mxc6655xaReleaseDriver(struct HdfDeviceObject *device)
218 {
219 CHECK_NULL_PTR_RETURN(device);
220
221 struct Mxc6655xaDrvData *drvData = (struct Mxc6655xaDrvData *)device->service;
222 CHECK_NULL_PTR_RETURN(drvData);
223
224 if (drvData->sensorCfg != NULL) {
225 AccelReleaseCfgData(drvData->sensorCfg);
226 drvData->sensorCfg = NULL;
227 }
228 OsalMemFree(drvData);
229 }
230
231 struct HdfDriverEntry g_accelMxc6655xaDevEntry = {
232 .moduleVersion = 1,
233 .moduleName = "HDF_SENSOR_ACCEL_MXC6655XA",
234 .Bind = Mxc6655xaBindDriver,
235 .Init = Mxc6655xaInitDriver,
236 .Release = Mxc6655xaReleaseDriver,
237 };
238
239 HDF_INIT(g_accelMxc6655xaDevEntry);
240