From 978642ef01190b045f7a8388f6b85472dc66b0b4 Mon Sep 17 00:00:00 2001 From: YOUR_NAME Date: Fri, 3 Sep 2021 18:08:46 +0800 Subject: [PATCH 1/5] magnetic_driver Signed-off-by: YOUR_NAME --- .../driver/chipset/magnetic/magnetic_lsm303.c | 233 +++++++++++ .../driver/chipset/magnetic/magnetic_lsm303.h | 82 ++++ .../driver/magnetic/sensor_magnetic_driver.c | 377 ++++++++++++++++++ .../driver/magnetic/sensor_magnetic_driver.h | 66 +++ 4 files changed, 758 insertions(+) create mode 100755 model/sensor/driver/chipset/magnetic/magnetic_lsm303.c create mode 100755 model/sensor/driver/chipset/magnetic/magnetic_lsm303.h create mode 100755 model/sensor/driver/magnetic/sensor_magnetic_driver.c create mode 100755 model/sensor/driver/magnetic/sensor_magnetic_driver.h diff --git a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c new file mode 100755 index 000000000..14bddf671 --- /dev/null +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c @@ -0,0 +1,233 @@ +/* + * Copyright (c) 2020-2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#include "magnetic_lsm303.h" +#include "osal_mem.h" +#include "osal_time.h" +#include +#include "sensor_config_controller.h" +#include "sensor_device_manager.h" +#include "sensor_magnetic_driver.h" + +static struct Lsm303DrvData *g_lsm303DrvData = NULL; + +struct Lsm303DrvData *Lsm303GetDrvData(void) +{ + return g_lsm303DrvData; +} + +/* IO config for int-pin and I2C-pin */ +#define SENSOR_I2C6_DATA_REG_ADDR 0x114f004c +#define SENSOR_I2C6_CLK_REG_ADDR 0x114f0048 +#define SENSOR_I2C_REG_CFG 0x403 + +static int32_t ReadLsm303RawData(struct SensorCfgData *data, struct MagneticData *rawData, int64_t *timestamp) +{ + uint8_t status = 0; + uint8_t reg[MAGNETIC_AXIS_BUTT]; + OsalTimespec time; + + (void)memset_s(&time, sizeof(time), 0, sizeof(time)); + (void)memset_s(reg, sizeof(reg), 0, sizeof(reg)); + + CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM); + + if (OsalGetTime(&time) != HDF_SUCCESS) { + HDF_LOGE("%s: Get time failed", __func__); + return HDF_FAILURE; + } + *timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */ + + int32_t ret = ReadSensor(&data->busCfg, LSM303_STATUS_ADDR, &status, sizeof(uint8_t)); + if (!(status & LSM303_DATA_READY_MASK) || (ret != HDF_SUCCESS)) { + HDF_LOGE("%s: data status [%u] ret [%d]", __func__, status, ret); + + return HDF_FAILURE; + } + + ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_X_MSB_ADDR, ®[MAGNETIC_X_AXIS_MSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_X_LSB_ADDR, ®[MAGNETIC_X_AXIS_LSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Y_MSB_ADDR, ®[MAGNETIC_Y_AXIS_MSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Y_LSB_ADDR, ®[MAGNETIC_Y_AXIS_LSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Z_MSB_ADDR, ®[MAGNETIC_Z_AXIS_MSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, LSM303_MAGNETIC_Z_LSB_ADDR, ®[MAGNETIC_Z_AXIS_LSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + rawData->x = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[MAGNETIC_X_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) | + reg[MAGNETIC_X_AXIS_LSB]); + rawData->y = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[MAGNETIC_Y_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) | + reg[MAGNETIC_Y_AXIS_LSB]); + rawData->z = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[MAGNETIC_Z_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) | + reg[MAGNETIC_Z_AXIS_LSB]); + + return HDF_SUCCESS; +} + +int32_t ReadLsm303Data(struct SensorCfgData *data) +{ + struct MagneticData rawData = { 0, 0, 0 }; + int32_t tmp[MAGNETIC_AXIS_NUM]; + struct SensorReportEvent event; + + (void)memset_s(&event, sizeof(event), 0, sizeof(event)); + (void)memset_s(tmp, sizeof(tmp), 0, sizeof(tmp)); + + CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM); + + int32_t ret = ReadLsm303RawData(data, &rawData, &event.timestamp); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: LSM303 read raw data failed", __func__); + + return HDF_FAILURE; + } + + event.sensorId = SENSOR_TAG_MAGNETIC_FIELD; + event.option = 0; + event.mode = SENSOR_WORK_MODE_REALTIME; + + tmp[MAGNETIC_X_AXIS] = rawData.x * 1000 / LSM303DLHC_M_SENSITIVITY_XY_4_7Ga; + tmp[MAGNETIC_Y_AXIS] = rawData.y * 1000 / LSM303DLHC_M_SENSITIVITY_XY_4_7Ga; + tmp[MAGNETIC_Z_AXIS] = rawData.z * 1000 / LSM303DLHC_M_SENSITIVITY_Z_4_7Ga; + + event.dataLen = sizeof(tmp); + event.data = (uint8_t *)&tmp; + ret = ReportSensorEvent(&event); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: LSM303 report data failed", __func__); + } + + return ret; +} + +static int32_t InitLsm303(struct SensorCfgData *data) +{ + int32_t ret; + + CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM); + + ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]); + + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Lsm303 sensor init config failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t InitMagneticPreConfig(void) +{ + if (SetSensorPinMux(SENSOR_I2C6_DATA_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) { + HDF_LOGE("%s: Data write mux pin failed", __func__); + return HDF_FAILURE; + } + if (SetSensorPinMux(SENSOR_I2C6_CLK_REG_ADDR, SENSOR_ADDR_WIDTH_4_BYTE, SENSOR_I2C_REG_CFG) != HDF_SUCCESS) { + HDF_LOGE("%s: Clk write mux pin failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t DispatchLsm303(struct HdfDeviceIoClient *client, + int cmd, struct HdfSBuf *data, struct HdfSBuf *reply) +{ + (void)client; + (void)cmd; + (void)data; + (void)reply; + + return HDF_SUCCESS; +} + +int32_t Lsm303BindDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + + struct Lsm303DrvData *drvData = (struct Lsm303DrvData *)OsalMemCalloc(sizeof(*drvData)); + if (drvData == NULL) { + HDF_LOGE("%s: Malloc Lsm303 drv data fail", __func__); + return HDF_ERR_MALLOC_FAIL; + } + + drvData->ioService.Dispatch = DispatchLsm303; + drvData->device = device; + device->service = &drvData->ioService; + g_lsm303DrvData = drvData; + + return HDF_SUCCESS; +} + +int32_t Lsm303InitDriver(struct HdfDeviceObject *device) +{ + int32_t ret; + struct MagneticOpsCall ops; + + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + struct Lsm303DrvData *drvData = (struct Lsm303DrvData *)device->service; + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + ret = InitMagneticPreConfig(); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Init Lsm303 bus mux config", __func__); + return HDF_FAILURE; + } + + drvData->sensorCfg = MagneticCreateCfgData(device->property); + if (drvData->sensorCfg == NULL) { + return HDF_ERR_NOT_SUPPORT; + } + + ops.Init = NULL; + ops.ReadData = ReadLsm303Data; + ret = MagneticRegisterChipOps(&ops); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Register lsm303 magnetic failed", __func__); + return HDF_FAILURE; + } + + ret = InitLsm303(drvData->sensorCfg); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Init lsm303 magnetic failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +void Lsm303ReleaseDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN(device); + + struct Lsm303DrvData *drvData = (struct Lsm303DrvData *)device->service; + CHECK_NULL_PTR_RETURN(drvData); + + MagneticReleaseCfgData(drvData->sensorCfg); + drvData->sensorCfg = NULL; + OsalMemFree(drvData); +} + +struct HdfDriverEntry g_magneticLsm303DevEntry = { + .moduleVersion = 1, + .moduleName = "HDF_SENSOR_MAGNETIC_LSM303", + .Bind = Lsm303BindDriver, + .Init = Lsm303InitDriver, + .Release = Lsm303ReleaseDriver, +}; + +HDF_INIT(g_magneticLsm303DevEntry); \ No newline at end of file diff --git a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h new file mode 100755 index 000000000..f884b45fd --- /dev/null +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2020-2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#ifndef MAGNETIC_LSM303_H +#define MAGNETIC_LSM303_H + +#include "sensor_config_parser.h" +#include "sensor_magnetic_driver.h" + +/* MAGNETIC SET RATE AND MODE ADDR */ +#define LSM303_CRA_REG_ADDR 0X00 +#define LSM303_CRB_REG_ADDR 0X01 +#define LSM303_MR_REG_ADDR 0X02 + +/* MAGNETIC DATA REGISTERS ADDR */ +#define LSM303_MAGNETIC_X_MSB_ADDR 0X03 +#define LSM303_MAGNETIC_X_LSB_ADDR 0X04 +#define LSM303_MAGNETIC_Y_MSB_ADDR 0X05 +#define LSM303_MAGNETIC_Y_LSB_ADDR 0X06 +#define LSM303_MAGNETIC_Z_MSB_ADDR 0X07 +#define LSM303_MAGNETIC_Z_LSB_ADDR 0X08 +#define LSM303_STATUS_ADDR 0X09 + +/* MAGNETIC DATA RATE CONFIG HZ */ +#define LSM303_DATA_RATE_0 0X00 +#define LSM303_DATA_RATE_1 0X04 +#define LSM303_DATA_RATE_2 0X08 +#define LSM303_DATA_RATE_3 0X0C +#define LSM303_DATA_RATE_4 0X10 +#define LSM303_DATA_RATE_5 0X14 +#define LSM303_DATA_RATE_6 0X18 +#define LSM303_DATA_RATE_7 0X1C + +/* MAGNETIC GAIN CONFIG GAUSS */ +#define LSM303_GAIN_RATE_0 0X20 +#define LSM303_GAIN_RATE_1 0X40 +#define LSM303_GAIN_RATE_2 0X60 +#define LSM303_GAIN_RATE_3 0X80 +#define LSM303_GAIN_RATE_4 0XA0 +#define LSM303_GAIN_RATE_5 0XC0 +#define LSM303_GAIN_RATE_6 0XE0 + +/* MAGNETIC GAIN SENSITIVITY RANGE */ +#define LSM303DLHC_M_SENSITIVITY_XY_1_3Ga 1100 +#define LSM303DLHC_M_SENSITIVITY_XY_1_9Ga 855 +#define LSM303DLHC_M_SENSITIVITY_XY_2_5Ga 670 +#define LSM303DLHC_M_SENSITIVITY_XY_4Ga 450 +#define LSM303DLHC_M_SENSITIVITY_XY_4_7Ga 400 +#define LSM303DLHC_M_SENSITIVITY_XY_5_6Ga 330 +#define LSM303DLHC_M_SENSITIVITY_XY_8_1Ga 230 +#define LSM303DLHC_M_SENSITIVITY_Z_1_3Ga 980 +#define LSM303DLHC_M_SENSITIVITY_Z_1_9Ga 760 +#define LSM303DLHC_M_SENSITIVITY_Z_2_5Ga 600 +#define LSM303DLHC_M_SENSITIVITY_Z_4Ga 400 +#define LSM303DLHC_M_SENSITIVITY_Z_4_7Ga 355 +#define LSM303DLHC_M_SENSITIVITY_Z_5_6Ga 295 +#define LSM303DLHC_M_SENSITIVITY_Z_8_1Ga 205 + +/* MAGNETIC MODE CONFIG */ +#define LSM303_OPERATING_MODE_1 0X00 +#define LSM303_OPERATING_MODE_2 0X01 +#define LSM303_OPERATING_MODE_3 0X02 +#define LSM303_OPERATING_MODE_4 0X03 + +/* MAGNETIC DATA READY */ +#define LSM303_DATA_READY_MASK 0x01 + +int32_t DetectMagneticLsm303Chip(struct SensorCfgData *data); +int32_t ReadLsm303Data(struct SensorCfgData *data); + +struct Lsm303DrvData { + struct IDeviceIoService ioService; + struct HdfDeviceObject *device; + struct SensorCfgData *sensorCfg; +}; + +#endif /* MAGNETIC_LSM303_H */ \ No newline at end of file diff --git a/model/sensor/driver/magnetic/sensor_magnetic_driver.c b/model/sensor/driver/magnetic/sensor_magnetic_driver.c new file mode 100755 index 000000000..14e4f80e8 --- /dev/null +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.c @@ -0,0 +1,377 @@ +/* + * Copyright (c) 2020-2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#include "sensor_magnetic_driver.h" +#include "magnetic_lsm303.h" +#include "hdf_base.h" +#include "hdf_device_desc.h" +#include "osal_math.h" +#include "osal_mem.h" +#include "securec.h" +#include "sensor_config_controller.h" +#include "sensor_device_manager.h" +#include "sensor_platform_if.h" + +#define HDF_LOG_TAG sensor_magnetic_driver_c + +#define HDF_MAGNETIC_WORK_QUEUE_NAME "hdf_magnetic_work_queue" + +static struct MagneticDrvData *g_magneticDrvData = NULL; + +static struct MagneticDrvData *MagneticGetDrvData(void) +{ + return g_magneticDrvData; +} + +static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL }; + +int32_t MagneticRegisterChipOps(const struct MagneticOpsCall *ops) +{ + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(ops, HDF_ERR_INVALID_PARAM); + + drvData->ops.Init = ops->Init; + drvData->ops.ReadData = ops->ReadData; + return HDF_SUCCESS; +} + +static void MagneticDataWorkEntry(void *arg) +{ + struct MagneticDrvData *drvData = NULL; + + drvData = (struct MagneticDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + + if (drvData->ops.ReadData == NULL) { + HDF_LOGE("%s: Magnetic readdata function NULL", __func__); + return; + } + if (drvData->ops.ReadData(drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic read data failed", __func__); + } +} + +static void MagneticTimerEntry(uintptr_t arg) +{ + int64_t interval; + int32_t ret; + struct MagneticDrvData *drvData = (struct MagneticDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + + if (!HdfAddWork(&drvData->magneticWorkQueue, &drvData->magneticWork)) { + HDF_LOGE("%s: Magnetic add work queue failed", __func__); + } + + interval = OsalDivS64(drvData->interval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT)); + interval = (interval < SENSOR_TIMER_MIN_TIME) ? SENSOR_TIMER_MIN_TIME : interval; + ret = OsalTimerSetTimeout(&drvData->magneticTimer, interval); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic modify time failed", __func__); + } +} + +static int32_t InitMagneticData(struct MagneticDrvData *drvData) +{ + if (HdfWorkQueueInit(&drvData->magneticWorkQueue, HDF_MAGNETIC_WORK_QUEUE_NAME) != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic init work queue failed", __func__); + return HDF_FAILURE; + } + + if (HdfWorkInit(&drvData->magneticWork, MagneticDataWorkEntry, drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic create thread failed", __func__); + return HDF_FAILURE; + } + + drvData->interval = SENSOR_TIMER_MIN_TIME; + drvData->enable = false; + drvData->detectFlag = false; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticEnable(void) +{ + int32_t ret; + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->magneticCfg, HDF_ERR_INVALID_PARAM); + + if (drvData->enable) { + HDF_LOGE("%s: Magnetic sensor is enabled", __func__); + return HDF_SUCCESS; + } + + ret = SetSensorRegCfgArray(&drvData->magneticCfg->busCfg, drvData->magneticCfg->regCfgGroup[SENSOR_ENABLE_GROUP]); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic sensor enable config failed", __func__); + return ret; + } + + ret = OsalTimerCreate(&drvData->magneticTimer, SENSOR_TIMER_MIN_TIME, MagneticTimerEntry, (uintptr_t)drvData); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic create timer failed[%d]", __func__, ret); + return ret; + } + + ret = OsalTimerStartLoop(&drvData->magneticTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic start timer failed[%d]", __func__, ret); + return ret; + } + drvData->enable = true; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticDisable(void) +{ + int32_t ret; + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->magneticCfg, HDF_ERR_INVALID_PARAM); + + if (!drvData->enable) { + HDF_LOGE("%s: Magnetic sensor had disable", __func__); + return HDF_SUCCESS; + } + + ret = SetSensorRegCfgArray(&drvData->magneticCfg->busCfg, drvData->magneticCfg->regCfgGroup[SENSOR_DISABLE_GROUP]); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic sensor disable config failed", __func__); + return ret; + } + + ret = OsalTimerDelete(&drvData->magneticTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic delete timer failed", __func__); + return ret; + } + drvData->enable = false; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticBatch(int64_t samplingInterval, int64_t interval) +{ + (void)interval; + + struct MagneticDrvData *drvData = NULL; + + drvData = MagneticGetDrvData(); + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + drvData->interval = samplingInterval; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticMode(int32_t mode) +{ + return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE; +} + +static int32_t SetMagneticOption(uint32_t option) +{ + (void)option; + + return HDF_SUCCESS; +} + +static int32_t DispatchMagnetic(struct HdfDeviceIoClient *client, + int cmd, struct HdfSBuf *data, struct HdfSBuf *reply) +{ + (void)client; + (void)cmd; + (void)data; + (void)reply; + + return HDF_SUCCESS; +} + +int32_t MagneticBindDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + + struct MagneticDrvData *drvData = (struct MagneticDrvData *)OsalMemCalloc(sizeof(*drvData)); + if (drvData == NULL) { + HDF_LOGE("%s: Malloc magnetic drv data fail!", __func__); + return HDF_ERR_MALLOC_FAIL; + } + + drvData->ioService.Dispatch = DispatchMagnetic; + drvData->device = device; + device->service = &drvData->ioService; + g_magneticDrvData = drvData; + + return HDF_SUCCESS; +} + +static int32_t InitMagneticOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo) +{ + CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); + + deviceInfo->ops.Enable = SetMagneticEnable; + deviceInfo->ops.Disable = SetMagneticDisable; + deviceInfo->ops.SetBatch = SetMagneticBatch; + deviceInfo->ops.SetMode = SetMagneticMode; + deviceInfo->ops.SetOption = SetMagneticOption; + + if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo), + &config->sensorInfo, sizeof(config->sensorInfo)) != EOK) { + HDF_LOGE("%s: Copy sensor info failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t InitMagneticAfterDetected(struct SensorCfgData *config) +{ + struct SensorDeviceInfo deviceInfo; + CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); + + if (InitMagneticOps(config, &deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: Init magnetic ops failed", __func__); + return HDF_FAILURE; + } + + if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: Add magnetic device failed", __func__); + return HDF_FAILURE; + } + + if (ParseSensorRegConfig(config) != HDF_SUCCESS) { + HDF_LOGE("%s: Parse sensor register failed", __func__); + (void)DeleteSensorDevice(&config->sensorInfo); + ReleaseSensorAllRegConfig(config); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +struct SensorCfgData *MagneticCreateCfgData(const struct DeviceResourceNode *node) +{ + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + if (drvData == NULL || node == NULL) { + HDF_LOGE("%s: Magnetic node pointer NULL", __func__); + + return NULL; + } + + if (drvData->detectFlag) { + HDF_LOGE("%s: Magnetic sensor have detected", __func__); + + return NULL; + } + + if (drvData->magneticCfg == NULL) { + HDF_LOGE("%s: Magnetic magneticCfg pointer NULL", __func__); + + return NULL; + } + + if (GetSensorBaseConfigData(node, drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: Get sensor base config failed", __func__); + goto BASE_CONFIG_EXIT; + } + + if (DetectSensorDevice(drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__); + drvData->detectFlag = false; + goto BASE_CONFIG_EXIT; + } + + drvData->detectFlag = true; + if (InitMagneticAfterDetected(drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__); + + goto INIT_EXIT; + } + return drvData->magneticCfg; + +INIT_EXIT: + (void)ReleaseSensorBusHandle(&drvData->magneticCfg->busCfg); +BASE_CONFIG_EXIT: + drvData->magneticCfg->root = NULL; + (void)memset_s(&drvData->magneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); + (void)memset_s(&drvData->magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); + (void)memset_s(&drvData->magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); + + return NULL; +} + +void MagneticReleaseCfgData(struct SensorCfgData *MagneticCfg) +{ + CHECK_NULL_PTR_RETURN(MagneticCfg); + + (void)DeleteSensorDevice(&MagneticCfg->sensorInfo); + ReleaseSensorAllRegConfig(MagneticCfg); + (void)ReleaseSensorBusHandle(&MagneticCfg->busCfg); + + MagneticCfg->root = NULL; + (void)memset_s(&MagneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); + (void)memset_s(&MagneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); + (void)memset_s(&MagneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); +} + +int32_t MagneticInitDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + if (InitMagneticData(drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: Init magnetic config failed", __func__); + return HDF_FAILURE; + } + + drvData->magneticCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->magneticCfg)); + if (drvData->magneticCfg == NULL) { + HDF_LOGE("%s: Malloc magnetic config data failed", __func__); + return HDF_FAILURE; + } + + drvData->magneticCfg->regCfgGroup = &g_regCfgGroup[0]; + + HDF_LOGI("%s: Init magnetic driver success", __func__); + return HDF_SUCCESS; +} + +void MagneticReleaseDriver(struct HdfDeviceObject *device) { + CHECK_NULL_PTR_RETURN(device); + + struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; + CHECK_NULL_PTR_RETURN(drvData); + + if (drvData->detectFlag) { + MagneticReleaseCfgData(drvData->magneticCfg); + } + + OsalMemFree(drvData->magneticCfg); + drvData->magneticCfg = NULL; + + HdfWorkDestroy(&drvData->magneticWork); + HdfWorkQueueDestroy(&drvData->magneticWorkQueue); + OsalMemFree(drvData); +} + +struct HdfDriverEntry g_sensorMagneticDevEntry = { + .moduleVersion = 1, + .moduleName = "HDF_SENSOR_MAGNETIC", + .Bind = MagneticBindDriver, + .Init = MagneticInitDriver, + .Release = MagneticReleaseDriver, +}; + +HDF_INIT(g_sensorMagneticDevEntry); \ No newline at end of file diff --git a/model/sensor/driver/magnetic/sensor_magnetic_driver.h b/model/sensor/driver/magnetic/sensor_magnetic_driver.h new file mode 100755 index 000000000..77e4a3963 --- /dev/null +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.h @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2020-2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#ifndef SENSOR_MAGNETIC_DRIVER_H +#define SENSOR_MAGNETIC_DRIVER_H + +#include "hdf_workqueue.h" +#include "osal_mutex.h" +#include "osal_timer.h" +#include "sensor_config_parser.h" +#include "sensor_platform_if.h" + +#define MAGNETIC_DEFAULT_SAMPLING_200_MS 200000000 +#define MAGNETIC_CHIP_NAME_LSM303 "lsm303" + +enum MagneticAxisNum { + MAGNETIC_X_AXIS = 0, + MAGNETIC_Y_AXIS = 1, + MAGNETIC_Z_AXIS = 2, + MAGNETIC_AXIS_NUM = 3, +}; + +enum MagneticAxisPart { + MAGNETIC_X_AXIS_MSB = 0, + MAGNETIC_X_AXIS_LSB = 1, + MAGNETIC_Y_AXIS_MSB = 2, + MAGNETIC_Y_AXIS_LSB = 3, + MAGNETIC_Z_AXIS_MSB = 4, + MAGNETIC_Z_AXIS_LSB = 5, + MAGNETIC_AXIS_BUTT, +}; + +struct MagneticData { + int32_t x; + int32_t y; + int32_t z; +}; + +struct MagneticOpsCall { + int32_t (*Init)(struct SensorCfgData *data); + int32_t (*ReadData)(struct SensorCfgData *data); +}; + +struct MagneticDrvData { + struct IDeviceIoService ioService; + struct HdfDeviceObject *device; + HdfWorkQueue magneticWorkQueue; + HdfWork magneticWork; + OsalTimer magneticTimer; + bool detectFlag; + bool enable; + int64_t interval; + struct SensorCfgData *magneticCfg; + struct MagneticOpsCall ops; +}; + +int32_t MagneticRegisterChipOps(const struct MagneticOpsCall *ops); +struct SensorCfgData *MagneticCreateCfgData(const struct DeviceResourceNode *node); +void MagneticReleaseCfgData(struct SensorCfgData *sensorCfgData); + +#endif /* SENSOR_MAGNETIC_DRIVER_H */ \ No newline at end of file -- Gitee From dde94cef8a02a63f809f889816a2444beeb730ec Mon Sep 17 00:00:00 2001 From: YOUR_NAME Date: Mon, 6 Sep 2021 17:50:56 +0800 Subject: [PATCH 2/5] modify code Signed-off-by: YOUR_NAME --- .../driver/chipset/magnetic/magnetic_lsm303.c | 10 +- .../driver/chipset/magnetic/magnetic_lsm303.h | 164 ++-- .../driver/magnetic/sensor_magnetic_driver.c | 752 +++++++++--------- .../driver/magnetic/sensor_magnetic_driver.h | 130 +-- 4 files changed, 529 insertions(+), 527 deletions(-) diff --git a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c index 14bddf671..93ad331c9 100755 --- a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c @@ -1,5 +1,5 @@ /* - * Copyright (c) 2020-2021 Huawei Device Co., Ltd. + * Copyright (c) 2021 Huawei Device Co., Ltd. * * HDF is dual licensed: you can use it either under the terms of * the GPL, or the BSD license, at your option. @@ -7,9 +7,9 @@ */ #include "magnetic_lsm303.h" +#include #include "osal_mem.h" #include "osal_time.h" -#include #include "sensor_config_controller.h" #include "sensor_device_manager.h" #include "sensor_magnetic_driver.h" @@ -100,9 +100,9 @@ int32_t ReadLsm303Data(struct SensorCfgData *data) event.option = 0; event.mode = SENSOR_WORK_MODE_REALTIME; - tmp[MAGNETIC_X_AXIS] = rawData.x * 1000 / LSM303DLHC_M_SENSITIVITY_XY_4_7Ga; - tmp[MAGNETIC_Y_AXIS] = rawData.y * 1000 / LSM303DLHC_M_SENSITIVITY_XY_4_7Ga; - tmp[MAGNETIC_Z_AXIS] = rawData.z * 1000 / LSM303DLHC_M_SENSITIVITY_Z_4_7Ga; + tmp[MAGNETIC_X_AXIS] = rawData.x * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47Ga; + tmp[MAGNETIC_Y_AXIS] = rawData.y * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47Ga; + tmp[MAGNETIC_Z_AXIS] = rawData.z * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_Z47Ga; event.dataLen = sizeof(tmp); event.data = (uint8_t *)&tmp; diff --git a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h index f884b45fd..d9e1380b4 100755 --- a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h @@ -1,82 +1,84 @@ -/* - * Copyright (c) 2020-2021 Huawei Device Co., Ltd. - * - * HDF is dual licensed: you can use it either under the terms of - * the GPL, or the BSD license, at your option. - * See the LICENSE file in the root of this repository for complete details. - */ - -#ifndef MAGNETIC_LSM303_H -#define MAGNETIC_LSM303_H - -#include "sensor_config_parser.h" -#include "sensor_magnetic_driver.h" - -/* MAGNETIC SET RATE AND MODE ADDR */ -#define LSM303_CRA_REG_ADDR 0X00 -#define LSM303_CRB_REG_ADDR 0X01 -#define LSM303_MR_REG_ADDR 0X02 - -/* MAGNETIC DATA REGISTERS ADDR */ -#define LSM303_MAGNETIC_X_MSB_ADDR 0X03 -#define LSM303_MAGNETIC_X_LSB_ADDR 0X04 -#define LSM303_MAGNETIC_Y_MSB_ADDR 0X05 -#define LSM303_MAGNETIC_Y_LSB_ADDR 0X06 -#define LSM303_MAGNETIC_Z_MSB_ADDR 0X07 -#define LSM303_MAGNETIC_Z_LSB_ADDR 0X08 -#define LSM303_STATUS_ADDR 0X09 - -/* MAGNETIC DATA RATE CONFIG HZ */ -#define LSM303_DATA_RATE_0 0X00 -#define LSM303_DATA_RATE_1 0X04 -#define LSM303_DATA_RATE_2 0X08 -#define LSM303_DATA_RATE_3 0X0C -#define LSM303_DATA_RATE_4 0X10 -#define LSM303_DATA_RATE_5 0X14 -#define LSM303_DATA_RATE_6 0X18 -#define LSM303_DATA_RATE_7 0X1C - -/* MAGNETIC GAIN CONFIG GAUSS */ -#define LSM303_GAIN_RATE_0 0X20 -#define LSM303_GAIN_RATE_1 0X40 -#define LSM303_GAIN_RATE_2 0X60 -#define LSM303_GAIN_RATE_3 0X80 -#define LSM303_GAIN_RATE_4 0XA0 -#define LSM303_GAIN_RATE_5 0XC0 -#define LSM303_GAIN_RATE_6 0XE0 - -/* MAGNETIC GAIN SENSITIVITY RANGE */ -#define LSM303DLHC_M_SENSITIVITY_XY_1_3Ga 1100 -#define LSM303DLHC_M_SENSITIVITY_XY_1_9Ga 855 -#define LSM303DLHC_M_SENSITIVITY_XY_2_5Ga 670 -#define LSM303DLHC_M_SENSITIVITY_XY_4Ga 450 -#define LSM303DLHC_M_SENSITIVITY_XY_4_7Ga 400 -#define LSM303DLHC_M_SENSITIVITY_XY_5_6Ga 330 -#define LSM303DLHC_M_SENSITIVITY_XY_8_1Ga 230 -#define LSM303DLHC_M_SENSITIVITY_Z_1_3Ga 980 -#define LSM303DLHC_M_SENSITIVITY_Z_1_9Ga 760 -#define LSM303DLHC_M_SENSITIVITY_Z_2_5Ga 600 -#define LSM303DLHC_M_SENSITIVITY_Z_4Ga 400 -#define LSM303DLHC_M_SENSITIVITY_Z_4_7Ga 355 -#define LSM303DLHC_M_SENSITIVITY_Z_5_6Ga 295 -#define LSM303DLHC_M_SENSITIVITY_Z_8_1Ga 205 - -/* MAGNETIC MODE CONFIG */ -#define LSM303_OPERATING_MODE_1 0X00 -#define LSM303_OPERATING_MODE_2 0X01 -#define LSM303_OPERATING_MODE_3 0X02 -#define LSM303_OPERATING_MODE_4 0X03 - -/* MAGNETIC DATA READY */ -#define LSM303_DATA_READY_MASK 0x01 - -int32_t DetectMagneticLsm303Chip(struct SensorCfgData *data); -int32_t ReadLsm303Data(struct SensorCfgData *data); - -struct Lsm303DrvData { - struct IDeviceIoService ioService; - struct HdfDeviceObject *device; - struct SensorCfgData *sensorCfg; -}; - +/* + * Copyright (c) 2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#ifndef MAGNETIC_LSM303_H +#define MAGNETIC_LSM303_H + +#include "sensor_config_parser.h" +#include "sensor_magnetic_driver.h" + +#define LSM303_MAGNETIC_GIN 1000 + +/* MAGNETIC SET RATE AND MODE ADDR */ +#define LSM303_CRA_REG_ADDR 0X00 +#define LSM303_CRB_REG_ADDR 0X01 +#define LSM303_MR_REG_ADDR 0X02 + +/* MAGNETIC DATA REGISTERS ADDR */ +#define LSM303_MAGNETIC_X_MSB_ADDR 0X03 +#define LSM303_MAGNETIC_X_LSB_ADDR 0X04 +#define LSM303_MAGNETIC_Y_MSB_ADDR 0X05 +#define LSM303_MAGNETIC_Y_LSB_ADDR 0X06 +#define LSM303_MAGNETIC_Z_MSB_ADDR 0X07 +#define LSM303_MAGNETIC_Z_LSB_ADDR 0X08 +#define LSM303_STATUS_ADDR 0X09 + +/* MAGNETIC DATA RATE CONFIG HZ */ +#define LSM303_DATA_RATE_0 0X00 +#define LSM303_DATA_RATE_1 0X04 +#define LSM303_DATA_RATE_2 0X08 +#define LSM303_DATA_RATE_3 0X0C +#define LSM303_DATA_RATE_4 0X10 +#define LSM303_DATA_RATE_5 0X14 +#define LSM303_DATA_RATE_6 0X18 +#define LSM303_DATA_RATE_7 0X1C + +/* MAGNETIC GAIN CONFIG GAUSS */ +#define LSM303_GAIN_RATE_0 0X20 +#define LSM303_GAIN_RATE_1 0X40 +#define LSM303_GAIN_RATE_2 0X60 +#define LSM303_GAIN_RATE_3 0X80 +#define LSM303_GAIN_RATE_4 0XA0 +#define LSM303_GAIN_RATE_5 0XC0 +#define LSM303_GAIN_RATE_6 0XE0 + +/* MAGNETIC GAIN SENSITIVITY RANGE */ +#define LSM303DLHC_SENSITIVITY_XY13Ga 1100 +#define LSM303DLHC_SENSITIVITY_XY19Ga 855 +#define LSM303DLHC_SENSITIVITY_XY25Ga 670 +#define LSM303DLHC_SENSITIVITY_XY40Ga 450 +#define LSM303DLHC_SENSITIVITY_XY47Ga 400 +#define LSM303DLHC_SENSITIVITY_XY56Ga 330 +#define LSM303DLHC_SENSITIVITY_XY81Ga 230 +#define LSM303DLHC_SENSITIVITY_Z13Ga 980 +#define LSM303DLHC_SENSITIVITY_Z19Ga 760 +#define LSM303DLHC_SENSITIVITY_Z25Ga 600 +#define LSM303DLHC_SENSITIVITY_Z40Ga 400 +#define LSM303DLHC_SENSITIVITY_Z47Ga 355 +#define LSM303DLHC_SENSITIVITY_Z56Ga 295 +#define LSM303DLHC_SENSITIVITY_Z81Ga 205 + +/* MAGNETIC MODE CONFIG */ +#define LSM303_OPERATING_MODE_1 0X00 +#define LSM303_OPERATING_MODE_2 0X01 +#define LSM303_OPERATING_MODE_3 0X02 +#define LSM303_OPERATING_MODE_4 0X03 + +/* MAGNETIC DATA READY */ +#define LSM303_DATA_READY_MASK 0x01 + +int32_t DetectMagneticLsm303Chip(struct SensorCfgData *data); +int32_t ReadLsm303Data(struct SensorCfgData *data); + +struct Lsm303DrvData { + struct IDeviceIoService ioService; + struct HdfDeviceObject *device; + struct SensorCfgData *sensorCfg; +}; + #endif /* MAGNETIC_LSM303_H */ \ No newline at end of file diff --git a/model/sensor/driver/magnetic/sensor_magnetic_driver.c b/model/sensor/driver/magnetic/sensor_magnetic_driver.c index 14e4f80e8..f8f1cd938 100755 --- a/model/sensor/driver/magnetic/sensor_magnetic_driver.c +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.c @@ -1,377 +1,377 @@ -/* - * Copyright (c) 2020-2021 Huawei Device Co., Ltd. - * - * HDF is dual licensed: you can use it either under the terms of - * the GPL, or the BSD license, at your option. - * See the LICENSE file in the root of this repository for complete details. - */ - -#include "sensor_magnetic_driver.h" -#include "magnetic_lsm303.h" -#include "hdf_base.h" -#include "hdf_device_desc.h" -#include "osal_math.h" -#include "osal_mem.h" -#include "securec.h" -#include "sensor_config_controller.h" -#include "sensor_device_manager.h" -#include "sensor_platform_if.h" - -#define HDF_LOG_TAG sensor_magnetic_driver_c - -#define HDF_MAGNETIC_WORK_QUEUE_NAME "hdf_magnetic_work_queue" - -static struct MagneticDrvData *g_magneticDrvData = NULL; - -static struct MagneticDrvData *MagneticGetDrvData(void) -{ - return g_magneticDrvData; -} - -static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL }; - -int32_t MagneticRegisterChipOps(const struct MagneticOpsCall *ops) -{ - struct MagneticDrvData *drvData = MagneticGetDrvData(); - - CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); - CHECK_NULL_PTR_RETURN_VALUE(ops, HDF_ERR_INVALID_PARAM); - - drvData->ops.Init = ops->Init; - drvData->ops.ReadData = ops->ReadData; - return HDF_SUCCESS; -} - -static void MagneticDataWorkEntry(void *arg) -{ - struct MagneticDrvData *drvData = NULL; - - drvData = (struct MagneticDrvData *)arg; - CHECK_NULL_PTR_RETURN(drvData); - - if (drvData->ops.ReadData == NULL) { - HDF_LOGE("%s: Magnetic readdata function NULL", __func__); - return; - } - if (drvData->ops.ReadData(drvData->magneticCfg) != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic read data failed", __func__); - } -} - -static void MagneticTimerEntry(uintptr_t arg) -{ - int64_t interval; - int32_t ret; - struct MagneticDrvData *drvData = (struct MagneticDrvData *)arg; - CHECK_NULL_PTR_RETURN(drvData); - - if (!HdfAddWork(&drvData->magneticWorkQueue, &drvData->magneticWork)) { - HDF_LOGE("%s: Magnetic add work queue failed", __func__); - } - - interval = OsalDivS64(drvData->interval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT)); - interval = (interval < SENSOR_TIMER_MIN_TIME) ? SENSOR_TIMER_MIN_TIME : interval; - ret = OsalTimerSetTimeout(&drvData->magneticTimer, interval); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic modify time failed", __func__); - } -} - -static int32_t InitMagneticData(struct MagneticDrvData *drvData) -{ - if (HdfWorkQueueInit(&drvData->magneticWorkQueue, HDF_MAGNETIC_WORK_QUEUE_NAME) != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic init work queue failed", __func__); - return HDF_FAILURE; - } - - if (HdfWorkInit(&drvData->magneticWork, MagneticDataWorkEntry, drvData) != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic create thread failed", __func__); - return HDF_FAILURE; - } - - drvData->interval = SENSOR_TIMER_MIN_TIME; - drvData->enable = false; - drvData->detectFlag = false; - - return HDF_SUCCESS; -} - -static int32_t SetMagneticEnable(void) -{ - int32_t ret; - struct MagneticDrvData *drvData = MagneticGetDrvData(); - - CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); - CHECK_NULL_PTR_RETURN_VALUE(drvData->magneticCfg, HDF_ERR_INVALID_PARAM); - - if (drvData->enable) { - HDF_LOGE("%s: Magnetic sensor is enabled", __func__); - return HDF_SUCCESS; - } - - ret = SetSensorRegCfgArray(&drvData->magneticCfg->busCfg, drvData->magneticCfg->regCfgGroup[SENSOR_ENABLE_GROUP]); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic sensor enable config failed", __func__); - return ret; - } - - ret = OsalTimerCreate(&drvData->magneticTimer, SENSOR_TIMER_MIN_TIME, MagneticTimerEntry, (uintptr_t)drvData); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic create timer failed[%d]", __func__, ret); - return ret; - } - - ret = OsalTimerStartLoop(&drvData->magneticTimer); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic start timer failed[%d]", __func__, ret); - return ret; - } - drvData->enable = true; - - return HDF_SUCCESS; -} - -static int32_t SetMagneticDisable(void) -{ - int32_t ret; - struct MagneticDrvData *drvData = MagneticGetDrvData(); - - CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); - CHECK_NULL_PTR_RETURN_VALUE(drvData->magneticCfg, HDF_ERR_INVALID_PARAM); - - if (!drvData->enable) { - HDF_LOGE("%s: Magnetic sensor had disable", __func__); - return HDF_SUCCESS; - } - - ret = SetSensorRegCfgArray(&drvData->magneticCfg->busCfg, drvData->magneticCfg->regCfgGroup[SENSOR_DISABLE_GROUP]); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic sensor disable config failed", __func__); - return ret; - } - - ret = OsalTimerDelete(&drvData->magneticTimer); - if (ret != HDF_SUCCESS) { - HDF_LOGE("%s: Magnetic delete timer failed", __func__); - return ret; - } - drvData->enable = false; - - return HDF_SUCCESS; -} - -static int32_t SetMagneticBatch(int64_t samplingInterval, int64_t interval) -{ - (void)interval; - - struct MagneticDrvData *drvData = NULL; - - drvData = MagneticGetDrvData(); - CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); - - drvData->interval = samplingInterval; - - return HDF_SUCCESS; -} - -static int32_t SetMagneticMode(int32_t mode) -{ - return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE; -} - -static int32_t SetMagneticOption(uint32_t option) -{ - (void)option; - - return HDF_SUCCESS; -} - -static int32_t DispatchMagnetic(struct HdfDeviceIoClient *client, - int cmd, struct HdfSBuf *data, struct HdfSBuf *reply) -{ - (void)client; - (void)cmd; - (void)data; - (void)reply; - - return HDF_SUCCESS; -} - -int32_t MagneticBindDriver(struct HdfDeviceObject *device) -{ - CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); - - struct MagneticDrvData *drvData = (struct MagneticDrvData *)OsalMemCalloc(sizeof(*drvData)); - if (drvData == NULL) { - HDF_LOGE("%s: Malloc magnetic drv data fail!", __func__); - return HDF_ERR_MALLOC_FAIL; - } - - drvData->ioService.Dispatch = DispatchMagnetic; - drvData->device = device; - device->service = &drvData->ioService; - g_magneticDrvData = drvData; - - return HDF_SUCCESS; -} - -static int32_t InitMagneticOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo) -{ - CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); - - deviceInfo->ops.Enable = SetMagneticEnable; - deviceInfo->ops.Disable = SetMagneticDisable; - deviceInfo->ops.SetBatch = SetMagneticBatch; - deviceInfo->ops.SetMode = SetMagneticMode; - deviceInfo->ops.SetOption = SetMagneticOption; - - if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo), - &config->sensorInfo, sizeof(config->sensorInfo)) != EOK) { - HDF_LOGE("%s: Copy sensor info failed", __func__); - return HDF_FAILURE; - } - - return HDF_SUCCESS; -} - -static int32_t InitMagneticAfterDetected(struct SensorCfgData *config) -{ - struct SensorDeviceInfo deviceInfo; - CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); - - if (InitMagneticOps(config, &deviceInfo) != HDF_SUCCESS) { - HDF_LOGE("%s: Init magnetic ops failed", __func__); - return HDF_FAILURE; - } - - if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) { - HDF_LOGE("%s: Add magnetic device failed", __func__); - return HDF_FAILURE; - } - - if (ParseSensorRegConfig(config) != HDF_SUCCESS) { - HDF_LOGE("%s: Parse sensor register failed", __func__); - (void)DeleteSensorDevice(&config->sensorInfo); - ReleaseSensorAllRegConfig(config); - return HDF_FAILURE; - } - - return HDF_SUCCESS; -} - -struct SensorCfgData *MagneticCreateCfgData(const struct DeviceResourceNode *node) -{ - struct MagneticDrvData *drvData = MagneticGetDrvData(); - - if (drvData == NULL || node == NULL) { - HDF_LOGE("%s: Magnetic node pointer NULL", __func__); - - return NULL; - } - - if (drvData->detectFlag) { - HDF_LOGE("%s: Magnetic sensor have detected", __func__); - - return NULL; - } - - if (drvData->magneticCfg == NULL) { - HDF_LOGE("%s: Magnetic magneticCfg pointer NULL", __func__); - - return NULL; - } - - if (GetSensorBaseConfigData(node, drvData->magneticCfg) != HDF_SUCCESS) { - HDF_LOGE("%s: Get sensor base config failed", __func__); - goto BASE_CONFIG_EXIT; - } - - if (DetectSensorDevice(drvData->magneticCfg) != HDF_SUCCESS) { - HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__); - drvData->detectFlag = false; - goto BASE_CONFIG_EXIT; - } - - drvData->detectFlag = true; - if (InitMagneticAfterDetected(drvData->magneticCfg) != HDF_SUCCESS) { - HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__); - - goto INIT_EXIT; - } - return drvData->magneticCfg; - -INIT_EXIT: - (void)ReleaseSensorBusHandle(&drvData->magneticCfg->busCfg); -BASE_CONFIG_EXIT: - drvData->magneticCfg->root = NULL; - (void)memset_s(&drvData->magneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); - (void)memset_s(&drvData->magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); - (void)memset_s(&drvData->magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); - - return NULL; -} - -void MagneticReleaseCfgData(struct SensorCfgData *MagneticCfg) -{ - CHECK_NULL_PTR_RETURN(MagneticCfg); - - (void)DeleteSensorDevice(&MagneticCfg->sensorInfo); - ReleaseSensorAllRegConfig(MagneticCfg); - (void)ReleaseSensorBusHandle(&MagneticCfg->busCfg); - - MagneticCfg->root = NULL; - (void)memset_s(&MagneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); - (void)memset_s(&MagneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); - (void)memset_s(&MagneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); -} - -int32_t MagneticInitDriver(struct HdfDeviceObject *device) -{ - CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); - struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; - CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); - - if (InitMagneticData(drvData) != HDF_SUCCESS) { - HDF_LOGE("%s: Init magnetic config failed", __func__); - return HDF_FAILURE; - } - - drvData->magneticCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->magneticCfg)); - if (drvData->magneticCfg == NULL) { - HDF_LOGE("%s: Malloc magnetic config data failed", __func__); - return HDF_FAILURE; - } - - drvData->magneticCfg->regCfgGroup = &g_regCfgGroup[0]; - - HDF_LOGI("%s: Init magnetic driver success", __func__); - return HDF_SUCCESS; -} - -void MagneticReleaseDriver(struct HdfDeviceObject *device) { - CHECK_NULL_PTR_RETURN(device); - - struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; - CHECK_NULL_PTR_RETURN(drvData); - - if (drvData->detectFlag) { - MagneticReleaseCfgData(drvData->magneticCfg); - } - - OsalMemFree(drvData->magneticCfg); - drvData->magneticCfg = NULL; - - HdfWorkDestroy(&drvData->magneticWork); - HdfWorkQueueDestroy(&drvData->magneticWorkQueue); - OsalMemFree(drvData); -} - -struct HdfDriverEntry g_sensorMagneticDevEntry = { - .moduleVersion = 1, - .moduleName = "HDF_SENSOR_MAGNETIC", - .Bind = MagneticBindDriver, - .Init = MagneticInitDriver, - .Release = MagneticReleaseDriver, -}; - +/* + * Copyright (c) 2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#include "sensor_magnetic_driver.h" +#include +#include "hdf_base.h" +#include "hdf_device_desc.h" +#include "osal_math.h" +#include "osal_mem.h" +#include "sensor_config_controller.h" +#include "sensor_device_manager.h" +#include "sensor_platform_if.h" + +#define HDF_LOG_TAG sensor_magnetic_driver_c + +#define HDF_MAGNETIC_WORK_QUEUE_NAME "hdf_magnetic_work_queue" + +static struct MagneticDrvData *g_magneticDrvData = NULL; + +static struct MagneticDrvData *MagneticGetDrvData(void) +{ + return g_magneticDrvData; +} + +static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL }; + +int32_t MagneticRegisterChipOps(const struct MagneticOpsCall *ops) +{ + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(ops, HDF_ERR_INVALID_PARAM); + + drvData->ops.Init = ops->Init; + drvData->ops.ReadData = ops->ReadData; + return HDF_SUCCESS; +} + +static void MagneticDataWorkEntry(void *arg) +{ + struct MagneticDrvData *drvData = NULL; + + drvData = (struct MagneticDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + + if (drvData->ops.ReadData == NULL) { + HDF_LOGE("%s: Magnetic readdata function NULL", __func__); + return; + } + if (drvData->ops.ReadData(drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic read data failed", __func__); + } +} + +static void MagneticTimerEntry(uintptr_t arg) +{ + int64_t interval; + int32_t ret; + struct MagneticDrvData *drvData = (struct MagneticDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + + if (!HdfAddWork(&drvData->magneticWorkQueue, &drvData->magneticWork)) { + HDF_LOGE("%s: Magnetic add work queue failed", __func__); + } + + interval = OsalDivS64(drvData->interval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT)); + interval = (interval < SENSOR_TIMER_MIN_TIME) ? SENSOR_TIMER_MIN_TIME : interval; + ret = OsalTimerSetTimeout(&drvData->magneticTimer, interval); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic modify time failed", __func__); + } +} + +static int32_t InitMagneticData(struct MagneticDrvData *drvData) +{ + if (HdfWorkQueueInit(&drvData->magneticWorkQueue, HDF_MAGNETIC_WORK_QUEUE_NAME) != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic init work queue failed", __func__); + return HDF_FAILURE; + } + + if (HdfWorkInit(&drvData->magneticWork, MagneticDataWorkEntry, drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic create thread failed", __func__); + return HDF_FAILURE; + } + + drvData->interval = SENSOR_TIMER_MIN_TIME; + drvData->enable = false; + drvData->detectFlag = false; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticEnable(void) +{ + int32_t ret; + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->magneticCfg, HDF_ERR_INVALID_PARAM); + + if (drvData->enable) { + HDF_LOGE("%s: Magnetic sensor is enabled", __func__); + return HDF_SUCCESS; + } + + ret = SetSensorRegCfgArray(&drvData->magneticCfg->busCfg, drvData->magneticCfg->regCfgGroup[SENSOR_ENABLE_GROUP]); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic sensor enable config failed", __func__); + return ret; + } + + ret = OsalTimerCreate(&drvData->magneticTimer, SENSOR_TIMER_MIN_TIME, MagneticTimerEntry, (uintptr_t)drvData); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic create timer failed[%d]", __func__, ret); + return ret; + } + + ret = OsalTimerStartLoop(&drvData->magneticTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic start timer failed[%d]", __func__, ret); + return ret; + } + drvData->enable = true; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticDisable(void) +{ + int32_t ret; + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->magneticCfg, HDF_ERR_INVALID_PARAM); + + if (!drvData->enable) { + HDF_LOGE("%s: Magnetic sensor had disable", __func__); + return HDF_SUCCESS; + } + + ret = SetSensorRegCfgArray(&drvData->magneticCfg->busCfg, drvData->magneticCfg->regCfgGroup[SENSOR_DISABLE_GROUP]); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic sensor disable config failed", __func__); + return ret; + } + + ret = OsalTimerDelete(&drvData->magneticTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: Magnetic delete timer failed", __func__); + return ret; + } + drvData->enable = false; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticBatch(int64_t samplingInterval, int64_t interval) +{ + (void)interval; + + struct MagneticDrvData *drvData = NULL; + + drvData = MagneticGetDrvData(); + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + drvData->interval = samplingInterval; + + return HDF_SUCCESS; +} + +static int32_t SetMagneticMode(int32_t mode) +{ + return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE; +} + +static int32_t SetMagneticOption(uint32_t option) +{ + (void)option; + + return HDF_SUCCESS; +} + +static int32_t DispatchMagnetic(struct HdfDeviceIoClient *client, + int cmd, struct HdfSBuf *data, struct HdfSBuf *reply) +{ + (void)client; + (void)cmd; + (void)data; + (void)reply; + + return HDF_SUCCESS; +} + +int32_t MagneticBindDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + + struct MagneticDrvData *drvData = (struct MagneticDrvData *)OsalMemCalloc(sizeof(*drvData)); + if (drvData == NULL) { + HDF_LOGE("%s: Malloc magnetic drv data fail!", __func__); + return HDF_ERR_MALLOC_FAIL; + } + + drvData->ioService.Dispatch = DispatchMagnetic; + drvData->device = device; + device->service = &drvData->ioService; + g_magneticDrvData = drvData; + + return HDF_SUCCESS; +} + +static int32_t InitMagneticOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo) +{ + CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); + + deviceInfo->ops.Enable = SetMagneticEnable; + deviceInfo->ops.Disable = SetMagneticDisable; + deviceInfo->ops.SetBatch = SetMagneticBatch; + deviceInfo->ops.SetMode = SetMagneticMode; + deviceInfo->ops.SetOption = SetMagneticOption; + + if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo), + &config->sensorInfo, sizeof(config->sensorInfo)) != EOK) { + HDF_LOGE("%s: Copy sensor info failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t InitMagneticAfterDetected(struct SensorCfgData *config) +{ + struct SensorDeviceInfo deviceInfo; + CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM); + + if (InitMagneticOps(config, &deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: Init magnetic ops failed", __func__); + return HDF_FAILURE; + } + + if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: Add magnetic device failed", __func__); + return HDF_FAILURE; + } + + if (ParseSensorRegConfig(config) != HDF_SUCCESS) { + HDF_LOGE("%s: Parse sensor register failed", __func__); + (void)DeleteSensorDevice(&config->sensorInfo); + ReleaseSensorAllRegConfig(config); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +struct SensorCfgData *MagneticCreateCfgData(const struct DeviceResourceNode *node) +{ + struct MagneticDrvData *drvData = MagneticGetDrvData(); + + if (drvData == NULL || node == NULL) { + HDF_LOGE("%s: Magnetic node pointer NULL", __func__); + + return NULL; + } + + if (drvData->detectFlag) { + HDF_LOGE("%s: Magnetic sensor have detected", __func__); + + return NULL; + } + + if (drvData->magneticCfg == NULL) { + HDF_LOGE("%s: Magnetic magneticCfg pointer NULL", __func__); + + return NULL; + } + + if (GetSensorBaseConfigData(node, drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: Get sensor base config failed", __func__); + goto BASE_CONFIG_EXIT; + } + + if (DetectSensorDevice(drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__); + drvData->detectFlag = false; + goto BASE_CONFIG_EXIT; + } + + drvData->detectFlag = true; + if (InitMagneticAfterDetected(drvData->magneticCfg) != HDF_SUCCESS) { + HDF_LOGI("%s: Magnetic sensor detect device no exist", __func__); + + goto INIT_EXIT; + } + return drvData->magneticCfg; + +INIT_EXIT: + (void)ReleaseSensorBusHandle(&drvData->magneticCfg->busCfg); +BASE_CONFIG_EXIT: + drvData->magneticCfg->root = NULL; + (void)memset_s(&drvData->magneticCfg->sensorInfo, + sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); + (void)memset_s(&drvData->magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); + (void)memset_s(&drvData->magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); + + return NULL; +} + +void MagneticReleaseCfgData(struct SensorCfgData *MagneticCfg) +{ + CHECK_NULL_PTR_RETURN(MagneticCfg); + + (void)DeleteSensorDevice(&MagneticCfg->sensorInfo); + ReleaseSensorAllRegConfig(MagneticCfg); + (void)ReleaseSensorBusHandle(&MagneticCfg->busCfg); + + MagneticCfg->root = NULL; + (void)memset_s(&MagneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); + (void)memset_s(&MagneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); + (void)memset_s(&MagneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); +} + +int32_t MagneticInitDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + if (InitMagneticData(drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: Init magnetic config failed", __func__); + return HDF_FAILURE; + } + + drvData->magneticCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->magneticCfg)); + if (drvData->magneticCfg == NULL) { + HDF_LOGE("%s: Malloc magnetic config data failed", __func__); + return HDF_FAILURE; + } + + drvData->magneticCfg->regCfgGroup = &g_regCfgGroup[0]; + + HDF_LOGI("%s: Init magnetic driver success", __func__); + return HDF_SUCCESS; +} + +void MagneticReleaseDriver(struct HdfDeviceObject *device) { + CHECK_NULL_PTR_RETURN(device); + + struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; + CHECK_NULL_PTR_RETURN(drvData); + + if (drvData->detectFlag) { + MagneticReleaseCfgData(drvData->magneticCfg); + } + + OsalMemFree(drvData->magneticCfg); + drvData->magneticCfg = NULL; + + HdfWorkDestroy(&drvData->magneticWork); + HdfWorkQueueDestroy(&drvData->magneticWorkQueue); + OsalMemFree(drvData); +} + +struct HdfDriverEntry g_sensorMagneticDevEntry = { + .moduleVersion = 1, + .moduleName = "HDF_SENSOR_MAGNETIC", + .Bind = MagneticBindDriver, + .Init = MagneticInitDriver, + .Release = MagneticReleaseDriver, +}; + HDF_INIT(g_sensorMagneticDevEntry); \ No newline at end of file diff --git a/model/sensor/driver/magnetic/sensor_magnetic_driver.h b/model/sensor/driver/magnetic/sensor_magnetic_driver.h index 77e4a3963..7b51d1914 100755 --- a/model/sensor/driver/magnetic/sensor_magnetic_driver.h +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.h @@ -1,66 +1,66 @@ -/* - * Copyright (c) 2020-2021 Huawei Device Co., Ltd. - * - * HDF is dual licensed: you can use it either under the terms of - * the GPL, or the BSD license, at your option. - * See the LICENSE file in the root of this repository for complete details. - */ - -#ifndef SENSOR_MAGNETIC_DRIVER_H -#define SENSOR_MAGNETIC_DRIVER_H - -#include "hdf_workqueue.h" -#include "osal_mutex.h" -#include "osal_timer.h" -#include "sensor_config_parser.h" -#include "sensor_platform_if.h" - -#define MAGNETIC_DEFAULT_SAMPLING_200_MS 200000000 -#define MAGNETIC_CHIP_NAME_LSM303 "lsm303" - -enum MagneticAxisNum { - MAGNETIC_X_AXIS = 0, - MAGNETIC_Y_AXIS = 1, - MAGNETIC_Z_AXIS = 2, - MAGNETIC_AXIS_NUM = 3, -}; - -enum MagneticAxisPart { - MAGNETIC_X_AXIS_MSB = 0, - MAGNETIC_X_AXIS_LSB = 1, - MAGNETIC_Y_AXIS_MSB = 2, - MAGNETIC_Y_AXIS_LSB = 3, - MAGNETIC_Z_AXIS_MSB = 4, - MAGNETIC_Z_AXIS_LSB = 5, - MAGNETIC_AXIS_BUTT, -}; - -struct MagneticData { - int32_t x; - int32_t y; - int32_t z; -}; - -struct MagneticOpsCall { - int32_t (*Init)(struct SensorCfgData *data); - int32_t (*ReadData)(struct SensorCfgData *data); -}; - -struct MagneticDrvData { - struct IDeviceIoService ioService; - struct HdfDeviceObject *device; - HdfWorkQueue magneticWorkQueue; - HdfWork magneticWork; - OsalTimer magneticTimer; - bool detectFlag; - bool enable; - int64_t interval; - struct SensorCfgData *magneticCfg; - struct MagneticOpsCall ops; -}; - -int32_t MagneticRegisterChipOps(const struct MagneticOpsCall *ops); -struct SensorCfgData *MagneticCreateCfgData(const struct DeviceResourceNode *node); -void MagneticReleaseCfgData(struct SensorCfgData *sensorCfgData); - +/* + * Copyright (c) 2021 Huawei Device Co., Ltd. + * + * HDF is dual licensed: you can use it either under the terms of + * the GPL, or the BSD license, at your option. + * See the LICENSE file in the root of this repository for complete details. + */ + +#ifndef SENSOR_MAGNETIC_DRIVER_H +#define SENSOR_MAGNETIC_DRIVER_H + +#include "hdf_workqueue.h" +#include "osal_mutex.h" +#include "osal_timer.h" +#include "sensor_config_parser.h" +#include "sensor_platform_if.h" + +#define MAGNETIC_DEFAULT_SAMPLING_200_MS 200000000 +#define MAGNETIC_CHIP_NAME_LSM303 "lsm303" + +enum MagneticAxisNum { + MAGNETIC_X_AXIS = 0, + MAGNETIC_Y_AXIS = 1, + MAGNETIC_Z_AXIS = 2, + MAGNETIC_AXIS_NUM = 3, +}; + +enum MagneticAxisPart { + MAGNETIC_X_AXIS_MSB = 0, + MAGNETIC_X_AXIS_LSB = 1, + MAGNETIC_Y_AXIS_MSB = 2, + MAGNETIC_Y_AXIS_LSB = 3, + MAGNETIC_Z_AXIS_MSB = 4, + MAGNETIC_Z_AXIS_LSB = 5, + MAGNETIC_AXIS_BUTT, +}; + +struct MagneticData { + int32_t x; + int32_t y; + int32_t z; +}; + +struct MagneticOpsCall { + int32_t (*Init)(struct SensorCfgData *data); + int32_t (*ReadData)(struct SensorCfgData *data); +}; + +struct MagneticDrvData { + struct IDeviceIoService ioService; + struct HdfDeviceObject *device; + HdfWorkQueue magneticWorkQueue; + HdfWork magneticWork; + OsalTimer magneticTimer; + bool detectFlag; + bool enable; + int64_t interval; + struct SensorCfgData *magneticCfg; + struct MagneticOpsCall ops; +}; + +int32_t MagneticRegisterChipOps(const struct MagneticOpsCall *ops); +struct SensorCfgData *MagneticCreateCfgData(const struct DeviceResourceNode *node); +void MagneticReleaseCfgData(struct SensorCfgData *sensorCfgData); + #endif /* SENSOR_MAGNETIC_DRIVER_H */ \ No newline at end of file -- Gitee From 6df451a98750a0197a7594c916667b28dceb26cc Mon Sep 17 00:00:00 2001 From: YOUR_NAME Date: Tue, 7 Sep 2021 08:54:34 +0800 Subject: [PATCH 3/5] modify_code Signed-off-by: YOUR_NAME --- .../driver/chipset/magnetic/magnetic_lsm303.c | 7 ++--- .../driver/chipset/magnetic/magnetic_lsm303.h | 28 +++++++++---------- 2 files changed, 17 insertions(+), 18 deletions(-) diff --git a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c index 93ad331c9..4726b7671 100755 --- a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c @@ -100,9 +100,9 @@ int32_t ReadLsm303Data(struct SensorCfgData *data) event.option = 0; event.mode = SENSOR_WORK_MODE_REALTIME; - tmp[MAGNETIC_X_AXIS] = rawData.x * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47Ga; - tmp[MAGNETIC_Y_AXIS] = rawData.y * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47Ga; - tmp[MAGNETIC_Z_AXIS] = rawData.z * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_Z47Ga; + tmp[MAGNETIC_X_AXIS] = rawData.x * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47GA; + tmp[MAGNETIC_Y_AXIS] = rawData.y * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_XY47GA; + tmp[MAGNETIC_Z_AXIS] = rawData.z * LSM303_MAGNETIC_GIN / LSM303DLHC_SENSITIVITY_Z47GA; event.dataLen = sizeof(tmp); event.data = (uint8_t *)&tmp; @@ -121,7 +121,6 @@ static int32_t InitLsm303(struct SensorCfgData *data) CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM); ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]); - if (ret != HDF_SUCCESS) { HDF_LOGE("%s: Lsm303 sensor init config failed", __func__); return HDF_FAILURE; diff --git a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h index d9e1380b4..6d20e1968 100755 --- a/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h @@ -48,20 +48,20 @@ #define LSM303_GAIN_RATE_6 0XE0 /* MAGNETIC GAIN SENSITIVITY RANGE */ -#define LSM303DLHC_SENSITIVITY_XY13Ga 1100 -#define LSM303DLHC_SENSITIVITY_XY19Ga 855 -#define LSM303DLHC_SENSITIVITY_XY25Ga 670 -#define LSM303DLHC_SENSITIVITY_XY40Ga 450 -#define LSM303DLHC_SENSITIVITY_XY47Ga 400 -#define LSM303DLHC_SENSITIVITY_XY56Ga 330 -#define LSM303DLHC_SENSITIVITY_XY81Ga 230 -#define LSM303DLHC_SENSITIVITY_Z13Ga 980 -#define LSM303DLHC_SENSITIVITY_Z19Ga 760 -#define LSM303DLHC_SENSITIVITY_Z25Ga 600 -#define LSM303DLHC_SENSITIVITY_Z40Ga 400 -#define LSM303DLHC_SENSITIVITY_Z47Ga 355 -#define LSM303DLHC_SENSITIVITY_Z56Ga 295 -#define LSM303DLHC_SENSITIVITY_Z81Ga 205 +#define LSM303DLHC_SENSITIVITY_XY13GA 1100 +#define LSM303DLHC_SENSITIVITY_XY19GA 855 +#define LSM303DLHC_SENSITIVITY_XY25GA 670 +#define LSM303DLHC_SENSITIVITY_XY40GA 450 +#define LSM303DLHC_SENSITIVITY_XY47GA 400 +#define LSM303DLHC_SENSITIVITY_XY56GA 330 +#define LSM303DLHC_SENSITIVITY_XY81GA 230 +#define LSM303DLHC_SENSITIVITY_Z13GA 980 +#define LSM303DLHC_SENSITIVITY_Z19GA 760 +#define LSM303DLHC_SENSITIVITY_Z25GA 600 +#define LSM303DLHC_SENSITIVITY_Z40GA 400 +#define LSM303DLHC_SENSITIVITY_Z47GA 355 +#define LSM303DLHC_SENSITIVITY_Z56GA 295 +#define LSM303DLHC_SENSITIVITY_Z81GA 205 /* MAGNETIC MODE CONFIG */ #define LSM303_OPERATING_MODE_1 0X00 -- Gitee From 09e576385bd16aea8e77231c28a93ef45d2a3c13 Mon Sep 17 00:00:00 2001 From: YOUR_NAME Date: Tue, 7 Sep 2021 10:22:05 +0800 Subject: [PATCH 4/5] modify2_code2 Signed-off-by: YOUR_NAME --- .../driver/magnetic/sensor_magnetic_driver.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/model/sensor/driver/magnetic/sensor_magnetic_driver.c b/model/sensor/driver/magnetic/sensor_magnetic_driver.c index f8f1cd938..5b0eb96c9 100755 --- a/model/sensor/driver/magnetic/sensor_magnetic_driver.c +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.c @@ -311,18 +311,18 @@ BASE_CONFIG_EXIT: return NULL; } -void MagneticReleaseCfgData(struct SensorCfgData *MagneticCfg) +void MagneticReleaseCfgData(struct SensorCfgData *magneticCfg) { - CHECK_NULL_PTR_RETURN(MagneticCfg); + CHECK_NULL_PTR_RETURN(magneticCfg); - (void)DeleteSensorDevice(&MagneticCfg->sensorInfo); - ReleaseSensorAllRegConfig(MagneticCfg); - (void)ReleaseSensorBusHandle(&MagneticCfg->busCfg); + (void)DeleteSensorDevice(&magneticCfg->sensorInfo); + ReleaseSensorAllRegConfig(magneticCfg); + (void)ReleaseSensorBusHandle(&magneticCfg->busCfg); MagneticCfg->root = NULL; - (void)memset_s(&MagneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); - (void)memset_s(&MagneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); - (void)memset_s(&MagneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); + (void)memset_s(&magneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); + (void)memset_s(&magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); + (void)memset_s(&magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); } int32_t MagneticInitDriver(struct HdfDeviceObject *device) @@ -348,7 +348,8 @@ int32_t MagneticInitDriver(struct HdfDeviceObject *device) return HDF_SUCCESS; } -void MagneticReleaseDriver(struct HdfDeviceObject *device) { +void MagneticReleaseDriver(struct HdfDeviceObject *device) +{ CHECK_NULL_PTR_RETURN(device); struct MagneticDrvData *drvData = (struct MagneticDrvData *)device->service; -- Gitee From 1bd22a09fd6d1b6d6168e65df5287287de5fd4da Mon Sep 17 00:00:00 2001 From: YOUR_NAME Date: Tue, 7 Sep 2021 10:43:15 +0800 Subject: [PATCH 5/5] modify3_code3 Signed-off-by: YOUR_NAME --- model/sensor/driver/magnetic/sensor_magnetic_driver.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/model/sensor/driver/magnetic/sensor_magnetic_driver.c b/model/sensor/driver/magnetic/sensor_magnetic_driver.c index 5b0eb96c9..c614ba33b 100755 --- a/model/sensor/driver/magnetic/sensor_magnetic_driver.c +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.c @@ -319,7 +319,7 @@ void MagneticReleaseCfgData(struct SensorCfgData *magneticCfg) ReleaseSensorAllRegConfig(magneticCfg); (void)ReleaseSensorBusHandle(&magneticCfg->busCfg); - MagneticCfg->root = NULL; + magneticCfg->root = NULL; (void)memset_s(&magneticCfg->sensorInfo, sizeof(struct SensorBasicInfo), 0, sizeof(struct SensorBasicInfo)); (void)memset_s(&magneticCfg->busCfg, sizeof(struct SensorBusCfg), 0, sizeof(struct SensorBusCfg)); (void)memset_s(&magneticCfg->sensorAttr, sizeof(struct SensorAttr), 0, sizeof(struct SensorAttr)); -- Gitee