From 969e4b222efed248c9ae2850411cbc2a9dfd042f Mon Sep 17 00:00:00 2001 From: kevin Date: Fri, 20 Aug 2021 17:00:17 +0800 Subject: [PATCH] sync sensor gyro driver Signed-off-by: kevin Change-Id: I4b0f9eabddb1393ff5e415f6ab135ac9f132f4b2 --- .../driver/chipset/vibrator_linear_driver.c | 8 +- .../vibrator/driver/src/vibrator_driver.c | 13 +- .../vibrator/driver/src/vibrator_haptic.c | 7 +- .../sensor/driver/chipset/gyro/gyro_bmi160.c | 152 +++++++ .../sensor/driver/chipset/gyro/gyro_bmi160.h | 54 +++ .../driver/common/src/sensor_config_parser.c | 14 +- model/sensor/driver/gyro/sensor_gyro_driver.c | 392 ++++++++++++++++++ model/sensor/driver/gyro/sensor_gyro_driver.h | 70 ++++ .../driver/include/sensor_platform_if.h | 14 +- 9 files changed, 695 insertions(+), 29 deletions(-) create mode 100644 model/sensor/driver/chipset/gyro/gyro_bmi160.c create mode 100644 model/sensor/driver/chipset/gyro/gyro_bmi160.h create mode 100644 model/sensor/driver/gyro/sensor_gyro_driver.c create mode 100644 model/sensor/driver/gyro/sensor_gyro_driver.h diff --git a/model/misc/vibrator/driver/chipset/vibrator_linear_driver.c b/model/misc/vibrator/driver/chipset/vibrator_linear_driver.c index bb7859a6..f58e6eca 100644 --- a/model/misc/vibrator/driver/chipset/vibrator_linear_driver.c +++ b/model/misc/vibrator/driver/chipset/vibrator_linear_driver.c @@ -157,9 +157,13 @@ int32_t InitLinearVibratorDriver(struct HdfDeviceObject *device) void ReleaseLinearVibratorDriver(struct HdfDeviceObject *device) { + if (device == NULL) { + HDF_LOGE("%s: Device is null", __func__); + return; + } struct VibratorLinearDriverData *drvData = (struct VibratorLinearDriverData *)device->service; - if (device == NULL || drvData == NULL) { - HDF_LOGE("%s: pointer is null and return errno", __func__); + if (drvData == NULL) { + HDF_LOGE("%s: DrvData pointer is null", __func__); return; } diff --git a/model/misc/vibrator/driver/src/vibrator_driver.c b/model/misc/vibrator/driver/src/vibrator_driver.c index 9d47a1be..5e8be184 100644 --- a/model/misc/vibrator/driver/src/vibrator_driver.c +++ b/model/misc/vibrator/driver/src/vibrator_driver.c @@ -291,9 +291,16 @@ int32_t InitVibratorDriver(struct HdfDeviceObject *device) void ReleaseVibratorDriver(struct HdfDeviceObject *device) { - struct VibratorDriverData *drvData = (struct VibratorDriverData *)device->service; - if (device == NULL || drvData == NULL) { - HDF_LOGE("%s: pointer is null and return errno", __func__); + struct VibratorDriverData *drvData = NULL; + + if (device == NULL) { + HDF_LOGE("%s: device is null", __func__); + return; + } + + drvData = (struct VibratorDriverData *)device->service; + if (drvData == NULL) { + HDF_LOGE("%s: drvData is null", __func__); return; } diff --git a/model/misc/vibrator/driver/src/vibrator_haptic.c b/model/misc/vibrator/driver/src/vibrator_haptic.c index 39eacb6f..9bd5ae45 100644 --- a/model/misc/vibrator/driver/src/vibrator_haptic.c +++ b/model/misc/vibrator/driver/src/vibrator_haptic.c @@ -74,13 +74,13 @@ static int32_t ParserHapticEffect(struct DeviceResourceIface *parser, const stru count = parser->GetElemNum(hapticNode, hapticAttr->name); // Minimum of two elements, including the type and sequence. if (count <= 1 || count > VIBRATOR_HAPTIC_SEQ_MAX) { - HDF_LOGD("%s: haptic [%s] parser seq count fail", __func__, hapticAttr->name); + HDF_LOGE("%s: haptic [%s] parser seq count fail", __func__, hapticAttr->name); continue; } effectNode = MallocEffectNode(count * VIBRATOR_HAPTIC_SEQ_SIZE); if (effectNode == NULL) { - HDF_LOGD("%s: malloc effect effectNode fail", __func__); + HDF_LOGE("%s: malloc effect effectNode fail", __func__); continue; } effectNode->effect = hapticAttr->name; @@ -201,17 +201,14 @@ void HapticTimerEntry(uintptr_t para) if (hapticData->effectType == VIBRATOR_TYPE_TIME) { duration = ProcessHapticTime(hapticData); - HDF_LOGE("%s:ProcessHapticTime duration[%d]", __func__, duration); } if (hapticData->effectType == VIBRATOR_TYPE_EFFECT) { duration = ProcessHapticEffect(hapticData); - HDF_LOGE("%s:ProcessHapticEffect duration[%d]", __func__, duration); } duration = ((duration > 0) && (duration < VIBRATOR_MIN_WAIT_TIME)) ? VIBRATOR_MIN_WAIT_TIME : duration; if ((duration > 0) && (OsalTimerSetTimeout(&hapticData->timer, duration) == HDF_SUCCESS)) { - HDF_LOGD("%s: modify haptic timer duration[%d]", __func__, duration); return; } diff --git a/model/sensor/driver/chipset/gyro/gyro_bmi160.c b/model/sensor/driver/chipset/gyro/gyro_bmi160.c new file mode 100644 index 00000000..f044a650 --- /dev/null +++ b/model/sensor/driver/chipset/gyro/gyro_bmi160.c @@ -0,0 +1,152 @@ +/* + * 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 "securec.h" +#include "gyro_bmi160.h" +#include "osal_time.h" +#include "sensor_gyro_driver.h" +#include "sensor_config_controller.h" +#include "sensor_device_manager.h" + +/* 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 ReadBmi160GyroRawData(struct SensorCfgData *data, struct GyroData *rawData, int64_t *timestamp) +{ + uint8_t status = 0; + uint8_t reg[GYRO_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, BMI160_STATUS_ADDR, &status, sizeof(uint8_t)); + if (!(status & BMI160_GYRO_DATA_READY_MASK) || (ret != HDF_SUCCESS)) { + return HDF_FAILURE; + } + + ret = ReadSensor(&data->busCfg, BMI160_GYRO_X_LSB_ADDR, ®[GYRO_X_AXIS_LSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, BMI160_GYRO_X_MSB_ADDR, ®[GYRO_X_AXIS_MSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, BMI160_GYRO_Y_LSB_ADDR, ®[GYRO_Y_AXIS_LSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, BMI160_GYRO_Y_MSB_ADDR, ®[GYRO_Y_AXIS_MSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, BMI160_GYRO_Z_LSB_ADDR, ®[GYRO_Z_AXIS_LSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + ret = ReadSensor(&data->busCfg, BMI160_GYRO_Z_MSB_ADDR, ®[GYRO_Z_AXIS_MSB], sizeof(uint8_t)); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "read data"); + + rawData->x = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_X_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) | + reg[GYRO_X_AXIS_LSB]); + rawData->y = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_Y_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) | + reg[GYRO_Y_AXIS_LSB]); + rawData->z = (int16_t)(SENSOR_DATA_SHIFT_LEFT(reg[GYRO_Z_AXIS_MSB], SENSOR_DATA_WIDTH_8_BIT) | + reg[GYRO_Z_AXIS_LSB]); + + return ret; +} + +int32_t ReadBmi160GyroData(struct SensorCfgData *data) +{ + int32_t ret; + struct GyroData rawData = { 0, 0, 0 }; + int32_t tmp[GYRO_AXIS_NUM]; + struct SensorReportEvent event; + + (void)memset_s(&event, sizeof(event), 0, sizeof(event)); + + ret = ReadBmi160GyroRawData(data, &rawData, &event.timestamp); + if (ret != HDF_SUCCESS) { + return HDF_FAILURE; + } + + event.sensorId = SENSOR_TAG_GYROSCOPE; + event.option = 0; + event.mode = SENSOR_WORK_MODE_REALTIME; + + tmp[GYRO_X_AXIS] = rawData.x * BMI160_GYRO_SENSITIVITY_2000DPS; + tmp[GYRO_Y_AXIS] = rawData.y * BMI160_GYRO_SENSITIVITY_2000DPS; + tmp[GYRO_Z_AXIS] = rawData.z * BMI160_GYRO_SENSITIVITY_2000DPS; + + event.dataLen = sizeof(tmp); + event.data = (uint8_t *)&tmp; + ret = ReportSensorEvent(&event); + return ret; +} + +static int32_t InitBmi160Gyro(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: BMI160 sensor init config failed", __func__); + return HDF_FAILURE; + } + return HDF_SUCCESS; +} + +static int32_t InitGyroPreConfig(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; +} + +int32_t DetectGyroBim160Chip(struct SensorCfgData *data) +{ + int32_t ret; + struct GyroOpsCall ops; + CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM); + + if (strcmp(GYRO_CHIP_NAME_BMI160, data->sensorAttr.chipName) != 0) { + return HDF_SUCCESS; + } + ret = InitGyroPreConfig(); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: init BMI160 bus mux config", __func__); + return HDF_FAILURE; + } + if (DetectSensorDevice(data) != HDF_SUCCESS) { + return HDF_FAILURE; + } + ops.Init = InitBmi160Gyro; + ops.ReadData = ReadBmi160GyroData; + ret = RegisterGyroChipOps(&ops); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: register BMI160 gyro failed", __func__); + (void)ReleaseSensorBusHandle(&data->busCfg); + return HDF_FAILURE; + } + return HDF_SUCCESS; +} diff --git a/model/sensor/driver/chipset/gyro/gyro_bmi160.h b/model/sensor/driver/chipset/gyro/gyro_bmi160.h new file mode 100644 index 00000000..5e71e306 --- /dev/null +++ b/model/sensor/driver/chipset/gyro/gyro_bmi160.h @@ -0,0 +1,54 @@ +/* + * 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 GYRO_BMI160_H +#define GYRO_BMI160_H + +#include "sensor_config_parser.h" + +/* GYRO DATA REGISTERS ADDR */ +#define BMI160_GYRO_X_LSB_ADDR 0X0C +#define BMI160_GYRO_X_MSB_ADDR 0X0D +#define BMI160_GYRO_Y_LSB_ADDR 0X0E +#define BMI160_GYRO_Y_MSB_ADDR 0X0F +#define BMI160_GYRO_Z_LSB_ADDR 0X10 +#define BMI160_GYRO_Z_MSB_ADDR 0X11 +#define BMI160_STATUS_ADDR 0X1B + +/* GYRO ODR */ +#define BMI160_GYRO_ODR_RESERVED 0x00 +#define BMI160_GYRO_ODR_25HZ 0x06 +#define BMI160_GYRO_ODR_50HZ 0x07 +#define BMI160_GYRO_ODR_100HZ 0x08 +#define BMI160_GYRO_ODR_200HZ 0x09 +#define BMI160_GYRO_ODR_400HZ 0x0A +#define BMI160_GYRO_ODR_800HZ 0x0B +#define BMI160_GYRO_ODR_1600HZ 0x0C +#define BMI160_GYRO_ODR_3200HZ 0x0D + +/* default HZ */ +#define BMI160_GYRO_DEFAULT_ODR_100HZ 100 +#define BMI160_GYRO_DEFAULT_ODR_25HZ 25 + +/* GYRO RANGE */ +#define BMI160_GYRO_RANGE_2000DPS 0X00 +#define BMI160_GYRO_RANGE_1000DPS 0X01 +#define BMI160_GYRO_RANGE_500DPS 0X02 +#define BMI160_GYRO_RANGE_250DPS 0X03 +#define BMI160_GYRO_RANGE_125DPS 0X04 + +/* GYRO sensitivity */ +#define BMI160_GYRO_SENSITIVITY_2000DPS 61 + +/* GYRO DATA READY */ +#define BMI160_GYRO_DATA_READY_MASK 0x40 + +int32_t DetectGyroBim160Chip(struct SensorCfgData *data); +int32_t ReadBmi160Data(struct SensorCfgData *data); + +#endif /* GYRO_BMI160_H */ diff --git a/model/sensor/driver/common/src/sensor_config_parser.c b/model/sensor/driver/common/src/sensor_config_parser.c index c91f8d0e..c844da6f 100644 --- a/model/sensor/driver/common/src/sensor_config_parser.c +++ b/model/sensor/driver/common/src/sensor_config_parser.c @@ -162,7 +162,7 @@ int32_t ParseSensorRegConfig(struct SensorCfgData *config) parser = DeviceResourceGetIfaceInstance(HDF_CONFIG_SOURCE); CHECK_NULL_PTR_RETURN_VALUE(parser, HDF_ERR_INVALID_PARAM); - regCfgNode = parser->GetChildNode(config->root, "accelRegConfig"); + regCfgNode = parser->GetChildNode(config->root, "sensorRegConfig"); CHECK_NULL_PTR_RETURN_VALUE(regCfgNode, HDF_ERR_INVALID_PARAM); DEV_RES_NODE_FOR_EACH_ATTR(regCfgNode, regAttr) { @@ -386,18 +386,18 @@ int32_t GetSensorBaseConfigData(const struct DeviceResourceNode *node, struct Se config->root = node; CHECK_NULL_PTR_RETURN_VALUE(parser->GetChildNode, HDF_ERR_INVALID_PARAM); - const struct DeviceResourceNode *infoNode = parser->GetChildNode(node, "accelInfo"); + const struct DeviceResourceNode *infoNode = parser->GetChildNode(node, "sensorInfo"); CHECK_NULL_PTR_RETURN_VALUE(infoNode, HDF_ERR_INVALID_PARAM); - const struct DeviceResourceNode *busNode = parser->GetChildNode(node, "accelBusConfig"); + const struct DeviceResourceNode *busNode = parser->GetChildNode(node, "sensorBusConfig"); CHECK_NULL_PTR_RETURN_VALUE(busNode, HDF_ERR_INVALID_PARAM); - const struct DeviceResourceNode *attrNode = parser->GetChildNode(node, "accelAttr"); + const struct DeviceResourceNode *attrNode = parser->GetChildNode(node, "sensorIdAttr"); CHECK_NULL_PTR_RETURN_VALUE(attrNode, HDF_ERR_INVALID_PARAM); ret = ParseSensorInfo(parser, infoNode, config); - CHECK_PARSER_RESULT_RETURN_VALUE(ret, "accelInfo"); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorInfo"); ret = ParseSensorBus(parser, busNode, config); - CHECK_PARSER_RESULT_RETURN_VALUE(ret, "accelBusConfig"); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorBusConfig"); ret = ParseSensorAttr(parser, attrNode, config); - CHECK_PARSER_RESULT_RETURN_VALUE(ret, "accelAttr"); + CHECK_PARSER_RESULT_RETURN_VALUE(ret, "sensorIdAttr"); return HDF_SUCCESS; } \ No newline at end of file diff --git a/model/sensor/driver/gyro/sensor_gyro_driver.c b/model/sensor/driver/gyro/sensor_gyro_driver.c new file mode 100644 index 00000000..290b0f4c --- /dev/null +++ b/model/sensor/driver/gyro/sensor_gyro_driver.c @@ -0,0 +1,392 @@ +/* + * 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 "securec.h" +#include "gyro_bmi160.h" +#include "hdf_base.h" +#include "hdf_device_desc.h" +#include "osal_math.h" +#include "osal_mem.h" +#include "sensor_gyro_driver.h" +#include "sensor_config_controller.h" +#include "sensor_device_manager.h" +#include "sensor_platform_if.h" + +#define HDF_LOG_TAG sensor_gyro_driver_c + +#define HDF_GYRO_WORK_QUEUE_NAME "hdf_gyro_work_queue" + +static struct GyroDetectIfList g_gyroDetectIfList[] = { + {GYRO_CHIP_NAME_BMI160, DetectGyroBim160Chip}, +}; + +static struct GyroDrvData *g_gyroDrvData = NULL; + +static struct GyroDrvData *GyroGetDrvData(void) +{ + return g_gyroDrvData; +} + +static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL }; + +int32_t RegisterGyroChipOps(const struct GyroOpsCall *ops) +{ + struct GyroDrvData *drvData = NULL; + + CHECK_NULL_PTR_RETURN_VALUE(ops, HDF_ERR_INVALID_PARAM); + + drvData = GyroGetDrvData(); + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + drvData->ops.Init = ops->Init; + drvData->ops.ReadData = ops->ReadData; + return HDF_SUCCESS; +} + +static void GyroDataWorkEntry(void *arg) +{ + int32_t ret; + struct GyroDrvData *drvData = (struct GyroDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + CHECK_NULL_PTR_RETURN(drvData->ops.ReadData); + + ret = drvData->ops.ReadData(drvData->gyroCfg); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro read data failed", __func__); + return; + } +} + +static void GyroTimerEntry(uintptr_t arg) +{ + int64_t interval; + int32_t ret; + struct GyroDrvData *drvData = (struct GyroDrvData *)arg; + CHECK_NULL_PTR_RETURN(drvData); + + if (!HdfAddWork(&drvData->gyroWorkQueue, &drvData->gyroWork)) { + HDF_LOGE("%s: gyro 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->gyroTimer, interval); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro modify time failed", __func__); + } +} + +static int32_t InitGyroData(void) +{ + struct GyroDrvData *drvData = GyroGetDrvData(); + int32_t ret; + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + if (drvData->initStatus) { + return HDF_SUCCESS; + } + + if (HdfWorkQueueInit(&drvData->gyroWorkQueue, HDF_GYRO_WORK_QUEUE_NAME) != HDF_SUCCESS) { + HDF_LOGE("%s: gyro init work queue failed", __func__); + return HDF_FAILURE; + } + + if (HdfWorkInit(&drvData->gyroWork, GyroDataWorkEntry, drvData) != HDF_SUCCESS) { + HDF_LOGE("%s: gyro create thread failed", __func__); + return HDF_FAILURE; + } + + CHECK_NULL_PTR_RETURN_VALUE(drvData->ops.Init, HDF_ERR_INVALID_PARAM); + + ret = drvData->ops.Init(drvData->gyroCfg); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro create thread failed", __func__); + return HDF_FAILURE; + } + + drvData->interval = SENSOR_TIMER_MIN_TIME; + drvData->initStatus = true; + drvData->enable = false; + + return HDF_SUCCESS; +} + +static int32_t SetGyroEnable(void) +{ + int32_t ret; + struct GyroDrvData *drvData = GyroGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->gyroCfg, HDF_ERR_INVALID_PARAM); + + if (drvData->enable) { + HDF_LOGE("%s: gyro sensor is enabled", __func__); + return HDF_SUCCESS; + } + + ret = SetSensorRegCfgArray(&drvData->gyroCfg->busCfg, drvData->gyroCfg->regCfgGroup[SENSOR_ENABLE_GROUP]); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro sensor enable config failed", __func__); + return ret; + } + + ret = OsalTimerCreate(&drvData->gyroTimer, SENSOR_TIMER_MIN_TIME, GyroTimerEntry, (uintptr_t)drvData); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro create timer failed[%d]", __func__, ret); + return ret; + } + + ret = OsalTimerStartLoop(&drvData->gyroTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro start timer failed[%d]", __func__, ret); + return ret; + } + drvData->enable = true; + + return HDF_SUCCESS; +} + +static int32_t SetGyroDisable(void) +{ + int32_t ret; + struct GyroDrvData *drvData = GyroGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->gyroCfg, HDF_ERR_INVALID_PARAM); + + if (!drvData->enable) { + HDF_LOGE("%s: gyro sensor had disable", __func__); + return HDF_SUCCESS; + } + + ret = SetSensorRegCfgArray(&drvData->gyroCfg->busCfg, drvData->gyroCfg->regCfgGroup[SENSOR_DISABLE_GROUP]); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro sensor disable config failed", __func__); + return ret; + } + + ret = OsalTimerDelete(&drvData->gyroTimer); + if (ret != HDF_SUCCESS) { + HDF_LOGE("%s: gyro delete timer failed", __func__); + return ret; + } + drvData->enable = false; + return HDF_SUCCESS; +} + +static int32_t SetGyroBatch(int64_t samplingInterval, int64_t interval) +{ + (void)interval; + + struct GyroDrvData *drvData = NULL; + + drvData = GyroGetDrvData(); + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + drvData->interval = samplingInterval; + + return HDF_SUCCESS; +} + +static int32_t SetGyroMode(int32_t mode) +{ + return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE; +} + +static int32_t SetGyroOption(uint32_t option) +{ + (void)option; + return HDF_SUCCESS; +} + +static int32_t DispatchGyro(struct HdfDeviceIoClient *client, + int cmd, struct HdfSBuf *data, struct HdfSBuf *reply) +{ + (void)client; + (void)cmd; + (void)data; + (void)reply; + + return HDF_SUCCESS; +} + +int32_t BindGyroDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + + struct GyroDrvData *drvData = (struct GyroDrvData *)OsalMemCalloc(sizeof(*drvData)); + if (drvData == NULL) { + HDF_LOGE("%s: malloc gyro drv data fail!", __func__); + return HDF_ERR_MALLOC_FAIL; + } + + drvData->ioService.Dispatch = DispatchGyro; + drvData->device = device; + device->service = &drvData->ioService; + g_gyroDrvData = drvData; + return HDF_SUCCESS; +} + +static int32_t InitGyroOps(struct SensorDeviceInfo *deviceInfo) +{ + struct GyroDrvData *drvData = GyroGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + deviceInfo->ops.Enable = SetGyroEnable; + deviceInfo->ops.Disable = SetGyroDisable; + deviceInfo->ops.SetBatch = SetGyroBatch; + deviceInfo->ops.SetMode = SetGyroMode; + deviceInfo->ops.SetOption = SetGyroOption; + + if (memcpy_s(&deviceInfo->sensorInfo, sizeof(deviceInfo->sensorInfo), + &drvData->gyroCfg->sensorInfo, sizeof(drvData->gyroCfg->sensorInfo)) != EOK) { + HDF_LOGE("%s: copy sensor info failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t InitGyroAfterConfig(void) +{ + struct SensorDeviceInfo deviceInfo; + + if (InitGyroData() != HDF_SUCCESS) { + HDF_LOGE("%s: init gyro config failed", __func__); + return HDF_FAILURE; + } + + if (InitGyroOps(&deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: init gyro ops failed", __func__); + return HDF_FAILURE; + } + + if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) { + HDF_LOGE("%s: add gyro device failed", __func__); + return HDF_FAILURE; + } + + return HDF_SUCCESS; +} + +static int32_t DetectGyroChip(void) +{ + int32_t num; + int32_t ret; + int32_t loop; + struct GyroDrvData *drvData = GyroGetDrvData(); + + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + CHECK_NULL_PTR_RETURN_VALUE(drvData->gyroCfg, HDF_ERR_INVALID_PARAM); + + num = sizeof(g_gyroDetectIfList) / sizeof(g_gyroDetectIfList[0]); + for (loop = 0; loop < num; ++loop) { + if (g_gyroDetectIfList[loop].DetectChip != NULL) { + ret = g_gyroDetectIfList[loop].DetectChip(drvData->gyroCfg); + if (ret == HDF_SUCCESS) { + drvData->detectFlag = true; + return HDF_SUCCESS; + } + } + } + + HDF_LOGE("%s: detect gyro device failed", __func__); + drvData->detectFlag = false; + return HDF_FAILURE; +} + +int32_t InitGyroDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM); + struct GyroDrvData *drvData = (struct GyroDrvData *)device->service; + CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM); + + if (drvData->detectFlag) { + HDF_LOGE("%s: gyro sensor have detected", __func__); + return HDF_SUCCESS; + } + + drvData->gyroCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->gyroCfg)); + if (drvData->gyroCfg == NULL) { + HDF_LOGE("%s: malloc sensor config data failed", __func__); + return HDF_FAILURE; + } + + drvData->gyroCfg->regCfgGroup = &g_regCfgGroup[0]; + + if (GetSensorBaseConfigData(device->property, drvData->gyroCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: get sensor base config failed", __func__); + goto BASE_CONFIG_EXIT; + } + + // if return failure, hdf framework go to next detect sensor + if (DetectGyroChip() != HDF_SUCCESS) { + HDF_LOGE("%s: gyro sensor detect device no exist", __func__); + goto DETECT_CHIP_EXIT; + } + drvData->detectFlag = true; + + if (ParseSensorRegConfig(drvData->gyroCfg) != HDF_SUCCESS) { + HDF_LOGE("%s: detect sensor device failed", __func__); + goto REG_CONFIG_EXIT; + } + + if (InitGyroAfterConfig() != HDF_SUCCESS) { + HDF_LOGE("%s: init gyro after config failed", __func__); + goto INIT_EXIT; + } + + HDF_LOGI("%s: init gyro driver success", __func__); + return HDF_SUCCESS; + +INIT_EXIT: + (void)DeleteSensorDevice(&drvData->gyroCfg->sensorInfo); +REG_CONFIG_EXIT: + ReleaseSensorAllRegConfig(drvData->gyroCfg); + (void)ReleaseSensorBusHandle(&drvData->gyroCfg->busCfg); +DETECT_CHIP_EXIT: + drvData->detectFlag = false; +BASE_CONFIG_EXIT: + drvData->gyroCfg->root = NULL; + drvData->gyroCfg->regCfgGroup = NULL; + OsalMemFree(drvData->gyroCfg); + drvData->gyroCfg = NULL; + return HDF_FAILURE; +} + +void ReleaseGyroDriver(struct HdfDeviceObject *device) +{ + CHECK_NULL_PTR_RETURN(device); + + struct GyroDrvData *drvData = (struct GyroDrvData *)device->service; + CHECK_NULL_PTR_RETURN(drvData); + + (void)DeleteSensorDevice(&drvData->gyroCfg->sensorInfo); + drvData->detectFlag = false; + + if (drvData->gyroCfg != NULL) { + drvData->gyroCfg->root = NULL; + drvData->gyroCfg->regCfgGroup = NULL; + ReleaseSensorAllRegConfig(drvData->gyroCfg); + (void)ReleaseSensorBusHandle(&drvData->gyroCfg->busCfg); + OsalMemFree(drvData->gyroCfg); + drvData->gyroCfg = NULL; + } + + drvData->initStatus = false; +} + +struct HdfDriverEntry g_sensorGyroDevEntry = { + .moduleVersion = 1, + .moduleName = "HDF_SENSOR_GYRO", + .Bind = BindGyroDriver, + .Init = InitGyroDriver, + .Release = ReleaseGyroDriver, +}; + +HDF_INIT(g_sensorGyroDevEntry); diff --git a/model/sensor/driver/gyro/sensor_gyro_driver.h b/model/sensor/driver/gyro/sensor_gyro_driver.h new file mode 100644 index 00000000..bb63e10d --- /dev/null +++ b/model/sensor/driver/gyro/sensor_gyro_driver.h @@ -0,0 +1,70 @@ +/* + * 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_GYRO_DRIVER_H +#define SENSOR_GYRO_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 GYRO_DEFAULT_SAMPLING_200_MS 200000000 +#define GYRO_CHIP_NAME_BMI160 "bmi160" + +enum GyroAxisNum { + GYRO_X_AXIS = 0, + GYRO_Y_AXIS = 1, + GYRO_Z_AXIS = 2, + GYRO_AXIS_NUM = 3, +}; + +enum GyroAxisPart { + GYRO_X_AXIS_LSB = 0, + GYRO_X_AXIS_MSB = 1, + GYRO_Y_AXIS_LSB = 2, + GYRO_Y_AXIS_MSB = 3, + GYRO_Z_AXIS_LSB = 4, + GYRO_Z_AXIS_MSB = 5, + GYRO_AXIS_BUTT, +}; + +struct GyroData { + int32_t x; + int32_t y; + int32_t z; +}; + +struct GyroDetectIfList { + char *chipName; + int32_t (*DetectChip)(struct SensorCfgData *data); +}; + +struct GyroOpsCall { + int32_t (*Init)(struct SensorCfgData *data); + int32_t (*ReadData)(struct SensorCfgData *data); +}; + +struct GyroDrvData { + struct IDeviceIoService ioService; + struct HdfDeviceObject *device; + HdfWorkQueue gyroWorkQueue; + HdfWork gyroWork; + OsalTimer gyroTimer; + bool detectFlag; + bool enable; + bool initStatus; + int64_t interval; + struct SensorCfgData *gyroCfg; + struct GyroOpsCall ops; +}; + +int32_t RegisterGyroChipOps(const struct GyroOpsCall *ops); + +#endif /* SENSOR_GYRO_DRIVER_H */ diff --git a/model/sensor/driver/include/sensor_platform_if.h b/model/sensor/driver/include/sensor_platform_if.h index bc91bf73..6bf64210 100644 --- a/model/sensor/driver/include/sensor_platform_if.h +++ b/model/sensor/driver/include/sensor_platform_if.h @@ -44,10 +44,10 @@ #define SENSOR_DATA_WIDTH_8_BIT 8 // 8 bit #define SENSOR_CONVERT_UNIT 1000 #define SENSOR_1K_UNIT 1024 -#define SENSOR_SPI_MAX_SPEED 115200 +#define SENSOR_SPI_MAX_SPEED 115200 #define SENSOR_SECOND_CONVERT_NANOSECOND (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT) -#define SENSOR_TIMER_MIN_TIME 20 +#define SENSOR_TIMER_MIN_TIME 20 enum SensorBusType { SENSOR_BUS_I2C = 0, @@ -76,16 +76,6 @@ struct SensorBusCfg { }; }; -enum SensorThreadStatus { - SENSOR_THREAD_NONE = 0, - SENSOR_THREAD_START = 1, - SENSOR_THREAD_RUNNING = 2, - SENSOR_THREAD_STOPPING = 3, - SENSOR_THREAD_STOPPED = 4, - SENSOR_THREAD_DESTROY = 5, - SENSOR_THREAD_STATUS_BUT, -}; - enum SENSORConfigValueIndex { SENSOR_ADDR_INDEX, SENSOR_VALUE_INDEX,