7 files changed, 591 insertions, 142 deletions

diff --git a/firmware/export/config.h b/firmware/export/config.h
index 46d4336e70..46c4d3dfd2 100644
--- a/firmware/export/config.h
+++ b/firmware/export/config.h
@@ -282,9 +282,13 @@
 #define HAVE_EXTENDED_MESSAGING_AND_NAME
 #endif
 
-#if (CONFIG_CODEC == SWCODEC) && !defined(SIMULATOR) && !defined(BOOTLOADER)
+#if (CONFIG_CODEC == SWCODEC) && !defined(BOOTLOADER)
+#ifndef SIMULATOR
 #define HAVE_PRIORITY_SCHEDULING
 #define HAVE_SCHEDULER_BOOSTCTRL
+#endif /* SIMULATOR */
+#define HAVE_SEMAPHORE_OBJECTS
+#define HAVE_EVENT_OBJECTS
 #endif
 
 /* define for all cpus from SH family */
@@ -363,31 +367,70 @@
 #define IRAM_LCDFRAMEBUFFER
 #endif
 
+/* Change this if you want to build a single-core firmware for a multicore
+ * target for debugging */
+#if defined(BOOTLOADER)
+#define FORCE_SINGLE_CORE
+#endif
+
+/* Core locking types - specifies type of atomic operation */
+#define CORELOCK_NONE   0
+#define SW_CORELOCK     1 /* Mutual exclusion provided by a software algorithm
+                             and not a special semaphore instruction */
+#define CORELOCK_SWAP   2 /* A swap (exchange) instruction */
+
 /* Dual core support - not yet working on the 1G/2G and 3G iPod */
 #if defined(CPU_PP)
 #define IDLE_STACK_SIZE  0x80
 #define IDLE_STACK_WORDS 0x20
 
-#if !defined(BOOTLOADER) && CONFIG_CPU != PP5002
+#if !defined(FORCE_SINGLE_CORE) && CONFIG_CPU != PP5002
+
 #define NUM_CORES 2
 #define CURRENT_CORE current_core()
-/* Hopefully at some point we will learn how to mark areas of main memory as
- * not to be cached.  Until then, use IRAM for variables shared across cores */
+/* Use IRAM for variables shared across cores - large memory buffers should
+ * use UNCACHED_ADDR(a) and be appropriately aligned and padded */
 #define NOCACHEBSS_ATTR IBSS_ATTR
 #define NOCACHEDATA_ATTR IDATA_ATTR
 
-#define IF_COP(...) __VA_ARGS__
+#define IF_COP(...)         __VA_ARGS__
+#define IF_COP_VOID(...)    __VA_ARGS__
+#define IF_COP_CORE(core)   core
+
+#if CONFIG_CPU == PP5020
+#define CONFIG_CORELOCK SW_CORELOCK /* SWP(B) is broken */
+#else
+#define CONFIG_CORELOCK CORELOCK_SWAP
+#endif
+
 #endif /* !defined(BOOTLOADER) && CONFIG_CPU != PP5002 */
+
 #endif /* CPU_PP */
 
+#ifndef CONFIG_CORELOCK
+#define CONFIG_CORELOCK CORELOCK_NONE
+#endif
+
+#if CONFIG_CORELOCK == SW_CORELOCK
+#define IF_SWCL(...) __VA_ARGS__
+#define IFN_SWCL(...)
+#else
+#define IF_SWCL(...)
+#define IFN_SWCL(...) __VA_ARGS__
+#endif /* CONFIG_CORELOCK == */
+
 #ifndef NUM_CORES
 /* Default to single core */
 #define NUM_CORES 1
 #define CURRENT_CORE CPU
 #define NOCACHEBSS_ATTR
 #define NOCACHEDATA_ATTR
+#define CONFIG_CORELOCK CORELOCK_NONE
 
 #define IF_COP(...)
+#define IF_COP_VOID(...)    void
+#define IF_COP_CORE(core)   CURRENT_CORE
+
 #endif /* NUM_CORES */
 
 #endif /* __CONFIG_H__ */
diff --git a/firmware/export/i2c-pp.h b/firmware/export/i2c-pp.h
index 3048acbaba..908db22554 100644
--- a/firmware/export/i2c-pp.h
+++ b/firmware/export/i2c-pp.h
@@ -45,6 +45,10 @@
 
 /* TODO: Fully implement i2c driver */
 
+/* To be used by drivers that need to do multiple i2c operations
+   atomically */
+extern struct spinlock i2c_spin;
+
 void i2c_init(void);
 int i2c_readbyte(unsigned int dev_addr, int addr);
 int pp_i2c_send(unsigned int addr, int data0, int data1);
diff --git a/firmware/export/kernel.h b/firmware/export/kernel.h
index 3d70e49a4c..a72e004b33 100644
--- a/firmware/export/kernel.h
+++ b/firmware/export/kernel.h
@@ -23,6 +23,8 @@
 #include <inttypes.h>
 #include "config.h"
 
+#include "thread.h"
+
 /* wrap-safe macros for tick comparison */
 #define TIME_AFTER(a,b)         ((long)(b) - (long)(a) < 0)
 #define TIME_BEFORE(a,b)        TIME_AFTER(b,a)
@@ -31,6 +33,7 @@
 
 #define MAX_NUM_TICK_TASKS 8
 
+#define MAX_NUM_QUEUES 32
 #define QUEUE_LENGTH 16 /* MUST be a power of 2 */
 #define QUEUE_LENGTH_MASK (QUEUE_LENGTH - 1)
 
@@ -72,7 +75,7 @@
 #define SYS_SCREENDUMP            MAKE_SYS_EVENT(SYS_EVENT_CLS_MISC, 0)
 #define SYS_CAR_ADAPTER_RESUME    MAKE_SYS_EVENT(SYS_EVENT_CLS_MISC, 1)
 
-struct event
+struct queue_event
 {
     long     id;
     intptr_t data;
@@ -91,20 +94,66 @@ struct queue_sender_list
 
 struct event_queue
 {
-    struct event events[QUEUE_LENGTH];
-    struct thread_entry *thread;
-    unsigned int read;
-    unsigned int write;
+    struct thread_queue queue;         /* Waiter list */
+    struct queue_event events[QUEUE_LENGTH]; /* list of events */
+    unsigned int read;                 /* head of queue */
+    unsigned int write;                /* tail of queue */
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
-    struct queue_sender_list *send;
+    struct queue_sender_list *send;    /* list of threads waiting for
+                                          reply to an event */
+#endif
+#if NUM_CORES > 1
+    struct corelock cl;                /* inter-core sync */
 #endif
 };
 
 struct mutex
 {
-    uint32_t locked;
-    struct thread_entry *thread;
+    struct thread_entry *queue;  /* Waiter list */
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct corelock cl;          /* inter-core sync */
+#endif
+    struct thread_entry *thread; /* thread that owns lock */
+    int count;                   /* lock owner recursion count */
+    unsigned char locked;        /* locked semaphore */
+};
+
+struct spinlock
+{
+#if NUM_CORES > 1
+    struct corelock cl;          /* inter-core sync */
+#endif
+    struct thread_entry *thread; /* lock owner */
+    int count;                   /* lock owner recursion count */
+    unsigned char locked;        /* is locked if nonzero */
+#if NUM_CORES > 1
+    unsigned char task_switch;   /* can task switch? */
+#endif
+};
+
+#ifdef HAVE_SEMAPHORE_OBJECTS
+struct semaphore
+{
+    struct thread_entry *queue;  /* Waiter list */
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct corelock cl;          /* inter-core sync */
+#endif
+    int count;                   /* # of waits remaining before unsignaled */
+    int max;                     /* maximum # of waits to remain signaled */
+};
+#endif
+
+#ifdef HAVE_EVENT_OBJECTS
+struct event
+{
+    struct thread_entry *queues[2]; /* waiters for each state */
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct corelock cl;             /* inter-core sync */
+#endif
+    unsigned char automatic;        /* event performs auto-reset */
+    unsigned char state;            /* state: 1 = signaled */
 };
+#endif
 
 /* global tick variable */
 #if defined(CPU_PP) && defined(BOOTLOADER)
@@ -127,6 +176,7 @@ extern void yield(void);
 extern void sleep(int ticks);
 int tick_add_task(void (*f)(void));
 int tick_remove_task(void (*f)(void));
+extern void tick_start(unsigned int interval_in_ms);
 
 struct timeout;
 
@@ -150,10 +200,17 @@ void timeout_register(struct timeout *tmo, timeout_cb_type callback,
                       int ticks, intptr_t data);
 void timeout_cancel(struct timeout *tmo);
 
+#define STATE_NONSIGNALED 0
+#define STATE_SIGNALED    1
+
+#define WAIT_TIMEDOUT     (-1)
+#define WAIT_SUCCEEDED    1
+
 extern void queue_init(struct event_queue *q, bool register_queue);
 extern void queue_delete(struct event_queue *q);
-extern void queue_wait(struct event_queue *q, struct event *ev);
-extern void queue_wait_w_tmo(struct event_queue *q, struct event *ev, int ticks);
+extern void queue_wait(struct event_queue *q, struct queue_event *ev);
+extern void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev,
+                             int ticks);
 extern void queue_post(struct event_queue *q, long id, intptr_t data);
 #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
 extern void queue_enable_queue_send(struct event_queue *q, struct queue_sender_list *send);
@@ -168,14 +225,26 @@ extern int queue_count(const struct event_queue *q);
 extern int queue_broadcast(long id, intptr_t data);
 
 extern void mutex_init(struct mutex *m);
-static inline void spinlock_init(struct mutex *m)
-{ mutex_init(m); } /* Same thing for now */
 extern void mutex_lock(struct mutex *m);
 extern void mutex_unlock(struct mutex *m);
-extern void spinlock_lock(struct mutex *m);
-extern void spinlock_unlock(struct mutex *m);
-extern void tick_start(unsigned int interval_in_ms);
-
+#define SPINLOCK_TASK_SWITCH 0x10
+#define SPINLOCK_NO_TASK_SWITCH 0x00
+extern void spinlock_init(struct spinlock *l IF_COP(, unsigned int flags));
+extern void spinlock_lock(struct spinlock *l);
+extern void spinlock_unlock(struct spinlock *l);
+extern int spinlock_lock_w_tmo(struct spinlock *l, int ticks);
+#ifdef HAVE_SEMAPHORE_OBJECTS
+extern void semaphore_init(struct semaphore *s, int max, int start);
+extern void semaphore_wait(struct semaphore *s);
+extern void semaphore_release(struct semaphore *s);
+#endif /* HAVE_SEMAPHORE_OBJECTS */
+#ifdef HAVE_EVENT_OBJECTS
+#define EVENT_AUTOMATIC 0x10
+#define EVENT_MANUAL    0x00
+extern void event_init(struct event *e, unsigned int flags);
+extern void event_wait(struct event *e, unsigned int for_state);
+extern void event_set_state(struct event *e, unsigned int state);
+#endif /* HAVE_EVENT_OBJECTS */
 #define IS_SYSEVENT(ev) ((ev & SYS_EVENT) == SYS_EVENT)
 
-#endif
+#endif /* _KERNEL_H_ */
diff --git a/firmware/export/pp5002.h b/firmware/export/pp5002.h
index b2e02f6174..021c248690 100644
--- a/firmware/export/pp5002.h
+++ b/firmware/export/pp5002.h
@@ -139,6 +139,8 @@
 #define CPU_CTL          (*(volatile unsigned char *)(0xcf004054))
 #define COP_CTL          (*(volatile unsigned char *)(0xcf004058))
 
+#define PROC_CTL(core)   ((&CPU_CTL)[(core)*4])
+
 #define PROC_SLEEP       0xca
 #define PROC_WAKE        0xce
 
diff --git a/firmware/export/pp5020.h b/firmware/export/pp5020.h
index 5654a7de63..b591bce695 100644
--- a/firmware/export/pp5020.h
+++ b/firmware/export/pp5020.h
@@ -34,11 +34,15 @@
 /* Each processor has two mailboxes it can write to and two which
    it can read from.  We define the first to be for sending messages
    and the second for replying to messages */
-#define CPU_MESSAGE      (*(volatile unsigned long *)(0x60001000))
-#define COP_MESSAGE      (*(volatile unsigned long *)(0x60001004))
-#define CPU_REPLY        (*(volatile unsigned long *)(0x60001008))
-#define COP_REPLY        (*(volatile unsigned long *)(0x6000100c))
-#define MBOX_CONTROL     (*(volatile unsigned long *)(0x60001010))
+#define CPU_MESSAGE         (*(volatile unsigned long *)(0x60001000))
+#define COP_MESSAGE         (*(volatile unsigned long *)(0x60001004))
+#define CPU_REPLY           (*(volatile unsigned long *)(0x60001008))
+#define COP_REPLY           (*(volatile unsigned long *)(0x6000100c))
+#define MBOX_CONTROL        (*(volatile unsigned long *)(0x60001010))
+
+/* Simple convenient array-like access */
+#define PROC_MESSAGE(core)  ((&CPU_MESSAGE)[core])
+#define PROC_REPLY(core)    ((&CPU_REPLY)[core])
 
 /* Interrupts */
 #define CPU_INT_STAT        (*(volatile unsigned long*)(0x60004000))
@@ -142,6 +146,7 @@
 /* Processors Control */
 #define CPU_CTL          (*(volatile unsigned long *)(0x60007000))
 #define COP_CTL          (*(volatile unsigned long *)(0x60007004))
+#define PROC_CTL(core)   ((&CPU_CTL)[core])
 
 #define PROC_SLEEP     0x80000000
 #define PROC_WAIT      0x40000000
diff --git a/firmware/export/system.h b/firmware/export/system.h
index 24e1a2d861..dc10c4545f 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -45,6 +45,10 @@ bool detect_original_firmware(void);
 #endif
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+#if NUM_CORES > 1
+extern struct spinlock boostctrl_spin;
+#endif
+void cpu_boost_init(void);
 #define FREQ cpu_frequency
 void set_cpu_frequency(long frequency);
 #ifdef CPU_BOOST_LOGGING
diff --git a/firmware/export/thread.h b/firmware/export/thread.h
index 7c683ddde5..20cde1a8e3 100644
--- a/firmware/export/thread.h
+++ b/firmware/export/thread.h
@@ -21,6 +21,7 @@
 
 #include "config.h"
 #include <inttypes.h>
+#include <stddef.h>
 #include <stdbool.h>
 
 /* Priority scheduling (when enabled with HAVE_PRIORITY_SCHEDULING) works
@@ -31,13 +32,15 @@
  * can change it own priority to REALTIME to override user interface and
  * prevent playback skipping.
  */
+#define HIGHEST_PRIORITY         1   /* The highest possible thread priority */
+#define LOWEST_PRIORITY          100 /* The lowest possible thread priority */
 #define PRIORITY_REALTIME        1
-#define PRIORITY_USER_INTERFACE  4 /* The main thread */
-#define PRIORITY_RECORDING       4 /* Recording thread */
-#define PRIORITY_PLAYBACK        4 /* or REALTIME when needed */
-#define PRIORITY_BUFFERING       4 /* Codec buffering thread */
-#define PRIORITY_SYSTEM          6 /* All other firmware threads */
-#define PRIORITY_BACKGROUND      8 /* Normal application threads */
+#define PRIORITY_USER_INTERFACE  4   /* The main thread */
+#define PRIORITY_RECORDING       4   /* Recording thread */
+#define PRIORITY_PLAYBACK        4   /* or REALTIME when needed */
+#define PRIORITY_BUFFERING       4   /* Codec buffering thread */
+#define PRIORITY_SYSTEM          6   /* All other firmware threads */
+#define PRIORITY_BACKGROUND      8   /* Normal application threads */
 
 #if CONFIG_CODEC == SWCODEC
 #define MAXTHREADS      16
@@ -47,6 +50,46 @@
 
 #define DEFAULT_STACK_SIZE 0x400 /* Bytes */
 
+/**
+ * "Busy" values that can be swapped into a variable to indicate
+ * that the variable or object pointed to is in use by another processor
+ * core. When accessed, the busy value is swapped-in while the current
+ * value is atomically returned. If the swap returns the busy value,
+ * the processor should retry the operation until some other value is
+ * returned. When modification is finished, the new value should be
+ * written which unlocks it and updates it atomically.
+ *
+ * Procedure:
+ * while ((curr_value = swap(&variable, BUSY_VALUE)) == BUSY_VALUE);
+ *
+ * Modify/examine object at mem location or variable. Create "new_value"
+ * as suitable.
+ *
+ * variable = new_value or curr_value;
+ *
+ * To check a value for busy and perform an operation if not:
+ * curr_value = swap(&variable, BUSY_VALUE);
+ *
+ * if (curr_value != BUSY_VALUE)
+ * {
+ *     Modify/examine object at mem location or variable. Create "new_value"
+ *     as suitable.
+ *     variable = new_value or curr_value;
+ * }
+ * else
+ * {
+ *     Do nothing - already busy
+ * }
+ *
+ * Only ever restore when an actual value is returned or else it could leave
+ * the variable locked permanently if another processor unlocked in the
+ * meantime. The next access attempt would deadlock for all processors since
+ * an abandoned busy status would be left behind.
+ */
+#define STATE_BUSYuptr    ((void*)UINTPTR_MAX)
+#define STATE_BUSYu8      UINT8_MAX
+#define STATE_BUSYi       INT_MIN
+
 #ifndef SIMULATOR
 /* Need to keep structures inside the header file because debug_menu
  * needs them. */
@@ -58,7 +101,7 @@ struct regs
     unsigned int a[5];   /* 28-44 - a2-a6 */
     void         *sp;    /*    48 - Stack pointer (a7) */
     void         *start; /*    52 - Thread start address, or NULL when started */
-} __attribute__((packed));
+};
 #elif CONFIG_CPU == SH7034
 struct regs
 {
@@ -66,7 +109,7 @@ struct regs
     void         *sp;    /*    28 - Stack pointer (r15) */
     void         *pr;    /*    32 - Procedure register */
     void         *start; /*    36 - Thread start address, or NULL when started */
-} __attribute__((packed));
+};
 #elif defined(CPU_ARM)
 struct regs
 {
@@ -74,7 +117,7 @@ struct regs
     void         *sp;    /*    32 - Stack pointer (r13) */
     unsigned int lr;     /*    36 - r14 (lr) */
     void         *start; /*    40 - Thread start address, or NULL when started */
-} __attribute__((packed));
+};
 #endif /* CONFIG_CPU */
 #else
 struct regs
@@ -85,58 +128,206 @@ struct regs
 };
 #endif /* !SIMULATOR */
 
-#define STATE_RUNNING             0x00000000
-#define STATE_BLOCKED             0x20000000
-#define STATE_SLEEPING            0x40000000
-#define STATE_BLOCKED_W_TMO       0x60000000
-
-#define THREAD_STATE_MASK         0x60000000
-#define STATE_ARG_MASK            0x1FFFFFFF
-
-#define GET_STATE_ARG(state)      (state & STATE_ARG_MASK)
-#define GET_STATE(state)          (state & THREAD_STATE_MASK)
-#define SET_STATE(var,state,arg)  (var = (state | ((arg) & STATE_ARG_MASK)))
-#define CLEAR_STATE_ARG(var)      (var &= ~STATE_ARG_MASK)
-
-#define STATE_BOOSTED             0x80000000
-#define STATE_IS_BOOSTED(var)     (var & STATE_BOOSTED)
-#define SET_BOOST_STATE(var)      (var |= STATE_BOOSTED)
-
-struct thread_entry {
-    struct regs context;
-    const char *name;
-    void *stack;
-    unsigned long statearg;
-    unsigned short stack_size;
-# if NUM_CORES > 1
-    unsigned char core; /* To which core threads belongs to. */
-# endif
-#ifdef HAVE_PRIORITY_SCHEDULING
-    unsigned char priority;
-    unsigned char priority_x;
-    long last_run;
+/* NOTE: The use of the word "queue" may also refer to a linked list of
+   threads being maintainted that are normally dealt with in FIFO order
+   and not nescessarily kernel event_queue */
+enum
+{
+    /* States without a timeout must be first */
+    STATE_KILLED = 0,    /* Thread is killed (default) */
+    STATE_RUNNING,       /* Thread is currently running */
+    STATE_BLOCKED,       /* Thread is indefinitely blocked on a queue */
+    /* These states involve adding the thread to the tmo list */
+    STATE_SLEEPING,      /* Thread is sleeping with a timeout */
+    STATE_BLOCKED_W_TMO, /* Thread is blocked on a queue with a timeout */
+    /* Miscellaneous states */
+    STATE_FROZEN,        /* Thread is suspended and will not run until
+                            thread_thaw is called with its ID */
+    THREAD_NUM_STATES,
+    TIMEOUT_STATE_FIRST = STATE_SLEEPING,
+#if NUM_CORES > 1
+    STATE_BUSY = STATE_BUSYu8, /* Thread slot is being examined */
 #endif
-    struct thread_entry *next, *prev;
-#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
-    intptr_t retval;
+};
+
+#if NUM_CORES > 1
+#define THREAD_DESTRUCT ((const char *)0x84905617)
 #endif
+
+/* Link information for lists thread is in */
+struct thread_entry; /* forward */
+struct thread_list
+{
+    struct thread_entry *prev; /* Previous thread in a list */
+    struct thread_entry *next; /* Next thread in a list */
 };
 
-struct core_entry {
-    struct thread_entry *running;
-    struct thread_entry *sleeping;
-    struct thread_entry *waking;
-    struct thread_entry **wakeup_list;
+/* Small objects for core-wise mutual exclusion */
+#if CONFIG_CORELOCK == SW_CORELOCK
+/* No reliable atomic instruction available - use Peterson's algorithm */
+struct corelock
+{
+    volatile unsigned char myl[NUM_CORES];
+    volatile unsigned char turn;
+} __attribute__((packed));
+
+void corelock_init(struct corelock *cl);
+void corelock_lock(struct corelock *cl);
+int corelock_try_lock(struct corelock *cl);
+void corelock_unlock(struct corelock *cl);
+#elif CONFIG_CORELOCK == CORELOCK_SWAP
+/* Use native atomic swap/exchange instruction */
+struct corelock
+{
+    unsigned char locked;
+} __attribute__((packed));
+
+#define corelock_init(cl) \
+    ({ (cl)->locked = 0; })
+#define corelock_lock(cl) \
+    ({ while (test_and_set(&(cl)->locked, 1)); })
+#define corelock_try_lock(cl) \
+    ({ test_and_set(&(cl)->locked, 1) ? 0 : 1; })
+#define corelock_unlock(cl) \
+    ({ (cl)->locked = 0; })
+#else
+/* No atomic corelock op needed or just none defined */
+#define corelock_init(cl)
+#define corelock_lock(cl)
+#define corelock_try_lock(cl)
+#define corelock_unlock(cl)
+#endif /* core locking selection */
+
+struct thread_queue
+{
+    struct thread_entry *queue; /* list of threads waiting -
+                                   _must_ be first member */
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct corelock cl;         /* lock for atomic list operations */
+#endif
+};
+
+/* Information kept in each thread slot
+ * members are arranged according to size - largest first - in order
+ * to ensure both alignment and packing at the same time.
+ */
+struct thread_entry
+{
+    struct regs context;       /* Register context at switch -
+                                  _must_ be first member */
+    void *stack;               /* Pointer to top of stack */
+    const char *name;          /* Thread name */
+    long tmo_tick;             /* Tick when thread should be woken from
+                                  timeout */
+    struct thread_list l;      /* Links for blocked/waking/running -
+                                  circular linkage in both directions */
+    struct thread_list tmo;    /* Links for timeout list -
+                                  Self-pointer-terminated in reverse direction,
+                                  NULL-terminated in forward direction */
+    struct thread_queue *bqp;  /* Pointer to list variable in kernel
+                                  object where thread is blocked - used
+                                  for implicit unblock and explicit wake */
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct thread_entry **bqnlp; /* Pointer to list variable in kernel
+                                    object where thread is blocked - non-locked
+                                    operations will be used */
+#endif
+    struct thread_entry *queue; /* List of threads waiting for thread to be
+                                  removed */
+#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
+    intptr_t retval;           /* Return value from a blocked operation */
+#endif
+#ifdef HAVE_PRIORITY_SCHEDULING
+    long last_run;             /* Last tick when started */
+#endif
+    unsigned short stack_size; /* Size of stack in bytes */
 #ifdef HAVE_PRIORITY_SCHEDULING
-    long highest_priority;
+    unsigned char priority;    /* Current priority */
+    unsigned char priority_x;  /* Inherited priority - right now just a
+                                  runtime guarantee flag */
 #endif
+    unsigned char state;       /* Thread slot state (STATE_*) */
 #if NUM_CORES > 1
-    volatile bool lock_issued;
-    volatile bool kernel_running;
+    unsigned char core;        /* The core to which thread belongs */
+#endif
+#ifdef HAVE_SCHEDULER_BOOSTCTRL
+    unsigned char boosted;     /* CPU frequency boost flag */
+#endif
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct corelock cl;        /* Corelock to lock thread slot */
+#endif
+};
+
+#if NUM_CORES > 1
+/* Operations to be performed just before stopping a thread and starting
+   a new one if specified before calling switch_thread */
+#define TBOP_UNLOCK_LIST     0x01 /* Set a pointer variable address var_ptrp */
+#if CONFIG_CORELOCK == CORELOCK_SWAP
+#define TBOP_SET_VARi        0x02 /* Set an int at address var_ip */
+#define TBOP_SET_VARu8       0x03 /* Set an unsigned char at address var_u8p */
+#define TBOP_VAR_TYPE_MASK   0x03 /* Mask for variable type*/
+#endif /* CONFIG_CORELOCK */
+#define TBOP_UNLOCK_CORELOCK 0x04
+#define TBOP_UNLOCK_THREAD   0x08 /* Unlock a thread's slot */
+#define TBOP_UNLOCK_CURRENT  0x10 /* Unlock the current thread's slot */
+#define TBOP_IRQ_LEVEL       0x20 /* Set a new irq level */
+#define TBOP_SWITCH_CORE     0x40 /* Call the core switch preparation routine */
+
+struct thread_blk_ops
+{
+    int irq_level;                    /* new IRQ level to set */
+#if CONFIG_CORELOCK != SW_CORELOCK
+    union
+    {
+        int var_iv;                   /* int variable value to set */
+        uint8_t var_u8v;              /* unsigned char valur to set */
+        struct thread_entry *list_v;  /* list pointer queue value to set */
+    };
+#endif
+    union
+    {
+#if CONFIG_CORELOCK != SW_CORELOCK
+        int *var_ip;                  /* pointer to int variable */
+        uint8_t *var_u8p;             /* pointer to unsigned char varuable */
+#endif
+        struct thread_queue *list_p;  /* pointer to list variable */
+    };
+#if CONFIG_CORELOCK == SW_CORELOCK
+    struct corelock *cl_p;            /* corelock to unlock */
+    struct thread_entry *thread;      /* thread to unlock */
+#elif CONFIG_CORELOCK == CORELOCK_SWAP
+    unsigned char state;              /* new thread state (performs unlock) */
+#endif /* SOFTWARE_CORELOCK */
+    unsigned char flags;    /* TBOP_* flags */
+};
+#endif /* NUM_CORES > 1 */
+
+/* Information kept for each core
+ * Member are arranged for the same reason as in thread_entry
+ */
+struct core_entry
+{
+    /* "Active" lists - core is constantly active on these and are never
+       locked and interrupts do not access them */
+    struct thread_entry *running;  /* threads that are running */
+    struct thread_entry *timeout;  /* threads that are on a timeout before
+                                      running again */
+    /* "Shared" lists - cores interact in a synchronized manner - access
+       is locked between cores and interrupts */
+    struct thread_queue  waking;   /* intermediate locked list that
+                                      hold threads other core should wake up
+                                      on next task switch */
+    long next_tmo_check;           /* soonest time to check tmo threads */
+#if NUM_CORES > 1
+    struct thread_blk_ops blk_ops; /* operations to perform when
+                                      blocking a thread */
+#else
+    #define STAY_IRQ_LEVEL (-1)
+    int irq_level;                 /* sets the irq level to irq_level */
+#endif /* NUM_CORES */
+#ifdef HAVE_PRIORITY_SCHEDULING
+    unsigned char highest_priority;
 #endif
-    long last_tick;
-    int switch_to_irq_level;
-    #define STAY_IRQ_LEVEL -1
 };
 
 #ifdef HAVE_PRIORITY_SCHEDULING
@@ -145,82 +336,210 @@ struct core_entry {
 #define IF_PRIO(...)
 #endif
 
-/* PortalPlayer chips have 2 cores, therefore need atomic mutexes 
- * Just use it for ARM, Coldfire and whatever else well...why not?
- */
-
 /* Macros generate better code than an inline function is this case */
-#if (defined (CPU_PP) || defined (CPU_ARM)) && CONFIG_CPU != PP5020
-#define test_and_set(x_, v_) \
-({ \
-    uint32_t old; \
-    asm volatile ( \
-        "swpb %[old], %[v], [%[x]] \r\n" \
-        : [old]"=r"(old) \
-        : [v]"r"((uint32_t)v_), [x]"r"((uint32_t *)x_) \
-    ); \
-    old; \
-    })
+#if (defined (CPU_PP) || defined (CPU_ARM))
+/* atomic */
+#ifdef SOFTWARE_CORELOCK
+#define test_and_set(a, v, cl) \
+    xchg8((a), (v), (cl))
+/* atomic */
+#define xchg8(a, v, cl) \
+({  uint32_t o;            \
+    corelock_lock(cl);     \
+    o = *(uint8_t *)(a);   \
+    *(uint8_t *)(a) = (v); \
+    corelock_unlock(cl);   \
+    o; })
+#define xchg32(a, v, cl) \
+({  uint32_t o;             \
+    corelock_lock(cl);      \
+    o = *(uint32_t *)(a);   \
+    *(uint32_t *)(a) = (v); \
+    corelock_unlock(cl);    \
+    o; })
+#define xchgptr(a, v, cl) \
+({  typeof (*(a)) o;     \
+    corelock_lock(cl);   \
+    o = *(a);            \
+    *(a) = (v);          \
+    corelock_unlock(cl); \
+    o; })
+#else
+/* atomic */
+#define test_and_set(a, v, ...) \
+    xchg8((a), (v))
+#define xchg8(a, v, ...) \
+({  uint32_t o;                \
+    asm volatile(              \
+        "swpb %0, %1, [%2]"    \
+        : "=r"(o)              \
+        : "r"(v),              \
+          "r"((uint8_t*)(a))); \
+    o; })
+/* atomic */
+#define xchg32(a, v, ...) \
+({  uint32_t o;                 \
+    asm volatile(               \
+        "swp %0, %1, [%2]"      \
+        : "=r"(o)               \
+        : "r"((uint32_t)(v)),   \
+          "r"((uint32_t*)(a))); \
+    o; })
+/* atomic */
+#define xchgptr(a, v, ...) \
+({  typeof (*(a)) o;        \
+    asm volatile(           \
+        "swp %0, %1, [%2]"  \
+        : "=r"(o)           \
+        : "r"(v), "r"(a));  \
+    o; })
+#endif /* SOFTWARE_CORELOCK */
 #elif defined (CPU_COLDFIRE)
-#define test_and_set(x_, v_) \
-({ \
-    uint8_t old; \
-    asm volatile ( \
-        "bset.l %[v], (%[x]) \r\n" \
-        "sne.b  %[old]       \r\n" \
-        : [old]"=d,d"(old) \
-        : [v]"i,d"((uint32_t)v_), [x]"a,a"((uint32_t *)x_) \
-    ); \
-    old; \
-    })
+/* atomic */
+/* one branch will be optimized away if v is a constant expression */
+#define test_and_set(a, v, ...) \
+({  uint32_t o = 0;                \
+    if (v) {                       \
+        asm volatile (             \
+            "bset.b #0, (%0)"      \
+            : : "a"((uint8_t*)(a)) \
+            : "cc");               \
+    } else {                       \
+        asm volatile (             \
+            "bclr.b #0, (%0)"      \
+            : : "a"((uint8_t*)(a)) \
+            : "cc");               \
+    }                              \
+    asm volatile ("sne.b %0"       \
+        : "+d"(o));                \
+    o; })
 #elif CONFIG_CPU == SH7034
-#define test_and_set(x_, v_) \
-({   \
-    uint32_t old; \
-    asm volatile ( \
-        "tas.b  @%[x]          \r\n" \
-        "mov    #-1, %[old]    \r\n" \
-        "negc   %[old], %[old] \r\n" \
-        : [old]"=r"(old) \
-        : [v]"M"((uint32_t)v_), /* Value of v_ must be 1 */ \
-          [x]"r"((uint8_t *)x_) \
-    ); \
-    old; \
-    })
-#else
-/* default for no asm version */
-#define test_and_set(x_, v_) \
-({ \
-    uint32_t old = *(uint32_t *)x_; \
-    *(uint32_t *)x_ = v_; \
-    old; \
-    })
-#endif
+/* atomic */
+#define test_and_set(a, v, ...) \
+({  uint32_t o;                 \
+    asm volatile (              \
+        "tas.b  @%2     \n"     \
+        "mov    #-1, %0 \n"     \
+        "negc   %0, %0  \n"     \
+        : "=r"(o)               \
+        : "M"((uint32_t)(v)),   /* Value of_v must be 1 */ \
+          "r"((uint8_t *)(a))); \
+    o; })
+#endif /* CONFIG_CPU == */
+
+/* defaults for no asm version */
+#ifndef test_and_set
+/* not atomic */
+#define test_and_set(a, v, ...) \
+({  uint32_t o = *(uint8_t *)(a); \
+    *(uint8_t *)(a) = (v);        \
+    o; })
+#endif /* test_and_set */
+#ifndef xchg8
+/* not atomic */
+#define xchg8(a, v, ...) \
+({  uint32_t o = *(uint8_t *)(a); \
+    *(uint8_t *)(a) = (v);        \
+    o; })
+#endif /* xchg8 */
+#ifndef xchg32
+/* not atomic */
+#define xchg32(a, v, ...) \
+({  uint32_t o = *(uint32_t *)(a); \
+    *(uint32_t *)(a) = (v);        \
+    o; })
+#endif /* xchg32 */
+#ifndef xchgptr
+/* not atomic */
+#define xchgptr(a, v, ...) \
+({  typeof (*(a)) o = *(a); \
+    *(a) = (v);             \
+    o; })
+#endif /* xchgptr */
 
+void core_idle(void);
+void core_wake(IF_COP_VOID(unsigned int core));
+
+#define CREATE_THREAD_FROZEN   0x00000001 /* Thread is frozen at create time */
 struct thread_entry*
     create_thread(void (*function)(void), void* stack, int stack_size,
-                  const char *name IF_PRIO(, int priority)
-                  IF_COP(, unsigned int core, bool fallback));
+                  unsigned flags, const char *name
+                  IF_PRIO(, int priority)
+                          IF_COP(, unsigned int core));
 
 #ifdef HAVE_SCHEDULER_BOOSTCTRL
 void trigger_cpu_boost(void);
 #else
 #define trigger_cpu_boost()
 #endif
-
+void thread_thaw(struct thread_entry *thread);
+void thread_wait(struct thread_entry *thread);
 void remove_thread(struct thread_entry *thread);
-void switch_thread(bool save_context, struct thread_entry **blocked_list);
+void switch_thread(struct thread_entry *old);
 void sleep_thread(int ticks);
-void block_thread(struct thread_entry **thread);
-void block_thread_w_tmo(struct thread_entry **thread, int timeout);
-void set_irq_level_and_block_thread(struct thread_entry **thread, int level);
-void set_irq_level_and_block_thread_w_tmo(struct thread_entry **list,
-                                          int timeout, int level);
-void wakeup_thread(struct thread_entry **thread);
-void wakeup_thread_irq_safe(struct thread_entry **thread);
+
+/**
+ * Setup to allow using thread queues as locked or non-locked without speed
+ * sacrifices in both core locking types.
+ *
+ * The blocking/waking function inline two different version of the real
+ * function into the stubs when a software or other separate core locking
+ * mechanism is employed.
+ *
+ * When a simple test-and-set or similar instruction is available, locking
+ * has no cost and so one version is used and the internal worker is called
+ * directly.
+ *
+ * CORELOCK_NONE is treated the same as when an atomic instruction can be
+ * used.
+ */
+
+/* Blocks the current thread on a thread queue */
+#if CONFIG_CORELOCK == SW_CORELOCK
+void block_thread(struct thread_queue *tq);
+void block_thread_no_listlock(struct thread_entry **list);
+#else
+void _block_thread(struct thread_queue *tq);
+static inline void block_thread(struct thread_queue *tq)
+    { _block_thread(tq); }
+static inline void block_thread_no_listlock(struct thread_entry **list)
+    { _block_thread((struct thread_queue *)list); }
+#endif /* CONFIG_CORELOCK */
+
+/* Blocks the current thread on a thread queue for a max amount of time
+ * There is no "_no_listlock" version because timeout blocks without sync on
+ * the blocking queues is not permitted since either core could access the
+ * list at any time to do an implicit wake. In other words, objects with
+ * timeout support require lockable queues. */
+void block_thread_w_tmo(struct thread_queue *tq, int timeout);
+
+/* Wakes up the thread at the head of the queue */
+#define THREAD_WAKEUP_NONE    ((struct thread_entry *)NULL)
+#define THREAD_WAKEUP_MISSING ((struct thread_entry *)(NULL+1))
+#if CONFIG_CORELOCK == SW_CORELOCK
+struct thread_entry * wakeup_thread(struct thread_queue *tq);
+struct thread_entry * wakeup_thread_no_listlock(struct thread_entry **list);
+#else
+struct thread_entry * _wakeup_thread(struct thread_queue *list);
+static inline struct thread_entry * wakeup_thread(struct thread_queue *tq)
+    { return _wakeup_thread(tq); }
+static inline struct thread_entry * wakeup_thread_no_listlock(struct thread_entry **list)
+    { return _wakeup_thread((struct thread_queue *)list); }
+#endif /* CONFIG_CORELOCK */
+
+/* Initialize a thread_queue object. */
+static inline void thread_queue_init(struct thread_queue *tq)
+    { tq->queue = NULL; IF_SWCL(corelock_init(&tq->cl);) }
+/* A convenience function for waking an entire queue of threads. */
+static inline void thread_queue_wake(struct thread_queue *tq)
+    { while (wakeup_thread(tq) != NULL); }
+/* The no-listlock version of thread_queue_wake() */
+static inline void thread_queue_wake_no_listlock(struct thread_entry **list)
+    { while (wakeup_thread_no_listlock(list) != NULL); }
+
 #ifdef HAVE_PRIORITY_SCHEDULING
 int thread_set_priority(struct thread_entry *thread, int priority);
-int  thread_get_priority(struct thread_entry *thread);
+int thread_get_priority(struct thread_entry *thread);
 /* Yield that guarantees thread execution once per round regardless of
    thread's scheduler priority - basically a transient realtime boost
    without altering the scheduler's thread precedence. */
@@ -228,17 +547,20 @@ void priority_yield(void);
 #else
 #define priority_yield  yield
 #endif /* HAVE_PRIORITY_SCHEDULING */
+#if NUM_CORES > 1
+unsigned int switch_core(unsigned int new_core);
+#endif
 struct thread_entry * thread_get_current(void);
 void init_threads(void);
 int thread_stack_usage(const struct thread_entry *thread);
 #if NUM_CORES > 1
 int idle_stack_usage(unsigned int core);
 #endif
-int thread_get_status(const struct thread_entry *thread);
+unsigned thread_get_status(const struct thread_entry *thread);
 void thread_get_name(char *buffer, int size,
                      struct thread_entry *thread);
 #ifdef RB_PROFILE
 void profile_thread(void);
 #endif
 
-#endif
+#endif /* THREAD_H */