@@ -120,4 +120,6 @@ source "drivers/staging/qlge/Kconfig"
source "drivers/staging/wfx/Kconfig"
+source "drivers/staging/hf_manager/Kconfig"
+
endif # STAGING
@@ -50,3 +50,4 @@ obj-$(CONFIG_FIELDBUS_DEV) += fieldbus/
obj-$(CONFIG_KPC2000) += kpc2000/
obj-$(CONFIG_QLGE) += qlge/
obj-$(CONFIG_WFX) += wfx/
+obj-$(CONFIG_HF_MANAGER) += hf_manager/
new file mode 100644
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0-only
+#
+# high frequency manager subsystem configuration
+#
+
+menuconfig HF_MANAGER
+ tristate "High frequency manager support"
+ help
+ The high frequency manager subsystem provides a unified framework for
+ drivers for many different types of embedded mems sensors using a
+ number of different physical interfaces (i2c, spi, etc).
\ No newline at end of file
new file mode 100644
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for high frequency manager.
+#
+
+obj-y += core/
+# obj-y += test/
new file mode 100644
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for high frequency manager core
+#
+
+obj-y += hf_manager.o
new file mode 100644
@@ -0,0 +1,1419 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#define pr_fmt(fmt) "[hf_manager]" fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/poll.h>
+#include <linux/bitmap.h>
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <uapi/linux/sched/types.h>
+#include <linux/sched_clock.h>
+#include <linux/log2.h>
+
+#include "hf_manager.h"
+
+static const struct coordinate coordinates[] = {
+ { { 1, 1, 1}, {0, 1, 2} },
+ { { -1, 1, 1}, {1, 0, 2} },
+ { { -1, -1, 1}, {0, 1, 2} },
+ { { 1, -1, 1}, {1, 0, 2} },
+
+ { { -1, 1, -1}, {0, 1, 2} },
+ { { 1, 1, -1}, {1, 0, 2} },
+ { { 1, -1, -1}, {0, 1, 2} },
+ { { -1, -1, -1}, {1, 0, 2} },
+};
+
+static DECLARE_BITMAP(sensor_list_bitmap, SENSOR_TYPE_SENSOR_MAX);
+static struct hf_core hfcore;
+
+static int hf_manager_find_client(struct hf_core *core,
+ struct hf_manager_event *event);
+
+static void init_hf_core(struct hf_core *core)
+{
+ int i = 0;
+
+ mutex_init(&core->manager_lock);
+ INIT_LIST_HEAD(&core->manager_list);
+ for (i = 0; i < SENSOR_TYPE_SENSOR_MAX; ++i) {
+ core->state[i].delay = S64_MAX;
+ core->state[i].latency = S64_MAX;
+ atomic64_set(&core->state[i].start_time, S64_MAX);
+ }
+
+ spin_lock_init(&core->client_lock);
+ INIT_LIST_HEAD(&core->client_list);
+
+ kthread_init_worker(&core->kworker);
+}
+
+void coordinate_map(unsigned char direction, s32 *data)
+{
+ s32 temp[3] = {0};
+
+ if (direction >= ARRAY_SIZE(coordinates))
+ return;
+
+ temp[coordinates[direction].map[0]] =
+ coordinates[direction].sign[0] * data[0];
+ temp[coordinates[direction].map[1]] =
+ coordinates[direction].sign[1] * data[1];
+ temp[coordinates[direction].map[2]] =
+ coordinates[direction].sign[2] * data[2];
+
+ data[0] = temp[0];
+ data[1] = temp[1];
+ data[2] = temp[2];
+}
+EXPORT_SYMBOL_GPL(coordinate_map);
+
+static bool filter_event_by_timestamp(struct hf_client_fifo *hf_fifo,
+ struct hf_manager_event *event)
+{
+ if (hf_fifo->last_time_stamp[event->sensor_type] ==
+ event->timestamp) {
+ return true;
+ }
+ hf_fifo->last_time_stamp[event->sensor_type] = event->timestamp;
+ return false;
+}
+
+static int hf_manager_report_event(struct hf_client *client,
+ struct hf_manager_event *event)
+{
+ unsigned long flags;
+ unsigned int next = 0;
+ s64 hang_time = 0;
+ const s64 max_hang_time = 1000000000LL;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+
+ spin_lock_irqsave(&hf_fifo->buffer_lock, flags);
+ if (unlikely(hf_fifo->buffull)) {
+ hang_time = ktime_get_boottime_ns() - hf_fifo->hang_begin;
+ if (hang_time >= max_hang_time) {
+ /* reset buffer */
+ hf_fifo->buffull = false;
+ hf_fifo->head = 0;
+ hf_fifo->tail = 0;
+ pr_err_ratelimited("[%s][%d:%d] buffer reset %lld\n",
+ client->proc_comm,
+ client->leader_pid,
+ client->ppid,
+ hang_time);
+ } else {
+ pr_err_ratelimited("[%s][%d:%d] buffer full %d %lld\n",
+ client->proc_comm,
+ client->leader_pid,
+ client->ppid,
+ event->sensor_type,
+ event->timestamp);
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+ wake_up_interruptible(&hf_fifo->wait);
+ /*
+ * must return -EAGAIN when buffer full,
+ * tell caller retry send data some times later.
+ */
+ return -EAGAIN;
+ }
+ }
+ /* only data action run filter event */
+ if (likely(event->action == DATA_ACTION) &&
+ unlikely(filter_event_by_timestamp(hf_fifo, event))) {
+ pr_err_ratelimited("[%s][%d:%d] buffer filter %d %lld\n",
+ client->proc_comm, client->leader_pid,
+ client->ppid, event->sensor_type,
+ event->timestamp);
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+ /*
+ * must return 0 when timestamp filtered, tell caller data
+ * already in buffer, don't need send again.
+ */
+ return 0;
+ }
+ hf_fifo->buffer[hf_fifo->head++] = *event;
+ hf_fifo->head &= hf_fifo->bufsize - 1;
+ /* remain 1 count */
+ next = hf_fifo->head + 1;
+ next &= hf_fifo->bufsize - 1;
+ if (unlikely(next == hf_fifo->tail)) {
+ hf_fifo->buffull = true;
+ if (hf_fifo->hang_begin > hf_fifo->client_active) {
+ hang_time = hf_fifo->hang_begin -
+ hf_fifo->client_active;
+ if (hang_time < max_hang_time)
+ hf_fifo->hang_begin = ktime_get_boottime_ns();
+ } else {
+ hf_fifo->hang_begin = ktime_get_boottime_ns();
+ }
+ }
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+
+ wake_up_interruptible(&hf_fifo->wait);
+ return 0;
+}
+
+static void hf_manager_io_schedule(struct hf_manager *manager, s64 timestamp)
+{
+ if (!atomic_read(&manager->io_enabled))
+ return;
+ set_interrupt_timestamp(manager, timestamp);
+ if (READ_ONCE(manager->hf_dev->device_bus) == HF_DEVICE_IO_ASYNC)
+ tasklet_schedule(&manager->io_work_tasklet);
+ else if (READ_ONCE(manager->hf_dev->device_bus) == HF_DEVICE_IO_SYNC)
+ kthread_queue_work(&manager->core->kworker,
+ &manager->io_kthread_work);
+}
+
+static int hf_manager_io_report(struct hf_manager *manager,
+ struct hf_manager_event *event)
+{
+ /* must return 0 when sensor_type exceed and no need to retry */
+ if (unlikely(event->sensor_type >= SENSOR_TYPE_SENSOR_MAX)) {
+ pr_err_ratelimited("Report failed, %u exceed max\n",
+ event->sensor_type);
+ return 0;
+ }
+ return hf_manager_find_client(manager->core, event);
+}
+
+static void hf_manager_io_complete(struct hf_manager *manager)
+{
+ clear_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags);
+}
+
+static void hf_manager_io_sample(struct hf_manager *manager)
+{
+ int retval;
+
+ if (!manager->hf_dev || !manager->hf_dev->sample)
+ return;
+
+ if (!test_and_set_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags)) {
+ retval = manager->hf_dev->sample(manager->hf_dev);
+ if (retval) {
+ clear_bit(HF_MANAGER_IO_IN_PROGRESS,
+ &manager->flags);
+ }
+ }
+}
+
+static void hf_manager_io_tasklet(unsigned long data)
+{
+ struct hf_manager *manager = (struct hf_manager *)data;
+
+ hf_manager_io_sample(manager);
+}
+
+static void hf_manager_io_kthread_work(struct kthread_work *work)
+{
+ struct hf_manager *manager =
+ container_of(work, struct hf_manager, io_kthread_work);
+
+ hf_manager_io_sample(manager);
+}
+
+static void hf_manager_sched_sample(struct hf_manager *manager, s64 timestamp)
+{
+ hf_manager_io_schedule(manager, timestamp);
+}
+
+static enum hrtimer_restart hf_manager_io_poll(struct hrtimer *timer)
+{
+ s64 interval = 0;
+
+ struct hf_manager *manager =
+ (struct hf_manager *)container_of(timer,
+ struct hf_manager, io_poll_timer);
+
+ hf_manager_sched_sample(manager, ktime_get_boottime_ns());
+ interval = atomic64_read(&manager->io_poll_interval);
+ hrtimer_forward_now(&manager->io_poll_timer, ns_to_ktime(interval));
+ return HRTIMER_RESTART;
+}
+
+static void hf_manager_io_interrupt(struct hf_manager *manager, s64 timestamp)
+{
+ hf_manager_sched_sample(manager, timestamp);
+}
+
+int hf_manager_create(struct hf_device *device)
+{
+ u8 sensor_type = 0;
+ int i = 0, err = 0;
+ u32 gain = 0;
+ struct hf_manager *manager = NULL;
+
+ if (!device || !device->dev_name ||
+ !device->support_list || !device->support_size)
+ return -EINVAL;
+
+ manager = kzalloc(sizeof(*manager), GFP_KERNEL);
+ if (!manager)
+ return -ENOMEM;
+
+ manager->hf_dev = device;
+ manager->core = &hfcore;
+ device->manager = manager;
+
+ atomic_set(&manager->io_enabled, 0);
+ atomic64_set(&manager->io_poll_interval, S64_MAX);
+
+ clear_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags);
+ clear_bit(HF_MANAGER_IO_READY, &manager->flags);
+
+ if (device->device_poll == HF_DEVICE_IO_POLLING) {
+ hrtimer_init(&manager->io_poll_timer,
+ CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+ manager->io_poll_timer.function = hf_manager_io_poll;
+ } else if (device->device_poll == HF_DEVICE_IO_INTERRUPT) {
+ manager->interrupt = hf_manager_io_interrupt;
+ }
+ manager->report = hf_manager_io_report;
+ manager->complete = hf_manager_io_complete;
+
+ if (device->device_bus == HF_DEVICE_IO_ASYNC)
+ tasklet_init(&manager->io_work_tasklet,
+ hf_manager_io_tasklet, (unsigned long)manager);
+ else if (device->device_bus == HF_DEVICE_IO_SYNC)
+ kthread_init_work(&manager->io_kthread_work,
+ hf_manager_io_kthread_work);
+
+ for (i = 0; i < device->support_size; ++i) {
+ sensor_type = device->support_list[i].sensor_type;
+ gain = device->support_list[i].gain;
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX || !gain)) {
+ pr_err("Device:%s register failed, %u invalid gain\n",
+ device->dev_name, sensor_type);
+ err = -EINVAL;
+ goto out_err;
+ }
+ if (test_and_set_bit(sensor_type, sensor_list_bitmap)) {
+ pr_err("Device:%s register failed, %u repeat\n",
+ device->dev_name, sensor_type);
+ err = -EBUSY;
+ goto out_err;
+ }
+ }
+
+ INIT_LIST_HEAD(&manager->list);
+ mutex_lock(&manager->core->manager_lock);
+ list_add(&manager->list, &manager->core->manager_list);
+ mutex_unlock(&manager->core->manager_lock);
+
+ return 0;
+out_err:
+ kfree(manager);
+ device->manager = NULL;
+ return err;
+}
+EXPORT_SYMBOL_GPL(hf_manager_create);
+
+int hf_manager_destroy(struct hf_manager *manager)
+{
+ u8 sensor_type = 0;
+ int i = 0;
+ struct hf_device *device = NULL;
+
+ if (!manager || !manager->hf_dev || !manager->hf_dev->support_list)
+ return -EINVAL;
+
+ device = manager->hf_dev;
+ for (i = 0; i < device->support_size; ++i) {
+ sensor_type = device->support_list[i].sensor_type;
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX)) {
+ pr_err("Device:%s unregister failed, %u exceed max\n",
+ device->dev_name, sensor_type);
+ continue;
+ }
+ clear_bit(sensor_type, sensor_list_bitmap);
+ }
+ mutex_lock(&manager->core->manager_lock);
+ list_del(&manager->list);
+ mutex_unlock(&manager->core->manager_lock);
+ if (device->device_poll == HF_DEVICE_IO_POLLING)
+ hrtimer_cancel(&manager->io_poll_timer);
+ if (device->device_bus == HF_DEVICE_IO_ASYNC)
+ tasklet_kill(&manager->io_work_tasklet);
+ else if (device->device_bus == HF_DEVICE_IO_SYNC)
+ kthread_flush_work(&manager->io_kthread_work);
+
+ while (test_bit(HF_MANAGER_IO_IN_PROGRESS, &manager->flags))
+ cpu_relax();
+
+ kfree(manager);
+ return 0;
+}
+EXPORT_SYMBOL_GPL(hf_manager_destroy);
+
+static int hf_manager_distinguish_event(struct hf_client *client,
+ struct hf_manager_event *event)
+{
+ int err = 0;
+ unsigned long flags;
+ struct sensor_state *request = &client->request[event->sensor_type];
+
+ switch (event->action) {
+ case DATA_ACTION:
+ /* must relay on enable status client requested */
+ if (READ_ONCE(request->enable) &&
+ (event->timestamp > atomic64_read(&request->start_time)))
+ err = hf_manager_report_event(client, event);
+ break;
+ case FLUSH_ACTION:
+ /*
+ * flush relay on flush count client requested,
+ * must not relay on enable status.
+ * flush may report both by looper thread and disable thread.
+ * spinlock prevent flush count report more than request.
+ * sequence:
+ * flush = 1
+ * looper thread flush > 0
+ * looper thread hf_manager_report_event
+ * disable thread flush > 0
+ * disable thread hf_manager_report_event
+ * flush complete report 2 times but request is 1.
+ */
+ spin_lock_irqsave(&client->request_lock, flags);
+ if (atomic_read(&request->flush) > 0) {
+ err = hf_manager_report_event(client, event);
+ /* return < 0, don't decrease flush count */
+ if (err < 0) {
+ spin_unlock_irqrestore(&client->request_lock,
+ flags);
+ return err;
+ }
+ atomic_dec_if_positive(&request->flush);
+ }
+ spin_unlock_irqrestore(&client->request_lock, flags);
+ break;
+ case BIAS_ACTION:
+ /* relay on status client requested, don't check return */
+ if (READ_ONCE(request->bias))
+ hf_manager_report_event(client, event);
+ break;
+ case CALI_ACTION:
+ /* cali on status client requested, don't check return */
+ if (READ_ONCE(request->cali))
+ hf_manager_report_event(client, event);
+ break;
+ case TEMP_ACTION:
+ /* temp on status client requested, don't check return */
+ if (READ_ONCE(request->temp))
+ hf_manager_report_event(client, event);
+ break;
+ case TEST_ACTION:
+ /* test on status client requested, don't check return */
+ if (READ_ONCE(request->test))
+ hf_manager_report_event(client, event);
+ break;
+ case RAW_ACTION:
+ /* raw on status client requested, don't check return */
+ if (READ_ONCE(request->raw))
+ hf_manager_report_event(client, event);
+ break;
+ default:
+ pr_err("Report %u failed, unknown action %u\n",
+ event->sensor_type, event->action);
+ /* unknown action must return 0 */
+ err = 0;
+ break;
+ }
+ return err;
+}
+
+static int hf_manager_find_client(struct hf_core *core,
+ struct hf_manager_event *event)
+{
+ int err = 0;
+ unsigned long flags;
+ struct hf_client *client = NULL;
+
+ spin_lock_irqsave(&core->client_lock, flags);
+ list_for_each_entry(client, &core->client_list, list) {
+ /* must (err |=), collect all err to decide retry */
+ err |= hf_manager_distinguish_event(client, event);
+ }
+ spin_unlock_irqrestore(&core->client_lock, flags);
+
+ return err;
+}
+
+static struct hf_manager *hf_manager_find_manager(struct hf_core *core,
+ u8 sensor_type)
+{
+ int i = 0;
+ struct hf_manager *manager = NULL;
+ struct hf_device *device = NULL;
+
+ list_for_each_entry(manager, &core->manager_list, list) {
+ device = READ_ONCE(manager->hf_dev);
+ if (!device || !device->support_list)
+ continue;
+ for (i = 0; i < device->support_size; ++i) {
+ if (sensor_type == device->support_list[i].sensor_type)
+ return manager;
+ }
+ }
+ pr_err("Failed to find manager, %u unregistered\n", sensor_type);
+ return NULL;
+}
+
+static void hf_manager_update_client_param(struct hf_client *client,
+ struct hf_manager_cmd *cmd,
+ struct sensor_state *old)
+{
+ struct sensor_state *request = &client->request[cmd->sensor_type];
+
+ /* only enable disable update action delay and latency */
+ if (cmd->action == HF_MANAGER_SENSOR_ENABLE) {
+ /* save enable delay latency and start_time to old */
+ old->enable = request->enable;
+ old->delay = request->delay;
+ old->latency = request->latency;
+ atomic64_set(&old->start_time,
+ atomic64_read(&request->start_time));
+ /* update new */
+ if (!request->enable)
+ atomic64_set(&request->start_time,
+ ktime_get_boottime_ns());
+ request->enable = true;
+ request->delay = cmd->delay;
+ request->latency = cmd->latency;
+ } else if (cmd->action == HF_MANAGER_SENSOR_DISABLE) {
+ atomic64_set(&request->start_time, S64_MAX);
+ request->enable = false;
+ request->delay = S64_MAX;
+ request->latency = S64_MAX;
+ }
+}
+
+static void hf_manager_clear_client_param(struct hf_client *client,
+ struct hf_manager_cmd *cmd,
+ struct sensor_state *old)
+{
+ struct sensor_state *request = &client->request[cmd->sensor_type];
+
+ if (cmd->action == HF_MANAGER_SENSOR_ENABLE) {
+ /*
+ * restore enable delay latency and start_time
+ * remember must not restore bias raw etc
+ */
+ atomic64_set(&request->start_time,
+ atomic64_read(&old->start_time));
+ request->enable = old->enable;
+ request->delay = old->delay;
+ request->latency = old->latency;
+ } else if (cmd->action == HF_MANAGER_SENSOR_DISABLE) {
+ atomic64_set(&request->start_time, S64_MAX);
+ request->enable = false;
+ request->delay = S64_MAX;
+ request->latency = S64_MAX;
+ }
+}
+
+static void hf_manager_find_best_param(struct hf_core *core,
+ u8 sensor_type, bool *action,
+ s64 *delay, s64 *latency)
+{
+ unsigned long flags;
+ struct hf_client *client = NULL;
+ struct sensor_state *request = NULL;
+ bool tmp_enable = false;
+ s64 tmp_delay = S64_MAX;
+ s64 tmp_latency = S64_MAX;
+
+ spin_lock_irqsave(&core->client_lock, flags);
+ list_for_each_entry(client, &core->client_list, list) {
+ request = &client->request[sensor_type];
+ if (request->enable) {
+ tmp_enable = true;
+ if (request->delay < tmp_delay)
+ tmp_delay = request->delay;
+ if (request->latency < tmp_latency)
+ tmp_latency = request->latency;
+ }
+ }
+ spin_unlock_irqrestore(&core->client_lock, flags);
+ *action = tmp_enable;
+ *delay = tmp_delay;
+ *latency = tmp_latency;
+
+#ifdef HF_MANAGER_DEBUG
+ if (tmp_enable)
+ pr_notice("Find best command %u %u %lld %lld\n",
+ sensor_type, tmp_enable, tmp_delay, tmp_latency);
+ else
+ pr_notice("Find best command %u %u\n",
+ sensor_type, tmp_enable);
+#endif
+}
+
+static inline bool device_rebatch(struct hf_core *core, u8 sensor_type,
+ s64 best_delay, s64 best_latency)
+{
+ if (core->state[sensor_type].delay != best_delay ||
+ core->state[sensor_type].latency != best_latency) {
+ core->state[sensor_type].delay = best_delay;
+ core->state[sensor_type].latency = best_latency;
+ return true;
+ }
+ return false;
+}
+
+static inline bool device_reenable(struct hf_core *core, u8 sensor_type,
+ bool best_enable)
+{
+ if (core->state[sensor_type].enable != best_enable) {
+ core->state[sensor_type].enable = best_enable;
+ return true;
+ }
+ return false;
+}
+
+static inline bool device_redisable(struct hf_core *core, u8 sensor_type,
+ bool best_enable, s64 best_delay,
+ s64 best_latency)
+{
+ if (core->state[sensor_type].enable != best_enable) {
+ core->state[sensor_type].enable = best_enable;
+ core->state[sensor_type].delay = best_delay;
+ core->state[sensor_type].latency = best_latency;
+ return true;
+ }
+ return false;
+}
+
+static inline void device_request_update(struct hf_core *core,
+ u8 sensor_type,
+ struct sensor_state *old)
+{
+ /* save enable delay and latency to old */
+ old->enable = core->state[sensor_type].enable;
+ old->delay = core->state[sensor_type].delay;
+ old->latency = core->state[sensor_type].latency;
+}
+
+static inline void device_request_clear(struct hf_core *core,
+ u8 sensor_type,
+ struct sensor_state *old)
+{
+ /*
+ * restore enable delay and latency
+ * remember must not restore bias raw etc
+ */
+ core->state[sensor_type].enable = old->enable;
+ core->state[sensor_type].delay = old->delay;
+ core->state[sensor_type].latency = old->latency;
+}
+
+static s64 device_poll_min_interval(struct hf_device *device)
+{
+ int i = 0;
+ u8 j = 0;
+ s64 interval = S64_MAX;
+ struct hf_core *core = device->manager->core;
+
+ for (i = 0; i < device->support_size; ++i) {
+ j = device->support_list[i].sensor_type;
+ if (core->state[j].enable) {
+ if (core->state[j].delay < interval)
+ interval = core->state[j].delay;
+ }
+ }
+ return interval;
+}
+
+static void device_poll_trigger(struct hf_device *device, bool enable)
+{
+ s64 min_interval = S64_MAX;
+ struct hf_manager *manager = device->manager;
+
+ WARN_ON(enable && !atomic_read(&manager->io_enabled));
+ min_interval = device_poll_min_interval(device);
+ WARN_ON(atomic_read(&manager->io_enabled) && min_interval == S64_MAX);
+ if (atomic64_read(&manager->io_poll_interval) == min_interval)
+ return;
+ atomic64_set(&manager->io_poll_interval, min_interval);
+ if (atomic_read(&manager->io_enabled))
+ hrtimer_start(&manager->io_poll_timer,
+ ns_to_ktime(min_interval), HRTIMER_MODE_REL);
+ else
+ hrtimer_cancel(&manager->io_poll_timer);
+}
+
+static int hf_manager_device_enable(struct hf_device *device, u8 sensor_type)
+{
+ int err = 0;
+ struct sensor_state old;
+ struct hf_manager *manager = device->manager;
+ struct hf_core *core = device->manager->core;
+ bool best_enable = false;
+ s64 best_delay = S64_MAX;
+ s64 best_latency = S64_MAX;
+
+ if (!device->enable || !device->batch)
+ return -EINVAL;
+
+ hf_manager_find_best_param(core, sensor_type, &best_enable,
+ &best_delay, &best_latency);
+
+ if (best_enable) {
+ device_request_update(core, sensor_type, &old);
+ if (device_rebatch(core, sensor_type,
+ best_delay, best_latency)) {
+ err = device->batch(device, sensor_type,
+ best_delay, best_latency);
+ /* handle error to return when batch fail */
+ if (err < 0) {
+ device_request_clear(core, sensor_type, &old);
+ return err;
+ }
+ }
+ if (device_reenable(core, sensor_type, best_enable)) {
+ /* must update io_enabled before enable */
+ atomic_inc(&manager->io_enabled);
+ err = device->enable(device, sensor_type, best_enable);
+ /* handle error to clear prev request */
+ if (err < 0) {
+ atomic_dec_if_positive(&manager->io_enabled);
+ /*
+ * rebatch success and enable fail.
+ * update prev request from old.
+ */
+ device_request_clear(core, sensor_type, &old);
+ return err;
+ }
+ }
+ if (device->device_poll == HF_DEVICE_IO_POLLING)
+ device_poll_trigger(device, best_enable);
+ } else {
+ if (device_redisable(core, sensor_type, best_enable,
+ best_delay, best_latency)) {
+ atomic_dec_if_positive(&manager->io_enabled);
+ err = device->enable(device, sensor_type, best_enable);
+ /*
+ * disable fail no need to handle error.
+ * run next to update hrtimer or tasklet.
+ */
+ }
+ if (device->device_poll == HF_DEVICE_IO_POLLING)
+ device_poll_trigger(device, best_enable);
+ if (device->device_bus == HF_DEVICE_IO_ASYNC &&
+ !atomic_read(&manager->io_enabled))
+ tasklet_kill(&manager->io_work_tasklet);
+ }
+
+ return err;
+}
+
+static int hf_manager_device_flush(struct hf_device *device, u8 sensor_type)
+{
+ if (!device->flush)
+ return -EINVAL;
+
+ return device->flush(device, sensor_type);
+}
+
+static int hf_manager_device_calibration(struct hf_device *device,
+ u8 sensor_type)
+{
+ if (device->calibration)
+ return device->calibration(device, sensor_type);
+ return 0;
+}
+
+static int hf_manager_device_config_cali(struct hf_device *device,
+ u8 sensor_type, s32 *data)
+{
+ if (device->config_cali)
+ return device->config_cali(device, sensor_type, data);
+ return 0;
+}
+
+static int hf_manager_device_selftest(struct hf_device *device, u8 sensor_type)
+{
+ if (device->selftest)
+ return device->selftest(device, sensor_type);
+ return 0;
+}
+
+static int hf_manager_device_rawdata(struct hf_device *device, u8 sensor_type)
+{
+ int err = 0;
+ unsigned long flags;
+ struct hf_core *core = device->manager->core;
+ struct hf_client *client = NULL;
+ struct sensor_state *request = NULL;
+ bool best_enable = false;
+
+ spin_lock_irqsave(&core->client_lock, flags);
+ list_for_each_entry(client, &core->client_list, list) {
+ request = &client->request[sensor_type];
+ if (request->raw)
+ best_enable = true;
+ }
+ spin_unlock_irqrestore(&core->client_lock, flags);
+
+ if (!device->rawdata)
+ return 0;
+ if (core->state[sensor_type].raw == best_enable)
+ return 0;
+ core->state[sensor_type].raw = best_enable;
+ err = device->rawdata(device, sensor_type, best_enable);
+ if (err < 0)
+ core->state[sensor_type].raw = false;
+ return err;
+}
+
+static int hf_manager_device_info(struct hf_client *client, u8 sensor_type,
+ struct sensor_info *info)
+{
+ int i = 0;
+ int ret = 0;
+ struct hf_manager *manager = NULL;
+ struct hf_device *device = NULL;
+ struct sensor_info *si = NULL;
+
+ mutex_lock(&client->core->manager_lock);
+ manager = hf_manager_find_manager(client->core, sensor_type);
+ if (!manager) {
+ ret = -EINVAL;
+ goto err_out;
+ }
+ device = manager->hf_dev;
+ if (!device || !device->support_list ||
+ !device->support_size) {
+ ret = -EINVAL;
+ goto err_out;
+ }
+ for (i = 0; i < device->support_size; ++i) {
+ if (device->support_list[i].sensor_type == sensor_type) {
+ si = &device->support_list[i];
+ break;
+ }
+ }
+ if (!si) {
+ ret = -EINVAL;
+ goto err_out;
+ }
+ *info = *si;
+
+err_out:
+ mutex_unlock(&client->core->manager_lock);
+ return ret;
+}
+
+static int hf_manager_custom_cmd(struct hf_client *client, u8 sensor_type,
+ struct custom_cmd *cust_cmd)
+{
+ struct hf_manager *manager = NULL;
+ struct hf_device *device = NULL;
+ int ret = 0;
+
+ mutex_lock(&client->core->manager_lock);
+ manager = hf_manager_find_manager(client->core, sensor_type);
+ if (!manager) {
+ ret = -EINVAL;
+ goto err_out;
+ }
+ device = manager->hf_dev;
+ if (!device || !device->dev_name) {
+ ret = -EINVAL;
+ goto err_out;
+ }
+ if (device->custom_cmd)
+ ret = device->custom_cmd(device, sensor_type, cust_cmd);
+
+err_out:
+ mutex_unlock(&client->core->manager_lock);
+ return ret;
+}
+
+static int hf_manager_drive_device(struct hf_client *client,
+ struct hf_manager_cmd *cmd)
+{
+ int err = 0;
+ struct sensor_state old;
+ struct hf_manager *manager = NULL;
+ struct hf_device *device = NULL;
+ struct sensor_state *request = NULL;
+ struct hf_core *core = client->core;
+ u8 sensor_type = cmd->sensor_type;
+
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+
+ mutex_lock(&core->manager_lock);
+ manager = hf_manager_find_manager(core, sensor_type);
+ if (!manager) {
+ mutex_unlock(&core->manager_lock);
+ return -EINVAL;
+ }
+ device = manager->hf_dev;
+ if (!device || !device->dev_name) {
+ mutex_unlock(&core->manager_lock);
+ return -EINVAL;
+ }
+
+#ifdef HF_MANAGER_DEBUG
+ pr_notice("Drive device:%s command %u %u %lld %lld\n",
+ device->dev_name, cmd->sensor_type, cmd->action,
+ cmd->delay, cmd->latency);
+#endif
+
+ switch (cmd->action) {
+ case HF_MANAGER_SENSOR_ENABLE:
+ case HF_MANAGER_SENSOR_DISABLE:
+ hf_manager_update_client_param(client, cmd, &old);
+ err = hf_manager_device_enable(device, sensor_type);
+ if (err < 0)
+ hf_manager_clear_client_param(client, cmd, &old);
+ break;
+ case HF_MANAGER_SENSOR_FLUSH:
+ request = &client->request[sensor_type];
+ atomic_inc(&request->flush);
+ err = hf_manager_device_flush(device, sensor_type);
+ if (err < 0)
+ atomic_dec_if_positive(&request->flush);
+ break;
+ case HF_MANAGER_SENSOR_ENABLE_CALI:
+ err = hf_manager_device_calibration(device, sensor_type);
+ break;
+ case HF_MANAGER_SENSOR_CONFIG_CALI:
+ err = hf_manager_device_config_cali(device, sensor_type,
+ cmd->data);
+ break;
+ case HF_MANAGER_SENSOR_SELFTEST:
+ err = hf_manager_device_selftest(device, sensor_type);
+ break;
+ case HF_MANAGER_SENSOR_RAWDATA:
+ client->request[sensor_type].raw =
+ cmd->data[0] ? true : false;
+ err = hf_manager_device_rawdata(device, sensor_type);
+ if (err < 0)
+ client->request[sensor_type].raw = false;
+ break;
+ default:
+ pr_err("Unknown action %u\n", cmd->action);
+ err = -EINVAL;
+ break;
+ }
+ mutex_unlock(&core->manager_lock);
+ return err;
+}
+
+static int hf_manager_get_sensor_info(struct hf_client *client, u8 sensor_type,
+ struct sensor_info *info)
+{
+ return hf_manager_device_info(client, sensor_type, info);
+}
+
+struct hf_client *hf_client_create(void)
+{
+ unsigned long flags;
+ struct hf_client *client = NULL;
+ struct hf_client_fifo *hf_fifo = NULL;
+
+ client = kzalloc(sizeof(*client), GFP_KERNEL);
+ if (!client)
+ goto err_out;
+
+ /* record process id and thread id for debug */
+ strlcpy(client->proc_comm, current->comm, sizeof(client->proc_comm));
+ client->leader_pid = current->group_leader->pid;
+ client->pid = current->pid;
+ client->core = &hfcore;
+
+#ifdef HF_MANAGER_DEBUG
+ pr_notice("Client create\n");
+#endif
+
+ INIT_LIST_HEAD(&client->list);
+
+ hf_fifo = &client->hf_fifo;
+ hf_fifo->head = 0;
+ hf_fifo->tail = 0;
+ hf_fifo->bufsize = roundup_pow_of_two(HF_CLIENT_FIFO_SIZE);
+ hf_fifo->buffull = false;
+ spin_lock_init(&hf_fifo->buffer_lock);
+ init_waitqueue_head(&hf_fifo->wait);
+ hf_fifo->buffer =
+ kcalloc(hf_fifo->bufsize, sizeof(*hf_fifo->buffer),
+ GFP_KERNEL);
+ if (!hf_fifo->buffer)
+ goto err_free;
+
+ spin_lock_init(&client->request_lock);
+
+ spin_lock_irqsave(&client->core->client_lock, flags);
+ list_add(&client->list, &client->core->client_list);
+ spin_unlock_irqrestore(&client->core->client_lock, flags);
+
+ return client;
+err_free:
+ kfree(client);
+err_out:
+ return NULL;
+}
+EXPORT_SYMBOL_GPL(hf_client_create);
+
+void hf_client_destroy(struct hf_client *client)
+{
+ unsigned long flags;
+
+#ifdef HF_MANAGER_DEBUG
+ pr_notice("Client destroy\n");
+#endif
+
+ spin_lock_irqsave(&client->core->client_lock, flags);
+ list_del(&client->list);
+ spin_unlock_irqrestore(&client->core->client_lock, flags);
+
+ kfree(client->hf_fifo.buffer);
+ kfree(client);
+}
+EXPORT_SYMBOL_GPL(hf_client_destroy);
+
+int hf_client_find_sensor(struct hf_client *client, u8 sensor_type)
+{
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+ if (!test_bit(sensor_type, sensor_list_bitmap))
+ return -EINVAL;
+ return 0;
+}
+EXPORT_SYMBOL_GPL(hf_client_find_sensor);
+
+int hf_client_get_sensor_info(struct hf_client *client, u8 sensor_type,
+ struct sensor_info *info)
+{
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+ if (!test_bit(sensor_type, sensor_list_bitmap))
+ return -EINVAL;
+ return hf_manager_device_info(client, sensor_type, info);
+}
+EXPORT_SYMBOL_GPL(hf_client_get_sensor_info);
+
+int hf_client_request_sensor_cali(struct hf_client *client, u8 sensor_type,
+ unsigned int cmd, bool status)
+{
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+ if (!test_bit(sensor_type, sensor_list_bitmap))
+ return -EINVAL;
+ switch (cmd) {
+ case HF_MANAGER_REQUEST_BIAS_DATA:
+ client->request[sensor_type].bias = status;
+ break;
+ case HF_MANAGER_REQUEST_CALI_DATA:
+ client->request[sensor_type].cali = status;
+ break;
+ case HF_MANAGER_REQUEST_TEMP_DATA:
+ client->request[sensor_type].temp = status;
+ break;
+ case HF_MANAGER_REQUEST_TEST_DATA:
+ client->request[sensor_type].test = status;
+ break;
+ default:
+ pr_err("Unknown command %u\n", cmd);
+ return -EINVAL;
+ }
+ return 0;
+}
+EXPORT_SYMBOL_GPL(hf_client_request_sensor_cali);
+
+int hf_client_control_sensor(struct hf_client *client,
+ struct hf_manager_cmd *cmd)
+{
+ return hf_manager_drive_device(client, cmd);
+}
+EXPORT_SYMBOL_GPL(hf_client_control_sensor);
+
+static int fetch_next(struct hf_client_fifo *hf_fifo,
+ struct hf_manager_event *event)
+{
+ unsigned long flags;
+ int have_event;
+
+ spin_lock_irqsave(&hf_fifo->buffer_lock, flags);
+ have_event = hf_fifo->head != hf_fifo->tail;
+ if (have_event) {
+ *event = hf_fifo->buffer[hf_fifo->tail++];
+ hf_fifo->tail &= hf_fifo->bufsize - 1;
+ hf_fifo->buffull = false;
+ hf_fifo->client_active = ktime_get_boottime_ns();
+ }
+ spin_unlock_irqrestore(&hf_fifo->buffer_lock, flags);
+ return have_event;
+}
+
+/* timeout: MAX_SCHEDULE_TIMEOUT or msecs_to_jiffies(ms) */
+int hf_client_poll_sensor_timeout(struct hf_client *client,
+ struct hf_manager_event *data,
+ int count,
+ long timeout)
+{
+ long ret = 0;
+ int read = 0;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+
+ /* ret must be long to fill timeout(MAX_SCHEDULE_TIMEOUT) */
+ ret = wait_event_interruptible_timeout(hf_fifo->wait,
+ hf_fifo->head != hf_fifo->tail,
+ timeout);
+
+ if (!ret)
+ return -ETIMEDOUT;
+ if (ret < 0)
+ return ret;
+
+ for (;;) {
+ if (hf_fifo->head == hf_fifo->tail)
+ return 0;
+ if (count == 0)
+ break;
+ while (read < count &&
+ fetch_next(hf_fifo, &data[read])) {
+ read++;
+ }
+ if (read)
+ break;
+ }
+ return read;
+}
+EXPORT_SYMBOL_GPL(hf_client_poll_sensor_timeout);
+
+int hf_client_custom_cmd(struct hf_client *client, u8 sensor_type,
+ struct custom_cmd *cust_cmd)
+{
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+ if (!test_bit(sensor_type, sensor_list_bitmap))
+ return -EINVAL;
+ return hf_manager_custom_cmd(client, sensor_type, cust_cmd);
+}
+EXPORT_SYMBOL_GPL(hf_client_custom_cmd);
+
+static int hf_manager_open(struct inode *inode, struct file *filp)
+{
+ struct hf_client *client = hf_client_create();
+
+ if (!client)
+ return -ENOMEM;
+
+ filp->private_data = client;
+ nonseekable_open(inode, filp);
+ return 0;
+}
+
+static int hf_manager_release(struct inode *inode, struct file *filp)
+{
+ struct hf_client *client = filp->private_data;
+
+ filp->private_data = NULL;
+ hf_client_destroy(client);
+ return 0;
+}
+
+static ssize_t hf_manager_read(struct file *filp,
+ char __user *buf,
+ size_t count,
+ loff_t *f_pos)
+{
+ struct hf_client *client = filp->private_data;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+ struct hf_manager_event event;
+ size_t read = 0;
+
+ if (count != 0 && count < sizeof(struct hf_manager_event))
+ return -EINVAL;
+
+ for (;;) {
+ if (hf_fifo->head == hf_fifo->tail)
+ return 0;
+ if (count == 0)
+ break;
+ while (read + sizeof(event) <= count &&
+ fetch_next(hf_fifo, &event)) {
+ if (copy_to_user(buf + read, &event, sizeof(event)))
+ return -EFAULT;
+ read += sizeof(event);
+ }
+ if (read)
+ break;
+ }
+ return read;
+}
+
+static ssize_t hf_manager_write(struct file *filp,
+ const char __user *buf,
+ size_t count,
+ loff_t *f_pos)
+{
+ struct hf_manager_cmd cmd;
+ struct hf_client *client = filp->private_data;
+
+ memset(&cmd, 0, sizeof(cmd));
+
+ if (count != sizeof(struct hf_manager_cmd))
+ return -EINVAL;
+
+ if (copy_from_user(&cmd, buf, count))
+ return -EFAULT;
+
+ return hf_manager_drive_device(client, &cmd);
+}
+
+static unsigned int hf_manager_poll(struct file *filp,
+ struct poll_table_struct *wait)
+{
+ struct hf_client *client = filp->private_data;
+ struct hf_client_fifo *hf_fifo = &client->hf_fifo;
+ unsigned int mask = 0;
+
+ client->ppid = current->pid;
+
+ poll_wait(filp, &hf_fifo->wait, wait);
+
+ if (hf_fifo->head != hf_fifo->tail)
+ mask |= POLLIN | POLLRDNORM;
+
+ return mask;
+}
+
+static long hf_manager_ioctl(struct file *filp,
+ unsigned int cmd, unsigned long arg)
+{
+ struct hf_client *client = filp->private_data;
+ unsigned int size = _IOC_SIZE(cmd);
+ void __user *ubuf = (void __user *)arg;
+ u8 sensor_type = 0;
+ struct ioctl_packet packet;
+ struct sensor_info info;
+ struct custom_cmd cust_cmd;
+
+ memset(&packet, 0, sizeof(packet));
+
+ if (size != sizeof(struct ioctl_packet))
+ return -EINVAL;
+ if (copy_from_user(&packet, ubuf, sizeof(packet)))
+ return -EFAULT;
+ sensor_type = packet.sensor_type;
+ if (unlikely(sensor_type >= SENSOR_TYPE_SENSOR_MAX))
+ return -EINVAL;
+
+ switch (cmd) {
+ case HF_MANAGER_REQUEST_REGISTER_STATUS:
+ packet.status = test_bit(sensor_type, sensor_list_bitmap);
+ if (copy_to_user(ubuf, &packet, sizeof(packet)))
+ return -EFAULT;
+ break;
+ case HF_MANAGER_REQUEST_BIAS_DATA:
+ client->request[sensor_type].bias = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_CALI_DATA:
+ client->request[sensor_type].cali = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_TEMP_DATA:
+ client->request[sensor_type].temp = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_TEST_DATA:
+ client->request[sensor_type].test = packet.status;
+ break;
+ case HF_MANAGER_REQUEST_SENSOR_INFO:
+ if (!test_bit(sensor_type, sensor_list_bitmap))
+ return -EINVAL;
+ memset(&info, 0, sizeof(info));
+ if (hf_manager_get_sensor_info(client, sensor_type, &info))
+ return -EINVAL;
+ if (sizeof(packet.byte) < sizeof(info))
+ return -EINVAL;
+ memcpy(packet.byte, &info, sizeof(info));
+ if (copy_to_user(ubuf, &packet, sizeof(packet)))
+ return -EFAULT;
+ break;
+ case HF_MANAGER_REQUEST_CUST_DATA:
+ if (!test_bit(sensor_type, sensor_list_bitmap))
+ return -EINVAL;
+ memset(&cust_cmd, 0, sizeof(cust_cmd));
+ memcpy(cust_cmd.data, packet.byte, sizeof(cust_cmd.data));
+ if (hf_manager_custom_cmd(client, sensor_type, &cust_cmd))
+ return -EINVAL;
+ if (sizeof(packet.byte) < sizeof(cust_cmd))
+ return -EINVAL;
+ memcpy(packet.byte, &cust_cmd, sizeof(cust_cmd));
+ if (copy_to_user(ubuf, &packet, sizeof(packet)))
+ return -EFAULT;
+ break;
+ default:
+ pr_err("Unknown command %u\n", cmd);
+ return -EINVAL;
+ }
+ return 0;
+}
+
+static const struct file_operations hf_manager_fops = {
+ .owner = THIS_MODULE,
+ .open = hf_manager_open,
+ .release = hf_manager_release,
+ .read = hf_manager_read,
+ .write = hf_manager_write,
+ .poll = hf_manager_poll,
+ .unlocked_ioctl = hf_manager_ioctl,
+ .compat_ioctl = hf_manager_ioctl,
+};
+
+static int hf_manager_proc_show(struct seq_file *m, void *v)
+{
+ int i = 0, j = 0, k = 0;
+ u8 sensor_type = 0;
+ s64 interval = 0, start_time = 0;
+ unsigned long flags;
+ struct hf_core *core = (struct hf_core *)m->private;
+ struct hf_manager *manager = NULL;
+ struct hf_client *client = NULL;
+ struct hf_device *device = NULL;
+
+ seq_puts(m, "**************************************************\n");
+ seq_puts(m, "Manager List:\n");
+ mutex_lock(&core->manager_lock);
+ j = 1;
+ k = 1;
+ list_for_each_entry(manager, &core->manager_list, list) {
+ device = READ_ONCE(manager->hf_dev);
+ if (!device || !device->support_list)
+ continue;
+ interval = atomic64_read(&manager->io_poll_interval);
+ interval = (interval == S64_MAX) ? -1 : interval;
+ seq_printf(m, "%d. manager:[%d,%lld]\n", j++,
+ atomic_read(&manager->io_enabled), interval);
+ seq_printf(m, " device:%s poll:%s bus:%s online\n",
+ device->dev_name,
+ device->device_poll ? "io_polling" : "io_interrupt",
+ device->device_bus ? "io_async" : "io_sync");
+ for (i = 0; i < device->support_size; ++i) {
+ sensor_type = device->support_list[i].sensor_type;
+ seq_printf(m, " (%d) type:%u info:[%u,%s,%s]\n",
+ k++,
+ sensor_type,
+ device->support_list[i].gain,
+ device->support_list[i].name,
+ device->support_list[i].vendor);
+ }
+ }
+ mutex_unlock(&core->manager_lock);
+
+ seq_puts(m, "**************************************************\n");
+ seq_puts(m, "Client List:\n");
+ spin_lock_irqsave(&core->client_lock, flags);
+ j = 1;
+ k = 1;
+ list_for_each_entry(client, &core->client_list, list) {
+ seq_printf(m, "%d. client:%s pid:[%d:%d,%d] online\n",
+ j++,
+ client->proc_comm,
+ client->leader_pid,
+ client->pid,
+ client->ppid);
+ for (i = 0; i < SENSOR_TYPE_SENSOR_MAX; ++i) {
+ if (!client->request[i].enable)
+ continue;
+ start_time =
+ atomic64_read(&client->request[i].start_time);
+ seq_printf(m, " (%d) type:%d param:[%lld,%lld,%lld]\n",
+ k++,
+ i,
+ client->request[i].delay,
+ client->request[i].latency,
+ start_time);
+ }
+ }
+ spin_unlock_irqrestore(&core->client_lock, flags);
+
+ seq_puts(m, "**************************************************\n");
+ seq_puts(m, "Active List:\n");
+ mutex_lock(&core->manager_lock);
+ j = 1;
+ for (i = 0; i < SENSOR_TYPE_SENSOR_MAX; ++i) {
+ if (!core->state[i].enable)
+ continue;
+ seq_printf(m, " (%d) type:%d param:[%lld,%lld]\n",
+ j++,
+ i,
+ core->state[i].delay,
+ core->state[i].latency);
+ }
+ mutex_unlock(&core->manager_lock);
+ return 0;
+}
+
+static int hf_manager_proc_open(struct inode *inode, struct file *filp)
+{
+ return single_open(filp, hf_manager_proc_show, PDE_DATA(inode));
+}
+
+static const struct proc_ops hf_manager_proc_ops = {
+ .proc_open = hf_manager_proc_open,
+ .proc_release = single_release,
+ .proc_read = seq_read,
+ .proc_lseek = seq_lseek,
+};
+
+static int __init hf_manager_init(void)
+{
+ int major = -1;
+ struct class *hf_manager_class;
+ struct device *dev;
+ struct task_struct *task;
+ struct sched_param param = { .sched_priority = MAX_RT_PRIO / 2 };
+
+ init_hf_core(&hfcore);
+
+ major = register_chrdev(0, "hf_manager", &hf_manager_fops);
+ if (major < 0) {
+ pr_err("Unable to get major\n");
+ return major;
+ }
+ hf_manager_class = class_create(THIS_MODULE, "hf_manager");
+ if (IS_ERR(hf_manager_class)) {
+ pr_err("Failed to create class\n");
+ return PTR_ERR(hf_manager_class);
+ }
+ dev = device_create(hf_manager_class, NULL, MKDEV(major, 0),
+ NULL, "hf_manager");
+ if (IS_ERR(dev)) {
+ pr_err("Failed to create device\n");
+ return PTR_ERR(dev);
+ }
+
+ if (!proc_create_data("hf_manager", 0440, NULL,
+ &hf_manager_proc_ops, &hfcore))
+ pr_err("Failed to create proc\n");
+
+ task = kthread_run(kthread_worker_fn,
+ &hfcore.kworker, "hf_manager");
+ if (IS_ERR(task)) {
+ pr_err("Failed to create kthread\n");
+ return PTR_ERR(task);
+ }
+ sched_setscheduler(task, SCHED_FIFO, ¶m);
+ return 0;
+}
+subsys_initcall(hf_manager_init);
+
+MODULE_DESCRIPTION("high frequency manager");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
new file mode 100644
@@ -0,0 +1,181 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#ifndef _HF_SENSOR_MANAGER_H_
+#define _HF_SENSOR_MANAGER_H_
+
+#include <linux/hrtimer.h>
+#include <linux/interrupt.h>
+#include <linux/kthread.h>
+#include <linux/ioctl.h>
+#include <linux/types.h>
+
+#include "hf_sensor_type.h"
+#include "hf_sensor_io.h"
+
+#define HF_MANAGER_IO_IN_PROGRESS 0
+#define HF_MANAGER_IO_READY 1
+
+#define HF_DEVICE_IO_SYNC 0
+#define HF_DEVICE_IO_ASYNC 1
+
+#define HF_DEVICE_IO_INTERRUPT 0
+#define HF_DEVICE_IO_POLLING 1
+
+#define HF_CLIENT_FIFO_SIZE 128
+
+struct coordinate {
+ s8 sign[3];
+ u8 map[3];
+};
+
+struct sensor_state {
+ bool enable;
+ bool bias;
+ bool cali;
+ bool temp;
+ bool test;
+ bool raw;
+ s64 delay;
+ s64 latency;
+ atomic_t flush;
+ atomic64_t start_time;
+};
+
+struct sensor_info {
+ u8 sensor_type;
+ u32 gain;
+ char name[16];
+ char vendor[16];
+};
+
+struct custom_cmd {
+ int data[16];
+};
+
+enum custom_action {
+ CUST_CMD_CALI = 0,
+ /*Add custom cmd action here!*/
+};
+
+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 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,
+ s64 delay, s64 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, s32 *data);
+ int (*selftest)(struct hf_device *hfdev, int sensor_type);
+ int (*rawdata)(struct hf_device *hfdev, int sensor_type, int en);
+ int (*custom_cmd)(struct hf_device *hfdev, int sensor_type,
+ struct custom_cmd *cust_cmd);
+
+ char *dev_name;
+ unsigned char device_poll;
+ unsigned char device_bus;
+
+ struct sensor_info *support_list;
+ unsigned int support_size;
+
+ struct hf_manager *manager;
+ void *private_data;
+};
+
+struct hf_client_fifo {
+ spinlock_t buffer_lock;
+ unsigned int head;
+ unsigned int tail;
+ unsigned int bufsize;
+ unsigned int buffull;
+ s64 hang_begin;
+ s64 client_active;
+ s64 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, s64 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];
+ pid_t leader_pid;
+ pid_t pid;
+ pid_t ppid;
+};
+
+#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_manager_create(struct hf_device *device);
+int hf_manager_destroy(struct hf_manager *manager);
+void coordinate_map(unsigned char direction, s32 *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, u8 sensor_type);
+int hf_client_get_sensor_info(struct hf_client *client,
+ u8 sensor_type, struct sensor_info *info);
+int hf_client_request_sensor_cali(struct hf_client *client, u8 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, u8 sensor_type,
+ struct custom_cmd *cust_cmd);
+
+#endif
new file mode 100644
@@ -0,0 +1,69 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#ifndef _HF_SENSOR_IO_H_
+#define _HF_SENSOR_IO_H_
+
+#include <linux/ioctl.h>
+#include <linux/types.h>
+
+enum {
+ HF_MANAGER_SENSOR_DISABLE,
+ HF_MANAGER_SENSOR_ENABLE,
+ HF_MANAGER_SENSOR_FLUSH,
+ HF_MANAGER_SENSOR_ENABLE_CALI,
+ HF_MANAGER_SENSOR_CONFIG_CALI,
+ HF_MANAGER_SENSOR_SELFTEST,
+ HF_MANAGER_SENSOR_RAWDATA,
+};
+
+enum {
+ DATA_ACTION,
+ FLUSH_ACTION,
+ BIAS_ACTION,
+ CALI_ACTION,
+ TEMP_ACTION,
+ TEST_ACTION,
+ RAW_ACTION,
+};
+
+struct hf_manager_cmd {
+ u8 sensor_type;
+ u8 action;
+ s64 delay;
+ s64 latency;
+ s32 data[12];
+} __packed;
+
+struct hf_manager_event {
+ s64 timestamp;
+ u8 sensor_type;
+ u8 accurancy;
+ u8 action;
+ u8 reserved;
+ union {
+ s32 word[6];
+ s8 byte[0];
+ };
+} __packed;
+
+struct ioctl_packet {
+ u8 sensor_type;
+ union {
+ bool status;
+ s8 byte[64];
+ };
+} __packed;
+
+#define HF_MANAGER_REQUEST_REGISTER_STATUS _IOWR('a', 1, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_BIAS_DATA _IOW('a', 2, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_CALI_DATA _IOW('a', 3, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_TEMP_DATA _IOW('a', 4, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_TEST_DATA _IOW('a', 5, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_SENSOR_INFO _IOWR('a', 6, struct ioctl_packet)
+#define HF_MANAGER_REQUEST_CUST_DATA _IOWR('a', 7, struct ioctl_packet)
+
+#endif
new file mode 100644
@@ -0,0 +1,56 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#ifndef _HF_SENSOR_TYPE_H_
+#define _HF_SENSOR_TYPE_H_
+
+enum {
+ 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,
+ SENSOR_TYPE_SENSOR_MAX,
+};
+
+enum {
+ SENSOR_ACCURANCY_UNRELIALE,
+ SENSOR_ACCURANCY_LOW,
+ SENSOR_ACCURANCY_MEDIUM,
+ SENSOR_ACCURANCY_HIGH,
+};
+
+#endif
new file mode 100644
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for high frequency manager test
+#
+
+ccflags-y += -I$(srctree)/drivers/staging/hf_manager/core
+
+obj-y += test.o
+obj-y += test_app.o
+obj-y += test_app1.o
+obj-y += test_app2.o
new file mode 100644
@@ -0,0 +1,182 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#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,
+ s64 delay, s64 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;
+
+ err = hf_manager_create(&test_driver1.hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ goto out1;
+ }
+ hf_device_set_private_data(&test_driver1.hf_dev, &test_driver1);
+
+ test_driver2.hf_dev.dev_name = "test_driver2";
+ test_driver2.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver2.hf_dev.device_bus = HF_DEVICE_IO_SYNC;
+ test_driver2.hf_dev.support_list = support_sensors2;
+ test_driver2.hf_dev.support_size = ARRAY_SIZE(support_sensors2);
+ test_driver2.hf_dev.enable = test_enable;
+ test_driver2.hf_dev.batch = test_batch;
+ test_driver2.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver2.hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ goto out2;
+ }
+ hf_device_set_private_data(&test_driver2.hf_dev, &test_driver2);
+
+ test_driver3.hf_dev.dev_name = "test_driver3";
+ test_driver3.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver3.hf_dev.device_bus = HF_DEVICE_IO_ASYNC;
+ test_driver3.hf_dev.support_list = support_sensors3;
+ test_driver3.hf_dev.support_size = ARRAY_SIZE(support_sensors3);
+ test_driver3.hf_dev.enable = test_enable;
+ test_driver3.hf_dev.batch = test_batch;
+ test_driver3.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver3.hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ goto out3;
+ }
+ hf_device_set_private_data(&test_driver3.hf_dev, &test_driver3);
+
+ test_driver4.hf_dev.dev_name = "test_driver4";
+ test_driver4.hf_dev.device_poll = HF_DEVICE_IO_POLLING;
+ test_driver4.hf_dev.device_bus = HF_DEVICE_IO_ASYNC;
+ test_driver4.hf_dev.support_list = support_sensors4;
+ test_driver4.hf_dev.support_size = ARRAY_SIZE(support_sensors4);
+ test_driver4.hf_dev.enable = test_enable;
+ test_driver4.hf_dev.batch = test_batch;
+ test_driver4.hf_dev.sample = test_sample;
+
+ err = hf_manager_create(&test_driver4.hf_dev);
+ if (err < 0) {
+ pr_err("%s hf_manager_create fail\n", __func__);
+ goto out4;
+ }
+ hf_device_set_private_data(&test_driver4.hf_dev, &test_driver4);
+ return 0;
+
+out4:
+ hf_manager_destroy(test_driver3.hf_dev.manager);
+out3:
+ hf_manager_destroy(test_driver2.hf_dev.manager);
+out2:
+ hf_manager_destroy(test_driver1.hf_dev.manager);
+out1:
+ return -EINVAL;
+}
+
+static int __init test_init(void)
+{
+ tests_init();
+ return 0;
+}
+
+module_init(test_init);
+
+MODULE_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
new file mode 100644
@@ -0,0 +1,174 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#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 void test_app_request_cali(u8 action, bool enable)
+{
+ switch (action) {
+ case CALI_ACTION:
+ hf_client_request_sensor_cali(test_app.client,
+ test_app.sensor_type,
+ HF_MANAGER_REQUEST_CALI_DATA,
+ enable);
+ break;
+ case TEST_ACTION:
+ hf_client_request_sensor_cali(test_app.client,
+ test_app.sensor_type,
+ HF_MANAGER_REQUEST_TEST_DATA,
+ enable);
+ break;
+ }
+}
+
+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("[%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 */
+ test_app_request_cali(data[i].action, false);
+ }
+ }
+ return 0;
+}
+
+#define test_app_attr(_name) \
+static struct kobj_attribute _name##_attr = { \
+ .attr = { \
+ .name = __stringify(_name), \
+ .mode = 0644, \
+ }, \
+ .show = _name##_show, \
+ .store = _name##_store, \
+}
+
+static ssize_t control_show(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ char *buf)
+{
+ return sprintf(buf, "sensor_type=%u,val1=%u,val2=%u\n",
+ test_app.sensor_type,
+ test_app.val1,
+ test_app.val2);
+}
+
+static ssize_t control_store(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ const char *buf, size_t n)
+{
+ int ret = 0;
+ struct hf_manager_cmd cmd;
+
+ if (!test_app.client)
+ goto out;
+
+ ret = sscanf(buf, "%u,%u,%u", &test_app.sensor_type,
+ &test_app.val1, &test_app.val2);
+ if (ret != 3) {
+ pr_err("control store param error\n");
+ goto out;
+ }
+
+ ret = hf_client_find_sensor(test_app.client, test_app.sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n",
+ test_app.sensor_type);
+ goto out;
+ }
+
+ test_app_request_cali(test_app.val1, true);
+
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = test_app.sensor_type;
+ cmd.action = test_app.val1;
+ cmd.delay = test_app.val2;
+ cmd.latency = 0;
+ ret = hf_client_control_sensor(test_app.client, &cmd);
+ if (ret < 0) {
+ pr_err("hf_client_control_sensor %u fail\n",
+ test_app.sensor_type);
+ goto out;
+ }
+out:
+ return n;
+}
+
+test_app_attr(control);
+
+static struct attribute *attr[] = {
+ &control_attr.attr,
+ NULL,
+};
+
+static const struct attribute_group attr_group = {
+ .attrs = attr,
+};
+
+static int __init test_app_init(void)
+{
+ test_app.task = kthread_run(test_app_kthread,
+ &test_app, "test_app");
+ if (IS_ERR(test_app.task))
+ pr_err("kthread_run create fail\n");
+
+ test_app.kobj = kobject_create_and_add("test_app", NULL);
+ if (!test_app.kobj) {
+ pr_err("kobject create fail\n");
+ return -ENOMEM;
+ }
+ if (sysfs_create_group(test_app.kobj, &attr_group)) {
+ pr_err("sysfs create fail\n");
+ return -EFAULT;
+ }
+ return 0;
+}
+
+module_init(test_app_init);
+
+MODULE_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
new file mode 100644
@@ -0,0 +1,143 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#define pr_fmt(fmt) "[test_app1] " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kobject.h>
+
+#include "hf_manager.h"
+
+#define test_app_attr(_name) \
+static struct kobj_attribute _name##_attr = { \
+ .attr = { \
+ .name = __stringify(_name), \
+ .mode = 0644, \
+ }, \
+ .show = _name##_show, \
+}
+
+static ssize_t test_app1_cmd(char *buf, int sensor_type,
+ int action, unsigned int request)
+{
+ ssize_t ret = 0;
+ struct hf_client *client = NULL;
+ struct hf_manager_cmd cmd;
+ struct hf_manager_event data[1];
+
+ client = hf_client_create();
+ if (!client) {
+ pr_err("hf_client_create fail\n");
+ return -ENOMEM;
+ }
+ ret = hf_client_find_sensor(client, sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n", sensor_type);
+ goto out;
+ }
+ hf_client_request_sensor_cali(client, sensor_type,
+ request, true);
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = sensor_type;
+ cmd.action = action;
+ ret = hf_client_control_sensor(client, &cmd);
+ if (ret < 0) {
+ pr_err("hf_client_control_sensor %u %u fail\n",
+ sensor_type, action);
+ goto out;
+ }
+ ret = hf_client_poll_sensor_timeout(client, data, ARRAY_SIZE(data),
+ msecs_to_jiffies(3000));
+ hf_client_request_sensor_cali(client, sensor_type,
+ request, false);
+ if (ret >= 0)
+ ret = sprintf(buf, "[%d,%d,%d,%lld,%d,%d,%d]\n",
+ data[0].sensor_type,
+ data[0].action,
+ data[0].accurancy,
+ data[0].timestamp,
+ data[0].word[0],
+ data[0].word[1],
+ data[0].word[2]);
+out:
+ hf_client_destroy(client);
+ return ret;
+}
+
+static ssize_t 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_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_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_seltest);
+test_app_attr(gyro_cali);
+test_app_attr(gyro_selftest);
+
+static struct attribute *attr[] = {
+ &acc_cali_attr.attr,
+ &acc_seltest_attr.attr,
+ &gyro_cali_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");
new file mode 100644
@@ -0,0 +1,162 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020 MediaTek Inc.
+ * Author: Hongxu Zhao <hongxu.zhao@mediatek.com>
+ */
+
+#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("[%d,%d,%lld,%d,%d,%d]\n",
+ data[i].sensor_type,
+ data[i].action,
+ data[i].timestamp,
+ data[i].word[0],
+ data[i].word[1],
+ data[i].word[2]);
+ }
+ }
+ return 0;
+}
+
+#define test_app_attr(_name) \
+static struct kobj_attribute _name##_attr = { \
+ .attr = { \
+ .name = __stringify(_name), \
+ .mode = 0644, \
+ }, \
+ .store = _name##_store, \
+}
+
+static ssize_t control_store(struct kobject *kobj,
+ struct kobj_attribute *attr,
+ const char *buf, size_t n)
+{
+ int ret = 0;
+ int sensor_type = 0, val1 = 0, val2 = 0;
+ struct hf_manager_cmd cmd;
+
+ ret = sscanf(buf, "%u,%u,%u", &sensor_type, &val1, &val2);
+ if (ret != 3) {
+ pr_err("control store param error\n");
+ return -EINVAL;
+ }
+
+ if (val1 == HF_MANAGER_SENSOR_ENABLE) {
+ if (test_app.client)
+ return -EINVAL;
+ test_app.client = hf_client_create();
+ if (!test_app.client) {
+ pr_err("hf_client_create fail\n");
+ return -ENOMEM;
+ }
+ if (!test_app.task) {
+ test_app.task = kthread_run(test_app_kthread,
+ &test_app, "test_app2");
+ if (IS_ERR(test_app.task)) {
+ pr_err("kthread_run create fail\n");
+ return -ENOMEM;
+ }
+ }
+ ret = hf_client_find_sensor(test_app.client, sensor_type);
+ if (ret < 0) {
+ pr_err("hf_client_find_sensor %u fail\n",
+ sensor_type);
+ return -EINVAL;
+ }
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = sensor_type;
+ cmd.action = val1;
+ cmd.delay = val2;
+ cmd.latency = 0;
+ ret = hf_client_control_sensor(test_app.client, &cmd);
+ if (ret < 0) {
+ pr_err("hf_client_control_sensor %u fail\n",
+ sensor_type);
+ return -EINVAL;
+ }
+ } else if (val1 == HF_MANAGER_SENSOR_DISABLE) {
+ if (test_app.client) {
+ memset(&cmd, 0, sizeof(cmd));
+ cmd.sensor_type = sensor_type;
+ cmd.action = val1;
+ hf_client_control_sensor(test_app.client, &cmd);
+ }
+ if (test_app.task) {
+ kthread_stop(test_app.task);
+ test_app.task = NULL;
+ }
+ if (test_app.client) {
+ hf_client_destroy(test_app.client);
+ test_app.client = NULL;
+ }
+ }
+
+ return n;
+}
+
+test_app_attr(control);
+
+static struct attribute *attr[] = {
+ &control_attr.attr,
+ NULL,
+};
+
+static const struct attribute_group attr_group = {
+ .attrs = attr,
+};
+
+static int __init test_app_init(void)
+{
+ struct kobject *kobj = kobject_create_and_add("test_app2", NULL);
+
+ if (!kobj) {
+ pr_err("kobject create fail\n");
+ return -ENOMEM;
+ }
+ if (sysfs_create_group(kobj, &attr_group)) {
+ pr_err("sysfs create fail\n");
+ return -EFAULT;
+ }
+ return 0;
+}
+
+module_init(test_app_init);
+
+MODULE_DESCRIPTION("high frequency manager test");
+MODULE_AUTHOR("Hongxu Zhao <hongxu.zhao@mediatek.com>");
+MODULE_LICENSE("GPL v2");
Add a new sensor framework into linux kernel which can support multi client request sensor data. There are the following features: 1.Ringbuffer between manager and client; 2.Kernel space user interface; 3.User space user interface with syscall; 4.Each client hang detect mechanism; 5.Polling timer management in framework no need driver concern; 6.Polling kthread work intergrated into a single kthread worker to save system resources in framework no need driver concern; 7.Proc file system to show manager device and client details; 8.Compitable with android and widely used in many mediatek platform products; Change-Id: I6361cdc2d51de50f66eede7df099c4575e7ec473 Signed-off-by: hongxu.zhao <hongxu.zhao@mediatek.com> --- drivers/staging/Kconfig | 2 + drivers/staging/Makefile | 1 + drivers/staging/hf_manager/Kconfig | 11 + drivers/staging/hf_manager/Makefile | 7 + drivers/staging/hf_manager/core/Makefile | 6 + drivers/staging/hf_manager/core/hf_manager.c | 1419 +++++++++++++++++ drivers/staging/hf_manager/core/hf_manager.h | 181 +++ .../staging/hf_manager/core/hf_sensor_io.h | 69 + .../staging/hf_manager/core/hf_sensor_type.h | 56 + drivers/staging/hf_manager/test/Makefile | 11 + drivers/staging/hf_manager/test/test.c | 182 +++ drivers/staging/hf_manager/test/test_app.c | 174 ++ drivers/staging/hf_manager/test/test_app1.c | 143 ++ drivers/staging/hf_manager/test/test_app2.c | 162 ++ 14 files changed, 2424 insertions(+) create mode 100644 drivers/staging/hf_manager/Kconfig create mode 100644 drivers/staging/hf_manager/Makefile create mode 100644 drivers/staging/hf_manager/core/Makefile create mode 100644 drivers/staging/hf_manager/core/hf_manager.c create mode 100644 drivers/staging/hf_manager/core/hf_manager.h create mode 100644 drivers/staging/hf_manager/core/hf_sensor_io.h create mode 100644 drivers/staging/hf_manager/core/hf_sensor_type.h create mode 100644 drivers/staging/hf_manager/test/Makefile create mode 100644 drivers/staging/hf_manager/test/test.c create mode 100644 drivers/staging/hf_manager/test/test_app.c create mode 100644 drivers/staging/hf_manager/test/test_app1.c create mode 100644 drivers/staging/hf_manager/test/test_app2.c