Implement gpio setting

1 files changed, 16 insertions, 5 deletions

diff --git a/main/gpio-expander.cpp b/main/gpio-expander.cpp
index 9e905734..d17f9f07 100644
--- a/main/gpio-expander.cpp
+++ b/main/gpio-expander.cpp
@@ -54,8 +54,7 @@ bool GpioExpander::charge_power_ok(void) {
 }
 
 bool GpioExpander::headphone_detect(void) {
-  // Active-low.
-  return (input_b_ & (1 << 0)) == 0; // TODO.
+  return (input_b_ & (1 << 0));
 }
 
 uint8_t GpioExpander::key_states(void) {
@@ -63,15 +62,27 @@ uint8_t GpioExpander::key_states(void) {
 }
 
 void GpioExpander::set_sd_mux(SdMuxController controller) {
-  port_a_ &= (1 << 5);
+  if (controller == USB) {
+    port_a_ |= (1 << 5);
+  } else {
+    port_a_ &= ~(1 << 5);
+  }
 }
 
 void GpioExpander::set_sd_cs(bool high) {
-  port_a_ &= (1 << 6);
+  if (high) {
+    port_a_ |= (1 << 6);
+  } else {
+    port_a_ &= ~(1 << 6);
+  }
 }
 
 void GpioExpander::set_display_cs(bool high) {
-  port_a_ &= (1 << 7);
+  if (high) {
+    port_a_ |= (1 << 7);
+  } else {
+    port_a_ &= ~(1 << 7);
+  }
 }
 
 } // namespace gay_ipod