feat: add gravity drive v10

Signed-off-by: YOUR_NAME <xuwenbin10@huawei.com>

model/sensor/driver/accel/sensor_accel_driver.c

@@ -29,6 +29,19 @@ static struct AccelDrvData *AccelGetDrvData(void)
 
 static struct SensorRegCfgGroupNode *g_regCfgGroup[SENSOR_GROUP_MAX] = { NULL };
 
+int32_t SubscribeAccelDataCallbackFunc(GravitySubscribeAccelCallback cb)
+{
+    CHECK_NULL_PTR_RETURN_VALUE(cb, HDF_ERR_INVALID_PARAM);
+
+    struct AccelDrvData *drvData = AccelGetDrvData();
+
+    CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
+
+    drvData->cb = cb;
+
+    return HDF_SUCCESS;
+}
+
 int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops)
 {
     struct AccelDrvData *drvData = AccelGetDrvData();
@@ -44,6 +57,7 @@ int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops)
 static void AccelDataWorkEntry(void *arg)
 {
     struct AccelDrvData *drvData = NULL;
+    struct SensorReportEvent event;
 
     drvData = (struct AccelDrvData *)arg;
     CHECK_NULL_PTR_RETURN(drvData);
@@ -52,8 +66,19 @@ static void AccelDataWorkEntry(void *arg)
         HDF_LOGI("%s: Accel ReadData function NULl", __func__);
         return;
     }
-    if (drvData->ops.ReadData(drvData->accelCfg) != HDF_SUCCESS) {
+
+    if (drvData->ops.ReadData(drvData->accelCfg, &event) != HDF_SUCCESS) {
         HDF_LOGE("%s: Accel read data failed", __func__);
+        return;
+    }
+
+    if (ReportSensorEvent(&event) != HDF_SUCCESS) {
+        HDF_LOGE("%s: report accel data failed", __func__);
+        return;
+    }
+
+    if (drvData->cb != NULL) {
+        drvData->cb((int32_t*)event.data, event.dataLen);
     }
 }
 
@@ -342,6 +367,7 @@ int32_t AccelInitDriver(struct HdfDeviceObject *device)
     }
 
     drvData->accelCfg->regCfgGroup = &g_regCfgGroup[0];
+    drvData->cb = NULL;
 
     HDF_LOGI("%s: Init accel driver success", __func__);
     return HDF_SUCCESS;

model/sensor/driver/accel/sensor_accel_driver.h

@@ -32,6 +32,8 @@ enum AccelAxisPart {
     ACCEL_AXIS_BUTT,
 };
 
+typedef int32_t (*GravitySubscribeAccelCallback)(int32_t *rawData, int32_t size);
+
 struct AccelData {
     int32_t x;
     int32_t y;
@@ -40,7 +42,7 @@ struct AccelData {
 
 struct AccelOpsCall {
     int32_t (*Init)(struct SensorCfgData *data);
-    int32_t (*ReadData)(struct SensorCfgData *data);
+    int32_t (*ReadData)(struct SensorCfgData *data, struct SensorReportEvent *event);
 };
 
 struct AccelDrvData {
@@ -54,10 +56,11 @@ struct AccelDrvData {
     int64_t interval;
     struct SensorCfgData *accelCfg;
     struct AccelOpsCall ops;
+    GravitySubscribeAccelCallback cb;
 };
 
 int32_t AccelRegisterChipOps(const struct AccelOpsCall *ops);
 struct SensorCfgData *AccelCreateCfgData(const struct DeviceResourceNode *node);
 void AccelReleaseCfgData(struct SensorCfgData *sensorCfgData);
-
+int32_t SubscribeAccelDataCallbackFunc(GravitySubscribeAccelCallback cb);
 #endif /* SENSOR_ACCEL_DRIVER_H */

model/sensor/driver/accel/sensor_gravity_driver.c

@@ -0,0 +1,335 @@
+/*
+ * 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_gravity_driver.h"
+#include "sensor_accel_driver.h"
+#include <securec.h>
+#include "hdf_base.h"
+#include "hdf_device_desc.h"
+#include "osal_math.h"
+#include "osal_mem.h"
+#include "osal_time.h"
+#include "sensor_device_manager.h"
+#include "sensor_platform_if.h"
+
+#define HDF_LOG_TAG    sensor_gravity_driver_c
+
+#define HDF_GRAVITY_WORK_QUEUE_NAME    "hdf_gravity_work_queue"
+
+static struct GravityDrvData *g_gravityDrvData = NULL;
+static int32_t g_accelRawData[GRAVITY_AXIS_NUM];
+
+static struct GravityDrvData *GravityGetDrvData(void)
+{
+    return g_gravityDrvData;
+}
+
+static int32_t GravitySubscribeAccelData(int32_t *accelData, int32_t size)
+{
+    CHECK_NULL_PTR_RETURN_VALUE(accelData, HDF_ERR_INVALID_PARAM);
+
+    if (size > GRAVITY_AXIS_NUM * sizeof(int32_t)) {
+        HDF_LOGE("%s: size exceed max value", __func__);
+        return HDF_FAILURE;
+    }
+
+    g_accelRawData[GRAVITY_X_AXIS] = accelData[GRAVITY_X_AXIS];
+    g_accelRawData[GRAVITY_Y_AXIS] = accelData[GRAVITY_Y_AXIS];
+    g_accelRawData[GRAVITY_Z_AXIS] = accelData[GRAVITY_Z_AXIS];
+
+    return HDF_SUCCESS;
+}
+
+static void GravityDataWorkEntry(void *arg)
+{
+    struct GravityDrvData *drvData = NULL;
+    static int32_t GravityRawData[GRAVITY_AXIS_NUM];
+    struct SensorReportEvent event;
+    drvData = (struct GravityDrvData *)arg;
+    CHECK_NULL_PTR_RETURN(drvData);
+    OsalTimespec time;
+
+    GravityRawData[GRAVITY_X_AXIS] = (85 * (GravityRawData[GRAVITY_X_AXIS] - g_accelRawData[GRAVITY_X_AXIS]) +
+        g_accelRawData[GRAVITY_X_AXIS] * 100) / 100;
+    GravityRawData[GRAVITY_Y_AXIS] = (85 * (GravityRawData[GRAVITY_Y_AXIS] - g_accelRawData[GRAVITY_Y_AXIS]) +
+        g_accelRawData[GRAVITY_Y_AXIS] * 100) / 100;
+    GravityRawData[GRAVITY_Z_AXIS] = (85 * (GravityRawData[GRAVITY_Z_AXIS] - g_accelRawData[GRAVITY_Z_AXIS]) +
+        g_accelRawData[GRAVITY_Z_AXIS] * 100) / 100;
+
+    if (OsalGetTime(&time) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Get time failed", __func__);
+        return;
+    }
+    event.timestamp = time.sec * SENSOR_SECOND_CONVERT_NANOSECOND + time.usec * SENSOR_CONVERT_UNIT; /* unit nanosecond */
+    event.sensorId = SENSOR_TAG_GRAVITY;
+    event.option = 0;
+    event.mode = SENSOR_WORK_MODE_REALTIME;
+    event.dataLen = sizeof(GravityRawData);
+    event.data = (uint8_t *)&GravityRawData;
+
+    if (ReportSensorEvent(&event) != HDF_SUCCESS) {
+        HDF_LOGE("%s: report gravity data failed", __func__);
+    }
+}
+
+static void GravityTimerEntry(uintptr_t arg)
+{
+    int32_t ret;
+    struct GravityDrvData *drvData = (struct GravityDrvData *)arg;
+    CHECK_NULL_PTR_RETURN(drvData);
+
+    if (!HdfAddWork(&drvData->gravityWorkQueue, &drvData->gravityWork)) {
+        HDF_LOGE("%s: Gravity add work queue failed", __func__);
+    }
+
+    ret = OsalTimerSetTimeout(&drvData->gravityTimer, drvData->interval);
+    if (ret != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity modify time failed", __func__);
+    }
+}
+
+static int32_t InitGravityData(struct GravityDrvData *drvData)
+{
+    if (HdfWorkQueueInit(&drvData->gravityWorkQueue, HDF_GRAVITY_WORK_QUEUE_NAME) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity init work queue failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    if (HdfWorkInit(&drvData->gravityWork, GravityDataWorkEntry, drvData) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity create thread failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    drvData->interval = GRAVITY_TIMER_MAX_TIME;
+    drvData->enable = false;
+
+    return HDF_SUCCESS;
+}
+
+static int32_t SetGravityEnable(void)
+{
+    int32_t ret;
+    struct GravityDrvData *drvData = GravityGetDrvData();
+
+    CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
+    CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM);
+
+    if (drvData->enable) {
+        HDF_LOGE("%s: Gravity sensor is enabled", __func__);
+        return HDF_SUCCESS;
+    }
+
+    ret = OsalTimerCreate(&drvData->gravityTimer, GRAVITY_TIMER_MAX_TIME, GravityTimerEntry, (uintptr_t)drvData);
+    if (ret != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity create timer failed[%d]", __func__, ret);
+        return ret;
+    }
+
+    ret = OsalTimerStartLoop(&drvData->gravityTimer);
+    if (ret != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity start timer failed[%d]", __func__, ret);
+        return ret;
+    }
+    drvData->enable = true;
+
+    return HDF_SUCCESS;
+}
+
+static int32_t SetGravityDisable(void)
+{
+    int32_t ret;
+    struct GravityDrvData *drvData = GravityGetDrvData();
+
+    CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
+    CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM);
+
+    if (!drvData->enable) {
+        HDF_LOGE("%s: Gravity sensor had disable", __func__);
+        return HDF_SUCCESS;
+    }
+
+    ret = OsalTimerDelete(&drvData->gravityTimer);
+    if (ret != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity delete timer failed", __func__);
+        return ret;
+    }
+    drvData->enable = false;
+
+    return HDF_SUCCESS;
+}
+
+static int32_t SetGravityBatch(int64_t samplingInterval, int64_t interval)
+{
+    (void)interval;
+
+    int64_t ms;
+    struct GravityDrvData *drvData = NULL;
+
+    drvData = GravityGetDrvData();
+    CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
+
+    ms = OsalDivS64(samplingInterval, (SENSOR_CONVERT_UNIT * SENSOR_CONVERT_UNIT));
+
+    drvData->interval = (ms <= GRAVITY_TIMER_MIN_TIME) ? GRAVITY_TIMER_MIN_TIME : GRAVITY_TIMER_MAX_TIME;
+
+    return HDF_SUCCESS;
+}
+
+static int32_t SetGravityMode(int32_t mode)
+{
+    return (mode == SENSOR_WORK_MODE_REALTIME) ? HDF_SUCCESS : HDF_FAILURE;
+}
+
+static int32_t SetGravityOption(uint32_t option)
+{
+    (void)option;
+    return HDF_SUCCESS;
+}
+
+static int32_t DispatchGravity(struct HdfDeviceIoClient *client,
+    int cmd, struct HdfSBuf *data, struct HdfSBuf *reply)
+{
+    (void)client;
+    (void)cmd;
+    (void)data;
+    (void)reply;
+
+    return HDF_SUCCESS;
+}
+
+int32_t GravityBindDriver(struct HdfDeviceObject *device)
+{
+    CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
+
+    struct GravityDrvData *drvData = (struct GravityDrvData *)OsalMemCalloc(sizeof(*drvData));
+    if (drvData == NULL) {
+        HDF_LOGE("%s: Malloc gravity drv data fail!", __func__);
+        return HDF_ERR_MALLOC_FAIL;
+    }
+
+    drvData->ioService.Dispatch = DispatchGravity;
+    drvData->device = device;
+    device->service = &drvData->ioService;
+    g_gravityDrvData = drvData;
+    return HDF_SUCCESS;
+}
+
+static int32_t InitGravityOps(struct SensorCfgData *config, struct SensorDeviceInfo *deviceInfo)
+{
+    CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
+
+    deviceInfo->ops.Enable = SetGravityEnable;
+    deviceInfo->ops.Disable = SetGravityDisable;
+    deviceInfo->ops.SetBatch = SetGravityBatch;
+    deviceInfo->ops.SetMode = SetGravityMode;
+    deviceInfo->ops.SetOption = SetGravityOption;
+
+    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;
+    }
+
+    if (SubscribeAccelDataCallbackFunc(GravitySubscribeAccelData) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Gravity register accel  failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    return HDF_SUCCESS;
+}
+
+static int32_t InitGravityAfterBase(struct SensorCfgData *config)
+{
+    struct SensorDeviceInfo deviceInfo;
+    CHECK_NULL_PTR_RETURN_VALUE(config, HDF_ERR_INVALID_PARAM);
+
+    if (InitGravityOps(config, &deviceInfo) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Init gravity ops failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    if (AddSensorDevice(&deviceInfo) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Add gravity device failed", __func__);
+        return HDF_FAILURE;
+    }
+    return HDF_SUCCESS;
+}
+
+static int32_t GravityCreateCfgData(const struct DeviceResourceNode *node)
+{
+    struct GravityDrvData *drvData = GravityGetDrvData();
+
+    CHECK_NULL_PTR_RETURN_VALUE(node, HDF_ERR_INVALID_PARAM);
+    CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
+    CHECK_NULL_PTR_RETURN_VALUE(drvData->gravityCfg, HDF_ERR_INVALID_PARAM);
+
+    if (GetSensorBaseConfigData(node, drvData->gravityCfg) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Get sensor base config failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    if (InitGravityAfterBase(drvData->gravityCfg) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Init gravity sensor device failed", __func__);
+        return HDF_FAILURE;
+    }
+    return HDF_SUCCESS;
+}
+
+int32_t GravityInitDriver(struct HdfDeviceObject *device)
+{
+    CHECK_NULL_PTR_RETURN_VALUE(device, HDF_ERR_INVALID_PARAM);
+    struct GravityDrvData *drvData = (struct GravityDrvData *)device->service;
+    CHECK_NULL_PTR_RETURN_VALUE(drvData, HDF_ERR_INVALID_PARAM);
+
+    if (InitGravityData(drvData) != HDF_SUCCESS) {
+        HDF_LOGE("%s: Init gravity config failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    drvData->gravityCfg = (struct SensorCfgData *)OsalMemCalloc(sizeof(*drvData->gravityCfg));
+    if (drvData->gravityCfg == NULL) {
+        HDF_LOGE("%s: Malloc gravity config data failed", __func__);
+        return HDF_FAILURE;
+    }
+
+    if (GravityCreateCfgData(device->property) != HDF_SUCCESS) {
+        HDF_LOGE("%s: create gravity device failed", __func__);
+        OsalMemFree(drvData->gravityCfg);
+        return HDF_FAILURE;
+    }
+    HDF_LOGI("%s: Init gravity driver success", __func__);
+    return HDF_SUCCESS;
+}
+
+void GravityReleaseDriver(struct HdfDeviceObject *device)
+{
+    CHECK_NULL_PTR_RETURN(device);
+
+    struct GravityDrvData *drvData = (struct GravityDrvData *)device->service;
+    CHECK_NULL_PTR_RETURN(drvData);
+
+    if (drvData->gravityCfg != NULL) {
+        (void)DeleteSensorDevice(&drvData->gravityCfg->sensorInfo);
+        OsalMemFree(drvData->gravityCfg);
+        drvData->gravityCfg = NULL;
+    }
+
+    HdfWorkDestroy(&drvData->gravityWork);
+    HdfWorkQueueDestroy(&drvData->gravityWorkQueue);
+    OsalMemFree(drvData);
+}
+
+struct HdfDriverEntry g_sensorGravityDevEntry = {
+    .moduleVersion = 1,
+    .moduleName = "HDF_SENSOR_GRAVITY",
+    .Bind = GravityBindDriver,
+    .Init = GravityInitDriver,
+    .Release = GravityReleaseDriver,
+};
+
+HDF_INIT(g_sensorGravityDevEntry);

model/sensor/driver/accel/sensor_gravity_driver.h

@@ -0,0 +1,44 @@
+/*
+ * 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_GRAVITY_DRIVER_H
+#define SENSOR_GRAVITY_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 GRAVITY_TIMER_MIN_TIME           10
+#define GRAVITY_TIMER_MAX_TIME           20
+enum GravityAxisNum {
+    GRAVITY_X_AXIS   = 0,
+    GRAVITY_Y_AXIS   = 1,
+    GRAVITY_Z_AXIS   = 2,
+    GRAVITY_AXIS_NUM = 3,
+};
+
+struct GravityData {
+    int32_t x;
+    int32_t y;
+    int32_t z;
+};
+
+struct GravityDrvData {
+    struct IDeviceIoService ioService;
+    struct HdfDeviceObject *device;
+    HdfWorkQueue gravityWorkQueue;
+    HdfWork gravityWork;
+    OsalTimer gravityTimer;
+    struct SensorCfgData *gravityCfg;
+    int64_t interval;
+    bool enable;
+};
+
+#endif /* SENSOR_GRAVITY_DRIVER_H */

model/sensor/driver/chipset/accel/accel_bmi160.c

@@ -79,24 +79,24 @@ static int32_t ReadBmi160RawData(struct SensorCfgData *data, struct AccelData *r
     return HDF_SUCCESS;
 }
 
-int32_t ReadBmi160Data(struct SensorCfgData *data)
+int32_t ReadBmi160Data(struct SensorCfgData *cfg, struct SensorReportEvent *event)
 {
     int32_t ret;
     struct AccelData rawData = { 0, 0, 0 };
-    int32_t tmp[ACCEL_AXIS_NUM];
-    struct SensorReportEvent event;
+    static int32_t tmp[ACCEL_AXIS_NUM];
 
-    (void)memset_s(&event, sizeof(event), 0, sizeof(event));
+    CHECK_NULL_PTR_RETURN_VALUE(cfg, HDF_ERR_INVALID_PARAM);
+    CHECK_NULL_PTR_RETURN_VALUE(event, HDF_ERR_INVALID_PARAM);
 
-    ret = ReadBmi160RawData(data, &rawData, &event.timestamp);
+    ret = ReadBmi160RawData(cfg, &rawData, &event->timestamp);
     if (ret != HDF_SUCCESS) {
         HDF_LOGE("%s: BMI160 read raw data failed", __func__);
         return HDF_FAILURE;
     }
 
-    event.sensorId = SENSOR_TAG_ACCELEROMETER;
-    event.option = 0;
-    event.mode = SENSOR_WORK_MODE_REALTIME;
+    event->sensorId = SENSOR_TAG_ACCELEROMETER;
+    event->option = 0;
+    event->mode = SENSOR_WORK_MODE_REALTIME;
 
     rawData.x = rawData.x * BMI160_ACC_SENSITIVITY_2G;
     rawData.y = rawData.y * BMI160_ACC_SENSITIVITY_2G;
@@ -106,24 +106,21 @@ int32_t ReadBmi160Data(struct SensorCfgData *data)
     tmp[ACCEL_Y_AXIS] = (rawData.y * SENSOR_1K_UNIT) / SENSOR_CONVERT_UNIT;
     tmp[ACCEL_Z_AXIS] = (rawData.z * SENSOR_1K_UNIT) / SENSOR_CONVERT_UNIT;
 
-    ret = SensorRawDataToRemapData(data->direction, tmp, sizeof(tmp) / sizeof(tmp[0]));
+    ret = SensorRawDataToRemapData(cfg->direction, tmp, sizeof(tmp) / sizeof(tmp[0]));
     if (ret != HDF_SUCCESS) {
         HDF_LOGE("%s: BMI160 convert raw data failed", __func__);
         return HDF_FAILURE;
     }
-    
-    event.dataLen = sizeof(tmp);
-    event.data = (uint8_t *)&tmp;
-    ret = ReportSensorEvent(&event);
-    if (ret != HDF_SUCCESS) {
-        HDF_LOGE("%s: BMI160 report data failed", __func__);
-    }
+
+    event->dataLen = sizeof(tmp);
+    event->data = (uint8_t *)&tmp;
+
     return ret;
 }
 
 static int32_t InitBmi160(struct SensorCfgData *data)
 {
-    int32_t ret;
+     int32_t ret;
 
     CHECK_NULL_PTR_RETURN_VALUE(data, HDF_ERR_INVALID_PARAM);
     ret = SetSensorRegCfgArray(&data->busCfg, data->regCfgGroup[SENSOR_INIT_GROUP]);

model/sensor/driver/chipset/accel/accel_bmi160.h

@@ -59,8 +59,7 @@
 #define BMI160_ACCEL_DATA_READY_MASK         0x80
 
 int32_t DetectAccelBim160Chip(struct SensorCfgData *data);
-int32_t ReadBmi160Data(struct SensorCfgData *data);
-
+int32_t ReadBmi160Data(struct SensorCfgData *cfg, struct SensorReportEvent *event);
 struct Bmi160DrvData {
     struct IDeviceIoService ioService;
     struct HdfDeviceObject *device;