6 files changed, 78 insertions, 40 deletions

diff --git a/firmware/target/arm/i2c-pp.c b/firmware/target/arm/i2c-pp.c
index 1cc25a1a10..e5813f9f9a 100644
--- a/firmware/target/arm/i2c-pp.c
+++ b/firmware/target/arm/i2c-pp.c
@@ -132,18 +132,18 @@ static int pp_i2c_send_byte(unsigned int addr, int data0)
 }
 
 /* Public functions */
-static struct mutex i2c_mutex;
+struct spinlock i2c_spin NOCACHEBSS_ATTR;
 
 int i2c_readbytes(unsigned int dev_addr, int addr, int len, unsigned char *data) {
     unsigned int temp;
     int i;
-    spinlock_lock(&i2c_mutex);
+    spinlock_lock(&i2c_spin);
     pp_i2c_send_byte(dev_addr, addr);
     for (i = 0; i < len; i++) {
         pp_i2c_read_byte(dev_addr, &temp);
         data[i] = temp;
     }
-    spinlock_unlock(&i2c_mutex);
+    spinlock_unlock(&i2c_spin);
     return i;
 }
 
@@ -151,10 +151,10 @@ int i2c_readbyte(unsigned int dev_addr, int addr)
 {
     int data;
 
-    spinlock_lock(&i2c_mutex);
+    spinlock_lock(&i2c_spin);
     pp_i2c_send_byte(dev_addr, addr);
     pp_i2c_read_byte(dev_addr, &data);
-    spinlock_unlock(&i2c_mutex);
+    spinlock_unlock(&i2c_spin);
 
     return data;
 }
@@ -167,9 +167,9 @@ int pp_i2c_send(unsigned int addr, int data0, int data1)
     data[0] = data0;
     data[1] = data1;
 
-    spinlock_lock(&i2c_mutex);
+    spinlock_lock(&i2c_spin);
     retval = pp_i2c_send_bytes(addr, 2, data);
-    spinlock_unlock(&i2c_mutex);
+    spinlock_unlock(&i2c_spin);
 
     return retval;
 }
@@ -221,7 +221,7 @@ void i2c_init(void)
 #endif
 #endif
 
-    spinlock_init(&i2c_mutex);
+    spinlock_init(&i2c_spin IF_COP(, SPINLOCK_TASK_SWITCH));
 
     i2c_readbyte(0x8, 0);
 }
diff --git a/firmware/target/arm/ipod/1g2g/adc-ipod-1g2g.c b/firmware/target/arm/ipod/1g2g/adc-ipod-1g2g.c
index 8866c3dcde..3a854afcdc 100644
--- a/firmware/target/arm/ipod/1g2g/adc-ipod-1g2g.c
+++ b/firmware/target/arm/ipod/1g2g/adc-ipod-1g2g.c
@@ -22,7 +22,7 @@
 #include "hwcompat.h"
 #include "kernel.h"
 
-static struct mutex adc_mutex NOCACHEBSS_ATTR;
+static struct spinlock adc_spin NOCACHEBSS_ATTR;
 
 /* used in the 2nd gen ADC interrupt */
 static unsigned int_data;
@@ -33,7 +33,7 @@ unsigned short adc_scan(int channel)
     unsigned short data = 0;
 
     (void)channel; /* there is only one */
-    spinlock_lock(&adc_mutex);
+    spinlock_lock(&adc_spin);
 
     if ((IPOD_HW_REVISION >> 16) == 1)
     {
@@ -69,7 +69,7 @@ unsigned short adc_scan(int channel)
 
         data = int_data & 0xff;
     }
-    spinlock_unlock(&adc_mutex);
+    spinlock_unlock(&adc_spin);
     return data;
 }
 
@@ -100,7 +100,7 @@ void ipod_2g_adc_int(void)
 
 void adc_init(void)
 {
-    spinlock_init(&adc_mutex);
+    spinlock_init(&adc_spin IF_COP(, SPINLOCK_TASK_SWITCH));
     
     GPIOB_ENABLE |= 0x1e;  /* enable B1..B4 */
 
diff --git a/firmware/target/arm/sandisk/adc-c200_e200.c b/firmware/target/arm/sandisk/adc-c200_e200.c
index 31321ece37..9dc8f3aabb 100644
--- a/firmware/target/arm/sandisk/adc-c200_e200.c
+++ b/firmware/target/arm/sandisk/adc-c200_e200.c
@@ -21,8 +21,6 @@
 #include "i2c-pp.h"
 #include "as3514.h"
 
-static struct mutex adc_mutex NOCACHEBSS_ATTR;
-
 /* Read 10-bit channel data */
 unsigned short adc_read(int channel)
 {
@@ -30,7 +28,7 @@ unsigned short adc_read(int channel)
 
     if ((unsigned)channel < NUM_ADC_CHANNELS)
     {
-        spinlock_lock(&adc_mutex);
+        spinlock_lock(&i2c_spin);
 
         /* Select channel */
         if (pp_i2c_send( AS3514_I2C_ADDR, ADC_0, (channel << 4)) >= 0)
@@ -44,7 +42,7 @@ unsigned short adc_read(int channel)
             }
         }
 
-        spinlock_unlock(&adc_mutex);
+        spinlock_unlock(&i2c_spin);
     }
     
     return data;
@@ -52,5 +50,4 @@ unsigned short adc_read(int channel)
 
 void adc_init(void)
 {
-    spinlock_init(&adc_mutex);
 }
diff --git a/firmware/target/arm/sandisk/ata-c200_e200.c b/firmware/target/arm/sandisk/ata-c200_e200.c
index 14be27e19d..8e17152e6f 100644
--- a/firmware/target/arm/sandisk/ata-c200_e200.c
+++ b/firmware/target/arm/sandisk/ata-c200_e200.c
@@ -162,7 +162,7 @@ static struct sd_card_status sd_status[NUM_VOLUMES] =
 /* Shoot for around 75% usage */
 static long sd_stack [(DEFAULT_STACK_SIZE*2 + 0x1c0)/sizeof(long)];
 static const char         sd_thread_name[] = "ata/sd";
-static struct mutex       sd_mtx;
+static struct spinlock    sd_spin NOCACHEBSS_ATTR;
 static struct event_queue sd_queue;
 
 /* Posted when card plugged status has changed */
@@ -801,7 +801,7 @@ int ata_read_sectors(IF_MV2(int drive,) unsigned long start, int incount,
     
     /* TODO: Add DMA support. */
 
-    spinlock_lock(&sd_mtx);
+    spinlock_lock(&sd_spin);
 
     ata_led(true);
 
@@ -888,7 +888,7 @@ ata_read_retry:
     while (1)
     {
         ata_led(false);
-        spinlock_unlock(&sd_mtx);
+        spinlock_unlock(&sd_spin);
 
         return ret;
 
@@ -916,7 +916,7 @@ int ata_write_sectors(IF_MV2(int drive,) unsigned long start, int count,
     const unsigned char *buf, *buf_end;
     int bank;
 
-    spinlock_lock(&sd_mtx);
+    spinlock_lock(&sd_spin);
 
     ata_led(true);
 
@@ -1016,7 +1016,7 @@ ata_write_retry:
     while (1)
     {
         ata_led(false);
-        spinlock_unlock(&sd_mtx);
+        spinlock_unlock(&sd_spin);
 
         return ret;
 
@@ -1034,7 +1034,7 @@ ata_write_error:
 static void sd_thread(void) __attribute__((noreturn));
 static void sd_thread(void)
 {
-    struct event ev;
+    struct queue_event ev;
     bool idle_notified = false;
     
     while (1)
@@ -1050,10 +1050,9 @@ static void sd_thread(void)
 
             /* Lock to keep us from messing with this variable while an init
                may be in progress */
-            spinlock_lock(&sd_mtx);
+            spinlock_lock(&sd_spin);
             card_info[1].initialized = 0;
             sd_status[1].retry = 0;
-            spinlock_unlock(&sd_mtx);
 
             /* Either unmount because the card was pulled or unmount and
                remount if already mounted since multiple messages may be
@@ -1073,6 +1072,8 @@ static void sd_thread(void)
 
             if (action != SDA_NONE)
                 queue_broadcast(SYS_FS_CHANGED, 0);
+
+            spinlock_unlock(&sd_spin);
             break;
             } /* SD_HOTSWAP */
 #endif /* HAVE_HOTSWAP */
@@ -1155,9 +1156,9 @@ int ata_init(void)
     {
         initialized = true;
 
-        spinlock_init(&sd_mtx);
+        spinlock_init(&sd_spin IF_COP(, SPINLOCK_TASK_SWITCH));
 
-        spinlock_lock(&sd_mtx);
+        spinlock_lock(&sd_spin);
 
         /* init controller */
         outl(inl(0x70000088) & ~(0x4), 0x70000088);
@@ -1181,8 +1182,8 @@ int ata_init(void)
             ret = currcard->initialized;
 
         queue_init(&sd_queue, true);
-        create_thread(sd_thread, sd_stack, sizeof(sd_stack),
-            sd_thread_name IF_PRIO(, PRIORITY_SYSTEM) IF_COP(, CPU, false));
+        create_thread(sd_thread, sd_stack, sizeof(sd_stack), 0,
+            sd_thread_name IF_PRIO(, PRIORITY_SYSTEM) IF_COP(, CPU));
 
         /* enable interupt for the mSD card */
         sleep(HZ/10);
@@ -1195,7 +1196,7 @@ int ata_init(void)
         GPIOA_INT_CLR = 0x80;
         GPIOA_INT_EN |= 0x80;
 #endif
-        spinlock_unlock(&sd_mtx);
+        spinlock_unlock(&sd_spin);
     }
 
     return ret;
diff --git a/firmware/target/arm/system-pp502x.c b/firmware/target/arm/system-pp502x.c
index 576459d6c1..d24d19f747 100644
--- a/firmware/target/arm/system-pp502x.c
+++ b/firmware/target/arm/system-pp502x.c
@@ -21,10 +21,6 @@
 #include "i2s.h"
 #include "i2c-pp.h"
 
-#if NUM_CORES > 1
-struct mutex boostctrl_mtx NOCACHEBSS_ATTR;
-#endif
-
 #ifndef BOOTLOADER
 extern void TIMER1(void);
 extern void TIMER2(void);
@@ -129,16 +125,42 @@ static void init_cache(void)
 }
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
+void scale_suspend_core(bool suspend) ICODE_ATTR;
+void scale_suspend_core(bool suspend)
+{
+    unsigned int core = CURRENT_CORE;
+    unsigned int othercore = 1 - core;
+    static unsigned long proc_bits IBSS_ATTR;
+    static int oldstatus IBSS_ATTR;
+
+    if (suspend)
+    {
+        oldstatus = set_interrupt_status(IRQ_FIQ_DISABLED, IRQ_FIQ_STATUS);
+        proc_bits = PROC_CTL(othercore) & 0xc0000000;
+        PROC_CTL(othercore) = 0x40000000; nop;
+        PROC_CTL(core) = 0x48000003; nop;
+    }
+    else
+    {
+        PROC_CTL(core) = 0x4800001f; nop;
+        if (proc_bits == 0)
+            PROC_CTL(othercore) = 0;
+        set_interrupt_status(oldstatus, IRQ_FIQ_STATUS);
+    }
+}
+
+void set_cpu_frequency(long frequency) ICODE_ATTR;
 void set_cpu_frequency(long frequency)
 #else
 static void pp_set_cpu_frequency(long frequency)
 #endif
 {
 #if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
-    /* Using mutex or spinlock isn't safe here. */
-    while (test_and_set(&boostctrl_mtx.locked, 1)) ;
+    spinlock_lock(&boostctrl_spin);
 #endif
 
+    scale_suspend_core(true);
+
     cpu_frequency = frequency;
 
     switch (frequency)
@@ -149,17 +171,20 @@ static void pp_set_cpu_frequency(long frequency)
        * have this limitation (and the post divider?) */
       case CPUFREQ_MAX:
         CLOCK_SOURCE = 0x10007772;  /* source #1: 24MHz, #2, #3, #4: PLL */
-        DEV_TIMING1  = 0x00000808;
+        DEV_TIMING1  = 0x00000303;
 #if CONFIG_CPU == PP5020
         PLL_CONTROL  = 0x8a020a03;  /* 10/3 * 24MHz */
         PLL_STATUS   = 0xd19b;      /* unlock frequencies > 66MHz */
         PLL_CONTROL  = 0x8a020a03;  /* repeat setup */
+        scale_suspend_core(false);
         udelay(500);                /* wait for relock */
 #elif (CONFIG_CPU == PP5022) || (CONFIG_CPU == PP5024)
         PLL_CONTROL  = 0x8a121403;  /* (20/3 * 24MHz) / 2 */
+        scale_suspend_core(false);
         udelay(250);
         while (!(PLL_STATUS & 0x80000000)); /* wait for relock */
 #endif
+        scale_suspend_core(true);
         break;
 
       case CPUFREQ_NORMAL:
@@ -167,18 +192,23 @@ static void pp_set_cpu_frequency(long frequency)
         DEV_TIMING1  = 0x00000303;
 #if CONFIG_CPU == PP5020
         PLL_CONTROL  = 0x8a020504;  /* 5/4 * 24MHz */
+        scale_suspend_core(false);
         udelay(500);                /* wait for relock */
 #elif (CONFIG_CPU == PP5022) || (CONFIG_CPU == PP5024)
         PLL_CONTROL  = 0x8a220501;  /* (5/1 * 24MHz) / 4 */
+        scale_suspend_core(false);
         udelay(250);
         while (!(PLL_STATUS & 0x80000000)); /* wait for relock */
 #endif
+        scale_suspend_core(true);
         break;
 
       case CPUFREQ_SLEEP:
         CLOCK_SOURCE = 0x10002202;  /* source #2: 32kHz, #1, #3, #4: 24MHz */
         PLL_CONTROL &= ~0x80000000; /* disable PLL */
+        scale_suspend_core(false);
         udelay(10000);              /* let 32kHz source stabilize? */
+        scale_suspend_core(true);
         break;
 
       default:
@@ -186,12 +216,19 @@ static void pp_set_cpu_frequency(long frequency)
         DEV_TIMING1  = 0x00000303;
         PLL_CONTROL &= ~0x80000000; /* disable PLL */
         cpu_frequency = CPUFREQ_DEFAULT;
+        PROC_CTL(CURRENT_CORE) = 0x4800001f; nop;
         break;
     }
+
+    if (frequency == CPUFREQ_MAX)
+        DEV_TIMING1 = 0x00000808;
+
     CLOCK_SOURCE = (CLOCK_SOURCE & ~0xf0000000) | 0x20000000;  /* select source #2 */
 
+    scale_suspend_core(false);
+
 #if defined(HAVE_ADJUSTABLE_CPU_FREQ) && (NUM_CORES > 1)
-    boostctrl_mtx.locked = 0;
+    spinlock_unlock(&boostctrl_spin);
 #endif
 }
 #endif /* !BOOTLOADER */
@@ -256,7 +293,7 @@ void system_init(void)
 
 #ifdef HAVE_ADJUSTABLE_CPU_FREQ
 #if NUM_CORES > 1
-        spinlock_init(&boostctrl_mtx);
+        cpu_boost_init();
 #endif
 #else
         pp_set_cpu_frequency(CPUFREQ_MAX);
diff --git a/firmware/target/arm/system-target.h b/firmware/target/arm/system-target.h
index 7a1ff4f79a..6e433be9d5 100644
--- a/firmware/target/arm/system-target.h
+++ b/firmware/target/arm/system-target.h
@@ -46,6 +46,10 @@
 #define inw(a) (*(volatile unsigned short *) (a))
 #define outw(a,b) (*(volatile unsigned short *) (b) = (a))
 
+#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && NUM_CORES > 1
+extern struct spinlock boostctrl_spin;
+#endif
+
 static inline void udelay(unsigned usecs)
 {
     unsigned stop = USEC_TIMER + usecs;
@@ -107,7 +111,6 @@ void flush_icache(void);
 
 #endif /* CPU_PP502x */
 
-
 #endif /* CPU_PP */
 
 #endif /* SYSTEM_TARGET_H */