[Feature][kernel] add kxtj3 sensor support for hw test
Only Configure:No
Affected branch:MR3.0-merge
Affected module:kernel
Is it affected on both ZXIC and MTK:only MTK
Self-test:Yes
Doc Update:No
Change-Id: I0530110e0bc25aa37b5dfda447018e8426851cac
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/Kconfig b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/Kconfig
new file mode 100644
index 0000000..1598dc1
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/Kconfig
@@ -0,0 +1,9 @@
+config MTK_SENSOR_DRIVERS
+ string "MTK SENSOR DRIVERS"
+ default "test"
+ help
+ Sensor config for sensor drivers in project.
+ Please put sensor drives's folder name in it
+ If unsure, set test.
+
+source "drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Kconfig"
\ No newline at end of file
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/Makefile
new file mode 100644
index 0000000..763abd5
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/Makefile
@@ -0,0 +1,10 @@
+#in case the platform does NOT support this type of sensors
+
+obj-y += core/
+
+ifeq ($(CONFIG_CUSTOM_KERNEL_SENSORHUB),y)
+obj-$(CONFIG_NANOHUB) += mtk_nanohub/
+endif
+
+SENSOR_DRIVER_LIST := $(subst ",,$(CONFIG_MTK_SENSOR_DRIVERS))
+obj-y += $(foreach DRIVER,$(SENSOR_DRIVER_LIST),$(DRIVER)/)
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/Makefile
new file mode 100644
index 0000000..55a1d29
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/Makefile
@@ -0,0 +1,3 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core/
+
+obj-y += bmp280_baro.o
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/bmp280_baro.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/bmp280_baro.c
new file mode 100644
index 0000000..dd9647f
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/bmp280_baro.c
@@ -0,0 +1,1012 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+
+#include "hf_manager.h"
+#include "bmp280_baro.h"
+#include "sensor_list.h"
+
+#define BMP280_BARO_NAME "bmp280_baro"
+//#define DEBUG_DATA 1
+
+#ifdef DEBUG_DATA
+#define BMP_DATA_LOG(fmt, args...) pr_info(fmt, ##args)
+#else
+#define BMP_DATA_LOG(fmt, args...)
+#endif
+
+static unsigned char support_sensors[] = {
+ SENSOR_TYPE_PRESSURE,
+};
+
+/* data type */
+#define BMP280_U16_t u16
+#define BMP280_S16_t s16
+#define BMP280_U32_t u32
+#define BMP280_S32_t s32
+#define BMP280_U64_t u64
+#define BMP280_S64_t s64
+
+#define C_I2C_FIFO_SIZE 8
+#define CHECK_CHIP_ID_TIME_MAX 5
+
+/* power mode */
+enum BMP_POWERMODE_ENUM {
+ BMP280_SLEEP_MODE = 0x00,
+ BMP280_FORCED_MODE = 0x01,
+ BMP280_NORMAL_MODE = 0x03,
+
+ BMP280_UNDEFINED_POWERMODE = 0xff
+};
+
+/* filter */
+enum BMP_FILTER_ENUM {
+ BMP_FILTER_OFF = 0x0,
+ BMP_FILTER_2,
+ BMP_FILTER_4,
+ BMP_FILTER_8,
+ BMP_FILTER_16,
+
+ BMP_UNDEFINED_FILTER = 0xff
+};
+/* oversampling */
+enum BMP_OVERSAMPLING_ENUM {
+ BMP_OVERSAMPLING_SKIPPED = 0x0,
+ BMP_OVERSAMPLING_1X,
+ BMP_OVERSAMPLING_2X,
+ BMP_OVERSAMPLING_4X,
+ BMP_OVERSAMPLING_8X,
+ BMP_OVERSAMPLING_16X,
+
+ BMP_UNDEFINED_OVERSAMPLING = 0xff
+};
+
+/* bmp280 calibration */
+struct bmp280_calibration_data {
+ BMP280_U16_t dig_T1;
+ BMP280_S16_t dig_T2;
+ BMP280_S16_t dig_T3;
+ BMP280_U16_t dig_P1;
+ BMP280_S16_t dig_P2;
+ BMP280_S16_t dig_P3;
+ BMP280_S16_t dig_P4;
+ BMP280_S16_t dig_P5;
+ BMP280_S16_t dig_P6;
+ BMP280_S16_t dig_P7;
+ BMP280_S16_t dig_P8;
+ BMP280_S16_t dig_P9;
+};
+
+#define C_MAX_FIR_LENGTH (32)
+#define BMP_DATA_NUM 1
+
+/* s/w filter */
+struct data_filter {
+ u32 raw[C_MAX_FIR_LENGTH][BMP_DATA_NUM];
+ int sum[BMP_DATA_NUM];
+ int num;
+ int idx;
+};
+
+struct baro_hw {
+ int i2c_num;
+ int direction;
+ int power_id;
+ int power_vol;
+ int firlen; /*!< the length of low pass filter */
+ int (*power)(struct baro_hw *hw, unsigned int on, char *devname);
+ //unsigned char i2c_addr[B_CUST_I2C_ADDR_NUM];
+
+ /*!< i2c address list,for chips which has different addresses with
+ * different HW layout
+ */
+ bool is_batch_supported;
+};
+
+struct bmp280_baro_device {
+ struct hf_device hf_dev;
+ atomic_t raw_enable;
+ enum BMP_POWERMODE_ENUM power_mode;
+ enum BMP_FILTER_ENUM filter_num;
+ enum BMP_OVERSAMPLING_ENUM ovsampling_p;
+ enum BMP_OVERSAMPLING_ENUM ovsampling_t;
+ unsigned long last_temp_measurement;
+ unsigned long temp_measurement_period;
+ struct bmp280_calibration_data bmp280_cali;
+
+ /* calculated temperature correction coefficient */
+ s32 t_fine;
+ atomic_t filter;
+ uint8_t placement[3];
+
+#if defined(CONFIG_BMP_LOWPASS)
+ struct baro_hw hw;
+ atomic_t firlen;
+ atomic_t fir_en;
+ struct data_filter fir;
+#endif
+
+};
+
+/* I2C operation functions */
+static int bmp280_baro_i2c_read_block(struct i2c_client *client,
+ u8 addr, u8 *data, u8 len)
+{
+ int err = 0;
+ u8 beg = addr;
+ struct i2c_msg msgs[2] = {
+ {/*.addr = client->addr,*/
+ .flags = 0,
+ .len = 1,
+ .buf = &beg},
+ {
+ /*.addr = client->addr*/
+ .flags = I2C_M_RD,
+ .len = len,
+ .buf = data,
+ } };
+ if (!client)
+ return -EINVAL;
+ msgs[0].addr = client->addr;
+ msgs[1].addr = client->addr;
+
+ err = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+ if (err != 2) {
+ pr_err_ratelimited("bmp280 i2c_trans err:%x %x (%d %p %d) %d\n",
+ msgs[0].addr, client->addr, addr, data, len,
+ err);
+ err = -EIO;
+ } else {
+ err = 0; /*no error*/
+ }
+ return err;
+}
+
+static int bmp280_baro_i2c_write_block(struct i2c_client *client,
+ u8 addr, u8 *data, u8 len)
+{
+ /* because address also occupies one byte,
+ * the maximum length for write is 7 bytes
+ */
+ int err = 0, idx = 0, num = 0;
+ char buf[32];
+
+ if (!client)
+ return -EINVAL;
+ else if (len > C_I2C_FIFO_SIZE) {
+ pr_err_ratelimited("bmp280 len %d fi %d\n", len,
+ C_I2C_FIFO_SIZE);
+ return -EINVAL;
+ }
+ buf[num++] = addr;
+ for (idx = 0; idx < len; idx++)
+ buf[num++] = data[idx];
+
+ err = i2c_master_send(client, buf, num);
+ if (err < 0) {
+ pr_err_ratelimited("bmp280 send command error!!\n");
+ return -EFAULT;
+ }
+
+ return err;
+}
+
+static int bmp280_baro_soft_reset(struct i2c_client *client)
+{
+ int err = -1;
+ uint8_t data = BMP280_BARO_SOFT_RESET_VALUE;
+
+ err = bmp280_baro_i2c_write_block(client,
+ BMP280_BARO_RESET_ADDR, &data, 1);
+
+ return err;
+}
+
+static int bmp280_baro_check_chip_id(struct i2c_client *client)
+{
+ int err = -1;
+ u8 chip_id = 0;
+ u8 read_count = 0;
+
+ while (read_count++ < CHECK_CHIP_ID_TIME_MAX) {
+ bmp280_baro_i2c_read_block(client,
+ BMP280_BARO_CHIP_ID_ADDR, &chip_id, 1);
+
+ if (chip_id != BMP280_BARO_CHIP_ID_VALUE1 &&
+ chip_id != BMP280_BARO_CHIP_ID_VALUE2 &&
+ chip_id != BMP280_BARO_CHIP_ID_VALUE3) {
+ pr_info("bmp280 get chipId fail(0x%2x).\n", chip_id);
+ } else {
+ err = 0;
+ pr_info("bmp280 get chipId success(0x%2x).\n", chip_id);
+ break;
+ }
+ }
+ return err;
+}
+
+static int bmp280_get_calibration_data(struct i2c_client *client)
+{
+ int status = 0;
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+ u8 a_data_u8r[BMP280_CALIBRATION_DATA_LENGTH] = {0};
+
+ status = bmp280_baro_i2c_read_block(
+ client, BMP280_CALIBRATION_DATA_START, a_data_u8r,
+ BMP280_CALIBRATION_DATA_LENGTH);
+ if (status < 0) {
+ pr_info("bmp280 get cali exit: read data fail!");
+ return status;
+ }
+
+ for (status = 0; status < BMP280_CALIBRATION_DATA_LENGTH; status++)
+ BMP_DATA_LOG("bmp280 cali data: data[%d]:0x%x\n", status,
+ a_data_u8r[status]);
+
+ driver_dev->bmp280_cali.dig_T1 = (BMP280_U16_t)(
+ (((BMP280_U16_t)((unsigned char)a_data_u8r[1]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[0]);
+ driver_dev->bmp280_cali.dig_T2 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[3]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[2]);
+ driver_dev->bmp280_cali.dig_T3 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[5]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[4]);
+ driver_dev->bmp280_cali.dig_P1 = (BMP280_U16_t)(
+ (((BMP280_U16_t)((unsigned char)a_data_u8r[7]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[6]);
+ driver_dev->bmp280_cali.dig_P2 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[9]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[8]);
+ driver_dev->bmp280_cali.dig_P3 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[11]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[10]);
+ driver_dev->bmp280_cali.dig_P4 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[13]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[12]);
+ driver_dev->bmp280_cali.dig_P5 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[15]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[14]);
+ driver_dev->bmp280_cali.dig_P6 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[17]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[16]);
+ driver_dev->bmp280_cali.dig_P7 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[19]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[18]);
+ driver_dev->bmp280_cali.dig_P8 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[21]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[20]);
+ driver_dev->bmp280_cali.dig_P9 = (BMP280_S16_t)(
+ (((BMP280_S16_t)((signed char)a_data_u8r[23]))
+ << SHIFT_LEFT_8_POSITION) |
+ a_data_u8r[22]);
+
+ return 0;
+}
+
+
+static int bmp280_baro_set_powermode(struct i2c_client *client,
+ enum BMP_POWERMODE_ENUM power_mode)
+{
+ int err;
+ u8 data = 0;
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+
+ if (power_mode == BMP280_UNDEFINED_POWERMODE) {
+ pr_info("bmp280 power invalid:%d,(old %d)\n",
+ power_mode, driver_dev->power_mode);
+ err = -1;
+ return err;
+ }
+
+ if (power_mode == driver_dev->power_mode) {
+ pr_info("bmp280 power no change:%d,(old %d)\n",
+ power_mode, driver_dev->power_mode);
+ return 0;
+ }
+
+ err = bmp280_baro_i2c_read_block(client, BMP280_CTRLMEAS_REG_MODE__REG,
+ &data, 1);
+ data = BMP_SET_BITSLICE(data, BMP280_CTRLMEAS_REG_MODE,
+ power_mode);
+ err += bmp280_baro_i2c_write_block(client,
+ BMP280_CTRLMEAS_REG_MODE__REG,
+ &data, 1);
+
+ if (err < 0)
+ pr_err("bmp280 set power mode failed, err = %d\n", err);
+ else
+ driver_dev->power_mode = power_mode;
+
+ return err;
+}
+
+static int bmp280_baro_set_filter(struct i2c_client *client,
+ enum BMP_FILTER_ENUM filter)
+{
+ int err;
+ u8 data = 0;
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+
+ if (filter == driver_dev->filter_num ||
+ filter == BMP_UNDEFINED_FILTER) {
+ pr_info("bmp280 filter no change or invalid:%d,(old %d)\n",
+ filter, driver_dev->filter_num);
+ return 0;
+ }
+
+ err = bmp280_baro_i2c_read_block(client, BMP280_CONFIG_REG_FILTER__REG,
+ &data, 1);
+ data = BMP_SET_BITSLICE(data, BMP280_CONFIG_REG_FILTER, filter);
+ err += bmp280_baro_i2c_write_block(client,
+ BMP280_CONFIG_REG_FILTER__REG,
+ &data, 1);
+
+ if (err < 0)
+ pr_err("bmp280 set filter failed, err = %d\n", err);
+ else
+ driver_dev->filter_num = filter;
+
+ return err;
+}
+
+static int bmp280_baro_set_oversampling_p
+ (struct i2c_client *client,
+ enum BMP_OVERSAMPLING_ENUM oversampling_p)
+{
+ int err;
+ u8 data = 0;
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+
+ if (oversampling_p == driver_dev->ovsampling_p ||
+ oversampling_p == BMP_UNDEFINED_OVERSAMPLING) {
+ pr_info("bmp280 ovsampling_p no change or invlid:%d,(old:%d)\n",
+ oversampling_p, driver_dev->ovsampling_p);
+ return 0;
+ }
+
+ err = bmp280_baro_i2c_read_block(client, BMP280_CTRLMEAS_REG_OSRSP__REG,
+ &data, 1);
+ data = BMP_SET_BITSLICE(data, BMP280_CTRLMEAS_REG_OSRSP,
+ oversampling_p);
+ err += bmp280_baro_i2c_write_block(client,
+ BMP280_CTRLMEAS_REG_OSRSP__REG,
+ &data, 1);
+
+ if (err < 0)
+ pr_err("bmp280 set oversampling_p failed, err = %d\n", err);
+ else
+ driver_dev->ovsampling_p = oversampling_p;
+
+ return err;
+}
+
+static int bmp280_baro_set_oversampling_t
+ (struct i2c_client *client,
+ enum BMP_OVERSAMPLING_ENUM oversampling_t)
+{
+ int err;
+ u8 data = 0;
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+
+ if (oversampling_t == driver_dev->ovsampling_t ||
+ oversampling_t == BMP_UNDEFINED_OVERSAMPLING) {
+ pr_info("bmp280 ovsampling_t no change or invlid:%d,(old:%d)\n",
+ oversampling_t, driver_dev->ovsampling_t);
+ return 0;
+ }
+
+ err = bmp280_baro_i2c_read_block(client, BMP280_CTRLMEAS_REG_OSRST__REG,
+ &data, 1);
+ data = BMP_SET_BITSLICE(data, BMP280_CTRLMEAS_REG_OSRST,
+ oversampling_t);
+ err += bmp280_baro_i2c_write_block(client,
+ BMP280_CTRLMEAS_REG_OSRST__REG,
+ &data, 1);
+
+ if (err < 0)
+ pr_err("bmp280 set oversampling_t failed, err = %d\n", err);
+ else
+ driver_dev->ovsampling_t = oversampling_t;
+
+ return err;
+}
+
+static int bmp280_baro_init_device(struct i2c_client *client)
+{
+ int err = 0;
+
+ err = bmp280_get_calibration_data(client);
+ if (err < 0) {
+ pr_err("get calibration data failed, err = %d\n", err);
+ return err;
+ }
+
+ err = bmp280_baro_set_powermode(client, BMP280_SLEEP_MODE);
+ if (err < 0) {
+ pr_err("bmp280 set power mode failed, err = %d\n", err);
+ return err;
+ }
+
+ err = bmp280_baro_set_filter(client, BMP_FILTER_8);
+ if (err < 0) {
+ pr_err("bmp280 set hw filter failed, err = %d\n", err);
+ return err;
+ }
+
+ err = bmp280_baro_set_oversampling_p(client, BMP_OVERSAMPLING_8X);
+ if (err < 0) {
+ pr_err("bmp280 set oversampling_p failed, err = %d\n", err);
+ return err;
+ }
+
+ err = bmp280_baro_set_oversampling_t(client, BMP_OVERSAMPLING_1X);
+ if (err < 0) {
+ pr_err("bmp280 set oversampling_t failed, err = %d\n", err);
+ return err;
+ }
+
+ pr_info("bmp280 baro init device success.\n");
+
+ return err;
+}
+
+static int bmp_read_raw_temperature(struct i2c_client *client,
+ s32 *temperature)
+{
+ struct bmp280_baro_device *obj;
+ s32 err = 0;
+ unsigned char a_data_u8r[3] = {0};
+
+ if (client == NULL) {
+ err = -EINVAL;
+ return err;
+ }
+
+ obj = i2c_get_clientdata(client);
+
+ err = bmp280_baro_i2c_read_block(client, BMP280_TEMPERATURE_MSB_REG,
+ a_data_u8r, 3);
+ if (err < 0) {
+ pr_err("bmp280 read raw temperature failed, err = %d\n", err);
+ return err;
+ }
+ *temperature = (BMP280_S32_t)((((BMP280_U32_t)(a_data_u8r[0]))
+ << SHIFT_LEFT_12_POSITION) |
+ (((BMP280_U32_t)(a_data_u8r[1]))
+ << SHIFT_LEFT_4_POSITION) |
+ ((BMP280_U32_t)a_data_u8r[2] >>
+ SHIFT_RIGHT_4_POSITION));
+
+ obj->last_temp_measurement = jiffies;
+
+ return err;
+}
+
+static int bmp_read_raw_pressure(struct i2c_client *client, s32 *pressure)
+{
+ struct bmp280_baro_device *priv;
+ s32 err = 0;
+ unsigned char a_data_u8r[3] = {0};
+
+ if (client == NULL) {
+ err = -EINVAL;
+ return err;
+ }
+
+ priv = i2c_get_clientdata(client);
+
+ err = bmp280_baro_i2c_read_block(client, BMP280_PRESSURE_MSB_REG,
+ a_data_u8r, 3);
+ if (err < 0) {
+ pr_err("bmp280 read raw pressure failed, err = %d\n", err);
+ return err;
+ }
+ *pressure = (BMP280_S32_t)((((BMP280_U32_t)(a_data_u8r[0]))
+ << SHIFT_LEFT_12_POSITION) |
+ (((BMP280_U32_t)(a_data_u8r[1]))
+ << SHIFT_LEFT_4_POSITION) |
+ ((BMP280_U32_t)a_data_u8r[2] >>
+ SHIFT_RIGHT_4_POSITION));
+
+#ifdef CONFIG_BMP_LOWPASS
+ /*
+ *Example: firlen = 16, filter buffer = [0] ... [15],
+ *when 17th data come, replace [0] with this new data.
+ *Then, average this filter buffer and report average value to upper
+ *layer.
+ */
+ if (atomic_read(&priv->filter)) {
+ if (atomic_read(&priv->fir_en) &&
+ !atomic_read(&priv->suspend)) {
+ int idx, firlen = atomic_read(&priv->firlen);
+
+ if (priv->fir.num < firlen) {
+ priv->fir.raw[priv->fir.num][BMP_PRESSURE] =
+ *pressure;
+ priv->fir.sum[BMP_PRESSURE] += *pressure;
+ if (atomic_read(&priv->trace) &
+ BAR_TRC_FILTER) {
+ pr_debug("add [%2d] [%5d] => [%5d]\n",
+ priv->fir.num,
+ priv->fir.raw[priv->fir.num]
+ [BMP_PRESSURE],
+ priv->fir.sum[BMP_PRESSURE]);
+ }
+ priv->fir.num++;
+ priv->fir.idx++;
+ } else {
+ idx = priv->fir.idx % firlen;
+ priv->fir.sum[BMP_PRESSURE] -=
+ priv->fir.raw[idx][BMP_PRESSURE];
+ priv->fir.raw[idx][BMP_PRESSURE] = *pressure;
+ priv->fir.sum[BMP_PRESSURE] += *pressure;
+ priv->fir.idx++;
+ *pressure =
+ priv->fir.sum[BMP_PRESSURE] / firlen;
+ if (atomic_read(&priv->trace) &
+ BAR_TRC_FILTER) {
+ pr_debug("add[%2d][%5d]=>[%5d]:[%5d]\n",
+ idx,
+ priv->fir
+ .raw[idx][BMP_PRESSURE],
+ priv->fir.sum[BMP_PRESSURE],
+ *pressure);
+ }
+ }
+ }
+ }
+#endif
+
+ return err;
+}
+
+/*
+ *get compensated temperature
+ *unit:10 degrees centigrade
+ */
+static int bmp_get_temperature(struct i2c_client *client, s32 *t_buf)
+{
+ struct bmp280_baro_device *driver_dev;
+ int status;
+ s32 utemp = 0; /* uncompensated temperature */
+ s32 temperature = 0;
+ BMP280_S32_t v_x1_u32r = 0;
+ BMP280_S32_t v_x2_u32r = 0;
+
+ if (client == NULL || t_buf == NULL) {
+ pr_err("bmp280 get temperature fail, invalid parameter\n");
+ return -1;
+ }
+
+ driver_dev = i2c_get_clientdata(client);
+
+ status = bmp_read_raw_temperature(client, &utemp);
+ if (status != 0) {
+ pr_info("bmp280 read raw temperature fail,status:%d\n", status);
+ return status;
+ }
+ v_x1_u32r = ((((utemp >> 3) -
+ ((BMP280_S32_t)driver_dev->bmp280_cali.dig_T1 << 1))) *
+ ((BMP280_S32_t)driver_dev->bmp280_cali.dig_T2)) >> 11;
+ v_x2_u32r = (((((utemp >> 4) -
+ ((BMP280_S32_t)driver_dev->bmp280_cali.dig_T1)) *
+ ((utemp >> 4) -
+ ((BMP280_S32_t)driver_dev->bmp280_cali.dig_T1))) >> 12) *
+ ((BMP280_S32_t)driver_dev->bmp280_cali.dig_T3)) >> 14;
+
+ driver_dev->t_fine = v_x1_u32r + v_x2_u32r;
+
+ temperature = (driver_dev->t_fine * 5 + 128) >> 8;
+
+ *t_buf = temperature;
+
+ BMP_DATA_LOG("bmp280 temperature: %d\n", temperature);
+ BMP_DATA_LOG("bmp280 temperature/100: %d\n", temperature / 100);
+
+ return status;
+}
+
+/*
+ *get compensated pressure
+ *unit: hectopascal(hPa)
+ */
+ #define BMP_BUFSIZE 128
+static int bmp_get_pressure(struct i2c_client *client, s32 *p_buf)
+{
+ struct bmp280_baro_device *driver_dev;
+ int status;
+ s32 temperature = 0, upressure = 0, pressure = 0;
+ BMP280_S64_t v_x1_u32r = 0;
+ BMP280_S64_t v_x2_u32r = 0;
+ BMP280_S64_t p = 0;
+
+ BMP_DATA_LOG("bmp280 get pressure\n");
+
+ if (client == NULL || p_buf == NULL) {
+ pr_err("bmp280 get pressure fail, invalid parameter\n");
+ return -1;
+ }
+
+ driver_dev = i2c_get_clientdata(client);
+ /* update the ambient temperature according to the given meas. period */
+ /* below method will have false problem when jiffies wrap around.
+ *so replace.
+ */
+
+ if (time_before_eq((unsigned long)(driver_dev->last_temp_measurement +
+ driver_dev->temp_measurement_period), jiffies)) {
+
+ status = bmp_get_temperature(client, &temperature);
+ if (status != 0) {
+ pr_info("bmp280 get temperature:%d, exit get prssure\n",
+ status);
+ goto exit;
+ }
+ }
+
+ status = bmp_read_raw_pressure(client, &upressure);
+ if (status != 0) {
+ pr_info("bmp280 get raw pressure status:%d, exit get prssure\n",
+ status);
+ goto exit;
+ }
+
+ v_x1_u32r = ((BMP280_S64_t)driver_dev->t_fine) - 128000;
+ v_x2_u32r = v_x1_u32r * v_x1_u32r *
+ (BMP280_S64_t)driver_dev->bmp280_cali.dig_P6;
+ v_x2_u32r = v_x2_u32r +
+ ((v_x1_u32r * (BMP280_S64_t)driver_dev->bmp280_cali.dig_P5)
+ << 17);
+ v_x2_u32r = v_x2_u32r +
+ (((BMP280_S64_t)driver_dev->bmp280_cali.dig_P4) << 35);
+ v_x1_u32r = ((v_x1_u32r * v_x1_u32r *
+ (BMP280_S64_t)driver_dev->bmp280_cali.dig_P3) >> 8) +
+ ((v_x1_u32r * (BMP280_S64_t)driver_dev->bmp280_cali.dig_P2)
+ << 12);
+ v_x1_u32r = (((((BMP280_S64_t)1) << 47) + v_x1_u32r)) *
+ ((BMP280_S64_t)driver_dev->bmp280_cali.dig_P1) >> 33;
+ if (v_x1_u32r == 0) {
+ /* Avoid exception caused by division by zero */
+ pr_info("bmp280 get_pressure v_x1_32=0, exit get pressure\n");
+ return -1;
+ }
+
+ p = 1048576 - upressure;
+ p = div64_s64(((p << 31) - v_x2_u32r) * 3125, v_x1_u32r);
+ v_x1_u32r = (((BMP280_S64_t)driver_dev->bmp280_cali.dig_P9) *
+ (p >> 13) * (p >> 13)) >> 25;
+ v_x2_u32r = (((BMP280_S64_t)driver_dev->bmp280_cali.dig_P8) * p) >> 19;
+ p = ((p + v_x1_u32r + v_x2_u32r) >> 8) +
+ (((BMP280_S64_t)driver_dev->bmp280_cali.dig_P7) << 4);
+ pressure = (BMP280_U32_t)p / 256;
+
+ *p_buf = pressure;
+ BMP_DATA_LOG("bmp280 pressure: %d\n", pressure);
+ BMP_DATA_LOG("bmp280 pressure/100: %d\n", pressure / 100);
+
+exit:
+ return status;
+}
+
+int bmp280_baro_sample(struct hf_device *hfdev)
+{
+ struct i2c_client *client;
+ struct bmp280_baro_device *driver_dev;
+ struct hf_manager *manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+ int value = 0, temperature = 0;
+ int err = 0;
+
+ BMP_DATA_LOG("Sample bmp280\n");
+
+ if (!hfdev) {
+ pr_err("bmp280 sample failed:invalid hfdev\n");
+ return -1;
+ }
+ client = hf_device_get_private_data(hfdev);
+ driver_dev = i2c_get_clientdata(client);
+ manager = driver_dev->hf_dev.manager;
+
+ err = bmp_get_pressure(client, &value);
+ if (err) {
+ pr_err("bmp280 get compensated pressure failed, err = %d\n",
+ err);
+ return -1;
+ }
+
+ err = bmp_get_temperature(client, &temperature);
+ if (err != 0) {
+ pr_err("bmp280 get temperature:%d, exit get prssure\n",
+ err);
+ return -1;
+ }
+
+ current_time = ktime_get_boot_ns();
+ if (atomic_read(&driver_dev->raw_enable)) {
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_PRESSURE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = RAW_ACTION;
+ event.word[0] = value;
+ event.word[1] = temperature;
+ manager->report(manager, &event);
+ }
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_PRESSURE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = DATA_ACTION;
+ event.word[0] = value;
+ event.word[1] = temperature;
+
+ manager->report(manager, &event);
+ manager->complete(manager);
+
+ return 0;
+}
+
+int bmp280_baro_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ struct i2c_client *client;
+ struct bmp280_baro_device *driver_dev;
+ int ret = 0;
+ int retry = 0;
+ enum BMP_POWERMODE_ENUM power;
+
+ pr_info("%s bmp280\n", en?"En":"Dis");
+ if (!hfdev) {
+ pr_err("bmp280 enable failed:invalid hfdev\n");
+ return -1;
+ }
+ client = hf_device_get_private_data(hfdev);
+ driver_dev = i2c_get_clientdata(client);
+
+ if (en)
+ power = BMP280_NORMAL_MODE;
+ else if (!en)
+ power = BMP280_SLEEP_MODE;
+
+ for (retry = 0; retry < 3; retry++) {
+ ret = bmp280_baro_set_powermode(client, power);
+ if (ret >= 0) {
+ pr_info("bmp280 set pwer mode(%d) done(retry:%d)\n",
+ power, retry);
+ break;
+ }
+ }
+ driver_dev->last_temp_measurement =
+ jiffies - driver_dev->temp_measurement_period;
+
+ if (ret < 0) {
+ pr_err("bmp280 set power mode(%d) fail!\n", power);
+ return -1;
+ }
+
+ pr_info("bmp280 (%s) success\n", power?"enable":"disable");
+ return 0;
+}
+
+static int bmp_set_delay(u64 ns)
+{
+ return 0;
+}
+
+int bmp280_baro_batch(struct hf_device *hfdev,
+ int sensor_type, int64_t delay, int64_t latency)
+{
+ struct i2c_client *client = hf_device_get_private_data(hfdev);
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ADDITIONAL_INFO;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.reserved = SENSOR_TYPE_PRESSURE;
+ event.action = DATA_ACTION;
+ event.byte[12] = 1;
+ event.byte[3] = driver_dev->placement[0];
+ event.byte[7] = driver_dev->placement[1];
+ event.byte[11] = driver_dev->placement[2];
+
+ manager->report(manager, &event);
+ manager->complete(manager);
+
+ return bmp_set_delay(delay);
+}
+
+static int bmp280_baro_flush(struct hf_device *hfdev, int sensor_type)
+{
+ struct i2c_client *client = hf_device_get_private_data(hfdev);
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.sensor_type = sensor_type;
+ event.timestamp = current_time;
+ event.action = FLUSH_ACTION;
+
+ manager->report(manager, &event);
+ manager->complete(manager);
+
+ return 0;
+}
+
+int bmp280_baro_raw_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ struct i2c_client *client = hf_device_get_private_data(hfdev);
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+
+ atomic_set(&driver_dev->raw_enable, en);
+ return 0;
+}
+
+static int bmp280_baro_remove(struct i2c_client *client)
+{
+ struct bmp280_baro_device *driver_dev = i2c_get_clientdata(client);
+
+ hf_manager_destroy(driver_dev->hf_dev.manager);
+ kfree(driver_dev);
+ return 0;
+}
+
+static int bmp280_baro_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+ int err = 0;
+ struct bmp280_baro_device *driver_dev;
+ struct sensorlist_info_t listinfo;
+
+ pr_info("%s enter Slaver 0x76.\n", __func__);
+
+ err = bmp280_baro_soft_reset(client);
+ if (err < 0) {
+ pr_err("bmp280 soft reset fail,exit probe!\n");
+ err = -EINVAL;
+ goto findHW_fail;
+ }
+
+ /* check chip id */
+ err = bmp280_baro_check_chip_id(client);
+ if (err < 0) {
+ pr_err("bmp280 chip id mismatch,exit probe!\n");
+ err = -EINVAL;
+ goto findHW_fail;
+ }
+ mdelay(1);
+
+ driver_dev = kzalloc(sizeof(*driver_dev), GFP_KERNEL);
+ if (!driver_dev) {
+ err = -ENOMEM;
+ goto malloc_fail;
+ }
+
+ if (of_property_read_u8_array(client->dev.of_node, "placement",
+ driver_dev->placement, ARRAY_SIZE(driver_dev->placement))) {
+ pr_err("%s get placement dts fail\n", __func__);
+ err = -EFAULT;
+ goto init_fail;
+ }
+
+ atomic_set(&driver_dev->raw_enable, 0);
+
+ driver_dev->hf_dev.dev_name = BMP280_BARO_NAME;
+ driver_dev->hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ driver_dev->hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ driver_dev->hf_dev.support_list = support_sensors;
+ driver_dev->hf_dev.support_size = ARRAY_SIZE(support_sensors);
+ driver_dev->hf_dev.sample = bmp280_baro_sample;
+ driver_dev->hf_dev.enable = bmp280_baro_enable;
+ driver_dev->hf_dev.batch = bmp280_baro_batch;
+ driver_dev->hf_dev.flush = bmp280_baro_flush;
+ driver_dev->hf_dev.rawdata = bmp280_baro_raw_enable;
+
+#ifdef CONFIG_BMP_LOWPASS
+ if (driver_dev->hw.firlen > C_MAX_FIR_LENGTH)
+ atomic_set(&driver_dev->firlen, C_MAX_FIR_LENGTH);
+ else
+ atomic_set(&driver_dev->firlen, driver_dev->hw.firlen);
+
+ if (atomic_read(&driver_dev->firlen) > 0)
+ atomic_set(&driver_dev->fir_en, 1);
+#endif
+
+ err = hf_manager_create(&driver_dev->hf_dev);
+ if (err < 0) {
+ pr_err("bmp280 hf_manager_create fail,exit probe!\n");
+ err = -1;
+ goto create_manager_fail;
+ }
+
+ i2c_set_clientdata(client, driver_dev);
+ hf_device_set_private_data(&driver_dev->hf_dev, client);
+
+ driver_dev->power_mode = BMP280_UNDEFINED_POWERMODE;
+ driver_dev->filter_num = BMP_UNDEFINED_FILTER;
+ driver_dev->ovsampling_p = BMP_UNDEFINED_OVERSAMPLING;
+ driver_dev->ovsampling_t = BMP_UNDEFINED_OVERSAMPLING;
+ driver_dev->last_temp_measurement = 0;
+ driver_dev->temp_measurement_period = 1 * HZ;
+
+ err = bmp280_baro_init_device(client);
+ if (err < 0) {
+ pr_err("bmp280 init device fail,exit probe!\n");
+ goto init_fail;
+ }
+
+ memset(&listinfo, 0, sizeof(struct sensorlist_info_t));
+ strlcpy(listinfo.name, BMP280_BARO_NAME, sizeof(listinfo.name));
+ err = sensorlist_register_devinfo(SENSOR_TYPE_PRESSURE, &listinfo);
+ if (err < 0) {
+ pr_err("bmp280 register sensorlist fail,exit probe!\n");
+ goto init_fail;
+ }
+
+ pr_info("%s success!\n", __func__);
+ return 0;
+
+init_fail:
+ hf_manager_destroy(driver_dev->hf_dev.manager);
+create_manager_fail:
+ kfree(driver_dev);
+malloc_fail:
+findHW_fail:
+ return err;
+}
+
+static const struct of_device_id bmp280_baro_of_match[] = {
+ {.compatible = "mediatek,bmp280_baro"},
+ {},
+};
+
+static const struct i2c_device_id bmp280_baro_id[] = {
+ {BMP280_BARO_NAME, 0}, {} };
+
+static struct i2c_driver bmp280_baro_driver = {
+ .driver = {
+ .name = BMP280_BARO_NAME,
+ .bus = &i2c_bus_type,
+ .owner = THIS_MODULE,
+ .of_match_table = bmp280_baro_of_match,
+ },
+ .probe = bmp280_baro_probe,
+ .remove = bmp280_baro_remove,
+ .id_table = bmp280_baro_id,
+};
+
+module_i2c_driver(bmp280_baro_driver);
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("bmp280 baro i2c driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/bmp280_baro.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/bmp280_baro.h
new file mode 100644
index 0000000..b704280
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp280_baro/bmp280_baro.h
@@ -0,0 +1,86 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef __BMP280_BARO_H__
+#define __BMP280_BARO_H__
+
+/* common definition */
+#define BMP_GET_BITSLICE(regvar, bitname)\
+ ((regvar & bitname##__MSK) >> bitname##__POS)
+
+#define BMP_SET_BITSLICE(regvar, bitname, val)\
+ ((regvar & ~bitname##__MSK) | ((val<<bitname##__POS)&bitname##__MSK))
+
+
+/*Define of registers*/
+/* Chip ID Register */
+#define BMP280_BARO_CHIP_ID_ADDR 0xD0
+
+#define BMP280_BARO_CHIP_ID_VALUE1 (0x56)
+#define BMP280_BARO_CHIP_ID_VALUE2 (0x57)
+#define BMP280_BARO_CHIP_ID_VALUE3 (0x58)
+
+/* soft reset */
+#define BMP280_BARO_RESET_ADDR 0xE0
+#define BMP280_BARO_SOFT_RESET_VALUE 0xB6
+
+/* Calibration data */
+#define BMP280_CALIBRATION_DATA_START 0x88
+#define BMP280_CALIBRATION_DATA_LENGTH 24
+
+#define SHIFT_RIGHT_4_POSITION 4
+#define SHIFT_LEFT_2_POSITION 2
+#define SHIFT_LEFT_4_POSITION 4
+#define SHIFT_LEFT_5_POSITION 5
+#define SHIFT_LEFT_8_POSITION 8
+#define SHIFT_LEFT_12_POSITION 12
+#define SHIFT_LEFT_16_POSITION 16
+
+
+/* Ctrl Measure Register */
+#define BMP280_CTRLMEAS_REG 0xF4
+
+#define BMP280_CTRLMEAS_REG_MODE__POS 0
+#define BMP280_CTRLMEAS_REG_MODE__MSK 0x03
+#define BMP280_CTRLMEAS_REG_MODE__LEN 2
+#define BMP280_CTRLMEAS_REG_MODE__REG BMP280_CTRLMEAS_REG
+
+#define BMP280_CTRLMEAS_REG_OSRSP__POS 2
+#define BMP280_CTRLMEAS_REG_OSRSP__MSK 0x1C
+#define BMP280_CTRLMEAS_REG_OSRSP__LEN 3
+#define BMP280_CTRLMEAS_REG_OSRSP__REG BMP280_CTRLMEAS_REG
+
+#define BMP280_CTRLMEAS_REG_OSRST__POS 5
+#define BMP280_CTRLMEAS_REG_OSRST__MSK 0xE0
+#define BMP280_CTRLMEAS_REG_OSRST__LEN 3
+#define BMP280_CTRLMEAS_REG_OSRST__REG BMP280_CTRLMEAS_REG
+
+//#define BMP280_OVERSAMPLING_SKIPPED 0x00
+//#define BMP280_OVERSAMPLING_1X 0x01
+//#define BMP280_OVERSAMPLING_2X 0x02
+//#define BMP280_OVERSAMPLING_4X 0x03
+//#define BMP280_OVERSAMPLING_8X 0x04
+//#define BMP280_OVERSAMPLING_16X 0x05
+
+/* config */
+#define BMP280_CONFIG_REG 0xF5
+
+#define BMP280_CONFIG_REG_FILTER__POS 2
+#define BMP280_CONFIG_REG_FILTER__MSK 0x1C
+#define BMP280_CONFIG_REG_FILTER__LEN 3
+#define BMP280_CONFIG_REG_FILTER__REG BMP280_CONFIG_REG
+
+#define BMP280_FILTERCOEFF_OFF 0x00
+#define BMP280_FILTERCOEFF_2 0x01
+#define BMP280_FILTERCOEFF_4 0x02
+#define BMP280_FILTERCOEFF_8 0x03
+#define BMP280_FILTERCOEFF_16 0x04
+
+/* data */
+#define BMP280_PRESSURE_MSB_REG 0xF7 /* Pressure MSB Reg */
+#define BMP280_TEMPERATURE_MSB_REG 0xFA /* Temperature MSB Reg */
+
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/Kconfig b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/Kconfig
new file mode 100644
index 0000000..43b2afc
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/Kconfig
@@ -0,0 +1,9 @@
+config MTK_BMP380_SUPPORT
+ bool " MTK_BMP380_SUPPORT for MediaTek package"
+ default n
+ help
+ Enable MTK BMP380 support.
+ BMP380 is a low-power and low-noise
+ barometric pressure sensor.
+ If in doubt, say N here.
+
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/Makefile
new file mode 100644
index 0000000..46dd553
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/Makefile
@@ -0,0 +1,3 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+
+obj-y += bmp380.o
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/bmp380.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/bmp380.c
new file mode 100644
index 0000000..740b5c1
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/bmp380/bmp380.c
@@ -0,0 +1,505 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+
+#include "hf_manager.h"
+
+#define CHECK_CHIP_ID_TIME_MAX 0x05
+#define C_I2C_FIFO_SIZE 0x08
+#define PRESSURE_SENSOR_NAME "bmp380"
+#define BOSCH_BMP380_ID 0x50
+
+#define BOSCH_BMP380_REG_RESET 0x7e
+#define BOSCH_BMP380_REG_DIG_T1 0x31
+#define BOSCH_BMP380_REG_ID 0x00
+#define BOSCH_BMP380_REG_CTRL_ODR 0x1d //Control the Output Data Rate
+#define BOSCH_BMP380_REG_CTRL_OSR 0x1c //Control the OverSampling
+#define BOSCH_BMP380_REG_CTRL_PWR 0x1b
+#define BOSCH_BMP380_REG_CONFIG 0x1f //iir filter coefficents
+#define BOSCH_BMP380_REG_PRESS_LSB 0x04
+#define BOSCH_BMP380_REG_FIFO_WTM_1 0x16
+#define BOSCH_BMP380_REG_FIFO_WTM_0 0x15
+#define BOSCH_BMP380_REG_FIFO_CONFIG_1 0x17
+#define BOSCH_BMP380_REG_FIFO_CONFIG_2 0x18
+
+struct BMP380CompParams {
+ uint16_t par_t1;
+ uint16_t par_t2;
+ int8_t par_t3;
+
+ int16_t par_p1;
+ int16_t par_p2;
+ int8_t par_p3;
+ int8_t par_p4;
+ u16 par_p5;
+ u16 par_p6;
+ int8_t par_p7;
+ int8_t par_p8;
+ int16_t par_p9;
+ int8_t par_p10;
+ int8_t par_p11;
+ s64 t_lin;
+} __attribute__((packed));
+
+static struct sensor_info bmp380_sensor_info[] = {
+ {
+ .sensor_type = SENSOR_TYPE_PRESSURE,
+ .gain = 100,
+ },
+};
+
+struct bmp380_device {
+ struct hf_device hf_dev;
+ uint32_t i2c_num;
+ uint32_t i2c_addr;
+ struct i2c_client *client;
+ struct BMP380CompParams comp;
+ atomic_t raw_enable;
+};
+
+/* I2C operation functions */
+static int bmp_i2c_read_block(struct i2c_client *client,
+ uint8_t addr, uint8_t *data, uint8_t len)
+{
+ int err = 0;
+ uint8_t beg = addr;
+ struct i2c_msg msgs[2] = {
+ {/*.addr = client->addr,*/
+ .flags = 0,
+ .len = 1,
+ .buf = &beg},
+ {
+ /*.addr = client->addr*/
+ .flags = I2C_M_RD,
+ .len = len,
+ .buf = data,
+ } };
+ if (!client)
+ return -EINVAL;
+ msgs[0].addr = client->addr;
+ msgs[1].addr = client->addr;
+
+ err = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+ if (err != 2) {
+ pr_err_ratelimited("bmp380 i2c_trans err:%x %x (%d %p %d) %d\n",
+ msgs[0].addr, client->addr, addr, data, len,
+ err);
+ err = -EIO;
+ } else {
+ err = 0; /*no error*/
+ }
+ return err;
+}
+
+static int bmp_i2c_write_block(struct i2c_client *client,
+ uint8_t addr, uint8_t *data, uint8_t len)
+{
+ /* because address also occupies one byte,
+ * the maximum length for write is 7 bytes
+ */
+ int err = 0, idx = 0, num = 0;
+ char buf[32];
+
+ if (!client)
+ return -EINVAL;
+ else if (len > C_I2C_FIFO_SIZE) {
+ pr_err_ratelimited("bmp380 len %d fi %d\n", len,
+ C_I2C_FIFO_SIZE);
+ return -EINVAL;
+ }
+ buf[num++] = addr;
+ for (idx = 0; idx < len; idx++)
+ buf[num++] = data[idx];
+
+ err = i2c_master_send(client, buf, num);
+ if (err < 0) {
+ pr_err_ratelimited("bmp380 send command error!!\n");
+ return -EFAULT;
+ }
+
+ return err;
+}
+
+static int bmp380_soft_reset(struct i2c_client *client)
+{
+ uint8_t data = 0xb6;
+ return bmp_i2c_write_block(client, BOSCH_BMP380_REG_RESET, &data, 1);
+}
+
+static int bmp380_check_chip_id(struct i2c_client *client)
+{
+ int err = -1;
+ uint8_t chip_id = 0;
+ uint8_t read_count = 0;
+
+ while (read_count++ < CHECK_CHIP_ID_TIME_MAX) {
+ bmp_i2c_read_block(client, BOSCH_BMP380_REG_ID,
+ &chip_id, 1);
+
+ if (chip_id != BOSCH_BMP380_ID) {
+ mdelay(1);
+ pr_err("%s fail(0x%2x).\n",
+ __func__, chip_id);
+ } else {
+ err = 0;
+ pr_info("%s success(0x%2x).\n",
+ __func__, chip_id);
+ break;
+ }
+ }
+ return err;
+}
+
+static int bmp380_init_device(struct i2c_client *client)
+{
+ int err = -1;
+ uint8_t tx_buf[2] = {0};
+ uint8_t rx_buf[8] = {0};
+ struct bmp380_device *driver_dev = i2c_get_clientdata(client);
+
+ err = bmp380_soft_reset(client);
+ if (err < 0) {
+ pr_err("bmp380 soft reset fail,exit probe!\n");
+ goto i2c_fail;
+ }
+
+ mdelay(2);
+
+ tx_buf[0] = BOSCH_BMP380_REG_DIG_T1;
+ err = bmp_i2c_read_block(client, tx_buf[0], rx_buf, 0x08);
+ if (err < 0)
+ goto i2c_fail;
+ memcpy((uint8_t*)&driver_dev->comp, rx_buf, 8);
+
+ tx_buf[0] = BOSCH_BMP380_REG_DIG_T1 + 8;
+ err = bmp_i2c_read_block(client, tx_buf[0], rx_buf, 0x08);
+ if (err < 0)
+ goto i2c_fail;
+ memcpy((uint8_t*)((uint8_t*)&driver_dev->comp + 8), rx_buf, 8);
+
+ tx_buf[0] = BOSCH_BMP380_REG_DIG_T1 + 16;
+ err = bmp_i2c_read_block(client, tx_buf[0], rx_buf, 0x05);
+ if (err < 0)
+ goto i2c_fail;
+ memcpy((uint8_t*)((uint8_t*)&driver_dev->comp + 16), rx_buf, 5);
+
+ tx_buf[0] = ((4 << 3) | (1 << 0));//config oversampling: baro:16x, temp:2x
+ err = bmp_i2c_write_block(client, BOSCH_BMP380_REG_CTRL_OSR,
+ tx_buf, 0x01);
+
+ tx_buf[0] = 4;//config standy time: 62.5ms
+ err = bmp_i2c_write_block(client, BOSCH_BMP380_REG_CTRL_ODR,
+ tx_buf, 0x01);
+ if (err < 0)
+ goto i2c_fail;
+
+i2c_fail:
+ pr_err("%s fail\n", __func__);
+ return err;
+}
+
+static int64_t compensate_temp(struct i2c_client *client,
+ uint32_t uncomp_temp)
+{
+
+ uint64_t partial_data1;
+ uint64_t partial_data2;
+ uint64_t partial_data3;
+ int64_t partial_data4;
+ int64_t partial_data5;
+ int64_t partial_data6;
+ int64_t comp_temp;
+ struct bmp380_device *driver_dev;
+
+ driver_dev = i2c_get_clientdata(client);
+ partial_data1 = uncomp_temp - (256 * driver_dev->comp.par_t1);
+ partial_data2 = driver_dev->comp.par_t2 * partial_data1;
+ partial_data3 = partial_data1 * partial_data1;
+ partial_data4 = (int64_t)partial_data3 * driver_dev->comp.par_t3;
+ partial_data5 = ((int64_t)(partial_data2 * 262144) + partial_data4);
+ partial_data6 = partial_data5 / 4294967296;
+ driver_dev->comp.t_lin = partial_data6;
+ comp_temp = (int64_t)((partial_data6 * 25) / 16384);
+
+ //return the tempeature in the unit of 0.01 centigrade.
+ return comp_temp;
+}
+
+static int64_t compensate_baro(struct i2c_client *client,
+ uint32_t uncomp_press)
+{
+ int64_t partial_data1;
+ int64_t partial_data2;
+ int64_t partial_data3;
+ int64_t partial_data4;
+ int64_t partial_data5;
+ int64_t partial_data6;
+ int64_t offset;
+ int64_t sensitivity;
+ uint64_t comp_press;
+ struct bmp380_device *driver_dev;
+
+ driver_dev = i2c_get_clientdata(client);
+ partial_data1 = driver_dev->comp.t_lin * driver_dev->comp.t_lin;
+ partial_data2 = partial_data1 / 64;
+ partial_data3 = (partial_data2 * driver_dev->comp.t_lin) / 256;
+ partial_data4 = (driver_dev->comp.par_p8 * partial_data3) / 32;
+ partial_data5 = (driver_dev->comp.par_p7 * partial_data1) * 16;
+ partial_data6 = (driver_dev->comp.par_p6 * driver_dev->comp.t_lin)
+ * 4194304;
+ offset = (driver_dev->comp.par_p5 * 140737488355328) + partial_data4
+ + partial_data5 + partial_data6;
+
+ partial_data2 = (driver_dev->comp.par_p4 * partial_data3) / 32;
+ partial_data4 = (driver_dev->comp.par_p3 * partial_data1) * 4;
+ partial_data5 = (driver_dev->comp.par_p2 - 16384)
+ * driver_dev->comp.t_lin * 2097152;
+ sensitivity = ((driver_dev->comp.par_p1 - 16384) * 70368744177664)
+ + partial_data2 + partial_data4 + partial_data5;
+
+ partial_data1 = (sensitivity / 16777216) * uncomp_press;
+ partial_data2 = driver_dev->comp.par_p10 * driver_dev->comp.t_lin;
+ partial_data3 = partial_data2 + (65536 * driver_dev->comp.par_p9);
+ partial_data4 = (partial_data3 * uncomp_press) / 8192;
+ partial_data5 = (partial_data4 * uncomp_press / 10) / 512 * 10;
+ partial_data6 = (int64_t)((uint64_t)uncomp_press
+ * (uint64_t)uncomp_press);
+ partial_data2 = (driver_dev->comp.par_p11 * partial_data6) / 65536;
+ partial_data3 = (partial_data2 * uncomp_press) / 128;
+ partial_data4 = (offset / 4) + partial_data1 + partial_data5
+ + partial_data3;
+ comp_press = (((uint64_t)partial_data4 * 25)
+ / (uint64_t)1099511627776);
+
+ //return the press in the unit of the 0.01 Pa.
+ return comp_press;
+}
+
+static int bmp_get_pressure(struct i2c_client *client, s32 *p_buf)
+{
+ uint32_t press_adc;
+ uint32_t temp_adc;
+ int ret = 0;
+ uint32_t data_xlsb;
+ uint32_t data_lsb;
+ uint32_t data_msb;
+ uint8_t tx_buf[1] = {0};
+ uint8_t rx_buf[6] = {0};
+ int64_t temp = 0, press = 0;
+
+ tx_buf[0] = BOSCH_BMP380_REG_PRESS_LSB;
+ ret = bmp_i2c_read_block(client, tx_buf[0], rx_buf, 6);
+ if (ret < 0) {
+ pr_err("%s failed\n", __func__);
+ return ret;
+ }
+
+ data_xlsb = (uint32_t)rx_buf[0];
+ data_lsb = (uint32_t)rx_buf[1] << 8;
+ data_msb = (uint32_t)rx_buf[2] << 16;
+ press_adc = data_msb | data_lsb | data_xlsb;
+
+ data_xlsb = (uint32_t)rx_buf[3];
+ data_lsb = (uint32_t)rx_buf[4] << 8;
+ data_msb = (uint32_t)rx_buf[5] << 16;
+ temp_adc = data_msb | data_lsb | data_xlsb;
+
+ temp = compensate_temp(client, temp_adc);
+ press = compensate_baro(client, press_adc);
+ *p_buf = (s32)press * bmp380_sensor_info[0].gain / 10000;
+ return 0;
+}
+
+static int bmp380_sample(struct hf_device *hfdev)
+{
+ struct i2c_client *client;
+ struct bmp380_device *driver_dev;
+ struct hf_manager *manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+ s32 value = 0;
+ int err = 0;
+
+ if (!hfdev) {
+ pr_err("bmp380 sample failed:invalid hfdev\n");
+ return -1;
+ }
+ client = hf_device_get_private_data(hfdev);
+ driver_dev = i2c_get_clientdata(client);
+ manager = driver_dev->hf_dev.manager;
+
+ err = bmp_get_pressure(client, &value);
+ if (err) {
+ pr_err("bmp_get_pressure failed\n");
+ return err;
+ }
+
+ current_time = ktime_get_boot_ns();
+ if (atomic_read(&driver_dev->raw_enable)) {
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_PRESSURE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = RAW_ACTION;
+ event.word[0] = value;
+ manager->report(manager, &event);
+ }
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_PRESSURE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = DATA_ACTION;
+ event.word[0] = value;
+
+ manager->report(manager, &event);
+ manager->complete(manager);
+
+ return 0;
+}
+
+static int bmp380_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ uint8_t ret = 0;
+ struct i2c_client *client;
+ struct bmp380_device *driver_dev;
+ uint8_t tx_buf[1] = {0};
+ int retry = 0;
+ client = hf_device_get_private_data(hfdev);
+ driver_dev = i2c_get_clientdata(client);
+
+ if (en)
+ tx_buf[0] = (3 << 4 | 1 << 1 | 1 << 0);
+ else
+ tx_buf[0] = (2 << 5 | 5 << 2);
+
+ for (retry = 0; retry < 3; retry++) {
+ ret = bmp_i2c_write_block(client,
+ BOSCH_BMP380_REG_CTRL_PWR,
+ tx_buf, 0x01);
+ if (ret >= 0) {
+ pr_debug("bmp380_enable (%d) done(retry:%d)\n",
+ en, retry);
+ break;
+ }
+ }
+
+ return ret;
+}
+
+static int bmp380_batch(struct hf_device *hfdev, int sensor_type,
+ int64_t delay, int64_t latency)
+{
+ pr_debug("%s id:%d delay:%lld latency:%lld\n", __func__, sensor_type,
+ delay, latency);
+ return 0;
+}
+
+int bmp380_raw_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ struct i2c_client *client = hf_device_get_private_data(hfdev);
+ struct bmp380_device *driver_dev = i2c_get_clientdata(client);
+
+ atomic_set(&driver_dev->raw_enable, en);
+ return 0;
+}
+
+static int bmp380_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+ int err = 0;
+ struct bmp380_device *driver_dev = NULL;
+
+ err = bmp380_check_chip_id(client);
+ if (err < 0) {
+ pr_err("bmp380 chip id mismatch,exit probe!\n");
+ goto findHW_fail;
+ }
+
+ driver_dev = kzalloc(sizeof(*driver_dev), GFP_KERNEL);
+ if (!driver_dev) {
+ err = -ENOMEM;
+ goto malloc_fail;
+ }
+
+ driver_dev->hf_dev.dev_name = PRESSURE_SENSOR_NAME;
+ driver_dev->hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ driver_dev->hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ driver_dev->hf_dev.support_list = bmp380_sensor_info;
+ driver_dev->hf_dev.support_size = ARRAY_SIZE(bmp380_sensor_info);
+ driver_dev->hf_dev.sample = bmp380_sample;
+ driver_dev->hf_dev.enable = bmp380_enable;
+ driver_dev->hf_dev.batch = bmp380_batch;
+ driver_dev->hf_dev.rawdata = bmp380_raw_enable;
+
+ err = hf_manager_create(&driver_dev->hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ err = -1;
+ goto create_manager_fail;
+ }
+
+ i2c_set_clientdata(client, driver_dev);
+ hf_device_set_private_data(&driver_dev->hf_dev, client);
+ err = bmp380_init_device(client);
+ if (err < 0) {
+ pr_err("%s fail\n", __func__);
+ goto init_fail;
+ }
+
+ pr_info("%s success!\n", __func__);
+ return 0;
+
+init_fail:
+create_manager_fail:
+ kfree(driver_dev);
+malloc_fail:
+findHW_fail:
+ pr_err("%s fail!\n", __func__);
+ return err;
+}
+
+static int bmp380_remove(struct i2c_client *client)
+{
+ struct bmp380_device *driver_dev = i2c_get_clientdata(client);
+
+ hf_manager_destroy(driver_dev->hf_dev.manager);
+ kfree(driver_dev);
+ return 0;
+}
+
+static const struct i2c_device_id bmp380_id[] = {
+ {PRESSURE_SENSOR_NAME, 0},
+ {},
+};
+
+static const struct of_device_id bmp380_of_match[] = {
+ {.compatible = "mediatek,barometer"},
+ {},
+};
+
+static struct i2c_driver bmp380_driver = {
+ .driver = {
+ .name = PRESSURE_SENSOR_NAME,
+ .bus = &i2c_bus_type,
+ .owner = THIS_MODULE,
+ .of_match_table = bmp380_of_match,
+ },
+ .probe = bmp380_probe,
+ .remove = bmp380_remove,
+ .id_table = bmp380_id
+};
+
+module_i2c_driver(bmp380_driver);
+
+MODULE_DESCRIPTION("bmp380 driver");
+MODULE_AUTHOR("Mediatek");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/Makefile
new file mode 100644
index 0000000..71d25c8
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/Makefile
@@ -0,0 +1,2 @@
+obj-y += hf_manager.o
+obj-y += sensor_list.o
\ No newline at end of file
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_manager.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_manager.c
new file mode 100644
index 0000000..9e4e870
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_manager.c
@@ -0,0 +1,1038 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "[hf_manager] " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/poll.h>
+#include <linux/bitmap.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <uapi/linux/sched/types.h>
+#include <linux/sched_clock.h>
+#include <linux/log2.h>
+
+#include "hf_manager.h"
+
+static DECLARE_BITMAP(sensor_list_bitmap, SENSOR_TYPE_SENSOR_MAX);
+static LIST_HEAD(hf_manager_list);
+static DEFINE_MUTEX(hf_manager_list_mtx);
+static LIST_HEAD(hf_client_list);
+static DEFINE_SPINLOCK(hf_client_list_lock);
+static struct sensor_state prev_request[SENSOR_TYPE_SENSOR_MAX];
+
+static struct task_struct *hf_manager_kthread_task;
+static struct kthread_worker hf_manager_kthread_worker;
+
+static int hf_manager_find_client(struct hf_manager_event *event);
+
+static struct coordinate coordinates[] = {
+ { { 1, 1, 1}, {0, 1, 2} },
+ { { -1, 1, 1}, {1, 0, 2} },
+ { { -1, -1, 1}, {0, 1, 2} },
+ { { 1, -1, 1}, {1, 0, 2} },
+
+ { { -1, 1, -1}, {0, 1, 2} },
+ { { 1, 1, -1}, {1, 0, 2} },
+ { { 1, -1, -1}, {0, 1, 2} },
+ { { -1, -1, -1}, {1, 0, 2} },
+};
+
+void coordinate_map(unsigned char direction, int32_t *data)
+{
+ int32_t temp[3] = {0};
+
+ if (direction >= ARRAY_SIZE(coordinates))
+ return;
+
+ temp[coordinates[direction].map[0]] =
+ coordinates[direction].sign[0] * data[0];
+ temp[coordinates[direction].map[1]] =
+ coordinates[direction].sign[1] * data[1];
+ temp[coordinates[direction].map[2]] =
+ coordinates[direction].sign[2] * data[2];
+
+ data[0] = temp[0];
+ data[1] = temp[1];
+ data[2] = temp[2];
+}
+
+void get_placement_info(unsigned char direction, int8_t *data)
+{
+ if (direction >= ARRAY_SIZE(coordinates))
+ return;
+
+ data[coordinates[direction].map[0] * 4 + 0] =
+ coordinates[direction].sign[0];
+ data[coordinates[direction].map[1] * 4 + 1] =
+ coordinates[direction].sign[1];
+ data[coordinates[direction].map[2] * 4 + 2] =
+ coordinates[direction].sign[2];
+}
+
+static bool filter_event_by_timestamp(struct hf_client_fifo *hf_fifo,
+ struct hf_manager_event *event)
+{
+ if (hf_fifo->last_time_stamp[event->sensor_type] ==
+ event->timestamp) {
+ return true;
+ }
+ hf_fifo->last_time_stamp[event->sensor_type] = event->timestamp;
+ return false;
+}
+
+static int hf_manager_report_event(struct hf_client *client,
+ struct hf_manager_event *event)
+{
+ unsigned long flags;
+ unsigned int next = 0;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+
+ spin_lock_irqsave(&hf_fifo->buffer_lock, flags);
+ if (unlikely(hf_fifo->buffull == true)) {
+ pr_err_ratelimited("%s [%s][%d:%d] buffer full, [%d,%lld]\n",
+ __func__, client->proc_comm, client->leader_pid,
+ client->pid, event->sensor_type, event->timestamp);
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+ wake_up_interruptible(&hf_fifo->wait);
+ /*
+ * must return -1 when buffer full, tell caller retry
+ * send data some times later.
+ */
+ return -1;
+ }
+ /* only data action run filter event */
+ if (likely(event->action == DATA_ACTION) &&
+ unlikely(filter_event_by_timestamp(hf_fifo, event))) {
+ pr_err_ratelimited("%s [%s][%d:%d] filterd, [%d,%lld]\n",
+ __func__, client->proc_comm, client->leader_pid,
+ client->pid, event->sensor_type, event->timestamp);
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+ /*
+ * must return 0 when timestamp filtered, tell caller data
+ * already in buffer, don't need send again.
+ */
+ return 0;
+ }
+ hf_fifo->buffer[hf_fifo->head++] = *event;
+ hf_fifo->head &= hf_fifo->bufsize - 1;
+ /* remain 1 count */
+ next = hf_fifo->head + 1;
+ next &= hf_fifo->bufsize - 1;
+ if (unlikely(next == hf_fifo->tail))
+ hf_fifo->buffull = true;
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+
+ wake_up_interruptible(&hf_fifo->wait);
+ return 0;
+}
+
+static void hf_manager_io_schedule(struct hf_manager *manager)
+{
+ if (!atomic_read(&manager->io_enabled))
+ return;
+ if (READ_ONCE(manager->hf_dev->device_bus) == HF_DEVICE_IO_ASYNC)
+ tasklet_schedule(&manager->io_work_tasklet);
+ else if (READ_ONCE(manager->hf_dev->device_bus) == HF_DEVICE_IO_SYNC)
+ kthread_queue_work(&hf_manager_kthread_worker,
+ &manager->io_kthread_work);
+}
+
+static int hf_manager_io_report(struct hf_manager *manager,
+ struct hf_manager_event *event)
+{
+ /* must return 0 when sensor_type exceed and no need to retry */
+ if (unlikely(event->sensor_type >= SENSOR_TYPE_SENSOR_MAX)) {
+ pr_err_ratelimited("%s %d exceed max sensor id\n", __func__,
+ event->sensor_type);
+ return 0;
+ }
+ return hf_manager_find_client(event);
+}
+
+static void hf_manager_io_complete(struct hf_manager *manager)
+{
+ clear_bit(HF_MANAGER_IO_IN_PROGRESS, &(manager->flags));
+ if (test_and_clear_bit(HF_MANAGER_IO_READY, &manager->flags))
+ hf_manager_io_schedule(manager);
+}
+
+static void hf_manager_io_sample(struct hf_manager *manager)
+{
+ int retval;
+
+ if (!manager->hf_dev || !manager->hf_dev->sample)
+ return;
+
+ if (!test_and_set_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags)) {
+ retval = manager->hf_dev->sample(manager->hf_dev);
+ if (retval) {
+ clear_bit(HF_MANAGER_IO_IN_PROGRESS,
+ &manager->flags);
+ hf_manager_io_schedule(manager);
+ }
+ }
+}
+
+static void hf_manager_io_tasklet(unsigned long data)
+{
+ struct hf_manager *manager = (struct hf_manager *)data;
+
+ hf_manager_io_sample(manager);
+}
+
+static void hf_manager_io_kthread_work(struct kthread_work *work)
+{
+ struct hf_manager *manager =
+ container_of(work, struct hf_manager, io_kthread_work);
+
+ hf_manager_io_sample(manager);
+}
+
+static void hf_manager_sched_sample(struct hf_manager *manager)
+{
+ if (!test_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags))
+ hf_manager_io_schedule(manager);
+ else
+ set_bit(HF_MANAGER_IO_READY, &manager->flags);
+}
+
+static enum hrtimer_restart hf_manager_io_poll(struct hrtimer *timer)
+{
+ struct hf_manager *manager =
+ (struct hf_manager *)container_of(timer,
+ struct hf_manager, io_poll_timer);
+
+ hf_manager_sched_sample(manager);
+ hrtimer_forward_now(&manager->io_poll_timer,
+ ns_to_ktime(atomic64_read(&manager->io_poll_interval)));
+ return HRTIMER_RESTART;
+}
+
+static void hf_manager_io_interrupt(struct hf_manager *manager)
+{
+ hf_manager_sched_sample(manager);
+}
+
+int hf_manager_create(struct hf_device *device)
+{
+ unsigned char sensor_type = 0;
+ int i = 0;
+ int err = 0;
+ struct hf_manager *manager = NULL;
+
+ if (!device || !device->dev_name ||
+ !device->support_list || !device->support_size)
+ return -EFAULT;
+
+ manager = kzalloc(sizeof(*manager), GFP_KERNEL);
+ if (!manager)
+ return -ENOMEM;
+
+ manager->hf_dev = device;
+ device->manager = manager;
+
+ atomic_set(&manager->io_enabled, 0);
+ atomic64_set(&manager->io_poll_interval, S64_MAX);
+
+ clear_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags);
+ clear_bit(HF_MANAGER_IO_READY, &manager->flags);
+
+ if (device->device_poll == HF_DEVICE_IO_POLLING) {
+ hrtimer_init(&manager->io_poll_timer,
+ CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+ manager->io_poll_timer.function = hf_manager_io_poll;
+ } else if (device->device_poll == HF_DEVICE_IO_INTERRUPT) {
+ manager->interrupt = hf_manager_io_interrupt;
+ }
+ manager->report = hf_manager_io_report;
+ manager->complete = hf_manager_io_complete;
+
+ if (device->device_bus == HF_DEVICE_IO_ASYNC)
+ tasklet_init(&manager->io_work_tasklet,
+ hf_manager_io_tasklet, (unsigned long)manager);
+ else if (device->device_bus == HF_DEVICE_IO_SYNC)
+ kthread_init_work(&manager->io_kthread_work,
+ hf_manager_io_kthread_work);
+
+ for (i = 0; i < device->support_size; ++i) {
+ sensor_type = device->support_list[i];
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX)) {
+ pr_err("%s %s %d exceed max sensor id\n", __func__,
+ device->dev_name, sensor_type);
+ err = -EINVAL;
+ goto out_err;
+ }
+ if (test_and_set_bit(sensor_type, sensor_list_bitmap)) {
+ pr_err("%s %s %d repeat\n", __func__,
+ device->dev_name, sensor_type);
+ err = -EBUSY;
+ goto out_err;
+ }
+ }
+
+ INIT_LIST_HEAD(&manager->list);
+ mutex_lock(&hf_manager_list_mtx);
+ list_add(&manager->list, &hf_manager_list);
+ mutex_unlock(&hf_manager_list_mtx);
+
+ return 0;
+out_err:
+ kfree(manager);
+ return err;
+}
+
+int hf_manager_destroy(struct hf_manager *manager)
+{
+ int i = 0;
+ struct hf_device *device = NULL;
+
+ if (!manager || !manager->hf_dev || !manager->hf_dev->support_list)
+ return -EFAULT;
+
+ device = manager->hf_dev;
+ for (i = 0; i < device->support_size; ++i) {
+ clear_bit(device->support_list[i],
+ sensor_list_bitmap);
+ }
+ mutex_lock(&hf_manager_list_mtx);
+ list_del(&manager->list);
+ mutex_unlock(&hf_manager_list_mtx);
+ if (manager->hf_dev->device_bus == HF_DEVICE_IO_ASYNC)
+ tasklet_kill(&manager->io_work_tasklet);
+
+ kfree(manager);
+ return 0;
+}
+
+static int hf_manager_distinguish_event(struct hf_client *client,
+ struct hf_manager_event *event)
+{
+ int err = 0;
+ unsigned long flags;
+ struct sensor_state *request = &client->request[event->sensor_type];
+
+ switch (event->action) {
+ case DATA_ACTION:
+ /* must relay on enable status client requested */
+ if (READ_ONCE(request->enable) &&
+ (event->timestamp >
+ atomic64_read(&request->start_time)))
+ err = hf_manager_report_event(client, event);
+ break;
+ case FLUSH_ACTION:
+ /*
+ * flush relay on flush count client requested,
+ * must not relay on enable status.
+ * flush may report both by looper thread and disable thread.
+ * spinlock prevent flush count report more than request.
+ * sequence:
+ * flush = 1
+ * looper thread flush > 0
+ * looper thread hf_manager_report_event
+ * disable thread flush > 0
+ * disable thread hf_manager_report_event
+ * flush complete report 2 times but request is 1.
+ */
+ spin_lock_irqsave(&client->request_lock, flags);
+ if (atomic_read(&request->flush) > 0) {
+ err = hf_manager_report_event(client, event);
+ /* return < 0, don't decrease flush count */
+ if (err < 0) {
+ spin_unlock_irqrestore(&client->request_lock,
+ flags);
+ return err;
+ }
+ atomic_dec_if_positive(&request->flush);
+ }
+ spin_unlock_irqrestore(&client->request_lock, flags);
+ break;
+ case BIAS_ACTION:
+ /* relay on status client requested, don't check return */
+ if (READ_ONCE(request->bias))
+ hf_manager_report_event(client, event);
+ break;
+ case CALI_ACTION:
+ /* cali on status client requested, don't check return */
+ if (READ_ONCE(request->cali))
+ hf_manager_report_event(client, event);
+ break;
+ case TEMP_ACTION:
+ /* temp on status client requested, don't check return */
+ if (READ_ONCE(request->temp))
+ hf_manager_report_event(client, event);
+ break;
+ case TEST_ACTION:
+ /* test on status client requested, don't check return */
+ if (READ_ONCE(request->test))
+ hf_manager_report_event(client, event);
+ break;
+ case RAW_ACTION:
+ /* raw on status client requested, don't check return */
+ if (READ_ONCE(request->raw))
+ hf_manager_report_event(client, event);
+ break;
+ }
+ return err;
+}
+
+static int hf_manager_find_client(struct hf_manager_event *event)
+{
+ int err = 0;
+ unsigned long flags;
+ struct hf_client *client = NULL;
+
+ spin_lock_irqsave(&hf_client_list_lock, flags);
+ list_for_each_entry(client, &hf_client_list, list) {
+ /* must (err |=), collect all err to decide retry */
+ err |= hf_manager_distinguish_event(client, event);
+ }
+ spin_unlock_irqrestore(&hf_client_list_lock, flags);
+
+ return err;
+}
+
+static struct hf_manager *hf_manager_find_manager(uint8_t sensor_type)
+{
+ int i = 0;
+ struct hf_manager *manager = NULL;
+ struct hf_device *device = NULL;
+
+ list_for_each_entry(manager, &hf_manager_list, list) {
+ device = READ_ONCE(manager->hf_dev);
+ if (!device || !device->support_list)
+ continue;
+ for (i = 0; i < device->support_size; ++i) {
+ if (sensor_type == device->support_list[i])
+ return manager;
+ }
+ }
+ return NULL;
+}
+
+static void hf_manager_update_client_param(
+ struct hf_client *client, struct hf_manager_cmd *cmd)
+{
+ struct sensor_state *request = &client->request[cmd->sensor_type];
+
+ /* only enable disable update action delay and latency */
+ if (cmd->action == HF_MANAGER_SENSOR_ENABLE) {
+ if (!request->enable)
+ atomic64_set(&request->start_time,
+ ktime_get_boot_ns());
+ request->enable = true;
+ request->delay = cmd->delay;
+ request->latency = cmd->latency;
+ } else if (cmd->action == HF_MANAGER_SENSOR_DISABLE) {
+ atomic64_set(&request->start_time, S64_MAX);
+ request->enable = false;
+ request->delay = S64_MAX;
+ request->latency = S64_MAX;
+ }
+}
+
+static void hf_manager_find_best_param(uint8_t sensor_type,
+ bool *action, int64_t *delay, int64_t *latency)
+{
+ unsigned long flags;
+ struct hf_client *client = NULL;
+ struct sensor_state *request = NULL;
+ bool tmp_enable = false;
+ int64_t tmp_delay = S64_MAX;
+ int64_t tmp_latency = S64_MAX;
+
+ spin_lock_irqsave(&hf_client_list_lock, flags);
+ list_for_each_entry(client, &hf_client_list, list) {
+ request = &client->request[sensor_type];
+ if (request->enable) {
+ tmp_enable = true;
+ if (request->delay < tmp_delay)
+ tmp_delay = request->delay;
+ if (request->latency < tmp_latency)
+ tmp_latency = request->latency;
+ }
+ }
+ spin_unlock_irqrestore(&hf_client_list_lock, flags);
+ *action = tmp_enable;
+ *delay = tmp_delay;
+ *latency = tmp_latency;
+
+#ifdef HF_MANAGER_DEBUG
+ if (tmp_enable)
+ pr_notice("%s: %d,%d,%lld,%lld\n", __func__,
+ sensor_type, tmp_enable, tmp_delay, tmp_latency);
+ else
+ pr_notice("%s: %d,%d\n", __func__, sensor_type, tmp_enable);
+#endif
+}
+
+static bool device_rebatch(uint8_t sensor_type,
+ int64_t best_delay, int64_t best_latency)
+{
+ if (prev_request[sensor_type].delay != best_delay ||
+ prev_request[sensor_type].latency != best_latency) {
+ prev_request[sensor_type].delay = best_delay;
+ prev_request[sensor_type].latency = best_latency;
+ return true;
+ }
+ return false;
+}
+
+static bool device_reenable(uint8_t sensor_type, bool best_enable)
+{
+ if (prev_request[sensor_type].enable != best_enable) {
+ prev_request[sensor_type].enable = best_enable;
+ return true;
+ }
+ return false;
+}
+
+static bool device_redisable(uint8_t sensor_type, bool best_enable,
+ int64_t best_delay, int64_t best_latency)
+{
+ if (prev_request[sensor_type].enable != best_enable) {
+ prev_request[sensor_type].enable = best_enable;
+ prev_request[sensor_type].delay = best_delay;
+ prev_request[sensor_type].latency = best_latency;
+ return true;
+ }
+ return false;
+}
+
+static int64_t device_poll_min_interval(struct hf_device *device)
+{
+ int i = 0, j = 0;
+ int64_t interval = S64_MAX;
+
+ for (i = 0; i < device->support_size; ++i) {
+ j = device->support_list[i];
+ if (prev_request[j].enable) {
+ if (prev_request[j].delay < interval)
+ interval = prev_request[j].delay;
+ }
+ }
+ return interval;
+}
+
+static void device_poll_trigger(struct hf_device *device, bool enable)
+{
+ int64_t min_interval = S64_MAX;
+ struct hf_manager *manager = device->manager;
+
+ BUG_ON(enable && !atomic_read(&manager->io_enabled));
+ min_interval = device_poll_min_interval(device);
+ BUG_ON(atomic_read(&manager->io_enabled) && min_interval == S64_MAX);
+ if (atomic64_read(&manager->io_poll_interval) == min_interval)
+ return;
+ atomic64_set(&manager->io_poll_interval, min_interval);
+ if (atomic_read(&manager->io_enabled))
+ hrtimer_start(&manager->io_poll_timer,
+ ns_to_ktime(min_interval), HRTIMER_MODE_REL);
+ else
+ hrtimer_cancel(&manager->io_poll_timer);
+}
+
+static int hf_manager_device_enable(struct hf_device *device,
+ uint8_t sensor_type)
+{
+ int err = 0;
+ struct hf_manager *manager = device->manager;
+ bool best_enable = false;
+ int64_t best_delay = S64_MAX;
+ int64_t best_latency = S64_MAX;
+
+ if (!device->enable || !device->batch)
+ return -EINVAL;
+
+ hf_manager_find_best_param(sensor_type, &best_enable,
+ &best_delay, &best_latency);
+
+ if (best_enable) {
+ if (device_rebatch(sensor_type, best_delay, best_latency))
+ err = device->batch(device, sensor_type,
+ best_delay, best_latency);
+ if (device_reenable(sensor_type, best_enable)) {
+ err = device->enable(device, sensor_type, best_enable);
+ /* must update io_enabled before hrtimer_start */
+ atomic_inc(&manager->io_enabled);
+ }
+ if (device->device_poll == HF_DEVICE_IO_POLLING)
+ device_poll_trigger(device, best_enable);
+ } else {
+ if (device_redisable(sensor_type, best_enable,
+ best_delay, best_latency)) {
+ err = device->enable(device, sensor_type, best_enable);
+ atomic_dec_if_positive(&manager->io_enabled);
+ }
+ if (device->device_poll == HF_DEVICE_IO_POLLING)
+ device_poll_trigger(device, best_enable);
+ if (device->device_bus == HF_DEVICE_IO_ASYNC &&
+ !atomic_read(&manager->io_enabled))
+ tasklet_kill(&manager->io_work_tasklet);
+ }
+ return err;
+}
+
+static int hf_manager_device_flush(struct hf_device *device,
+ uint8_t sensor_type)
+{
+ if (!device->flush)
+ return -EINVAL;
+
+ return device->flush(device, sensor_type);
+}
+
+static int hf_manager_device_calibration(struct hf_device *device,
+ uint8_t sensor_type)
+{
+ if (device->calibration)
+ return device->calibration(device, sensor_type);
+ return 0;
+}
+
+static int hf_manager_device_config_cali(struct hf_device *device,
+ uint8_t sensor_type, int32_t *data)
+{
+ if (device->config_cali)
+ return device->config_cali(device, sensor_type, data);
+ return 0;
+}
+
+static int hf_manager_device_selftest(struct hf_device *device,
+ uint8_t sensor_type)
+{
+ if (device->selftest)
+ return device->selftest(device, sensor_type);
+ return 0;
+}
+
+static int hf_manager_device_rawdata(struct hf_device *device,
+ uint8_t sensor_type)
+{
+ unsigned long flags;
+ struct hf_client *client = NULL;
+ struct sensor_state *request = NULL;
+ bool best_enable = false;
+
+ spin_lock_irqsave(&hf_client_list_lock, flags);
+ list_for_each_entry(client, &hf_client_list, list) {
+ request = &client->request[sensor_type];
+ if (request->raw)
+ best_enable = true;
+ }
+ spin_unlock_irqrestore(&hf_client_list_lock, flags);
+
+ if (prev_request[sensor_type].raw == best_enable)
+ return 0;
+ prev_request[sensor_type].raw = best_enable;
+ if (device->rawdata)
+ return device->rawdata(device, sensor_type, best_enable);
+ return 0;
+}
+
+static int hf_manager_drive_device(struct hf_client *client,
+ struct hf_manager_cmd *cmd)
+{
+ int err = 0;
+ struct hf_manager *manager = NULL;
+ struct hf_device *device = NULL;
+ uint8_t sensor_type = cmd->sensor_type;
+
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+
+ mutex_lock(&hf_manager_list_mtx);
+ manager = hf_manager_find_manager(sensor_type);
+ if (!manager) {
+ pr_err("%s: no manager finded\n", __func__);
+ mutex_unlock(&hf_manager_list_mtx);
+ return -EINVAL;
+ }
+ device = manager->hf_dev;
+ if (!device || !device->dev_name) {
+ pr_err("%s: no hf device or important param finded\n",
+ __func__);
+ mutex_unlock(&hf_manager_list_mtx);
+ return -EINVAL;
+ }
+
+#ifdef HF_MANAGER_DEBUG
+ pr_notice("%s: %s: %d,%d,%lld,%lld\n", __func__, device->dev_name,
+ cmd->sensor_type, cmd->action, cmd->delay, cmd->latency);
+#endif
+
+ switch (cmd->action) {
+ case HF_MANAGER_SENSOR_ENABLE:
+ case HF_MANAGER_SENSOR_DISABLE:
+ hf_manager_update_client_param(client, cmd);
+ err = hf_manager_device_enable(device, sensor_type);
+ break;
+ case HF_MANAGER_SENSOR_FLUSH:
+ atomic_inc(&client->request[sensor_type].flush);
+ err = hf_manager_device_flush(device, sensor_type);
+ if (err < 0)
+ atomic_dec_if_positive(
+ &client->request[sensor_type].flush);
+ break;
+ case HF_MANAGER_SENSOR_ENABLE_CALI:
+ err = hf_manager_device_calibration(device, sensor_type);
+ break;
+ case HF_MANAGER_SENSOR_CONFIG_CALI:
+ err = hf_manager_device_config_cali(device,
+ sensor_type, cmd->data);
+ break;
+ case HF_MANAGER_SENSOR_SELFTEST:
+ err = hf_manager_device_selftest(device, sensor_type);
+ break;
+ case HF_MANAGER_SENSOR_RAWDATA:
+ client->request[sensor_type].raw =
+ cmd->data[0] ? true : false;
+ err = hf_manager_device_rawdata(device, sensor_type);
+ break;
+ }
+ mutex_unlock(&hf_manager_list_mtx);
+ return err;
+}
+
+static int hf_manager_open(struct inode *inode, struct file *filp)
+{
+ int err = 0;
+ unsigned long flags;
+ struct hf_client *client = NULL;
+ struct hf_client_fifo *hf_fifo = NULL;
+
+ client = kzalloc(sizeof(*client), GFP_KERNEL);
+ if (!client) {
+ err = -ENOMEM;
+ goto err_out;
+ }
+
+ /* record process id and thread id for debug */
+ strlcpy(client->proc_comm, current->comm, sizeof(client->proc_comm));
+ client->leader_pid = current->group_leader->pid;
+ client->pid = current->pid;
+
+ pr_notice("%s: [%s][%d:%d]\n", __func__, current->comm,
+ current->group_leader->pid, current->pid);
+
+ INIT_LIST_HEAD(&client->list);
+
+ hf_fifo = &client->hf_fifo;
+ hf_fifo->head = 0;
+ hf_fifo->tail = 0;
+ hf_fifo->bufsize = roundup_pow_of_two(HF_CLIENT_FIFO_SIZE);
+ hf_fifo->buffull = false;
+ spin_lock_init(&hf_fifo->buffer_lock);
+ init_waitqueue_head(&hf_fifo->wait);
+ hf_fifo->buffer =
+ kcalloc(hf_fifo->bufsize, sizeof(*hf_fifo->buffer),
+ GFP_KERNEL);
+ if (!hf_fifo->buffer) {
+ err = -ENOMEM;
+ goto err_free;
+ }
+
+ spin_lock_init(&client->request_lock);
+
+ filp->private_data = client;
+
+ spin_lock_irqsave(&hf_client_list_lock, flags);
+ list_add(&client->list, &hf_client_list);
+ spin_unlock_irqrestore(&hf_client_list_lock, flags);
+
+ nonseekable_open(inode, filp);
+ return 0;
+err_free:
+ kfree(client);
+err_out:
+ return err;
+}
+
+static int hf_manager_release(struct inode *inode, struct file *filp)
+{
+ unsigned long flags;
+ struct hf_client *client = filp->private_data;
+
+ pr_notice("%s: [%s][%d:%d]\n", __func__, current->comm,
+ current->group_leader->pid, current->pid);
+
+ filp->private_data = NULL;
+
+ spin_lock_irqsave(&hf_client_list_lock, flags);
+ list_del(&client->list);
+ spin_unlock_irqrestore(&hf_client_list_lock, flags);
+
+ kfree(client->hf_fifo.buffer);
+ kfree(client);
+ return 0;
+}
+
+static int hf_manager_fetch_next(struct hf_client_fifo *hf_fifo,
+ struct hf_manager_event *event)
+{
+ unsigned long flags;
+ int have_event;
+
+ spin_lock_irqsave(&hf_fifo->buffer_lock, flags);
+ have_event = hf_fifo->head != hf_fifo->tail;
+ if (have_event) {
+ *event = hf_fifo->buffer[hf_fifo->tail++];
+ hf_fifo->tail &= hf_fifo->bufsize - 1;
+ hf_fifo->buffull = false;
+ }
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+ return have_event;
+}
+
+static ssize_t hf_manager_read(struct file *filp,
+ char __user *buf, size_t count, loff_t *f_pos)
+{
+ struct hf_client *client = filp->private_data;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+ struct hf_manager_event event;
+ size_t read = 0;
+
+ if (count != 0 && count < sizeof(struct hf_manager_event))
+ return -EINVAL;
+
+ for (;;) {
+ if (hf_fifo->head == hf_fifo->tail)
+ return 0;
+ if (count == 0)
+ break;
+ while (read + sizeof(struct hf_manager_event) <= count &&
+ hf_manager_fetch_next(hf_fifo, &event)) {
+ if (copy_to_user(buf + read,
+ &event, sizeof(struct hf_manager_event)))
+ return -EFAULT;
+ read += sizeof(struct hf_manager_event);
+ }
+ if (read)
+ break;
+ }
+ return read;
+}
+
+static ssize_t hf_manager_write(struct file *filp,
+ const char __user *buf, size_t count, loff_t *f_pos)
+{
+ struct hf_manager_cmd cmd;
+ struct hf_client *client = filp->private_data;
+
+ memset(&cmd, 0, sizeof(struct hf_manager_cmd));
+
+ if (count != sizeof(struct hf_manager_cmd))
+ return -EFAULT;
+
+ if (copy_from_user(&cmd, buf, count))
+ return -EFAULT;
+
+ return hf_manager_drive_device(client, &cmd);
+}
+
+static unsigned int hf_manager_poll(struct file *filp,
+ struct poll_table_struct *wait)
+{
+ struct hf_client *client = filp->private_data;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+ unsigned int mask = 0;
+
+ poll_wait(filp, &hf_fifo->wait, wait);
+
+ if (hf_fifo->head != hf_fifo->tail)
+ mask |= POLLIN | POLLRDNORM;
+
+ return mask;
+}
+
+static long hf_manager_ioctl(struct file *filp,
+ unsigned int cmd, unsigned long arg)
+{
+ struct hf_client *client = filp->private_data;
+ unsigned int size = _IOC_SIZE(cmd);
+ void __user *ubuf = (void __user *)arg;
+ unsigned int sensor_type = 0;
+ struct ioctl_packet packet;
+
+ memset(&packet, 0, sizeof(struct ioctl_packet));
+
+ if (size != sizeof(struct ioctl_packet))
+ return -EINVAL;
+ if (copy_from_user(&packet, ubuf, sizeof(struct ioctl_packet)))
+ return -EFAULT;
+ sensor_type = packet.sensor_type;
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+
+ switch (cmd) {
+ case HF_MANAGER_REQUEST_REGISTER_STATUS:
+ packet.status = test_bit(sensor_type, sensor_list_bitmap);
+ if (copy_to_user(ubuf, &packet, sizeof(struct ioctl_packet)))
+ return -EFAULT;
+ break;
+ case HF_MANAGER_REQUEST_BIAS_DATA:
+ client->request[sensor_type].bias = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_CALI_DATA:
+ client->request[sensor_type].cali = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_TEMP_DATA:
+ client->request[sensor_type].temp = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_TEST_DATA:
+ client->request[sensor_type].test = packet.status;
+ break;
+ }
+ return 0;
+}
+
+static ssize_t client_info_show(struct device *dev,
+ struct device_attribute *attr, char *buf)
+{
+ return 0;
+}
+
+DEVICE_ATTR(client_info, 0644, client_info_show, NULL);
+
+static struct attribute *hf_manager_attrs[] = {
+ &dev_attr_client_info.attr,
+ NULL
+};
+
+static struct attribute_group hf_manager_group = {
+ .attrs = hf_manager_attrs
+};
+
+static const struct file_operations hf_manager_fops = {
+ .owner = THIS_MODULE,
+ .open = hf_manager_open,
+ .release = hf_manager_release,
+ .read = hf_manager_read,
+ .write = hf_manager_write,
+ .poll = hf_manager_poll,
+ .unlocked_ioctl = hf_manager_ioctl,
+ .compat_ioctl = hf_manager_ioctl,
+};
+
+static int hf_manager_proc_show(struct seq_file *m, void *v)
+{
+ int i = 0, sensor_type = 0;
+ unsigned long flags;
+ struct hf_manager *manager = NULL;
+ struct hf_client *client = NULL;
+ struct hf_device *device = NULL;
+
+ mutex_lock(&hf_manager_list_mtx);
+ list_for_each_entry(manager, &hf_manager_list, list) {
+ device = READ_ONCE(manager->hf_dev);
+ if (!device || !device->support_list)
+ continue;
+ seq_printf(m, "manager: param:[%d,%lld]\n",
+ atomic_read(&manager->io_enabled),
+ (int64_t)atomic64_read(&manager->io_poll_interval));
+ seq_printf(m, "device:%s poll:%s bus:%s online\n",
+ device->dev_name,
+ device->device_poll ? "io_polling" : "io_interrupt",
+ device->device_bus ? "io_async" : "io_sync");
+ for (i = 0; i < device->support_size; ++i) {
+ sensor_type = device->support_list[i];
+ seq_printf(m, "support:%d now param:[%d,%lld,%lld]\n",
+ sensor_type,
+ prev_request[sensor_type].enable,
+ prev_request[sensor_type].delay,
+ prev_request[sensor_type].latency);
+ }
+ }
+ mutex_unlock(&hf_manager_list_mtx);
+
+ spin_lock_irqsave(&hf_client_list_lock, flags);
+ list_for_each_entry(client, &hf_client_list, list) {
+ seq_printf(m, "client:%s pid:[%d:%d] online\n",
+ client->proc_comm,
+ client->leader_pid,
+ client->pid);
+ for (i = 0; i < SENSOR_TYPE_SENSOR_MAX; ++i) {
+ if (!client->request[i].enable)
+ continue;
+ seq_printf(m, "request:%d param:[%d,%lld,%lld,%lld]\n",
+ i,
+ client->request[i].enable,
+ client->request[i].delay,
+ client->request[i].latency,
+ (int64_t)atomic64_read(
+ &client->request[i].start_time));
+ }
+ }
+ spin_unlock_irqrestore(&hf_client_list_lock, flags);
+ return 0;
+}
+
+static int hf_manager_proc_open(struct inode *inode,
+ struct file *filp)
+{
+ return single_open(filp, hf_manager_proc_show, NULL);
+}
+
+static const struct file_operations hf_manager_proc_fops = {
+ .open = hf_manager_proc_open,
+ .release = single_release,
+ .read = seq_read,
+ .llseek = seq_lseek,
+};
+
+
+static int __init hf_manager_init(void)
+{
+ int major = -1, i = 0;
+ struct class *hf_manager_class;
+ struct device *dev;
+ struct sched_param param = { .sched_priority = MAX_RT_PRIO / 2 };
+
+ for (i = 0; i < SENSOR_TYPE_SENSOR_MAX; ++i) {
+ prev_request[i].delay = S64_MAX;
+ prev_request[i].latency = S64_MAX;
+ atomic64_set(&prev_request[i].start_time, S64_MAX);
+ }
+
+ major = register_chrdev(0, "hf_manager", &hf_manager_fops);
+ if (major < 0) {
+ pr_err("%s unable to get major %d\n", __func__, major);
+ return -1;
+ }
+ hf_manager_class = class_create(THIS_MODULE, "hf_manager");
+ if (IS_ERR(hf_manager_class))
+ return PTR_ERR(hf_manager_class);
+ dev = device_create(hf_manager_class, NULL, MKDEV(major, 0),
+ NULL, "hf_manager");
+ if (IS_ERR(dev))
+ return -1;
+
+ proc_create("hf_manager", 0644, NULL, &hf_manager_proc_fops);
+
+ if (sysfs_create_group(&dev->kobj, &hf_manager_group) < 0)
+ return -1;
+
+ kthread_init_worker(&hf_manager_kthread_worker);
+ hf_manager_kthread_task = kthread_run(kthread_worker_fn,
+ &hf_manager_kthread_worker, "hf_manager");
+ if (IS_ERR(hf_manager_kthread_task)) {
+ pr_err("%s failed to create kthread\n", __func__);
+ return -1;
+ }
+ sched_setscheduler(hf_manager_kthread_task, SCHED_FIFO, ¶m);
+ return 0;
+}
+subsys_initcall(hf_manager_init);
+
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("high freq sensor manaer driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_manager.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_manager.h
new file mode 100644
index 0000000..f92317b
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_manager.h
@@ -0,0 +1,125 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef _HF_SENSOR_MANAGER_H_
+#define _HF_SENSOR_MANAGER_H_
+
+#include <linux/hrtimer.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <linux/ioctl.h>
+#include <linux/types.h>
+
+#include "hf_sensor_type.h"
+#include "hf_sensor_io.h"
+
+#define HF_MANAGER_IO_IN_PROGRESS 0
+#define HF_MANAGER_IO_READY 1
+
+#define HF_DEVICE_IO_SYNC 0
+#define HF_DEVICE_IO_ASYNC 1
+
+#define HF_DEVICE_IO_INTERRUPT 0
+#define HF_DEVICE_IO_POLLING 1
+
+#define HF_CLIENT_FIFO_SIZE 128
+
+struct coordinate {
+ int8_t sign[3];
+ uint8_t map[3];
+};
+
+struct sensor_state {
+ bool enable;
+ bool bias;
+ bool cali;
+ bool temp;
+ bool test;
+ bool raw;
+ int64_t delay;
+ int64_t latency;
+ atomic_t flush;
+ atomic64_t start_time;
+};
+
+struct hf_device {
+ int (*sample)(struct hf_device *hfdev);
+ int (*enable)(struct hf_device *hfdev, int sensor_type, int en);
+ int (*batch)(struct hf_device *hfdev, int sensor_type,
+ int64_t delay, int64_t latency);
+ int (*flush)(struct hf_device *hfdev, int sensor_type);
+ int (*calibration)(struct hf_device *hfdev, int sensor_type);
+ int (*config_cali)(struct hf_device *hfdev,
+ int sensor_type, int32_t *data);
+ int (*selftest)(struct hf_device *hfdev, int sensor_type);
+ int (*rawdata)(struct hf_device *hfdev, int sensor_type, int en);
+
+ char *dev_name;
+ unsigned char device_poll;
+ unsigned char device_bus;
+
+ unsigned char *support_list;
+ unsigned int support_size;
+
+ struct hf_manager *manager;
+ void *private_data;
+};
+
+struct hf_client_fifo {
+ spinlock_t buffer_lock;
+ unsigned int head;
+ unsigned int tail;
+ unsigned int bufsize;
+ unsigned int buffull;
+ int64_t last_time_stamp[SENSOR_TYPE_SENSOR_MAX];
+ struct hf_manager_event *buffer;
+ wait_queue_head_t wait;
+};
+
+struct hf_manager {
+ struct list_head list;
+ struct tasklet_struct io_work_tasklet;
+ struct kthread_work io_kthread_work;
+ struct hrtimer io_poll_timer;
+ atomic64_t io_poll_interval;
+ atomic_t io_enabled;
+ unsigned long flags;
+ struct hf_device *hf_dev;
+
+ int (*report)(struct hf_manager *manager,
+ struct hf_manager_event *event);
+ void (*complete)(struct hf_manager *manager);
+ void (*interrupt)(struct hf_manager *manager);
+};
+
+struct hf_client {
+ struct list_head list;
+ struct hf_client_fifo hf_fifo;
+ struct sensor_state request[SENSOR_TYPE_SENSOR_MAX];
+ spinlock_t request_lock;
+
+ /* record process info */
+ char proc_comm[TASK_COMM_LEN];
+ pid_t leader_pid;
+ pid_t pid;
+};
+
+static inline void hf_device_set_private_data(struct hf_device *device,
+ void *data)
+{
+ device->private_data = data;
+}
+
+static inline void *hf_device_get_private_data(struct hf_device *device)
+{
+ return device->private_data;
+}
+
+int hf_manager_create(struct hf_device *device);
+int hf_manager_destroy(struct hf_manager *manager);
+void coordinate_map(unsigned char direction, int32_t *data);
+void get_placement_info(unsigned char direction, int8_t *data);
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_sensor_io.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_sensor_io.h
new file mode 100644
index 0000000..c26fd75
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_sensor_io.h
@@ -0,0 +1,63 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef _HF_SENSOR_IO_H_
+#define _HF_SENSOR_IO_H_
+
+#include <linux/ioctl.h>
+#include <linux/types.h>
+
+enum {
+ HF_MANAGER_SENSOR_DISABLE,
+ HF_MANAGER_SENSOR_ENABLE,
+ HF_MANAGER_SENSOR_FLUSH,
+ HF_MANAGER_SENSOR_ENABLE_CALI,
+ HF_MANAGER_SENSOR_CONFIG_CALI,
+ HF_MANAGER_SENSOR_SELFTEST,
+ HF_MANAGER_SENSOR_RAWDATA,
+};
+
+enum {
+ DATA_ACTION,
+ FLUSH_ACTION,
+ BIAS_ACTION,
+ CALI_ACTION,
+ TEMP_ACTION,
+ TEST_ACTION,
+ RAW_ACTION,
+};
+
+struct hf_manager_cmd {
+ uint8_t sensor_type;
+ uint8_t action;
+ int64_t delay;
+ int64_t latency;
+ int32_t data[12];
+} __packed;
+
+struct hf_manager_event {
+ int64_t timestamp;
+ uint8_t sensor_type;
+ uint8_t accurancy;
+ uint8_t action;
+ uint8_t reserved;
+ union {
+ int32_t word[6];
+ int8_t byte[0];
+ };
+} __packed;
+
+struct ioctl_packet {
+ uint8_t sensor_type;
+ bool status;
+} __packed;
+
+#define HF_MANAGER_REQUEST_REGISTER_STATUS _IOWR('a', 1, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_BIAS_DATA _IOW('a', 2, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_CALI_DATA _IOW('a', 3, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_TEMP_DATA _IOW('a', 4, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_TEST_DATA _IOW('a', 5, struct ioctl_packet)
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_sensor_type.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_sensor_type.h
new file mode 100644
index 0000000..50cd911
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/hf_sensor_type.h
@@ -0,0 +1,140 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef _HF_SENSOR_TYPE_H_
+#define _HF_SENSOR_TYPE_H_
+
+enum {
+ /* follow google default sensor type */
+ SENSOR_TYPE_ACCELEROMETER = 1,
+ SENSOR_TYPE_MAGNETIC_FIELD,
+ SENSOR_TYPE_ORIENTATION,
+ SENSOR_TYPE_GYROSCOPE,
+ SENSOR_TYPE_LIGHT,
+ SENSOR_TYPE_PRESSURE,
+ SENSOR_TYPE_TEMPERATURE,
+ SENSOR_TYPE_PROXIMITY,
+ SENSOR_TYPE_GRAVITY,
+ SENSOR_TYPE_LINEAR_ACCELERATION,
+ SENSOR_TYPE_ROTATION_VECTOR,
+ SENSOR_TYPE_RELATIVE_HUMIDITY,
+ SENSOR_TYPE_AMBIENT_TEMPERATURE,
+ SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+ SENSOR_TYPE_GAME_ROTATION_VECTOR,
+ SENSOR_TYPE_GYROSCOPE_UNCALIBRATED,
+ SENSOR_TYPE_SIGNIFICANT_MOTION,
+ SENSOR_TYPE_STEP_DETECTOR,
+ SENSOR_TYPE_STEP_COUNTER,
+ SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR,
+ SENSOR_TYPE_HEART_RATE,
+ SENSOR_TYPE_TILT_DETECTOR,
+ SENSOR_TYPE_WAKE_GESTURE,
+ SENSOR_TYPE_GLANCE_GESTURE,
+ SENSOR_TYPE_PICK_UP_GESTURE,
+ SENSOR_TYPE_WRIST_TILT_GESTURE,
+ SENSOR_TYPE_DEVICE_ORIENTATION,
+ SENSOR_TYPE_POSE_6DOF,
+ SENSOR_TYPE_STATIONARY_DETECT,
+ SENSOR_TYPE_MOTION_DETECT,
+ SENSOR_TYPE_HEART_BEAT,
+ SENSOR_TYPE_DYNAMIC_SENSOR_META,
+ SENSOR_TYPE_ADDITIONAL_INFO,
+ SENSOR_TYPE_LOW_LATENCY_OFFBODY_DETECT,
+ SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED,
+
+ /* follow mtk add sensor type */
+ SENSOR_TYPE_PEDOMETER = 55,
+ SENSOR_TYPE_IN_POCKET,
+ SENSOR_TYPE_ACTIVITY,
+ SENSOR_TYPE_PDR,
+ SENSOR_TYPE_FREEFALL,
+ SENSOR_TYPE_FLAT,
+ SENSOR_TYPE_FACE_DOWN,
+ SENSOR_TYPE_SHAKE,
+ SENSOR_TYPE_BRINGTOSEE,
+ SENSOR_TYPE_ANSWER_CALL,
+ SENSOR_TYPE_GEOFENCE,
+ SENSOR_TYPE_FLOOR_COUNTER,
+ SENSOR_TYPE_EKG,
+ SENSOR_TYPE_PPG1,
+ SENSOR_TYPE_PPG2,
+ SENSOR_TYPE_RGBW,
+ SENSOR_TYPE_GYRO_TEMPERATURE,
+ SENSOR_TYPE_SAR,
+ SENSOR_TYPE_GYRO_SECONDARY,
+ SENSOR_TYPE_SENSOR_MAX,
+};
+
+enum {
+ ID_OFFSET = 1,
+
+ /* follow google default sensor type */
+ ID_ACCELEROMETER = 0,
+ ID_MAGNETIC_FIELD,
+ ID_ORIENTATION,
+ ID_GYROSCOPE,
+ ID_LIGHT,
+ ID_PRESSURE,
+ ID_TEMPERATURE,
+ ID_PROXIMITY,
+ ID_GRAVITY,
+ ID_LINEAR_ACCELERATION,
+ ID_ROTATION_VECTOR,
+ ID_RELATIVE_HUMIDITY,
+ ID_AMBIENT_TEMPERATURE,
+ ID_MAGNETIC_FIELD_UNCALIBRATED,
+ ID_GAME_ROTATION_VECTOR,
+ ID_GYROSCOPE_UNCALIBRATED,
+ ID_SIGNIFICANT_MOTION,
+ ID_STEP_DETECTOR,
+ ID_STEP_COUNTER,
+ ID_GEOMAGNETIC_ROTATION_VECTOR,
+ ID_HEART_RATE,
+ ID_TILT_DETECTOR,
+ ID_WAKE_GESTURE,
+ ID_GLANCE_GESTURE,
+ ID_PICK_UP_GESTURE,
+ ID_WRIST_TILT_GESTURE,
+ ID_DEVICE_ORIENTATION,
+ ID_POSE_6DOF,
+ ID_STATIONARY_DETECT,
+ ID_MOTION_DETECT,
+ ID_HEART_BEAT,
+ ID_DYNAMIC_SENSOR_META,
+ ID_ADDITIONAL_INFO,
+ ID_LOW_LATENCY_OFFBODY_DETECT,
+ ID_ACCELEROMETER_UNCALIBRATED,
+
+ /* follow mtk add sensor type */
+ ID_PEDOMETER = SENSOR_TYPE_PEDOMETER - ID_OFFSET,
+ ID_IN_POCKET,
+ ID_ACTIVITY,
+ ID_PDR,
+ ID_FREEFALL,
+ ID_FLAT,
+ ID_FACE_DOWN,
+ ID_SHAKE,
+ ID_BRINGTOSEE,
+ ID_ANSWER_CALL,
+ ID_GEOFENCE,
+ ID_FLOOR_COUNTER,
+ ID_EKG,
+ ID_PPG1,
+ ID_PPG2,
+ ID_RGBW,
+ ID_GYRO_TEMPERATURE,
+ ID_SAR,
+ ID_GYRO_SECONDARY,
+ ID_SENSOR_MAX,
+};
+
+enum {
+ SENSOR_ACCURANCY_UNRELIALE,
+ SENSOR_ACCURANCY_LOW,
+ SENSOR_ACCURANCY_MEDIUM,
+ SENSOR_ACCURANCY_HIGH,
+};
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/sensor_list.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/sensor_list.c
new file mode 100644
index 0000000..399fcc2
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/sensor_list.c
@@ -0,0 +1,235 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "<sensorlist> " fmt
+
+#include <linux/module.h>
+#include <linux/miscdevice.h>
+#include <linux/types.h>
+#include <linux/spinlock.h>
+#include <linux/uaccess.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+
+#include "sensor_list.h"
+#include "hf_sensor_type.h"
+
+enum sensorlist {
+ accel,
+ gyro,
+ mag,
+ als,
+ ps,
+ baro,
+ sar,
+ maxhandle,
+};
+
+static struct sensorlist_info_t sensorlist_info[maxhandle];
+static struct mag_libinfo_t mag_libinfo;
+static DEFINE_SPINLOCK(sensorlist_info_lock);
+
+int sensorlist_find_sensor(int sensor)
+{
+ int handle = -1;
+
+ switch (sensor) {
+ case SENSOR_TYPE_ACCELEROMETER:
+ handle = accel;
+ break;
+ case SENSOR_TYPE_GYROSCOPE:
+ handle = gyro;
+ break;
+ case SENSOR_TYPE_MAGNETIC_FIELD:
+ handle = mag;
+ break;
+ case SENSOR_TYPE_LIGHT:
+ handle = als;
+ break;
+ case SENSOR_TYPE_PROXIMITY:
+ handle = ps;
+ break;
+ case SENSOR_TYPE_PRESSURE:
+ handle = baro;
+ break;
+ case SENSOR_TYPE_SAR:
+ handle = sar;
+ break;
+ }
+ return handle;
+}
+
+int sensorlist_find_type(int handle)
+{
+ int type = -1;
+
+ switch (handle) {
+ case accel:
+ type = SENSOR_TYPE_ACCELEROMETER;
+ break;
+ case gyro:
+ type = SENSOR_TYPE_GYROSCOPE;
+ break;
+ case mag:
+ type = SENSOR_TYPE_MAGNETIC_FIELD;
+ break;
+ case als:
+ type = SENSOR_TYPE_LIGHT;
+ break;
+ case ps:
+ type = SENSOR_TYPE_PROXIMITY;
+ break;
+ case baro:
+ type = SENSOR_TYPE_PRESSURE;
+ break;
+ case sar:
+ type = SENSOR_TYPE_SAR;
+ break;
+ }
+ return type;
+}
+
+static void init_sensorlist_info(void)
+{
+ int handle = -1;
+
+ spin_lock(&sensorlist_info_lock);
+ for (handle = accel; handle < maxhandle; ++handle)
+ strlcpy(sensorlist_info[handle].name, "NULL",
+ sizeof(sensorlist_info[handle].name));
+ spin_unlock(&sensorlist_info_lock);
+}
+
+int sensorlist_register_devinfo(int sensor,
+ struct sensorlist_info_t *devinfo)
+{
+ int handle = -1;
+
+ handle = sensorlist_find_sensor(sensor);
+ if (handle < 0)
+ return -1;
+ pr_notice("name(%s) type(%d) registered\n", devinfo->name, sensor);
+ spin_lock(&sensorlist_info_lock);
+ strlcpy(sensorlist_info[handle].name, devinfo->name,
+ sizeof(sensorlist_info[handle].name));
+ spin_unlock(&sensorlist_info_lock);
+ return 0;
+}
+
+int sensorlist_register_maginfo(struct mag_libinfo_t *maginfo)
+{
+ spin_lock(&sensorlist_info_lock);
+ memcpy(&mag_libinfo, maginfo, sizeof(struct mag_libinfo_t));
+ spin_unlock(&sensorlist_info_lock);
+ return 0;
+}
+
+static int sensorlist_open(struct inode *inode, struct file *file)
+{
+ return nonseekable_open(inode, file);
+}
+
+static ssize_t
+sensorlist_read(struct file *file, char __user *buf,
+ size_t count, loff_t *ptr)
+{
+ struct sensorlist_info_t temp[maxhandle];
+
+ if (count == 0)
+ return -EINVAL;
+ if (count < sizeof(struct sensorlist_info_t))
+ return -EINVAL;
+ if (count > maxhandle * sizeof(struct sensorlist_info_t))
+ count = maxhandle * sizeof(struct sensorlist_info_t);
+
+ memset(temp, 0, sizeof(temp));
+ spin_lock(&sensorlist_info_lock);
+ memcpy(temp, sensorlist_info, sizeof(temp));
+ spin_unlock(&sensorlist_info_lock);
+ if (copy_to_user(buf, temp, count))
+ return -EFAULT;
+ return count;
+}
+
+static long sensorlist_ioctl(struct file *filp,
+ unsigned int cmd, unsigned long arg)
+{
+ unsigned int size = _IOC_SIZE(cmd);
+ void __user *ubuf = (void __user *)arg;
+ struct mag_libinfo_t temp;
+
+ if (size != sizeof(struct mag_libinfo_t))
+ return -EINVAL;
+
+ switch (cmd) {
+ case SENSOR_LIST_GET_MAG_LIB_INFO:
+ memset(&temp, 0, sizeof(struct mag_libinfo_t));
+ spin_lock(&sensorlist_info_lock);
+ memcpy(&temp, &mag_libinfo, sizeof(struct mag_libinfo_t));
+ spin_unlock(&sensorlist_info_lock);
+ if (copy_to_user(ubuf, &temp, sizeof(struct mag_libinfo_t)))
+ return -EFAULT;
+ break;
+ }
+ return 0;
+}
+
+static const struct file_operations sensorlist_fops = {
+ .owner = THIS_MODULE,
+ .open = sensorlist_open,
+ .read = sensorlist_read,
+ .unlocked_ioctl = sensorlist_ioctl,
+ .compat_ioctl = sensorlist_ioctl,
+};
+
+static struct miscdevice sensorlist_miscdev = {
+ .minor = MISC_DYNAMIC_MINOR,
+ .name = "sensorlist",
+ .fops = &sensorlist_fops,
+};
+
+static int sensor_list_proc_show(struct seq_file *m, void *v)
+{
+ int handle = -1, type = -1;
+
+ seq_puts(m, "dynamic hardware sensorlist:\n");
+ spin_lock(&sensorlist_info_lock);
+ for (handle = accel; handle < maxhandle; ++handle) {
+ type = sensorlist_find_type(handle);
+ if (type < 0)
+ continue;
+ seq_printf(m, "sensortype:%d chipname:%s\n",
+ type, sensorlist_info[handle].name);
+ }
+ spin_unlock(&sensorlist_info_lock);
+ return 0;
+}
+
+static int sensor_list_proc_open(struct inode *inode,
+ struct file *filp)
+{
+ return single_open(filp, sensor_list_proc_show, NULL);
+}
+
+static const struct file_operations sensor_list_proc_fops = {
+ .open = sensor_list_proc_open,
+ .release = single_release,
+ .read = seq_read,
+ .llseek = seq_lseek,
+};
+
+static int __init sensorlist_init(void)
+{
+ init_sensorlist_info();
+ if (misc_register(&sensorlist_miscdev) < 0)
+ return -1;
+ proc_create("sensorlist", 0644, NULL, &sensor_list_proc_fops);
+ return 0;
+}
+
+subsys_initcall(sensorlist_init);
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("dynamic sensorlist driver");
+MODULE_AUTHOR("hongxu.zhao@mediatek.com");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/sensor_list.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/sensor_list.h
new file mode 100644
index 0000000..d32cead
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/core/sensor_list.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef _SENSOR_LIST_H_
+#define _SENSOR_LIST_H_
+
+#include <linux/ioctl.h>
+
+struct sensorlist_info_t {
+ char name[16];
+};
+
+struct mag_libinfo_t {
+ char libname[64];
+ int32_t layout;
+ int32_t deviceid;
+};
+
+int sensorlist_register_maginfo(struct mag_libinfo_t *mag_info);
+int sensorlist_register_devinfo(int sensor,
+ struct sensorlist_info_t *devinfo);
+int sensorlist_find_sensor(int sensor);
+
+#define SENSOR_LIST_GET_MAG_LIB_INFO _IOWR('a', 1, struct mag_libinfo_t)
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/Kconfig b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/Kconfig
new file mode 100644
index 0000000..c4935d9
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/Kconfig
@@ -0,0 +1,9 @@
+config MTK_KXTJ3_SUPPORT
+ bool " MTK_KXTJ3_SUPPORT for MediaTek package"
+ default n
+ help
+ Enable MTK KXTJ3 support.
+ KXTJ3 is a low-power and low-noise
+ barometric pressure sensor.
+ If in doubt, say N here.
+
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/Makefile
new file mode 100644
index 0000000..f3bf3e8
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/Makefile
@@ -0,0 +1,3 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core/
+
+obj-y += kxtj3_acc_i2c.o
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/kxtj3_acc_i2c.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/kxtj3_acc_i2c.c
new file mode 100644
index 0000000..91f143d
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/kxtj3_acc_i2c.c
@@ -0,0 +1,563 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/atomic.h>
+
+#include "hf_manager.h"
+#include "kxtj3_acc_i2c.h"
+#include "sensor_list.h"
+
+#define KXTJ3_ACC_I2C_NAME "kxtj3_acc_i2c"
+#define KXTJ3_ACC_AXES_NUM 3
+#define C_I2C_FIFO_SIZE 8
+
+//#define GRAVITY_EARTH_SCALAR 9.807f
+//#define ACCELEROMETER_INCREASE_NUM_AP 1000
+
+#define KXTJ3_ACC_SOFT_RESET_VALUE 0x80
+#define KXTJ3_ACC_CHIP_ID_VALUE (0x35)
+#define CHECK_CHIP_ID_TIME_MAX 5
+
+
+
+#define KXTJ3_ACC_ODR_7HZ 0x0b
+#define KXTJ3_ACC_ODR_12HZ 0x00
+#define KXTJ3_ACC_ODR_25HZ 0x01
+#define KXTJ3_ACC_ODR_50HZ 0x02
+#define KXTJ3_ACC_ODR_100HZ 0x03
+
+#define KXTJ3_ACC_ODR_200HZ 0x04
+#define KXTJ3_ACC_ODR_400HZ 0x05
+#define KXTJ3_ACC_ODR_800HZ 0x06
+#define KXTJ3_ACC_ODR_1600HZ 0x0F
+
+
+#define KXTJ3_ACC_RANGE_2G 0x40
+#define KXTJ3_ACC_RANGE_4G 0x48
+#define KXTJ3_ACC_RANGE_8G_1 0x50
+#define KXTJ3_ACC_RANGE_8G_2 0x58
+#define KXTJ3_ACC_RANGE_16G_1 0x44
+#define KXTJ3_ACC_RANGE_16G_2 0x4C
+#define KXTJ3_ACC_RANGE_16G_3 0x54
+#define KXTJ3_ACC_RANGE_16G_4 0x5C
+
+
+
+
+
+//static struct sensor_info support_sensors[] = {
+// {
+// .sensor_type = SENSOR_TYPE_ACCELEROMETER,
+// .gain = 100,
+// },
+//};
+static unsigned char support_sensors[] = {
+ SENSOR_TYPE_ACCELEROMETER,
+// SENSOR_TYPE_GYROSCOPE,
+// SENSOR_TYPE_GYRO_TEMPERATURE,
+};
+struct kxtj3_acc_device {
+ struct hf_device hf_dev;
+ struct i2c_client *acc_client;
+ uint32_t direction;
+ uint8_t placement[3];
+ atomic_t raw_enable;
+ atomic_t acc_enable;
+};
+
+/* I2C operation functions */
+static int kxtj3_acc_i2c_read_block(struct i2c_client *client,
+ u8 addr, u8 *data, u8 len)
+{
+ int err = 0;
+ u8 beg = addr;
+ struct i2c_msg msgs[2] = {
+ {/*.addr = client->addr,*/
+ .flags = 0,
+ .len = 1,
+ .buf = &beg},
+ {
+ /*.addr = client->addr*/
+ .flags = I2C_M_RD,
+ .len = len,
+ .buf = data,
+ } };
+ if (!client)
+ return -EINVAL;
+ msgs[0].addr = client->addr;
+ msgs[1].addr = client->addr;
+pr_info("kxtj3_acc_i2c_read_block.\n");
+ err = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+ if (err != 2) {
+ pr_err_ratelimited("i2c_trans err: %x %x (%d %p %d) %d\n",
+ msgs[0].addr, client->addr, addr, data, len, err);
+ err = -EIO;
+ } else {
+ err = 0; /*no error*/
+ }
+ return err;
+}
+
+static int kxtj3_acc_i2c_write_block(struct i2c_client *client,
+ u8 addr, u8 *data, u8 len)
+{
+ /* because address also occupies one byte,
+ * the maximum length for write is 7 bytes
+ */
+ int err = 0, idx = 0, num = 0;
+ char buf[32];
+ pr_info("kxtj3_acc_i2c_write_block.\n");
+ if (!client){
+ return -EINVAL;
+ }
+ else if (len > C_I2C_FIFO_SIZE) {
+ pr_err_ratelimited("len %d fi %d\n", len, C_I2C_FIFO_SIZE);
+ return -EINVAL;
+ }
+ buf[num++] = addr;
+ for (idx = 0; idx < len; idx++)
+ buf[num++] = data[idx];
+
+ err = i2c_master_send(client, buf, num);
+ if (err < 0) {
+ pr_err_ratelimited("send command error!!\n");
+ return -EFAULT;
+ }
+
+ return err;
+}
+
+
+static int kxtj3_acc_init_device(struct i2c_client *client)
+{
+ int err = 0;
+ uint8_t data = 0;
+pr_info("kxtj3_acc_init_device.\n");
+ data = KXTJ3_ACC_ODR_100HZ;
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_BW_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+#if 0
+ data = 0x00; //filter and lock enable
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_RATED_HBW_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+#endif
+ data = KXTJ3_ACC_RANGE_4G;
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_RANGE_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //disable int
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_INT_ENABLE1_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+#if 0
+ data = 0x80; //map int
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_INT_MAP_1_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //push-pull, active low
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_INT_OUT_CTRL_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+#endif
+ pr_info("KXTJ3_ACC init OK.\n");
+
+ return err;
+}
+
+static int kxtj3_acc_set_soft_reset(struct i2c_client *client)
+{
+ int err = 0;
+/* uint8_t data = KXTJ3_ACC_SOFT_RESET_VALUE;
+pr_info("kxtj3_acc_set_soft_reset.\n");
+ err = kxtj3_acc_i2c_write_block(client,
+ KXTJ3_ACC_BGW_SOFTRESET_ADDR, &data, 1);*/
+ return err;
+}
+
+static int kxtj3_acc_check_chip_id(struct i2c_client *client)
+{
+ int err = -1;
+ u8 chip_id = 0;
+ u8 read_count = 0;
+pr_info("kxtj3_acc_check_chip_id\n");
+ while (read_count++ < CHECK_CHIP_ID_TIME_MAX) {
+ kxtj3_acc_i2c_read_block(client,
+ KXTJ3_ACC_CHIP_ID_ADDR, &chip_id, 1);
+
+ if ((chip_id & 0xff) != KXTJ3_ACC_CHIP_ID_VALUE) {
+ continue;
+ } else {
+ err = 0;
+ pr_info("kxtj3_acc_check_chip_id err = 0\n");
+ break;
+ }
+ }
+ return err;
+}
+
+
+
+
+
+
+
+
+
+int kxtj3_acc_sample(struct hf_device *hfdev)
+{
+
+ int err = 0;
+ uint8_t buf[KXTJ3_ACC_AXES_NUM * 2] = {0};
+ int32_t data[KXTJ3_ACC_AXES_NUM] = {0};
+ struct hf_manager_event event;
+ int64_t current_time;
+// struct i2c_client *client = hf_device_get_private_data(hfdev);
+ struct kxtj3_acc_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+pr_info("kxtj3_acc_sample\n");
+ current_time = ktime_get_boot_ns();
+ if (atomic_read(&driver_dev->acc_enable)) {
+ err = kxtj3_acc_i2c_read_block(driver_dev->acc_client,
+ KXTJ3_ACC_RATE_X_LSB_ADDR,
+ &buf[0], KXTJ3_ACC_AXES_NUM * 2);
+ if (err < 0) {
+ pr_err_ratelimited("read fail\n");
+ return err;
+ }
+
+ data[0] = ((int16_t)(buf[0] | (buf[1] << 8))) >> 4;
+ data[1] = ((int16_t)(buf[2] | (buf[3] << 8))) >> 4;
+ data[2] = ((int16_t)(buf[4] | (buf[5] << 8))) >> 4;
+
+ pr_err("kxtj3 register :%x, %x, %x %x, %x, %x data: %x, %x, %x\n",
+ buf[0], buf[1], buf[2],
+ buf[3], buf[4], buf[5],
+ data[0], data[1], data[2]);
+
+ coordinate_map(driver_dev->direction, data);
+ current_time = ktime_get_boot_ns();
+
+// if (atomic_read(&driver_dev->raw_enable)) {
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ACCELEROMETER;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = RAW_ACTION;
+ event.word[0] =
+ (int32_t)div64_s64((int64_t)data[0] * 9807, 256);
+ event.word[1] =
+ (int32_t)div64_s64((int64_t)data[1] * 9807, 256);
+ event.word[2] =
+ (int32_t)div64_s64((int64_t)data[2] * 9807, 256);
+ /* pr_err("kxtj3 raw %d, %d, %d, m/s %d, %d, %d\n",
+ * data[0], data[1], data[2],
+ * event.word[0], event.word[1], event.word[2]);
+ */
+ manager->report(manager, &event);
+ }
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ACCELEROMETER;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = DATA_ACTION;
+
+ event.word[0] = (int32_t)div64_s64((int64_t)data[0] * 1000, 512);
+ event.word[1] = (int32_t)div64_s64((int64_t)data[1] * 1000, 512);
+ event.word[2] = (int32_t)div64_s64((int64_t)data[2] * 1000, 512);
+
+ /* pr_err("kxtj3 raw %d, %d, %d, m/s %d, %d, %d\n",
+ * data[0], data[1], data[2],
+ * event.word[0], event.word[1], event.word[2]);
+ */
+
+ manager->report(manager, &event);
+
+ manager->complete(manager);
+
+ return 0;
+}
+
+int kxtj3_acc_raw_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ struct kxtj3_acc_device *driver_dev = hf_device_get_private_data(hfdev);
+// struct kxtj3_acc_device *driver_dev = i2c_get_clientdata(client);
+pr_info("kxtj3_acc_raw_enable\n");
+ atomic_set(&driver_dev->raw_enable, en);
+ return 0;
+}
+
+int kxtj3_acc_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ int err = 0;
+ uint8_t data = 0;
+ struct kxtj3_acc_device *driver_dev = hf_device_get_private_data(hfdev);
+
+ pr_info("%s id:%d en:%d\n", __func__, sensor_type, en);
+
+ if (en) {
+
+ kxtj3_acc_i2c_read_block(driver_dev->acc_client,KXTJ3_ACC_RANGE_ADDR, &data, 1);
+ data |= 1<<7;
+ err = kxtj3_acc_i2c_write_block(driver_dev->acc_client, KXTJ3_ACC_RANGE_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write power mode failed.\n");
+ return -1;
+ }
+ mdelay(1);
+ atomic_set(&driver_dev->acc_enable, en);
+ } else {
+ data = 0x00;
+ err = kxtj3_acc_i2c_write_block(driver_dev->acc_client,KXTJ3_ACC_RANGE_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write power mode failed.\n");
+ return -1;
+ }
+ atomic_set(&driver_dev->acc_enable, en);
+ mdelay(1);
+ }
+ return 0;
+
+}
+
+int kxtj3_acc_batch(struct hf_device *hfdev, int sensor_type,
+ int64_t delay, int64_t latency)
+{
+
+ int err = 0;
+ uint8_t data = 0;
+ int value = 0;
+ struct kxtj3_acc_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+
+ pr_info("%s id:%d rate:%lld latency:%lld\n",
+ __func__, sensor_type, delay, latency);
+
+ value = div64_s64(1000000000LL, delay);
+
+ if (value <= 7)
+ data = KXTJ3_ACC_ODR_7HZ;
+ else if (value <= 12)
+ data = KXTJ3_ACC_ODR_12HZ;
+ else if (value <= 25)
+ data = KXTJ3_ACC_ODR_25HZ;
+ else if (value <= 100)
+ data = KXTJ3_ACC_ODR_100HZ;
+ else if (value <= 200)
+ data = KXTJ3_ACC_ODR_200HZ;
+ else if (value <= 400)
+ data = KXTJ3_ACC_ODR_400HZ;
+ else if (value <= 800)
+ data = KXTJ3_ACC_ODR_800HZ;
+ else
+ data = KXTJ3_ACC_ODR_1600HZ;
+
+ err = kxtj3_acc_i2c_write_block(driver_dev->acc_client, KXTJ3_ACC_BW_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write rate failed.\n");
+ return -1;
+ }
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ADDITIONAL_INFO;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.reserved = SENSOR_TYPE_ACCELEROMETER;
+ event.action = DATA_ACTION;
+ get_placement_info(driver_dev->direction, event.byte);
+ event.byte[12] = 1;
+ event.byte[3] = driver_dev->placement[0];
+ event.byte[7] = driver_dev->placement[1];
+ event.byte[11] = driver_dev->placement[2];
+
+ manager->report(manager, &event);
+ mdelay(1);
+ manager->complete(manager);
+ return 0;
+
+}
+
+static int kxtj3_acc_flush(struct hf_device *hfdev, int sensor_type)
+{
+ struct kxtj3_acc_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.sensor_type = sensor_type;
+ event.timestamp = current_time;
+ event.action = FLUSH_ACTION;
+
+ manager->report(manager, &event);
+ manager->complete(manager);
+
+ return 0;
+}
+
+static int kxtj3_acc_i2c_remove(struct i2c_client *client)
+{
+ struct kxtj3_acc_device *driver_dev = i2c_get_clientdata(client);
+pr_info("kxtj3_acc_i2c_remove\n");
+ hf_manager_destroy(driver_dev->hf_dev.manager);
+ kfree(driver_dev);
+ return 0;
+}
+
+static int kxtj3_acc_i2c_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+
+ int err = 0;
+ struct kxtj3_acc_device *driver_dev;
+ struct sensorlist_info_t listinfo;
+
+ pr_info("%s\n", __func__);
+
+ driver_dev = devm_kzalloc(&client->dev, sizeof(*driver_dev),
+ GFP_KERNEL);
+ if (!driver_dev)
+ return -ENOMEM;
+
+// driver_dev->acc_client = client;
+ /* check chip id */
+ err = kxtj3_acc_check_chip_id(client);
+ if (err < 0) {
+ pr_err("Bosch Sensortec Device not found, chip id mismatch\n");
+ err = -EINVAL;
+ goto init_fail;
+ }
+
+ err = kxtj3_acc_set_soft_reset(client);
+ if (err < 0) {
+ pr_err("erro soft reset!\n");
+ err = -EINVAL;
+ goto init_fail;
+ }
+ mdelay(3);
+
+ err = kxtj3_acc_init_device(client);
+ if (err < 0) {
+ pr_err("%s init device fail\n", __func__);
+ goto init_fail;
+ }
+
+ driver_dev = kzalloc(sizeof(*driver_dev), GFP_KERNEL);
+ if (!driver_dev) {
+ err = -ENOMEM;
+ goto malloc_fail;
+ }
+
+ driver_dev->acc_client = client;
+
+ if (of_property_read_u32(client->dev.of_node,
+ "direction", &driver_dev->direction)) {
+ pr_err("%s get direction dts fail\n", __func__);
+ err = -EFAULT;
+ goto dts_fail;
+ }
+if(0){
+ if (of_property_read_u8_array(client->dev.of_node, "placement",
+ driver_dev->placement, ARRAY_SIZE(driver_dev->placement))) {
+ pr_err("%s get placement dts fail\n", __func__);
+ err = -EFAULT;
+ goto dts_fail;
+ }
+}
+ atomic_set(&driver_dev->raw_enable, 0);
+
+ driver_dev->hf_dev.dev_name = KXTJ3_ACC_I2C_NAME;
+ driver_dev->hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ driver_dev->hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ driver_dev->hf_dev.support_list = support_sensors;
+ driver_dev->hf_dev.support_size = ARRAY_SIZE(support_sensors);
+ driver_dev->hf_dev.enable = kxtj3_acc_enable;
+ driver_dev->hf_dev.batch = kxtj3_acc_batch;
+ driver_dev->hf_dev.flush = kxtj3_acc_flush;
+ driver_dev->hf_dev.sample = kxtj3_acc_sample;
+ driver_dev->hf_dev.rawdata = kxtj3_acc_raw_enable;
+
+ err = hf_manager_create(&driver_dev->hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ err = -1;
+ goto create_manager_fail;
+ }
+
+ i2c_set_clientdata(client, driver_dev);
+ hf_device_set_private_data(&driver_dev->hf_dev, driver_dev);
+
+ //memset(&listinfo, 0, sizeof(struct sensorlist_info_t));
+ //strlcpy(listinfo.name, KXTJ3_ACC_I2C_NAME, sizeof(listinfo.name));
+ //sensorlist_register_devinfo(SENSOR_TYPE_ACCELEROMETER, &listinfo);
+ memset(&listinfo, 0, sizeof(struct sensorlist_info_t));
+ strlcpy(listinfo.name, KXTJ3_ACC_I2C_NAME, sizeof(listinfo.name));
+ sensorlist_register_devinfo(SENSOR_TYPE_ACCELEROMETER, &listinfo);
+
+ pr_info("%s success!\n", __func__);
+ return 0;
+
+create_manager_fail:
+dts_fail:
+ kfree(driver_dev);
+malloc_fail:
+init_fail:
+ pr_err("%s fail!\n", __func__);
+ return err;
+}
+
+static const struct of_device_id kxtj3_acc_of_match[] = {
+ {.compatible = "mediatek,kxtj3_gsensor"},
+ {},
+};
+static const struct i2c_device_id kxtj3_acc_i2c_id[] = {
+ {KXTJ3_ACC_I2C_NAME, 0}, {} };
+
+static struct i2c_driver kxtj3_acc_i2c_driver = {
+ .driver = {
+ .name = KXTJ3_ACC_I2C_NAME,
+ .bus = &i2c_bus_type,
+ .owner = THIS_MODULE,
+ .of_match_table = kxtj3_acc_of_match,
+ },
+ .probe = kxtj3_acc_i2c_probe,
+ .remove = kxtj3_acc_i2c_remove,
+ .id_table = kxtj3_acc_i2c_id,
+};
+
+module_i2c_driver(kxtj3_acc_i2c_driver);
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("kxtj3 acc i2c driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/kxtj3_acc_i2c.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/kxtj3_acc_i2c.h
new file mode 100644
index 0000000..5ea7661
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/kxtj3/kxtj3_acc_i2c.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2016 MediaTek Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See http://www.gnu.org/licenses/gpl-2.0.html for more details.
+ */
+
+#ifndef __KXTJ3_ACC_H__
+#define __KXTJ3_ACC_H__
+
+ /*Define of registers*/
+
+ /* Hard Wired */
+#define KXTJ3_ACC_CHIP_ID_ADDR 0x0f
+ /**<Address of Chip ID Register*/
+
+ /* Data Register */
+#define KXTJ3_ACC_RATE_X_LSB_ADDR 0x06
+ /**< Address of X axis Rate LSB Register */
+#define KXTJ3_ACC_RATE_X_MSB_ADDR 0x07
+ /**< Address of X axis Rate MSB Register */
+#define KXTJ3_ACC_RATE_Y_LSB_ADDR 0x08
+ /**< Address of Y axis Rate LSB Register */
+#define KXTJ3_ACC_RATE_Y_MSB_ADDR 0x09
+ /**< Address of Y axis Rate MSB Register */
+#define KXTJ3_ACC_RATE_Z_LSB_ADDR 0x0a
+ /**< Address of Z axis Rate LSB Register */
+#define KXTJ3_ACC_RATE_Z_MSB_ADDR 0x0b
+ /**< Address of Z axis Rate MSB Register */
+//#define KXTJ3_ACC_TEMP_ADDR 0x08
+ /**< Address of Temperature Data LSB Register */
+
+//#define KXTJ3_ACC_INT_STATUS1_ADDR 0x0A
+ /**< Address of Interrupt status Register 1*/
+
+ /* Control Register */
+#define KXTJ3_ACC_RANGE_ADDR 0x1b
+ /**< Address of Range address Register */
+#define KXTJ3_ACC_BW_ADDR 0x21
+ /**< Address of Bandwidth Register */
+//#define KXTJ3_ACC_RATED_HBW_ADDR 0x13
+ /**< Address of Rate HBW Register */
+#define KXTJ3_ACC_BGW_SOFTRESET_ADDR 0x1b
+ /**< Address of BGW Softreset Register */
+
+#define KXTJ3_ACC_INT_ENABLE1_ADDR 0x1e
+ /**< Address of Interrupt Enable 1 */
+
+//#define KXTJ3_ACC_INT_MAP_1_ADDR 0x1A
+ /**< Address of Interrupt MAP 1 */
+
+//#define KXTJ3_ACC_INT_SRC_ADDR 0x1E
+ /**< Address of Interrupt SRC 1 */
+
+//#define KXTJ3_ACC_INT_OUT_CTRL_ADDR 0x20
+ /**< Address of Interrupt MAP 1 */
+
+//#define KXTJ3_ACC_BGW_SPI3_WDT_ADDR 0x34
+ /**< Address of BGW SPI3,WDT Register */
+
+//#define KXTJ3_ACC_SELF_TEST_ADDR 0x32
+ /**< Address of BGW Self test Register */
+
+#define KXTJ3_ACC_DATA_INT_EN 0x28
+
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Kconfig b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Kconfig
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Kconfig
@@ -0,0 +1 @@
+
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Makefile
new file mode 100644
index 0000000..d32fe0e
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Makefile
@@ -0,0 +1,5 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+ccflags-y += -I$(srctree)/drivers/staging/nanohub
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/scp/$(CONFIG_MTK_PLATFORM)
+
+obj-y += mtk_nanohub.o mtk_nanohub_ipi.o
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.c
new file mode 100644
index 0000000..201de38
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.c
@@ -0,0 +1,2336 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "[mtk_nanohub] " fmt
+
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/kthread.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/atomic.h>
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/suspend.h>
+#include <linux/time.h>
+#include <asm/arch_timer.h>
+#include <linux/math64.h>
+#include <linux/delay.h>
+
+#include "scp_ipi.h"
+#include "scp_helper.h"
+#include "scp_excep.h"
+#include "mtk_nanohub.h"
+#include "comms.h"
+#include "hf_manager.h"
+#include "sensor_list.h"
+#include "mtk_nanohub_ipi.h"
+
+/* ALGIN TO SCP SENSOR_IPI_SIZE AT FILE CONTEXTHUB_FW.H, ALGIN
+ * TO SCP_SENSOR_HUB_DATA UNION, ALGIN TO STRUCT DATA_UNIT_T
+ * SIZEOF(STRUCT DATA_UNIT_T) = SCP_SENSOR_HUB_DATA = SENSOR_IPI_SIZE
+ * BUT AT THE MOMENT AP GET DATA THROUGH IPI, WE ONLY TRANSFER
+ * 44 BYTES DATA_UNIT_T, THERE ARE 4 BYTES HEADER IN SCP_SENSOR_HUB_DATA
+ * HEAD
+ */
+#define SENSOR_IPI_SIZE 48
+/*
+ * experience number for delay_count per DELAY_COUNT sensor input delay 10ms
+ * msleep(10) system will schedule to hal process then read input node
+ */
+#define SENSOR_IPI_HEADER_SIZE 4
+#define SENSOR_IPI_PACKET_SIZE (SENSOR_IPI_SIZE - SENSOR_IPI_HEADER_SIZE)
+#define SENSOR_DATA_SIZE 44
+
+#if SENSOR_DATA_SIZE > SENSOR_IPI_PACKET_SIZE
+#error "SENSOR_DATA_SIZE > SENSOR_IPI_PACKET_SIZE, out of memory"
+#endif
+
+#define SYNC_TIME_CYCLC 10000
+#define SYNC_TIME_START_CYCLC 3000
+
+struct curr_wp_queue {
+ spinlock_t buffer_lock;
+ uint32_t head;
+ uint32_t tail;
+ uint32_t bufsize;
+ uint32_t *ringbuffer;
+};
+
+struct mtk_nanohub_device {
+ struct hf_device hf_dev;
+ struct timer_list sync_time_timer;
+ struct work_struct sync_time_worker;
+ struct wakeup_source time_sync_wakeup_src;
+ struct wakeup_source data_notify_wakeup_src;
+
+ struct sensor_fifo *scp_sensor_fifo;
+ struct curr_wp_queue wp_queue;
+ phys_addr_t shub_dram_phys;
+ phys_addr_t shub_dram_virt;
+ atomic_t traces[ID_SENSOR_MAX];
+
+ atomic_t get_list_first_boot;
+ atomic_t cfg_data_after_reboot;
+ atomic_t start_timesync_first_boot;
+
+ int32_t acc_config_data[6];
+ int32_t gyro_config_data[12];
+ int32_t mag_config_data[9];
+ int32_t light_config_data[1];
+ int32_t proximity_config_data[2];
+};
+
+static uint8_t rtc_compensation_suspend;
+static struct SensorState mSensorState[SENSOR_TYPE_SENSOR_MAX];
+static unsigned char support_sensors[SENSOR_TYPE_SENSOR_MAX];
+static DEFINE_MUTEX(mSensorState_mtx);
+static atomic_t power_status = ATOMIC_INIT(SENSOR_POWER_DOWN);
+static DECLARE_WAIT_QUEUE_HEAD(chre_kthread_wait);
+static DECLARE_WAIT_QUEUE_HEAD(power_reset_wait);
+static uint8_t chre_kthread_wait_condition;
+static DEFINE_SPINLOCK(scp_state_lock);
+static DEFINE_SPINLOCK(config_data_lock);
+static uint8_t scp_system_ready;
+static uint8_t scp_chre_ready;
+static struct mtk_nanohub_device *mtk_nanohub_dev;
+
+static int mtk_nanohub_send_timestamp_to_hub(void);
+static int mtk_nanohub_server_dispatch_data(uint32_t *currWp);
+static int mtk_nanohub_report_to_manager(struct data_unit_t *data);
+
+enum scp_ipi_status __attribute__((weak)) scp_ipi_registration(enum ipi_id id,
+ void (*ipi_handler)(int id, void *data, unsigned int len),
+ const char *name)
+{
+ return SCP_IPI_ERROR;
+}
+
+void __attribute__((weak)) scp_A_register_notify(struct notifier_block *nb)
+{
+
+}
+
+phys_addr_t __attribute__((weak))
+ scp_get_reserve_mem_virt(enum scp_reserve_mem_id_t id)
+{
+ return 0;
+}
+
+phys_addr_t __attribute__((weak))
+ scp_get_reserve_mem_phys(enum scp_reserve_mem_id_t id)
+{
+ return 0;
+}
+
+phys_addr_t __attribute__((weak))
+ scp_get_reserve_mem_size(enum scp_reserve_mem_id_t id)
+{
+ return 0;
+}
+
+void __attribute__((weak)) scp_register_feature(enum feature_id id)
+{
+}
+
+/* arch counter is 13M, mult is 161319385, shift is 21 */
+static inline uint64_t arch_counter_to_ns(uint64_t cyc)
+{
+#define ARCH_TIMER_MULT 161319385
+#define ARCH_TIMER_SHIFT 21
+ return (cyc * ARCH_TIMER_MULT) >> ARCH_TIMER_SHIFT;
+}
+
+#define FILTER_DATAPOINTS 16
+#define FILTER_TIMEOUT 10000000000ULL /* 10 seconds, ~100us drift */
+#define FILTER_FREQ 10000000ULL /* 10 ms */
+struct moving_average {
+ uint64_t last_time;
+ int64_t input[FILTER_DATAPOINTS];
+ atomic64_t output;
+ uint8_t cnt;
+ uint8_t tail;
+};
+static struct moving_average moving_average_algo;
+static uint8_t rtc_compensation_suspend;
+static void moving_average_filter(struct moving_average *filter,
+ uint64_t ap_time, uint64_t hub_time)
+{
+ int i = 0;
+ int64_t avg;
+ int64_t ret_avg = 0;
+
+ if (ap_time > filter->last_time + FILTER_TIMEOUT ||
+ filter->last_time == 0) {
+ filter->tail = 0;
+ filter->cnt = 0;
+ } else if (ap_time < filter->last_time + FILTER_FREQ) {
+ return;
+ }
+ filter->last_time = ap_time;
+
+ filter->input[filter->tail++] = ap_time - hub_time;
+ filter->tail &= (FILTER_DATAPOINTS - 1);
+ if (filter->cnt < FILTER_DATAPOINTS)
+ filter->cnt++;
+
+ /* pr_err("hongxu raw_offset=%lld\n", ap_time - hub_time); */
+
+ for (i = 1, avg = 0; i < filter->cnt; i++)
+ avg += (filter->input[i] - filter->input[0]);
+ ret_avg = div_s64(avg, filter->cnt) + filter->input[0];
+ atomic64_set(&filter->output, ret_avg);
+}
+
+static uint64_t get_filter_output(struct moving_average *filter)
+{
+ return atomic64_read(&filter->output);
+}
+
+struct mtk_nanohub_cmd {
+ uint32_t reason;
+ void (*handler)(union SCP_SENSOR_HUB_DATA *rsp, unsigned int rx_len);
+};
+
+#define MTK_NANOHUB_CMD(_reason, _handler) \
+ {.reason = _reason, .handler = _handler}
+
+#define type_to_id(type) (type - ID_OFFSET)
+#define id_to_type(id) (id + ID_OFFSET)
+
+int mtk_nanohub_req_send(union SCP_SENSOR_HUB_DATA *data)
+{
+ int ret = 0;
+
+ if (data->req.sensorType >= ID_SENSOR_MAX) {
+ pr_err("invalid sensor type %d\n", data->rsp.sensorType);
+ return -1;
+ }
+ ret = mtk_nanohub_ipi_sync((unsigned char *)data,
+ SENSOR_IPI_SIZE);
+ if (ret != 0 || data->rsp.errCode != 0)
+ return -1;
+ return 0;
+}
+
+static void mtk_nanohub_write_wp_queue(union SCP_SENSOR_HUB_DATA *rsp)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ struct curr_wp_queue *wp_queue = &device->wp_queue;
+
+ spin_lock(&wp_queue->buffer_lock);
+ wp_queue->ringbuffer[wp_queue->head++] = rsp->notify_rsp.currWp;
+ wp_queue->head &= wp_queue->bufsize - 1;
+ if (unlikely(wp_queue->head == wp_queue->tail))
+ pr_err("dropped currWp due to ringbuffer is full\n");
+ spin_unlock(&wp_queue->buffer_lock);
+}
+
+static int mtk_nanohub_fetch_next_wp(uint32_t *currWp)
+{
+ int have_event;
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ struct curr_wp_queue *wp_queue = &device->wp_queue;
+
+ spin_lock_irq(&wp_queue->buffer_lock);
+
+ have_event = wp_queue->head != wp_queue->tail;
+ if (have_event) {
+ *currWp = wp_queue->ringbuffer[wp_queue->tail++];
+ wp_queue->tail &= wp_queue->bufsize - 1;
+ }
+ spin_unlock_irq(&wp_queue->buffer_lock);
+ /* pr_err("head:%d, tail:%d, currWp:%d\n",
+ * wp_queue->head, wp_queue->tail, *currWp);
+ */
+ return have_event;
+}
+
+static int mtk_nanohub_read_wp_queue(void)
+{
+ uint32_t currWp = 0;
+
+ while (mtk_nanohub_fetch_next_wp(&currWp)) {
+ if (mtk_nanohub_server_dispatch_data(&currWp))
+ return -EFAULT;
+ }
+ return 0;
+}
+
+static void mtk_nanohub_sync_time_work(struct work_struct *work)
+
+{
+ mtk_nanohub_send_timestamp_to_hub();
+}
+
+static void mtk_nanohub_sync_time_func(unsigned long data)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+
+ schedule_work(&device->sync_time_worker);
+
+ mod_timer(&device->sync_time_timer,
+ jiffies + msecs_to_jiffies(SYNC_TIME_CYCLC));
+}
+
+static int mtk_nanohub_direct_push_work(void *data)
+{
+ for (;;) {
+ wait_event(chre_kthread_wait,
+ READ_ONCE(chre_kthread_wait_condition));
+ WRITE_ONCE(chre_kthread_wait_condition, false);
+ mtk_nanohub_read_wp_queue();
+ }
+ return 0;
+}
+
+static void mtk_nanohub_common_cmd(union SCP_SENSOR_HUB_DATA *rsp,
+ unsigned int rx_len)
+{
+ mtk_nanohub_ipi_complete((unsigned char *)rsp, rx_len);
+}
+
+static void mtk_nanohub_moving_average(union SCP_SENSOR_HUB_DATA *rsp)
+{
+ uint64_t ap_now_time = 0, arch_counter = 0;
+ uint64_t scp_raw_time = 0, scp_now_time = 0;
+ uint64_t ipi_transfer_time = 0;
+
+ if (!timekeeping_rtc_skipresume()) {
+ if (READ_ONCE(rtc_compensation_suspend))
+ return;
+ }
+ ap_now_time = ktime_get_boot_ns();
+ arch_counter = arch_counter_get_cntvct();
+ scp_raw_time = rsp->notify_rsp.scp_timestamp;
+ ipi_transfer_time = arch_counter_to_ns(arch_counter -
+ rsp->notify_rsp.arch_counter);
+ scp_now_time = scp_raw_time + ipi_transfer_time;
+ moving_average_filter(&moving_average_algo, ap_now_time, scp_now_time);
+}
+
+static void mtk_nanohub_notify_cmd(union SCP_SENSOR_HUB_DATA *rsp,
+ unsigned int rx_len)
+{
+ unsigned long flags = 0;
+
+ switch (rsp->notify_rsp.event) {
+ case SCP_DIRECT_PUSH:
+ case SCP_FIFO_FULL:
+ mtk_nanohub_moving_average(rsp);
+ mtk_nanohub_write_wp_queue(rsp);
+ WRITE_ONCE(chre_kthread_wait_condition, true);
+ wake_up(&chre_kthread_wait);
+ break;
+ case SCP_NOTIFY:
+ break;
+ case SCP_INIT_DONE:
+ spin_lock_irqsave(&scp_state_lock, flags);
+ WRITE_ONCE(scp_chre_ready, true);
+ if (READ_ONCE(scp_system_ready) && READ_ONCE(scp_chre_ready)) {
+ spin_unlock_irqrestore(&scp_state_lock, flags);
+ atomic_set(&power_status, SENSOR_POWER_UP);
+ //scp_power_monitor_notify(SENSOR_POWER_UP, NULL);
+ /* schedule_work(&device->power_up_work); */
+ wake_up(&power_reset_wait);
+ } else
+ spin_unlock_irqrestore(&scp_state_lock, flags);
+ break;
+ default:
+ break;
+ }
+}
+
+static const struct mtk_nanohub_cmd mtk_nanohub_cmds[] = {
+ MTK_NANOHUB_CMD(SENSOR_HUB_NOTIFY,
+ mtk_nanohub_notify_cmd),
+ MTK_NANOHUB_CMD(SENSOR_HUB_GET_DATA,
+ mtk_nanohub_common_cmd),
+ MTK_NANOHUB_CMD(SENSOR_HUB_SET_CONFIG,
+ mtk_nanohub_common_cmd),
+ MTK_NANOHUB_CMD(SENSOR_HUB_SET_CUST,
+ mtk_nanohub_common_cmd),
+ MTK_NANOHUB_CMD(SENSOR_HUB_SET_TIMESTAMP,
+ mtk_nanohub_common_cmd),
+ MTK_NANOHUB_CMD(SENSOR_HUB_RAW_DATA,
+ mtk_nanohub_common_cmd),
+};
+
+const struct mtk_nanohub_cmd *
+mtk_nanohub_find_cmd(uint32_t packetReason)
+{
+ int i;
+ const struct mtk_nanohub_cmd *cmd;
+
+ for (i = 0; i < ARRAY_SIZE(mtk_nanohub_cmds); i++) {
+ cmd = &mtk_nanohub_cmds[i];
+ if (cmd->reason == packetReason)
+ return cmd;
+ }
+ return NULL;
+}
+
+static void mtk_nanohub_ipi_handler(int id,
+ void *data, unsigned int len)
+{
+ union SCP_SENSOR_HUB_DATA *rsp = (union SCP_SENSOR_HUB_DATA *)data;
+ const struct mtk_nanohub_cmd *cmd;
+
+ if (len > SENSOR_IPI_SIZE) {
+ pr_err("%s len=%d error\n", __func__, len);
+ return;
+ }
+ /*pr_err("sensorType:%d, action=%d event:%d len:%d\n",
+ * rsp->rsp.sensorType, rsp->rsp.action, rsp->notify_rsp.event, len);
+ */
+ cmd = mtk_nanohub_find_cmd(rsp->rsp.action);
+ if (cmd != NULL)
+ cmd->handler(rsp, len);
+ else
+ pr_err("cannot find cmd!\n");
+}
+
+static void mtk_nanohub_init_sensor_state(void)
+{
+ mSensorState[SENSOR_TYPE_ACCELEROMETER].sensorType =
+ SENSOR_TYPE_ACCELEROMETER;
+ mSensorState[SENSOR_TYPE_ACCELEROMETER].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_GYROSCOPE].sensorType = SENSOR_TYPE_GYROSCOPE;
+ mSensorState[SENSOR_TYPE_GYROSCOPE].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_MAGNETIC_FIELD].sensorType =
+ SENSOR_TYPE_MAGNETIC_FIELD;
+ mSensorState[SENSOR_TYPE_MAGNETIC_FIELD].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_LIGHT].sensorType = SENSOR_TYPE_LIGHT;
+ mSensorState[SENSOR_TYPE_LIGHT].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_PROXIMITY].sensorType = SENSOR_TYPE_PROXIMITY;
+ mSensorState[SENSOR_TYPE_PROXIMITY].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_PRESSURE].sensorType = SENSOR_TYPE_PRESSURE;
+ mSensorState[SENSOR_TYPE_PRESSURE].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_ORIENTATION].sensorType =
+ SENSOR_TYPE_ORIENTATION;
+ mSensorState[SENSOR_TYPE_ORIENTATION].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_ROTATION_VECTOR].sensorType =
+ SENSOR_TYPE_ROTATION_VECTOR;
+ mSensorState[SENSOR_TYPE_ROTATION_VECTOR].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_GAME_ROTATION_VECTOR].sensorType =
+ SENSOR_TYPE_GAME_ROTATION_VECTOR;
+ mSensorState[SENSOR_TYPE_GAME_ROTATION_VECTOR].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR].sensorType =
+ SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR;
+ mSensorState[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR].timestamp_filter =
+ true;
+
+ mSensorState[SENSOR_TYPE_LINEAR_ACCELERATION].sensorType =
+ SENSOR_TYPE_LINEAR_ACCELERATION;
+ mSensorState[SENSOR_TYPE_LINEAR_ACCELERATION].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_GRAVITY].sensorType = SENSOR_TYPE_GRAVITY;
+ mSensorState[SENSOR_TYPE_GRAVITY].timestamp_filter = true;
+
+ mSensorState[SENSOR_TYPE_SIGNIFICANT_MOTION].sensorType =
+ SENSOR_TYPE_SIGNIFICANT_MOTION;
+ mSensorState[SENSOR_TYPE_SIGNIFICANT_MOTION].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_SIGNIFICANT_MOTION].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_STEP_COUNTER].sensorType =
+ SENSOR_TYPE_STEP_COUNTER;
+ mSensorState[SENSOR_TYPE_STEP_COUNTER].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_STEP_COUNTER].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_STEP_DETECTOR].sensorType =
+ SENSOR_TYPE_STEP_DETECTOR;
+ mSensorState[SENSOR_TYPE_STEP_DETECTOR].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_STEP_DETECTOR].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_TILT_DETECTOR].sensorType =
+ SENSOR_TYPE_TILT_DETECTOR;
+ mSensorState[SENSOR_TYPE_TILT_DETECTOR].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_TILT_DETECTOR].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_IN_POCKET].sensorType = SENSOR_TYPE_IN_POCKET;
+ mSensorState[SENSOR_TYPE_IN_POCKET].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_IN_POCKET].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_ACTIVITY].sensorType = SENSOR_TYPE_ACTIVITY;
+ mSensorState[SENSOR_TYPE_ACTIVITY].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_GLANCE_GESTURE].sensorType =
+ SENSOR_TYPE_GLANCE_GESTURE;
+ mSensorState[SENSOR_TYPE_GLANCE_GESTURE].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_GLANCE_GESTURE].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_PICK_UP_GESTURE].sensorType =
+ SENSOR_TYPE_PICK_UP_GESTURE;
+ mSensorState[SENSOR_TYPE_PICK_UP_GESTURE].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_PICK_UP_GESTURE].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_WAKE_GESTURE].sensorType =
+ SENSOR_TYPE_WAKE_GESTURE;
+ mSensorState[SENSOR_TYPE_WAKE_GESTURE].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_WAKE_GESTURE].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_ANSWER_CALL].sensorType =
+ SENSOR_TYPE_ANSWER_CALL;
+ mSensorState[SENSOR_TYPE_ANSWER_CALL].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_ANSWER_CALL].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_STATIONARY_DETECT].sensorType =
+ SENSOR_TYPE_STATIONARY_DETECT;
+ mSensorState[SENSOR_TYPE_STATIONARY_DETECT].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_STATIONARY_DETECT].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_MOTION_DETECT].sensorType =
+ SENSOR_TYPE_MOTION_DETECT;
+ mSensorState[SENSOR_TYPE_MOTION_DETECT].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_MOTION_DETECT].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_DEVICE_ORIENTATION].sensorType =
+ SENSOR_TYPE_DEVICE_ORIENTATION;
+ mSensorState[SENSOR_TYPE_DEVICE_ORIENTATION].rate =
+ SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_DEVICE_ORIENTATION].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_GEOFENCE].sensorType = SENSOR_TYPE_GEOFENCE;
+ mSensorState[SENSOR_TYPE_GEOFENCE].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_GEOFENCE].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_FLOOR_COUNTER].sensorType =
+ SENSOR_TYPE_FLOOR_COUNTER;
+ mSensorState[SENSOR_TYPE_FLOOR_COUNTER].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_FLOOR_COUNTER].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_FLAT].sensorType = SENSOR_TYPE_FLAT;
+ mSensorState[SENSOR_TYPE_FLAT].rate = SENSOR_RATE_ONESHOT;
+ mSensorState[SENSOR_TYPE_FLAT].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_RGBW].sensorType = SENSOR_TYPE_RGBW;
+ mSensorState[SENSOR_TYPE_RGBW].timestamp_filter = false;
+
+ mSensorState[SENSOR_TYPE_SAR].sensorType = SENSOR_TYPE_SAR;
+ mSensorState[SENSOR_TYPE_SAR].rate = SENSOR_RATE_ONCHANGE;
+ mSensorState[SENSOR_TYPE_SAR].timestamp_filter = false;
+}
+
+static void init_sensor_config_cmd(struct ConfigCmd *cmd,
+ int sensor_type)
+{
+ uint8_t alt = mSensorState[sensor_type].alt;
+ bool enable = 0;
+
+ memset(cmd, 0x00, sizeof(*cmd));
+
+ cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT;
+ cmd->sensorType = mSensorState[sensor_type].sensorType;
+
+ if (alt && mSensorState[alt].enable &&
+ mSensorState[sensor_type].enable) {
+ cmd->cmd = CONFIG_CMD_ENABLE;
+ if (mSensorState[alt].rate > mSensorState[sensor_type].rate)
+ cmd->rate = mSensorState[alt].rate;
+ else
+ cmd->rate = mSensorState[sensor_type].rate;
+ if (mSensorState[alt].latency <
+ mSensorState[sensor_type].latency)
+ cmd->latency = mSensorState[alt].latency;
+ else
+ cmd->latency = mSensorState[sensor_type].latency;
+ } else if (alt && mSensorState[alt].enable) {
+ enable = mSensorState[alt].enable;
+ cmd->cmd = enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
+ cmd->rate = mSensorState[alt].rate;
+ cmd->latency = mSensorState[alt].latency;
+ } else { /* !alt || !mSensorState[alt].enable */
+ enable = mSensorState[sensor_type].enable;
+ cmd->cmd = enable ? CONFIG_CMD_ENABLE : CONFIG_CMD_DISABLE;
+ cmd->rate = mSensorState[sensor_type].rate;
+ cmd->latency = mSensorState[sensor_type].latency;
+ }
+}
+
+static int mtk_nanohub_report_data(struct data_unit_t *data_t)
+{
+ int err = 0, sensor_type = 0, sensor_id = 0;
+
+ sensor_id = data_t->sensor_type;
+ sensor_type = id_to_type(sensor_id);
+ data_t->time_stamp += get_filter_output(&moving_average_algo);
+
+ if (sensor_id >= ID_SENSOR_MAX || sensor_id < 0) {
+ pr_err("invalid sensor id %d\n", sensor_id);
+ return 0;
+ }
+ /* must check report err for retry sending */
+ err = mtk_nanohub_report_to_manager(data_t);
+ /* for flush only !err true we decrease flushcnt */
+ if (data_t->flush_action == FLUSH_ACTION && !err)
+ atomic_dec_if_positive(&mSensorState[sensor_type].flushCnt);
+ return err;
+}
+static int mtk_nanohub_server_dispatch_data(uint32_t *currWp)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ char *pStart, *pEnd, *rp, *wp;
+ struct data_unit_t event, event_copy;
+ uint32_t wp_copy;
+ int err = 0;
+
+ pStart = (char *)READ_ONCE(device->scp_sensor_fifo) +
+ offsetof(struct sensor_fifo, data);
+ pEnd = pStart + READ_ONCE(device->scp_sensor_fifo->fifo_size);
+ wp_copy = *currWp;
+ rp = pStart + READ_ONCE(device->scp_sensor_fifo->rp);
+ wp = pStart + wp_copy;
+
+
+ if (wp < pStart || pEnd < wp) {
+ pr_err("FIFO wp invalid : %p, %p, %p\n", pStart, pEnd, wp);
+ return -5;
+ }
+ if (rp == wp) {
+ pr_err("FIFO empty\n");
+ return 0;
+ }
+ /*
+ * opimize for dram,no cache,we should cpy data to cacheable ram
+ * event and event_copy are cacheable ram, mtk_nanohub_report_data
+ * will change time_stamp field, so when mtk_nanohub_report_data fail
+ * we should reinit the time_stamp by memcpy to event_copy;
+ * why memcpy_fromio(&event_copy), because rp is not cacheable
+ */
+ if (rp < wp) {
+ while (rp < wp) {
+ memcpy_fromio(&event, rp, SENSOR_DATA_SIZE);
+ /* sleep safe enough, data save in dram and not lost */
+ do {
+ /* init event_copy when retry */
+ event_copy = event;
+ err = mtk_nanohub_report_data(&event_copy);
+ if (err < 0)
+ usleep_range(2000, 4000);
+ } while (err < 0);
+ rp += SENSOR_DATA_SIZE;
+ }
+ } else if (rp > wp) {
+ while (rp < pEnd) {
+ memcpy_fromio(&event, rp, SENSOR_DATA_SIZE);
+ do {
+ /* init event_copy when retry */
+ event_copy = event;
+ err = mtk_nanohub_report_data(&event_copy);
+ if (err < 0)
+ usleep_range(2000, 4000);
+ } while (err < 0);
+ rp += SENSOR_DATA_SIZE;
+ }
+ rp = pStart;
+ while (rp < wp) {
+ memcpy_fromio(&event, rp, SENSOR_DATA_SIZE);
+ do {
+ /* init event_copy when retry */
+ event_copy = event;
+ err = mtk_nanohub_report_data(&event_copy);
+ if (err < 0)
+ usleep_range(2000, 4000);
+ } while (err < 0);
+ rp += SENSOR_DATA_SIZE;
+ }
+ }
+ /*
+ * must device->scp_sensor_fifo->rp = wp,
+ * not device->scp_sensor_fifo->rp = device->scp_sensor_fifo->wp
+ */
+ WRITE_ONCE(device->scp_sensor_fifo->rp, wp_copy);
+ return 0;
+}
+
+static int mtk_nanohub_send_dram_info_to_hub(void)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ union SCP_SENSOR_HUB_DATA data;
+ unsigned int len = 0;
+ int err = 0, retry = 0, total = 10;
+
+ device->shub_dram_phys = scp_get_reserve_mem_phys(SENS_MEM_ID);
+ device->shub_dram_virt = scp_get_reserve_mem_virt(SENS_MEM_ID);
+
+ data.set_config_req.sensorType = 0;
+ data.set_config_req.action = SENSOR_HUB_SET_CONFIG;
+ data.set_config_req.bufferBase =
+ (unsigned int)(device->shub_dram_phys & 0xFFFFFFFF);
+
+ len = sizeof(data.set_config_req);
+ for (retry = 0; retry < total; ++retry) {
+ err = mtk_nanohub_req_send(&data);
+ if (err < 0 || data.rsp.action != SENSOR_HUB_SET_CONFIG) {
+ pr_err("%s fail!\n", __func__);
+ continue;
+ }
+ break;
+ }
+ if (retry < total)
+ pr_notice("%s success\n", __func__);
+ return SCP_SENSOR_HUB_SUCCESS;
+}
+
+static int mtk_nanohub_enable_rawdata_to_hub(int sensor_id,
+ int en)
+{
+ int err = 0;
+ union SCP_SENSOR_HUB_DATA req;
+
+ req.req.sensorType = sensor_id;
+ req.req.action = SENSOR_HUB_RAW_DATA;
+ req.req.data[0] = en;
+
+ err = mtk_nanohub_req_send(&req);
+ if (err < 0 || sensor_id != req.rsp.sensorType ||
+ req.rsp.action != SENSOR_HUB_RAW_DATA) {
+ pr_err("%s fail!\n", __func__);
+ return -1;
+ }
+ return err;
+}
+
+static int mtk_nanohub_send_timestamp_wake_locked(void)
+{
+ union SCP_SENSOR_HUB_DATA req;
+ int len;
+ int err = 0;
+ uint64_t now_time, arch_counter;
+
+ /* send_timestamp_to_hub is process context, disable irq is safe */
+ local_irq_disable();
+ now_time = ktime_get_boot_ns();
+ arch_counter = arch_counter_get_cntvct();
+ local_irq_enable();
+ req.set_config_req.sensorType = 0;
+ req.set_config_req.action = SENSOR_HUB_SET_TIMESTAMP;
+ req.set_config_req.ap_timestamp = now_time;
+ req.set_config_req.arch_counter = arch_counter;
+ /* pr_err("ns=%lld, arch_counter=%lld!\n", now_time, arch_counter); */
+ len = sizeof(req.set_config_req);
+ err = mtk_nanohub_req_send(&req);
+ if (err < 0 || req.rsp.action != SENSOR_HUB_SET_TIMESTAMP) {
+ pr_err("%s fail!\n", __func__);
+ return -1;
+ }
+ return err;
+}
+
+static int mtk_nanohub_send_timestamp_to_hub(void)
+{
+ int err = 0;
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+
+ if (READ_ONCE(rtc_compensation_suspend)) {
+ pr_err("rtc_compensation_suspend suspend,drop time sync\n");
+ return 0;
+ }
+
+ __pm_stay_awake(&device->time_sync_wakeup_src);
+ err = mtk_nanohub_send_timestamp_wake_locked();
+ __pm_relax(&device->time_sync_wakeup_src);
+ return err;
+}
+
+int mtk_nanohub_enable_to_hub(uint8_t sensor_id, int enabledisable)
+{
+ uint8_t sensor_type = id_to_type(sensor_id);
+ struct ConfigCmd cmd;
+ int ret = 0;
+
+ if (enabledisable == 1)
+ scp_register_feature(SENS_FEATURE_ID);
+ mutex_lock(&mSensorState_mtx);
+ if (sensor_id >= ID_SENSOR_MAX) {
+ pr_err("invalid id %d\n", sensor_id);
+ mutex_unlock(&mSensorState_mtx);
+ return -1;
+ }
+ if (!mSensorState[sensor_type].sensorType) {
+ pr_err("unhandle id %d, is inited?\n", sensor_id);
+ mutex_unlock(&mSensorState_mtx);
+ return -1;
+ }
+ mSensorState[sensor_type].enable = enabledisable;
+ init_sensor_config_cmd(&cmd, sensor_type);
+ if (atomic_read(&power_status) == SENSOR_POWER_UP) {
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_err("fail enable: [%d,%d]\n", sensor_id, cmd.cmd);
+ }
+ if (!enabledisable && atomic_read(&mSensorState[sensor_type].flushCnt))
+ pr_err("id=%d flush count not 0 when disable\n", sensor_id);
+ mutex_unlock(&mSensorState_mtx);
+ return ret < 0 ? ret : 0;
+}
+
+int mtk_nanohub_batch_to_hub(uint8_t sensor_id,
+ int flag, int64_t samplingPeriodNs,
+ int64_t maxBatchReportLatencyNs)
+{
+ uint8_t sensor_type = id_to_type(sensor_id);
+ struct ConfigCmd cmd;
+ int ret = 0;
+ uint64_t rate = 1024000000000ULL;
+
+ mutex_lock(&mSensorState_mtx);
+ if (sensor_id >= ID_SENSOR_MAX) {
+ pr_err("invalid id %d\n", sensor_id);
+ mutex_unlock(&mSensorState_mtx);
+ return -1;
+ }
+ if (!mSensorState[sensor_type].sensorType) {
+ pr_err("unhandle type %d, is inited?\n", sensor_type);
+ mutex_unlock(&mSensorState_mtx);
+ return -1;
+ }
+ if (samplingPeriodNs > 0 &&
+ mSensorState[sensor_type].rate != SENSOR_RATE_ONCHANGE &&
+ mSensorState[sensor_type].rate != SENSOR_RATE_ONESHOT) {
+ rate = div64_u64(rate, samplingPeriodNs);
+ mSensorState[sensor_type].rate = rate;
+ }
+ mSensorState[sensor_type].latency = maxBatchReportLatencyNs;
+ init_sensor_config_cmd(&cmd, sensor_type);
+ if (atomic_read(&power_status) == SENSOR_POWER_UP) {
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_err("failed batch: [%d,%d,%lld,%d]\n",
+ sensor_id, cmd.rate, cmd.latency, cmd.cmd);
+ }
+ mutex_unlock(&mSensorState_mtx);
+ return ret < 0 ? ret : 0;
+}
+
+int mtk_nanohub_flush_to_hub(uint8_t sensor_id)
+{
+ uint8_t sensor_type = id_to_type(sensor_id);
+ struct ConfigCmd cmd;
+ int ret = 0;
+
+ mutex_lock(&mSensorState_mtx);
+ if (sensor_id >= ID_SENSOR_MAX) {
+ pr_err("invalid id %d\n", sensor_id);
+ mutex_unlock(&mSensorState_mtx);
+ return -1;
+ }
+ if (!mSensorState[sensor_type].sensorType) {
+ pr_err("unhandle id %d, is inited?\n", sensor_id);
+ mutex_unlock(&mSensorState_mtx);
+ return -1;
+ }
+ atomic_inc(&mSensorState[sensor_type].flushCnt);
+ init_sensor_config_cmd(&cmd, sensor_type);
+ cmd.cmd = CONFIG_CMD_FLUSH;
+ if (atomic_read(&power_status) == SENSOR_POWER_UP) {
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_err("failed flush: [%d]\n", sensor_id);
+ }
+ mutex_unlock(&mSensorState_mtx);
+ return ret < 0 ? ret : 0;
+}
+
+int mtk_nanohub_cfg_to_hub(uint8_t sensor_id, uint8_t *data, uint8_t count)
+{
+ struct ConfigCmd *cmd = NULL;
+ int ret = 0;
+
+ if (sensor_id >= ID_SENSOR_MAX) {
+ pr_err("invalid id %d\n", sensor_id);
+ return -1;
+ }
+ cmd = vzalloc(sizeof(struct ConfigCmd) + count);
+ if (!cmd)
+ return -1;
+ cmd->evtType = EVT_NO_SENSOR_CONFIG_EVENT;
+ cmd->sensorType = id_to_type(sensor_id);
+ cmd->cmd = CONFIG_CMD_CFG_DATA;
+ memcpy(cmd->data, data, count);
+ if (atomic_read(&power_status) == SENSOR_POWER_UP) {
+ ret = nanohub_external_write((const uint8_t *)cmd,
+ sizeof(struct ConfigCmd) + count);
+ if (ret < 0)
+ pr_err("failed cfg: [%d,%d]\n", sensor_id, cmd->cmd);
+ }
+ vfree(cmd);
+ return ret < 0 ? ret : 0;
+}
+
+int mtk_nanohub_calibration_to_hub(uint8_t sensor_id)
+{
+ uint8_t sensor_type = id_to_type(sensor_id);
+ struct ConfigCmd cmd;
+ int ret = 0;
+
+ if (sensor_id >= ID_SENSOR_MAX) {
+ pr_err("invalid id %d\n", sensor_id);
+ return -1;
+ }
+ if (!mSensorState[sensor_type].sensorType) {
+ pr_err("unhandle id %d, is inited?\n", sensor_id);
+ return -1;
+ }
+ init_sensor_config_cmd(&cmd, sensor_type);
+ cmd.cmd = CONFIG_CMD_CALIBRATE;
+ if (atomic_read(&power_status) == SENSOR_POWER_UP) {
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_err("failed calibration: [%d]\n", sensor_id);
+ }
+ return ret < 0 ? ret : 0;
+}
+
+int mtk_nanohub_selftest_to_hub(uint8_t sensor_id)
+{
+ uint8_t sensor_type = id_to_type(sensor_id);
+ struct ConfigCmd cmd;
+ int ret = 0;
+
+ if (sensor_id >= ID_SENSOR_MAX) {
+ pr_err("invalid id %d\n", sensor_id);
+ return -1;
+ }
+ if (!mSensorState[sensor_type].sensorType) {
+ pr_err("unhandle id %d, is inited?\n", sensor_id);
+ return -1;
+ }
+ init_sensor_config_cmd(&cmd, sensor_type);
+ cmd.cmd = CONFIG_CMD_SELF_TEST;
+ if (atomic_read(&power_status) == SENSOR_POWER_UP) {
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_err("failed selfttest: [%d]\n", sensor_id);
+ }
+ return ret < 0 ? ret : 0;
+}
+
+int mtk_nanohub_get_data_from_hub(uint8_t sensor_id,
+ struct data_unit_t *data)
+{
+ union SCP_SENSOR_HUB_DATA req;
+ struct data_unit_t *data_t;
+ int len = 0, err = 0;
+
+ if (atomic_read(&power_status) == SENSOR_POWER_DOWN) {
+ pr_err("scp power down, we can not access scp\n");
+ return -1;
+ }
+
+ req.get_data_req.sensorType = sensor_id;
+ req.get_data_req.action = SENSOR_HUB_GET_DATA;
+ len = sizeof(req.get_data_req);
+ err = mtk_nanohub_req_send(&req);
+ if (err < 0) {
+ pr_err("get_data fail:%d!\n", err);
+ return -1;
+ }
+ if (sensor_id != req.get_data_rsp.sensorType ||
+ req.get_data_rsp.action != SENSOR_HUB_GET_DATA ||
+ req.get_data_rsp.errCode != 0) {
+ pr_err("req id: %d, rsp Type:%d action:%d, errcode:%d\n",
+ sensor_id, req.get_data_rsp.sensorType,
+ req.get_data_rsp.action, req.get_data_rsp.errCode);
+
+ return req.get_data_rsp.errCode;
+ }
+
+ data_t = (struct data_unit_t *)req.get_data_rsp.data.int8_Data;
+ switch (sensor_id) {
+ case ID_ACCELEROMETER:
+ data->time_stamp = data_t->time_stamp;
+ data->accelerometer_t.x = data_t->accelerometer_t.x;
+ data->accelerometer_t.y = data_t->accelerometer_t.y;
+ data->accelerometer_t.z = data_t->accelerometer_t.z;
+ data->accelerometer_t.x_bias = data_t->accelerometer_t.x_bias;
+ data->accelerometer_t.y_bias = data_t->accelerometer_t.y_bias;
+ data->accelerometer_t.z_bias = data_t->accelerometer_t.z_bias;
+ data->accelerometer_t.status = data_t->accelerometer_t.status;
+ break;
+ case ID_LIGHT:
+ data->time_stamp = data_t->time_stamp;
+ data->light = data_t->light;
+ break;
+ case ID_PROXIMITY:
+ data->time_stamp = data_t->time_stamp;
+ data->proximity_t.steps = data_t->proximity_t.steps;
+ data->proximity_t.oneshot = data_t->proximity_t.oneshot;
+ break;
+ case ID_PRESSURE:
+ data->time_stamp = data_t->time_stamp;
+ data->pressure_t.pressure = data_t->pressure_t.pressure;
+ data->pressure_t.status = data_t->pressure_t.status;
+ break;
+ case ID_GYROSCOPE:
+ data->time_stamp = data_t->time_stamp;
+ data->gyroscope_t.x = data_t->gyroscope_t.x;
+ data->gyroscope_t.y = data_t->gyroscope_t.y;
+ data->gyroscope_t.z = data_t->gyroscope_t.z;
+ data->gyroscope_t.x_bias = data_t->gyroscope_t.x_bias;
+ data->gyroscope_t.y_bias = data_t->gyroscope_t.y_bias;
+ data->gyroscope_t.z_bias = data_t->gyroscope_t.z_bias;
+ data->gyroscope_t.status = data_t->gyroscope_t.status;
+ break;
+ case ID_MAGNETIC_FIELD:
+ data->time_stamp = data_t->time_stamp;
+ data->magnetic_t.x = data_t->magnetic_t.x;
+ data->magnetic_t.y = data_t->magnetic_t.y;
+ data->magnetic_t.z = data_t->magnetic_t.z;
+ data->magnetic_t.x_bias = data_t->magnetic_t.x_bias;
+ data->magnetic_t.y_bias = data_t->magnetic_t.y_bias;
+ data->magnetic_t.z_bias = data_t->magnetic_t.z_bias;
+ data->magnetic_t.status = data_t->magnetic_t.status;
+ break;
+ case ID_SAR:
+ data->time_stamp = data_t->time_stamp;
+ data->sar_event.data[0] = data_t->sar_event.data[0];
+ data->sar_event.data[1] = data_t->sar_event.data[1];
+ data->sar_event.data[2] = data_t->sar_event.data[2];
+ break;
+ default:
+ err = -1;
+ break;
+ }
+ return err;
+}
+
+int mtk_nanohub_set_cmd_to_hub(uint8_t sensor_id,
+ enum CUST_ACTION action, void *data)
+{
+ union SCP_SENSOR_HUB_DATA req;
+ int len = 0, err = 0;
+ struct SCP_SENSOR_HUB_GET_RAW_DATA *pGetRawData;
+
+ req.get_data_req.sensorType = sensor_id;
+ req.get_data_req.action = SENSOR_HUB_SET_CUST;
+
+ if (atomic_read(&power_status) == SENSOR_POWER_DOWN) {
+ pr_err("scp power down, we can not access scp\n");
+ return -1;
+ }
+
+ switch (sensor_id) {
+ case ID_ACCELEROMETER:
+ req.set_cust_req.sensorType = ID_ACCELEROMETER;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_RESET_CALI:
+ req.set_cust_req.resetCali.action =
+ CUST_ACTION_RESET_CALI;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.resetCali);
+ break;
+ case CUST_ACTION_SET_CALI:
+ req.set_cust_req.setCali.action = CUST_ACTION_SET_CALI;
+ req.set_cust_req.setCali.int32_data[SCP_SENSOR_HUB_X]
+ = *((int32_t *) data + SCP_SENSOR_HUB_X);
+ req.set_cust_req.setCali.int32_data[SCP_SENSOR_HUB_Y]
+ = *((int32_t *) data + SCP_SENSOR_HUB_Y);
+ req.set_cust_req.setCali.int32_data[SCP_SENSOR_HUB_Z]
+ = *((int32_t *) data + SCP_SENSOR_HUB_Z);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setCali);
+ break;
+ case CUST_ACTION_SET_TRACE:
+ req.set_cust_req.setTrace.action =
+ CUST_ACTION_SET_TRACE;
+ req.set_cust_req.setTrace.trace = *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setTrace);
+ break;
+ case CUST_ACTION_SET_DIRECTION:
+ req.set_cust_req.setDirection.action =
+ CUST_ACTION_SET_DIRECTION;
+ req.set_cust_req.setDirection.direction =
+ *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setDirection);
+ break;
+ case CUST_ACTION_SET_FACTORY:
+ req.set_cust_req.setFactory.action =
+ CUST_ACTION_SET_FACTORY;
+ req.set_cust_req.setFactory.factory =
+ *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setFactory);
+ break;
+ case CUST_ACTION_SHOW_REG:
+ req.set_cust_req.showReg.action = CUST_ACTION_SHOW_REG;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showReg);
+ break;
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ case ID_LIGHT:
+ req.set_cust_req.sensorType = ID_LIGHT;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_GET_RAW_DATA:
+ req.set_cust_req.getRawData.action =
+ CUST_ACTION_GET_RAW_DATA;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getRawData);
+ err = mtk_nanohub_req_send(&req);
+ if (err == 0) {
+ if ((req.set_cust_rsp.action !=
+ SENSOR_HUB_SET_CUST)
+ || (req.set_cust_rsp.errCode != 0)) {
+ pr_err("get_raw fail!\n");
+ return -1;
+ }
+ if (req.set_cust_rsp.getRawData.action !=
+ CUST_ACTION_GET_RAW_DATA) {
+ pr_err("get_raw fail!\n");
+ return -1;
+ }
+ pGetRawData = &req.set_cust_rsp.getRawData;
+ *((uint8_t *) data) =
+ pGetRawData->uint8_data[0];
+ } else {
+ pr_err("get_raw failed!\n");
+ }
+ return 0;
+ case CUST_ACTION_SHOW_ALSLV:
+ req.set_cust_req.showAlslv.action =
+ CUST_ACTION_SHOW_ALSLV;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showAlslv);
+ break;
+ case CUST_ACTION_SHOW_ALSVAL:
+ req.set_cust_req.showAlsval.action =
+ CUST_ACTION_GET_RAW_DATA;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showAlsval);
+ break;
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ case ID_PROXIMITY:
+ req.set_cust_req.sensorType = ID_PROXIMITY;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_RESET_CALI:
+ req.set_cust_req.resetCali.action =
+ CUST_ACTION_RESET_CALI;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.resetCali);
+ break;
+ case CUST_ACTION_SET_CALI:
+ req.set_cust_req.setCali.action = CUST_ACTION_SET_CALI;
+ req.set_cust_req.setCali.int32_data[0] =
+ *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setCali);
+ break;
+ case CUST_ACTION_SET_TRACE:
+ req.set_cust_req.setTrace.action =
+ CUST_ACTION_SET_TRACE;
+ req.set_cust_req.setTrace.trace = *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setTrace);
+ break;
+ case CUST_ACTION_SHOW_REG:
+ req.set_cust_req.showReg.action = CUST_ACTION_SHOW_REG;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showReg);
+ break;
+ case CUST_ACTION_SET_PS_THRESHOLD:
+ req.set_cust_req.setPSThreshold.action =
+ CUST_ACTION_SET_PS_THRESHOLD;
+ req.set_cust_req.setPSThreshold.threshold[0]
+ = *((int32_t *) data + 0);
+ req.set_cust_req.setPSThreshold.threshold[1]
+ = *((int32_t *) data + 1);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setPSThreshold);
+ break;
+ case CUST_ACTION_GET_RAW_DATA:
+ req.set_cust_req.getRawData.action =
+ CUST_ACTION_GET_RAW_DATA;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getRawData);
+ err = mtk_nanohub_req_send(&req);
+ if (err == 0) {
+ if ((req.set_cust_rsp.action !=
+ SENSOR_HUB_SET_CUST)
+ || (req.set_cust_rsp.errCode != 0)) {
+ pr_err("get_raw fail!\n");
+ return -1;
+ }
+ if (req.set_cust_rsp.getRawData.action !=
+ CUST_ACTION_GET_RAW_DATA) {
+ pr_err("get_raw fail!\n");
+ return -1;
+ }
+ pGetRawData = &req.set_cust_rsp.getRawData;
+ *((uint16_t *) data) =
+ pGetRawData->uint16_data[0];
+ } else {
+ pr_err("get_raw failed!\n");
+ }
+ return 0;
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ case ID_PRESSURE:
+ req.set_cust_req.sensorType = ID_PRESSURE;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_SET_TRACE:
+ req.set_cust_req.setTrace.action =
+ CUST_ACTION_SET_TRACE;
+ req.set_cust_req.setTrace.trace = *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setTrace);
+ break;
+ case CUST_ACTION_SHOW_REG:
+ req.set_cust_req.showReg.action = CUST_ACTION_SHOW_REG;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showReg);
+ break;
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ case ID_GYROSCOPE:
+ req.set_cust_req.sensorType = ID_GYROSCOPE;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_RESET_CALI:
+ req.set_cust_req.resetCali.action =
+ CUST_ACTION_RESET_CALI;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.resetCali);
+ break;
+ case CUST_ACTION_SET_CALI:
+ req.set_cust_req.setCali.action = CUST_ACTION_SET_CALI;
+ req.set_cust_req.setCali.int32_data[SCP_SENSOR_HUB_X]
+ = *((int32_t *) data + SCP_SENSOR_HUB_X);
+ req.set_cust_req.setCali.int32_data[SCP_SENSOR_HUB_Y]
+ = *((int32_t *) data + SCP_SENSOR_HUB_Y);
+ req.set_cust_req.setCali.int32_data[SCP_SENSOR_HUB_Z]
+ = *((int32_t *) data + SCP_SENSOR_HUB_Z);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setCali);
+ break;
+ case CUST_ACTION_SET_TRACE:
+ req.set_cust_req.setTrace.action =
+ CUST_ACTION_SET_TRACE;
+ req.set_cust_req.setTrace.trace = *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setTrace);
+ break;
+ case CUST_ACTION_SET_DIRECTION:
+ req.set_cust_req.setDirection.action =
+ CUST_ACTION_SET_DIRECTION;
+ req.set_cust_req.setDirection.direction =
+ *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setDirection);
+ break;
+ case CUST_ACTION_SET_FACTORY:
+ req.set_cust_req.setFactory.action =
+ CUST_ACTION_SET_FACTORY;
+ req.set_cust_req.setFactory.factory =
+ *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setFactory);
+ break;
+ case CUST_ACTION_SHOW_REG:
+ req.set_cust_req.showReg.action =
+ CUST_ACTION_SHOW_REG;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showReg);
+ break;
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ case ID_MAGNETIC_FIELD:
+ req.set_cust_req.sensorType = ID_MAGNETIC_FIELD;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_SET_TRACE:
+ req.set_cust_req.setTrace.action =
+ CUST_ACTION_SET_TRACE;
+ req.set_cust_req.setTrace.trace = *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setTrace);
+ break;
+ case CUST_ACTION_SET_DIRECTION:
+ req.set_cust_req.setDirection.action =
+ CUST_ACTION_SET_DIRECTION;
+ req.set_cust_req.setDirection.direction =
+ *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setDirection);
+ break;
+ case CUST_ACTION_SHOW_REG:
+ req.set_cust_req.showReg.action = CUST_ACTION_SHOW_REG;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.showReg);
+ break;
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ case ID_SAR:
+ req.set_cust_req.sensorType = ID_SAR;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_GET_SENSOR_INFO:
+ req.set_cust_req.getInfo.action =
+ CUST_ACTION_GET_SENSOR_INFO;
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.getInfo);
+ break;
+ default:
+ return -1;
+ }
+ break;
+ default:
+ req.set_cust_req.sensorType = sensor_id;
+ req.set_cust_req.action = SENSOR_HUB_SET_CUST;
+ switch (action) {
+ case CUST_ACTION_SET_TRACE:
+ req.set_cust_req.setTrace.action =
+ CUST_ACTION_SET_TRACE;
+ req.set_cust_req.setTrace.trace = *((int32_t *) data);
+ len = offsetof(struct SCP_SENSOR_HUB_SET_CUST_REQ,
+ custData) + sizeof(req.set_cust_req.setTrace);
+ break;
+ default:
+ return -1;
+ }
+ }
+ err = mtk_nanohub_req_send(&req);
+ if (err < 0) {
+ pr_err("set_cust fail!\n");
+ return -1;
+ }
+ if (sensor_id != req.get_data_rsp.sensorType
+ || SENSOR_HUB_SET_CUST != req.get_data_rsp.action
+ || 0 != req.get_data_rsp.errCode) {
+ pr_err("error : %d\n", req.get_data_rsp.errCode);
+ return req.get_data_rsp.errCode;
+ }
+
+ switch (action) {
+ case CUST_ACTION_GET_SENSOR_INFO:
+ if (req.set_cust_rsp.getInfo.action !=
+ CUST_ACTION_GET_SENSOR_INFO) {
+ pr_info("get_info failed!\n");
+ return -1;
+ }
+ memcpy((struct sensorInfo_t *)data,
+ &req.set_cust_rsp.getInfo.sensorInfo,
+ sizeof(struct sensorInfo_t));
+ break;
+ default:
+ break;
+ }
+ return err;
+}
+
+static void mtk_nanohub_restoring_sensor(int sensor_id)
+{
+ uint8_t sensor_type = id_to_type(sensor_id);
+ int ret = 0;
+ int flush_cnt = 0;
+ struct ConfigCmd cmd;
+
+ if (mSensorState[sensor_type].sensorType &&
+ mSensorState[sensor_type].enable) {
+ init_sensor_config_cmd(&cmd, sensor_type);
+ pr_debug("restoring: [%d,%d,%d,%lld]\n",
+ sensor_id, mSensorState[sensor_type].enable,
+ mSensorState[sensor_type].rate,
+ mSensorState[sensor_type].latency);
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_notice("failed registerlistener [%d,%d]\n",
+ sensor_id, cmd.cmd);
+
+ cmd.cmd = CONFIG_CMD_FLUSH;
+ for (flush_cnt = 0; flush_cnt <
+ atomic_read(&mSensorState[sensor_type].flushCnt);
+ flush_cnt++) {
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_notice("failed flush:%d\n", sensor_id);
+ }
+ }
+}
+
+static void mtk_nanohub_get_devinfo(void)
+{
+ int id = 0, sensor = 0;
+ struct sensorInfo_t hubinfo;
+ struct sensorlist_info_t listinfo;
+ struct mag_libinfo_t maginfo;
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+
+ if (likely(atomic_xchg(&device->get_list_first_boot, 1)))
+ return;
+
+ for (id = 0; id < ID_SENSOR_MAX; ++id) {
+ sensor = id_to_type(id);
+ if (sensorlist_find_sensor(sensor) < 0)
+ continue;
+ memset(&hubinfo, 0, sizeof(struct sensorInfo_t));
+ if (mtk_nanohub_set_cmd_to_hub(id,
+ CUST_ACTION_GET_SENSOR_INFO, &hubinfo) < 0) {
+ pr_err("type(%d) not registered\n", sensor);
+ continue;
+ }
+ memset(&listinfo, 0, sizeof(struct sensorlist_info_t));
+ strlcpy(listinfo.name, hubinfo.name, sizeof(listinfo.name));
+ sensorlist_register_devinfo(sensor, &listinfo);
+ /* restore mag lib info */
+ if (sensor == SENSOR_TYPE_MAGNETIC_FIELD) {
+ memset(&maginfo, 0, sizeof(struct mag_libinfo_t));
+ maginfo.deviceid = hubinfo.mag_dev_info.deviceid;
+ maginfo.layout = hubinfo.mag_dev_info.layout;
+ strlcpy(maginfo.libname, hubinfo.mag_dev_info.libname,
+ sizeof(maginfo.libname));
+ sensorlist_register_maginfo(&maginfo);
+ }
+ }
+}
+
+static void mtk_nanohub_restoring_config(void)
+{
+ int length = 0;
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ uint8_t *data = NULL;
+
+ if (unlikely(!atomic_xchg(&device->cfg_data_after_reboot, 1)))
+ return;
+
+ pr_notice("restoring sensor config\n");
+
+ length = sizeof(device->acc_config_data);
+ data = vzalloc(length);
+ spin_lock(&config_data_lock);
+ memcpy(data, device->acc_config_data, length);
+ spin_unlock(&config_data_lock);
+ mtk_nanohub_cfg_to_hub(ID_ACCELEROMETER, data, length);
+ vfree(data);
+
+ length = sizeof(device->gyro_config_data);
+ data = vzalloc(length);
+ spin_lock(&config_data_lock);
+ memcpy(data, device->gyro_config_data, length);
+ spin_unlock(&config_data_lock);
+ mtk_nanohub_cfg_to_hub(ID_GYROSCOPE, data, length);
+ vfree(data);
+
+ length = sizeof(device->mag_config_data);
+ data = vzalloc(length);
+ spin_lock(&config_data_lock);
+ memcpy(data, device->mag_config_data, length);
+ spin_unlock(&config_data_lock);
+ mtk_nanohub_cfg_to_hub(ID_MAGNETIC_FIELD, data, length);
+ vfree(data);
+
+ length = sizeof(device->light_config_data);
+ data = vzalloc(length);
+ spin_lock(&config_data_lock);
+ memcpy(data, device->light_config_data, length);
+ spin_unlock(&config_data_lock);
+ mtk_nanohub_cfg_to_hub(ID_LIGHT, data, length);
+ vfree(data);
+
+ length = sizeof(device->proximity_config_data);
+ data = vzalloc(length);
+ spin_lock(&config_data_lock);
+ memcpy(data, device->proximity_config_data, length);
+ spin_unlock(&config_data_lock);
+ mtk_nanohub_cfg_to_hub(ID_PROXIMITY, data, length);
+ vfree(data);
+}
+
+static void mtk_nanohub_start_timesync(void)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+
+ if (likely(atomic_xchg(&device->start_timesync_first_boot, 1)))
+ return;
+
+ mod_timer(&device->sync_time_timer,
+ jiffies + msecs_to_jiffies(SYNC_TIME_START_CYCLC));
+}
+
+void mtk_nanohub_power_up_loop(void *data)
+{
+ int id = 0;
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ unsigned long flags = 0;
+
+ wait_event(power_reset_wait,
+ READ_ONCE(scp_system_ready) && READ_ONCE(scp_chre_ready));
+ spin_lock_irqsave(&scp_state_lock, flags);
+ WRITE_ONCE(scp_chre_ready, false);
+ WRITE_ONCE(scp_system_ready, false);
+ spin_unlock_irqrestore(&scp_state_lock, flags);
+
+ /* firstly we should update dram information */
+ /* 1. reset wp queue head and tail */
+ device->wp_queue.head = 0;
+ device->wp_queue.tail = 0;
+ /* 2. init dram information */
+ WRITE_ONCE(device->scp_sensor_fifo,
+ (struct sensor_fifo *)
+ (long)scp_get_reserve_mem_virt(SENS_MEM_ID));
+ WARN_ON(device->scp_sensor_fifo == NULL);
+ WRITE_ONCE(device->scp_sensor_fifo->wp, 0);
+ WRITE_ONCE(device->scp_sensor_fifo->rp, 0);
+ WRITE_ONCE(device->scp_sensor_fifo->fifo_size,
+ ((long)scp_get_reserve_mem_size(SENS_MEM_ID) -
+ offsetof(struct sensor_fifo, data)) /
+ SENSOR_DATA_SIZE * SENSOR_DATA_SIZE);
+ pr_debug("scp_sensor_fifo =%p, wp =%d, rp =%d, size =%d\n",
+ READ_ONCE(device->scp_sensor_fifo),
+ READ_ONCE(device->scp_sensor_fifo->wp),
+ READ_ONCE(device->scp_sensor_fifo->rp),
+ READ_ONCE(device->scp_sensor_fifo->fifo_size));
+ /* 3. send dram information to scp */
+ mtk_nanohub_send_dram_info_to_hub();
+ /* 4. get device info for mag lib and dynamic list */
+ mtk_nanohub_get_devinfo();
+ /* 5. start timesync */
+ mtk_nanohub_start_timesync();
+ /* 6. we restore sensor calibration data when scp reboot */
+ mtk_nanohub_restoring_config();
+ /* 7. we enable sensor which sensor is enable by framework */
+ mutex_lock(&mSensorState_mtx);
+ for (id = 0; id < ID_SENSOR_MAX; id++)
+ mtk_nanohub_restoring_sensor(id);
+ mutex_unlock(&mSensorState_mtx);
+}
+
+static int mtk_nanohub_power_up_work(void *data)
+{
+ for (;;)
+ mtk_nanohub_power_up_loop(data);
+ return 0;
+}
+
+static int mtk_nanohub_ready_event(struct notifier_block *this,
+ unsigned long event, void *ptr)
+{
+ unsigned long flags = 0;
+
+ if (event == SCP_EVENT_STOP) {
+ spin_lock_irqsave(&scp_state_lock, flags);
+ WRITE_ONCE(scp_system_ready, false);
+ spin_unlock_irqrestore(&scp_state_lock, flags);
+ atomic_set(&power_status, SENSOR_POWER_DOWN);
+ //scp_power_monitor_notify(SENSOR_POWER_DOWN, ptr);
+ }
+
+ if (event == SCP_EVENT_READY) {
+ spin_lock_irqsave(&scp_state_lock, flags);
+ WRITE_ONCE(scp_system_ready, true);
+ if (READ_ONCE(scp_system_ready) && READ_ONCE(scp_chre_ready)) {
+ spin_unlock_irqrestore(&scp_state_lock, flags);
+ atomic_set(&power_status, SENSOR_POWER_UP);
+ //scp_power_monitor_notify(SENSOR_POWER_UP, ptr);
+ /* schedule_work(&device->power_up_work); */
+ wake_up(&power_reset_wait);
+ } else
+ spin_unlock_irqrestore(&scp_state_lock, flags);
+ }
+
+ return NOTIFY_DONE;
+}
+
+static struct notifier_block mtk_nanohub_ready_notifier = {
+ .notifier_call = mtk_nanohub_ready_event,
+};
+
+static int mtk_nanohub_enable(struct hf_device *hfdev,
+ int sensor_type, int en)
+{
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d,%d]\n", __func__, sensor_type, en);
+ return mtk_nanohub_enable_to_hub(type_to_id(sensor_type), en);
+}
+
+static int mtk_nanohub_batch(struct hf_device *hfdev,
+ int sensor_type, int64_t delay, int64_t latency)
+{
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d,%lld,%lld]\n", __func__, sensor_type, delay, latency);
+ return mtk_nanohub_batch_to_hub(type_to_id(sensor_type),
+ 0, delay, latency);
+}
+
+static int mtk_nanohub_flush(struct hf_device *hfdev,
+ int sensor_type)
+{
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d]\n", __func__, sensor_type);
+ return mtk_nanohub_flush_to_hub(type_to_id(sensor_type));
+}
+
+static int mtk_nanohub_calibration(struct hf_device *hfdev,
+ int sensor_type)
+{
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d]\n", __func__, sensor_type);
+ return mtk_nanohub_calibration_to_hub(type_to_id(sensor_type));
+}
+
+static int mtk_nanohub_config(struct hf_device *hfdev,
+ int sensor_type, int32_t *data)
+{
+ int length = 0;
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d]\n", __func__, sensor_type);
+ switch (type_to_id(sensor_type)) {
+ case ID_ACCELEROMETER:
+ length = sizeof(device->acc_config_data);
+ spin_lock(&config_data_lock);
+ memcpy(device->acc_config_data, data, length);
+ spin_unlock(&config_data_lock);
+ break;
+ case ID_GYROSCOPE:
+ length = sizeof(device->gyro_config_data);
+ spin_lock(&config_data_lock);
+ memcpy(device->gyro_config_data, data, length);
+ spin_unlock(&config_data_lock);
+ break;
+ case ID_MAGNETIC_FIELD:
+ length = sizeof(device->mag_config_data);
+ spin_lock(&config_data_lock);
+ memcpy(device->mag_config_data, data, length);
+ spin_unlock(&config_data_lock);
+ break;
+ case ID_LIGHT:
+ length = sizeof(device->light_config_data);
+ spin_lock(&config_data_lock);
+ memcpy(device->light_config_data, data, length);
+ spin_unlock(&config_data_lock);
+ break;
+ case ID_PROXIMITY:
+ length = sizeof(device->proximity_config_data);
+ spin_lock(&config_data_lock);
+ memcpy(device->proximity_config_data, data, length);
+ spin_unlock(&config_data_lock);
+ break;
+ }
+ if (!length) {
+ pr_err("%s type(%d) length fail\n", __func__, sensor_type);
+ return 0;
+ }
+ return mtk_nanohub_cfg_to_hub(type_to_id(sensor_type),
+ (uint8_t *)data, length);
+}
+
+static int mtk_nanohub_selftest(struct hf_device *hfdev,
+ int sensor_type)
+{
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d]\n", __func__, sensor_type);
+ return mtk_nanohub_selftest_to_hub(type_to_id(sensor_type));
+}
+
+static int mtk_nanohub_rawdata(struct hf_device *hfdev,
+ int sensor_type, int en)
+{
+ if (sensor_type <= 0)
+ return 0;
+ pr_notice("%s [%d,%d]\n", __func__, sensor_type, en);
+ return mtk_nanohub_enable_rawdata_to_hub(type_to_id(sensor_type), en);
+}
+
+static int mtk_nanohub_report_to_manager(struct data_unit_t *data)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ struct hf_manager *manager = mtk_nanohub_dev->hf_dev.manager;
+ struct hf_manager_event event;
+
+ if (!manager)
+ return 0;
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ if (data->flush_action == DATA_ACTION) {
+ switch (data->sensor_type) {
+ case ID_ACCELEROMETER:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->accelerometer_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->accelerometer_t.x;
+ event.word[1] = data->accelerometer_t.y;
+ event.word[2] = data->accelerometer_t.z;
+ event.word[3] = data->accelerometer_t.x_bias;
+ event.word[4] = data->accelerometer_t.y_bias;
+ event.word[5] = data->accelerometer_t.z_bias;
+ break;
+ case ID_MAGNETIC_FIELD:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->magnetic_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->magnetic_t.x;
+ event.word[1] = data->magnetic_t.y;
+ event.word[2] = data->magnetic_t.z;
+ event.word[3] = data->magnetic_t.x_bias;
+ event.word[4] = data->magnetic_t.y_bias;
+ event.word[5] = data->magnetic_t.z_bias;
+ break;
+ case ID_GYROSCOPE:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->gyroscope_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->gyroscope_t.x;
+ event.word[1] = data->gyroscope_t.y;
+ event.word[2] = data->gyroscope_t.z;
+ event.word[3] = data->gyroscope_t.x_bias;
+ event.word[4] = data->gyroscope_t.y_bias;
+ event.word[5] = data->gyroscope_t.z_bias;
+ break;
+ case ID_LIGHT:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->light;
+ break;
+ case ID_PROXIMITY:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->proximity_t.oneshot;
+ event.word[1] = data->proximity_t.steps;
+ break;
+ case ID_PRESSURE:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.accurancy = data->pressure_t.status;
+ event.word[0] = data->pressure_t.pressure;
+ break;
+ case ID_ORIENTATION:
+ case ID_ROTATION_VECTOR:
+ case ID_GAME_ROTATION_VECTOR:
+ case ID_GEOMAGNETIC_ROTATION_VECTOR:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->orientation_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->orientation_t.azimuth;
+ event.word[1] = data->orientation_t.pitch;
+ event.word[2] = data->orientation_t.roll;
+ event.word[3] = data->orientation_t.scalar;
+ break;
+ case ID_LINEAR_ACCELERATION:
+ case ID_GRAVITY:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->accelerometer_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->accelerometer_t.x;
+ event.word[1] = data->accelerometer_t.y;
+ event.word[2] = data->accelerometer_t.z;
+ break;
+ case ID_STEP_COUNTER:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] =
+ data->step_counter_t.accumulated_step_count;
+ break;
+ case ID_STEP_DETECTOR:
+ case ID_SIGNIFICANT_MOTION:
+ case ID_ANSWER_CALL:
+ case ID_FLAT:
+ case ID_GLANCE_GESTURE:
+ case ID_IN_POCKET:
+ case ID_MOTION_DETECT:
+ case ID_PICK_UP_GESTURE:
+ case ID_STATIONARY_DETECT:
+ case ID_WAKE_GESTURE:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->smd_t.state;
+ break;
+ case ID_TILT_DETECTOR:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->tilt_event.state;
+ break;
+ case ID_DEVICE_ORIENTATION:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->tilt_event.state;
+ break;
+ case ID_SAR:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->sar_event.data[0];
+ event.word[1] = data->sar_event.data[1];
+ event.word[2] = data->sar_event.data[2];
+ break;
+ default:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->data[0];
+ event.word[1] = data->data[1];
+ event.word[2] = data->data[2];
+ event.word[3] = data->data[3];
+ event.word[4] = data->data[4];
+ event.word[5] = data->data[5];
+ break;
+ }
+ } else if (data->flush_action == FLUSH_ACTION) {
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->data[0];
+ event.word[1] = data->data[1];
+ event.word[2] = data->data[2];
+ event.word[3] = data->data[3];
+ event.word[4] = data->data[4];
+ event.word[5] = data->data[5];
+ pr_notice("%s [%d] flush complete\n",
+ __func__, event.sensor_type);
+ } else if (data->flush_action == BIAS_ACTION) {
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ switch (data->sensor_type) {
+ case ID_ACCELEROMETER:
+ event.word[0] = data->accelerometer_t.x_bias;
+ event.word[1] = data->accelerometer_t.y_bias;
+ event.word[2] = data->accelerometer_t.z_bias;
+ break;
+ case ID_MAGNETIC_FIELD:
+ event.word[0] = data->magnetic_t.x_bias;
+ event.word[1] = data->magnetic_t.y_bias;
+ event.word[2] = data->magnetic_t.z_bias;
+ break;
+ case ID_GYROSCOPE:
+ event.word[0] = data->gyroscope_t.x_bias;
+ event.word[1] = data->gyroscope_t.y_bias;
+ event.word[2] = data->gyroscope_t.z_bias;
+ break;
+ }
+ } else if (data->flush_action == CALI_ACTION) {
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ switch (data->sensor_type) {
+ case ID_ACCELEROMETER:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->accelerometer_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->accelerometer_t.x_bias;
+ event.word[1] = data->accelerometer_t.y_bias;
+ event.word[2] = data->accelerometer_t.z_bias;
+ break;
+ case ID_GYROSCOPE:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->gyroscope_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->gyroscope_t.x_bias;
+ event.word[1] = data->gyroscope_t.y_bias;
+ event.word[2] = data->gyroscope_t.z_bias;
+ break;
+ case ID_PROXIMITY:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->data[0];
+ event.word[1] = data->data[1];
+ break;
+ case ID_LIGHT:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->data[0];
+ break;
+ }
+ } else if (data->flush_action == TEMP_ACTION) {
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ switch (data->sensor_type) {
+ case ID_GYROSCOPE:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.accurancy = data->gyroscope_t.status;
+ event.action = data->flush_action;
+ event.word[0] = data->data[0];
+ event.word[1] = data->data[1];
+ event.word[2] = data->data[2];
+ event.word[3] = data->data[3];
+ event.word[4] = data->data[4];
+ event.word[5] = data->data[5];
+ break;
+ }
+ } else if (data->flush_action == TEST_ACTION) {
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ switch (data->sensor_type) {
+ case ID_ACCELEROMETER:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->accelerometer_t.status;
+ break;
+ case ID_MAGNETIC_FIELD:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->magnetic_t.status;
+ break;
+ case ID_GYROSCOPE:
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->gyroscope_t.status;
+ break;
+ }
+ } else {
+ event.timestamp = data->time_stamp;
+ event.sensor_type = id_to_type(data->sensor_type);
+ event.action = data->flush_action;
+ event.word[0] = data->data[0];
+ event.word[1] = data->data[1];
+ event.word[2] = data->data[2];
+ event.word[3] = data->data[3];
+ event.word[4] = data->data[4];
+ event.word[5] = data->data[5];
+ }
+ /*
+ * oneshot proximity tiledetect should wakeup source when data action
+ */
+ if (data->flush_action == DATA_ACTION) {
+ if (data->sensor_type == ID_PROXIMITY ||
+ data->sensor_type == ID_TILT_DETECTOR ||
+ mSensorState[id_to_type(data->sensor_type)].rate ==
+ SENSOR_RATE_ONESHOT) {
+ __pm_wakeup_event(&device->data_notify_wakeup_src,
+ 250);
+ }
+ }
+ return manager->report(manager, &event);
+}
+
+static int mtk_nanohub_get_sensor_state(unsigned char *list,
+ int size)
+{
+ int i = 0, j = 0;
+ int count = ARRAY_SIZE(mSensorState);
+
+ count = (count < size) ? count : size;
+ for (i = 0; i < count; ++i) {
+ if (!mSensorState[i].sensorType)
+ continue;
+ list[j++] = mSensorState[i].sensorType;
+ }
+ return j;
+}
+
+static int mtk_nanohub_pm_event(struct notifier_block *notifier,
+ unsigned long pm_event,
+ void *unused)
+{
+ switch (pm_event) {
+ case PM_POST_SUSPEND:
+ pr_debug("resume boottime=%lld\n", ktime_get_boot_ns());
+ WRITE_ONCE(rtc_compensation_suspend, false);
+ mtk_nanohub_send_timestamp_to_hub();
+ return NOTIFY_DONE;
+ case PM_SUSPEND_PREPARE:
+ pr_debug("suspend boottime=%lld\n", ktime_get_boot_ns());
+ WRITE_ONCE(rtc_compensation_suspend, true);
+ return NOTIFY_DONE;
+ default:
+ return NOTIFY_OK;
+ }
+ return NOTIFY_OK;
+}
+
+static struct notifier_block mtk_nanohub_pm_notifier_func = {
+ .notifier_call = mtk_nanohub_pm_event,
+ .priority = 0,
+};
+
+static int mtk_nanohub_create_manager(void)
+{
+ int err = 0;
+ int support_size = 0;
+ struct hf_device *hf_dev = &mtk_nanohub_dev->hf_dev;
+
+ memset(hf_dev, 0, sizeof(*hf_dev));
+
+ support_size = mtk_nanohub_get_sensor_state(support_sensors,
+ ARRAY_SIZE(support_sensors));
+
+ hf_dev->dev_name = "mtk_nanohub";
+ hf_dev->device_poll = HF_DEVICE_IO_INTERRUPT;
+ hf_dev->device_bus = HF_DEVICE_IO_ASYNC;
+ hf_dev->support_list = support_sensors;
+ hf_dev->support_size = support_size;
+ hf_dev->enable = mtk_nanohub_enable;
+ hf_dev->batch = mtk_nanohub_batch;
+ hf_dev->flush = mtk_nanohub_flush;
+ hf_dev->calibration = mtk_nanohub_calibration;
+ hf_dev->config_cali = mtk_nanohub_config;
+ hf_dev->selftest = mtk_nanohub_selftest;
+ hf_dev->rawdata = mtk_nanohub_rawdata;
+
+ err = hf_manager_create(hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ return err;
+ }
+ return err;
+}
+
+static ssize_t mtk_nanohub_trace_show(struct device_driver *ddri,
+ char *buf)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ int i;
+ ssize_t res = 0;
+
+ for (i = 0; i < ID_SENSOR_MAX; i++)
+ res += snprintf(&buf[res], PAGE_SIZE, "%2d:[%d]\n",
+ i, atomic_read(&device->traces[i]));
+ return res;
+}
+
+static ssize_t mtk_nanohub_trace_store(struct device_driver *ddri,
+ const char *buf, size_t count)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+ int id, trace = 0;
+ int res = 0;
+
+ if (sscanf(buf, "%d,%d", &id, &trace) != 2) {
+ pr_err("invalid content: '%s', length = %zu\n", buf, count);
+ goto err_out;
+ }
+
+ if (id < 0 || id >= ID_SENSOR_MAX) {
+ pr_debug("invalid id value:%d,should be '0<=id<=%d'\n",
+ trace, ID_SENSOR_MAX);
+ goto err_out;
+ }
+
+ if (trace != 0 && trace != 1) {
+ pr_debug("invalid trace value:%d,trace should be '0' or '1'",
+ trace);
+ goto err_out;
+ }
+
+ res = mtk_nanohub_set_cmd_to_hub(id,
+ CUST_ACTION_SET_TRACE, &trace);
+ if (res < 0) {
+ pr_err("cmd_to_hub (ID: %d),(action: %d)err: %d\n", id,
+ CUST_ACTION_SET_TRACE, res);
+ } else
+ atomic_set(&device->traces[id], trace);
+
+err_out:
+ return count;
+}
+
+static DRIVER_ATTR_RW(mtk_nanohub_trace);
+
+static struct driver_attribute *mtk_nanohub_attrs[] = {
+ &driver_attr_mtk_nanohub_trace,
+};
+
+static int mtk_nanohub_create_attr(struct device_driver *driver)
+{
+ int idx = 0, err = 0;
+ int num = (int)(ARRAY_SIZE(mtk_nanohub_attrs));
+
+ if (driver == NULL)
+ return -EINVAL;
+
+ for (idx = 0; idx < num; idx++) {
+ err = driver_create_file(driver, mtk_nanohub_attrs[idx]);
+ if (err) {
+ pr_err("driver_create_file (%s) = %d\n",
+ mtk_nanohub_attrs[idx]->attr.name, err);
+ break;
+ }
+ }
+ return err;
+}
+
+static int mtk_nanohub_delete_attr(struct device_driver *driver)
+{
+ int idx = 0, err = 0;
+ int num = (int)(ARRAY_SIZE(mtk_nanohub_attrs));
+
+ if (!driver)
+ return -EINVAL;
+
+ for (idx = 0; idx < num; idx++)
+ driver_remove_file(driver, mtk_nanohub_attrs[idx]);
+
+ return err;
+}
+
+static int mtk_nanohub_probe(struct platform_device *pdev)
+{
+ int err = 0, index;
+ struct mtk_nanohub_device *device;
+ struct task_struct *task = NULL, *task_power_reset = NULL;
+ struct sched_param param = { .sched_priority = MAX_RT_PRIO - 1 };
+
+ mtk_nanohub_init_sensor_state();
+ device = kzalloc(sizeof(*device), GFP_KERNEL);
+ if (!device) {
+ err = -ENOMEM;
+ goto exit;
+ }
+ mtk_nanohub_dev = device;
+ /* init sensor share dram write pointer event queue */
+ spin_lock_init(&device->wp_queue.buffer_lock);
+ device->wp_queue.head = 0;
+ device->wp_queue.tail = 0;
+ device->wp_queue.bufsize = 32;
+ device->wp_queue.ringbuffer =
+ vzalloc(device->wp_queue.bufsize * sizeof(uint32_t));
+ if (!device->wp_queue.ringbuffer) {
+ err = -ENOMEM;
+ goto exit_kfree;
+ }
+ /* init the debug trace flag */
+ for (index = 0; index < ID_SENSOR_MAX; index++)
+ atomic_set(&device->traces[index], 0);
+ /* init scp boot flags */
+ atomic_set(&device->get_list_first_boot, 0);
+ atomic_set(&device->cfg_data_after_reboot, 0);
+ atomic_set(&device->start_timesync_first_boot, 0);
+ /* init timestamp sync worker */
+ INIT_WORK(&device->sync_time_worker, mtk_nanohub_sync_time_work);
+ device->sync_time_timer.expires =
+ jiffies + msecs_to_jiffies(SYNC_TIME_START_CYCLC);
+ device->sync_time_timer.function = mtk_nanohub_sync_time_func;
+ init_timer(&device->sync_time_timer);
+ /* init wakeup source */
+ wakeup_source_init(&device->time_sync_wakeup_src, "sync_time");
+ wakeup_source_init(&device->data_notify_wakeup_src, "data_notify");
+ /* init nanohub ipi */
+ mtk_nanohub_ipi_init();
+ /* register ipi interrupt handler */
+ scp_ipi_registration(IPI_SENSOR,
+ mtk_nanohub_ipi_handler, "mtk_nanohub");
+ /* this call back can get scp power down status */
+ scp_A_register_notify(&mtk_nanohub_ready_notifier);
+ /* init data path */
+ WRITE_ONCE(chre_kthread_wait_condition, false);
+ task = kthread_run(mtk_nanohub_direct_push_work,
+ NULL, "chre_kthread");
+ if (IS_ERR(task)) {
+ pr_err("mtk_nanohub_direct_push_work create fail!\n");
+ goto exit_scp;
+ }
+ sched_setscheduler(task, SCHED_FIFO, ¶m);
+ /* this call back can get scp power UP status */
+ task_power_reset = kthread_run(mtk_nanohub_power_up_work,
+ NULL, "scp_power_reset");
+ if (IS_ERR(task_power_reset)) {
+ pr_err("mtk_nanohub_power_up_work create fail!\n");
+ goto exit_scp;
+ }
+ err = mtk_nanohub_create_attr(pdev->dev.driver);
+ if (err < 0) {
+ pr_err("create attribute err\n");
+ goto exit_scp;
+ }
+ err = register_pm_notifier(&mtk_nanohub_pm_notifier_func);
+ if (err < 0) {
+ pr_err("Failed to register PM notifier.\n");
+ goto exit_attr;
+ }
+ /* create mamanger last */
+ err = mtk_nanohub_create_manager();
+ if (err < 0) {
+ pr_err("Failed to create manager.\n");
+ goto exit_manager;
+ }
+
+ pr_info("init done, data_unit_t:%d, SCP_SENSOR_HUB_DATA:%d\n",
+ (int)sizeof(struct data_unit_t),
+ (int)sizeof(union SCP_SENSOR_HUB_DATA));
+ BUG_ON(sizeof(struct data_unit_t) != SENSOR_DATA_SIZE
+ || sizeof(union SCP_SENSOR_HUB_DATA) != SENSOR_IPI_SIZE);
+ return 0;
+
+exit_manager:
+ unregister_pm_notifier(&mtk_nanohub_pm_notifier_func);
+exit_attr:
+ mtk_nanohub_delete_attr(pdev->dev.driver);
+exit_scp:
+ scp_A_unregister_notify(&mtk_nanohub_ready_notifier);
+ scp_ipi_unregistration(IPI_SENSOR);
+ vfree(device->wp_queue.ringbuffer);
+exit_kfree:
+ kfree(device);
+exit:
+ pr_err("%s: err = %d\n", __func__, err);
+ return err;
+}
+
+static int mtk_nanohub_remove(struct platform_device *pdev)
+{
+ struct mtk_nanohub_device *device = mtk_nanohub_dev;
+
+ del_timer_sync(&device->sync_time_timer);
+ hf_manager_destroy(device->hf_dev.manager);
+ unregister_pm_notifier(&mtk_nanohub_pm_notifier_func);
+ mtk_nanohub_delete_attr(pdev->dev.driver);
+ scp_A_unregister_notify(&mtk_nanohub_ready_notifier);
+ scp_ipi_unregistration(IPI_SENSOR);
+ vfree(device->wp_queue.ringbuffer);
+ kfree(device);
+ return 0;
+}
+
+static void mtk_nanohub_shutdown(struct platform_device *pdev)
+{
+ int id = 0;
+ uint8_t sensor_type;
+ struct ConfigCmd cmd;
+ int ret = 0;
+
+ mutex_lock(&mSensorState_mtx);
+ for (id = 0; id < ID_SENSOR_MAX; id++) {
+ sensor_type = id_to_type(id);
+ if (mSensorState[sensor_type].sensorType &&
+ mSensorState[sensor_type].enable) {
+ mSensorState[sensor_type].enable = false;
+ init_sensor_config_cmd(&cmd, sensor_type);
+
+ ret = nanohub_external_write((const uint8_t *)&cmd,
+ sizeof(struct ConfigCmd));
+ if (ret < 0)
+ pr_notice("failed registerlistener [%d,%d]\n",
+ id, cmd.cmd);
+ }
+ }
+ mutex_unlock(&mSensorState_mtx);
+}
+
+static struct platform_device mtk_nanohub_pdev = {
+ .name = "mtk_nanohub",
+ .id = -1,
+};
+
+static struct platform_driver mtk_nanohub_pdrv = {
+ .driver = {
+ .name = "mtk_nanohub",
+ },
+ .probe = mtk_nanohub_probe,
+ .remove = mtk_nanohub_remove,
+ .shutdown = mtk_nanohub_shutdown,
+};
+
+static int __init mtk_nanohub_init(void)
+{
+ if (platform_device_register(&mtk_nanohub_pdev)) {
+ pr_err("mtk_nanohub platform device error\n");
+ return -1;
+ }
+ if (platform_driver_register(&mtk_nanohub_pdrv)) {
+ pr_err("mtk_nanohub platform driver error\n");
+ return -1;
+ }
+ return 0;
+}
+
+static void __exit mtk_nanohub_exit(void)
+{
+}
+
+module_init(mtk_nanohub_init);
+module_exit(mtk_nanohub_exit);
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("mtk_nanohub driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.h
new file mode 100644
index 0000000..a3d61a2
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.h
@@ -0,0 +1,522 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef SCP_SENSOR_HUB_H
+#define SCP_SENSOR_HUB_H
+
+#include <linux/ioctl.h>
+
+#define EVT_NO_SENSOR_CONFIG_EVENT 0x00000300
+#define SENSOR_RATE_ONCHANGE 0xFFFFFF01UL
+#define SENSOR_RATE_ONESHOT 0xFFFFFF02UL
+
+enum {
+ CONFIG_CMD_DISABLE = 0,
+ CONFIG_CMD_ENABLE = 1,
+ CONFIG_CMD_FLUSH = 2,
+ CONFIG_CMD_CFG_DATA = 3,
+ CONFIG_CMD_CALIBRATE = 4,
+ CONFIG_CMD_SELF_TEST = 5,
+};
+
+struct ConfigCmd {
+ uint32_t evtType;
+ uint64_t latency;
+ uint32_t rate;
+ uint8_t sensorType;
+ uint8_t cmd;
+ uint16_t flags;
+ uint8_t data[];
+} __packed;
+
+struct SensorState {
+ uint64_t latency;
+ uint32_t rate;
+ uint8_t sensorType;
+ uint8_t alt;
+ bool enable;
+ bool timestamp_filter;
+ atomic_t flushCnt;
+};
+
+#define SCP_SENSOR_HUB_SUCCESS 0
+#define SCP_SENSOR_HUB_FAILURE (-1)
+
+#define SCP_SENSOR_HUB_X 0
+#define SCP_SENSOR_HUB_Y 1
+#define SCP_SENSOR_HUB_Z 2
+#define SCP_SENSOR_HUB_AXES_NUM 3
+
+/* SCP_ACTION */
+#define SENSOR_HUB_ACTIVATE 0
+#define SENSOR_HUB_SET_DELAY 1
+#define SENSOR_HUB_GET_DATA 2
+#define SENSOR_HUB_BATCH 3
+#define SENSOR_HUB_SET_CONFIG 4
+#define SENSOR_HUB_SET_CUST 5
+#define SENSOR_HUB_NOTIFY 6
+#define SENSOR_HUB_BATCH_TIMEOUT 7
+#define SENSOR_HUB_SET_TIMESTAMP 8
+#define SENSOR_HUB_POWER_NOTIFY 9
+#define SENSOR_HUB_RAW_DATA 10
+
+/* SCP_NOTIFY EVENT */
+#define SCP_INIT_DONE 0
+#define SCP_FIFO_FULL 1
+#define SCP_NOTIFY 2
+#define SCP_BATCH_TIMEOUT 3
+#define SCP_DIRECT_PUSH 4
+
+enum {
+ SENSOR_POWER_UP = 0,
+ SENSOR_POWER_DOWN,
+};
+
+struct sensor_vec_t {
+ union {
+ struct {
+ int32_t x;
+ int32_t y;
+ int32_t z;
+ int32_t x_bias;
+ int32_t y_bias;
+ int32_t z_bias;
+ int32_t reserved : 14;
+ int32_t temp_result : 2;
+ int32_t temperature : 16;
+ };
+ struct {
+ int32_t azimuth;
+ int32_t pitch;
+ int32_t roll;
+ int32_t scalar;
+ };
+ };
+ uint32_t status;
+};
+
+struct heart_rate_event_t {
+ int32_t bpm;
+ int32_t status;
+};
+
+struct significant_motion_event_t {
+ int32_t state;
+};
+
+struct step_counter_event_t {
+ uint32_t accumulated_step_count;
+};
+
+struct step_detector_event_t {
+ uint32_t step_detect;
+};
+
+struct floor_counter_event_t {
+ uint32_t accumulated_floor_count;
+};
+
+enum gesture_type_t {
+ GESTURE_NONE,
+ SHAKE,
+ TAP,
+ TWIST,
+ FLIP,
+ SNAPSHOT,
+ PICKUP,
+ CHECK
+};
+
+struct gesture_t {
+ int32_t probability;
+};
+
+struct pedometer_event_t {
+ uint32_t accumulated_step_count;
+ uint32_t accumulated_step_length;
+ uint32_t step_frequency;
+ uint32_t step_length;
+};
+
+struct pressure_vec_t {
+ int32_t pressure; /* Pa, i.e. hPa * 100 */
+ int32_t temperature;
+ uint32_t status;
+};
+
+struct proximity_vec_t {
+ uint32_t steps;
+ int32_t oneshot;
+};
+
+struct relative_humidity_vec_t {
+ int32_t relative_humidity;
+ int32_t temperature;
+ uint32_t status;
+};
+
+struct sleepmonitor_event_t {
+ int32_t state; /* sleep, restless, awake */
+};
+
+enum fall_type {
+ FALL_NONE,
+ FALL,
+ FLOP,
+ FALL_MAX
+};
+
+struct fall_t {
+ uint8_t probability[FALL_MAX]; /* 0~100 */
+};
+
+struct tilt_event_t {
+ int32_t state; /* 0,1 */
+};
+
+struct in_pocket_event_t {
+ int32_t state; /* 0,1 */
+};
+
+struct geofence_event_t {
+ uint32_t state; /* geofence [source, result, operation_mode] */
+};
+
+struct sar_event_t {
+ struct {
+ int32_t data[3];
+ int32_t x_bias;
+ int32_t y_bias;
+ int32_t z_bias;
+ };
+ uint32_t status;
+};
+
+enum activity_type_t {
+ STILL,
+ STANDING,
+ SITTING,
+ LYING,
+ ON_FOOT,
+ WALKING,
+ RUNNING,
+ CLIMBING,
+ ON_BICYCLE,
+ IN_VEHICLE,
+ TILTING,
+ UNKNOWN,
+ ACTIVITY_MAX
+};
+
+struct activity_t {
+ uint8_t probability[ACTIVITY_MAX]; /* 0~100 */
+};
+
+struct data_unit_t {
+ uint8_t sensor_type;
+ uint8_t flush_action;
+ uint8_t reserve[2];
+ uint64_t time_stamp;
+ union {
+ struct sensor_vec_t accelerometer_t;
+ struct sensor_vec_t gyroscope_t;
+ struct sensor_vec_t magnetic_t;
+ struct sensor_vec_t orientation_t;
+ struct sensor_vec_t pdr_event;
+
+ int32_t light;
+ struct proximity_vec_t proximity_t;
+ int32_t temperature;
+ struct pressure_vec_t pressure_t;
+ struct relative_humidity_vec_t relative_humidity_t;
+
+ struct sensor_vec_t uncalibrated_acc_t;
+ struct sensor_vec_t uncalibrated_mag_t;
+ struct sensor_vec_t uncalibrated_gyro_t;
+
+ struct pedometer_event_t pedometer_t;
+
+ struct heart_rate_event_t heart_rate_t;
+ struct significant_motion_event_t smd_t;
+ struct step_detector_event_t step_detector_t;
+ struct step_counter_event_t step_counter_t;
+ struct floor_counter_event_t floor_counter_t;
+ struct activity_t activity_data_t;
+ struct gesture_t gesture_data_t;
+ struct fall_t fall_data_t;
+ struct tilt_event_t tilt_event;
+ struct in_pocket_event_t inpocket_event;
+ struct geofence_event_t geofence_data_t;
+ struct sar_event_t sar_event;
+ int32_t data[8];
+ };
+} __packed;
+
+struct sensor_fifo {
+ uint32_t rp;
+ uint32_t wp;
+ uint32_t fifo_size;
+ uint32_t reserve;
+ struct data_unit_t data[0];
+};
+
+struct SCP_SENSOR_HUB_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t reserve[2];
+ uint32_t data[11];
+};
+
+struct SCP_SENSOR_HUB_RSP {
+ uint8_t sensorType;
+ uint8_t action;
+ int8_t errCode;
+ uint8_t reserve[1];
+ /* uint32_t reserved[9]; */
+};
+
+struct SCP_SENSOR_HUB_ACTIVATE_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t reserve[2];
+ uint32_t enable; /* 0 : disable ; 1 : enable */
+ /* uint32_t reserved[9]; */
+};
+
+#define SCP_SENSOR_HUB_ACTIVATE_RSP SCP_SENSOR_HUB_RSP
+/* typedef SCP_SENSOR_HUB_RSP SCP_SENSOR_HUB_ACTIVATE_RSP; */
+
+struct SCP_SENSOR_HUB_SET_DELAY_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t reserve[2];
+ uint32_t delay; /* ms */
+ /* uint32_t reserved[9]; */
+};
+
+#define SCP_SENSOR_HUB_SET_DELAY_RSP SCP_SENSOR_HUB_RSP
+/* typedef SCP_SENSOR_HUB_RSP SCP_SENSOR_HUB_SET_DELAY_RSP; */
+
+struct SCP_SENSOR_HUB_GET_DATA_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t reserve[2];
+ /* uint32_t reserved[10]; */
+};
+
+struct SCP_SENSOR_HUB_GET_DATA_RSP {
+ uint8_t sensorType;
+ uint8_t action;
+ int8_t errCode;
+ uint8_t reserve[1];
+ /* struct data_unit_t data_t; */
+ union {
+ int8_t int8_Data[0];
+ int16_t int16_Data[0];
+ int32_t int32_Data[0];
+ } data;
+};
+
+struct SCP_SENSOR_HUB_BATCH_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t flag;
+ uint8_t reserve[1];
+ uint32_t period_ms; /* batch reporting time in ms */
+ uint32_t timeout_ms; /* sampling time in ms */
+ /* uint32_t reserved[7]; */
+};
+
+#define SCP_SENSOR_HUB_BATCH_RSP SCP_SENSOR_HUB_RSP
+/* typedef SCP_SENSOR_HUB_RSP SCP_SENSOR_HUB_BATCH_RSP; */
+
+struct SCP_SENSOR_HUB_SET_CONFIG_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t reserve[2];
+ /* struct sensorFIFO *bufferBase; */
+ uint32_t bufferBase;/* use int to store buffer DRAM base LSB 32 bits */
+ uint32_t bufferSize;
+ uint64_t ap_timestamp;
+ uint64_t arch_counter;
+ /* uint32_t reserved[8]; */
+};
+
+#define SCP_SENSOR_HUB_SET_CONFIG_RSP SCP_SENSOR_HUB_RSP
+/* typedef SCP_SENSOR_HUB_RSP SCP_SENSOR_HUB_SET_CONFIG_RSP; */
+
+enum CUST_ACTION {
+ CUST_ACTION_SET_CUST = 1,
+ CUST_ACTION_SET_CALI,
+ CUST_ACTION_RESET_CALI,
+ CUST_ACTION_SET_TRACE,
+ CUST_ACTION_SET_DIRECTION,
+ CUST_ACTION_SHOW_REG,
+ CUST_ACTION_GET_RAW_DATA,
+ CUST_ACTION_SET_PS_THRESHOLD,
+ CUST_ACTION_SHOW_ALSLV,
+ CUST_ACTION_SHOW_ALSVAL,
+ CUST_ACTION_SET_FACTORY,
+ CUST_ACTION_GET_SENSOR_INFO,
+};
+
+struct SCP_SENSOR_HUB_CUST {
+ enum CUST_ACTION action;
+};
+
+struct SCP_SENSOR_HUB_SET_CUST {
+ enum CUST_ACTION action;
+ int32_t data[0];
+};
+
+struct SCP_SENSOR_HUB_SET_TRACE {
+ enum CUST_ACTION action;
+ int trace;
+};
+
+struct SCP_SENSOR_HUB_SET_DIRECTION {
+ enum CUST_ACTION action;
+ int direction;
+};
+
+struct SCP_SENSOR_HUB_SET_FACTORY {
+ enum CUST_ACTION action;
+ unsigned int factory;
+};
+
+struct SCP_SENSOR_HUB_SET_CALI {
+ enum CUST_ACTION action;
+ union {
+ int8_t int8_data[0];
+ uint8_t uint8_data[0];
+ int16_t int16_data[0];
+ uint16_t uint16_data[0];
+ int32_t int32_data[0];
+ uint32_t uint32_data[SCP_SENSOR_HUB_AXES_NUM];
+ };
+};
+
+#define SCP_SENSOR_HUB_RESET_CALI SCP_SENSOR_HUB_CUST
+/* typedef SCP_SENSOR_HUB_CUST SCP_SENSOR_HUB_RESET_CALI; */
+
+struct SCP_SENSOR_HUB_SETPS_THRESHOLD {
+ enum CUST_ACTION action;
+ int32_t threshold[2];
+};
+
+#define SCP_SENSOR_HUB_SHOW_REG SCP_SENSOR_HUB_CUST
+#define SCP_SENSOR_HUB_SHOW_ALSLV SCP_SENSOR_HUB_CUST
+#define SCP_SENSOR_HUB_SHOW_ALSVAL SCP_SENSOR_HUB_CUST
+/*
+ * typedef SCP_SENSOR_HUB_CUST SCP_SENSOR_HUB_SHOW_REG;
+ * typedef SCP_SENSOR_HUB_CUST SCP_SENSOR_HUB_SHOW_ALSLV;
+ * typedef SCP_SENSOR_HUB_CUST SCP_SENSOR_HUB_SHOW_ALSVAL;
+ */
+
+struct SCP_SENSOR_HUB_GET_RAW_DATA {
+ enum CUST_ACTION action;
+ union {
+ int8_t int8_data[0];
+ uint8_t uint8_data[0];
+ int16_t int16_data[0];
+ uint16_t uint16_data[0];
+ int32_t int32_data[0];
+ uint32_t uint32_data[SCP_SENSOR_HUB_AXES_NUM];
+ };
+};
+
+struct mag_dev_info_t {
+ char libname[16];
+ int8_t layout;
+ int8_t deviceid;
+};
+
+struct sensorInfo_t {
+ char name[16];
+ struct mag_dev_info_t mag_dev_info;
+};
+
+struct scp_sensor_hub_get_sensor_info {
+ enum CUST_ACTION action;
+ union {
+ int32_t int32_data[0];
+ struct sensorInfo_t sensorInfo;
+ };
+};
+
+enum {
+ USE_OUT_FACTORY_MODE = 0,
+ USE_IN_FACTORY_MODE
+};
+
+struct SCP_SENSOR_HUB_SET_CUST_REQ {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t reserve[2];
+ union {
+ uint32_t custData[11];
+ struct SCP_SENSOR_HUB_CUST cust;
+ struct SCP_SENSOR_HUB_SET_CUST setCust;
+ struct SCP_SENSOR_HUB_SET_CALI setCali;
+ struct SCP_SENSOR_HUB_RESET_CALI resetCali;
+ struct SCP_SENSOR_HUB_SET_TRACE setTrace;
+ struct SCP_SENSOR_HUB_SET_DIRECTION setDirection;
+ struct SCP_SENSOR_HUB_SHOW_REG showReg;
+ struct SCP_SENSOR_HUB_GET_RAW_DATA getRawData;
+ struct SCP_SENSOR_HUB_SETPS_THRESHOLD setPSThreshold;
+ struct SCP_SENSOR_HUB_SHOW_ALSLV showAlslv;
+ struct SCP_SENSOR_HUB_SHOW_ALSVAL showAlsval;
+ struct SCP_SENSOR_HUB_SET_FACTORY setFactory;
+ struct scp_sensor_hub_get_sensor_info getInfo;
+ };
+};
+
+struct SCP_SENSOR_HUB_SET_CUST_RSP {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t errCode;
+ uint8_t reserve[1];
+ union {
+ uint32_t custData[11];
+ struct SCP_SENSOR_HUB_GET_RAW_DATA getRawData;
+ struct scp_sensor_hub_get_sensor_info getInfo;
+ };
+};
+
+struct SCP_SENSOR_HUB_NOTIFY_RSP {
+ uint8_t sensorType;
+ uint8_t action;
+ uint8_t event;
+ uint8_t reserve[1];
+ union {
+ int8_t int8_Data[0];
+ int16_t int16_Data[0];
+ int32_t int32_Data[0];
+ struct {
+ uint32_t currWp;
+ uint64_t scp_timestamp;
+ uint64_t arch_counter;
+ };
+ };
+};
+
+union SCP_SENSOR_HUB_DATA {
+ struct SCP_SENSOR_HUB_REQ req;
+ struct SCP_SENSOR_HUB_RSP rsp;
+ struct SCP_SENSOR_HUB_ACTIVATE_REQ activate_req;
+ struct SCP_SENSOR_HUB_ACTIVATE_RSP activate_rsp;
+ struct SCP_SENSOR_HUB_SET_DELAY_REQ set_delay_req;
+ struct SCP_SENSOR_HUB_SET_DELAY_RSP set_delay_rsp;
+ struct SCP_SENSOR_HUB_GET_DATA_REQ get_data_req;
+ struct SCP_SENSOR_HUB_GET_DATA_RSP get_data_rsp;
+ struct SCP_SENSOR_HUB_BATCH_REQ batch_req;
+ struct SCP_SENSOR_HUB_BATCH_RSP batch_rsp;
+ struct SCP_SENSOR_HUB_SET_CONFIG_REQ set_config_req;
+ struct SCP_SENSOR_HUB_SET_CONFIG_RSP set_config_rsp;
+ struct SCP_SENSOR_HUB_SET_CUST_REQ set_cust_req;
+ struct SCP_SENSOR_HUB_SET_CUST_RSP set_cust_rsp;
+ struct SCP_SENSOR_HUB_NOTIFY_RSP notify_rsp;
+};
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.c
new file mode 100644
index 0000000..a84787b
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.c
@@ -0,0 +1,254 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "[mtk_nanohub_ipi] " fmt
+
+#include <linux/module.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+
+#include "mtk_nanohub_ipi.h"
+#include "scp_ipi.h"
+#include "scp_helper.h"
+#include "scp_excep.h"
+
+enum scp_ipi_status __attribute__((weak)) scp_ipi_send(enum ipi_id id,
+ void *buf, unsigned int len,
+ unsigned int wait, enum scp_core_id scp_id)
+{
+ return SCP_IPI_ERROR;
+}
+
+struct ipi_hw_master {
+ spinlock_t lock;
+ bool running;
+ struct list_head head;
+ struct workqueue_struct *workqueue;
+ struct work_struct work;
+};
+
+struct ipi_hw_transfer {
+ struct completion done;
+ int count;
+ /* data buffers */
+ const unsigned char *tx;
+ unsigned char *rx;
+ unsigned int tx_len;
+ unsigned int rx_len;
+ void *context;
+};
+
+static struct ipi_hw_master hw_master;
+static struct ipi_hw_transfer hw_transfer;
+static DEFINE_SPINLOCK(hw_transfer_lock);
+
+static int ipi_txrx_bufs(struct ipi_transfer *t)
+{
+ int status = 0, retry = 0;
+ int timeout;
+ unsigned long flags;
+ struct ipi_hw_transfer *hw = &hw_transfer;
+
+ spin_lock_irqsave(&hw_transfer_lock, flags);
+ hw->tx = t->tx_buf;
+ hw->rx = t->rx_buf;
+ hw->tx_len = t->tx_len;
+ hw->rx_len = t->rx_len;
+
+ init_completion(&hw->done);
+ hw->context = &hw->done;
+ spin_unlock_irqrestore(&hw_transfer_lock, flags);
+ do {
+ status = scp_ipi_send(IPI_SENSOR,
+ (unsigned char *)hw->tx, hw->tx_len, 0, SCP_A_ID);
+ if (status == SCP_IPI_ERROR) {
+ pr_err("scp_ipi_send fail\n");
+ return -1;
+ }
+ if (status == SCP_IPI_BUSY) {
+ if (retry++ == 1000) {
+ pr_err("retry fail\n");
+ return -1;
+ }
+ if (retry % 100 == 0)
+ usleep_range(1000, 2000);
+ }
+ } while (status == SCP_IPI_BUSY);
+
+ if (retry >= 100)
+ pr_debug("retry time:%d\n", retry);
+
+ timeout = wait_for_completion_timeout(&hw->done,
+ msecs_to_jiffies(500));
+ spin_lock_irqsave(&hw_transfer_lock, flags);
+ if (!timeout) {
+ pr_err("transfer timeout!");
+ hw->count = -1;
+ }
+ hw->context = NULL;
+ spin_unlock_irqrestore(&hw_transfer_lock, flags);
+ return hw->count;
+}
+
+static void ipi_complete(void *arg)
+{
+ complete(arg);
+}
+
+static void ipi_transfer_messages(void)
+{
+ struct ipi_message *m;
+ struct ipi_transfer *t = NULL;
+ int status = 0;
+ unsigned long flags;
+
+ spin_lock_irqsave(&hw_master.lock, flags);
+ if (list_empty(&hw_master.head) || hw_master.running)
+ goto out;
+ hw_master.running = true;
+ while (!list_empty(&hw_master.head)) {
+ m = list_first_entry(&hw_master.head,
+ struct ipi_message, list);
+ list_del(&m->list);
+ spin_unlock_irqrestore(&hw_master.lock, flags);
+ list_for_each_entry(t, &m->transfers, transfer_list) {
+ if (!t->tx_buf && t->tx_len) {
+ status = -EINVAL;
+ pr_err("transfer param wrong :%d\n",
+ status);
+ break;
+ }
+ if (t->tx_len)
+ status = ipi_txrx_bufs(t);
+ if (status < 0) {
+ status = -EREMOTEIO;
+ /* pr_err("transfer err :%d\n", status); */
+ break;
+ } else if (status != t->rx_len) {
+ pr_err("ack err :%d %d\n", status, t->rx_len);
+ status = -EREMOTEIO;
+ break;
+ }
+ status = 0;
+ }
+ m->status = status;
+ m->complete(m->context);
+ spin_lock_irqsave(&hw_master.lock, flags);
+ }
+ hw_master.running = false;
+out:
+ spin_unlock_irqrestore(&hw_master.lock, flags);
+}
+
+static void ipi_prefetch_messages(void)
+{
+ ipi_transfer_messages();
+}
+
+static void ipi_work(struct work_struct *work)
+{
+ ipi_transfer_messages();
+}
+
+static int __ipi_transfer(struct ipi_message *m)
+{
+ unsigned long flags;
+
+ m->status = -EINPROGRESS;
+
+ spin_lock_irqsave(&hw_master.lock, flags);
+ list_add_tail(&m->list, &hw_master.head);
+ queue_work(hw_master.workqueue, &hw_master.work);
+ spin_unlock_irqrestore(&hw_master.lock, flags);
+ return 0;
+}
+
+static int __ipi_xfer(struct ipi_message *message)
+{
+ DECLARE_COMPLETION_ONSTACK(done);
+ int status;
+
+ message->complete = ipi_complete;
+ message->context = &done;
+
+ status = __ipi_transfer(message);
+
+ if (status == 0) {
+ ipi_prefetch_messages();
+ wait_for_completion(&done);
+ status = message->status;
+ }
+ message->context = NULL;
+ return status;
+}
+
+static int ipi_sync(const unsigned char *txbuf, unsigned int n_tx,
+ unsigned char *rxbuf, unsigned int n_rx)
+{
+ struct ipi_transfer t;
+ struct ipi_message m;
+
+ t.tx_buf = txbuf;
+ t.tx_len = n_tx;
+ t.rx_buf = rxbuf;
+ t.rx_len = n_rx;
+
+ ipi_message_init(&m);
+ ipi_message_add_tail(&t, &m);
+
+ return __ipi_xfer(&m);
+}
+
+static int ipi_async(struct ipi_message *m)
+{
+ return __ipi_transfer(m);
+}
+
+int mtk_nanohub_ipi_sync(unsigned char *buffer, unsigned int len)
+{
+ return ipi_sync(buffer, len, buffer, len);
+}
+
+int mtk_nanohub_ipi_async(struct ipi_message *m)
+{
+ return ipi_async(m);
+}
+
+void mtk_nanohub_ipi_complete(unsigned char *buffer, unsigned int len)
+{
+ struct ipi_hw_transfer *hw = &hw_transfer;
+
+ spin_lock(&hw_transfer_lock);
+ if (!hw->context) {
+ pr_err("after ipi timeout ack occur then dropped this\n");
+ goto out;
+ }
+ /* only copy hw->rx_len bytes to hw->rx to avoid memory corruption */
+ memcpy(hw->rx, buffer, hw->rx_len);
+ /* hw->count give real len */
+ hw->count = len;
+ complete(hw->context);
+out:
+ spin_unlock(&hw_transfer_lock);
+}
+
+int mtk_nanohub_ipi_init(void)
+{
+ INIT_WORK(&hw_master.work, ipi_work);
+ INIT_LIST_HEAD(&hw_master.head);
+ spin_lock_init(&hw_master.lock);
+ hw_master.workqueue = create_singlethread_workqueue("ipi_master");
+ if (hw_master.workqueue == NULL) {
+ pr_err("workqueue fail\n");
+ return -1;
+ }
+
+ return 0;
+}
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("mtk_nanohub_ipi driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.h
new file mode 100644
index 0000000..ceadca5
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef _MTK_NANOHUB_IPI_H_
+#define _MTK_NANOHUB_IPI_H_
+
+#include <linux/list.h>
+
+struct ipi_transfer {
+ const unsigned char *tx_buf;
+ unsigned char *rx_buf;
+ unsigned int tx_len;
+ unsigned int rx_len;
+ struct list_head transfer_list;
+};
+
+struct ipi_message {
+ struct list_head transfers;
+ struct list_head list;
+ void *context;
+ int status;
+ void (*complete)(void *context);
+};
+
+static inline void ipi_message_init(struct ipi_message *m)
+{
+ memset(m, 0, sizeof(*m));
+ INIT_LIST_HEAD(&m->transfers);
+}
+
+static inline void ipi_message_add_tail(struct ipi_transfer *t,
+ struct ipi_message *m)
+{
+ list_add_tail(&t->transfer_list, &m->transfers);
+}
+
+int mtk_nanohub_ipi_sync(unsigned char *buffer, unsigned int len);
+int mtk_nanohub_ipi_async(struct ipi_message *m);
+void mtk_nanohub_ipi_complete(unsigned char *buffer, unsigned int len);
+int mtk_nanohub_ipi_init(void);
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/Makefile
new file mode 100644
index 0000000..68a73f0
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/Makefile
@@ -0,0 +1,3 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core/
+
+obj-y += smi130_i2c.o
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/smi130_i2c.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/smi130_i2c.c
new file mode 100644
index 0000000..53f1813
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/smi130_i2c.c
@@ -0,0 +1,741 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+#include <linux/math64.h>
+#include <linux/atomic.h>
+
+#include "hf_manager.h"
+#include "smi130_i2c.h"
+#include "sensor_list.h"
+
+#define GYRO_TEMP_AMP 100
+#define SMI130_I2C_NAME "smi130_i2c"
+#define GYRO_ADDR 0x68
+#define SMI130_AXES_NUM 3
+#define C_I2C_FIFO_SIZE 8
+
+#define SMI130_SOFT_RESET_VALUE 0xB6
+#define SMI130_ACC_CHIP_ID_VALUE (0xfa)
+#define SMI130_GYRO_CHIP_ID_VALUE (0x0f)
+#define CHECK_CHIP_ID_TIME_MAX 5
+
+#define SMI130_ACC_ODR_7HZ 0x08
+#define SMI130_ACC_ODR_15HZ 0x09
+#define SMI130_ACC_ODR_31HZ 0x0A
+#define SMI130_ACC_ODR_62HZ 0x0B
+#define SMI130_ACC_ODR_125HZ 0x0C
+#define SMI130_ACC_ODR_250HZ 0x0D
+#define SMI130_ACC_ODR_500HZ 0x0E
+#define SMI130_ACC_ODR_1000HZ 0x0F
+#define SMI130_ACC_RANGE_4G 0x03
+#define SMI130_ACC_RANGE_8G 0x05
+#define SMI130_ACC_RANGE_16G 0x08
+#define SMI130_ACC_RANGE_32G 0x0C
+
+#define SMI130_GYRO_ODR_32HZ 0x07
+#define SMI130_GYRO_ODR_64HZ 0x06
+#define SMI130_GYRO_ODR_12HZ 0x05
+#define SMI130_GYRO_ODR_23HZ 0x04
+#define SMI130_GYRO_ODR_47HZ 0x03
+#define SMI130_GYRO_ODR_116HZ 0x02
+#define SMI130_GYRO_ODR_230HZ 0x01
+#define SMI130_GYRO_ODR_523HZ 0x00
+#define SMI130_GYRO_RANGE_2000 0x00
+#define SMI130_GYRO_RANGE_1000 0x01
+#define SMI130_GYRO_RANGE_500 0x02
+#define SMI130_GYRO_RANGE_250 0x03
+#define SMI130_GYRO_RANGE_125 0x04
+
+static unsigned char support_sensors[] = {
+ SENSOR_TYPE_ACCELEROMETER,
+ SENSOR_TYPE_GYROSCOPE,
+ SENSOR_TYPE_GYRO_TEMPERATURE,
+};
+
+struct smi130_device {
+ struct hf_device hf_dev;
+ struct i2c_client *acc_client;
+ struct i2c_client *gyro_client;
+ uint32_t direction;
+ uint8_t placement[3];
+ atomic_t raw_acc_enable;
+ atomic_t raw_gyro_enable;
+ atomic_t acc_enable;
+ atomic_t gyro_enable;
+ atomic_t gyro_temp_enable;
+};
+
+/* I2C operation functions */
+static int smi130_i2c_read_block(struct i2c_client *client,
+ u8 addr, u8 *data, u8 len)
+{
+ int err = 0;
+ u8 beg = addr;
+ struct i2c_msg msgs[2] = {
+ {/*.addr = client->addr,*/
+ .flags = 0,
+ .len = 1,
+ .buf = &beg},
+ {
+ /*.addr = client->addr*/
+ .flags = I2C_M_RD,
+ .len = len,
+ .buf = data,
+ } };
+ if (!client)
+ return -EINVAL;
+ msgs[0].addr = client->addr;
+ msgs[1].addr = client->addr;
+
+ err = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+ if (err != 2) {
+ pr_err_ratelimited("i2c_trans err: %x %x (%d %p %d) %d\n",
+ msgs[0].addr, client->addr, addr, data, len, err);
+ err = -EIO;
+ } else {
+ err = 0; /*no error*/
+ }
+ return err;
+}
+
+static int smi130_i2c_write_block(struct i2c_client *client,
+ u8 addr, u8 *data, u8 len)
+{
+ /* because address also occupies one byte,
+ * the maximum length for write is 7 bytes
+ */
+ int err = 0, idx = 0, num = 0;
+ char buf[32];
+
+ if (!client)
+ return -EINVAL;
+ else if (len > C_I2C_FIFO_SIZE) {
+ pr_err_ratelimited("len %d fi %d\n", len, C_I2C_FIFO_SIZE);
+ return -EINVAL;
+ }
+ buf[num++] = addr;
+ for (idx = 0; idx < len; idx++)
+ buf[num++] = data[idx];
+
+ err = i2c_master_send(client, buf, num);
+ if (err < 0) {
+ pr_err_ratelimited("send command error!!\n");
+ return -EFAULT;
+ }
+
+ return err;
+}
+
+
+static int smi130_acc_init_device(struct i2c_client *client)
+{
+ int err = 0;
+ uint8_t data = 0;
+
+ data = SMI130_ACC_ODR_7HZ;
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_BW_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //filter and lock enable
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_RATED_HBW_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = SMI130_ACC_RANGE_16G;
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_RANGE_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //disable int
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_INT_ENABLE1_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x80; //map int
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_INT_MAP_1_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //push-pull, active low
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_INT_OUT_CTRL_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ pr_debug("SMI130_ACC init OK.\n");
+
+ return err;
+}
+
+static int smi130_acc_set_soft_reset(struct i2c_client *client)
+{
+ int err = 0;
+ uint8_t data = SMI130_SOFT_RESET_VALUE;
+
+ err = smi130_i2c_write_block(client,
+ SMI130_ACC_BGW_SOFTRESET_ADDR, &data, 1);
+ return err;
+}
+
+static int smi130_acc_check_chip_id(struct i2c_client *client)
+{
+ int err = -1;
+ u8 chip_id = 0;
+ u8 read_count = 0;
+
+ while (read_count++ < CHECK_CHIP_ID_TIME_MAX) {
+ smi130_i2c_read_block(client,
+ SMI130_ACC_CHIP_ID_ADDR, &chip_id, 1);
+
+ if ((chip_id & 0xff) != SMI130_ACC_CHIP_ID_VALUE) {
+ continue;
+ } else {
+ err = 0;
+ break;
+ }
+ }
+ return err;
+}
+
+static int smi130_gyro_init_device(struct i2c_client *client)
+{
+ int err = 0;
+ uint8_t data = 0;
+
+ data = SMI130_GYRO_ODR_32HZ;
+ err = smi130_i2c_write_block(client,
+ SMI130_GYRO_BW_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //filter and lock enable
+ err = smi130_i2c_write_block(client,
+ SMI130_GYRO_RATED_HBW_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = SMI130_GYRO_RANGE_2000;
+ err = smi130_i2c_write_block(client,
+ SMI130_GYRO_RANGE_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //disable int
+ err = smi130_i2c_write_block(client,
+ SMI130_GYRO_INT_ENABLE0_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x01; //map int
+ err = smi130_i2c_write_block(client,
+ SMI130_GYRO_INT_MAP_1_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ data = 0x00; //push-pull, active low
+ err = smi130_i2c_write_block(client,
+ SMI130_GYRO_INT_ENABLE1_ADDR, &data, 1);
+ if (err < 0)
+ return err;
+
+ pr_debug("SMI130 gyro init OK.\n");
+
+ return err;
+}
+
+static int smi130_gyro_check_chip_id(struct i2c_client *client)
+{
+ int err = -1;
+ u8 chip_id = 0;
+ u8 read_count = 0;
+
+ while (read_count++ < CHECK_CHIP_ID_TIME_MAX) {
+ smi130_i2c_read_block(client,
+ SMI130_GYRO_CHIP_ID_ADDR, &chip_id, 1);
+
+ if ((chip_id & 0xff) != SMI130_GYRO_CHIP_ID_VALUE) {
+ continue;
+ } else {
+ err = 0;
+ break;
+ }
+ }
+ return err;
+}
+
+int smi130_sample(struct hf_device *hfdev)
+{
+
+ int err = 0;
+ uint8_t buf[SMI130_AXES_NUM * 2] = {0};
+ int32_t data[SMI130_AXES_NUM] = {0};
+ int8_t gyro_temp[1] = {0};
+ struct hf_manager_event event;
+ int64_t current_time;
+ struct smi130_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+
+ current_time = ktime_get_boot_ns();
+ if (atomic_read(&driver_dev->acc_enable)) {
+ err = smi130_i2c_read_block(driver_dev->acc_client,
+ SMI130_ACC_RATE_X_LSB_ADDR,
+ &buf[0], SMI130_AXES_NUM * 2);
+ if (err < 0) {
+ pr_err_ratelimited("read fail\n");
+ return err;
+ }
+
+ data[0] = ((int16_t)(buf[0] | (buf[1] << 8))) >> 4;
+ data[1] = ((int16_t)(buf[2] | (buf[3] << 8))) >> 4;
+ data[2] = ((int16_t)(buf[4] | (buf[5] << 8))) >> 4;
+
+ coordinate_map(driver_dev->direction, data);
+
+ if (atomic_read(&driver_dev->raw_acc_enable)) {
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ACCELEROMETER;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = RAW_ACTION;
+ event.word[0] =
+ (int32_t)div64_s64((int64_t)data[0] * 9807,
+ 256);
+ event.word[1] =
+ (int32_t)div64_s64((int64_t)data[1] * 9807,
+ 256);
+ event.word[2] =
+ (int32_t)div64_s64((int64_t)data[2] * 9807,
+ 256);
+ manager->report(manager, &event);
+ }
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ACCELEROMETER;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = DATA_ACTION;
+ event.word[0] = (int32_t)div64_s64((int64_t)data[0] * 9807,
+ 256);
+ event.word[1] = (int32_t)div64_s64((int64_t)data[1] * 9807,
+ 256);
+ event.word[2] = (int32_t)div64_s64((int64_t)data[2] * 9807,
+ 256);
+ manager->report(manager, &event);
+ }
+
+ if (atomic_read(&driver_dev->gyro_enable)) {
+ err = smi130_i2c_read_block(driver_dev->gyro_client,
+ SMI130_GYRO_RATE_X_LSB_ADDR, &buf[0],
+ SMI130_AXES_NUM * 2);
+ if (err < 0) {
+ pr_err_ratelimited("read fail\n");
+ return err;
+ }
+
+ data[0] = (int16_t)(buf[0] | (buf[1] << 8));
+ data[1] = (int16_t)(buf[2] | (buf[3] << 8));
+ data[2] = (int16_t)(buf[4] | (buf[5] << 8));
+
+ coordinate_map(driver_dev->direction, data);
+
+ /* read gyro temp */
+ if (atomic_read(&driver_dev->gyro_temp_enable)) {
+ err = smi130_i2c_read_block(driver_dev->gyro_client,
+ SMI130_GYRO_TEMP_ADDR, &gyro_temp[0], 1);
+ if (err < 0) {
+ pr_err_ratelimited("read gyro temp fail\n");
+ return err;
+ }
+ }
+ if (atomic_read(&driver_dev->raw_gyro_enable)) {
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_GYROSCOPE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = RAW_ACTION;
+ event.word[0] =
+ (int32_t)div64_s64((int64_t)data[0] * 1310000,
+ 164);
+ event.word[1] =
+ (int32_t)div64_s64((int64_t)data[1] * 1310000,
+ 164);
+ event.word[2] =
+ (int32_t)div64_s64((int64_t)data[2] * 1310000,
+ 164);
+ manager->report(manager, &event);
+ }
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_GYROSCOPE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = DATA_ACTION;
+ event.word[0] = (int32_t)div64_s64((int64_t)data[0] * 1310000,
+ 164);
+ event.word[1] = (int32_t)div64_s64((int64_t)data[1] * 1310000,
+ 164);
+ event.word[2] = (int32_t)div64_s64((int64_t)data[2] * 1310000,
+ 164);
+ manager->report(manager, &event);
+
+ if (atomic_read(&driver_dev->gyro_temp_enable)) {
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_GYRO_TEMPERATURE;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.action = DATA_ACTION;
+ event.word[0] = (int32_t)gyro_temp[0] * GYRO_TEMP_AMP /
+ 2 + GYRO_TEMP_AMP * 24;
+ manager->report(manager, &event);
+ }
+ }
+
+ manager->complete(manager);
+
+ return 0;
+}
+
+int smi130_raw_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ struct smi130_device *driver_dev = hf_device_get_private_data(hfdev);
+
+ if (sensor_type == SENSOR_TYPE_ACCELEROMETER)
+ atomic_set(&driver_dev->raw_acc_enable, en);
+ else if (sensor_type == SENSOR_TYPE_GYROSCOPE)
+ atomic_set(&driver_dev->raw_gyro_enable, en);
+ return 0;
+}
+
+int smi130_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ int err = 0;
+ uint8_t data = 0;
+ struct smi130_device *driver_dev = hf_device_get_private_data(hfdev);
+
+ pr_debug("%s id:%d en:%d\n", __func__, sensor_type, en);
+
+ if (sensor_type == SENSOR_TYPE_ACCELEROMETER) {
+ if (en) {
+ data = SMI130_ACC_DATA_INT_EN;
+ err = smi130_i2c_write_block(driver_dev->acc_client,
+ SMI130_ACC_INT_ENABLE1_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write power mode failed\n");
+ return -1;
+ }
+ atomic_set(&driver_dev->acc_enable, en);
+ } else {
+ data = 0x00;
+ err = smi130_i2c_write_block(driver_dev->acc_client,
+ SMI130_ACC_INT_ENABLE1_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write power mode failed\n");
+ return -1;
+ }
+ atomic_set(&driver_dev->acc_enable, en);
+ }
+ } else if (sensor_type == SENSOR_TYPE_GYROSCOPE) {
+ if (en) {
+ data = SMI130_GYRO_DATA_INT_EN;
+ err = smi130_i2c_write_block(driver_dev->gyro_client,
+ SMI130_GYRO_INT_ENABLE0_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write power mode failed\n");
+ return -1;
+ }
+ atomic_set(&driver_dev->gyro_enable, en);
+ } else {
+ data = 0x00;
+ err = smi130_i2c_write_block(driver_dev->gyro_client,
+ SMI130_GYRO_INT_ENABLE0_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write power mode failed\n");
+ return -1;
+ }
+ atomic_set(&driver_dev->gyro_enable, en);
+ }
+ } else if (sensor_type == SENSOR_TYPE_GYRO_TEMPERATURE)
+ atomic_set(&driver_dev->gyro_temp_enable, en);
+
+ return 0;
+
+}
+
+int smi130_batch(struct hf_device *hfdev, int sensor_type,
+ int64_t delay, int64_t latency)
+{
+
+ int err = 0;
+ uint8_t data = 0;
+ int value = 0;
+ struct smi130_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+
+ pr_debug("%s id:%d rate:%lld latency:%lld\n",
+ __func__, sensor_type, delay, latency);
+
+ value = div64_s64(1000000000LL, delay);
+
+ if (sensor_type == SENSOR_TYPE_ACCELEROMETER) {
+ if (value <= 7)
+ data = SMI130_ACC_ODR_7HZ;
+ else if (value <= 15)
+ data = SMI130_ACC_ODR_15HZ;
+ else if (value <= 31)
+ data = SMI130_ACC_ODR_31HZ;
+ else if (value <= 62)
+ data = SMI130_ACC_ODR_62HZ;
+ else if (value <= 125)
+ data = SMI130_ACC_ODR_125HZ;
+ else if (value <= 250)
+ data = SMI130_ACC_ODR_250HZ;
+ else if (value <= 500)
+ data = SMI130_ACC_ODR_500HZ;
+ else
+ data = SMI130_ACC_ODR_1000HZ;
+
+ err = smi130_i2c_write_block(driver_dev->acc_client,
+ SMI130_ACC_BW_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write rate failed.\n");
+ return -1;
+ }
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ADDITIONAL_INFO;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.reserved = SENSOR_TYPE_ACCELEROMETER;
+ event.action = DATA_ACTION;
+ get_placement_info(driver_dev->direction, event.byte);
+ event.byte[12] = 1;
+ event.byte[3] = driver_dev->placement[0];
+ event.byte[7] = driver_dev->placement[1];
+ event.byte[11] = driver_dev->placement[2];
+
+ manager->report(manager, &event);
+ } else if (sensor_type == SENSOR_TYPE_GYROSCOPE) {
+ if (value <= 12)
+ data = SMI130_GYRO_ODR_12HZ;
+ else if (value <= 23)
+ data = SMI130_GYRO_ODR_23HZ;
+ else if (value <= 32)
+ data = SMI130_GYRO_ODR_32HZ;
+ else if (value <= 47)
+ data = SMI130_GYRO_ODR_47HZ;
+ else if (value <= 64)
+ data = SMI130_GYRO_ODR_64HZ;
+ else if (value <= 116)
+ data = SMI130_GYRO_ODR_116HZ;
+ else if (value <= 230)
+ data = SMI130_GYRO_ODR_230HZ;
+ else
+ data = SMI130_GYRO_ODR_523HZ;
+
+ err = smi130_i2c_write_block(driver_dev->gyro_client,
+ SMI130_GYRO_BW_ADDR, &data, 1);
+ if (err < 0) {
+ pr_err_ratelimited("write rate failed.\n");
+ return -1;
+ }
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.timestamp = current_time;
+ event.sensor_type = SENSOR_TYPE_ADDITIONAL_INFO;
+ event.accurancy = SENSOR_ACCURANCY_HIGH;
+ event.reserved = SENSOR_TYPE_GYROSCOPE;
+ event.action = DATA_ACTION;
+ get_placement_info(driver_dev->direction, event.byte);
+ event.byte[12] = 1;
+ event.byte[3] = driver_dev->placement[0];
+ event.byte[7] = driver_dev->placement[1];
+ event.byte[11] = driver_dev->placement[2];
+
+ manager->report(manager, &event);
+ }
+
+ manager->complete(manager);
+
+ return 0;
+
+}
+
+static int smi130_flush(struct hf_device *hfdev, int sensor_type)
+{
+ struct smi130_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+ struct hf_manager_event event;
+ int64_t current_time;
+
+ memset(&event, 0, sizeof(struct hf_manager_event));
+ current_time = ktime_get_boot_ns();
+ event.sensor_type = sensor_type;
+ event.timestamp = current_time;
+ event.action = FLUSH_ACTION;
+
+ manager->report(manager, &event);
+ manager->complete(manager);
+
+ return 0;
+}
+
+static int smi130_i2c_remove(struct i2c_client *client)
+{
+ struct smi130_device *driver_dev = i2c_get_clientdata(client);
+
+ hf_manager_destroy(driver_dev->hf_dev.manager);
+ kfree(driver_dev);
+ return 0;
+}
+
+static int smi130_i2c_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+
+ int err = 0;
+ struct smi130_device *driver_dev;
+ struct sensorlist_info_t listinfo;
+
+ pr_info("%s\n", __func__);
+
+ driver_dev = devm_kzalloc(&client->dev, sizeof(*driver_dev),
+ GFP_KERNEL);
+ if (!driver_dev)
+ return -ENOMEM;
+
+ driver_dev->acc_client = client;
+ driver_dev->gyro_client = i2c_new_dummy(client->adapter, GYRO_ADDR);
+ if (!driver_dev->gyro_client) {
+ pr_err("Failed to allocate i2c device for gyro\n");
+ return -ENODEV;
+ }
+
+ /* acc init */
+ err = smi130_acc_check_chip_id(client);
+ if (err < 0) {
+ pr_err("smi130 acc chip id mismatch\n");
+ err = -EINVAL;
+ goto init_fail;
+ }
+
+ err = smi130_acc_set_soft_reset(client);
+ if (err < 0) {
+ pr_err("erro soft reset!\n");
+ err = -EINVAL;
+ goto init_fail;
+ }
+
+ err = smi130_acc_init_device(client);
+ if (err < 0) {
+ pr_err("%s init device fail\n", __func__);
+ goto init_fail;
+ }
+
+ /* gyro init */
+ err = smi130_gyro_check_chip_id(driver_dev->gyro_client);
+ if (err < 0) {
+ pr_err("smi130 gyro chip id mismatch\n");
+ err = -EINVAL;
+ goto init_fail;
+ }
+
+ err = smi130_gyro_init_device(driver_dev->gyro_client);
+ if (err < 0) {
+ pr_err("%s init device fail\n", __func__);
+ goto init_fail;
+ }
+
+ if (of_property_read_u32(client->dev.of_node,
+ "direction", &driver_dev->direction)) {
+ pr_err("%s get direction dts fail\n", __func__);
+ err = -EFAULT;
+ goto dts_fail;
+ }
+
+ if (of_property_read_u8_array(client->dev.of_node, "placement",
+ driver_dev->placement, ARRAY_SIZE(driver_dev->placement))) {
+ pr_err("%s get placement dts fail\n", __func__);
+ err = -EFAULT;
+ goto dts_fail;
+ }
+
+ atomic_set(&driver_dev->raw_acc_enable, 0);
+ atomic_set(&driver_dev->raw_gyro_enable, 0);
+
+ driver_dev->hf_dev.dev_name = SMI130_I2C_NAME;
+ driver_dev->hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ driver_dev->hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ driver_dev->hf_dev.support_list = support_sensors;
+ driver_dev->hf_dev.support_size = ARRAY_SIZE(support_sensors);
+ driver_dev->hf_dev.enable = smi130_enable;
+ driver_dev->hf_dev.batch = smi130_batch;
+ driver_dev->hf_dev.flush = smi130_flush;
+ driver_dev->hf_dev.sample = smi130_sample;
+ driver_dev->hf_dev.rawdata = smi130_raw_enable;
+
+ err = hf_manager_create(&driver_dev->hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ err = -1;
+ goto create_manager_fail;
+ }
+
+ i2c_set_clientdata(client, driver_dev);
+ hf_device_set_private_data(&driver_dev->hf_dev, driver_dev);
+
+ memset(&listinfo, 0, sizeof(struct sensorlist_info_t));
+ strlcpy(listinfo.name, SMI130_I2C_NAME, sizeof(listinfo.name));
+ sensorlist_register_devinfo(SENSOR_TYPE_ACCELEROMETER, &listinfo);
+ sensorlist_register_devinfo(SENSOR_TYPE_GYROSCOPE, &listinfo);
+
+ pr_info("%s success!\n", __func__);
+ return 0;
+
+create_manager_fail:
+dts_fail:
+init_fail:
+ i2c_unregister_device(driver_dev->gyro_client);
+ return err;
+}
+
+static const struct of_device_id smi130_acc_of_match[] = {
+ {.compatible = "mediatek,smi130_sensor"},
+ {},
+};
+static const struct i2c_device_id smi130_acc_i2c_id[] = {
+ {SMI130_I2C_NAME, 0}, {} };
+
+static struct i2c_driver smi130_acc_i2c_driver = {
+ .driver = {
+ .name = SMI130_I2C_NAME,
+ .bus = &i2c_bus_type,
+ .owner = THIS_MODULE,
+ .of_match_table = smi130_acc_of_match,
+ },
+ .probe = smi130_i2c_probe,
+ .remove = smi130_i2c_remove,
+ .id_table = smi130_acc_i2c_id,
+};
+
+module_i2c_driver(smi130_acc_i2c_driver);
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("smi130 acc i2c driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/smi130_i2c.h b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/smi130_i2c.h
new file mode 100644
index 0000000..7287976
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/smi130_i2c/smi130_i2c.h
@@ -0,0 +1,114 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+#ifndef __SMI130_ACC_H__
+#define __SMI130_ACC_H__
+
+ /*Define of registers*/
+
+ /* Hard Wired */
+#define SMI130_ACC_CHIP_ID_ADDR 0x00
+ /**<Address of Chip ID Register*/
+
+ /* Data Register */
+#define SMI130_ACC_RATE_X_LSB_ADDR 0x02
+ /**< Address of X axis Rate LSB Register */
+#define SMI130_ACC_RATE_X_MSB_ADDR 0x03
+ /**< Address of X axis Rate MSB Register */
+#define SMI130_ACC_RATE_Y_LSB_ADDR 0x04
+ /**< Address of Y axis Rate LSB Register */
+#define SMI130_ACC_RATE_Y_MSB_ADDR 0x05
+ /**< Address of Y axis Rate MSB Register */
+#define SMI130_ACC_RATE_Z_LSB_ADDR 0x06
+ /**< Address of Z axis Rate LSB Register */
+#define SMI130_ACC_RATE_Z_MSB_ADDR 0x07
+ /**< Address of Z axis Rate MSB Register */
+#define SMI130_ACC_TEMP_ADDR 0x08
+ /**< Address of Temperature Data LSB Register */
+
+#define SMI130_ACC_INT_STATUS1_ADDR 0x0A
+ /**< Address of Interrupt status Register 1*/
+
+ /* Control Register */
+#define SMI130_ACC_RANGE_ADDR 0x0F
+ /**< Address of Range address Register */
+#define SMI130_ACC_BW_ADDR 0x10
+ /**< Address of Bandwidth Register */
+#define SMI130_ACC_RATED_HBW_ADDR 0x13
+ /**< Address of Rate HBW Register */
+#define SMI130_ACC_BGW_SOFTRESET_ADDR 0x14
+ /**< Address of BGW Softreset Register */
+
+#define SMI130_ACC_INT_ENABLE1_ADDR 0x17
+ /**< Address of Interrupt Enable 1 */
+
+#define SMI130_ACC_INT_MAP_1_ADDR 0x1A
+ /**< Address of Interrupt MAP 1 */
+
+#define SMI130_ACC_INT_SRC_ADDR 0x1E
+ /**< Address of Interrupt SRC 1 */
+
+#define SMI130_ACC_INT_OUT_CTRL_ADDR 0x20
+ /**< Address of Interrupt MAP 1 */
+
+#define SMI130_ACC_BGW_SPI3_WDT_ADDR 0x34
+ /**< Address of BGW SPI3,WDT Register */
+
+#define SMI130_ACC_SELF_TEST_ADDR 0x32
+ /**< Address of BGW Self test Register */
+
+#define SMI130_ACC_DATA_INT_EN 0x10
+
+/* gyro reg */
+#define SMI130_GYRO_CHIP_ID_ADDR 0x00
+ /**<Address of Chip ID Register */
+
+ /* Data Register */
+#define SMI130_GYRO_RATE_X_LSB_ADDR 0x02
+ /**< Address of X axis Rate LSB Register */
+#define SMI130_GYRO_RATE_X_MSB_ADDR 0x03
+ /**< Address of X axis Rate MSB Register */
+#define SMI130_GYRO_RATE_Y_LSB_ADDR 0x04
+ /**< Address of Y axis Rate LSB Register */
+#define SMI130_GYRO_RATE_Y_MSB_ADDR 0x05
+ /**< Address of Y axis Rate MSB Register */
+#define SMI130_GYRO_RATE_Z_LSB_ADDR 0x06
+ /**< Address of Z axis Rate LSB Register */
+#define SMI130_GYRO_RATE_Z_MSB_ADDR 0x07
+ /**< Address of Z axis Rate MSB Register */
+#define SMI130_GYRO_TEMP_ADDR 0x08
+ /**< Address of Temperature Data LSB Register */
+
+#define SMI130_GYRO_INT_STATUS1_ADDR 0x0A
+ /**< Address of Interrupt status Register 1 */
+
+ /* Control Register */
+#define SMI130_GYRO_RANGE_ADDR 0x0F
+ /**< Address of Range address Register */
+#define SMI130_GYRO_BW_ADDR 0x10
+ /**< Address of Bandwidth Register */
+#define SMI130_GYRO_RATED_HBW_ADDR 0x13
+ /**< Address of Rate HBW Register */
+#define SMI130_GYRO_BGW_SOFTRESET_ADDR 0x14
+ /**< Address of BGW Softreset Register */
+#define SMI130_GYRO_INT_ENABLE0_ADDR 0x15
+ /**< Address of Interrupt Enable 0 */
+#define SMI130_GYRO_INT_ENABLE1_ADDR 0x16
+ /**< Address of Interrupt Enable 1 */
+
+#define SMI130_GYRO_INT_MAP_1_ADDR 0x18
+ /**< Address of Interrupt MAP 0 */
+
+#define SMI130_GYRO_BGW_SPI3_WDT_ADDR 0x34
+ /**< Address of BGW SPI3,WDT Register */
+
+#define SMI130_GYRO_SELF_TEST_ADDR 0x3C
+ /**< Address of BGW Self test Register */
+
+
+#define SMI130_GYRO_DATA_INT_EN 0x80
+
+
+#endif
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/Kconfig b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/Kconfig
new file mode 100644
index 0000000..c9e2325
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/Kconfig
@@ -0,0 +1,8 @@
+config MTK_HF_TEST_CASE
+ bool "high frequency manager test demo for MediaTek package"
+ help
+ It support high frequency manager test demo.
+ If this option is set,
+ it will support
+ high frequency manager test demo.
+
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/Makefile
new file mode 100644
index 0000000..0a6dae4
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/Makefile
@@ -0,0 +1,3 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+
+obj-y += test.o
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test.c
new file mode 100644
index 0000000..6ca6239
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test.c
@@ -0,0 +1,138 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+#include "hf_manager.h"
+
+struct test_device {
+ struct hf_device hf_dev;
+};
+
+struct test_device test_driver1;
+struct test_device test_driver2;
+struct test_device test_driver3;
+struct test_device test_driver4;
+
+static unsigned char support_sensors1[] = {
+ 1,
+};
+static unsigned char support_sensors2[] = {
+ 2,
+};
+static unsigned char support_sensors3[] = {
+ 3,
+};
+static unsigned char support_sensors4[] = {
+ 4,
+};
+
+static int test_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+ pr_debug("%s id:%d en:%d\n", __func__, sensor_type, en);
+ return 0;
+}
+
+static int test_batch(struct hf_device *hfdev, int sensor_type,
+ int64_t delay, int64_t latency)
+{
+ pr_debug("%s id:%d delay:%lld latency:%lld\n", __func__, sensor_type,
+ delay, latency);
+ return 0;
+}
+
+static int test_sample(struct hf_device *hfdev)
+{
+ struct test_device *driver_dev = hf_device_get_private_data(hfdev);
+ struct hf_manager *manager = driver_dev->hf_dev.manager;
+
+ pr_debug("%s %s\n", __func__, driver_dev->hf_dev.dev_name);
+ manager->complete(manager);
+ return 0;
+}
+
+static int tests_init(void)
+{
+ int err = 0;
+
+ test_driver1.hf_dev.dev_name = "test_driver1";
+ test_driver1.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver1.hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ test_driver1.hf_dev.support_list = support_sensors1;
+ test_driver1.hf_dev.support_size = ARRAY_SIZE(support_sensors1);
+ test_driver1.hf_dev.enable = test_enable;
+ test_driver1.hf_dev.batch = test_batch;
+ test_driver1.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver1.hf_dev);
+ if (err < 0)
+ pr_err("%s hf_manager_create fail\n", __func__);
+ hf_device_set_private_data(&test_driver1.hf_dev, &test_driver1);
+
+ test_driver2.hf_dev.dev_name = "test_driver2";
+ test_driver2.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver2.hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ test_driver2.hf_dev.support_list = support_sensors2;
+ test_driver2.hf_dev.support_size = ARRAY_SIZE(support_sensors2);
+ test_driver2.hf_dev.enable = test_enable;
+ test_driver2.hf_dev.batch = test_batch;
+ test_driver2.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver2.hf_dev);
+ if (err < 0)
+ pr_err("%s hf_manager_create fail\n", __func__);
+ hf_device_set_private_data(&test_driver2.hf_dev, &test_driver2);
+
+ test_driver3.hf_dev.dev_name = "test_driver3";
+ test_driver3.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver3.hf_dev.device_bus = HF_DEVICE_IO_ASYNC;
+ test_driver3.hf_dev.support_list = support_sensors3;
+ test_driver3.hf_dev.support_size = ARRAY_SIZE(support_sensors3);
+ test_driver3.hf_dev.enable = test_enable;
+ test_driver3.hf_dev.batch = test_batch;
+ test_driver3.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver3.hf_dev);
+ if (err < 0)
+ pr_err("%s hf_manager_create fail\n", __func__);
+ hf_device_set_private_data(&test_driver3.hf_dev, &test_driver3);
+
+ test_driver4.hf_dev.dev_name = "test_driver4";
+ test_driver4.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver4.hf_dev.device_bus = HF_DEVICE_IO_ASYNC;
+ test_driver4.hf_dev.support_list = support_sensors4;
+ test_driver4.hf_dev.support_size = ARRAY_SIZE(support_sensors4);
+ test_driver4.hf_dev.enable = test_enable;
+ test_driver4.hf_dev.batch = test_batch;
+ test_driver4.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver4.hf_dev);
+ if (err < 0)
+ pr_err("%s hf_manager_create fail\n", __func__);
+ hf_device_set_private_data(&test_driver4.hf_dev, &test_driver4);
+ return 0;
+}
+
+static int __init test_init(void)
+{
+ tests_init();
+ return 0;
+}
+
+static void __exit test_exit(void)
+{
+
+}
+
+module_init(test_init);
+module_exit(test_exit);
+
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("test driver");
+MODULE_LICENSE("GPL");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app.c
new file mode 100644
index 0000000..33713a8
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app.c
@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "[test_app] " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/kobject.h>
+
+#include "hf_manager.h"
+
+struct test_app_t {
+ struct task_struct *task;
+ struct hf_client *client;
+ struct kobject *kobj;
+ int sensor_type;
+ int val1;
+ int val2;
+};
+
+static struct test_app_t test_app;
+
+static int test_app_kthread(void *arg)
+{
+ struct hf_client *client = NULL;
+ struct hf_manager_event data[4];
+ int size = 0, i = 0;
+
+ client = hf_client_create();
+ if (!client) {
+ pr_err("hf_client_create fail\n");
+ return -ENOMEM;
+ }
+ test_app.client = client;
+
+ while (!kthread_should_stop()) {
+ memset(data, 0, sizeof(data));
+ size = hf_client_poll_sensor(client, data, ARRAY_SIZE(data));
+ if (size < 0)
+ continue;
+ for (i = 0; i < size; ++i) {
+ pr_info_ratelimited("[%d,%d,%lld,%d,%d,%d]\n",
+ data[i].sensor_type,
+ data[i].action,
+ data[i].timestamp,
+ data[i].word[0],
+ data[i].word[1],
+ data[i].word[2]);
+
+ /* need derequest sensor cali */
+ switch (data[i].action) {
+ case CALI_ACTION:
+ hf_client_request_sensor_cali(test_app.client,
+ test_app.sensor_type,
+ HF_MANAGER_REQUEST_CALI_DATA,
+ false);
+ break;
+ case TEST_ACTION:
+ hf_client_request_sensor_cali(test_app.client,
+ test_app.sensor_type,
+ HF_MANAGER_REQUEST_TEST_DATA,
+ false);
+ break;
+ }
+ }
+ }
+ return 0;
+}
+
+#define test_app_attr(_name) \
+static struct kobj_attribute _name##_attr = { \
+ .attr = { \
+ .name = __stringify(_name), \
+ .mode = 0644, \
+ }, \
+ .show = _name##_show, \
+ .store = _name##_store, \
+}
+
+static ssize_t control_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return sprintf(buf, "sensor_type=%u,val1=%u,val2=%u\n",
+ test_app.sensor_type,
+ test_app.val1,
+ test_app.val2);
+}
+
+static ssize_t control_store(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ const char *buf, size_t n)
+{
+ int ret = 0;
+ struct hf_manager_cmd cmd;
+
+ if (!test_app.client)
+ goto out;
+
+ ret = sscanf(buf, "%u,%u,%u", &test_app.sensor_type,
+ &test_app.val1, &test_app.val2);
+ if (ret != 3) {
+ pr_err("control store param error\n");
+ goto out;
+ }
+
+ ret = hf_client_find_sensor(test_app.client, test_app.sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n",
+ test_app.sensor_type);
+ goto out;
+ }
+
+ switch (test_app.val1) {
+ case HF_MANAGER_SENSOR_ENABLE_CALI:
+ hf_client_request_sensor_cali(test_app.client,
+ test_app.sensor_type,
+ HF_MANAGER_REQUEST_CALI_DATA,
+ true);
+ break;
+ case HF_MANAGER_SENSOR_SELFTEST:
+ hf_client_request_sensor_cali(test_app.client,
+ test_app.sensor_type,
+ HF_MANAGER_REQUEST_TEST_DATA,
+ true);
+ break;
+ }
+
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = test_app.sensor_type;
+ cmd.action = test_app.val1;
+ cmd.delay = test_app.val2;
+ cmd.latency = 0;
+ ret = hf_client_control_sensor(test_app.client, &cmd);
+ if (ret < 0) {
+ pr_err("hf_client_control_sensor %u fail\n",
+ test_app.sensor_type);
+ goto out;
+ }
+out:
+ return n;
+}
+
+test_app_attr(control);
+
+static struct attribute *attr[] = {
+ &control_attr.attr,
+ NULL,
+};
+
+static const struct attribute_group attr_group = {
+ .attrs = attr,
+};
+
+static int __init test_app_init(void)
+{
+ test_app.task = kthread_run(test_app_kthread,
+ &test_app, "test_app");
+ if (IS_ERR(test_app.task))
+ pr_err("kthread_run create fail\n");
+
+ test_app.kobj = kobject_create_and_add("test_app", NULL);
+ if (!test_app.kobj) {
+ pr_err("kobject create fail\n");
+ return -ENOMEM;
+ }
+ if (sysfs_create_group(test_app.kobj, &attr_group)) {
+ pr_err("sysfs create fail\n");
+ return -EFAULT;
+ }
+ return 0;
+}
+
+module_init(test_app_init);
+
+MODULE_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app1.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app1.c
new file mode 100644
index 0000000..994bd60
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app1.c
@@ -0,0 +1,202 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "[test_app1] " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kobject.h>
+
+#include "hf_manager.h"
+
+#define test_app_attr(_name) \
+static struct kobj_attribute _name##_attr = { \
+ .attr = { \
+ .name = __stringify(_name), \
+ .mode = 0644, \
+ }, \
+ .show = _name##_show, \
+}
+
+static ssize_t test_app1_cmd(char *buf, int sensor_type,
+ int action, unsigned int request)
+{
+ ssize_t ret = 0;
+ struct hf_client *client = NULL;
+ struct hf_manager_cmd cmd;
+ struct hf_manager_event data[1];
+
+ client = hf_client_create();
+ if (!client) {
+ pr_err("hf_client_create fail\n");
+ return -ENOMEM;
+ }
+ ret = hf_client_find_sensor(client, sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n", sensor_type);
+ goto out;
+ }
+ hf_client_request_sensor_cali(client, sensor_type,
+ request, true);
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = sensor_type;
+ cmd.action = action;
+ ret = hf_client_control_sensor(client, &cmd);
+ if (ret < 0) {
+ pr_err("hf_client_control_sensor %u %u fail\n",
+ sensor_type, action);
+ goto out;
+ }
+ ret = hf_client_poll_sensor_timeout(client, data, ARRAY_SIZE(data),
+ msecs_to_jiffies(3000));
+ hf_client_request_sensor_cali(client, sensor_type,
+ request, false);
+ if (ret >= 0)
+ ret = sprintf(buf, "[%d,%d,%d,%lld,%d,%d,%d]\n",
+ data[0].sensor_type,
+ data[0].action,
+ data[0].accurancy,
+ data[0].timestamp,
+ data[0].word[0],
+ data[0].word[1],
+ data[0].word[2]);
+out:
+ hf_client_destroy(client);
+ return ret;
+}
+
+static ssize_t test_app1_cust(char *buf, int sensor_type,
+ int action)
+{
+ ssize_t ret = 0;
+ struct hf_client *client = NULL;
+ struct custom_cmd cmd;
+
+ client = hf_client_create();
+ if (!client) {
+ pr_err("hf_client_create fail\n");
+ return -ENOMEM;
+ }
+ ret = hf_client_find_sensor(client, sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n", sensor_type);
+ goto out;
+ }
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.data[0] = action;
+ ret = hf_client_custom_cmd(client, sensor_type, &cmd);
+ if (ret >= 0)
+ ret = sprintf(buf, "[%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d]\n",
+ cmd.data[0],
+ cmd.data[1],
+ cmd.data[2],
+ cmd.data[3],
+ cmd.data[4],
+ cmd.data[5],
+ cmd.data[6],
+ cmd.data[7],
+ cmd.data[8],
+ cmd.data[9],
+ cmd.data[10],
+ cmd.data[11]);
+out:
+ hf_client_destroy(client);
+ return ret;
+}
+
+static ssize_t acc_cali_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return test_app1_cmd(buf, SENSOR_TYPE_ACCELEROMETER,
+ HF_MANAGER_SENSOR_ENABLE_CALI,
+ HF_MANAGER_REQUEST_CALI_DATA);
+}
+
+static ssize_t acc_cust_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return test_app1_cust(buf, SENSOR_TYPE_ACCELEROMETER,
+ CUST_CMD_CALI);
+}
+
+
+static ssize_t acc_seltest_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return test_app1_cmd(buf, SENSOR_TYPE_ACCELEROMETER,
+ HF_MANAGER_SENSOR_SELFTEST,
+ HF_MANAGER_REQUEST_TEST_DATA);
+}
+
+static ssize_t gyro_cali_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return test_app1_cmd(buf, SENSOR_TYPE_GYROSCOPE,
+ HF_MANAGER_SENSOR_ENABLE_CALI,
+ HF_MANAGER_REQUEST_CALI_DATA);
+}
+
+static ssize_t gyro_cust_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return test_app1_cust(buf, SENSOR_TYPE_GYROSCOPE,
+ CUST_CMD_CALI);
+}
+
+static ssize_t gyro_selftest_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return test_app1_cmd(buf, SENSOR_TYPE_GYROSCOPE,
+ HF_MANAGER_SENSOR_SELFTEST,
+ HF_MANAGER_REQUEST_TEST_DATA);
+}
+
+test_app_attr(acc_cali);
+test_app_attr(acc_cust);
+test_app_attr(acc_seltest);
+test_app_attr(gyro_cali);
+test_app_attr(gyro_cust);
+test_app_attr(gyro_selftest);
+
+static struct attribute *attr[] = {
+ &acc_cali_attr.attr,
+ &acc_cust_attr.attr,
+ &acc_seltest_attr.attr,
+ &gyro_cali_attr.attr,
+ &gyro_cust_attr.attr,
+ &gyro_selftest_attr.attr,
+ NULL,
+};
+
+static const struct attribute_group attr_group = {
+ .attrs = attr,
+};
+
+static int __init test_app_init(void)
+{
+ struct kobject *kobj = kobject_create_and_add("test_app1", NULL);
+
+ if (!kobj) {
+ pr_err("kobject create fail\n");
+ return -ENOMEM;
+ }
+ if (sysfs_create_group(kobj, &attr_group)) {
+ pr_err("sysfs create fail\n");
+ return -EFAULT;
+ }
+ return 0;
+}
+
+module_init(test_app_init);
+
+MODULE_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app2.c b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app2.c
new file mode 100644
index 0000000..9066f32
--- /dev/null
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/2.0/test/test_app2.c
@@ -0,0 +1,160 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "[test_app2] " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/kobject.h>
+
+#include "hf_manager.h"
+
+struct test_app_t {
+ struct task_struct *task;
+ struct hf_client *client;
+};
+
+static struct test_app_t test_app;
+
+static int test_app_kthread(void *arg)
+{
+ struct hf_client *client = test_app.client;
+ struct hf_manager_event data[4];
+ int size = 0, i = 0;
+
+ if (!client)
+ return -EINVAL;
+
+ while (!kthread_should_stop()) {
+ memset(data, 0, sizeof(data));
+ /*
+ * must use timeout api to wakeup kthread and do exit
+ * otherwise kthread_stop will be blocked forever
+ */
+ size = hf_client_poll_sensor_timeout(client, data,
+ ARRAY_SIZE(data), msecs_to_jiffies(500));
+ if (size < 0)
+ continue;
+ for (i = 0; i < size; ++i) {
+ pr_info_ratelimited("[%d,%d,%lld,%d,%d,%d]\n",
+ data[i].sensor_type,
+ data[i].action,
+ data[i].timestamp,
+ data[i].word[0],
+ data[i].word[1],
+ data[i].word[2]);
+ }
+ }
+ return 0;
+}
+
+#define test_app_attr(_name) \
+static struct kobj_attribute _name##_attr = { \
+ .attr = { \
+ .name = __stringify(_name), \
+ .mode = 0644, \
+ }, \
+ .store = _name##_store, \
+}
+
+static ssize_t control_store(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ const char *buf, size_t n)
+{
+ int ret = 0;
+ int sensor_type = 0, val1 = 0, val2 = 0;
+ struct hf_manager_cmd cmd;
+
+ ret = sscanf(buf, "%u,%u,%u", &sensor_type, &val1, &val2);
+ if (ret != 3) {
+ pr_err("control store param error\n");
+ return -EINVAL;
+ }
+
+ if (val1 == HF_MANAGER_SENSOR_ENABLE) {
+ if (test_app.client)
+ return -EINVAL;
+ test_app.client = hf_client_create();
+ if (!test_app.client) {
+ pr_err("hf_client_create fail\n");
+ return -ENOMEM;
+ }
+ if (!test_app.task) {
+ test_app.task = kthread_run(test_app_kthread,
+ &test_app, "test_app2");
+ if (IS_ERR(test_app.task)) {
+ pr_err("kthread_run create fail\n");
+ return -ENOMEM;
+ }
+ }
+ ret = hf_client_find_sensor(test_app.client, sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n",
+ sensor_type);
+ return -EINVAL;
+ }
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = sensor_type;
+ cmd.action = val1;
+ cmd.delay = val2;
+ cmd.latency = 0;
+ ret = hf_client_control_sensor(test_app.client, &cmd);
+ if (ret < 0) {
+ pr_err("hf_client_control_sensor %u fail\n",
+ sensor_type);
+ return -EINVAL;
+ }
+ } else if (val1 == HF_MANAGER_SENSOR_DISABLE) {
+ if (test_app.client) {
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = sensor_type;
+ cmd.action = val1;
+ hf_client_control_sensor(test_app.client, &cmd);
+ }
+ if (test_app.task) {
+ kthread_stop(test_app.task);
+ test_app.task = NULL;
+ }
+ if (test_app.client) {
+ hf_client_destroy(test_app.client);
+ test_app.client = NULL;
+ }
+ }
+
+ return n;
+}
+
+test_app_attr(control);
+
+static struct attribute *attr[] = {
+ &control_attr.attr,
+ NULL,
+};
+
+static const struct attribute_group attr_group = {
+ .attrs = attr,
+};
+
+static int __init test_app_init(void)
+{
+ struct kobject *kobj = kobject_create_and_add("test_app2", NULL);
+
+ if (!kobj) {
+ pr_err("kobject create fail\n");
+ return -ENOMEM;
+ }
+ if (sysfs_create_group(kobj, &attr_group)) {
+ pr_err("sysfs create fail\n");
+ return -EFAULT;
+ }
+ return 0;
+}
+
+module_init(test_app_init);
+
+MODULE_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Kconfig b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Kconfig
index d0205ab..f4bf3f3 100644
--- a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Kconfig
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Kconfig
@@ -7,3 +7,4 @@
If unsure, say N.
source "drivers/misc/mediatek/sensor/iio/Kconfig"
+source "drivers/misc/mediatek/sensor/2.0/Kconfig"
diff --git a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Makefile b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Makefile
index 3bd9bfc..da44493 100644
--- a/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Makefile
+++ b/src/kernel/linux/v4.19/drivers/misc/mediatek/sensor/Makefile
@@ -1 +1,2 @@
obj-y += iio/
+obj-y += 2.0/