From ee3c5d4e6f7b8ef003d940a9e2c75b5068599577 Mon Sep 17 00:00:00 2001
From: NIIBE Yutaka <gniibe@fsij.org>
Date: Tue, 8 Sep 2015 17:06:53 +0900
Subject: [PATCH] prepare kkojima's patch for Cortex-A7

---
 ChangeLog |  6 +++++-
 chopstx.c | 19 ++++++++++---------
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/ChangeLog b/ChangeLog
index b820664..73651f5 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -1,6 +1,10 @@
 2015-09-08  Niibe Yutaka  <gniibe@fsij.org>
 
-	* chopstx.c (intr_lock): New variable.
+	* chopstx.c (chx_request_preemption): Add PRIO argument and check
+	the condition inside.
+	(chx_timer_expired, chx_handle_intr): Call unconditionally.
+	(intr_lock): New variable.
+
 	* chopstx.h (chx_intr): Remove member LOCK.
 
 2015-09-07  Niibe Yutaka  <gniibe@fsij.org>
diff --git a/chopstx.c b/chopstx.c
index 4776dbd..2df444a 100644
--- a/chopstx.c
+++ b/chopstx.c
@@ -124,7 +124,7 @@ static struct chx_queue q_join;
 
 
 /* Forward declaration(s). */
-static void chx_request_preemption (void);
+static void chx_request_preemption (uint16_t prio);
 static void chx_timer_dequeue (struct chx_thread *tp);
 static void chx_timer_insert (struct chx_thread *tp, uint32_t usec);
 
@@ -706,8 +706,7 @@ chx_timer_expired (void)
 	}
     }
 
-  if (running == NULL || (uint16_t)running->prio < prio)
-    chx_request_preemption ();
+  chx_request_preemption (prio);
   chx_spin_unlock (&q_timer.lock);
 }
 
@@ -765,8 +764,7 @@ chx_handle_intr (void)
       if (intr->tp != running && intr->tp->state == THREAD_WAIT_INT)
 	{
 	  chx_ready_enqueue (intr->tp);
-	  if (running == NULL || running->prio < intr->tp->prio)
-	    chx_request_preemption ();
+	  chx_request_preemption (intr->tp->prio);
 	}
     }
   chx_spin_unlock (&intr_lock);
@@ -852,10 +850,13 @@ chopstx_main_init (chopstx_prio_t prio)
 
 
 static void
-chx_request_preemption (void)
+chx_request_preemption (uint16_t prio)
 {
-  *ICSR = (1 << 28);
-  asm volatile ("" : : : "memory");
+  if (running == NULL || (uint16_t)running->prio < prio)
+    {
+      *ICSR = (1 << 28);
+      asm volatile ("" : : : "memory");
+    }
 }
 
 #define CHX_SLEEP 0
@@ -976,13 +977,13 @@ chopstx_create (uint32_t flags_and_prio,
   stack = (void *)(stack_addr + stack_size - sizeof (struct chx_thread)
 		   - sizeof (struct chx_stack_regs));
   memset (stack, 0, sizeof (struct chx_stack_regs));
+  tp = (struct chx_thread *)(stack + sizeof (struct chx_stack_regs));
   p = (struct chx_stack_regs *)stack;
   p->reg[REG_R0] = (uint32_t)arg;
   p->reg[REG_LR] = (uint32_t)chopstx_exit;
   p->reg[REG_PC] = (uint32_t)thread_entry;
   p->reg[REG_XPSR] = INITIAL_XPSR;
 
-  tp = (struct chx_thread *)(stack + sizeof (struct chx_stack_regs));
   memset (&tp->tc, 0, sizeof (tp->tc));
   tp->tc.reg[REG_SP] = (uint32_t)stack;
   tp->next = tp->prev = tp;