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