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 "magnetic_lsm303.h"
10 #include <securec.h>
11 #include "osal_mem.h"
12 #include "osal_time.h"
13 #include "sensor_config_controller.h"
14 #include "sensor_device_manager.h"
15 #include "sensor_magnetic_driver.h"
16
17 #define HDF_LOG_TAG hdf_sensor_magnetic
18
19 static struct Lsm303DrvData *g_lsm303DrvData = NULL;
20
Lsm303GetDrvData(void)21 static struct Lsm303DrvData *Lsm303GetDrvData(void)
22 {
23 return g_lsm303DrvData;
24 }
25
26 /* IO config for int-pin and I2C-pin */
27 #define SENSOR_I2C6_DATA_REG_ADDR 0x114f004c
28 #define SENSOR_I2C6_CLK_REG_ADDR 0x114f0048
29 #define SENSOR_I2C_REG_CFG 0x403
30
ReadLsm303RawData(struct SensorCfgData * data,struct MagneticData * rawData,uint64_t * timestamp)31 static int32_t ReadLsm303RawData(struct SensorCfgData *data, struct MagneticData *rawData, uint64_t *timestamp)
32 {
33 uint8_t status = 0;
34 uint8_t reg[MAGNETIC_AXIS_BUTT];
35 OsalTimespec time;
36
37 (void)memset_s(&time, sizeof(time), 0, sizeof(time));
38 (void)memset_s(reg, sizeof(reg), 0, sizeof(reg));
39
40 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
41
42 if (OsalGetTime(&time) != HDF_SUCCESS) {
43 HDF_LOGE("%s: Get time failed", __func__);
44 return HDF_FAILURE;
45 }
46 *timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */
47
48 int32_t ret = ReadSensor(&data->busCfg, LSM303_STATUS_ADDR, &status, sizeof(uint8_t));
49 if (!(status & LSM303_DATA_READY_MASK) || (ret != HDF_SUCCESS)) {
50 HDF_LOGE("%s: data status [%u] ret [%d]", __func__, status, ret);
51 return HDF_FAILURE;
52 }
53
54 ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_X_MSB_ADDR, ®[MAGNETIC_X_AXIS_MSB], sizeof(uint8_t));
55 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
56
57 ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_X_LSB_ADDR, ®[MAGNETIC_X_AXIS_LSB], sizeof(uint8_t));
58 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
59
60 ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Y_MSB_ADDR, ®[MAGNETIC_Y_AXIS_MSB], sizeof(uint8_t));
61 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
62
63 ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Y_LSB_ADDR, ®[MAGNETIC_Y_AXIS_LSB], sizeof(uint8_t));
64 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
65
66 ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Z_MSB_ADDR, ®[MAGNETIC_Z_AXIS_MSB], sizeof(uint8_t));
67 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
68
69 ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Z_LSB_ADDR, ®[MAGNETIC_Z_AXIS_LSB], sizeof(uint8_t));
70 CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data");
71
72 rawData->x = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[MAGNETIC_X_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
73 reg[MAGNETIC_X_AXIS_LSB]);
74 rawData->y = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[MAGNETIC_Y_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
75 reg[MAGNETIC_Y_AXIS_LSB]);
76 rawData->z = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[MAGNETIC_Z_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) |
77 reg[MAGNETIC_Z_AXIS_LSB]);
78
79 return HDF_SUCCESS;
80 }
81
ReadLsm303Data(struct SensorCfgData * data)82 int32_t ReadLsm303Data(struct SensorCfgData *data)
83 {
84 struct MagneticData rawData = { 0, 0, 0 };
85 int32_t tmp[MAGNETIC_AXIS_NUM];
86 struct SensorReportEvent event;
87
88 (void)memset_s(&event, sizeof(event), 0, sizeof(event));
89 (void)memset_s(tmp, sizeof(tmp), 0, sizeof(tmp));
90
91 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
92
93 int32_t ret = ReadLsm303RawData(data, &rawData, &event.timestamp);
94 if (ret != HDF_SUCCESS) {
95 HDF_LOGE("%s: LSM303 read raw data failed", __func__);
96 return HDF_FAILURE;
97 }
98
99 event.sensorId = SENSOR_TAG_MAGNETIC_FIELD;
100 event.option = 0;
101 event.mode = SENSOR_WORK_MODE_REALTIME;
102
103 tmp[MAGNETIC_X_AXIS] = rawData.x * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47GA;
104 tmp[MAGNETIC_Y_AXIS] = rawData.y * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47GA;
105 tmp[MAGNETIC_Z_AXIS] = rawData.z * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_Z47GA;
106
107 ret = SensorRawDataToRemapData(data->direction, tmp, sizeof(tmp) / sizeof(tmp[0]));
108 if (ret != HDF_SUCCESS) {
109 HDF_LOGE("%s: LSM303 convert raw data failed", __func__);
110 return HDF_FAILURE;
111 }
112
113 event.dataLen = sizeof(tmp);
114 event.data = (uint8_t *)&tmp;
115 ret = ReportSensorEvent(&event);
116 if (ret != HDF_SUCCESS) {
117 HDF_LOGE("%s: LSM303 report data failed", __func__);
118 }
119
120 return ret;
121 }
122
InitLsm303(struct SensorCfgData * data)123 static int32_t InitLsm303(struct SensorCfgData *data)
124 {
125 int32_t ret;
126
127 CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
128
129 ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]);
130 if (ret != HDF_SUCCESS) {
131 HDF_LOGE("%s: Lsm303 sensor init config failed", __func__);
132 return HDF_FAILURE;
133 }
134
135 return HDF_SUCCESS;
136 }
137
InitMagneticPreConfig(void)138 static int32_t InitMagneticPreConfig(void)
139 {
140 if (SetSensorPinMux(SENSOR_I2C6_DATA_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) {
141 HDF_LOGE("%s: Data write mux pin failed", __func__);
142 return HDF_FAILURE;
143 }
144 if (SetSensorPinMux(SENSOR_I2C6_CLK_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) {
145 HDF_LOGE("%s: Clk write mux pin failed", __func__);
146 return HDF_FAILURE;
147 }
148
149 return HDF_SUCCESS;
150 }
151
DispatchLsm303(struct HdfDeviceIoClient * client,int cmd,struct HdfSBuf * data,struct HdfSBuf * reply)152 static int32_t DispatchLsm303(struct HdfDeviceIoClient *client,
153 int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
154 {
155 (void)client;
156 (void)cmd;
157 (void)data;
158 (void)reply;
159
160 return HDF_SUCCESS;
161 }
162
Lsm303BindDriver(struct HdfDeviceObject * device)163 int32_t Lsm303BindDriver(struct HdfDeviceObject *device)
164 {
165 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
166
167 struct Lsm303DrvData *drvData = (struct Lsm303DrvData *)OsalMemCalloc(sizeof(*drvData));
168 if (drvData == NULL) {
169 HDF_LOGE("%s: Malloc Lsm303 drv data fail", __func__);
170 return HDF_ERR_MALLOC_FAIL;
171 }
172
173 drvData->ioService.Dispatch = DispatchLsm303;
174 drvData->device = device;
175 device->service = &drvData->ioService;
176 g_lsm303DrvData = drvData;
177
178 return HDF_SUCCESS;
179 }
180
Lsm303InitDriver(struct HdfDeviceObject * device)181 int32_t Lsm303InitDriver(struct HdfDeviceObject *device)
182 {
183 int32_t ret;
184 struct MagneticOpsCall ops;
185
186 CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
187 struct Lsm303DrvData *drvData = (struct Lsm303DrvData *)device->service;
188 CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
189
190 ret = InitMagneticPreConfig();
191 if (ret != HDF_SUCCESS) {
192 HDF_LOGE("%s: Init Lsm303 bus mux config", __func__);
193 return HDF_FAILURE;
194 }
195
196 drvData->sensorCfg = MagneticCreateCfgData(device->property);
197 if (drvData->sensorCfg == NULL || drvData->sensorCfg->root == NULL) {
198 HDF_LOGD("%s: Creating magneticcfg failed because detection failed", __func__);
199 return HDF_ERR_NOT_SUPPORT;
200 }
201
202 ops.Init = NULL;
203 ops.ReadData = ReadLsm303Data;
204 ret = MagneticRegisterChipOps(&ops);
205 if (ret != HDF_SUCCESS) {
206 HDF_LOGE("%s: Register lsm303 magnetic failed", __func__);
207 return HDF_FAILURE;
208 }
209
210 ret = InitLsm303(drvData->sensorCfg);
211 if (ret != HDF_SUCCESS) {
212 HDF_LOGE("%s: Init lsm303 magnetic failed", __func__);
213 return HDF_FAILURE;
214 }
215
216 return HDF_SUCCESS;
217 }
218
Lsm303ReleaseDriver(struct HdfDeviceObject * device)219 void Lsm303ReleaseDriver(struct HdfDeviceObject *device)
220 {
221 CHECK_NULL_PTR_RETURN(device);
222
223 struct Lsm303DrvData *drvData = (struct Lsm303DrvData *)device->service;
224 CHECK_NULL_PTR_RETURN(drvData);
225
226 if (drvData->sensorCfg != NULL) {
227 MagneticReleaseCfgData(drvData->sensorCfg);
228 drvData->sensorCfg = NULL;
229 }
230 OsalMemFree(drvData);
231 }
232
233 struct HdfDriverEntry g_magneticLsm303DevEntry = {
234 .moduleVersion = 1,
235 .moduleName = "HDF_SENSOR_MAGNETIC_LSM303",
236 .Bind = Lsm303BindDriver,
237 .Init = Lsm303InitDriver,
238 .Release = Lsm303ReleaseDriver,
239 };
240
241 HDF_INIT(g_magneticLsm303DevEntry);