// SPDX-License-Identifier: GPL-2.0 /* * Copyright (C) 2020 MediaTek Inc. */ #define pr_fmt(fmt) "transceiver " fmt #include #include #include #include #include #include #include #include #include #include #include #include #include #include "ready.h" #include "sensor_comm.h" #include "sensor_list.h" #include "share_memory.h" #include "timesync.h" #include "debug.h" #include "custom_cmd.h" #include "sensor_freq.h" struct transceiver_config { uint8_t length; uint8_t data[0] __aligned(4); }; struct transceiver_state { bool enable; uint8_t flush; struct sensor_comm_batch batch; struct transceiver_config *config; }; struct transceiver_device { struct hf_device hf_dev; struct timesync_filter filter; struct mutex enable_lock; struct mutex flush_lock; struct mutex config_lock; struct transceiver_state state[SENSOR_TYPE_SENSOR_MAX]; struct sensor_info support_list[SENSOR_TYPE_SENSOR_MAX]; unsigned int support_size; struct share_mem shm_reader; struct share_mem_data shm_buffer[8]; struct share_mem shm_super_reader; struct share_mem_super_data shm_super_buffer[4]; struct wakeup_source *wakeup_src; int64_t raw_ts_reverse_debug[SENSOR_TYPE_SENSOR_MAX]; int64_t comp_ts_reverse_debug[SENSOR_TYPE_SENSOR_MAX]; atomic_t first_bootup; atomic_t normal_wp_dropped; atomic_t super_wp_dropped; struct task_struct *task; }; static struct transceiver_device transceiver_dev; static DEFINE_RATELIMIT_STATE(ratelimit, 5 * HZ, 10); DEFINE_SPINLOCK(transceiver_fifo_lock); DECLARE_COMPLETION(transceiver_done); DEFINE_KFIFO(transceiver_fifo, uint32_t, 32); DEFINE_KFIFO(transceiver_super_fifo, uint32_t, 32); static void transceiver_notify_func(struct sensor_comm_notify *n, void *private_data) { uint32_t wp = 0; struct transceiver_device *dev = private_data; struct data_notify *dnotify = (struct data_notify *)n->value; uint64_t start_time = 0, end_time = 0, timeout_ns = 5000000; uint64_t wait_spin_lock_end_time = 0, timesync_end_time = 0; uint64_t kfifo_end_time = 0, complete_end_time = 0; if (n->command != SENS_COMM_NOTIFY_DATA_CMD && n->command != SENS_COMM_NOTIFY_FULL_CMD) return; start_time = ktime_get_boottime_ns(); spin_lock(&transceiver_fifo_lock); wait_spin_lock_end_time = ktime_get_boottime_ns(); timesync_filter_set(&dev->filter, dnotify->scp_timestamp, dnotify->scp_archcounter); timesync_end_time = ktime_get_boottime_ns(); if (kfifo_is_full(&transceiver_fifo)) { if (kfifo_out(&transceiver_fifo, &wp, 1)) atomic_inc(&dev->normal_wp_dropped); } wp = dnotify->write_position; kfifo_in(&transceiver_fifo, &wp, 1); kfifo_end_time = ktime_get_boottime_ns(); complete(&transceiver_done); complete_end_time = ktime_get_boottime_ns(); spin_unlock(&transceiver_fifo_lock); end_time = ktime_get_boottime_ns(); if (end_time - start_time > timeout_ns && __ratelimit(&ratelimit)) printk_deferred("time monitor:%llu, %llu, %llu, %llu, %llu, %llu\n", start_time, wait_spin_lock_end_time, timesync_end_time, kfifo_end_time, complete_end_time, end_time); } static void transceiver_super_notify_func(struct sensor_comm_notify *n, void *private_data) { uint32_t wp = 0; struct transceiver_device *dev = private_data; struct data_notify *dnotify = (struct data_notify *)n->value; if (n->command != SENS_COMM_NOTIFY_SUPER_DATA_CMD && n->command != SENS_COMM_NOTIFY_SUPER_FULL_CMD) return; spin_lock(&transceiver_fifo_lock); timesync_filter_set(&dev->filter, dnotify->scp_timestamp, dnotify->scp_archcounter); if (kfifo_is_full(&transceiver_super_fifo)) { if (kfifo_out(&transceiver_super_fifo, &wp, 1)) atomic_inc(&dev->super_wp_dropped); } wp = dnotify->write_position; kfifo_in(&transceiver_super_fifo, &wp, 1); complete(&transceiver_done); spin_unlock(&transceiver_fifo_lock); } static bool transceiver_wakeup_check(uint8_t action, uint8_t sensor_type) { /* * oneshot proximity tiledetect should wakeup source when data action */ if (action == DATA_ACTION && (sensor_type == SENSOR_TYPE_PROXIMITY || sensor_type == SENSOR_TYPE_STEP_DETECTOR || sensor_type == SENSOR_TYPE_SIGNIFICANT_MOTION || sensor_type == SENSOR_TYPE_WAKE_GESTURE || sensor_type == SENSOR_TYPE_GLANCE_GESTURE || sensor_type == SENSOR_TYPE_PICK_UP_GESTURE || sensor_type == SENSOR_TYPE_STATIONARY_DETECT || sensor_type == SENSOR_TYPE_MOTION_DETECT || sensor_type == SENSOR_TYPE_IN_POCKET || sensor_type == SENSOR_TYPE_ANSWER_CALL || sensor_type == SENSOR_TYPE_FLAT)) return true; return false; } static void transceiver_copy_config(struct transceiver_config *dst, struct hf_manager_event *src, uint8_t bias_len, uint8_t cali_len, uint8_t temp_len) { if (dst->length < (bias_len + cali_len + temp_len)) { pr_err_ratelimited("can't copy config %u %u %u %u %u %u\n", src->sensor_type, src->action, dst->length, bias_len, cali_len, temp_len); return; } if (bias_len > sizeof(src->word) || cali_len > sizeof(src->word) || temp_len > sizeof(src->word)) { pr_err_ratelimited("can't copy config %u %u %u %u %u\n", src->sensor_type, src->action, bias_len, cali_len, temp_len); return; } if (src->action == BIAS_ACTION) memcpy(dst->data, src->word, bias_len); else if (src->action == CALI_ACTION) memcpy(dst->data + bias_len, src->word, cali_len); else if (src->action == TEMP_ACTION) memcpy(dst->data + bias_len + cali_len, src->word, temp_len); } static void transceiver_update_config(struct transceiver_device *dev, struct hf_manager_event *src) { struct transceiver_config *dst = NULL; mutex_lock(&dev->config_lock); dst = dev->state[src->sensor_type].config; if (!dst) { mutex_unlock(&dev->config_lock); return; } switch (src->sensor_type) { case SENSOR_TYPE_ACCELEROMETER: transceiver_copy_config(dst, src, 12, 12, 0); break; case SENSOR_TYPE_MAGNETIC_FIELD: transceiver_copy_config(dst, src, 12, 24, 0); break; case SENSOR_TYPE_GYROSCOPE: transceiver_copy_config(dst, src, 12, 12, 24); break; default: /* * NOTE: default branch only handle CALI_ACTION. * if you add new sensor type that only cali need store, you * can use default branch. * for example: * SENSOR_TYPE_LIGHT,SENSOR_TYPE_PRESSURE, * SENSOR_TYPE_PROXIMITY, SENSOR_TYPE_SAR, SENSOR_TYPE_OIS * and so on can use this branch. */ if (src->action == CALI_ACTION && dst->length <= sizeof(src->word)) transceiver_copy_config(dst, src, 0, dst->length, 0); else pr_err_ratelimited("can't update config %u %u %u\n", src->sensor_type, src->action, dst->length); break; } mutex_unlock(&dev->config_lock); } static void transceiver_report(struct transceiver_device *dev, struct hf_manager_event *event) { int ret = 0; bool need_wakeup = false; uint8_t sensor_type = 0, action = 0; struct hf_manager *manager = dev->hf_dev.manager; struct transceiver_state *state = NULL; if (!manager) return; action = event->action; sensor_type = event->sensor_type; state = &dev->state[sensor_type]; need_wakeup = transceiver_wakeup_check(action, sensor_type); if (action == BIAS_ACTION || action == CALI_ACTION || action == TEMP_ACTION) transceiver_update_config(dev, event); do { if (action != FLUSH_ACTION) { if (need_wakeup) __pm_wakeup_event(dev->wakeup_src, 250); ret = manager->report(manager, event); } else { /* * NOTE: only for flush ret = 0 we decrease * ret must reset to 0 for each loop */ ret = 0; mutex_lock(&dev->flush_lock); if (state->flush > 0) { ret = manager->report(manager, event); if (!ret) state->flush--; } mutex_unlock(&dev->flush_lock); } if (ret < 0) usleep_range(2000, 4000); } while (ret < 0); } static int transceiver_translate(struct transceiver_device *dev, struct hf_manager_event *dst, const struct share_mem_data *src) { int64_t remap_timestamp = 0; if (src->sensor_type >= SENSOR_TYPE_SENSOR_MAX || src->action >= MAX_ACTION) { pr_err_ratelimited("invalid sensor event %u %u\n", src->sensor_type, src->action); return -EINVAL; } remap_timestamp = src->timestamp + timesync_filter_get(&dev->filter); if (src->action == DATA_ACTION) { dst->timestamp = remap_timestamp; dst->sensor_type = src->sensor_type; dst->accurancy = src->accurancy; dst->action = src->action; switch (src->sensor_type) { case SENSOR_TYPE_ACCELEROMETER: case SENSOR_TYPE_MAGNETIC_FIELD: case SENSOR_TYPE_GYROSCOPE: dst->word[0] = src->value[0]; dst->word[1] = src->value[1]; dst->word[2] = src->value[2]; break; case SENSOR_TYPE_ORIENTATION: case SENSOR_TYPE_ROTATION_VECTOR: case SENSOR_TYPE_GAME_ROTATION_VECTOR: case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR: dst->word[0] = src->value[0]; dst->word[1] = src->value[1]; dst->word[2] = src->value[2]; dst->word[3] = src->value[3]; break; case SENSOR_TYPE_LINEAR_ACCELERATION: case SENSOR_TYPE_GRAVITY: dst->word[0] = src->value[0]; dst->word[1] = src->value[1]; dst->word[2] = src->value[2]; break; case SENSOR_TYPE_ACCELEROMETER_UNCALIBRATED: case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: dst->word[0] = src->value[0]; dst->word[1] = src->value[1]; dst->word[2] = src->value[2]; dst->word[3] = src->value[3]; dst->word[4] = src->value[4]; dst->word[5] = src->value[5]; break; case SENSOR_TYPE_GYRO_SECONDARY: dst->word[0] = src->value[0]; dst->word[1] = src->value[1]; dst->word[2] = src->value[2]; break; case SENSOR_TYPE_PROXIMITY: dst->word[0] = src->value[0]; //#ifdef NT_EDIT //modify to fix AERO-161 dst->word[1] = src->value[1]; //#endif /* NT_EDIT */ break; case SENSOR_TYPE_LIGHT: case SENSOR_TYPE_PRESSURE: case SENSOR_TYPE_STEP_COUNTER: dst->word[0] = src->value[0]; break; default: memcpy(dst->word, src->value, min(sizeof(dst->word), sizeof(src->value))); break; } } else if (src->action == FLUSH_ACTION) { dst->timestamp = remap_timestamp; dst->sensor_type = src->sensor_type; dst->action = src->action; } else { /* * BIAS_ACTION, CALI_ACTION, TEMP_ACTION, * TEST_ACTION and RAW_ACTION */ dst->timestamp = remap_timestamp; dst->sensor_type = src->sensor_type; dst->accurancy = src->accurancy; dst->action = src->action; memcpy(dst->word, src->value, min(sizeof(dst->word), sizeof(src->value))); } return 0; } static void transceiver_read(struct transceiver_device *dev, uint32_t write_position) { int ret = 0; int i = 0; struct share_mem *shm = &dev->shm_reader; struct share_mem_data *buffer = dev->shm_buffer; uint32_t size = sizeof(dev->shm_buffer); uint32_t item_size = sizeof(dev->shm_buffer[0]); struct hf_manager_event evt; ret = share_mem_seek(shm, write_position); if (ret < 0) { pr_err("%s seek fail %d\n", shm->name, ret); return; } while (1) { ret = share_mem_read(shm, buffer, size); if (ret < 0 || ret > size) { pr_err("%s read fail %d\n", shm->name, ret); break; } if (ret == 0) break; if (ret % item_size) { pr_err("%s times greater fail %d\n", shm->name, ret); break; } for (i = 0; i < (ret / item_size); i++) { if (transceiver_translate(dev, &evt, &buffer[i]) < 0) continue; transceiver_report(dev, &evt); } } } static void transceiver_process(struct transceiver_device *dev) { unsigned int ret = 0; uint32_t wp = 0; unsigned long flags = 0; while (1) { spin_lock_irqsave(&transceiver_fifo_lock, flags); ret = kfifo_out(&transceiver_fifo, &wp, 1); spin_unlock_irqrestore(&transceiver_fifo_lock, flags); if (!ret) break; transceiver_read(dev, wp); } } static int transceiver_translate_super(struct transceiver_device *dev, struct hf_manager_event *dst, const struct share_mem_super_data *src) { if (src->sensor_type >= SENSOR_TYPE_SENSOR_MAX || src->action >= MAX_ACTION) { pr_err_ratelimited("invalid sensor event %u %u\n", src->sensor_type, src->action); return -EINVAL; } dst->timestamp = src->timestamp + timesync_filter_get(&dev->filter); dst->sensor_type = src->sensor_type; dst->accurancy = src->accurancy; dst->action = src->action; memcpy(dst->word, src->value, min(sizeof(dst->word), sizeof(src->value))); return 0; } static void transceiver_read_super(struct transceiver_device *dev, uint32_t write_position) { int ret = 0; int i = 0; struct share_mem *shm = &dev->shm_super_reader; struct share_mem_super_data *buffer = dev->shm_super_buffer; uint32_t size = sizeof(dev->shm_super_buffer); uint32_t item_size = sizeof(dev->shm_super_buffer[0]); struct hf_manager_event evt; ret = share_mem_seek(shm, write_position); if (ret < 0) { pr_err("%s seek fail %d\n", shm->name, ret); return; } while (1) { ret = share_mem_read(shm, buffer, size); if (ret < 0 || ret > size) { pr_err("%s read fail %d\n", shm->name, ret); break; } if (ret == 0) break; if (ret % item_size) { pr_err("%s times greater fail %d\n", shm->name, ret); break; } for (i = 0; i < (ret / item_size); i++) { if (transceiver_translate_super(dev, &evt, &buffer[i]) < 0) continue; transceiver_report(dev, &evt); } } } static void transceiver_process_super(struct transceiver_device *dev) { unsigned int ret = 0; uint32_t wp = 0; unsigned long flags = 0; while (1) { spin_lock_irqsave(&transceiver_fifo_lock, flags); ret = kfifo_out(&transceiver_super_fifo, &wp, 1); spin_unlock_irqrestore(&transceiver_fifo_lock, flags); if (!ret) break; transceiver_read_super(dev, wp); } } static int transceiver_thread(void *data) { int ret = 0; struct transceiver_device *dev = data; int32_t normal_wp_dropped = 0, super_wp_dropped = 0; while (!kthread_should_stop()) { ret = wait_for_completion_interruptible(&transceiver_done); if (ret) continue; normal_wp_dropped = atomic_xchg(&dev->normal_wp_dropped, 0); super_wp_dropped = atomic_xchg(&dev->super_wp_dropped, 0); if (unlikely(normal_wp_dropped)) pr_err_ratelimited("drop normal write position:%u\n", normal_wp_dropped); if (unlikely(super_wp_dropped)) pr_err_ratelimited("drop super write position:%u\n", super_wp_dropped); transceiver_process(dev); transceiver_process_super(dev); } return 0; } static int transceiver_comm_with(int sensor_type, int cmd, void *data, uint8_t length) { int ret = 0; struct sensor_comm_ctrl *ctrl = NULL; uint32_t ctrl_size = 0; ctrl_size = ipi_comm_size(sizeof(*ctrl) + length); ctrl = kzalloc(ctrl_size, GFP_KERNEL); ctrl->sensor_type = sensor_type; ctrl->command = cmd; ctrl->length = length; if (length) memcpy(ctrl->data, data, length); ret = sensor_comm_ctrl_send(ctrl, ctrl_size); kfree(ctrl); return ret; } static int transceiver_enable(struct hf_device *hf_dev, int sensor_type, int en) { int ret = 0; struct transceiver_device *dev = hf_dev->private_data; struct transceiver_state *state = NULL; state = &dev->state[sensor_type]; mutex_lock(&dev->enable_lock); if (en) { sensor_register_freq(sensor_type); ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_ENABLE_CMD, &state->batch, sizeof(state->batch)); if (ret >= 0) state->enable = true; else sensor_deregister_freq(sensor_type); } else { ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_DISABLE_CMD, NULL, 0); state->batch.delay = S64_MAX; state->batch.latency = S64_MAX; state->enable = false; sensor_deregister_freq(sensor_type); /* * NOTE: different from nanohub architecture(change: 2464846), * disable no need send flush, due to sensorhub architecture * can send flush when sensor disabled. */ } mutex_unlock(&dev->enable_lock); return ret; } static int transceiver_batch(struct hf_device *hf_dev, int sensor_type, int64_t delay, int64_t latency) { int ret = 0; struct transceiver_device *dev = hf_dev->private_data; struct transceiver_state *state = NULL; struct sensor_comm_batch batch = {.delay = delay, .latency = latency}; state = &dev->state[sensor_type]; mutex_lock(&dev->enable_lock); if (!state->enable) { state->batch = batch; mutex_unlock(&dev->enable_lock); return ret; } ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_ENABLE_CMD, &batch, sizeof(batch)); if (ret >= 0) state->batch = batch; mutex_unlock(&dev->enable_lock); return ret; } static int transceiver_flush(struct hf_device *hf_dev, int sensor_type) { int ret = 0; struct transceiver_device *dev = hf_dev->private_data; struct transceiver_state *state = NULL; state = &dev->state[sensor_type]; mutex_lock(&dev->flush_lock); state->flush++; mutex_unlock(&dev->flush_lock); ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_FLUSH_CMD, NULL, 0); if (ret < 0) { mutex_lock(&dev->flush_lock); if (state->flush > 0) state->flush--; mutex_unlock(&dev->flush_lock); } return ret; } static int transceiver_calibration(struct hf_device *hf_dev, int sensor_type) { return transceiver_comm_with(sensor_type, SENS_COMM_CTRL_CALI_CMD, NULL, 0); } static int transceiver_config(struct hf_device *hf_dev, int sensor_type, void *data, uint8_t length) { struct transceiver_device *dev = hf_dev->private_data; struct transceiver_config *cfg = NULL; mutex_lock(&dev->config_lock); cfg = dev->state[sensor_type].config; if (!cfg) { cfg = kzalloc(sizeof(*cfg) + length, GFP_KERNEL); if (!cfg) { mutex_unlock(&dev->config_lock); return -ENOMEM; } dev->state[sensor_type].config = cfg; } else { if (cfg->length != length) { pr_err("length not equal to prev length\n"); mutex_unlock(&dev->config_lock); return -EINVAL; } } cfg->length = length; memcpy(cfg->data, data, length); mutex_unlock(&dev->config_lock); return transceiver_comm_with(sensor_type, SENS_COMM_CTRL_CONFIG_CMD, data, length); } static transceiver_selftest(struct hf_device *hf_dev, int sensor_type) { return transceiver_comm_with(sensor_type, SENS_COMM_CTRL_SELF_TEST_CMD, NULL, 0); } static int transceiver_rawdata(struct hf_device *hf_dev, int sensor_type, int en) { int ret = 0; if (en) ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_ENABLE_RAW_CMD, NULL, 0); else ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_DISABLE_RAW_CMD, NULL, 0); return ret; } static int transceiver_debug(struct hf_device *hfdev, int sensor_type, uint8_t *buffer, unsigned int len) { return debug_get_debug(sensor_type, buffer, len); } static int transceiver_custom_cmd(struct hf_device *hfdev, int sensor_type, struct custom_cmd *cust_cmd) { return custom_cmd_comm_with(sensor_type, cust_cmd); } static void transceiver_restore_sensor(struct transceiver_device *dev) { int ret = 0, index = 0, flush = 0; uint8_t sensor_type = 0; struct transceiver_state *state = NULL; /* NOTE: config restore all firstly, don't put in one for loop */ mutex_lock(&dev->config_lock); for (index = 0; index < dev->support_size; index++) { sensor_type = dev->support_list[index].sensor_type; state = &dev->state[sensor_type]; if (!state->config) continue; ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_CONFIG_CMD, state->config->data, state->config->length); if (ret < 0) pr_err("restore config %u fail %d\n", sensor_type, ret); } mutex_unlock(&dev->config_lock); /* enable restore */ mutex_lock(&dev->enable_lock); for (index = 0; index < dev->support_size; index++) { sensor_type = dev->support_list[index].sensor_type; state = &dev->state[sensor_type]; if (!state->enable) continue; ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_ENABLE_CMD, &state->batch, sizeof(state->batch)); if (ret < 0) pr_err("restore enable %u fail %d\n", sensor_type, ret); } mutex_unlock(&dev->enable_lock); /* flush restore */ mutex_lock(&dev->flush_lock); for (index = 0; index < dev->support_size; index++) { sensor_type = dev->support_list[index].sensor_type; state = &dev->state[sensor_type]; flush = state->flush; while (flush-- > 0) { ret = transceiver_comm_with(sensor_type, SENS_COMM_CTRL_FLUSH_CMD, NULL, 0); if (ret < 0) pr_err("restore flush %u remain %u fail %d\n", sensor_type, flush, ret); } } mutex_unlock(&dev->flush_lock); } static int transceiver_create_manager(struct transceiver_device *dev) { int ret = 0; struct hf_device *hf_dev = &dev->hf_dev; memset(dev->support_list, 0, sizeof(dev->support_list)); ret = sensor_list_get_list(dev->support_list, ARRAY_SIZE(dev->support_list)); if (ret < 0) return ret; dev->support_size = ret; /* refill support_list and support_size then create manager */ dev->hf_dev.support_list = dev->support_list; dev->hf_dev.support_size = dev->support_size; return hf_manager_create(hf_dev); } static void transceiver_destroy_manager(struct transceiver_device *dev) { hf_manager_destroy(dev->hf_dev.manager); } static void transceiver_sensor_bootup(struct transceiver_device *dev) { int ret = 0; ret = share_mem_config(); if (ret < 0) { pr_err("share mem config fail %d\n", ret); return; } if (likely(atomic_xchg(&dev->first_bootup, false))) { timesync_start(); ret = transceiver_create_manager(dev); if (ret < 0) { pr_err("create manager fail %d\n", ret); return; } } else { transceiver_restore_sensor(dev); } } static int transceiver_ready_notifier_call(struct notifier_block *this, unsigned long event, void *ptr) { if (event) transceiver_sensor_bootup(&transceiver_dev); return NOTIFY_DONE; } static struct notifier_block transceiver_ready_notifier = { .notifier_call = transceiver_ready_notifier_call, .priority = READY_HIGHPRI, }; static int transceiver_pm_notifier_call(struct notifier_block *notifier, unsigned long pm_event, void *unused) { switch (pm_event) { case PM_POST_SUSPEND: transceiver_comm_with(SENSOR_TYPE_INVALID, SENS_COMM_CTRL_UNMASK_NOTIFY_CMD, NULL, 0); timesync_resume(); return NOTIFY_DONE; case PM_SUSPEND_PREPARE: transceiver_comm_with(SENSOR_TYPE_INVALID, SENS_COMM_CTRL_MASK_NOTIFY_CMD, NULL, 0); timesync_suspend(); return NOTIFY_DONE; default: return NOTIFY_OK; } return NOTIFY_OK; } static struct notifier_block transceiver_pm_notifier = { .notifier_call = transceiver_pm_notifier_call, }; static int transceiver_shm_cfg(struct share_mem_config *cfg, void *private_data) { unsigned long flags = 0; struct transceiver_device *dev = private_data; spin_lock_irqsave(&transceiver_fifo_lock, flags); kfifo_reset(&transceiver_fifo); spin_unlock_irqrestore(&transceiver_fifo_lock, flags); dev->shm_reader.name = "trans_r"; dev->shm_reader.item_size = sizeof(struct share_mem_data); dev->shm_reader.buffer_full_detect = false; return share_mem_init(&dev->shm_reader, cfg); } static int transceiver_shm_super_cfg(struct share_mem_config *cfg, void *private_data) { unsigned long flags = 0; struct transceiver_device *dev = private_data; spin_lock_irqsave(&transceiver_fifo_lock, flags); kfifo_reset(&transceiver_super_fifo); spin_unlock_irqrestore(&transceiver_fifo_lock, flags); dev->shm_super_reader.name = "trans_super_r"; dev->shm_super_reader.item_size = sizeof(struct share_mem_super_data); dev->shm_super_reader.buffer_full_detect = false; return share_mem_init(&dev->shm_super_reader, cfg); } static int __init transceiver_init(void) { int ret = 0; struct transceiver_device *dev = &transceiver_dev; struct sched_param param = { .sched_priority = MAX_RT_PRIO / 2 }; mutex_init(&dev->enable_lock); mutex_init(&dev->flush_lock); mutex_init(&dev->config_lock); dev->wakeup_src = wakeup_source_register(NULL, "trans_data"); if (!dev->wakeup_src) { pr_err("trans_data wakeup source register fail\n"); return -ENOMEM; } atomic_set(&dev->first_bootup, true); atomic_set(&dev->normal_wp_dropped, 0); atomic_set(&dev->super_wp_dropped, 0); memset(&dev->hf_dev, 0, sizeof(dev->hf_dev)); dev->hf_dev.dev_name = "transceiver"; dev->hf_dev.device_poll = HF_DEVICE_IO_INTERRUPT; dev->hf_dev.device_bus = HF_DEVICE_IO_ASYNC; dev->hf_dev.support_list = dev->support_list; dev->hf_dev.support_size = dev->support_size; dev->hf_dev.enable = transceiver_enable; dev->hf_dev.batch = transceiver_batch; dev->hf_dev.flush = transceiver_flush; dev->hf_dev.calibration = transceiver_calibration; dev->hf_dev.config_cali = transceiver_config; dev->hf_dev.selftest = transceiver_selftest; dev->hf_dev.rawdata = transceiver_rawdata; dev->hf_dev.debug = transceiver_debug; dev->hf_dev.custom_cmd = transceiver_custom_cmd; dev->hf_dev.private_data = dev; ret = hf_device_register(&dev->hf_dev); if (ret < 0) { pr_err("register hf device fail %d\n", ret); return ret; } ret = sensor_comm_init(); if (ret < 0) { pr_err("sensor comm init fail %d\n", ret); goto out_device; } ret = sensor_list_init(); if (ret < 0) { pr_err("sensor list init fail %d\n", ret); goto out_sensor_comm; } ret = debug_init(); if (ret < 0) { pr_err("debug init fail %d\n", ret); goto out_sensor_list; } ret = custom_cmd_init(); if (ret < 0) { pr_err("custom command init fail %d\n", ret); goto out_debug; } dev->filter.max_diff = 10000000000LL; dev->filter.min_diff = 10000000LL; dev->filter.bufsize = 16; dev->filter.name = "transceiver"; ret = timesync_filter_init(&dev->filter); if (ret < 0) { pr_err("timesync filter init fail %d\n", ret); goto out_cust_cmd; } ret = timesync_init(); if (ret < 0) { pr_err("timesync init fail %d\n", ret); goto out_timesync_filter; } ret = register_pm_notifier(&transceiver_pm_notifier); if (ret < 0) { pr_err("register pm notifier fail %d\n", ret); goto out_timesync; } dev->task = kthread_run(transceiver_thread, dev, "transceiver"); if (IS_ERR(dev->task)) { ret = -ENOMEM; pr_err("create thread fail %d\n", ret); goto out_pm_notify; } sched_setscheduler_nocheck(dev->task, SCHED_FIFO, ¶m); /* * NOTE: handler resgiter must before host ready to avoid lost * share mem config etc. */ sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_DATA_CMD, transceiver_notify_func, dev); sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_FULL_CMD, transceiver_notify_func, dev); sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_SUPER_DATA_CMD, transceiver_super_notify_func, dev); sensor_comm_notify_handler_register(SENS_COMM_NOTIFY_SUPER_FULL_CMD, transceiver_super_notify_func, dev); share_mem_config_handler_register(SHARE_MEM_DATA_PAYLOAD_TYPE, transceiver_shm_cfg, dev); share_mem_config_handler_register(SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE, transceiver_shm_super_cfg, dev); /* * NOTE: sensor ready must before host ready to avoid lost ready notify * host ready init must at the end of function. */ sensor_ready_notifier_chain_register(&transceiver_ready_notifier); ret = host_ready_init(); if (ret < 0) { pr_err("host ready init fail %d\n", ret); goto out_ready; } return 0; out_ready: host_ready_exit(); sensor_ready_notifier_chain_unregister(&transceiver_ready_notifier); share_mem_config_handler_unregister(SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE); share_mem_config_handler_unregister(SHARE_MEM_DATA_PAYLOAD_TYPE); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_SUPER_FULL_CMD); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_SUPER_DATA_CMD); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_FULL_CMD); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_DATA_CMD); out_pm_notify: unregister_pm_notifier(&transceiver_pm_notifier); out_timesync: timesync_exit(); out_timesync_filter: timesync_filter_exit(&dev->filter); out_cust_cmd: custom_cmd_exit(); out_debug: debug_exit(); out_sensor_list: sensor_list_exit(); out_sensor_comm: sensor_comm_exit(); out_device: hf_device_unregister(&dev->hf_dev); wakeup_source_unregister(dev->wakeup_src); return ret; } static void __exit transceiver_exit(void) { struct transceiver_device *dev = &transceiver_dev; share_mem_config_handler_unregister(SHARE_MEM_SUPER_DATA_PAYLOAD_TYPE); share_mem_config_handler_unregister(SHARE_MEM_DATA_PAYLOAD_TYPE); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_SUPER_FULL_CMD); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_SUPER_DATA_CMD); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_FULL_CMD); sensor_comm_notify_handler_unregister(SENS_COMM_NOTIFY_DATA_CMD); if (!IS_ERR(dev->task)) kthread_stop(dev->task); unregister_pm_notifier(&transceiver_pm_notifier); timesync_exit(); custom_cmd_exit(); debug_exit(); sensor_list_exit(); sensor_ready_notifier_chain_unregister(&transceiver_ready_notifier); host_ready_exit(); sensor_comm_exit(); hf_device_unregister(&dev->hf_dev); wakeup_source_unregister(dev->wakeup_src); transceiver_destroy_manager(dev); } module_init(transceiver_init); module_exit(transceiver_exit); MODULE_AUTHOR("Mediatek"); MODULE_DESCRIPTION("transceiver driver"); MODULE_LICENSE("GPL");