ANDROID: ARM: fiq_glue: Add fiq_glue (47fe2f8d) · Commits · e / devices / android_kernel_xiaomi_nabu

arch/arm/common/Kconfig

+4 −0

Original line number	Diff line number	Diff line
		@@ -15,3 +15,7 @@ config SHARP_PARAM

		config SHARP_SCOOP
		bool

		config FIQ_GLUE
		bool
		select FIQ

arch/arm/common/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -5,6 +5,7 @@

		obj-y += firmware.o

		obj-$(CONFIG_FIQ_GLUE) += fiq_glue.o fiq_glue_setup.o
		obj-$(CONFIG_SA1111) += sa1111.o
		obj-$(CONFIG_DMABOUNCE) += dmabounce.o
		obj-$(CONFIG_SHARP_LOCOMO) += locomo.o

arch/arm/common/fiq_glue.S

0 → 100644

+111 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2008 Google, Inc.
		*
		* This software is licensed under the terms of the GNU General Public
		* License version 2, as published by the Free Software Foundation, and
		* may be copied, distributed, and modified under those terms.
		*
		* This program is distributed in the hope that it will be useful,
		* but WITHOUT ANY WARRANTY; without even the implied warranty of
		* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		* GNU General Public License for more details.
		*
		*/

		#include <linux/linkage.h>
		#include <asm/assembler.h>

		.text

		.global fiq_glue_end

		/* fiq stack: r0-r15,cpsr,spsr of interrupted mode */

		ENTRY(fiq_glue)
		/* store pc, cpsr from previous mode */
		mrs r12, spsr
		sub r11, lr, #4
		subs r10, #1
		bne nested_fiq

		stmfd sp!, {r11-r12, lr}

		/* store r8-r14 from previous mode */
		sub sp, sp, #(7 * 4)
		stmia sp, {r8-r14}^
		nop

		/* store r0-r7 from previous mode */
		stmfd sp!, {r0-r7}

		/* setup func(data,regs) arguments */
		mov r0, r9
		mov r1, sp
		mov r3, r8

		mov r7, sp

		/* Get sp and lr from non-user modes */
		and r4, r12, #MODE_MASK
		cmp r4, #USR_MODE
		beq fiq_from_usr_mode

		mov r7, sp
		orr r4, r4, #(PSR_I_BIT \| PSR_F_BIT)
		msr cpsr_c, r4
		str sp, [r7, #(4 * 13)]
		str lr, [r7, #(4 * 14)]
		mrs r5, spsr
		str r5, [r7, #(4 * 17)]

		cmp r4, #(SVC_MODE \| PSR_I_BIT \| PSR_F_BIT)
		/* use fiq stack if we reenter this mode */
		subne sp, r7, #(4 * 3)

		fiq_from_usr_mode:
		msr cpsr_c, #(SVC_MODE \| PSR_I_BIT \| PSR_F_BIT)
		mov r2, sp
		sub sp, r7, #12
		stmfd sp!, {r2, ip, lr}
		/* call func(data,regs) */
		blx r3
		ldmfd sp, {r2, ip, lr}
		mov sp, r2

		/* restore/discard saved state */
		cmp r4, #USR_MODE
		beq fiq_from_usr_mode_exit

		msr cpsr_c, r4
		ldr sp, [r7, #(4 * 13)]
		ldr lr, [r7, #(4 * 14)]
		msr spsr_cxsf, r5

		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}
		exit_fiq:
		msr spsr_cxsf, r12
		add r10, #1
		movs pc, r11

		nested_fiq:
		orr r12, r12, #(PSR_F_BIT)
		b exit_fiq

		fiq_glue_end:

		ENTRY(fiq_glue_setup) /* func, data, sp */
		mrs r3, cpsr
		msr cpsr_c, #(FIQ_MODE \| PSR_I_BIT \| PSR_F_BIT)
		movs r8, r0
		mov r9, r1
		mov sp, r2
		moveq r10, #0
		movne r10, #1
		msr cpsr_c, r3
		bx lr

arch/arm/common/fiq_glue_setup.c

0 → 100644

+100 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2010 Google, Inc.
		*
		* This software is licensed under the terms of the GNU General Public
		* License version 2, as published by the Free Software Foundation, and
		* may be copied, distributed, and modified under those terms.
		*
		* This program is distributed in the hope that it will be useful,
		* but WITHOUT ANY WARRANTY; without even the implied warranty of
		* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		* GNU General Public License for more details.
		*/

		#include <linux/kernel.h>
		#include <linux/percpu.h>
		#include <linux/slab.h>
		#include <asm/fiq.h>
		#include <asm/fiq_glue.h>

		extern unsigned char fiq_glue, fiq_glue_end;
		extern void fiq_glue_setup(void func, void data, void *sp);

		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 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);
		}

		int fiq_glue_register_handler(struct fiq_glue_handler *handler)
		{
		int ret;
		int cpu;

		if (!handler \|\| !handler->fiq)
		return -EINVAL;

		mutex_lock(&fiq_glue_lock);
		if (fiq_stack) {
		ret = -EBUSY;
		goto err_busy;
		}

		for_each_possible_cpu(cpu) {
		void *stack;
		stack = (void *)__get_free_pages(GFP_KERNEL, THREAD_SIZE_ORDER);
		if (WARN_ON(!stack)) {
		ret = -ENOMEM;
		goto err_alloc_fiq_stack;
		}
		per_cpu(fiq_stack, cpu) = stack;
		}

		ret = claim_fiq(&fiq_debbuger_fiq_handler);
		if (WARN_ON(ret))
		goto err_claim_fiq;

		current_handler = handler;
		on_each_cpu(fiq_glue_setup_helper, handler, true);
		set_fiq_handler(&fiq_glue, &fiq_glue_end - &fiq_glue);

		mutex_unlock(&fiq_glue_lock);
		return 0;

		err_claim_fiq:
		err_alloc_fiq_stack:
		for_each_possible_cpu(cpu) {
		__free_pages(per_cpu(fiq_stack, cpu), THREAD_SIZE_ORDER);
		per_cpu(fiq_stack, cpu) = NULL;
		}
		err_busy:
		mutex_unlock(&fiq_glue_lock);
		return ret;
		}

		/**
		* fiq_glue_resume - Restore fiqs after suspend or low power idle states
		*
		* This must be called before calling local_fiq_enable after returning from a
		* power state where the fiq mode registers were lost. If a driver provided
		* a resume hook when it registered the handler it will be called.
		*/

		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);
		if (current_handler->resume)
		current_handler->resume(current_handler);
		}

arch/arm/include/asm/fiq_glue.h

0 → 100644

+30 −0

Original line number	Diff line number	Diff line
		/*
		* Copyright (C) 2010 Google, Inc.
		*
		* This software is licensed under the terms of the GNU General Public
		* License version 2, as published by the Free Software Foundation, and
		* may be copied, distributed, and modified under those terms.
		*
		* This program is distributed in the hope that it will be useful,
		* but WITHOUT ANY WARRANTY; without even the implied warranty of
		* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
		* GNU General Public License for more details.
		*/

		#ifndef __ASM_FIQ_GLUE_H
		#define __ASM_FIQ_GLUE_H

		struct fiq_glue_handler {
		void (fiq)(struct fiq_glue_handler h, void regs, void svc_sp);
		void (resume)(struct fiq_glue_handler h);
		};

		int fiq_glue_register_handler(struct fiq_glue_handler *handler);

		#ifdef CONFIG_FIQ_GLUE
		void fiq_glue_resume(void);
		#else
		static inline void fiq_glue_resume(void) {}
		#endif

		#endif