Initial commit: Linux kernel 4.19 for Unplugged OS

drivers/misc/mediatek/sensor/2.0/Kconfig

@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+
+source "drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Kconfig"
+source "drivers/misc/mediatek/sensor/2.0/sensorhub/Kconfig"
+source "drivers/misc/mediatek/sensor/2.0/test/Kconfig"
+source "drivers/misc/mediatek/sensor/2.0/lsm6dsm_secondary/Kconfig"

drivers/misc/mediatek/sensor/2.0/Makefile

@@ -0,0 +1,8 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-y += core/
+
+obj-$(CONFIG_CUSTOM_KERNEL_SENSORHUB) += mtk_nanohub/
+obj-$(CONFIG_MTK_SENSORHUB) += sensorhub/
+obj-$(CONFIG_MTK_HF_LSM6DSM_SECONDARY) += lsm6dsm_secondary/
+obj-$(CONFIG_MTK_HF_TEST_CASE) += test/

drivers/misc/mediatek/sensor/2.0/core/Makefile

@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_MTK_SENSOR_SUPPORT) += hf_manager.o

drivers/misc/mediatek/sensor/2.0/core/hf_manager.h

@@ -0,0 +1,172 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 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 sensor_state {
+	uint8_t enable : 1;
+	uint8_t bias : 1;
+	uint8_t cali : 1;
+	uint8_t temp : 1;
+	uint8_t test : 1;
+	uint8_t raw : 1;
+	uint8_t down_sample : 1;
+	uint8_t flush;
+	uint8_t down_sample_cnt;
+	uint8_t down_sample_div;
+	int64_t delay;
+	int64_t latency;
+	int64_t start_time;
+};
+
+struct hf_core {
+	struct mutex manager_lock;
+	struct list_head manager_list;
+	struct sensor_state state[SENSOR_TYPE_SENSOR_MAX];
+
+	spinlock_t client_lock;
+	struct list_head client_list;
+
+	struct mutex device_lock;
+	struct list_head device_list;
+
+	struct kthread_worker kworker;
+};
+
+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, void *data, uint8_t length);
+	int (*selftest)(struct hf_device *hfdev, int sensor_type);
+	int (*rawdata)(struct hf_device *hfdev, int sensor_type, int en);
+	int (*debug)(struct hf_device *hfdev, int sensor_type,
+		uint8_t *buffer, unsigned int len);
+	int (*custom_cmd)(struct hf_device *hfdev, int sensor_type,
+		struct custom_cmd *cust_cmd);
+
+	unsigned char device_poll;
+	unsigned char device_bus;
+	struct sensor_info *support_list;
+	unsigned int support_size;
+
+	char *dev_name;
+
+	struct hf_manager *manager;
+	struct list_head list;
+	bool ready;
+
+	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 hang_begin;
+	int64_t client_active;
+	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;
+	atomic64_t timestamp;
+	atomic_t io_enabled;
+	unsigned long flags;
+	struct hf_device *hf_dev;
+	struct hf_core *core;
+
+	int (*report)(struct hf_manager *manager,
+		struct hf_manager_event *event);
+	void (*complete)(struct hf_manager *manager);
+	void (*interrupt)(struct hf_manager *manager, int64_t timestamp);
+};
+
+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;
+	struct hf_core *core;
+
+	/* record process info */
+	char proc_comm[TASK_COMM_LEN]; /* process name */
+	pid_t leader_pid;              /* process pid */
+	pid_t pid;                     /* control thread pid */
+	pid_t ppid;                    /* poll thread pid */
+};
+
+#define set_interrupt_timestamp(m, t) (atomic64_set(&m->timestamp, t))
+#define get_interrupt_timestamp(m) (atomic64_read(&m->timestamp))
+
+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_device_register(struct hf_device *device);
+void hf_device_unregister(struct hf_device *device);
+int hf_manager_create(struct hf_device *device);
+void hf_manager_destroy(struct hf_manager *manager);
+int hf_device_register_manager_create(struct hf_device *device);
+void hf_device_unregister_manager_destroy(struct hf_device *device);
+void coordinate_map(unsigned char direction, int32_t *data);
+struct hf_client *hf_client_create(void);
+void hf_client_destroy(struct hf_client *client);
+int hf_client_find_sensor(struct hf_client *client, uint8_t sensor_type);
+int hf_client_get_sensor_info(struct hf_client *client,
+		uint8_t sensor_type, struct sensor_info *info);
+int hf_client_request_sensor_cali(struct hf_client *client,
+		uint8_t sensor_type, unsigned int cmd, bool status);
+int hf_client_control_sensor(struct hf_client *client,
+		struct hf_manager_cmd *cmd);
+int hf_client_poll_sensor_timeout(struct hf_client *client,
+		struct hf_manager_event *data, int count, long timeout);
+#define hf_client_poll_sensor(client, data, count)		\
+	hf_client_poll_sensor_timeout(client, data, count,	\
+		MAX_SCHEDULE_TIMEOUT)
+int hf_client_custom_cmd(struct hf_client *client,
+		uint8_t sensor_type, struct custom_cmd *cust_cmd);
+
+#endif

drivers/misc/mediatek/sensor/2.0/core/hf_sensor_io.h

@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 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,
+	HF_MANAGER_SENSOR_MAX_ACTION,
+};
+
+enum {
+	DATA_ACTION,
+	FLUSH_ACTION,
+	BIAS_ACTION,
+	CALI_ACTION,
+	TEMP_ACTION,
+	TEST_ACTION,
+	RAW_ACTION,
+	MAX_ACTION,
+};
+
+struct hf_manager_batch {
+	int64_t delay;
+	int64_t latency;
+} __packed __aligned(4);
+
+struct hf_manager_cmd {
+	uint8_t sensor_type;
+	uint8_t action : 4;
+	uint8_t down_sample : 1;
+	uint8_t length;
+	uint8_t padding[1];
+	int8_t data[48] __aligned(4);
+} __packed __aligned(4);
+
+struct hf_manager_event {
+	int64_t timestamp;
+	uint8_t sensor_type;
+	uint8_t accurancy;
+	uint8_t action;
+	uint8_t padding[1];
+	union {
+		int32_t word[16];
+		int8_t byte[0];
+	};
+} __packed __aligned(4);
+
+struct sensor_info {
+	uint8_t sensor_type;
+	uint8_t padding[3];
+	uint32_t gain;
+	char name[16];
+	char vendor[16];
+} __packed __aligned(4);
+
+struct custom_cmd {
+	uint8_t command;
+	uint8_t tx_len;
+	uint8_t rx_len;
+	uint8_t padding[1];
+	union {
+		int32_t data[15];
+		int32_t word[15];
+		int8_t byte[0];
+	};
+} __packed __aligned(4);
+
+struct ioctl_packet {
+	uint8_t sensor_type;
+	uint8_t padding[3];
+	union {
+		bool status;
+		int8_t byte[64];
+	};
+} __packed __aligned(4);
+
+#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)
+#define HF_MANAGER_REQUEST_SENSOR_INFO      _IOWR('a', 6, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_CUST_DATA        _IOWR('a', 7, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_READY_STATUS     _IOWR('a', 8, struct ioctl_packet)
+
+#endif

drivers/misc/mediatek/sensor/2.0/core/hf_sensor_type.h

@@ -0,0 +1,149 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _HF_SENSOR_TYPE_H_
+#define _HF_SENSOR_TYPE_H_
+
+enum {
+	SENSOR_TYPE_INVALID = 0,
+	/* 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_OIS,
+	SENSOR_TYPE_FLICKER,
+	SENSOR_TYPE_GYRO_SECONDARY,
+	SENSOR_TYPE_FLICKER_REAR,
+	SENSOR_TYPE_RGBW_REAR,
+	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_OIS,
+	ID_FLICKER,
+	ID_GYRO_SECONDARY,
+	ID_FLICKER_REAR,
+	ID_RGBW_REAR,
+	ID_SENSOR_MAX,
+};
+
+enum {
+	SENSOR_ACCURANCY_UNRELIALE,
+	SENSOR_ACCURANCY_LOW,
+	SENSOR_ACCURANCY_MEDIUM,
+	SENSOR_ACCURANCY_HIGH,
+};
+
+#endif

drivers/misc/mediatek/sensor/2.0/lsm6dsm_secondary/Kconfig

@@ -0,0 +1,7 @@
+config MTK_HF_LSM6DSM_SECONDARY
+	bool "lsm6dsm for MediaTek package"
+	help
+	  It support lsm6dsm secondary gyroscope sensor.
+	  If this option is set,
+	  it will support
+	  lsm6dsm secondary gyroscope sensor.

drivers/misc/mediatek/sensor/2.0/lsm6dsm_secondary/Makefile

@@ -0,0 +1,3 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+
+obj-y += lsm6dsm_secondary.o

drivers/misc/mediatek/sensor/2.0/lsm6dsm_secondary/lsm6dsm_secondary.c

@@ -0,0 +1,241 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spi/spi.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+#include "hf_manager.h"
+
+#define LSM6DSM_SECONDARY_NAME "lsm6dsm_secondary"
+
+#define LSM6DSM_ASYNC_BUFFER_SIZE 8
+
+#define SPI_WRITE_CMD(reg) (reg)
+#define SPI_READ_CMD(reg) ((unsigned char)(reg & 0xFF) | 0x80)
+
+/* fullscale 125dps ==> 4.375
+ * 4.375 * rawdata = mdps
+ * 4375 * rawdata = udps
+ */
+#define GYRO_SCALER (4375)
+
+struct lsm6dsm_device {
+	struct hf_device hf_dev;
+
+	struct spi_message spi_msg;
+	struct spi_transfer spi_xfer[2];
+
+	unsigned char async_rx_buffer[LSM6DSM_ASYNC_BUFFER_SIZE];
+	unsigned char async_tx_buffer[LSM6DSM_ASYNC_BUFFER_SIZE];
+
+	uint32_t direction;
+};
+
+static sensor_info support_sensors[] = {
+	.sensor_type = SENSOR_TYPE_GYRO_SECONDARY,
+	.gain = 1,
+	.name = {'a', 'c', 'c', 'e', 'l'},
+	.vendor = {'m', 't', 'k'},
+};
+
+static int lsm6dsm_enable(struct hf_device *hfdev, int sensor_type, int en)
+{
+	int err = 0;
+	unsigned char tx_buffer[2] = {0};
+	struct spi_device *spi_dev = hf_device_get_private_data(hfdev);
+
+	pr_debug("%s id:%d en:%d\n", __func__, sensor_type, en);
+	if (en) {
+		/* fullscale 125dps, OIS_EN_SPI2 */
+		tx_buffer[0] = SPI_WRITE_CMD(0x70);
+		tx_buffer[1] = 0x03;
+		err = spi_write(spi_dev, tx_buffer, 2);
+		/* down-sanpled 1khz @ 173hz cutoff, HP_EN_OIS */
+		tx_buffer[0] = SPI_WRITE_CMD(0x71);
+		tx_buffer[1] = 0x05;
+		err = spi_write(spi_dev, tx_buffer, 2);
+	} else {
+		/* fullscale 125dps */
+		tx_buffer[0] = SPI_WRITE_CMD(0x70);
+		tx_buffer[1] = 0x02;
+		err = spi_write(spi_dev, tx_buffer, 2);
+		/* down-sanpled 1khz, 173hz cutoff */
+		tx_buffer[0] = SPI_WRITE_CMD(0x71);
+		tx_buffer[1] = 0x04;
+		err = spi_write(spi_dev, tx_buffer, 2);
+	}
+	return err;
+}
+
+static int lsm6dsm_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 void lsm6dsm_sample_complete(void *ctx)
+{
+	struct lsm6dsm_device *driver_dev = ctx;
+	struct hf_manager *manager = driver_dev->hf_dev.manager;
+	struct hf_manager_event event;
+	int32_t data[3] = {0};
+
+	data[0] = (int16_t)((driver_dev->async_rx_buffer[1] << 8) |
+		(driver_dev->async_rx_buffer[0]));
+	data[1] = (int16_t)((driver_dev->async_rx_buffer[3] << 8) |
+		(driver_dev->async_rx_buffer[2]));
+	data[2] = (int16_t)((driver_dev->async_rx_buffer[5] << 8) |
+		(driver_dev->async_rx_buffer[4]));
+	coordinate_map(driver_dev->direction, data);
+	memset(&event, 0, sizeof(struct hf_manager_event));
+	event.timestamp = ktime_get_boot_ns();
+	event.sensor_type = SENSOR_TYPE_GYRO_SECONDARY;
+	event.accurancy = SENSOR_ACCURANCY_HIGH;
+	event.action = DATA_ACTION;
+	event.word[0] = data[0] * GYRO_SCALER;
+	event.word[1] = data[1] * GYRO_SCALER;
+	event.word[2] = data[2] * GYRO_SCALER;
+	manager->report(manager, &event);
+	manager->complete(manager);
+}
+
+static int lsm6dsm_sample(struct hf_device *hfdev)
+{
+	struct spi_device *spi_dev = hf_device_get_private_data(hfdev);
+	struct lsm6dsm_device *driver_dev = spi_get_drvdata(spi_dev);
+
+	spi_message_init(&driver_dev->spi_msg);
+
+	driver_dev->spi_msg.context = driver_dev;
+	driver_dev->spi_msg.complete = lsm6dsm_sample_complete;
+
+	memset(driver_dev->spi_xfer, 0, sizeof(driver_dev->spi_xfer));
+
+	memset(driver_dev->async_tx_buffer, 0,
+		sizeof(driver_dev->async_tx_buffer));
+	memset(driver_dev->async_rx_buffer, 0,
+		sizeof(driver_dev->async_rx_buffer));
+
+	driver_dev->async_tx_buffer[0] = SPI_READ_CMD(0x22);
+	driver_dev->spi_xfer[0].len = 1;
+	driver_dev->spi_xfer[0].tx_buf = driver_dev->async_tx_buffer;
+	spi_message_add_tail(&driver_dev->spi_xfer[0], &driver_dev->spi_msg);
+
+	driver_dev->spi_xfer[1].len = 6;
+	driver_dev->spi_xfer[1].rx_buf = driver_dev->async_rx_buffer;
+	spi_message_add_tail(&driver_dev->spi_xfer[1], &driver_dev->spi_msg);
+
+	return spi_async(spi_dev, &driver_dev->spi_msg);
+}
+
+static int lsm6dsm_init_device(struct spi_device *spi_dev)
+{
+	int err = 0;
+	unsigned char tx_buffer[2] = {0};
+	unsigned char rx_buffer[2] = {0};
+
+	tx_buffer[0] = SPI_READ_CMD(0x0f);
+	err = spi_write_then_read(spi_dev, tx_buffer, 1, rx_buffer, 1);
+
+	/* fullscale 125dps */
+	tx_buffer[0] = SPI_WRITE_CMD(0x70);
+	tx_buffer[1] = 0x02;
+	err = spi_write(spi_dev, tx_buffer, 2);
+
+	/* down-sanpled 1khz, 173hz cutoff */
+	tx_buffer[0] = SPI_WRITE_CMD(0x71);
+	tx_buffer[1] = 0x04;
+	err = spi_write(spi_dev, tx_buffer, 2);
+	return err;
+}
+
+static int lsm6dsm_probe(struct spi_device *spi_dev)
+{
+	int err = 0;
+	struct lsm6dsm_device *driver_dev = NULL;
+
+	spi_dev->mode = SPI_MODE_0;
+	spi_dev->bits_per_word = 8;
+	spi_dev->max_speed_hz = 10 * 1000 * 1000;
+
+	err = lsm6dsm_init_device(spi_dev);
+	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 init_fail;
+	}
+
+	if (of_property_read_u32(spi_dev->dev.of_node,
+		"direction", &driver_dev->direction)) {
+		pr_err("%s get direction dts fail\n", __func__);
+		goto dts_fail;
+	}
+
+	driver_dev->hf_dev.dev_name = LSM6DSM_SECONDARY_NAME;
+	driver_dev->hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+	driver_dev->hf_dev.device_bus = HF_DEVICE_IO_ASYNC;
+	driver_dev->hf_dev.support_list = support_sensors;
+	driver_dev->hf_dev.support_size = ARRAY_SIZE(support_sensors);
+	driver_dev->hf_dev.enable = lsm6dsm_enable;
+	driver_dev->hf_dev.batch = lsm6dsm_batch;
+	driver_dev->hf_dev.sample = lsm6dsm_sample;
+	hf_device_set_private_data(&driver_dev->hf_dev, spi_dev);
+	err = hf_device_register_manager_create(&driver_dev->hf_dev);
+	if (err < 0) {
+		pr_err("%s hf_manager_create fail\n", __func__);
+		err = -1;
+		goto create_manager_fail;
+	}
+	spi_set_drvdata(spi_dev, driver_dev);
+	return 0;
+
+create_manager_fail:
+dts_fail:
+	kfree(driver_dev);
+init_fail:
+	return err;
+}
+
+static int lsm6dsm_remove(struct spi_device *spi_dev)
+{
+	struct lsm6dsm_device *driver_dev = spi_get_drvdata(spi_dev);
+
+	hf_manager_destroy(driver_dev->hf_dev.manager);
+	kfree(driver_dev);
+	return 0;
+}
+
+static const struct of_device_id lsm6dsm_ids[] = {
+	{.compatible = "mediatek,lsm6dsm_secondary"},
+	{},
+};
+
+static struct spi_driver lsm6dsm_driver = {
+	.driver = {
+		.name = LSM6DSM_SECONDARY_NAME,
+		.bus = &spi_bus_type,
+		.owner = THIS_MODULE,
+		.of_match_table = lsm6dsm_ids,
+	},
+	.probe = lsm6dsm_probe,
+	.remove = lsm6dsm_remove,
+};
+
+module_spi_driver(lsm6dsm_driver);
+
+MODULE_AUTHOR("Mediatek");
+MODULE_DESCRIPTION("lsm6dsm secondary driver");
+MODULE_LICENSE("GPL");
+

drivers/misc/mediatek/sensor/2.0/lsm6dsm_secondary/lsm6dsm_secondary.h

@@ -0,0 +1,4 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Kconfig

@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config CUSTOM_KERNEL_SENSORHUB
+	tristate "CUSTOM KERNEL SENSORHUB support"
+	help
+	  Kernel space sensor hub driver.
+	  Define project used all sensor hub
+	  sensors. The value is combination of sensor hub driver(get data...etc)
+	  and sensor hub framework driver(transmit shf info)
+
+source "drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/Kconfig"

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/Makefile

@@ -0,0 +1,13 @@
+# SPDX-License-Identifier: GPL-2.0
+
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/scp/include
+
+obj-$(CONFIG_CUSTOM_KERNEL_SENSORHUB) += nanohub.o
+nanohub-objs += mtk_nanohub.o
+nanohub-objs += mtk_nanohub_ipi.o
+nanohub-objs += sensor_list.o
+nanohub-$(CONFIG_NANOHUB) += nanohub/main.o
+nanohub-$(CONFIG_NANOHUB) += nanohub/comms.o
+nanohub-$(CONFIG_NANOHUB_MTK_IPI) += nanohub/nanohub-mtk.o

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub.h

@@ -0,0 +1,524 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 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;
+	int flushcnt;
+	uint32_t gain;
+	char name[16];
+	char vendor[16];
+};
+
+#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

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.c

@@ -0,0 +1,251 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 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.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;
+
+	reinit_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("IPI_SENSOR send fail\n");
+			return -1;
+		}
+		if (status == SCP_IPI_BUSY) {
+			if (retry++ == 1000) {
+				pr_err("IPI_SENSOR send retry fail\n");
+				return -1;
+			}
+			if (retry % 100 == 0)
+				usleep_range(1000, 2000);
+		}
+	} while (status == SCP_IPI_BUSY);
+
+	if (retry >= 100)
+		pr_debug("IPI_SENSOR send 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("IPI_SENSOR 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, null tx buf\n");
+				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_completion(&hw_transfer.done);
+	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");

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/mtk_nanohub_ipi.h

@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 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

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/Kconfig

@@ -0,0 +1,21 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config NANOHUB
+	bool "Nanohub"
+	help
+	  Enable support for the nanohub sensorhub driver.
+	  This driver supports the android nanohub sensorhub.
+	  MTK sensor v1.0&v2.0 need this.
+	  If in doubt, say N here.
+
+if NANOHUB
+
+config NANOHUB_MTK_IPI
+	bool "Nanohub MTK IPI"
+	help
+	  Enable nanohub MTK IPI support.
+	  Either this or NANOHUB_SPI/NANOHUB_I2C should be selected.
+	  MTK platform only protocol.
+	  If in doubt, say N here.
+
+endif # NANOHUB

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/comms.c

@@ -0,0 +1,523 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/semaphore.h>
+#include <linux/gpio.h>
+
+#include "main.h"
+#include "comms.h"
+
+#define READ_ACK_TIMEOUT_MS	10
+#define READ_MSG_TIMEOUT_MS	70
+
+static const u32 crc_table[] = {
+	0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9,
+	0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
+	0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61,
+	0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD
+};
+
+static u32 crc32_word(u32 crc, u32 data, int cnt)
+{
+	int i;
+
+	crc = crc ^ data;
+	for (i = 0; i < cnt; i++)
+		crc = (crc << 4) ^ crc_table[crc >> 28];
+
+	return crc;
+}
+
+u32 crc32(const u8 *buffer, int length, u32 crc)
+{
+	u32 *data = (u32 *)buffer;
+	u32 word;
+	int i;
+
+	/* word by word crc32 */
+	for (i = 0; i < (length >> 2); i++)
+		crc = crc32_word(crc, data[i], 8);
+
+	/* zero pad last word if required */
+	if (length & 0x3) {
+		for (i *= 4, word = 0; i < length; i++)
+			word |= buffer[i] << ((i & 0x3) * 8);
+		crc = crc32_word(crc, word, 8);
+	}
+
+	return crc;
+}
+
+static inline size_t pad(size_t length)
+{
+	return (length + 3) & ~3;
+}
+
+static inline size_t tot_len(size_t length)
+{
+	/* [TYPE:1] [LENGTH:3] [DATA] [PAD:0-3] [CRC:4] */
+	return sizeof(u32) + pad(length) + sizeof(u32);
+}
+
+static struct nanohub_packet_pad *packet_alloc(int flags)
+{
+	int len =
+	    sizeof(struct nanohub_packet_pad) + MAX_UINT8 +
+	    sizeof(struct nanohub_packet_crc);
+	u8 *packet = kmalloc(len, flags);
+
+	if (packet)
+		memset(packet, 0xFF, len);
+	return (struct nanohub_packet_pad *)packet;
+}
+
+static int packet_create(struct nanohub_packet *packet, u32 seq,
+			 u32 reason, u8 len, const u8 *data,
+			 bool user)
+{
+	struct nanohub_packet_crc crc;
+	int ret = sizeof(struct nanohub_packet) + len +
+	    sizeof(struct nanohub_packet_crc);
+
+	if (packet) {
+		packet->sync = COMMS_SYNC;
+		packet->seq = seq;
+		packet->reason = reason;
+		packet->len = len;
+		if (len > 0) {
+			if (user) {
+				if (copy_from_user(packet->data, data, len) !=
+				    0)
+					ret = ERROR_NACK;
+			} else {
+				memcpy(packet->data, data, len);
+			}
+		}
+		crc.crc =
+		    crc32((u8 *)packet,
+			  sizeof(struct nanohub_packet) + len, ~0);
+		memcpy(&packet->data[len], &crc.crc,
+		       sizeof(struct nanohub_packet_crc));
+	} else {
+		ret = ERROR_NACK;
+	}
+
+	return ret;
+}
+
+static int packet_verify(struct nanohub_packet *packet)
+{
+	struct nanohub_packet_crc crc;
+	int cmp;
+
+	crc.crc =
+	    crc32((u8 *)packet,
+		  sizeof(struct nanohub_packet) + packet->len, ~0);
+
+	cmp =
+	    memcmp(&crc.crc, &packet->data[packet->len],
+		   sizeof(struct nanohub_packet_crc));
+
+	if (cmp != 0)
+		pr_debug("nanohub: gen crc: %08x, got crc: %08x\n", crc.crc,
+			 *(u32 *)&packet->data[packet->len]);
+
+	return cmp;
+}
+
+static void packet_free(struct nanohub_packet_pad *packet)
+{
+	kfree(packet);
+}
+
+static int read_ack(struct nanohub_data *data,
+		struct nanohub_packet *response, int timeout)
+{
+	int ret, i;
+	const int max_size = sizeof(struct nanohub_packet) + MAX_UINT8 +
+	    sizeof(struct nanohub_packet_crc);
+	unsigned long end = jiffies + msecs_to_jiffies(READ_ACK_TIMEOUT_MS);
+
+	for (i = 0; time_before(jiffies, end); i++) {
+		ret =
+		    data->comms.read(data, (u8 *)response, max_size,
+				     timeout);
+
+		if (ret == 0) {
+			pr_debug("%s: %d: empty packet\n", __func__, i);
+			ret = ERROR_NACK;
+			continue;
+		} else if (ret < sizeof(struct nanohub_packet)) {
+			pr_debug("%s: %d: too small\n", __func__, i);
+			ret = ERROR_NACK;
+			continue;
+		} else if (ret <
+			   sizeof(struct nanohub_packet) + response->len +
+			   sizeof(struct nanohub_packet_crc)) {
+			pr_debug("%s: %d: too small length\n", __func__,
+				 i);
+			ret = ERROR_NACK;
+			continue;
+		} else if (ret !=
+			   sizeof(struct nanohub_packet) + response->len +
+			   sizeof(struct nanohub_packet_crc)) {
+			pr_debug("%s: %d: wrong length\n", __func__, i);
+			ret = ERROR_NACK;
+			break;
+		} else if (packet_verify(response) != 0) {
+			pr_debug("%s: %d: invalid crc\n", __func__, i);
+			ret = ERROR_NACK;
+			break;
+		}
+		break;
+	}
+
+	return ret;
+}
+
+static int read_msg(struct nanohub_data *data,
+		struct nanohub_packet *response, int timeout)
+{
+	int ret, i;
+	const int max_size = sizeof(struct nanohub_packet) + MAX_UINT8 +
+	    sizeof(struct nanohub_packet_crc);
+	unsigned long end = jiffies + msecs_to_jiffies(READ_MSG_TIMEOUT_MS);
+
+	for (i = 0; time_before(jiffies, end); i++) {
+		ret =
+		    data->comms.read(data, (u8 *)response, max_size,
+				     timeout);
+
+		if (ret == 0) {
+			pr_debug("%s: %d: empty packet\n", __func__, i);
+			ret = ERROR_NACK;
+			continue;
+		} else if (ret < sizeof(struct nanohub_packet)) {
+			pr_debug("%s: %d: too small\n", __func__, i);
+			ret = ERROR_NACK;
+			continue;
+		} else if (ret <
+			   sizeof(struct nanohub_packet) + response->len +
+			   sizeof(struct nanohub_packet_crc)) {
+			pr_debug("%s: %d: too small length\n", __func__,
+				 i);
+			ret = ERROR_NACK;
+			continue;
+		} else if (ret !=
+			   sizeof(struct nanohub_packet) + response->len +
+			   sizeof(struct nanohub_packet_crc)) {
+			pr_debug("%s: %d: wrong length\n", __func__, i);
+			ret = ERROR_NACK;
+			break;
+		} else if (packet_verify(response) != 0) {
+			pr_debug("%s: %d: invalid crc\n", __func__, i);
+			ret = ERROR_NACK;
+			break;
+		}
+		break;
+	}
+
+	return ret;
+}
+
+static int get_reply(struct nanohub_data *data,
+		struct nanohub_packet *response, u32 seq)
+{
+	int ret;
+
+	ret = read_ack(data, response, data->comms.timeout_ack);
+
+	if (ret >= 0 && response->seq == seq) {
+		if (response->reason == CMD_COMMS_ACK) {
+			if (response->len == sizeof(data->interrupts))
+				memcpy(data->interrupts, response->data,
+				       response->len);
+			ret =
+			    read_msg(data, response, data->comms.timeout_reply);
+			if (ret < 0)
+				ret = ERROR_NACK;
+		} else {
+			if (response->reason == CMD_COMMS_NACK)
+				ret = ERROR_NACK;
+			else if (response->reason == CMD_COMMS_BUSY)
+				ret = ERROR_BUSY;
+		}
+
+		if (response->seq != seq)
+			ret = ERROR_NACK;
+	} else {
+		ret = ERROR_NACK;
+	}
+	return ret;
+}
+
+static int nanohub_comms_tx_rx(struct nanohub_data *data,
+			       struct nanohub_packet_pad *pad, int packet_size,
+			       u32 seq, u8 *rx, size_t rx_len)
+{
+	int ret;
+
+	ret = data->comms.write(data, (u8 *)&pad->packet, packet_size,
+				data->comms.timeout_write);
+
+	if (ret == packet_size) {
+		ret = get_reply(data, &pad->packet, seq);
+
+		if (ret >= 0) {
+			if (pad->packet.len > 0) {
+				if (pad->packet.len > rx_len) {
+					memcpy(rx, pad->packet.data, rx_len);
+					ret = rx_len;
+				} else {
+					memcpy(rx, pad->packet.data,
+					       pad->packet.len);
+					ret = pad->packet.len;
+				}
+			} else {
+				ret = 0;
+			}
+		}
+	} else {
+		ret = ERROR_NACK;
+	}
+
+	return ret;
+}
+
+int nanohub_comms_rx_retrans_boottime(struct nanohub_data *data,
+						u32 cmd, u8 *rx, size_t rx_len,
+						int retrans_cnt,
+						int retrans_delay)
+{
+	int packet_size = 0;
+	struct nanohub_packet_pad *pad = packet_alloc(GFP_KERNEL);
+	int delay = 0;
+	int ret;
+	u32 seq;
+	s64 boottime;
+
+	if (!pad)
+		return ERROR_NACK;
+
+	mutex_lock(&data->comms_lock);
+	seq = data->comms.seq++;
+
+	do {
+		data->comms.open(data);
+		boottime = ktime_get_boot_ns();
+		packet_size =
+		    packet_create(&pad->packet, seq, cmd, sizeof(boottime),
+				  (u8 *)&boottime, false);
+
+		ret =
+		    nanohub_comms_tx_rx(data, pad, packet_size, seq, rx,
+					rx_len);
+
+		if (nanohub_wakeup_eom(data,
+				       (ret == ERROR_BUSY) ||
+				       (ret == ERROR_NACK && retrans_cnt >= 0)))
+			ret = -EFAULT;
+
+		data->comms.close(data);
+
+		if (ret == ERROR_NACK) {
+			retrans_cnt--;
+			delay += retrans_delay;
+			if (retrans_cnt >= 0)
+				udelay(retrans_delay);
+		} else if (ret == ERROR_BUSY) {
+			usleep_range(100000, 200000);
+		}
+	} while ((ret == ERROR_BUSY) ||
+		(ret == ERROR_NACK && retrans_cnt >= 0));
+
+	mutex_unlock(&data->comms_lock);
+	packet_free(pad);
+
+	return ret;
+}
+
+int nanohub_comms_tx_rx_retrans(struct nanohub_data *data, u32 cmd,
+				const u8 *tx, u8 tx_len,
+				u8 *rx, size_t rx_len, bool user,
+				int retrans_cnt, int retrans_delay)
+{
+	int packet_size = 0;
+	struct nanohub_packet_pad *pad = packet_alloc(GFP_KERNEL);
+	int delay = 0;
+	int ret;
+	u32 seq;
+
+	if (!pad)
+		return ERROR_NACK;
+
+	mutex_lock(&data->comms_lock);
+	seq = data->comms.seq++;
+
+	do {
+		packet_size =
+		    packet_create(&pad->packet, seq, cmd, tx_len, tx, user);
+
+		data->comms.open(data);
+		ret =
+		    nanohub_comms_tx_rx(data, pad, packet_size, seq, rx,
+					rx_len);
+
+		if (nanohub_wakeup_eom(data,
+				       (ret == ERROR_BUSY) ||
+				       (ret == ERROR_NACK && retrans_cnt >= 0)))
+			ret = -EFAULT;
+
+		data->comms.close(data);
+
+		if (ret == ERROR_NACK) {
+			retrans_cnt--;
+			delay += retrans_delay;
+			if (retrans_cnt >= 0)
+				udelay(retrans_delay);
+		} else if (ret == ERROR_BUSY) {
+			usleep_range(100000, 200000);
+		}
+	} while ((ret == ERROR_BUSY) ||
+		(ret == ERROR_NACK && retrans_cnt >= 0));
+
+	mutex_unlock(&data->comms_lock);
+	packet_free(pad);
+
+	return ret;
+}
+
+struct firmware_header {
+	u32 size;
+	u32 crc;
+	u8 type;
+} __packed;
+
+struct firmware_chunk {
+	u32 offset;
+	u8 data[252 - sizeof(u32)];
+} __packed;
+
+static int nanohub_comms_download(struct nanohub_data *data,
+				  const u8 *image, size_t length,
+				  u8 type)
+{
+	u8 accepted;
+	struct firmware_header header;
+	struct firmware_chunk chunk;
+	int max_chunk_size = sizeof(chunk.data);
+	int chunk_size;
+	u32 offset = 0;
+	int ret;
+	u8 chunk_reply = 0, upload_reply = 0;
+	u32 clear_interrupts[8] = { 0x00000008 };
+
+	header.type = type;
+	header.size = cpu_to_le32(length);
+	header.crc = cpu_to_le32(~crc32(image, length, ~0));
+
+	if (request_wakeup(data))
+		return -ERESTARTSYS;
+	ret = nanohub_comms_tx_rx_retrans(data, CMD_COMMS_START_KERNEL_UPLOAD,
+					  (const u8 *)&header,
+					  sizeof(header), &accepted,
+					  sizeof(accepted), false, 10, 10);
+	release_wakeup(data);
+
+	if (ret == 1 && accepted == 1) {
+		do {
+			if (request_wakeup(data))
+				continue;
+
+			chunk.offset = cpu_to_le32(offset);
+			if (offset + max_chunk_size > length)
+				chunk_size = length - offset;
+			else
+				chunk_size = max_chunk_size;
+			memcpy(chunk.data, image + offset, chunk_size);
+
+			ret =
+			    nanohub_comms_tx_rx_retrans(data,
+							CMD_COMMS_KERNEL_CHUNK,
+							(const u8 *)&chunk,
+							sizeof(u32) +
+							chunk_size,
+							&chunk_reply,
+							sizeof(chunk_reply),
+							false, 10, 10);
+
+			pr_debug("nanohub: ret=%d, chunk_reply=%d, offset=%d\n",
+				 ret, chunk_reply, offset);
+			if (ret == sizeof(chunk_reply)) {
+				if (chunk_reply == CHUNK_REPLY_ACCEPTED) {
+					offset += chunk_size;
+				} else if (chunk_reply == CHUNK_REPLY_WAIT) {
+					ret = nanohub_wait_for_interrupt(data);
+					if (ret < 0) {
+						release_wakeup(data);
+						continue;
+					}
+					nanohub_comms_tx_rx_retrans(data,
+						CMD_COMMS_CLR_GET_INTR,
+						(u8 *)clear_interrupts,
+						sizeof(clear_interrupts),
+						(u8 *)data->interrupts,
+						sizeof(data->interrupts),
+						false, 10, 0);
+				} else if (chunk_reply == CHUNK_REPLY_RESEND) {
+					;
+				} else if (chunk_reply == CHUNK_REPLY_RESTART) {
+					offset = 0;
+				} else if (chunk_reply == CHUNK_REPLY_CANCEL ||
+					(chunk_reply ==
+					CHUNK_REPLY_CANCEL_NO_RETRY)) {
+					release_wakeup(data);
+					break;
+				}
+			} else if (ret <= 0) {
+				release_wakeup(data);
+				break;
+			}
+			release_wakeup(data);
+		} while (offset < length);
+	}
+
+	do {
+		if (request_wakeup(data)) {
+			ret = sizeof(upload_reply);
+			upload_reply = UPLOAD_REPLY_PROCESSING;
+			continue;
+		}
+		ret = nanohub_comms_tx_rx_retrans(data,
+			CMD_COMMS_FINISH_KERNEL_UPLOAD, NULL, 0,
+			&upload_reply, sizeof(upload_reply), false, 10, 10);
+		release_wakeup(data);
+	} while (ret == sizeof(upload_reply) &&
+		 upload_reply == UPLOAD_REPLY_PROCESSING);
+
+	pr_info("%s: ret=%d, upload_reply=%d\n", __func__,
+		ret, upload_reply);
+
+	return 0;
+}
+
+int nanohub_comms_kernel_download(struct nanohub_data *data,
+				  const u8 *image, size_t length)
+{
+	return nanohub_comms_download(data, image, length,
+				      COMMS_FLASH_KERNEL_ID);
+}
+
+int nanohub_comms_app_download(struct nanohub_data *data, const u8 *image,
+			       size_t length)
+{
+	return nanohub_comms_download(data, image, length, COMMS_FLASH_APP_ID);
+}

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/comms.h

@@ -0,0 +1,133 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+
+#ifndef _NANOHUB_COMMS_H
+#define _NANOHUB_COMMS_H
+
+struct __attribute__ ((__packed__)) nanohub_packet {
+	u8 sync;
+	u32 seq;
+	u32 reason;
+	u8 len;
+	u8 data[];
+};
+
+struct __attribute__ ((__packed__)) nanohub_packet_pad {
+	u8 pad[3];
+	struct nanohub_packet
+	 packet;
+};
+
+struct __attribute__ ((__packed__)) nanohub_packet_crc {
+	u32 crc;
+};
+
+struct nanohub_data;
+
+struct nanohub_comms {
+	struct semaphore sem;
+	u32 seq;
+	int timeout_write;
+	int timeout_ack;
+	int timeout_reply;
+	int (*open)(void *data);
+	void (*close)(void *data);
+	int (*write)(void *data, u8 *buffer, int len, int timeout);
+	int (*read)(void *data, u8 *buffer, int len, int timeout);
+
+	union {
+		struct i2c_client *i2c_client;
+		struct spi_device *spi_device;
+	};
+
+	u8 *tx_buffer;
+	u8 *rx_buffer;
+};
+
+int nanohub_comms_kernel_download(struct nanohub_data *data,
+				const u8 *image, size_t length);
+int nanohub_comms_app_download(struct nanohub_data *data,
+				const u8 *image, size_t length);
+int nanohub_comms_rx_retrans_boottime(struct nanohub_data *data,
+						u32 cmd, u8 *rx, size_t rx_len,
+						int retrans_cnt,
+						int retrans_delay);
+int nanohub_comms_tx_rx_retrans(struct nanohub_data *data, u32 cmd,
+				const u8 *tx, u8 tx_len,
+				u8 *rx, size_t rx_len, bool user,
+				int retrans_cnt, int retrans_delay);
+
+#define ERROR_NACK			-1
+#define ERROR_BUSY			-2
+
+#define MAX_UINT8			((1 << (8 * sizeof(u8))) - 1)
+
+#define COMMS_SYNC			0x31
+#define COMMS_FLASH_KERNEL_ID		0x1
+#define COMMS_FLASH_EEDATA_ID		0x2
+#define COMMS_FLASH_APP_ID		0x4
+
+#define CMD_COMMS_ACK			0x00000000
+#define CMD_COMMS_NACK			0x00000001
+#define CMD_COMMS_BUSY			0x00000002
+
+#define CMD_COMMS_GET_OS_HW_VERSIONS	0x00001000
+#define CMD_COMMS_GET_APP_VERSIONS	0x00001001
+#define CMD_COMMS_QUERY_APP_INFO	0x00001002
+
+#define CMD_COMMS_START_KERNEL_UPLOAD	0x00001040
+#define CMD_COMMS_KERNEL_CHUNK		0x00001041
+#define CMD_COMMS_FINISH_KERNEL_UPLOAD	0x00001042
+
+#define CMD_COMMS_START_APP_UPLOAD	0x00001050
+#define CMD_COMMS_APP_CHUNK		0x00001051
+
+#define CMD_COMMS_CLR_GET_INTR		0x00001080
+#define CMD_COMMS_MASK_INTR		0x00001081
+#define CMD_COMMS_UNMASK_INTR		0x00001082
+#define CMD_COMMS_READ			0x00001090
+#define CMD_COMMS_WRITE			0x00001091
+
+#define CHUNK_REPLY_ACCEPTED		0
+#define CHUNK_REPLY_WAIT                1
+#define CHUNK_REPLY_RESEND              2
+#define CHUNK_REPLY_RESTART             3
+#define CHUNK_REPLY_CANCEL              4
+#define CHUNK_REPLY_CANCEL_NO_RETRY     5
+
+#define UPLOAD_REPLY_SUCCESS			0
+#define UPLOAD_REPLY_PROCESSING			1
+#define UPLOAD_REPLY_WAITING_FOR_DATA		2
+#define UPLOAD_REPLY_APP_SEC_KEY_NOT_FOUND	3
+#define UPLOAD_REPLY_APP_SEC_HEADER_ERROR	4
+#define UPLOAD_REPLY_APP_SEC_TOO_MUCH_DATA	5
+#define UPLOAD_REPLY_APP_SEC_TOO_LITTLE_DATA	6
+#define UPLOAD_REPLY_APP_SEC_SIG_VERIFY_FAIL	7
+#define UPLOAD_REPLY_APP_SEC_SIG_DECODE_FAIL	8
+#define UPLOAD_REPLY_APP_SEC_SIG_ROOT_UNKNOWN	9
+#define UPLOAD_REPLY_APP_SEC_MEMORY_ERROR	10
+#define UPLOAD_REPLY_APP_SEC_INVALID_DATA	11
+#define UPLOAD_REPLY_APP_SEC_BAD		12
+
+static inline int nanohub_comms_write(struct nanohub_data *data,
+				      const u8 *buffer, size_t buffer_len)
+{
+	u8 ret;
+
+	if (nanohub_comms_tx_rx_retrans
+	    (data, CMD_COMMS_WRITE, buffer, buffer_len, &ret, sizeof(ret), true,
+	     10, 10) == sizeof(ret)) {
+		if (ret)
+			return buffer_len;
+		else
+			return 0;
+	} else {
+		return ERROR_NACK;
+	}
+}
+
+ssize_t nanohub_external_write(const char *buffer, size_t length);
+#endif

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/main.c

@@ -0,0 +1,161 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/firmware.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/delay.h>
+#include <linux/of_gpio.h>
+#include <linux/of_irq.h>
+#include <linux/interrupt.h>
+#include <linux/poll.h>
+#include <linux/list.h>
+#include <linux/vmalloc.h>
+#include <linux/spinlock.h>
+#include <linux/semaphore.h>
+#include <uapi/linux/sched/types.h>
+#include <linux/sched/rt.h>
+
+#include "main.h"
+#include "comms.h"
+#include "nanohub-mtk.h"
+#include "nanohub.h"
+
+#define WAKEUP_TIMEOUT_MS 1000
+
+static struct nanohub_data *g_nanohub_data_p;
+
+int nanohub_wait_for_interrupt(struct nanohub_data *data)
+{
+	return 0;
+}
+
+int nanohub_wakeup_eom(struct nanohub_data *data, bool repeat)
+{
+	return 0;
+}
+
+static void __nanohub_interrupt_cfg(struct nanohub_data *data,
+				    u8 interrupt, bool mask)
+{
+	int ret;
+	u8 mask_ret = 0;
+	int cnt = 10;
+	int cmd = mask ? CMD_COMMS_MASK_INTR : CMD_COMMS_UNMASK_INTR;
+
+	do {
+		ret = request_wakeup_timeout(data, WAKEUP_TIMEOUT_MS);
+		if (ret)
+			return;
+
+		ret =
+		    nanohub_comms_tx_rx_retrans(data, cmd,
+						&interrupt, sizeof(interrupt),
+						&mask_ret, sizeof(mask_ret),
+						false, 10, 0);
+		release_wakeup(data);
+	} while ((ret != 1 || mask_ret != 1) && --cnt > 0);
+}
+
+static inline void nanohub_mask_interrupt(struct nanohub_data *data,
+					  u8 interrupt)
+{
+	__nanohub_interrupt_cfg(data, interrupt, true);
+}
+
+static inline void nanohub_unmask_interrupt(struct nanohub_data *data,
+					    u8 interrupt)
+{
+	__nanohub_interrupt_cfg(data, interrupt, false);
+}
+
+ssize_t nanohub_external_write(const char *buffer, size_t length)
+{
+	struct nanohub_data *data = g_nanohub_data_p;
+	int ret;
+	u8 ret_data;
+
+	if (request_wakeup(data))
+		return -ERESTARTSYS;
+
+	if (nanohub_comms_tx_rx_retrans
+		(data, CMD_COMMS_WRITE, buffer, length, &ret_data,
+		sizeof(ret_data), false,
+		10, 10) == sizeof(ret_data)) {
+		if (ret_data)
+			ret = length;
+		else
+			ret = 0;
+	} else {
+		ret = ERROR_NACK;
+	}
+
+	release_wakeup(data);
+
+	return ret;
+}
+
+struct nanohub_device *
+nanohub_probe(struct device *dev, struct nanohub_device *nano_dev)
+{
+	/* const struct nanohub_platform_data *pdata;*/
+	struct nanohub_data *data;
+
+	data = nano_dev->drv_data;
+	g_nanohub_data_p = data;
+	data->nanohub_dev = nano_dev;
+	/* data->pdata = pdata; */
+	data->pdata = devm_kzalloc(dev, sizeof(struct nanohub_platform_data),
+		GFP_KERNEL);
+
+	mutex_init(&data->comms_lock);
+
+	usleep_range(25, 30);
+
+	return nano_dev;
+}
+
+int nanohub_suspend(struct nanohub_device *nano_dev)
+{
+	struct nanohub_data *data = nano_dev->drv_data;
+
+	nanohub_mask_interrupt(data, 2);
+	return 0;
+}
+
+int nanohub_resume(struct nanohub_device *nano_dev)
+{
+	struct nanohub_data *data = nano_dev->drv_data;
+
+	nanohub_unmask_interrupt(data, 2);
+	return 0;
+}
+
+int nanohub_init(void)
+{
+	int ret = 0;
+
+#ifdef CONFIG_NANOHUB_I2C
+	if (ret == 0)
+		ret = nanohub_i2c_init();
+#endif
+#ifdef CONFIG_NANOHUB_SPI
+	if (ret == 0)
+		ret = nanohub_spi_init();
+#endif
+#ifdef CONFIG_NANOHUB_MTK_IPI
+		ret = nanohub_ipi_init();
+#endif
+	return ret;
+}
+
+MODULE_AUTHOR("Ben Fennema");
+MODULE_LICENSE("GPL");

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/main.h

@@ -0,0 +1,94 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+
+#ifndef _NANOHUB_MAIN_H
+#define _NANOHUB_MAIN_H
+
+#include <linux/kernel.h>
+#include <linux/cdev.h>
+#include <linux/gpio.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/semaphore.h>
+#include <linux/pm_wakeup.h>
+
+#include "comms.h"
+
+#define NANOHUB_NAME "nanohub"
+
+struct nanohub_device {
+	void *drv_data;
+};
+
+struct nanohub_data;
+
+struct nanohub_io {
+	struct device *dev;
+	struct nanohub_data *data;
+	wait_queue_head_t buf_wait;
+	struct list_head buf_list;
+};
+
+struct nanohub_data {
+	/* indices for io[] array */
+	#define ID_NANOHUB_SENSOR 0
+	#define ID_NANOHUB_COMMS 1
+	#define ID_NANOHUB_MAX 2
+
+	struct nanohub_device *nanohub_dev;
+	struct nanohub_io io[ID_NANOHUB_MAX];
+
+	struct nanohub_comms comms;
+	const struct nanohub_platform_data *pdata;
+	int irq1;
+	int irq2;
+
+	atomic_t kthread_run;
+	atomic_t thread_state;
+	wait_queue_head_t kthread_wait;
+
+	struct wakeup_source ws;
+
+	struct nanohub_io free_pool;
+
+	atomic_t lock_mode;
+	/* these 3 vars should be accessed only with wakeup_wait.lock held */
+	atomic_t wakeup_cnt;
+	atomic_t wakeup_lock_cnt;
+	atomic_t wakeup_acquired;
+	wait_queue_head_t wakeup_wait;
+
+	u32 interrupts[8];
+
+	int err_cnt;
+	void *vbuf;
+	struct task_struct *thread;
+
+	struct mutex comms_lock;
+};
+
+int nanohub_wait_for_interrupt(struct nanohub_data *data);
+int nanohub_wakeup_eom(struct nanohub_data *data, bool repeat);
+struct nanohub_device *
+nanohub_probe(struct device *dev, struct nanohub_device *nano_dev);
+int nanohub_suspend(struct nanohub_device *nano_dev);
+int nanohub_resume(struct nanohub_device *nano_dev);
+
+static inline int request_wakeup_timeout(struct nanohub_data *data, int timeout)
+{
+	return 0;
+}
+
+static inline int request_wakeup(struct nanohub_data *data)
+{
+	return 0;
+}
+
+static inline void release_wakeup(struct nanohub_data *data)
+{
+}
+
+#endif

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/nanohub-mtk.c

@@ -0,0 +1,313 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/completion.h>
+#include <linux/workqueue.h>
+#include <linux/slab.h>
+#include <scp.h>
+
+#include "main.h"
+#include "comms.h"
+#include "nanohub-mtk.h"
+
+#define CHRE_IPI_DEBUG	0
+struct nanohub_ipi_rx_st {
+	u8 *buff;
+	int copy_size;
+	struct completion isr_comp;
+};
+
+struct nanohub_ipi_rx_st nanohub_ipi_rx;
+
+struct semaphore scp_nano_ipi_sem;
+
+struct nanohub_ipi_data {
+	struct nanohub_data data;
+	struct nanohub_device *nanohub_dev;
+	/* todo */
+};
+
+static struct nanohub_ipi_data *g_nanohub_ipi_data;
+
+/* scp_nano_ipi_status: 1 :ready to ipi  0:not ready*/
+int scp_nano_ipi_status;
+
+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;
+}
+
+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;
+}
+
+#define NANOHUB_IPI_SEND_RETRY 100
+void mtk_ipi_scp_isr_sim(int got_size)
+{
+	int token = got_size;
+	int ret;
+	int retry = NANOHUB_IPI_SEND_RETRY;
+	/* add retry to avoid SCP busy timeout */
+	while (retry-- && (READ_ONCE(scp_nano_ipi_status) == 1)) {
+		ret = scp_ipi_send(IPI_CHREX, &token, sizeof(token),
+			0, SCP_A_ID);
+		if (ret != SCP_IPI_BUSY)
+			break;
+		usleep_range(100, 200);
+	}
+}
+
+static void nano_ipi_start(void)
+{
+	WRITE_ONCE(scp_nano_ipi_status, 1);
+}
+
+static void nano_ipi_stop(void)
+{
+	WRITE_ONCE(scp_nano_ipi_status, 0);
+}
+
+static int nano_ipi_event(struct notifier_block *this,
+	unsigned long event, void *ptr)
+{
+	switch (event) {
+	case SCP_EVENT_READY:
+		nano_ipi_start();
+		break;
+	case SCP_EVENT_STOP:
+		nano_ipi_stop();
+		break;
+	}
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block nano_ipi_notifier = {
+	.notifier_call = nano_ipi_event,
+};
+
+int nanohub_ipi_write(void *data, u8 *tx, int length, int timeout)
+{
+	int ret;
+	int retry = NANOHUB_IPI_SEND_RETRY;
+#if CHRE_IPI_DEBUG
+	int i;
+
+	pr_debug("nanohub AP send ipi(%d) ", length);
+	for (i = 0; i < length; i++)
+		pr_debug("%02x ", tx[i]);
+	pr_debug("\n");
+#endif
+	ret = SCP_IPI_ERROR;
+	while (retry-- && (READ_ONCE(scp_nano_ipi_status) == 1)) {
+		ret = scp_ipi_send(IPI_CHRE, tx, length, 0, SCP_A_ID);
+		if (ret != SCP_IPI_BUSY)
+			break;
+		usleep_range(100, 200);
+	}
+
+	if (ret == SCP_IPI_BUSY)
+		pr_debug("nanohub ipi busy, ret=%d\n", ret);
+
+	if (ret == SCP_IPI_DONE)
+		return length;
+	else
+		return ERROR_NACK;
+}
+
+int nanohub_ipi_read(void *data, u8 *rx, int max_length, int timeout)
+{
+	int ret;
+	const int min_size = sizeof(struct nanohub_packet) +
+		 sizeof(struct nanohub_packet_crc);
+
+	if (max_length < min_size)
+		return -1;
+	/* todo: support interruptible? please check it! */
+	if (wait_for_completion_interruptible_timeout(&nanohub_ipi_rx.isr_comp,
+		timeout) == 0) {
+		ret = 0;	/* return as empty packet */
+	} else {
+		ret = nanohub_ipi_rx.copy_size;
+		memcpy(rx, g_nanohub_ipi_data->data.comms.rx_buffer, ret);
+		/* send back isr sim */
+		mtk_ipi_scp_isr_sim(ret);
+	}
+#if CHRE_IPI_DEBUG
+	pr_debug("%s ret %d\n", __func__, ret);
+#endif
+	return ret;	/* return packet size */
+}
+
+static int nanohub_ipi_open(void *data)
+{
+	down(&scp_nano_ipi_sem);
+	reinit_completion(&nanohub_ipi_rx.isr_comp); /*reset when retry start*/
+	return 0;
+}
+
+static void nanohub_ipi_close(void *data)
+{
+	up(&scp_nano_ipi_sem);
+}
+
+static void nanohub_ipi_comms_init(struct nanohub_ipi_data *ipi_data)
+{
+	struct nanohub_comms *comms = &ipi_data->data.comms;
+
+	comms->seq = 1;
+	comms->timeout_write = msecs_to_jiffies(512);
+	comms->timeout_ack = msecs_to_jiffies(3);
+	comms->timeout_reply = msecs_to_jiffies(3);
+	comms->open = nanohub_ipi_open;
+	comms->close = nanohub_ipi_close;
+	comms->write = nanohub_ipi_write;
+	comms->read = nanohub_ipi_read;
+/*	comms->tx_buffer = kmalloc(4096, GFP_KERNEL | GFP_DMA); */
+	comms->rx_buffer = kmalloc(4096, GFP_KERNEL | GFP_DMA);
+	WARN_ON(comms->rx_buffer == NULL);
+	nanohub_ipi_rx.buff = comms->rx_buffer;
+	sema_init(&scp_nano_ipi_sem, 1);
+}
+
+static int nanohub_ipi_remove(struct platform_device *pdev);
+
+struct platform_device nanohub_ipi_pdev = {
+	.name = "nanohub_ipi",
+	.id = -1,
+};
+
+int nanohub_ipi_suspend(struct platform_device *dev, pm_message_t state)
+{
+	int ret = 0;
+
+	if (READ_ONCE(scp_nano_ipi_status) == 1)
+		ret = nanohub_suspend(g_nanohub_ipi_data->nanohub_dev);
+	else
+		ret = 0;
+
+	return ret;
+}
+
+int nanohub_ipi_resume(struct platform_device *dev)
+{
+	int ret = 0;
+
+	if (READ_ONCE(scp_nano_ipi_status) == 1)
+		ret = nanohub_resume(g_nanohub_ipi_data->nanohub_dev);
+	else
+		ret = 0;
+
+	return ret;
+}
+
+void scp_to_ap_ipi_handler(int id, void *data, unsigned int len)
+{
+#if CHRE_IPI_DEBUG
+	int i;
+	unsigned char *data_p = data;
+
+	pr_info("%s %d, IPI_CHRE ->AP(len:%d)\n", __func__, __LINE__, len);
+	for (i = 0; i < len; i++)
+		pr_debug("%02x ", data_p[i]);
+	pr_debug("\n");
+#endif
+	nanohub_ipi_rx.copy_size = len;
+	memcpy(g_nanohub_ipi_data->data.comms.rx_buffer, data, len);
+	/*todo: check size ? */
+	complete(&nanohub_ipi_rx.isr_comp);
+}
+
+int nanohub_ipi_probe(struct platform_device *pdev)
+{
+	struct nanohub_ipi_data *ipi_data;
+	struct nanohub_device *nano_dev;
+	enum scp_ipi_status status;
+
+	nano_dev = kzalloc(sizeof(*nano_dev), GFP_KERNEL);
+	if (!nano_dev)
+		return -ENOMEM;
+
+	ipi_data = kzalloc(sizeof(*ipi_data), GFP_KERNEL);
+	if (!ipi_data) {
+		kfree(nano_dev);
+		return -ENOMEM;
+	}
+
+	ipi_data->nanohub_dev = nano_dev;
+	nano_dev->drv_data = &ipi_data->data;
+	nanohub_probe(&pdev->dev, nano_dev);
+	platform_set_drvdata(pdev, &ipi_data->data.free_pool);
+	g_nanohub_ipi_data = ipi_data;
+
+	nanohub_ipi_comms_init(ipi_data);
+	init_completion(&nanohub_ipi_rx.isr_comp);
+	status = scp_ipi_registration(IPI_CHRE,
+		scp_to_ap_ipi_handler, "chre_ap_rx");
+
+	if (status != SCP_IPI_DONE) {
+		pr_err("nonahub: IPI_CHRE(%d) register fail, err:%d\n",
+			   IPI_CHRE, status);
+		goto exit_kfree;
+	}
+	/*init nano scp ipi status*/
+	WRITE_ONCE(scp_nano_ipi_status, 1);
+	scp_A_register_notify(&nano_ipi_notifier);
+
+	return 0;
+
+exit_kfree:
+	kfree(nano_dev);
+	kfree(ipi_data);
+	return status;
+}
+
+static int nanohub_ipi_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static struct platform_driver nanohub_ipi_pdrv = {
+	.probe = nanohub_ipi_probe,
+	.remove = nanohub_ipi_remove,
+	.suspend = nanohub_ipi_suspend,
+	.resume = nanohub_ipi_resume,
+	.driver = {
+		.name = "nanohub_ipi",
+		.owner = THIS_MODULE,
+		/*.of_match_table = scpdvfs_of_ids,*/
+	},
+};
+
+int nanohub_ipi_init(void)
+{
+	int ret = 0;
+
+	ret = platform_device_register(&nanohub_ipi_pdev);
+	if (ret) {
+		pr_err("nanohub_ipi platform dev register fail,ret:%d\n", ret);
+		goto err_exit;
+	}
+
+	ret = platform_driver_register(&nanohub_ipi_pdrv);
+	if (ret) {
+		pr_warn("nanohub_ipi platform driver register fail,ret:%d\n", ret);
+		goto err_unregister_device;
+	}
+
+	return 0;
+
+err_unregister_device:
+	platform_device_unregister(&nanohub_ipi_pdev);
+err_exit:
+	return ret;
+}

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/nanohub-mtk.h

@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _NANOHUB_MTK_IPI_H
+#define _NANOHUB_MTK_IPI_H
+
+#include "scp.h"
+
+int nanohub_ipi_init(void);
+
+#endif
+

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/nanohub/nanohub.h

@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ */
+
+#ifndef __LINUX_PLATFORM_DATA_NANOHUB_H
+#define __LINUX_PLATFORM_DATA_NANOHUB_H
+
+#include <linux/types.h>
+
+struct nanohub_flash_bank {
+	int bank;
+	u32 address;
+	size_t length;
+};
+
+struct nanohub_platform_data {
+	u32 wakeup_gpio;
+	u32 nreset_gpio;
+	u32 boot0_gpio;
+	u32 irq1_gpio;
+	u32 irq2_gpio;
+	u32 spi_cs_gpio;
+	u32 bl_addr;
+	u32 num_flash_banks;
+	struct nanohub_flash_bank *flash_banks;
+	u32 num_shared_flash_banks;
+	struct nanohub_flash_bank *shared_flash_banks;
+};
+
+#endif /* __LINUX_PLATFORM_DATA_NANOHUB_H */

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/sensor_list.c

@@ -0,0 +1,93 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "<sensorlist> " fmt
+
+#include <linux/module.h>
+
+#include "sensor_list.h"
+#include "hf_sensor_type.h"
+
+enum sensorlist {
+	accel,
+	gyro,
+	mag,
+	als,
+	ps,
+	baro,
+	sar,
+	ois,
+	maxhandle,
+};
+
+int sensorlist_sensor_to_handle(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;
+	case SENSOR_TYPE_OIS:
+		handle = ois;
+		break;
+	}
+	return handle;
+}
+
+int sensorlist_handle_to_sensor(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;
+	case ois:
+		type = SENSOR_TYPE_OIS;
+		break;
+	}
+	return type;
+}
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("dynamic sensorlist driver");
+MODULE_AUTHOR("hongxu.zhao@mediatek.com");

drivers/misc/mediatek/sensor/2.0/mtk_nanohub/sensor_list.h

@@ -0,0 +1,12 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _SENSOR_LIST_H_
+#define _SENSOR_LIST_H_
+
+int sensorlist_sensor_to_handle(int sensor);
+int sensorlist_handle_to_sensor(int handle);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/Kconfig

@@ -0,0 +1,7 @@
+config MTK_SENSORHUB
+	tristate "sensorhub architecture for MediaTek package"
+	help
+	  It support mediatek sensorhub architecture.
+	  If this option is set,
+	  it will support
+	  mediatek sensorhub architecture.

drivers/misc/mediatek/sensor/2.0/sensorhub/Makefile

@@ -0,0 +1,17 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/sensorhub
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/scp/include
+
+obj-$(CONFIG_MTK_SENSORHUB) += sensorhub.o
+sensorhub-objs += tiny_crc8.o
+sensorhub-objs += ready.o
+sensorhub-objs += ipi_comm.o
+sensorhub-objs += sensor_comm.o
+sensorhub-objs += sensor_list.o
+sensorhub-objs += transceiver.o
+sensorhub-objs += share_memory.o
+sensorhub-objs += timesync.o
+sensorhub-objs += debug.o
+sensorhub-objs += custom_cmd.o
+
+#obj-y += test/

drivers/misc/mediatek/sensor/2.0/sensorhub/custom_cmd.c

@@ -0,0 +1,293 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2021 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "cust_cmd " fmt
+
+#include <linux/spinlock.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/atomic.h>
+
+#include "sensor_comm.h"
+#include "custom_cmd.h"
+#include "share_memory.h"
+
+static DEFINE_MUTEX(bus_user_lock);
+static atomic_t cust_cmd_sequence;
+static DECLARE_COMPLETION(cust_cmd_done);
+static DEFINE_SPINLOCK(rx_notify_lock);
+static struct sensor_comm_notify rx_notify;
+static DECLARE_COMPLETION(cust_cmd_fast_done);
+static DEFINE_SPINLOCK(rx_fast_notify_lock);
+static struct sensor_comm_notify rx_fast_notify;
+static struct share_mem cust_cmd_shm_rx;
+static struct share_mem cust_cmd_shm_tx;
+
+
+static void custom_cmd_notify_handler(struct sensor_comm_notify *n,
+		void *private_data)
+{
+	spin_lock(&rx_notify_lock);
+	memcpy(&rx_notify, n, sizeof(rx_notify));
+	spin_unlock(&rx_notify_lock);
+	complete(&cust_cmd_done);
+}
+
+static void custom_cmd_fast_notify_handler(struct sensor_comm_notify *n,
+		void *private_data)
+{
+	spin_lock(&rx_fast_notify_lock);
+	memcpy(&rx_fast_notify, n, sizeof(rx_fast_notify));
+	spin_unlock(&rx_fast_notify_lock);
+	complete(&cust_cmd_fast_done);
+}
+
+static int custom_cmd_slow_seq(int sensor_type, struct share_mem_cmd *shm_cmd)
+{
+	int ret = 0;
+	int timeout = 0;
+	unsigned long flags = 0;
+	uint32_t write_position = 0;
+	struct share_mem_notify notify;
+
+	/*
+	 * NOTE: must reinit_completion before sensor_comm_notify
+	 * wrong sequence:
+	 * sensor_comm_notify ---> reinit_completion -> wait_for_completion
+	 *		       |
+	 *		    complete
+	 * if complete before reinit_completion, will lose this complete
+	 * right sequence:
+	 * reinit_completion -> sensor_comm_notify -> wait_for_completion
+	 */
+	reinit_completion(&cust_cmd_done);
+
+	share_mem_write_reset(&cust_cmd_shm_tx);
+	ret = share_mem_write(&cust_cmd_shm_tx, shm_cmd, sizeof(*shm_cmd));
+	if (ret < 0)
+		return ret;
+	if (!ret)
+		return -ENOMEM;
+	/* safe sequence given by atomic, round from 0 to 255 */
+	notify.sequence = atomic_inc_return(&cust_cmd_sequence);
+	notify.sensor_type = sensor_type;
+	notify.notify_cmd = SENS_COMM_NOTIFY_CUSTOM_CMD;
+	ret = share_mem_flush(&cust_cmd_shm_tx, &notify);
+	if (ret < 0)
+		return ret;
+
+	timeout = wait_for_completion_timeout(&cust_cmd_done,
+		msecs_to_jiffies(100));
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	spin_lock_irqsave(&rx_notify_lock, flags);
+	if (rx_notify.sequence != notify.sequence &&
+	    rx_notify.sensor_type != notify.sensor_type &&
+	    rx_notify.command != notify.notify_cmd) {
+		spin_unlock_irqrestore(&rx_notify_lock, flags);
+		return -EREMOTEIO;
+	}
+	write_position = rx_notify.value[0];
+	spin_unlock_irqrestore(&rx_notify_lock, flags);
+
+	ret = share_mem_seek(&cust_cmd_shm_rx, write_position);
+	if (ret < 0)
+		return ret;
+	ret = share_mem_read(&cust_cmd_shm_rx, shm_cmd, sizeof(*shm_cmd));
+	if (ret < 0)
+		return ret;
+	if (!ret)
+		return -ENOMEM;
+	share_mem_read_reset(&cust_cmd_shm_rx);
+	return 0;
+}
+
+static int custom_cmd_slow_comm(int sensor_type, struct custom_cmd *cust_cmd)
+{
+	int retry = 0, ret = 0;
+	const int max_retry = 3;
+	struct share_mem_cmd shm_cmd;
+
+	if (sizeof(shm_cmd) != sizeof(*cust_cmd) ||
+		sizeof(shm_cmd.data) != sizeof(cust_cmd->data) ||
+		offsetof(typeof(shm_cmd), data) !=
+		offsetof(typeof(*cust_cmd), data)) {
+		pr_err("%u %u struct not match\n",
+			sensor_type, cust_cmd->command);
+		return -EINVAL;
+	}
+
+	shm_cmd.command = cust_cmd->command;
+	shm_cmd.tx_len = cust_cmd->tx_len;
+	shm_cmd.rx_len = cust_cmd->rx_len;
+	memcpy(shm_cmd.data, cust_cmd->data, shm_cmd.tx_len);
+
+	mutex_lock(&bus_user_lock);
+	do {
+		ret = custom_cmd_slow_seq(sensor_type, &shm_cmd);
+	} while (retry++ < max_retry && ret < 0);
+	mutex_unlock(&bus_user_lock);
+
+	if (ret < 0) {
+		pr_err("%u %u comm fail %d\n",
+			sensor_type, cust_cmd->command, ret);
+		return ret;
+	}
+	if (shm_cmd.command != cust_cmd->command ||
+		shm_cmd.rx_len > sizeof(shm_cmd.data)) {
+		pr_err("%u %u param error\n",
+			sensor_type, cust_cmd->command);
+		return -EINVAL;
+	}
+
+	cust_cmd->rx_len = shm_cmd.rx_len;
+	memcpy(cust_cmd->data, shm_cmd.data, cust_cmd->rx_len);
+
+	return 0;
+}
+
+static int custom_cmd_fast_seq(int sensor_type, struct custom_cmd *cust_cmd)
+{
+	int ret = 0;
+	int timeout = 0;
+	unsigned long flags = 0;
+	struct sensor_comm_notify notify;
+	struct custom_cmd *rx_cmd = NULL;
+
+	/*
+	 * NOTE: must reinit_completion before sensor_comm_notify
+	 * wrong sequence:
+	 * sensor_comm_notify ---> reinit_completion -> wait_for_completion
+	 *		       |
+	 *		    complete
+	 * if complete before reinit_completion, will lose this complete
+	 * right sequence:
+	 * reinit_completion -> sensor_comm_notify -> wait_for_completion
+	 */
+	reinit_completion(&cust_cmd_fast_done);
+
+	/* safe sequence given by atomic, round from 0 to 255 */
+	notify.sequence = atomic_inc_return(&cust_cmd_sequence);
+	notify.sensor_type = sensor_type;
+	notify.command = SENS_COMM_NOTIFY_FAST_CUST_CMD;
+	notify.length = offsetof(typeof(*cust_cmd), data) + cust_cmd->tx_len;
+	memcpy(notify.value, cust_cmd, notify.length);
+	ret = sensor_comm_notify(&notify);
+	if (ret < 0)
+		return ret;
+
+	timeout = wait_for_completion_timeout(&cust_cmd_fast_done,
+		msecs_to_jiffies(100));
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	spin_lock_irqsave(&rx_fast_notify_lock, flags);
+	if (rx_fast_notify.sequence != notify.sequence &&
+	    rx_fast_notify.sensor_type != notify.sensor_type &&
+	    rx_fast_notify.command != notify.command) {
+		spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
+		return -EREMOTEIO;
+	}
+	rx_cmd = (struct custom_cmd *)rx_fast_notify.value;
+	if (rx_cmd->command != cust_cmd->command ||
+		rx_cmd->rx_len > sizeof(cust_cmd->data) ||
+		rx_cmd->rx_len > cust_cmd->rx_len) {
+		spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
+		return -EIO;
+	}
+	cust_cmd->rx_len = rx_cmd->rx_len;
+	memcpy(cust_cmd->data, rx_cmd->data, cust_cmd->rx_len);
+	spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
+
+	return 0;
+}
+
+static int custom_cmd_fast_comm(int sensor_type, struct custom_cmd *cust_cmd)
+{
+	int retry = 0, ret = 0;
+	const int max_retry = 3;
+
+	mutex_lock(&bus_user_lock);
+	do {
+		ret = custom_cmd_fast_seq(sensor_type, cust_cmd);
+	} while (retry++ < max_retry && ret < 0);
+	mutex_unlock(&bus_user_lock);
+
+	if (ret < 0)
+		pr_err("%u %u fast comm fail %d\n",
+			sensor_type, cust_cmd->command, ret);
+
+	return ret;
+}
+
+int custom_cmd_comm_with(int sensor_type, struct custom_cmd *cust_cmd)
+{
+	int ret = 0;
+	uint8_t header_len = offsetof(typeof(*cust_cmd), data);
+	uint16_t tx_len = header_len + cust_cmd->tx_len;
+	uint16_t rx_len = header_len + cust_cmd->rx_len;
+
+	if (tx_len > sizeof(rx_fast_notify.value) ||
+		rx_len > sizeof(rx_fast_notify.value))
+		ret = custom_cmd_slow_comm(sensor_type, cust_cmd);
+	else
+		ret = custom_cmd_fast_comm(sensor_type, cust_cmd);
+
+	return ret;
+}
+
+static int custom_cmd_w_shm_cfg(struct share_mem_config *cfg,
+		void *private_data)
+{
+	cust_cmd_shm_tx.name = "cust_cmd_w";
+	cust_cmd_shm_tx.item_size = sizeof(struct share_mem_cmd);
+	cust_cmd_shm_tx.buffer_full_detect = false;
+
+	return share_mem_init(&cust_cmd_shm_tx, cfg);
+}
+
+static int custom_cmd_r_shm_cfg(struct share_mem_config *cfg,
+		void *private_data)
+{
+	cust_cmd_shm_rx.name = "cust_cmd_r";
+	cust_cmd_shm_rx.item_size = sizeof(struct share_mem_cmd);
+	cust_cmd_shm_rx.buffer_full_detect = false;
+
+	return share_mem_init(&cust_cmd_shm_rx, cfg);
+}
+
+int custom_cmd_init(void)
+{
+	unsigned long flags = 0;
+
+	atomic_set(&cust_cmd_sequence, 0);
+
+	spin_lock_irqsave(&rx_notify_lock, flags);
+	memset(&rx_notify, 0, sizeof(rx_notify));
+	spin_unlock_irqrestore(&rx_notify_lock, flags);
+
+	spin_lock_irqsave(&rx_fast_notify_lock, flags);
+	memset(&rx_fast_notify, 0, sizeof(rx_fast_notify));
+	spin_unlock_irqrestore(&rx_fast_notify_lock, flags);
+
+	sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_CUSTOM_CMD,
+		custom_cmd_notify_handler, NULL);
+	sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_FAST_CUST_CMD,
+		custom_cmd_fast_notify_handler, NULL);
+	share_mem_config_handler_register(SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE,
+		custom_cmd_w_shm_cfg, NULL);
+	share_mem_config_handler_register(SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE,
+		custom_cmd_r_shm_cfg, NULL);
+	return 0;
+}
+
+void custom_cmd_exit(void)
+{
+	sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_CUSTOM_CMD);
+	sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_FAST_CUST_CMD);
+	share_mem_config_handler_unregister(SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE);
+	share_mem_config_handler_unregister(SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE);
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/custom_cmd.h

@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2021 MediaTek Inc.
+ */
+
+#ifndef _CUSTOM_CMD_H_
+#define _CUSTOM_CMD_H_
+
+#include "hf_sensor_io.h"
+
+int custom_cmd_comm_with(int sensor_type, struct custom_cmd *cust_cmd);
+int custom_cmd_init(void);
+void custom_cmd_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/debug.c

@@ -0,0 +1,166 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "debug " fmt
+
+#include <linux/spinlock.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/atomic.h>
+#include <linux/slab.h>
+
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+#include "debug.h"
+#include "share_memory.h"
+
+static DEFINE_MUTEX(bus_user_lock);
+static atomic_t debug_sequence;
+static DECLARE_COMPLETION(debug_done);
+static DEFINE_SPINLOCK(rx_notify_lock);
+static struct sensor_comm_notify rx_notify;
+static struct share_mem debug_shm_reader;
+
+static void debug_notify_handler(struct sensor_comm_notify *n,
+		void *private_data)
+{
+	spin_lock(&rx_notify_lock);
+	memcpy(&rx_notify, n, sizeof(rx_notify));
+	spin_unlock(&rx_notify_lock);
+	complete(&debug_done);
+}
+
+static int debug_seq_get_debug(uint8_t sensor_type, uint8_t *buffer,
+		unsigned int len)
+{
+	int ret = 0;
+	int timeout = 0;
+	unsigned long flags = 0;
+	struct sensor_comm_ctrl *ctrl = NULL;
+	struct share_mem_debug *shm_debug = NULL;
+	uint32_t write_position = 0;
+
+	ret = share_mem_read_reset(&debug_shm_reader);
+	if (ret < 0) {
+		pr_err("%u reset fail %d\n", sensor_type, ret);
+		return ret;
+	}
+
+	/*
+	 * NOTE: must reinit_completion before sensor_comm_notify
+	 * wrong sequence:
+	 * sensor_comm_ctrl_send ---> reinit_completion -> wait_for_completion
+	 *                        |
+	 *                    complete
+	 * if complete before reinit_completion, will lose this complete
+	 * right sequence:
+	 * reinit_completion -> sensor_comm_ctrl_send -> wait_for_completion
+	 */
+	reinit_completion(&debug_done);
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	ctrl->sensor_type = sensor_type;
+	ctrl->command = SENS_COMM_CTRL_DEBUG_CMD;
+	ctrl->length = sizeof(ctrl->data[0]);
+	/* safe sequence given by atomic, round from 0 to 255 */
+	ctrl->data[0] = atomic_inc_return(&debug_sequence);
+	ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
+	if (ret < 0) {
+		pr_err("%u send fail %d\n", sensor_type, ret);
+		goto out1;
+	}
+
+	timeout = wait_for_completion_timeout(&debug_done,
+		msecs_to_jiffies(100));
+	if (!timeout) {
+		pr_err("%u wait completion timeout\n", sensor_type);
+		ret = -ETIMEDOUT;
+		goto out1;
+	}
+
+	spin_lock_irqsave(&rx_notify_lock, flags);
+	if (rx_notify.sequence != ctrl->data[0] &&
+	    rx_notify.sensor_type != ctrl->sensor_type &&
+	    rx_notify.command != ctrl->command) {
+		pr_err("%u reply fail\n", sensor_type);
+		spin_unlock_irqrestore(&rx_notify_lock, flags);
+		ret = -EREMOTEIO;
+		goto out1;
+	}
+	write_position = rx_notify.value[0];
+	spin_unlock_irqrestore(&rx_notify_lock, flags);
+
+	ret = share_mem_seek(&debug_shm_reader, write_position);
+	if (ret < 0) {
+		pr_err("%u seek fail %d\n", sensor_type, ret);
+		goto out1;
+	}
+	shm_debug = kzalloc(sizeof(*shm_debug), GFP_KERNEL);
+	ret = share_mem_read(&debug_shm_reader, shm_debug, sizeof(*shm_debug));
+	if (ret < 0) {
+		pr_err("%u read fail %d\n", sensor_type, ret);
+		goto out2;
+	} else if (ret != sizeof(*shm_debug)) {
+		pr_err("%u size fail %d\n", sensor_type, ret);
+		ret = -EPROTO;
+		goto out2;
+	}
+	len = len < shm_debug->written ? len : shm_debug->written;
+	strlcpy(buffer, shm_debug->buffer, len);
+	ret = len;
+out2:
+	kfree(shm_debug);
+out1:
+	kfree(ctrl);
+	return ret;
+}
+
+/* SENSOR_TYPE_INVALID for get sensor_manager debug information */
+int debug_get_debug(uint8_t sensor_type, uint8_t *buffer, unsigned int len)
+{
+	int retry = 0, ret = 0;
+	const int max_retry = 3;
+
+	mutex_lock(&bus_user_lock);
+	do {
+		ret = debug_seq_get_debug(sensor_type, buffer, len);
+	} while (retry++ < max_retry && ret < 0);
+	mutex_unlock(&bus_user_lock);
+
+	return ret;
+}
+
+static int debug_share_mem_cfg(struct share_mem_config *cfg,
+		void *private_data)
+{
+	debug_shm_reader.name = "debug_r";
+	debug_shm_reader.item_size = sizeof(struct share_mem_debug);
+	debug_shm_reader.buffer_full_detect = false;
+
+	return share_mem_init(&debug_shm_reader, cfg);
+}
+
+int debug_init(void)
+{
+	unsigned long flags = 0;
+
+	atomic_set(&debug_sequence, 0);
+
+	spin_lock_irqsave(&rx_notify_lock, flags);
+	memset(&rx_notify, 0, sizeof(rx_notify));
+	spin_unlock_irqrestore(&rx_notify_lock, flags);
+
+	sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_DEBUG_CMD,
+		debug_notify_handler, NULL);
+	share_mem_config_handler_register(SHARE_MEM_DEBUG_PAYLOAD_TYPE,
+		debug_share_mem_cfg, NULL);
+	return 0;
+}
+
+void debug_exit(void)
+{
+	sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_DEBUG_CMD);
+	share_mem_config_handler_unregister(SHARE_MEM_DEBUG_PAYLOAD_TYPE);
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/debug.h

@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _SENSOR_DEBUG_H_
+#define _SENSOR_DEBUG_H_
+
+int debug_get_debug(uint8_t sensor_type, uint8_t *buffer, unsigned int len);
+int debug_init(void);
+void debug_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/ipi_comm.c

@@ -0,0 +1,314 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "ipi_comm " fmt
+
+#include <linux/module.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+
+#include "scp.h"
+#include "ipi_comm.h"
+
+struct ipi_controller {
+	spinlock_t lock;
+	bool running;
+	struct list_head head;
+	struct workqueue_struct *workqueue;
+	struct work_struct work;
+	void (*notify_callback)(int id, void *data, unsigned int len);
+};
+
+struct ipi_hw_transfer {
+	struct completion done;
+	int count;
+	/* data buffers */
+	int id;
+	const unsigned char *tx;
+	unsigned char *rx;
+	unsigned int tx_len;
+	unsigned int rx_len;
+	void *context;
+};
+
+#define ipi_len(x) (((x) + MBOX_SLOT_SIZE - 1) / MBOX_SLOT_SIZE)
+
+static struct ipi_controller controller;
+static struct ipi_hw_transfer hw_transfer;
+static DEFINE_SPINLOCK(hw_transfer_lock);
+static uint8_t ctrl_payload[PIN_IN_SIZE_SENSOR_CTRL * MBOX_SLOT_SIZE];
+static uint8_t notify_payload[PIN_IN_SIZE_SENSOR_NOTIFY * MBOX_SLOT_SIZE];
+
+static inline int ipi_retry_transfer(int id, void *tx, int tx_len)
+{
+	int ret = 0, retry = 0;
+
+	do {
+		ret = mtk_ipi_send(&scp_ipidev, id, 0, tx, ipi_len(tx_len), 0);
+		if (ret < 0 && ret != IPI_PIN_BUSY)
+			return -EIO;
+		if (ret == IPI_PIN_BUSY) {
+			if (retry++ >= 1000)
+				return -EBUSY;
+			if (retry % 100 == 0)
+				usleep_range(1000, 2000);
+		}
+	} while (ret == IPI_PIN_BUSY);
+
+	return 0;
+}
+
+static int ipi_transfer_buffer(struct ipi_transfer *t)
+{
+	int ret = 0;
+	int timeout;
+	unsigned long flags;
+	struct ipi_hw_transfer *hw = &hw_transfer;
+
+	spin_lock_irqsave(&hw_transfer_lock, flags);
+	hw->id = t->id;
+	hw->tx = t->tx_buf;
+	hw->rx = t->rx_buf;
+	hw->tx_len = t->tx_len;
+	hw->rx_len = t->rx_len;
+
+	reinit_completion(&hw->done);
+	hw->context = &hw->done;
+	spin_unlock_irqrestore(&hw_transfer_lock, flags);
+
+	ret = ipi_retry_transfer(hw->id, (void *)hw->tx, hw->tx_len);
+	if (ret < 0)
+		return ret;
+
+	timeout = wait_for_completion_timeout(&hw->done,
+			msecs_to_jiffies(100));
+	spin_lock_irqsave(&hw_transfer_lock, flags);
+	if (!timeout) {
+		hw->count = -ETIMEDOUT;
+		pr_err_ratelimited("timeout %u %u %u %u %u\n",
+			hw->tx[0], hw->tx[1], hw->tx[2],
+			hw->tx[3], hw->tx[4]);
+	}
+	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(&controller.lock, flags);
+	if (list_empty(&controller.head) || controller.running)
+		goto out;
+	controller.running = true;
+	while (!list_empty(&controller.head)) {
+		m = list_first_entry(&controller.head,
+			struct ipi_message, list);
+		list_del(&m->list);
+		spin_unlock_irqrestore(&controller.lock, flags);
+		list_for_each_entry(t, &m->transfers, transfer_list) {
+			if (!t->tx_buf && t->tx_len) {
+				status = -EINVAL;
+				break;
+			}
+			if (t->tx_len)
+				status = ipi_transfer_buffer(t);
+			if (status < 0) {
+				break;
+			} else if (status != t->rx_len) {
+				status = -EBADMSG;
+				break;
+			}
+			status = 0;
+		}
+		m->status = status;
+		m->complete(m->context);
+		spin_lock_irqsave(&controller.lock, flags);
+	}
+	controller.running = false;
+out:
+	spin_unlock_irqrestore(&controller.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(&controller.lock, flags);
+	list_add_tail(&m->list, &controller.head);
+	queue_work(controller.workqueue, &controller.work);
+	spin_unlock_irqrestore(&controller.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(int id, const unsigned char *txbuf, unsigned int n_tx,
+		unsigned char *rxbuf, unsigned int n_rx)
+{
+	struct ipi_transfer t;
+	struct ipi_message m;
+
+	t.id = id;
+	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 ipi_comm_sync(int id, unsigned char *tx, unsigned int n_tx,
+		unsigned char *rx, unsigned int n_rx)
+{
+	return ipi_sync(id, tx, n_tx, rx, n_rx);
+}
+
+int ipi_comm_async(struct ipi_message *m)
+{
+	return ipi_async(m);
+}
+
+int ipi_comm_noack(int id, unsigned char *tx, unsigned int n_tx)
+{
+	return ipi_retry_transfer(id, tx, n_tx);
+}
+
+static void ipi_comm_complete(unsigned char *buffer, unsigned int len)
+{
+	struct ipi_hw_transfer *hw = &hw_transfer;
+
+	spin_lock(&hw_transfer_lock);
+	if (!hw->context) {
+		pr_err("dropped transfer\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);
+}
+
+static int ipi_comm_ctrl_handler(unsigned int id, void *prdata,
+		void *data, unsigned int len)
+{
+	WARN_ON(id != IPI_IN_SENSOR_CTRL);
+	ipi_comm_complete(data, len);
+	return 0;
+}
+
+static int ipi_comm_notify_handler(unsigned int id, void *prdata,
+		void *data, unsigned int len)
+{
+	WARN_ON(id != IPI_IN_SENSOR_NOTIFY);
+	if (controller.notify_callback)
+		controller.notify_callback(id, data, len);
+	return 0;
+}
+
+int get_ctrl_id(void)
+{
+	return IPI_OUT_SENSOR_CTRL;
+}
+
+int get_notify_id(void)
+{
+	return IPI_OUT_SENSOR_NOTIFY;
+}
+
+void ipi_comm_notify_handler_register(
+		void (*f)(int id, void *data, unsigned int len))
+{
+	controller.notify_callback = f;
+}
+
+void ipi_comm_notify_handler_unregister(void)
+{
+	controller.notify_callback = NULL;
+}
+
+int ipi_comm_init(void)
+{
+	int ret = 0;
+
+	init_completion(&hw_transfer.done);
+	INIT_WORK(&controller.work, ipi_work);
+	INIT_LIST_HEAD(&controller.head);
+	spin_lock_init(&controller.lock);
+	controller.workqueue = alloc_workqueue("ipi_comm",
+		WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
+	if (controller.workqueue == NULL) {
+		pr_err("create workqueue fail\n");
+		return -1;
+	}
+	ret = mtk_ipi_register(&scp_ipidev, IPI_IN_SENSOR_CTRL,
+		ipi_comm_ctrl_handler, NULL, ctrl_payload);
+	if (ret < 0)
+		pr_err("register ipi %u fail %d\n", IPI_IN_SENSOR_CTRL, ret);
+	ret = mtk_ipi_register(&scp_ipidev, IPI_IN_SENSOR_NOTIFY,
+		ipi_comm_notify_handler, NULL, notify_payload);
+	if (ret < 0)
+		pr_err("register ipi %u fail %d\n", IPI_IN_SENSOR_NOTIFY, ret);
+	return 0;
+}
+
+void ipi_comm_exit(void)
+{
+	mtk_ipi_unregister(&scp_ipidev, IPI_IN_SENSOR_CTRL);
+	mtk_ipi_unregister(&scp_ipidev, IPI_IN_SENSOR_NOTIFY);
+	flush_workqueue(controller.workqueue);
+	destroy_workqueue(controller.workqueue);
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/ipi_comm.h

@@ -0,0 +1,52 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _MTK_NANOHUB_IPI_H_
+#define _MTK_NANOHUB_IPI_H_
+
+#include <linux/list.h>
+
+struct ipi_transfer {
+	int id;
+	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 get_ctrl_id(void);
+int get_notify_id(void);
+int ipi_comm_sync(int id, unsigned char *tx, unsigned int n_tx,
+		unsigned char *rx, unsigned int n_rx);
+int ipi_comm_async(struct ipi_message *m);
+int ipi_comm_noack(int id, unsigned char *tx, unsigned int n_tx);
+void ipi_comm_notify_handler_register(
+		void (*f)(int id, void *data, unsigned int len));
+void ipi_comm_notify_handler_unregister(void);
+int ipi_comm_init(void);
+void ipi_comm_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/ready.c

@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "sensor_ready " fmt
+
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+
+#include "scp.h"
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+#include "ready.h"
+
+static DEFINE_SPINLOCK(sensor_ready_lock);
+static bool scp_platform_ready;
+static bool scp_sensor_ready;
+static bool sensor_ready;
+static BLOCKING_NOTIFIER_HEAD(sensor_ready_notifier_head);
+static struct workqueue_struct *sensor_ready_workqueue;
+static struct work_struct sensor_ready_work;
+static struct delayed_work sensor_rescure_work;
+
+void sensor_ready_notifier_chain_register(struct notifier_block *nb)
+{
+	unsigned long flags = 0;
+	bool status = false;
+
+	blocking_notifier_chain_register(&sensor_ready_notifier_head, nb);
+	/*
+	 * NOTE: must copy sensor_ready to status to avoid sensor_ready
+	 * modified during notifier calling, keep notify the same status.
+	 */
+	spin_lock_irqsave(&sensor_ready_lock, flags);
+	status = sensor_ready;
+	spin_unlock_irqrestore(&sensor_ready_lock, flags);
+	if (status)
+		nb->notifier_call(nb, status, NULL);
+}
+
+void sensor_ready_notifier_chain_unregister(struct notifier_block *nb)
+{
+	blocking_notifier_chain_unregister(&sensor_ready_notifier_head, nb);
+}
+
+static void scp_sensor_ready_notify_handler(struct sensor_comm_notify *n,
+		void *private_data)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&sensor_ready_lock, flags);
+	scp_sensor_ready = true;
+	if (scp_platform_ready && scp_sensor_ready) {
+		sensor_ready = true;
+		cancel_delayed_work(&sensor_rescure_work);
+		queue_work(sensor_ready_workqueue, &sensor_ready_work);
+	}
+	spin_unlock_irqrestore(&sensor_ready_lock, flags);
+}
+
+static int scp_platform_ready_notifier_call(struct notifier_block *this,
+		unsigned long event, void *ptr)
+{
+	unsigned long flags = 0;
+	int ret = 0;
+	struct sensor_comm_notify notify;
+
+	if (event == SCP_EVENT_STOP) {
+		cancel_delayed_work(&sensor_rescure_work);
+		spin_lock_irqsave(&sensor_ready_lock, flags);
+		scp_platform_ready = false;
+		scp_sensor_ready = false;
+		sensor_ready = false;
+		queue_work(sensor_ready_workqueue, &sensor_ready_work);
+		spin_unlock_irqrestore(&sensor_ready_lock, flags);
+	} else if (event == SCP_EVENT_READY) {
+		notify.sequence = 0;
+		notify.sensor_type = SENSOR_TYPE_INVALID;
+		notify.command = SENS_COMM_NOTIFY_READY_CMD;
+		notify.length = 0;
+		ret = sensor_comm_notify_bypass(&notify);
+		if (ret < 0)
+			pr_err("notify ready to scp fail %d\n", ret);
+		queue_delayed_work(sensor_ready_workqueue,
+			&sensor_rescure_work, msecs_to_jiffies(5000));
+		spin_lock_irqsave(&sensor_ready_lock, flags);
+		scp_platform_ready = true;
+		if (scp_platform_ready && scp_sensor_ready) {
+			sensor_ready = true;
+			queue_work(sensor_ready_workqueue,
+				&sensor_ready_work);
+		}
+		spin_unlock_irqrestore(&sensor_ready_lock, flags);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block scp_platform_ready_notifier = {
+	.notifier_call = scp_platform_ready_notifier_call,
+};
+
+static void sensor_ready_work_fn(struct work_struct *work)
+{
+	unsigned long flags = 0;
+	bool status = false;
+
+	/*
+	 * NOTE: must copy sensor_ready to status to avoid sensor_ready
+	 * modified during notifier calling, keep notify the same status.
+	 */
+	spin_lock_irqsave(&sensor_ready_lock, flags);
+	status = sensor_ready;
+	spin_unlock_irqrestore(&sensor_ready_lock, flags);
+	blocking_notifier_call_chain(&sensor_ready_notifier_head,
+		status, NULL);
+}
+
+static void sensor_rescure_work_fn(struct work_struct *work)
+{
+	unsigned long flags = 0;
+
+	spin_lock_irqsave(&sensor_ready_lock, flags);
+	if (scp_platform_ready && !scp_sensor_ready) {
+		pr_alert("rescure sensor by scp reset due to no ready ack\n");
+		scp_wdt_reset(0);
+	}
+	spin_unlock_irqrestore(&sensor_ready_lock, flags);
+}
+
+int host_ready_init(void)
+{
+	INIT_WORK(&sensor_ready_work, sensor_ready_work_fn);
+	INIT_DELAYED_WORK(&sensor_rescure_work, sensor_rescure_work_fn);
+	sensor_ready_workqueue = alloc_workqueue("sensor_ready",
+		WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
+	if (!sensor_ready_workqueue) {
+		pr_err("alloc workqueue fail\n");
+		return -ENOMEM;
+	}
+	/*
+	 * NOTE: sensor comm notify must before scp register notify
+	 * to avoid lost sensor ready notify.
+	 */
+	sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_READY_CMD,
+		scp_sensor_ready_notify_handler, NULL);
+	scp_A_register_notify(&scp_platform_ready_notifier);
+	return 0;
+}
+
+void host_ready_exit(void)
+{
+	scp_A_unregister_notify(&scp_platform_ready_notifier);
+	sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_READY_CMD);
+	flush_workqueue(sensor_ready_workqueue);
+	destroy_workqueue(sensor_ready_workqueue);
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/ready.h

@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _READY_H_
+#define _READY_H_
+
+#include <linux/notifier.h>
+
+enum sensor_ready_priority {
+	READY_STDPRI, /* others use std */
+	READY_HIGHPRI, /* transceiver use high */
+	READY_HIGHESTPRI, /* sensor comm use highest */
+};
+
+void sensor_ready_notifier_chain_register(struct notifier_block *nb);
+void sensor_ready_notifier_chain_unregister(struct notifier_block *nb);
+int host_ready_init(void);
+void host_ready_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/sensor_comm.c

@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "sensor_comm " fmt
+
+#include <linux/err.h>
+#include <linux/device.h>
+#include <linux/printk.h>
+#include <linux/atomic.h>
+#include <linux/string.h>
+#include <linux/timekeeping.h>
+
+#include "tiny_crc8.h"
+#include "sensor_comm.h"
+#include "ready.h"
+
+struct sensor_notify_handle {
+	void (*handler)(struct sensor_comm_notify *n, void *private_data);
+	void *private_data;
+};
+
+static bool scp_status;
+static atomic_t sensor_comm_sequence;
+static
+struct sensor_notify_handle sens_notify_handle[MAX_SENS_COMM_NOTIFY_CMD];
+
+static void sensor_comm_notify_handler(int id, void *data, unsigned int len)
+{
+	uint8_t crc = 0;
+	struct sensor_comm_notify *notify = data;
+	struct sensor_notify_handle *handle = NULL;
+
+	crc = tiny_crc8((uint8_t *)notify, offsetof(typeof(*notify), crc8));
+	if (notify->crc8 != crc) {
+		pr_err("unrecognized packet %u %u %u %u %u %u\n",
+			notify->sequence, notify->sensor_type, notify->command,
+			notify->length, notify->crc8, crc);
+		return;
+	}
+	if (sizeof(*notify) != len)
+		return;
+	if (notify->command >= MAX_SENS_COMM_NOTIFY_CMD)
+		return;
+	handle = &sens_notify_handle[notify->command];
+	if (!handle->handler)
+		return;
+	handle->handler(notify, handle->private_data);
+}
+
+static int sensor_comm_ctrl_seq_send(struct sensor_comm_ctrl *ctrl,
+		unsigned int size)
+{
+	int ret = 0;
+	uint8_t crc = 0;
+	struct sensor_comm_ack ack;
+
+	memset(&ack, 0, sizeof(ack));
+	/* safe sequence given by atomic, round from 0 to 255 */
+	ctrl->sequence = atomic_inc_return(&sensor_comm_sequence);
+	ctrl->crc8 = tiny_crc8((uint8_t *)ctrl, offsetof(typeof(*ctrl), crc8));
+	ret = ipi_comm_sync(get_ctrl_id(), (unsigned char *)ctrl, size,
+		(unsigned char *)&ack, sizeof(ack));
+	if (ret < 0)
+		return ret;
+	crc = tiny_crc8((uint8_t *)&ack, offsetof(typeof(ack), crc8));
+	if (ack.crc8 != crc) {
+		pr_err("unrecognized packet %u %u %u %u %u %u\n",
+			ack.sequence, ack.sensor_type, ack.command,
+			ack.ret_val, ack.crc8, crc);
+		return -EBADMSG;
+	}
+	if (ctrl->sequence != ack.sequence)
+		return -EILSEQ;
+	if (ctrl->sensor_type != ack.sensor_type)
+		return -EPROTO;
+	if (ctrl->command != ack.command)
+		return -EPROTO;
+	if (ack.ret_val < 0)
+		return -EREMOTEIO;
+	return 0;
+}
+
+int sensor_comm_ctrl_send(struct sensor_comm_ctrl *ctrl, unsigned int size)
+{
+	int retry = 0, ret = 0;
+	const int max_retry = 10;
+	const int64_t timeout = 10000000000LL;
+	int64_t start_time = 0, duration = 0;
+
+	start_time = ktime_get_boottime_ns();
+	if (!READ_ONCE(scp_status)) {
+		pr_err_ratelimited("dropped comm %u %u\n",
+			ctrl->sensor_type, ctrl->command);
+		return 0;
+	}
+
+	do {
+		ret = sensor_comm_ctrl_seq_send(ctrl, size);
+	} while (retry++ < max_retry && ret < 0);
+
+	duration = ktime_get_boottime_ns() - start_time;
+	if (duration > timeout)
+		pr_notice("running time %lld, type %u, cmd %u, retries %d\n",
+			duration, ctrl->sensor_type, ctrl->command, retry);
+	return ret;
+}
+
+int sensor_comm_notify(struct sensor_comm_notify *notify)
+{
+	if (!READ_ONCE(scp_status)) {
+		pr_err_ratelimited("dropped comm %u %u\n",
+			notify->sensor_type, notify->command);
+		return 0;
+	}
+
+	notify->crc8 =
+		tiny_crc8((uint8_t *)notify, offsetof(typeof(*notify), crc8));
+	return ipi_comm_noack(get_notify_id(), (unsigned char *)notify,
+		sizeof(*notify));
+}
+
+/*
+ * no need check scp_status due to send ready to scp.
+ */
+int sensor_comm_notify_bypass(struct sensor_comm_notify *notify)
+{
+	notify->crc8 =
+		tiny_crc8((uint8_t *)notify, offsetof(typeof(*notify), crc8));
+	return ipi_comm_noack(get_notify_id(), (unsigned char *)notify,
+		sizeof(*notify));
+}
+
+void sensor_comm_notify_handler_register(uint8_t cmd,
+		void (*f)(struct sensor_comm_notify *n, void *private_data),
+		void *private_data)
+{
+	if (cmd >= MAX_SENS_COMM_NOTIFY_CMD)
+		return;
+
+	sens_notify_handle[cmd].private_data = private_data;
+	sens_notify_handle[cmd].handler = f;
+}
+
+void sensor_comm_notify_handler_unregister(uint8_t cmd)
+{
+	if (cmd >= MAX_SENS_COMM_NOTIFY_CMD)
+		return;
+
+	sens_notify_handle[cmd].handler = NULL;
+	sens_notify_handle[cmd].private_data = NULL;
+}
+
+static int sensor_comm_ready_notifier_call(struct notifier_block *this,
+		unsigned long event, void *ptr)
+{
+	WRITE_ONCE(scp_status, !!event);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block sensor_comm_ready_notifier = {
+	.notifier_call = sensor_comm_ready_notifier_call,
+	.priority = READY_HIGHESTPRI,
+};
+
+int sensor_comm_init(void)
+{
+	atomic_set(&sensor_comm_sequence, 0);
+	ipi_comm_init();
+	ipi_comm_notify_handler_register(sensor_comm_notify_handler);
+	sensor_ready_notifier_chain_register(&sensor_comm_ready_notifier);
+	return 0;
+}
+
+void sensor_comm_exit(void)
+{
+	sensor_ready_notifier_chain_unregister(&sensor_comm_ready_notifier);
+	ipi_comm_notify_handler_unregister();
+	ipi_comm_exit();
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/sensor_comm.h

@@ -0,0 +1,101 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _SENSOR_COMM_H_
+#define _SENSOR_COMM_H_
+
+#include "ipi_comm.h"
+
+enum sensor_comm_ctrl_cmd {
+	SENS_COMM_CTRL_DISABLE_CMD,
+	SENS_COMM_CTRL_ENABLE_CMD,
+	SENS_COMM_CTRL_FLUSH_CMD,
+	SENS_COMM_CTRL_CALI_CMD,
+	SENS_COMM_CTRL_CONFIG_CMD,
+	SENS_COMM_CTRL_SELF_TEST_CMD,
+	SENS_COMM_CTRL_ENABLE_RAW_CMD,
+	SENS_COMM_CTRL_DISABLE_RAW_CMD,
+	SENS_COMM_CTRL_MASK_NOTIFY_CMD,
+	SENS_COMM_CTRL_UNMASK_NOTIFY_CMD,
+	SENS_COMM_CTRL_TIMESYNC_CMD,
+	SENS_COMM_CTRL_SHARE_MEMORY_CMD,
+	SENS_COMM_CTRL_DEBUG_CMD,
+	MAX_SENS_COMM_CTRL_CMD,
+};
+
+enum sensor_comm_notify_cmd {
+	SENS_COMM_NOTIFY_DATA_CMD,
+	SENS_COMM_NOTIFY_FULL_CMD,
+	SENS_COMM_NOTIFY_SUPER_DATA_CMD,
+	SENS_COMM_NOTIFY_SUPER_FULL_CMD,
+	SENS_COMM_NOTIFY_READY_CMD,
+	SENS_COMM_NOTIFY_LIST_CMD,
+	SENS_COMM_NOTIFY_DEBUG_CMD,
+	SENS_COMM_NOTIFY_CUSTOM_CMD,
+	SENS_COMM_NOTIFY_FAST_CUST_CMD,
+	MAX_SENS_COMM_NOTIFY_CMD,
+};
+
+struct sensor_comm_batch {
+	int64_t delay;
+	int64_t latency;
+} __packed __aligned(4);
+
+struct sensor_comm_timesync {
+	int64_t host_timestamp;
+	int64_t host_archcounter;
+} __packed __aligned(4);
+
+struct sensor_comm_share_mem {
+	uint8_t available_num;
+	struct {
+		uint8_t payload_type;
+		uint32_t payload_base;
+	} __aligned(4) base_info[4];
+} __packed __aligned(4);
+
+struct sensor_comm_ctrl {
+	uint8_t sequence;
+	uint8_t sensor_type;
+	uint8_t command;
+	uint8_t length;
+	uint8_t crc8;
+	uint8_t data[0] __aligned(4);
+} __packed __aligned(4);
+
+struct sensor_comm_ack {
+	uint8_t sequence;
+	uint8_t sensor_type;
+	uint8_t command;
+	int8_t ret_val;
+	uint8_t crc8;
+} __packed __aligned(4);
+
+struct data_notify {
+	int32_t write_position;
+	int64_t scp_timestamp;
+	int64_t scp_archcounter;
+} __packed __aligned(4);
+
+struct sensor_comm_notify {
+	uint8_t sequence;
+	uint8_t sensor_type;
+	uint8_t command;
+	uint8_t length;
+	uint8_t crc8;
+	int32_t value[5] __aligned(4);
+} __packed __aligned(4);
+
+int sensor_comm_ctrl_send(struct sensor_comm_ctrl *ctrl, unsigned int size);
+int sensor_comm_notify(struct sensor_comm_notify *notify);
+int sensor_comm_notify_bypass(struct sensor_comm_notify *notify);
+void sensor_comm_notify_handler_register(uint8_t cmd,
+		void (*f)(struct sensor_comm_notify *n, void *private_data),
+		void *private_data);
+void sensor_comm_notify_handler_unregister(uint8_t cmd);
+int sensor_comm_init(void);
+void sensor_comm_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/sensor_list.c

@@ -0,0 +1,167 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "sensor_list " fmt
+
+#include <linux/spinlock.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+#include <linux/atomic.h>
+#include <linux/slab.h>
+
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+#include "sensor_list.h"
+#include "share_memory.h"
+
+static DEFINE_MUTEX(bus_user_lock);
+static atomic_t sensor_list_sequence;
+static DECLARE_COMPLETION(sensor_list_done);
+static DEFINE_SPINLOCK(rx_notify_lock);
+static struct sensor_comm_notify rx_notify;
+static struct share_mem sensor_list_shm_reader;
+
+static void sensor_list_notify_handler(struct sensor_comm_notify *n,
+		void *private_data)
+{
+	spin_lock(&rx_notify_lock);
+	memcpy(&rx_notify, n, sizeof(rx_notify));
+	spin_unlock(&rx_notify_lock);
+	complete(&sensor_list_done);
+}
+
+static int sensor_list_seq_get_list(struct sensor_info *list,
+		unsigned int num)
+{
+	int ret = 0;
+	int timeout = 0;
+	unsigned long flags = 0;
+	struct sensor_comm_notify notify;
+	uint32_t write_position = 0;
+	struct share_mem_info info;
+	unsigned int i = 0;
+
+	ret = share_mem_read_reset(&sensor_list_shm_reader);
+	if (ret < 0) {
+		pr_err("reset fail %d\n", ret);
+		return ret;
+	}
+
+	/*
+	 * NOTE: must reinit_completion before sensor_comm_notify
+	 * wrong sequence:
+	 * sensor_comm_notify ---> reinit_completion -> wait_for_completion
+	 *                     |
+	 *                  complete
+	 * if complete before reinit_completion, will lose this complete
+	 * right sequence:
+	 * reinit_completion -> sensor_comm_notify -> wait_for_completion
+	 */
+	reinit_completion(&sensor_list_done);
+
+	/* safe sequence given by atomic, round from 0 to 255 */
+	notify.sequence = atomic_inc_return(&sensor_list_sequence);
+	notify.sensor_type = SENSOR_TYPE_INVALID;
+	notify.command = SENS_COMM_NOTIFY_LIST_CMD;
+	notify.length = 0;
+	ret = sensor_comm_notify(&notify);
+	if (ret < 0) {
+		pr_err("notify fail %d\n", ret);
+		return ret;
+	}
+
+	timeout = wait_for_completion_timeout(&sensor_list_done,
+		msecs_to_jiffies(100));
+	if (!timeout) {
+		pr_err("wait completion timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	spin_lock_irqsave(&rx_notify_lock, flags);
+	if (rx_notify.sequence != notify.sequence &&
+	    rx_notify.sensor_type != notify.sensor_type &&
+	    rx_notify.command != notify.command) {
+		pr_err("reply fail\n");
+		spin_unlock_irqrestore(&rx_notify_lock, flags);
+		return -EREMOTEIO;
+	}
+	write_position = rx_notify.value[0];
+	spin_unlock_irqrestore(&rx_notify_lock, flags);
+
+	ret = share_mem_seek(&sensor_list_shm_reader, write_position);
+	if (ret < 0) {
+		pr_err("seek fail %d\n", ret);
+		return ret;
+	}
+	/* NOTE: i++ must not put into for loop */
+	for (i = 0; i < num; ) {
+		ret = share_mem_read(&sensor_list_shm_reader,
+			&info, sizeof(info));
+		if (ret == 0)
+			break;
+		if (ret != sizeof(info)) {
+			pr_err("read fail %d\n", ret);
+			break;
+		}
+		if (info.sensor_type >= SENSOR_TYPE_SENSOR_MAX ||
+				!info.sensor_type || !info.gain) {
+			pr_err("read wrong sensor info\n");
+			continue;
+		}
+		list[i].sensor_type = info.sensor_type;
+		list[i].gain = info.gain;
+		strlcpy(list[i].name, info.name, sizeof(list[i].name));
+		strlcpy(list[i].vendor, info.vendor, sizeof(list[i].vendor));
+		i++;
+	}
+	return i;
+}
+
+int sensor_list_get_list(struct sensor_info *list, unsigned int num)
+{
+	int retry = 0, ret = 0;
+	const int max_retry = 3;
+
+	mutex_lock(&bus_user_lock);
+	do {
+		ret = sensor_list_seq_get_list(list, num);
+	} while (retry++ < max_retry && ret < 0);
+	mutex_unlock(&bus_user_lock);
+
+	return ret;
+}
+
+static int sensor_list_share_mem_cfg(struct share_mem_config *cfg,
+		void *private_data)
+{
+	sensor_list_shm_reader.name = "sensor_list_r";
+	sensor_list_shm_reader.item_size = sizeof(struct share_mem_info);
+	sensor_list_shm_reader.buffer_full_detect = false;
+
+	return share_mem_init(&sensor_list_shm_reader, cfg);
+}
+
+int sensor_list_init(void)
+{
+	unsigned long flags = 0;
+
+	atomic_set(&sensor_list_sequence, 0);
+
+	spin_lock_irqsave(&rx_notify_lock, flags);
+	memset(&rx_notify, 0, sizeof(rx_notify));
+	spin_unlock_irqrestore(&rx_notify_lock, flags);
+
+	sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_LIST_CMD,
+		sensor_list_notify_handler, NULL);
+	share_mem_config_handler_register(SHARE_MEM_LIST_PAYLOAD_TYPE,
+		sensor_list_share_mem_cfg, NULL);
+	return 0;
+}
+
+void sensor_list_exit(void)
+{
+	sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_LIST_CMD);
+	share_mem_config_handler_unregister(SHARE_MEM_LIST_PAYLOAD_TYPE);
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/sensor_list.h

@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _SENSOR_LIST_H_
+#define _SENSOR_LIST_H_
+
+#include "hf_manager.h"
+
+int sensor_list_get_list(struct sensor_info *list, unsigned int size);
+int sensor_list_init(void);
+void sensor_list_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/share_memory.c

@@ -0,0 +1,417 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "share_mem " fmt
+
+#include <linux/err.h>
+#include <linux/string.h>
+#include <linux/printk.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+
+#include "sensor_comm.h"
+#include "share_memory.h"
+#include "hf_sensor_type.h"
+
+
+struct share_mem_config_handle {
+	int (*handler)(struct share_mem_config *cfg, void *private_data);
+	void *private_data;
+};
+
+struct share_mem_usage {
+	uint8_t payload_type;
+	bool init_status;
+	int id;
+};
+
+static struct share_mem_config_handle shm_handle[MAX_SHARE_MEM_PAYLOAD_TYPE];
+
+static struct share_mem_usage shm_usage_table[] = {
+	{
+		.payload_type = SHARE_MEM_DATA_PAYLOAD_TYPE,
+		.id = SENS_MEM_ID,
+	},
+	{
+		.payload_type = SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE,
+		.id = SENS_SUPER_MEM_ID,
+	},
+	{
+		.payload_type = SHARE_MEM_LIST_PAYLOAD_TYPE,
+		.id = SENS_LIST_MEM_ID,
+	},
+	{
+		.payload_type = SHARE_MEM_DEBUG_PAYLOAD_TYPE,
+		.id = SENS_DEBUG_MEM_ID,
+	},
+	{
+		.payload_type = SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE,
+		.id = SENS_CUSTOM_W_MEM_ID,
+	},
+	{
+		.payload_type = SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE,
+		.id = SENS_CUSTOM_R_MEM_ID,
+	},
+};
+
+static int share_mem_notify(struct share_mem *shm,
+		struct share_mem_notify *notify)
+{
+	int ret = 0;
+	struct sensor_comm_notify n;
+
+	if (shm->write_position == shm->last_write_position)
+		return 0;
+
+	n.sequence = notify->sequence;
+	n.sensor_type = notify->sensor_type;
+	n.command = notify->notify_cmd;
+	n.value[0] = shm->write_position;
+	n.length = sizeof(n.value[0]);
+	ret = sensor_comm_notify(&n);
+	if (ret < 0)
+		return ret;
+	shm->last_write_position = shm->write_position;
+	return ret;
+}
+
+static void share_mem_buffer_full_detect(struct share_mem *shm,
+		uint32_t curr_written)
+{
+	int ret = 0;
+	uint32_t rp = 0, wp = 0, buffer_size = 0;
+	struct share_mem_notify notify;
+
+	shm->buffer_full_written += curr_written;
+	if (shm->buffer_full_written < shm->buffer_full_threshold)
+		return;
+
+	rp = shm->base->rp;
+	wp = shm->base->wp;
+	buffer_size = shm->base->buffer_size;
+
+	shm->buffer_full_written = (wp > rp) ?
+		(wp - rp) : (buffer_size - rp + wp);
+	if (shm->buffer_full_written >= shm->buffer_full_threshold) {
+		notify.sequence = 0;
+		notify.sensor_type = SENSOR_TYPE_INVALID;
+		notify.notify_cmd = shm->buffer_full_cmd;
+		ret = share_mem_notify(shm, &notify);
+		if (ret < 0)
+			pr_err("%s buffer full notify fail %d\n",
+				shm->name, ret);
+		else
+			shm->buffer_full_written = 0;
+	}
+}
+
+int share_mem_seek(struct share_mem *shm, uint32_t write_position)
+{
+	if (!shm->base)
+		return -EINVAL;
+
+	mutex_lock(&shm->lock);
+	shm->write_position = write_position;
+	mutex_unlock(&shm->lock);
+	return 0;
+}
+
+int share_mem_read_reset(struct share_mem *shm)
+{
+	if (!shm->base)
+		return -EINVAL;
+
+	mutex_lock(&shm->lock);
+	shm->base->rp = 0;
+	shm->write_position = 0;
+	mutex_unlock(&shm->lock);
+	return 0;
+}
+
+int share_mem_write_reset(struct share_mem *shm)
+{
+	if (!shm->base)
+		return -EINVAL;
+
+	mutex_lock(&shm->lock);
+	shm->base->wp = 0;
+	shm->write_position = 0;
+	shm->last_write_position = 0;
+	shm->buffer_full_written = 0;
+	mutex_unlock(&shm->lock);
+	return 0;
+}
+
+static int share_mem_read_dram(struct share_mem *shm,
+		void *buf, uint32_t count)
+{
+	uint32_t rp = 0, wp = 0, buffer_size = 0, item_size = 0;
+	uint8_t *src = NULL, *dst = buf;
+	uint32_t first = 0, second = 0, read = 0;
+
+	if (!shm->item_size || count % shm->item_size)
+		return -EINVAL;
+
+	wp = shm->write_position;
+	rp = shm->base->rp;
+	buffer_size = shm->base->buffer_size;
+	item_size = shm->base->item_size;
+	src = (uint8_t *)shm->base + offsetof(struct share_mem_base, data);
+
+	if (item_size != shm->item_size)
+		return -EIO;
+
+	if (wp == rp)
+		return 0;
+
+	if (wp > rp) {
+		first = wp - rp;
+		second = 0;
+	} else {
+		first = buffer_size - rp;
+		second = wp;
+	}
+
+	first = min(first, count);
+	second = min(count - first, second);
+
+	memcpy_fromio(dst, src + rp, first);
+	memcpy_fromio(dst + first, src, second);
+	read = first + second;
+	rp += read;
+	rp %= buffer_size;
+
+	/*
+	 * make sure that the data is copied before
+	 * incrementing the rp index counter
+	 */
+	smp_wmb();
+	shm->base->rp = rp;
+
+	return read;
+}
+
+int share_mem_read(struct share_mem *shm, void *buf, uint32_t count)
+{
+	int ret = 0;
+
+	if (!shm->base || !buf || !count)
+		return -EINVAL;
+
+	mutex_lock(&shm->lock);
+	ret = share_mem_read_dram(shm, buf, count);
+	mutex_unlock(&shm->lock);
+	return ret;
+}
+
+static int share_mem_write_dram(struct share_mem *shm,
+		void *buf, uint32_t count)
+{
+	uint32_t rp = 0, wp = 0, buffer_size = 0, item_size = 0, write = 0;
+	uint8_t *src = buf, *dst = NULL;
+
+	if (!shm->item_size || count % shm->item_size)
+		return -EINVAL;
+
+	rp = shm->base->rp;
+	wp = shm->base->wp;
+	buffer_size = shm->base->buffer_size;
+	item_size = shm->base->item_size;
+	dst = (uint8_t *)shm->base + offsetof(struct share_mem_base, data);
+
+	if (item_size != shm->item_size)
+		return -EIO;
+
+	/* remain 1 count */
+	while ((write < count) && ((wp + item_size) % buffer_size != rp)) {
+		memcpy_toio(dst + wp, src + write, item_size);
+		write += item_size;
+		wp += item_size;
+		wp %= buffer_size;
+	}
+
+	if (!write)
+		return 0;
+
+	/*
+	 * make sure that the data is copied before
+	 * incrementing the wp index counter
+	 */
+	smp_wmb();
+	shm->base->wp = wp;
+	shm->write_position = wp;
+
+	if (shm->buffer_full_detect)
+		share_mem_buffer_full_detect(shm, write);
+
+	return write;
+}
+
+int share_mem_write(struct share_mem *shm, void *buf, uint32_t count)
+{
+	int ret = 0;
+
+	if (!shm->base || !buf || !count)
+		return -EINVAL;
+
+	mutex_lock(&shm->lock);
+	ret = share_mem_write_dram(shm, buf, count);
+	mutex_unlock(&shm->lock);
+	return ret;
+}
+
+int share_mem_flush(struct share_mem *shm, struct share_mem_notify *notify)
+{
+	int ret = 0;
+
+	if (!shm->base)
+		return -EINVAL;
+
+	mutex_lock(&shm->lock);
+	ret = share_mem_notify(shm, notify);
+	mutex_unlock(&shm->lock);
+	return ret;
+}
+
+int share_mem_init(struct share_mem *shm, struct share_mem_config *cfg)
+{
+	int ret = 0;
+
+	if (!shm->name || !shm->item_size)
+		return -EINVAL;
+
+	mutex_init(&shm->lock);
+
+	cfg->base->wp = 0;
+	cfg->base->rp = 0;
+	cfg->base->item_size = shm->item_size;
+	cfg->base->buffer_size =
+		(((long)cfg->buffer_size -
+		offsetof(struct share_mem_base, data)) /
+		shm->item_size) * shm->item_size;
+
+	shm->write_position = 0;
+	shm->last_write_position = 0;
+
+	if (shm->buffer_full_detect) {
+		shm->buffer_full_written = 0;
+		shm->buffer_full_threshold =
+			((uint32_t)(((cfg->base->buffer_size - shm->item_size) /
+			shm->item_size) * 8 / 10)) *
+			shm->item_size;
+		if (shm->buffer_full_threshold <= shm->item_size) {
+			ret = -EINVAL;
+			goto exit;
+		}
+	}
+
+	shm->base = cfg->base;
+exit:
+	return ret;
+}
+
+static int share_mem_send_config(void)
+{
+	int ret = 0;
+	uint32_t i = 0, index = 0;
+	struct share_mem_usage *usage = NULL;
+	struct sensor_comm_ctrl *ctrl = NULL;
+	struct sensor_comm_share_mem *comm_shm = NULL;
+
+	ctrl = kzalloc(sizeof(*ctrl) + sizeof(*comm_shm), GFP_KERNEL);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->sensor_type = 0;
+	ctrl->command = SENS_COMM_CTRL_SHARE_MEMORY_CMD;
+	ctrl->length = sizeof(*comm_shm);
+	comm_shm = (struct sensor_comm_share_mem *)ctrl->data;
+
+	memset(comm_shm, 0, sizeof(*comm_shm));
+	for (i = 0; i < ARRAY_SIZE(shm_usage_table); i++) {
+		usage = &shm_usage_table[i];
+		if (usage->payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE)
+			continue;
+		/* host init share mem ready we can send share mem to scp */
+		if (usage->init_status) {
+			comm_shm->base_info[index].payload_type =
+				usage->payload_type;
+			comm_shm->base_info[index].payload_base =
+				(uint32_t)scp_get_reserve_mem_phys(usage->id);
+			WARN_ON(!comm_shm->base_info[index].payload_base);
+			comm_shm->available_num = ++index;
+		}
+		if (index == ARRAY_SIZE(comm_shm->base_info) ||
+		    (i == (ARRAY_SIZE(shm_usage_table) - 1) && index)) {
+			ret = sensor_comm_ctrl_send(ctrl,
+				sizeof(*ctrl) + ctrl->length);
+			if (ret < 0)
+				break;
+			index = 0;
+			memset(comm_shm, 0, sizeof(*comm_shm));
+		}
+	}
+	kfree(ctrl);
+	return ret;
+}
+
+int share_mem_config(void)
+{
+	int ret = 0;
+	uint32_t i = 0;
+	struct share_mem_config cfg;
+	struct share_mem_config_handle *handle = NULL;
+	struct share_mem_usage *usage = NULL;
+
+	for (i = 0; i < ARRAY_SIZE(shm_usage_table); i++) {
+		usage = &shm_usage_table[i];
+		/* must reset init_status to false scp reset each times */
+		usage->init_status = false;
+		if (usage->payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE) {
+			pr_err("payload type %u invalid index %u\n",
+				usage->payload_type, i);
+			BUG_ON(1);
+		}
+		handle = &shm_handle[usage->payload_type];
+		if (!handle->handler) {
+			pr_err("payload type %u handler NULL index %u\n",
+				usage->payload_type, i);
+			BUG_ON(1);
+		}
+		memset(&cfg, 0, sizeof(cfg));
+		cfg.payload_type = usage->payload_type;
+		cfg.base =
+			(void *)(long)scp_get_reserve_mem_virt(usage->id);
+		cfg.buffer_size =
+			(uint32_t)scp_get_reserve_mem_size(usage->id);
+		BUG_ON(!cfg.base);
+		ret = handle->handler(&cfg, handle->private_data);
+		if (ret < 0)
+			continue;
+		usage->init_status = true;
+	}
+
+	return share_mem_send_config();
+}
+
+void share_mem_config_handler_register(uint8_t payload_type,
+		int (*f)(struct share_mem_config *cfg, void *private_data),
+		void *private_data)
+{
+	if (payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE)
+		return;
+
+	shm_handle[payload_type].private_data = private_data;
+	shm_handle[payload_type].handler = f;
+}
+
+void share_mem_config_handler_unregister(uint8_t payload_type)
+{
+	if (payload_type >= MAX_SHARE_MEM_PAYLOAD_TYPE)
+		return;
+
+	shm_handle[payload_type].handler = NULL;
+	shm_handle[payload_type].private_data = NULL;
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/share_memory.h

@@ -0,0 +1,112 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _SHARE_MEMORY_H_
+#define _SHARE_MEMORY_H_
+
+#include "scp.h"
+
+enum share_mem_payload_type {
+	SHARE_MEM_DATA_PAYLOAD_TYPE,
+	SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE,
+	SHARE_MEM_LIST_PAYLOAD_TYPE,
+	SHARE_MEM_DEBUG_PAYLOAD_TYPE,
+	SHARE_MEM_CUSTOM_W_PAYLOAD_TYPE,
+	SHARE_MEM_CUSTOM_R_PAYLOAD_TYPE,
+	MAX_SHARE_MEM_PAYLOAD_TYPE,
+};
+
+struct share_mem_data {
+	uint8_t sensor_type;
+	uint8_t action;
+	uint8_t accurancy;
+	uint8_t padding[1];
+	int64_t timestamp;
+	int32_t value[6] __aligned(4);
+} __packed __aligned(4);
+
+struct share_mem_super_data {
+	uint8_t sensor_type;
+	uint8_t action;
+	uint8_t accurancy;
+	uint8_t padding[1];
+	int64_t timestamp;
+	int32_t value[16] __aligned(4);
+} __packed __aligned(4);
+
+struct share_mem_debug {
+	uint8_t sensor_type;
+	uint8_t padding[3];
+	uint32_t written;
+	uint8_t buffer[4032] __aligned(4); //2048+1024+512+256+128+64
+} __packed __aligned(4);
+
+struct share_mem_info {
+	uint8_t sensor_type;
+	uint8_t padding[3];
+	uint32_t gain;
+	uint8_t name[16];
+	uint8_t vendor[16];
+} __packed __aligned(4);
+
+struct share_mem_cmd {
+	uint8_t command;
+	uint8_t tx_len;
+	uint8_t rx_len;
+	uint8_t padding[1];
+	int32_t data[15] __aligned(4);
+} __packed __aligned(4);
+
+struct share_mem_base {
+	uint32_t rp;
+	uint32_t wp;
+	uint32_t buffer_size;
+	uint32_t item_size;
+	uint8_t data[0] __aligned(4);
+} __packed __aligned(4);
+
+struct share_mem {
+	struct share_mem_base *base;
+
+	struct mutex lock;
+	uint32_t item_size;
+
+	uint32_t write_position;
+	uint32_t last_write_position;
+
+	bool buffer_full_detect;
+	uint8_t buffer_full_cmd;
+	uint32_t buffer_full_written;
+	uint32_t buffer_full_threshold;
+
+	char *name;
+};
+
+struct share_mem_notify {
+	uint8_t sequence;
+	uint8_t sensor_type;
+	uint8_t notify_cmd;
+};
+
+struct share_mem_config {
+	uint8_t payload_type;
+	struct share_mem_base *base;
+	uint32_t buffer_size;
+};
+
+int share_mem_seek(struct share_mem *shm, uint32_t write_position);
+int share_mem_read_reset(struct share_mem *shm);
+int share_mem_write_reset(struct share_mem *shm);
+int share_mem_read(struct share_mem *shm, void *buf, uint32_t count);
+int share_mem_write(struct share_mem *shm, void *buf, uint32_t count);
+int share_mem_flush(struct share_mem *shm, struct share_mem_notify *notify);
+int share_mem_init(struct share_mem *shm, struct share_mem_config *cfg);
+int share_mem_config(void);
+void share_mem_config_handler_register(uint8_t payload_type,
+	int (*f)(struct share_mem_config *cfg, void *private_data),
+	void *private_data);
+void share_mem_config_handler_unregister(uint8_t payload_type);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/test/test.c

@@ -0,0 +1,98 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "test_sensor " fmt
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+#include "ready.h"
+
+struct test_sensor_t {
+	char *name;
+	struct hrtimer test_timer;
+	struct work_struct test_work;
+	struct workqueue_struct *workqueue;
+};
+
+static struct test_sensor_t test_sensor = {
+	.name = "test_sensor",
+};
+
+static int enable_disable_count;
+
+static void test_work_func(struct work_struct *work)
+{
+	int ret = 0;
+	struct sensor_comm_ctrl *ctrl;
+	struct sensor_comm_batch *batch;
+
+	if (enable_disable_count++ % 2) {
+		ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+		ctrl->sensor_type = SENSOR_TYPE_ACCELEROMETER;
+		ctrl->command = SENS_COMM_CTRL_DISABLE_CMD;
+		ctrl->length = 0;
+	} else {
+		ctrl = kzalloc(sizeof(*ctrl) + sizeof(*batch), GFP_KERNEL);
+		ctrl->sensor_type = SENSOR_TYPE_ACCELEROMETER;
+		ctrl->command = SENS_COMM_CTRL_ENABLE_CMD;
+		ctrl->length = sizeof(*batch);
+		batch = (struct sensor_comm_batch *)ctrl->data;
+		batch->delay = 20000000;
+		batch->latency = 1000000000;
+	}
+	ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
+	if (ret < 0)
+		pr_err("enable disable fail %d\n", ret);
+	kfree(ctrl);
+}
+
+static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
+{
+	queue_work(test_sensor.workqueue, &test_sensor.test_work);
+	hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
+	return HRTIMER_RESTART;
+}
+
+static int test_ready_notifier_call(struct notifier_block *this,
+		unsigned long event, void *ptr)
+{
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block test_ready_notifier = {
+	.notifier_call = test_ready_notifier_call,
+	.priority = READY_STDPRI,
+};
+
+static int __init test_init(void)
+{
+	INIT_WORK(&test_sensor.test_work, test_work_func);
+	test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
+	hrtimer_init(&test_sensor.test_timer,
+			CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	test_sensor.test_timer.function = test_timer_func;
+	hrtimer_start(&test_sensor.test_timer,
+			ns_to_ktime(10000000000), HRTIMER_MODE_REL);
+	sensor_ready_notifier_chain_register(&test_ready_notifier);
+	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");

drivers/misc/mediatek/sensor/2.0/sensorhub/test/test1.c

@@ -0,0 +1,80 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "test_sensor1 " fmt
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+#include <linux/time.h>
+#include <asm/arch_timer.h>
+
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+
+struct test_sensor_t {
+	char *name;
+	struct hrtimer test_timer;
+	struct work_struct test_work;
+	struct workqueue_struct *workqueue;
+};
+
+static struct test_sensor_t test_sensor = {
+	.name = "test_sensor1",
+};
+
+static void test_work_func(struct work_struct *work)
+{
+	int ret = 0;
+	struct sensor_comm_ctrl *ctrl = NULL;
+	struct sensor_comm_timesync *timesync = NULL;
+
+	ctrl = kzalloc(sizeof(*ctrl) + sizeof(*timesync), GFP_KERNEL);
+	ctrl->sensor_type = SENSOR_TYPE_INVALID;
+	ctrl->command = SENS_COMM_CTRL_TIMESYNC_CMD;
+	ctrl->length = sizeof(*timesync);
+	timesync = (struct sensor_comm_timesync *)ctrl->data;
+	local_irq_disable();
+	timesync->host_timestamp = ktime_get_boot_ns();
+	timesync->host_archcounter = arch_counter_get_cntvct();
+	local_irq_enable();
+	ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
+	if (ret < 0)
+		pr_err("timesync fail %d\n", ret);
+	kfree(ctrl);
+}
+
+static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
+{
+	queue_work(test_sensor.workqueue, &test_sensor.test_work);
+	hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
+	return HRTIMER_RESTART;
+}
+
+static int __init test_init(void)
+{
+	INIT_WORK(&test_sensor.test_work, test_work_func);
+	test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
+	hrtimer_init(&test_sensor.test_timer,
+			CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	test_sensor.test_timer.function = test_timer_func;
+	hrtimer_start(&test_sensor.test_timer,
+			ns_to_ktime(10000000000), HRTIMER_MODE_REL);
+	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");

drivers/misc/mediatek/sensor/2.0/sensorhub/test/test2.c

@@ -0,0 +1,85 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "test_sensor2 " fmt
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+
+struct test_sensor_t {
+	char *name;
+	struct hrtimer test_timer;
+	struct work_struct test_work;
+	struct workqueue_struct *workqueue;
+};
+
+static struct test_sensor_t test_sensor = {
+	.name = "test_sensor2",
+};
+
+static int enable_disable_count;
+
+static void test_work_func(struct work_struct *work)
+{
+	int ret = 0;
+	struct sensor_comm_ctrl *ctrl;
+	struct sensor_comm_batch *batch;
+
+	if (enable_disable_count++ % 2) {
+		ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+		ctrl->sensor_type = SENSOR_TYPE_MAGNETIC_FIELD;
+		ctrl->command = SENS_COMM_CTRL_DISABLE_CMD;
+		ctrl->length = 0;
+	} else {
+		ctrl = kzalloc(sizeof(*ctrl) + sizeof(*batch), GFP_KERNEL);
+		ctrl->sensor_type = SENSOR_TYPE_MAGNETIC_FIELD;
+		ctrl->command = SENS_COMM_CTRL_ENABLE_CMD;
+		ctrl->length = sizeof(*batch);
+		batch = (struct sensor_comm_batch *)ctrl->data;
+		batch->delay = 20000000;
+		batch->latency = 2000000000;
+	}
+	ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
+	if (ret < 0)
+		pr_err("enable disable fail %d\n", ret);
+	kfree(ctrl);
+}
+
+static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
+{
+	queue_work(test_sensor.workqueue, &test_sensor.test_work);
+	hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
+	return HRTIMER_RESTART;
+}
+
+static int __init test_init(void)
+{
+	INIT_WORK(&test_sensor.test_work, test_work_func);
+	test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
+	hrtimer_init(&test_sensor.test_timer,
+			CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	test_sensor.test_timer.function = test_timer_func;
+	hrtimer_start(&test_sensor.test_timer,
+			ns_to_ktime(10000000000), HRTIMER_MODE_REL);
+	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");

drivers/misc/mediatek/sensor/2.0/sensorhub/test/test3.c

@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "test_sensor3 " fmt
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+#include <linux/time.h>
+#include <asm/arch_timer.h>
+
+#include "hf_sensor_type.h"
+#include "sensor_comm.h"
+#include "debug.h"
+
+struct test_sensor_t {
+	char *name;
+	struct hrtimer test_timer;
+	struct work_struct test_work;
+	struct workqueue_struct *workqueue;
+};
+
+static struct test_sensor_t test_sensor = {
+	.name = "test_sensor3",
+};
+
+static void test_config(void)
+{
+	int ret = 0;
+	struct sensor_comm_ctrl *ctrl = NULL;
+	int32_t cfg[3] = {500, 100, 203};
+
+	ctrl = kzalloc(sizeof(*ctrl) + sizeof(cfg), GFP_KERNEL);
+	ctrl->sensor_type = SENSOR_TYPE_ACCELEROMETER;
+	ctrl->command = SENS_COMM_CTRL_CONFIG_CMD;
+	ctrl->length = sizeof(cfg);
+	memcpy(ctrl->data, cfg, sizeof(cfg));
+	ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
+	if (ret < 0)
+		pr_err("config fail %d\n", ret);
+	kfree(ctrl);
+}
+
+static void test_work_func(struct work_struct *work)
+{
+	int ret = 0;
+	unsigned int len = 1024;
+	uint8_t *buffer = kzalloc(len, GFP_KERNEL);
+
+	ret = debug_get_debug(SENSOR_TYPE_INVALID, buffer, len);
+	if (ret > 0)
+		pr_err("\n%s\n", buffer);
+	kfree(buffer);
+	test_config();
+}
+
+static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
+{
+	queue_work(test_sensor.workqueue, &test_sensor.test_work);
+	hrtimer_forward_now(&test_sensor.test_timer, ns_to_ktime(10000000000));
+	return HRTIMER_RESTART;
+}
+
+static int __init test_init(void)
+{
+	INIT_WORK(&test_sensor.test_work, test_work_func);
+	test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
+	hrtimer_init(&test_sensor.test_timer,
+			CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	test_sensor.test_timer.function = test_timer_func;
+	hrtimer_start(&test_sensor.test_timer,
+			ns_to_ktime(10000000000), HRTIMER_MODE_REL);
+	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");

drivers/misc/mediatek/sensor/2.0/sensorhub/test/test4.c

@@ -0,0 +1,102 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "test_sensor4 " fmt
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/hrtimer.h>
+
+#include "hf_sensor_type.h"
+#include "hf_manager.h"
+#include "sensor_comm.h"
+#include "sensor_list.h"
+
+struct test_sensor_t {
+	int count;
+	char *name;
+	struct hrtimer test_timer;
+	struct work_struct test_work;
+	struct workqueue_struct *workqueue;
+};
+
+static struct test_sensor_t test_sensor = {
+	.name = "test_sensor4",
+};
+
+static struct sensor_info sensor_list[SENSOR_TYPE_SENSOR_MAX];
+
+static void test_work_func(struct work_struct *work)
+{
+	int ret = 0;
+	struct sensor_comm_notify notify;
+	unsigned int i = 0;
+
+	notify.sequence = 0;
+	notify.sensor_type = SENSOR_TYPE_ACCELEROMETER;
+	notify.command = SENS_COMM_NOTIFY_TEST_CMD;
+	notify.length = sizeof(notify.value[0]);
+	notify.value[0] = test_sensor.count++;
+	ret = sensor_comm_notify(&notify);
+	if (ret < 0)
+		pr_err("notify failed %d\n", ret);
+
+	memset(sensor_list, 0, sizeof(sensor_list));
+	ret = sensor_list_get_list(sensor_list, ARRAY_SIZE(sensor_list));
+	if (ret < 0) {
+		pr_err("get sensor list failed %d\n", ret);
+		return;
+	}
+
+	for (i = 0; i < ret; i++) {
+		if (sensor_list[i].sensor_type == SENSOR_TYPE_INVALID)
+			continue;
+
+		pr_err("sensor list type:%u, gain:%u, name:%s, vendor:%s\n",
+			sensor_list[i].sensor_type,
+			sensor_list[i].gain, sensor_list[i].name,
+			sensor_list[i].vendor);
+	}
+}
+
+static enum hrtimer_restart test_timer_func(struct hrtimer *timer)
+{
+	queue_work(test_sensor.workqueue, &test_sensor.test_work);
+	return HRTIMER_NORESTART;
+}
+
+static void notify_func(struct sensor_comm_notify *n, void *private_data)
+{
+	hrtimer_start(&test_sensor.test_timer,
+		ns_to_ktime(10000000000), HRTIMER_MODE_REL);
+}
+
+static int __init test_init(void)
+{
+	sensor_comm_notify_handler_register(
+		SENS_COMM_NOTIFY_TEST_CMD, notify_func, NULL);
+	INIT_WORK(&test_sensor.test_work, test_work_func);
+	test_sensor.workqueue = create_singlethread_workqueue(test_sensor.name);
+	hrtimer_init(&test_sensor.test_timer,
+			CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	test_sensor.test_timer.function = test_timer_func;
+	hrtimer_start(&test_sensor.test_timer,
+		ns_to_ktime(10000000000), HRTIMER_MODE_REL);
+	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");

drivers/misc/mediatek/sensor/2.0/sensorhub/timesync.c

@@ -0,0 +1,230 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "timesync " fmt
+
+#include <linux/slab.h>
+#include <linux/timekeeping.h>
+#include <linux/timer.h>
+#include <linux/suspend.h>
+#include <asm/arch_timer.h>
+#include <linux/workqueue.h>
+#include <linux/atomic.h>
+#include <linux/spinlock.h>
+
+#include "timesync.h"
+#include "sensor_comm.h"
+
+static bool timesync_suspend_flag;
+static struct timer_list timesync_timer;
+static struct work_struct timesync_work;
+static struct wakeup_source *wakeup_src;
+
+/* arch counter is 13M, mult is 161319385, shift is 21 */
+static inline int64_t arch_counter_to_ns(int64_t cyc)
+{
+#define ARCH_TIMER_MULT 161319385
+#define ARCH_TIMER_SHIFT 21
+
+	return (cyc * ARCH_TIMER_MULT) >> ARCH_TIMER_SHIFT;
+}
+
+static void timesync_filter_calculate(struct timesync_filter *filter,
+		int64_t host_time, int64_t scp_time)
+{
+	int i = 0;
+	int64_t avg = 0, delta = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&filter->lock, flags);
+	if (host_time > filter->last_time + filter->max_diff ||
+			filter->last_time == 0) {
+		filter->tail = 0;
+		filter->cnt = 0;
+	} else if (host_time < filter->last_time + filter->min_diff) {
+		spin_unlock_irqrestore(&filter->lock, flags);
+		return;
+	}
+	filter->last_time = host_time;
+
+	filter->buffer[filter->tail++] = host_time - scp_time;
+	filter->tail &= (filter->bufsize - 1);
+	if (filter->cnt < filter->bufsize)
+		filter->cnt++;
+
+	for (i = 1, avg = 0; i < filter->cnt; i++)
+		avg += (filter->buffer[i] - filter->buffer[0]);
+	filter->offset = div_s64(avg, filter->cnt) + filter->buffer[0];
+	if (!filter->offset_debug) {
+		filter->offset_debug = filter->offset;
+	} else {
+		delta = filter->offset_debug - filter->offset;
+		if (unlikely(delta >= 2500000) || unlikely(delta <= -2500000))
+			pr_err("%s host with scp jump too large %lld\n",
+				filter->name, delta);
+		filter->offset_debug = filter->offset;
+	}
+	spin_unlock_irqrestore(&filter->lock, flags);
+}
+
+static int timesync_comm_with_nolock(void)
+{
+	int ret = 0;
+	unsigned long flags;
+	struct sensor_comm_ctrl *ctrl = NULL;
+	struct sensor_comm_timesync *time = NULL;
+	int64_t now_time = 0, arch_counter = 0;
+
+	if (READ_ONCE(timesync_suspend_flag))
+		return 0;
+
+	ctrl = kzalloc(sizeof(*ctrl) + sizeof(*time), GFP_KERNEL);
+	if (!ctrl)
+		return -ENOMEM;
+	ctrl->sensor_type = 0;
+	ctrl->command = SENS_COMM_CTRL_TIMESYNC_CMD;
+	ctrl->length = sizeof(*time);
+	time = (struct sensor_comm_timesync *)ctrl->data;
+
+	local_irq_save(flags);
+	now_time = ktime_get_boottime_ns();
+	arch_counter = __arch_counter_get_cntvct();
+	local_irq_restore(flags);
+	pr_info("host boottime %lld\n", now_time);
+
+	time->host_timestamp = now_time;
+	time->host_archcounter = arch_counter;
+	ret = sensor_comm_ctrl_send(ctrl, sizeof(*ctrl) + ctrl->length);
+	kfree(ctrl);
+
+	return ret;
+}
+
+static int timesync_comm_with(void)
+{
+	int ret = 0;
+
+	if (READ_ONCE(timesync_suspend_flag))
+		return 0;
+
+	__pm_stay_awake(wakeup_src);
+	ret = timesync_comm_with_nolock();
+	__pm_relax(wakeup_src);
+	return ret;
+}
+
+static void timesync_work_func(struct work_struct *work)
+{
+	timesync_comm_with();
+}
+
+static void timesync_timer_func(struct timer_list *list)
+{
+	schedule_work(&timesync_work);
+	timesync_start();
+}
+
+void timesync_filter_set(struct timesync_filter *filter,
+		int64_t scp_timestamp, int64_t scp_archcounter)
+{
+	unsigned long flags;
+	int64_t host_timestamp = 0, host_archcounter = 0;
+	int64_t ipi_transfer_time = 0;
+
+	/*
+	 *if (!timekeeping_rtc_skipresume()) {
+	 *	if (READ_ONCE(timesync_suspend_flag))
+	 *		return;
+	 *}
+	*/
+
+	local_irq_save(flags);
+	host_timestamp = ktime_get_boottime_ns();
+	host_archcounter = __arch_counter_get_cntvct();
+	local_irq_restore(flags);
+	ipi_transfer_time = arch_counter_to_ns(host_archcounter -
+		scp_archcounter);
+	scp_timestamp = scp_timestamp + ipi_transfer_time;
+
+	timesync_filter_calculate(filter, host_timestamp, scp_timestamp);
+}
+
+int64_t timesync_filter_get(struct timesync_filter *filter)
+{
+	unsigned long flags;
+	int64_t offset = 0;
+
+	spin_lock_irqsave(&filter->lock, flags);
+	offset = filter->offset;
+	spin_unlock_irqrestore(&filter->lock, flags);
+	return offset;
+}
+
+int timesync_filter_init(struct timesync_filter *filter)
+{
+	if (!filter->max_diff)
+		filter->max_diff = 10000000000LL;
+	if (!filter->min_diff)
+		filter->min_diff = 10000000LL;
+	if (!filter->bufsize)
+		filter->bufsize = 16;
+	WARN_ON(!filter->name);
+	spin_lock_init(&filter->lock);
+	filter->bufsize = roundup_pow_of_two(filter->bufsize);
+	filter->buffer = kcalloc(filter->bufsize, sizeof(*filter->buffer),
+			GFP_KERNEL);
+	if (!filter->buffer)
+		return -ENOMEM;
+	return 0;
+}
+
+void timesync_filter_exit(struct timesync_filter *filter)
+{
+	kfree(filter->buffer);
+	filter->buffer = NULL;
+}
+
+void timesync_start(void)
+{
+	mod_timer(&timesync_timer, jiffies + msecs_to_jiffies(10000));
+}
+
+void timesync_stop(void)
+{
+	del_timer_sync(&timesync_timer);
+}
+
+void timesync_resume(void)
+{
+	pr_info("host resume boottime %lld\n", ktime_get_boottime_ns());
+	WRITE_ONCE(timesync_suspend_flag, false);
+	timesync_comm_with();
+}
+
+void timesync_suspend(void)
+{
+	pr_info("host suspend boottime %lld\n", ktime_get_boottime_ns());
+	WRITE_ONCE(timesync_suspend_flag, true);
+}
+
+int timesync_init(void)
+{
+	INIT_WORK(&timesync_work, timesync_work_func);
+	timer_setup(&timesync_timer, timesync_timer_func, 0);
+	wakeup_src = wakeup_source_register(NULL, "timesync");
+	if (!wakeup_src) {
+		pr_err("timesync wakeup source init fail\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+void timesync_exit(void)
+{
+	timesync_stop();
+	wakeup_source_unregister(wakeup_src);
+	flush_work(&timesync_work);
+}

drivers/misc/mediatek/sensor/2.0/sensorhub/timesync.h

@@ -0,0 +1,37 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _TIMESYNC_H_
+#define _TIMESYNC_H_
+
+#include <linux/spinlock.h>
+
+struct timesync_filter {
+	spinlock_t lock;
+	int64_t last_time;
+	int64_t max_diff;
+	int64_t min_diff;
+	uint32_t cnt;
+	uint32_t tail;
+	uint32_t bufsize;
+	int64_t *buffer;
+	int64_t offset;
+	int64_t offset_debug;
+	char *name;
+};
+
+void timesync_filter_set(struct timesync_filter *filter,
+		int64_t scp_timestamp, int64_t scp_archcounter);
+int64_t timesync_filter_get(struct timesync_filter *filter);
+int timesync_filter_init(struct timesync_filter *filter);
+void timesync_filter_exit(struct timesync_filter *filter);
+void timesync_start(void);
+void timesync_stop(void);
+void timesync_resume(void);
+void timesync_suspend(void);
+int timesync_init(void);
+void timesync_exit(void);
+
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/tiny_crc8.c

@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#define pr_fmt(fmt) "tiny_crc8 " fmt
+
+#include <linux/printk.h>
+
+#include "tiny_crc8.h"
+
+#define TINY_CRC8
+
+#ifndef TINY_CRC8
+/*
+ * CRC using polynomial:
+ *     1. X^8 + X^5 + X^4 + X^0
+ *     2. Little Endian XOR
+ *     3. Polynomial = 0x8c
+ */
+static const uint8_t tiny_crc8_table[] = {
+	0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
+	0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
+	0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
+	0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
+	0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0,
+	0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
+	0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d,
+	0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
+	0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
+	0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
+	0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58,
+	0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
+	0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6,
+	0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
+	0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
+	0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
+	0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f,
+	0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
+	0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92,
+	0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
+	0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
+	0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
+	0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1,
+	0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
+	0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49,
+	0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
+	0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
+	0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
+	0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a,
+	0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
+	0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7,
+	0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35
+};
+
+uint8_t tiny_crc8(uint8_t *ptr, uint8_t len)
+{
+	uint8_t crc = 0;
+
+	while (len-- > 0)
+		crc = tiny_crc8_table[*ptr++ ^ crc];
+
+	return crc;
+}
+#else
+/*
+ * CRC using polynomial:
+ *     1. X^8 + X^5 + X^4 + X^0
+ *     2. Little Endian XOR
+ *     3. Polynomial = 0x8c
+ */
+static const uint8_t tiny_crc8_table[] = {
+	0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
+	0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
+	0x00, 0x9d, 0x23, 0xbe, 0x46, 0xdb, 0x65, 0xf8,
+	0x8c, 0x11, 0xaf, 0x32, 0xca, 0x57, 0xe9, 0x74
+};
+
+uint8_t tiny_crc8(uint8_t *ptr, uint8_t len)
+{
+	uint8_t crc = 0;
+
+	while (len-- > 0) {
+		crc = *ptr++ ^ crc;
+		crc = tiny_crc8_table[crc & 0x0f] ^
+			tiny_crc8_table[16 + ((crc >> 4) & 0x0f)];
+	}
+
+	return crc;
+}
+#endif

drivers/misc/mediatek/sensor/2.0/sensorhub/tiny_crc8.h

@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2020 MediaTek Inc.
+ */
+
+#ifndef _TINY_CRC8_H_
+#define _TINY_CRC8_H_
+
+uint8_t tiny_crc8(uint8_t *ptr, uint8_t len);
+
+#endif

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"
+	default n
+	help
+	  It support high frequency manager test demo.
+	  If this option is set,
+	  it will support
+	  high frequency manager test demo.

drivers/misc/mediatek/sensor/2.0/test/Makefile

@@ -0,0 +1,6 @@
+ccflags-y += -I$(srctree)/drivers/misc/mediatek/sensor/2.0/core
+
+#obj-y += test.o
+obj-y += test_app.o
+obj-y += test_app1.o
+obj-y += test_app2.o

drivers/misc/mediatek/sensor/2.0/test/test.c

@@ -0,0 +1,180 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2020 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 struct sensor_info support_sensors1[] = {
+	{
+		.sensor_type = SENSOR_TYPE_ACCELEROMETER,
+		.gain = 1,
+		.name = {'a', 'c', 'c', 'e', 'l'},
+		.vendor = {'m', 't', 'k'},
+	},
+};
+static struct sensor_info support_sensors2[] = {
+	{
+		.sensor_type = SENSOR_TYPE_MAGNETIC_FIELD,
+		.gain = 1,
+		.name = {'m', 'a', 'g'},
+		.vendor = {'m', 't', 'k'},
+	},
+};
+static struct sensor_info support_sensors3[] = {
+	{
+		.sensor_type = SENSOR_TYPE_GYROSCOPE,
+		.gain = 1,
+		.name = {'g', 'y', 'r', 'o'},
+		.vendor = {'m', 't', 'k'},
+	},
+};
+static struct sensor_info support_sensors4[] = {
+	{
+		.sensor_type = SENSOR_TYPE_PRESSURE,
+		.gain = 1,
+		.name = {'p', 'r', 'e', 's', 's'},
+		.vendor = {'m', 't', 'k'},
+	},
+};
+
+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;
+	struct hf_manager_event event;
+
+	pr_debug("%s %s\n", __func__, driver_dev->hf_dev.dev_name);
+
+	memset(&event, 0, sizeof(struct hf_manager_event));
+	event.timestamp = get_interrupt_timestamp(manager);
+	event.sensor_type = driver_dev->hf_dev.support_list[0].sensor_type;
+	event.accurancy = SENSOR_ACCURANCY_HIGH;
+	event.action = DATA_ACTION;
+	event.word[0] = 0;
+	event.word[1] = 0;
+	event.word[2] = 0;
+	manager->report(manager, &event);
+	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;
+	hf_device_set_private_data(&test_driver1.hf_dev, &test_driver1);
+	err = hf_device_register_manager_create(&test_driver1.hf_dev);
+	if (err < 0) {
+		pr_err("%s hf_manager_create fail\n", __func__);
+		goto out1;
+	}
+
+	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;
+	hf_device_set_private_data(&test_driver2.hf_dev, &test_driver2);
+	err = hf_device_register_manager_create(&test_driver2.hf_dev);
+	if (err < 0) {
+		pr_err("%s hf_manager_create fail\n", __func__);
+		goto out2;
+	}
+
+	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;
+	hf_device_set_private_data(&test_driver3.hf_dev, &test_driver3);
+	err = hf_device_register_manager_create(&test_driver3.hf_dev);
+	if (err < 0) {
+		pr_err("%s hf_manager_create fail\n", __func__);
+		goto out3;
+	}
+
+	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;
+	hf_device_set_private_data(&test_driver4.hf_dev, &test_driver4);
+	err = hf_device_register_manager_create(&test_driver4.hf_dev);
+	if (err < 0) {
+		pr_err("%s hf_manager_create fail\n", __func__);
+		goto out4;
+	}
+	return 0;
+
+out4:
+	hf_device_unregister_manager_destroy(test_driver3.hf_dev.manager);
+out3:
+	hf_device_unregister_manager_destroy(test_driver2.hf_dev.manager);
+out2:
+	hf_device_unregister_manager_destroy(test_driver1.hf_dev.manager);
+out1:
+	return -EINVAL;
+}
+
+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_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");

drivers/misc/mediatek/sensor/2.0/test/test_app.c

@@ -0,0 +1,184 @@
+// 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;
+	struct hf_manager_batch *batch = NULL;
+
+	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;
+	batch = (struct hf_manager_batch *)cmd.data;
+	batch->delay = test_app.val2;
+	batch->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");
+

drivers/misc/mediatek/sensor/2.0/test/test_app1.c

@@ -0,0 +1,209 @@
+// 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"
+
+enum {
+	CUST_CMD_TEST,
+	MAX_CUST_CMD,
+};
+
+#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.command = action;
+	cmd.tx_len = 0;
+	cmd.rx_len = 48;
+	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_TEST);
+}
+
+
+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_TEST);
+}
+
+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");

drivers/misc/mediatek/sensor/2.0/test/test_app2.c

@@ -0,0 +1,163 @@
+// 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;
+	struct hf_manager_batch *batch = NULL;
+
+	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;
+		batch = (struct hf_manager_batch *)cmd.data;
+		batch->delay = val2;
+		batch->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");
+

drivers/misc/mediatek/sensor/Kconfig

@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config MTK_SENSOR_SUPPORT
+	tristate "MTK SENSOR SUPPORT"
+	help
+	  Sensor config for sensor feature in project.
+	  Please say Y here if you want to support and sensors
+	  with mediatek sensor architecture.
+	  If unsure, say N.
+
+#source "drivers/misc/mediatek/sensor/1.0/Kconfig"
+source "drivers/misc/mediatek/sensor/2.0/Kconfig"

drivers/misc/mediatek/sensor/Makefile

@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-y += 2.0/