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 0000000000000000000000000000000000000000..4726b7671174271ce819f39683a38f9e8823b416 --- /dev/null +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.c @@ -0,0 +1,232 @@ +/* + * 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 "magnetic_lsm303.h" +#include +#include "osal_mem.h" +#include "osal_time.h" +#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 * 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; + 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 0000000000000000000000000000000000000000..6d20e19682b738632ac268446b19dbed5f2f551d --- /dev/null +++ b/model/sensor/driver/chipset/magnetic/magnetic_lsm303.h @@ -0,0 +1,84 @@ +/* + * 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 new file mode 100755 index 0000000000000000000000000000000000000000..c614ba33bdda2d5169399c2b05ad36ac6b4f6330 --- /dev/null +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.c @@ -0,0 +1,378 @@ +/* + * 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 new file mode 100755 index 0000000000000000000000000000000000000000..7b51d1914d2123107730195a23992edade8bdb44 --- /dev/null +++ b/model/sensor/driver/magnetic/sensor_magnetic_driver.h @@ -0,0 +1,66 @@ +/* + * 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