1 files changed, 55 insertions, 5 deletions

diff --git a/firmware/target/arm/as3525/sansa-m200v2/button-m200v2.c b/firmware/target/arm/as3525/sansa-m200v2/button-m200v2.c
index cd31d06720..c5d6c4941d 100644
--- a/firmware/target/arm/as3525/sansa-m200v2/button-m200v2.c
+++ b/firmware/target/arm/as3525/sansa-m200v2/button-m200v2.c
@@ -26,18 +26,68 @@
 
 void button_init_device(void)
 {
-
+    GPIOA_DIR &= ~((1<<3) | (1<<2) | (1<<1) | (1<<0));  /* A3-A0 is input */
+    GPIOA_DIR |= ((1<<6) | (1<<5) | (1<<4)); /* A4-A6 row outputs */
 }
 
 int button_read_device(void)
 {
-    int btn = BUTTON_NONE;
+    int result = BUTTON_NONE;
+
+    /* direct GPIO connections */
+    if (GPIOA_PIN(3))
+        result |= BUTTON_MENU;
+
+    /* This is a keypad using A4-A6 as columns and A0-A2 as rows */
+    GPIOA_PIN(4) = (1<<4);
+
+    /* A4A0 is unused */
+
+    if (GPIOA_PIN(1))
+        result |= BUTTON_VOLDOWN;
+
+    if (GPIOA_PIN(2))
+        result |= BUTTON_PLAYPAUSE;
+
+    GPIOA_PIN(4) = 0x00;  
+    
+    GPIOA_PIN(5) = (1<<5);
+
+    if (GPIOA_PIN(0))
+        result |= BUTTON_LEFT;
+
+    if (GPIOA_PIN(1))
+        result |= BUTTON_SELECT;
+
+    if (GPIOA_PIN(2))
+        result |= BUTTON_RIGHT;
+
+    GPIOA_PIN(5) = 0x00;
+
+
+    GPIOA_PIN(6) = (1<<6);
+
+    if (GPIOA_PIN(0))
+        result |= BUTTON_REPEATAB;
+
+    if (GPIOA_PIN(1))
+        result |= BUTTON_VOLUP;
+
+    if (GPIOA_PIN(2))
+        result |= BUTTON_HOLD;
+    GPIOA_PIN(6) = 0x00;
 
-    return btn;
+    return result;
 }
 
 bool button_hold(void)
 {
-    /* TODO */
-    return false;
+    bool ret = false;
+    
+    GPIOA_PIN(6) = (1<<6);
+    if (GPIOA_PIN(2))
+        ret = true;
+    GPIOA_PIN(6) = 0x00; 
+    
+    return ret;
 }