From 35ccfaff94dd538025adf89fe7b70dfa270d9001 Mon Sep 17 00:00:00 2001 From: =?utf8?q?Arve=20Hj=C3=B8nnev=C3=A5g?= Date: Mon, 24 Jun 2013 18:02:05 -0700 Subject: [PATCH] ARM: fiq_glue: Add custom fiq return handler api. MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit Change-Id: I5ff2764e85151ca0a88576542fda07c2d33dd065 Signed-off-by: Arve Hjønnevåg --- arch/arm/common/fiq_glue.S | 25 +++++++++------ arch/arm/common/fiq_glue_setup.c | 53 ++++++++++++++++++++++++++++++-- arch/arm/include/asm/fiq_glue.h | 3 ++ 3 files changed, 69 insertions(+), 12 deletions(-) diff --git a/arch/arm/common/fiq_glue.S b/arch/arm/common/fiq_glue.S index 9e3455a09f8f..24b42cec4813 100644 --- a/arch/arm/common/fiq_glue.S +++ b/arch/arm/common/fiq_glue.S @@ -22,13 +22,14 @@ /* fiq stack: r0-r15,cpsr,spsr of interrupted mode */ ENTRY(fiq_glue) - /* store pc, cpsr from previous mode */ + /* store pc, cpsr from previous mode, reserve space for spsr */ mrs r12, spsr - sub r11, lr, #4 + sub lr, lr, #4 subs r10, #1 bne nested_fiq - stmfd sp!, {r11-r12, lr} + str r12, [sp, #-8]! + str lr, [sp, #-4]! /* store r8-r14 from previous mode */ sub sp, sp, #(7 * 4) @@ -85,12 +86,15 @@ fiq_from_usr_mode_exit: msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) ldmfd sp!, {r0-r7} - add sp, sp, #(7 * 4) - ldmfd sp!, {r11-r12, lr} + ldr lr, [sp, #(4 * 7)] + ldr r12, [sp, #(4 * 8)] + add sp, sp, #(10 * 4) exit_fiq: msr spsr_cxsf, r12 add r10, #1 - movs pc, r11 + cmp r11, #0 + moveqs pc, lr + bx r11 /* jump to custom fiq return function */ nested_fiq: orr r12, r12, #(PSR_F_BIT) @@ -98,14 +102,17 @@ nested_fiq: fiq_glue_end: -ENTRY(fiq_glue_setup) /* func, data, sp */ - mrs r3, cpsr +ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */ + stmfd sp!, {r4} + mrs r4, cpsr msr cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT) movs r8, r0 mov r9, r1 mov sp, r2 + mov r11, r3 moveq r10, #0 movne r10, #1 - msr cpsr_c, r3 + msr cpsr_c, r4 + ldmfd sp!, {r4} bx lr diff --git a/arch/arm/common/fiq_glue_setup.c b/arch/arm/common/fiq_glue_setup.c index 4044c7db95c8..8cb1b611c6d5 100644 --- a/arch/arm/common/fiq_glue_setup.c +++ b/arch/arm/common/fiq_glue_setup.c @@ -18,20 +18,23 @@ #include extern unsigned char fiq_glue, fiq_glue_end; -extern void fiq_glue_setup(void *func, void *data, void *sp); +extern void fiq_glue_setup(void *func, void *data, void *sp, + fiq_return_handler_t fiq_return_handler); static struct fiq_handler fiq_debbuger_fiq_handler = { .name = "fiq_glue", }; DEFINE_PER_CPU(void *, fiq_stack); static struct fiq_glue_handler *current_handler; +static fiq_return_handler_t fiq_return_handler; static DEFINE_MUTEX(fiq_glue_lock); static void fiq_glue_setup_helper(void *info) { struct fiq_glue_handler *handler = info; fiq_glue_setup(handler->fiq, handler, - __get_cpu_var(fiq_stack) + THREAD_START_SP); + __get_cpu_var(fiq_stack) + THREAD_START_SP, + fiq_return_handler); } int fiq_glue_register_handler(struct fiq_glue_handler *handler) @@ -80,6 +83,49 @@ err_busy: return ret; } +static void fiq_glue_update_return_handler(void (*fiq_return)(void)) +{ + fiq_return_handler = fiq_return; + if (current_handler) + on_each_cpu(fiq_glue_setup_helper, current_handler, true); +} + +int fiq_glue_set_return_handler(void (*fiq_return)(void)) +{ + int ret; + + mutex_lock(&fiq_glue_lock); + if (fiq_return_handler) { + ret = -EBUSY; + goto err_busy; + } + fiq_glue_update_return_handler(fiq_return); + ret = 0; +err_busy: + mutex_unlock(&fiq_glue_lock); + + return ret; +} +EXPORT_SYMBOL(fiq_glue_set_return_handler); + +int fiq_glue_clear_return_handler(void (*fiq_return)(void)) +{ + int ret; + + mutex_lock(&fiq_glue_lock); + if (WARN_ON(fiq_return_handler != fiq_return)) { + ret = -EINVAL; + goto err_inval; + } + fiq_glue_update_return_handler(NULL); + ret = 0; +err_inval: + mutex_unlock(&fiq_glue_lock); + + return ret; +} +EXPORT_SYMBOL(fiq_glue_clear_return_handler); + /** * fiq_glue_resume - Restore fiqs after suspend or low power idle states * @@ -93,7 +139,8 @@ void fiq_glue_resume(void) if (!current_handler) return; fiq_glue_setup(current_handler->fiq, current_handler, - __get_cpu_var(fiq_stack) + THREAD_START_SP); + __get_cpu_var(fiq_stack) + THREAD_START_SP, + fiq_return_handler); if (current_handler->resume) current_handler->resume(current_handler); } diff --git a/arch/arm/include/asm/fiq_glue.h b/arch/arm/include/asm/fiq_glue.h index d54c29db97a8..a9e244f9f197 100644 --- a/arch/arm/include/asm/fiq_glue.h +++ b/arch/arm/include/asm/fiq_glue.h @@ -18,8 +18,11 @@ struct fiq_glue_handler { void (*fiq)(struct fiq_glue_handler *h, void *regs, void *svc_sp); void (*resume)(struct fiq_glue_handler *h); }; +typedef void (*fiq_return_handler_t)(void); int fiq_glue_register_handler(struct fiq_glue_handler *handler); +int fiq_glue_set_return_handler(fiq_return_handler_t fiq_return); +int fiq_glue_clear_return_handler(fiq_return_handler_t fiq_return); #ifdef CONFIG_FIQ_GLUE void fiq_glue_resume(void); -- 2.34.1