mirror of
https://github.com/openharmony/drivers_framework.git
synced 2026-08-26 18:17:07 -04:00
openharmony_ci
+232
@@ -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 <securec.h>
|
||||
#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);
|
||||
+84
@@ -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 */
|
||||
+378
@@ -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 <securec.h>
|
||||
#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);
|
||||
+66
@@ -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 */
|
||||
Reference in New Issue
Block a user