ish: fix s/w generated interrupt request

Current s/w generated IRQ uses LAPIC's ICR but it causes pending
interrupts for other IRQs in IOAPIC and leads LVT error with
illegal vector. So instead of using ICR, we use "int" instruction.

BRANCH=none
BUG=b:129937881,b:124128140
TEST=Tested on Arcada platform

Change-Id: I49c4120e7355f9a98d20d5ed259c4fdf6bad5196
Signed-off-by: Hyungwoo Yang <hyungwoo.yang@intel.com>
Signed-off-by: Jett Rink <jettrink@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1568786
Commit-Ready: Jack Rosenthal <jrosenth@chromium.org>
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>

6 files changed, 177 insertions, 131 deletions

diff --git a/core/minute-ia/interrupts.c b/core/minute-ia/interrupts.c
index 87926a9736..a08e02de90 100644
--- a/core/minute-ia/interrupts.c
+++ b/core/minute-ia/interrupts.c
@@ -349,6 +349,11 @@ __asm__ (
 		"movl %esp, %eax\n"
 		"movl $stack_end, %esp\n"
 		"push %eax\n"
+#ifdef CONFIG_TASK_PROFILING
+		"push $" STRINGIFY(CONFIG_IRQ_COUNT) "\n"
+		"call task_start_irq_handler\n"
+		"addl $0x04, %esp\n"
+#endif
 		"call handle_lapic_lvt_error\n"
 		"pop %esp\n"
 		"movl $0x00, (0xFEE000B0)\n"	/* Set EOI for LAPIC */
@@ -369,6 +374,30 @@ void unhandled_vector(void)
 /* This needs to be moved to link_defs.h */
 extern const struct irq_data __irq_data[], __irq_data_end[];
 
+/**
+ * Called from SOFTIRQ_VECTOR when software is trigger an IRQ manually
+ *
+ * If IRQ is out of range, then no routine should be called
+ */
+void call_irq_service_routine(uint32_t irq)
+{
+	const struct irq_data *p = __irq_data;
+
+	/* If just rescheduling a task, we won't have a routine to call */
+	if (irq >= CONFIG_IRQ_COUNT)
+		return;
+
+	for (; p < __irq_data_end; p++) {
+		if (p->irq == irq) {
+			p->routine();
+			break;
+		}
+	}
+
+	if (p == __irq_data_end)
+		CPRINTS("IRQ %d routine not found!", irq);
+}
+
 void init_interrupts(void)
 {
 	unsigned entry;
@@ -378,7 +407,11 @@ void init_interrupts(void)
 
 	/* Setup gates for IRQs declared by drivers using DECLARE_IRQ */
 	for (; p < __irq_data_end; p++)
-		set_interrupt_gate(IRQ_TO_VEC(p->irq), p->routine, IDT_DESC_FLAGS);
+		set_interrupt_gate(IRQ_TO_VEC(p->irq), p->ioapic_routine,
+				   IDT_DESC_FLAGS);
+
+	/* Software generated IRQ */
+	set_interrupt_gate(SOFTIRQ_VECTOR, sw_irq_handler, IDT_DESC_FLAGS);
 
 	/* Setup gate for LAPIC_LVT_ERROR vector; clear any remnant error. */
 	REG32(LAPIC_ESR_REG) = 0;
diff --git a/core/minute-ia/irq_handler.h b/core/minute-ia/irq_handler.h
index b691dc71fb..1d44a577bd 100644
--- a/core/minute-ia/irq_handler.h
+++ b/core/minute-ia/irq_handler.h
@@ -11,35 +11,11 @@
 #include "registers.h"
 #include "task_defs.h"
 
-#ifdef CONFIG_FPU
-#define save_fpu_ctx	"movl "USE_FPU_OFFSET_STR"(%eax), %ebx\n"	\
-			"test %ebx, %ebx\n"				\
-			"jz 9f\n"					\
-			"fnsave "FPU_CTX_OFFSET_STR"(%eax)\n"		\
-			"9:\n"
-
-#define rstr_fpu_ctx	"movl "USE_FPU_OFFSET_STR"(%eax), %ebx\n"	\
-			"test %ebx, %ebx\n"				\
-			"jz 9f\n"					\
-			"frstor "FPU_CTX_OFFSET_STR"(%eax)\n"		\
-			"9:\n"
-#else
-#define save_fpu_ctx
-#define rstr_fpu_ctx
-#endif
-
-#ifdef CONFIG_TASK_PROFILING
-#define task_start_irq_handler_call(vector) \
-			"push $"#vector"\n"	\
-			"call task_start_irq_handler\n"	\
-			"addl $0x4, %esp\n"
-#else
-#define task_start_irq_handler_call(vector)
-#endif
-
+asm (".include \"core/minute-ia/irq_handler_common.S\"");
 
 struct irq_data {
 	void (*routine)(void);
+	void (*ioapic_routine)(void);
 	int irq;
 };
 
@@ -55,43 +31,28 @@ struct irq_data {
  * Note: currently we don't allow nested irq handling
  */
 #define DECLARE_IRQ(irq, routine) DECLARE_IRQ_(irq, routine, irq + 32 + 10)
-/* Each irq has a irq_data structure placed in .rodata.irqs section,
- * to be used for dynamically setting up interrupt gates */
-#define DECLARE_IRQ_(irq, routine, vector)				\
-	void __keep routine(void);					\
-	void IRQ_HANDLER(irq)(void); 					\
-	__asm__ (".section .rodata.irqs\n");				\
-	const struct irq_data __keep CONCAT4(__irq_, irq, _, routine)	\
-	__attribute__((section(".rodata.irqs")))= {IRQ_HANDLER(irq), irq};\
-	__asm__ (							\
-		".section .text._irq_"#irq"_handler\n"			\
-		"_irq_"#irq"_handler:\n"				\
-			"pusha\n"					\
-			ASM_LOCK_PREFIX "addl  $1, __in_isr\n"		\
-			"movl %esp, %eax\n"                             \
-			"movl $stack_end, %esp\n"                       \
-			"push %eax\n"                                   \
-			task_start_irq_handler_call(vector)		\
-			"call "#routine"\n"				\
-			"push $0\n"					\
-			"push $0\n"					\
-			"call switch_handler\n"				\
-			"addl $0x08, %esp\n"				\
-			"pop %esp\n"                                    \
-			"test %eax, %eax\n"				\
-			"je 1f\n"					\
-			"movl current_task, %eax\n"			\
-			save_fpu_ctx 					\
-			"movl %esp, (%eax)\n"				\
-			"movl next_task, %eax\n"			\
-			"movl %eax, current_task\n"			\
-			"movl (%eax), %esp\n"				\
-			rstr_fpu_ctx					\
-			"1:\n"						\
-			"movl $"#vector ", (0xFEC00040)\n"              \
-			"movl $0x00, (0xFEE000B0)\n" 			\
-			ASM_LOCK_PREFIX "subl  $1, __in_isr\n"		\
-			"popa\n"					\
-			"iret\n"					\
+/*
+ * Each irq has a irq_data structure placed in .rodata.irqs section,
+ * to be used for dynamically setting up interrupt gates
+ */
+#define DECLARE_IRQ_(irq, routine, vector)				       \
+	void __keep routine(void);					       \
+	void IRQ_HANDLER(irq)(void);					       \
+	__asm__ (".section .rodata.irqs\n");				       \
+	const struct irq_data __keep CONCAT4(__irq_, irq, _, routine)	       \
+	__attribute__((section(".rodata.irqs"))) = { routine,		       \
+						     IRQ_HANDLER(irq),	       \
+						     irq};		       \
+	__asm__ (							       \
+		".section .text._irq_"#irq"_handler\n"			       \
+		"_irq_"#irq"_handler:\n"				       \
+			"pusha\n"					       \
+			ASM_LOCK_PREFIX "addl  $1, __in_isr\n"		       \
+			"irq_handler_common $0 $0 $"#irq"\n"		       \
+			"movl $"#vector ", " STRINGIFY(IOAPIC_EOI_REG) "\n"    \
+			"movl $0x00, " STRINGIFY(LAPIC_EOI_REG) "\n"	       \
+			ASM_LOCK_PREFIX "subl  $1, __in_isr\n"		       \
+			"popa\n"					       \
+			"iret\n"					       \
 		);
 #endif  /* __CROS_EC_IRQ_HANDLER_H */
diff --git a/core/minute-ia/irq_handler_common.S b/core/minute-ia/irq_handler_common.S
new file mode 100644
index 0000000000..e07cf26ce1
--- /dev/null
+++ b/core/minute-ia/irq_handler_common.S
@@ -0,0 +1,68 @@
+/* Copyright 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * x86 task swtching and interrupt handling
+ */
+
+#include "config.h"
+#include "registers.h"
+#include "task_defs.h"
+
+# desched resched irq are all inputs and should be set to registers or immediate
+# values
+.macro irq_handler_common desched resched irq
+	# __schedule() copies 'resched' to %ecx and 'desched' to %edx before
+	movl %esp, %eax
+	movl $stack_end, %esp		# use system stack
+	push %eax			# push sp of preempted context
+
+	# Push resched and desched on stack to pass them as function parameters
+	# to switch_handler(desched, resched). After call, we clean up stack
+	# pointer. Note, we do this now before call_irq_service_routine has a
+	# chance to clobber these caller-saved registers.
+	push \resched
+	push \desched
+
+	push \irq
+#ifdef CONFIG_TASK_PROFILING
+	call task_start_irq_handler
+#endif
+	# Leave IRQ on stack for handler
+	call call_irq_service_routine
+	addl $0x04, %esp
+
+	# Call switch_handler(desched, resched).
+	call switch_handler		# switch task if needed
+	addl $0x08, %esp
+
+	pop %esp			# restore sp of preempted context
+
+	test %eax, %eax			# Check if task switch required
+	jz 1f
+
+	movl current_task, %eax
+
+#ifdef CONFIG_FPU
+	movl USE_FPU_OFFSET(%eax), %ecx
+	test %ecx, %ecx
+	jz 2f
+	fnsave FPU_CTX_OFFSET(%eax) # Save current FPU context(current->fp_ctx)
+	2:
+#endif
+
+	# Save SP of current task and switch to new task
+	movl %esp, (%eax)
+	movl next_task, %eax
+	movl %eax, current_task
+	movl (%eax), %esp
+
+#ifdef CONFIG_FPU
+	movl USE_FPU_OFFSET(%eax), %ecx
+	test %ecx, %ecx
+	jz 1f
+	frstor FPU_CTX_OFFSET(%eax)		# Restore next FPU context
+#endif
+
+	1:
+.endm
diff --git a/core/minute-ia/switch.S b/core/minute-ia/switch.S
index 1fdcc61bb8..c5098b9003 100644
--- a/core/minute-ia/switch.S
+++ b/core/minute-ia/switch.S
@@ -9,6 +9,8 @@
 #include "registers.h"
 #include "task_defs.h"
 
+#include "irq_handler_common.S"
+
 .text
 
 .extern current_task
@@ -17,6 +19,7 @@
 .global __task_start
 .global __switchto
 .global default_int_handler
+.global sw_irq_handler
 
 # Start the task scheduling. Start current_task (hook_task)
 # This function is not an ISR but imitates the sequence.
@@ -67,72 +70,43 @@ default_int_handler:
 	ASM_LOCK_PREFIX subl $1, __in_isr
 	popa
 	iret
+.endfunc
+
+.align 4
+.func sw_irq_handler
+sw_irq_handler:
+	pusha
+	ASM_LOCK_PREFIX addl $1, __in_isr
+
+	# Call sw irq handler with irq number(%ecx) from task_trigger_irq.
+	# Pass 0 for both desched and resched since we don't need to deschedule
+	# our current task (and idle task can always be rescheduled)
+	irq_handler_common $0 $0 %ecx
+
+	# Indicate completion of servicing the interrupt to LAPIC.
+	# No IOAPIC EOI needed as this is SW triggered.
+	movl $0x00, LAPIC_EOI_REG
 
+	# Decrement ISR counter and restore general purpose registers.
+	ASM_LOCK_PREFIX subl $1, __in_isr
+	popa
+	iret
 .endfunc
 
+
 # Switches from one task to another if ready.
 # __schedule triggers software interrupt ISH_TS_VECTOR, which is handled by
 # __switchto
 .align 4
 .func __switchto
 __switchto:
-
-	# Save current task
 	pusha
 	ASM_LOCK_PREFIX addl $1, __in_isr
 
-	movl %esp, %eax
-	movl $stack_end, %esp		# use system stack
-	push %eax			# push sp of preempted context
-
 	# __schedule() copies 'resched' to %ecx and 'desched' to %edx before
-	# triggering ISH_TS_VECTOR
-	#
-	# Push %ecx and %edx into stack to pass them as function parameters
-	# to switch_handler(desched, resched). After call, we clean up stack
-	# pointer. Note, we do this now before task_start_irq has a chance
-	# to clobber these caller-saved registers.
-	push %ecx
-	push %edx
-
-#ifdef CONFIG_TASK_PROFILING
-	push $ISH_TS_VECTOR
-	call task_start_irq_handler
-	addl $0x4, %esp
-#endif
-
-	# Stack is already set up from previous pushes
-	call switch_handler
-	addl $0x8, %esp		# Clean up stack
-	pop %esp		# restore sp of preempted context
-
-	test %eax, %eax		# Check if task switch required
-	jz 1f
-
-	movl current_task, %eax
-
-#ifdef CONFIG_FPU
-	movl USE_FPU_OFFSET(%eax), %ebx
-	test %ebx, %ebx
-	jz 2f
-	fnsave FPU_CTX_OFFSET(%eax) # Save current FPU context(current->fp_ctx)
-	2:
-#endif
-
-	# Save SP of current task and switch to new task
-	movl %esp, (%eax)
-	movl next_task, %eax
-	movl %eax, current_task
-	movl (%eax), %esp
-
-#ifdef CONFIG_FPU
-	movl USE_FPU_OFFSET(%eax), %ebx
-	test %ebx, %ebx
-	jz 1f
-	frstor FPU_CTX_OFFSET(%eax)		# Restore next FPU context
-#endif
-
-	1:
+	# triggering ISH_TS_VECTOR.
+	# Call sw_irq with an invalid IRQ so it will skip calling a routine
+	irq_handler_common %edx %ecx $CONFIG_IRQ_COUNT
 
 	# Indicate completion of servicing the interrupt to LAPIC.
 	# No IOAPIC EOI needed as this is SW triggered.
@@ -142,5 +116,4 @@ __switchto:
 	ASM_LOCK_PREFIX subl $1, __in_isr
 	popa
 	iret
-
 .endfunc
diff --git a/core/minute-ia/task.c b/core/minute-ia/task.c
index 8c3d9fd9f2..18c23f5515 100644
--- a/core/minute-ia/task.c
+++ b/core/minute-ia/task.c
@@ -261,11 +261,9 @@ uint32_t switch_handler(int desched, task_id_t resched)
 
 void __schedule(int desched, int resched)
 {
-	__asm__ __volatile__ ("int %0"
-			:
-			: "i" (ISH_TS_VECTOR),
-			  "d" (desched), "c" (resched)
-			);
+	__asm__ __volatile__("int %0"
+			     :
+			     : "i"(ISH_TS_VECTOR), "d"(desched), "c"(resched));
 }
 
 #ifdef CONFIG_TASK_PROFILING
@@ -276,15 +274,14 @@ void __keep task_start_irq_handler(void *data)
 	 * pre-empted.
 	 */
 	uint32_t t = get_time().le.lo;
-	uint32_t vector = (uint32_t)data;
-	int irq = VEC_TO_IRQ(vector);
+	int irq = (uint32_t)data;
 
 	/*
 	 * Track IRQ distribution.  No need for atomic add, because an IRQ
 	 * can't pre-empt itself. If less than 0, then the vector did not map
 	 * to an IRQ but was for a synchronous exception instead (TS_VECTOR)
 	 */
-	if (irq > 0 && irq < ARRAY_SIZE(irq_dist))
+	if (irq < CONFIG_IRQ_COUNT)
 		irq_dist[irq]++;
 	else
 		/* Track total number of service calls */
@@ -420,11 +417,20 @@ void task_clear_pending_irq(int irq)
 
 void task_trigger_irq(int irq)
 {
-	/* Writing to Local APIC Interrupt Command Register (ICR) causes an
-	 * IPI (Inter-processor interrupt) on the APIC bus. Here we direct the
-	 * IPI to originating prccessor to generate self-interrupt
+	/* ISR should not be called before the first task is scheduled */
+	if (!task_start_called())
+		return;
+
+	/* we don't allow nested interrupt */
+	if (in_interrupt_context())
+		return;
+
+	/*
+	 * "int" instruction accepts vector only as immediate value.
+	 * so here, we use one vector(SOFTIRQ_VECTOR) and pass
+	 * the address of ISR of irq in ecx register.
 	 */
-	REG32(LAPIC_ICR_REG) = LAPIC_ICR_BITS | IRQ_TO_VEC(irq);
+	__asm__ __volatile__("int %0\n" : : "i"(SOFTIRQ_VECTOR), "c"(irq));
 }
 
 void mutex_lock(struct mutex *mtx)
diff --git a/core/minute-ia/task_defs.h b/core/minute-ia/task_defs.h
index 15aca6b8b5..01632392cb 100644
--- a/core/minute-ia/task_defs.h
+++ b/core/minute-ia/task_defs.h
@@ -19,6 +19,10 @@
 
 #define USE_FPU_OFFSET_STR	STRINGIFY(USE_FPU_OFFSET)	/* "20" */
 #define FPU_CTX_OFFSET_STR	STRINGIFY(FPU_CTX_OFFSET)	/* "24" */
+
+asm (".equ USE_FPU_OFFSET, "USE_FPU_OFFSET_STR);
+asm (".equ FPU_CTX_OFFSET, "FPU_CTX_OFFSET_STR);
+
 #endif
 #endif /* CONFIG_FPU */
 
@@ -42,6 +46,7 @@ typedef union {
 
 int __task_start(int *start_called);
 void __switchto(void);
+void sw_irq_handler(void);
 
 /* Only the IF bit is set so tasks start with interrupts enabled. */
 #define INITIAL_EFLAGS		(0x200UL)