427 lines
9.9 KiB
C
427 lines
9.9 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
|
/*
|
|
* Copyright (c) 2019 MediaTek Inc.
|
|
*/
|
|
|
|
#include <linux/cpuidle.h>
|
|
#include <linux/cpu_pm.h>
|
|
#include <linux/device.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/module.h>
|
|
#include <linux/of.h>
|
|
#include <linux/sched.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/spinlock.h>
|
|
#include <linux/suspend.h>
|
|
#include <linux/syscore_ops.h>
|
|
#include <asm/cpuidle.h>
|
|
#include <asm/suspend.h>
|
|
|
|
#include <mtk_lpm.h>
|
|
#include "mtk_lpm_registry.h"
|
|
|
|
#define MTK_LPM_MODULE_MAGIC 0xFCD03DC3
|
|
#define MTK_LPM_MODLE 0xD1
|
|
#define MTK_LPM_SYS_ISSUER 0xD2
|
|
|
|
struct mtk_lpm_model_info {
|
|
const char *name;
|
|
void *mod;
|
|
};
|
|
|
|
struct mtk_lpm_module_reg {
|
|
int magic;
|
|
int type;
|
|
union {
|
|
struct mtk_lpm_issuer *issuer;
|
|
struct mtk_lpm_model_info info;
|
|
} data;
|
|
};
|
|
|
|
struct mtk_lpm {
|
|
struct mtk_lpm_model suspend;
|
|
struct mtk_lpm_issuer *issuer;
|
|
};
|
|
|
|
static struct mtk_lpm mtk_lpm_system;
|
|
|
|
static DEFINE_SPINLOCK(mtk_lp_mod_locker);
|
|
static DEFINE_SPINLOCK(mtk_lpm_sys_locker);
|
|
static RAW_NOTIFIER_HEAD(mtk_lpm_notifier);
|
|
|
|
struct mtk_lpm_models {
|
|
struct mtk_lpm_model *mod[CPUIDLE_STATE_MAX];
|
|
};
|
|
static DEFINE_PER_CPU(struct mtk_lpm_models, mtk_lpm_mods);
|
|
|
|
|
|
#define mtk_lp_pm_notify(_id, _data)\
|
|
({raw_notifier_call_chain(\
|
|
&mtk_lpm_notifier, _id, _data); })
|
|
|
|
|
|
|
|
#define mtk_lp_notify_var(action, var)\
|
|
((var & MTK_LPM_NB_MASK) | action)
|
|
|
|
|
|
#define IS_MTK_LPM_MODS_VALID(idx)\
|
|
(idx < CPUIDLE_STATE_MAX)
|
|
|
|
|
|
int mtk_lpm_model_percpu_set(int cpu, struct mtk_lpm_module_reg *p)
|
|
{
|
|
struct mtk_lpm_models *ptr;
|
|
struct cpuidle_driver *drv;
|
|
struct cpuidle_device *dev;
|
|
int idx;
|
|
|
|
dev = cpuidle_get_device();
|
|
drv = cpuidle_get_cpu_driver(dev);
|
|
|
|
if (!drv) {
|
|
pr_info("[name:mtk_lpm][P] - cpuidle drv is null (%s:%d)\n",
|
|
__func__, __LINE__);
|
|
return -EINVAL;
|
|
}
|
|
|
|
for (idx = 0; idx < drv->state_count; ++idx) {
|
|
if (!IS_MTK_LPM_MODS_VALID(idx)) {
|
|
pr_info("[name:mtk_lpm][P] - mod(%s) out of index (%d) (%s:%d)\n",
|
|
p->data.info.name, idx,
|
|
__func__, __LINE__);
|
|
break;
|
|
}
|
|
if (!strcmp(p->data.info.name, drv->states[idx].name)) {
|
|
ptr = &per_cpu(mtk_lpm_mods, cpu);
|
|
ptr->mod[idx] = p->data.info.mod;
|
|
break;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int mtk_lpm_notifier_register(struct notifier_block *n)
|
|
{
|
|
return raw_notifier_chain_register(&mtk_lpm_notifier, n);
|
|
}
|
|
EXPORT_SYMBOL(mtk_lpm_notifier_register);
|
|
int mtk_lpm_notifier_unregister(struct notifier_block *n)
|
|
{
|
|
return raw_notifier_chain_unregister(&mtk_lpm_notifier, n);
|
|
}
|
|
EXPORT_SYMBOL(mtk_lpm_notifier_unregister);
|
|
|
|
void mtk_lpm_system_spin_lock(unsigned long *irqflag)
|
|
{
|
|
if (irqflag) {
|
|
unsigned long flag;
|
|
|
|
spin_lock_irqsave(&mtk_lpm_sys_locker, flag);
|
|
*irqflag = flag;
|
|
} else
|
|
spin_lock(&mtk_lpm_sys_locker);
|
|
}
|
|
void mtk_lpm_system_spin_unlock(unsigned long *irqflag)
|
|
{
|
|
if (irqflag) {
|
|
unsigned long flag = *irqflag;
|
|
|
|
spin_unlock_irqrestore(&mtk_lpm_sys_locker, flag);
|
|
} else
|
|
spin_unlock(&mtk_lpm_sys_locker);
|
|
}
|
|
|
|
int mtk_lpm_module_register_blockcall(int cpu, void *p)
|
|
{
|
|
int ret = 0;
|
|
struct mtk_lpm_module_reg *reg = (struct mtk_lpm_module_reg *)p;
|
|
|
|
if (!reg || (reg->magic != MTK_LPM_MODULE_MAGIC)) {
|
|
pr_info("[name:mtk_lpm][P] - registry(%d) fail (%s:%d)\n",
|
|
reg ? reg->type : -1, __func__, __LINE__);
|
|
return -EINVAL;
|
|
}
|
|
|
|
switch (reg->type) {
|
|
case MTK_LPM_MODLE:
|
|
if (reg->data.info.name)
|
|
mtk_lpm_model_percpu_set(cpu, reg);
|
|
break;
|
|
case MTK_LPM_SYS_ISSUER:
|
|
mtk_lpm_system.issuer =
|
|
(struct mtk_lpm_issuer *)reg->data.issuer;
|
|
break;
|
|
default:
|
|
ret = -EINVAL;
|
|
break;
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
int __mtk_lp_model_register(const char *name, struct mtk_lpm_model *lpm)
|
|
{
|
|
struct mtk_lpm_module_reg reg = {
|
|
.magic = MTK_LPM_MODULE_MAGIC,
|
|
.type = MTK_LPM_MODLE,
|
|
.data.info.name = name,
|
|
.data.info.mod = lpm,
|
|
};
|
|
|
|
return mtk_lpm_do_work(MTK_LPM_REG_PER_CPU,
|
|
mtk_lpm_module_register_blockcall,
|
|
®);
|
|
}
|
|
|
|
int mtk_lp_model_register(const char *name, struct mtk_lpm_model *lpm)
|
|
{
|
|
return __mtk_lp_model_register(name, lpm);
|
|
}
|
|
EXPORT_SYMBOL(mtk_lp_model_register);
|
|
|
|
int mtk_lp_model_unregister(const char *name)
|
|
{
|
|
return __mtk_lp_model_register(name, NULL);
|
|
}
|
|
EXPORT_SYMBOL(mtk_lp_model_unregister);
|
|
|
|
int __mtk_lp_issuer_register(struct mtk_lpm_issuer *issuer)
|
|
{
|
|
struct mtk_lpm_module_reg reg = {
|
|
.magic = MTK_LPM_MODULE_MAGIC,
|
|
.type = MTK_LPM_SYS_ISSUER,
|
|
.data.issuer = issuer,
|
|
};
|
|
|
|
return mtk_lpm_do_work(MTK_LPM_REG_ALL_ONLINE,
|
|
mtk_lpm_module_register_blockcall,
|
|
®);
|
|
}
|
|
|
|
int mtk_lp_issuer_register(struct mtk_lpm_issuer *issuer)
|
|
{
|
|
return __mtk_lp_issuer_register(issuer);
|
|
}
|
|
EXPORT_SYMBOL(mtk_lp_issuer_register);
|
|
|
|
int mtk_lp_issuer_unregister(struct mtk_lpm_issuer *issuer)
|
|
{
|
|
if (issuer != mtk_lpm_system.issuer)
|
|
return -EINVAL;
|
|
return __mtk_lp_issuer_register(NULL);
|
|
}
|
|
EXPORT_SYMBOL(mtk_lp_issuer_unregister);
|
|
|
|
int mtk_lp_cpuidle_prepare(struct cpuidle_driver *drv, int index)
|
|
{
|
|
struct mtk_lpm_models *lpmmods = NULL;
|
|
struct mtk_lpm_model *lpm = NULL;
|
|
struct mtk_lpm_nb_data nb_data;
|
|
int prompt = 0;
|
|
unsigned int model_flags = 0;
|
|
unsigned long flags;
|
|
const int cpuid = smp_processor_id();
|
|
|
|
lpmmods = this_cpu_ptr(&mtk_lpm_mods);
|
|
|
|
if (index < 0)
|
|
return -EINVAL;
|
|
|
|
if (lpmmods && lpmmods->mod[index])
|
|
lpm = lpmmods->mod[index];
|
|
|
|
model_flags = (lpm) ? lpm->flag : 0;
|
|
|
|
nb_data.cpu = cpuid;
|
|
nb_data.index = index;
|
|
nb_data.model = lpm;
|
|
nb_data.issuer = mtk_lpm_system.issuer;
|
|
|
|
spin_lock_irqsave(&mtk_lp_mod_locker, flags);
|
|
|
|
if (lpm && lpm->op.prompt)
|
|
prompt = lpm->op.prompt(cpuid, nb_data.issuer);
|
|
|
|
if (!unlikely(flags & MTK_LP_REQ_NOBROADCAST)) {
|
|
prompt = mtk_lp_notify_var(MTK_LPM_NB_AFTER_PROMPT, prompt);
|
|
mtk_lp_pm_notify(prompt, &nb_data);
|
|
}
|
|
|
|
spin_unlock_irqrestore(&mtk_lp_mod_locker, flags);
|
|
|
|
if (lpm && lpm->op.prepare_enter)
|
|
lpm->op.prepare_enter(prompt, cpuid, nb_data.issuer);
|
|
|
|
mtk_lp_pm_notify(MTK_LPM_NB_PREPARE, &nb_data);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void mtk_lp_cpuidle_resume(struct cpuidle_driver *drv, int index)
|
|
{
|
|
struct mtk_lpm_models *lpmmods = NULL;
|
|
struct mtk_lpm_model *lpm = NULL;
|
|
struct mtk_lpm_nb_data nb_data;
|
|
unsigned int model_flags = 0;
|
|
unsigned long flags;
|
|
const int cpuid = smp_processor_id();
|
|
|
|
lpmmods = this_cpu_ptr(&mtk_lpm_mods);
|
|
|
|
if (index < 0)
|
|
return;
|
|
|
|
if (lpmmods && lpmmods->mod[index])
|
|
lpm = lpmmods->mod[index];
|
|
|
|
nb_data.cpu = cpuid;
|
|
nb_data.index = index;
|
|
nb_data.model = lpm;
|
|
nb_data.issuer = mtk_lpm_system.issuer;
|
|
|
|
model_flags = (lpm) ? lpm->flag : 0;
|
|
|
|
mtk_lp_pm_notify(MTK_LPM_NB_RESUME, &nb_data);
|
|
|
|
if (lpm && lpm->op.prepare_resume)
|
|
lpm->op.prepare_resume(cpuid, nb_data.issuer);
|
|
|
|
spin_lock_irqsave(&mtk_lp_mod_locker, flags);
|
|
|
|
if (!unlikely(flags & MTK_LP_REQ_NOBROADCAST))
|
|
mtk_lp_pm_notify(MTK_LPM_NB_BEFORE_REFLECT, &nb_data);
|
|
|
|
if (lpm && lpm->op.reflect)
|
|
lpm->op.reflect(cpuid, nb_data.issuer);
|
|
|
|
spin_unlock_irqrestore(&mtk_lp_mod_locker, flags);
|
|
}
|
|
|
|
struct mtk_cpuidle_op mtk_lpm_cpu_pm_op = {
|
|
.cpuidle_prepare = mtk_lp_cpuidle_prepare,
|
|
.cpuidle_resume = mtk_lp_cpuidle_resume,
|
|
};
|
|
|
|
int mtk_lpm_suspend_enter(void)
|
|
{
|
|
int ret = 0;
|
|
unsigned long flags;
|
|
const int cpuid = smp_processor_id();
|
|
|
|
if (mtk_lpm_system.suspend.flag & MTK_LP_REQ_NOSUSPEND)
|
|
return -EACCES;
|
|
|
|
spin_lock_irqsave(&mtk_lp_mod_locker, flags);
|
|
if (mtk_lpm_system.suspend.op.prompt)
|
|
ret = mtk_lpm_system.suspend.op.prompt(cpuid,
|
|
mtk_lpm_system.issuer);
|
|
spin_unlock_irqrestore(&mtk_lp_mod_locker, flags);
|
|
return ret;
|
|
}
|
|
|
|
void mtk_lpm_suspend_resume(void)
|
|
{
|
|
unsigned long flags;
|
|
const int cpuid = smp_processor_id();
|
|
|
|
spin_lock_irqsave(&mtk_lp_mod_locker, flags);
|
|
if (mtk_lpm_system.suspend.op.reflect)
|
|
mtk_lpm_system.suspend.op.reflect(cpuid, mtk_lpm_system.issuer);
|
|
spin_unlock_irqrestore(&mtk_lp_mod_locker, flags);
|
|
}
|
|
|
|
struct syscore_ops mtk_lpm_suspend = {
|
|
.suspend = mtk_lpm_suspend_enter,
|
|
.resume = mtk_lpm_suspend_resume,
|
|
};
|
|
|
|
int mtk_lpm_suspend_type_get(void)
|
|
{
|
|
return (mtk_lpm_system.suspend.flag & MTK_LP_REQ_NOSYSCORE_CB)? 1 : 0; //1: MTK_LPM_SUSPEND_S2IDLE 0:MTK_LPM_SUSPEND_SYSTEM
|
|
}
|
|
EXPORT_SYMBOL(mtk_lpm_suspend_type_get);
|
|
int mtk_lpm_suspend_registry(const char *name, struct mtk_lpm_model *suspend)
|
|
{
|
|
unsigned long flags;
|
|
|
|
if (!suspend)
|
|
return -EINVAL;
|
|
|
|
|
|
if (mtk_lpm_system.suspend.flag &
|
|
MTK_LP_REQ_NOSYSCORE_CB) {
|
|
mtk_lp_model_register(name, suspend);
|
|
} else {
|
|
spin_lock_irqsave(&mtk_lp_mod_locker, flags);
|
|
memcpy(&mtk_lpm_system.suspend, suspend,
|
|
sizeof(struct mtk_lpm_model));
|
|
spin_unlock_irqrestore(&mtk_lp_mod_locker, flags);
|
|
}
|
|
return 0;
|
|
}
|
|
EXPORT_SYMBOL(mtk_lpm_suspend_registry);
|
|
|
|
static struct wakeup_source *mtk_lpm_lock;
|
|
|
|
static int __init mtk_lpm_init(void)
|
|
{
|
|
struct device_node *mtk_lpm;
|
|
unsigned long flags;
|
|
|
|
mtk_lpm_system.suspend.flag = MTK_LP_REQ_NOSUSPEND;
|
|
|
|
mtk_lpm = of_find_compatible_node(NULL, NULL, MTK_LPM_DTS_COMPATIBLE);
|
|
mtk_lpm_drv_cpuidle_ops_set(&mtk_lpm_cpu_pm_op);
|
|
|
|
spin_lock_irqsave(&mtk_lp_mod_locker, flags);
|
|
|
|
if (mtk_lpm) {
|
|
const char *pMethod = NULL;
|
|
|
|
of_property_read_string(mtk_lpm, "suspend-method", &pMethod);
|
|
|
|
if (pMethod) {
|
|
mtk_lpm_system.suspend.flag &=
|
|
~MTK_LP_REQ_NOSUSPEND;
|
|
|
|
if (!strcmp(pMethod, "s2idle"))
|
|
mtk_lpm_system.suspend.flag |=
|
|
MTK_LP_REQ_NOSYSCORE_CB;
|
|
else if (!strcmp(pMethod, "system"))
|
|
mtk_lpm_system.suspend.flag &=
|
|
~MTK_LP_REQ_NOSYSCORE_CB;
|
|
else
|
|
mtk_lpm_system.suspend.flag |=
|
|
MTK_LP_REQ_NOSUSPEND;
|
|
|
|
pr_info("[name:mtk_lpm][P] - suspend-method:%s (%s:%d)\n",
|
|
pMethod, __func__, __LINE__);
|
|
}
|
|
of_node_put(mtk_lpm);
|
|
}
|
|
|
|
spin_unlock_irqrestore(&mtk_lp_mod_locker, flags);
|
|
|
|
if (mtk_lpm_system.suspend.flag & MTK_LP_REQ_NOSUSPEND) {
|
|
mtk_lpm_lock = wakeup_source_register(NULL, "mtk_lpm_lock");
|
|
if (!mtk_lpm_lock) {
|
|
pr_info("[name:mtk_lpm][P] - initialize mtk_lpm_lock wakeup source fail\n");
|
|
return -1;
|
|
}
|
|
__pm_stay_awake(mtk_lpm_lock);
|
|
pr_info("[name:mtk_lpm][P] - device not support kernel suspend\n");
|
|
}
|
|
|
|
if (!(mtk_lpm_system.suspend.flag &
|
|
MTK_LP_REQ_NOSYSCORE_CB))
|
|
register_syscore_ops(&mtk_lpm_suspend);
|
|
#ifdef CONFIG_SUSPEND
|
|
else
|
|
mem_sleep_current = PM_SUSPEND_TO_IDLE;
|
|
#endif
|
|
return 0;
|
|
}
|
|
device_initcall(mtk_lpm_init);
|
|
|