1 files changed, 29 insertions, 13 deletions

diff --git a/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c b/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
index 7a6407e809..4dd295c21a 100644
--- a/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
+++ b/firmware/target/arm/s5l8700/ipodnano2g/pmu-nano2g.c
@@ -24,30 +24,30 @@
 #include "i2c-s5l8700.h"
 
 static struct mutex pmu_adc_mutex;
-int pmu_initialized;
+int pmu_initialized = 0;
 
-unsigned char pmu_read(int address)
+void pmu_read_multiple(int address, int count, unsigned char* buffer)
 {
-    unsigned char tmp;
-
-    i2c_read(0xe6, address, 1, &tmp);
-
-    return tmp;
+    i2c_read(0xe6, address, count, buffer);
 }
 
-void pmu_write(int address, unsigned char val)
+void pmu_write_multiple(int address, int count, unsigned char* buffer)
 {
-    i2c_write(0xe6, address, 1, &val);
+    i2c_write(0xe6, address, count, buffer);
 }
 
-void pmu_read_multiple(int address, int count, unsigned char* buffer)
+unsigned char pmu_read(int address)
 {
-    i2c_read(0xe6, address, count, buffer);
+    unsigned char tmp;
+
+    pmu_read_multiple(address, 1, &tmp);
+
+    return tmp;
 }
 
-void pmu_write_multiple(int address, int count, unsigned char* buffer)
+void pmu_write(int address, unsigned char val)
 {
-    i2c_write(0xe6, address, count, buffer);
+    pmu_write_multiple(address, 1, &val);
 }
 
 void pmu_init(void)
@@ -88,3 +88,19 @@ int pmu_read_battery_current(void)
     mutex_unlock(&pmu_adc_mutex);
     return milliamps;
 }
+
+void pmu_switch_power(int gate, int onoff)
+{
+    if (gate < 4)
+    {
+        unsigned char newval = pmu_read(0x3B) & ~(1 << (2 * gate));
+        if (onoff) newval |= 1 << (2 * gate);
+        pmu_write(0x3B, newval);
+    }
+    else if (gate < 7)
+    {
+        unsigned char newval = pmu_read(0x3C) & ~(1 << (2 * (gate - 4)));
+        if (onoff) newval |= 1 << (2 * (gate - 4));
+        pmu_write(0x3C, newval);
+    }
+}
\ No newline at end of file