From 52838e1c2ab41cd08310e47b01c409f184b2f0c0 Mon Sep 17 00:00:00 2001
From: Riccardo <riccardo.biasini@gmail.com>
Date: Wed, 26 Jun 2019 14:47:06 -0700
Subject: [PATCH 1/4] WIP

---
 board/drivers/llcan.h | 5 +++--
 board/gpio.h          | 4 ++--
 board/libc.h          | 8 ++++++--
 3 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/board/drivers/llcan.h b/board/drivers/llcan.h
index baa3a880bc5d61..2666f8166b5529 100644
--- a/board/drivers/llcan.h
+++ b/board/drivers/llcan.h
@@ -31,12 +31,13 @@ bool llcan_set_speed(CAN_TypeDef *CAN, uint32_t speed, bool loopback, bool silen
 
   #define CAN_TIMEOUT 1000000
   int tmp = 0;
+  bool ret = false;
   while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++;
   if (tmp < CAN_TIMEOUT) {
-    return true;
+    ret = true;
   }
 
-  return false;
+  return ret;
 }
 
 void llcan_init(CAN_TypeDef *CAN) {
diff --git a/board/gpio.h b/board/gpio.h
index ca8a2aa6895d3d..9d53cfbaadca18 100644
--- a/board/gpio.h
+++ b/board/gpio.h
@@ -139,13 +139,13 @@ void set_can_enable(CAN_TypeDef *CAN, int enabled) {
 #endif
 
 void set_led(int led_num, int on) {
-  if (led_num == -1) return;
-
+  if (led_num != -1) {
   #ifdef PANDA
     set_gpio_output(GPIOC, led_num, !on);
   #else
     set_gpio_output(GPIOB, led_num, !on);
   #endif
+  }
 }
 
 void set_can_mode(int can, bool use_gmlan) {
diff --git a/board/libc.h b/board/libc.h
index 62d4d0bd1490c6..953bcce18d530f 100644
--- a/board/libc.h
+++ b/board/libc.h
@@ -26,10 +26,14 @@ void *memcpy(void *dest, const void *src, unsigned int n) {
 
 int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {
   unsigned int i;
+  int ret = 0;
   for (i = 0; i < num; i++) {
-    if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) return -1;
+    if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) {
+      ret = -1;
+      break;
+    }
   }
-  return 0;
+  return ret;
 }
 
 // ********************* IRQ helpers *********************

From e3c5e7da8cffdf67da02cdd9145de0f801bfad80 Mon Sep 17 00:00:00 2001
From: Riccardo <riccardo.biasini@gmail.com>
Date: Wed, 26 Jun 2019 15:52:34 -0700
Subject: [PATCH 2/4] Fix Misra 15_5 violations

---
 board/drivers/can.h       | 114 ++++++++++++++++++--------------------
 board/drivers/gmlan_alt.h |  21 +++----
 board/drivers/uart.h      |  12 ++--
 board/drivers/usb.h       |   6 +-
 board/main.c              |  12 ++--
 board/power_saving.h      |  90 +++++++++++++++---------------
 6 files changed, 130 insertions(+), 125 deletions(-)

diff --git a/board/drivers/can.h b/board/drivers/can.h
index 1dbfa278bd1758..f2472fe1504ba7 100644
--- a/board/drivers/can.h
+++ b/board/drivers/can.h
@@ -130,29 +130,24 @@ void can_set_speed(uint8_t can_number) {
   CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
   uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
 
-  while (true) {
-    if (llcan_set_speed(CAN, can_speed[bus_number], can_loopback, can_silent & (1 << can_number))) {
-      return;
-    }
-
+  if (!llcan_set_speed(CAN, can_speed[bus_number], can_loopback, can_silent & (1 << can_number))) {
     puts("CAN init FAILED!!!!!\n");
     puth(can_number); puts(" ");
     puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n");
-    return;
   }
 }
 
 void can_init(uint8_t can_number) {
-  if (can_number == 0xff) return;
+  if (can_number != 0xff) {
+    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+    set_can_enable(CAN, 1);
+    can_set_speed(can_number);
 
-  CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
-  set_can_enable(CAN, 1);
-  can_set_speed(can_number);
+    llcan_init(CAN);
 
-  llcan_init(CAN);
-
-  // in case there are queued up messages
-  process_can(can_number);
+    // in case there are queued up messages
+    process_can(can_number);
+  }
 }
 
 void can_init_all(void) {
@@ -234,57 +229,58 @@ void can_sce(CAN_TypeDef *CAN) {
 // ***************************** CAN *****************************
 
 void process_can(uint8_t can_number) {
-  if (can_number == 0xff) return;
-
-  enter_critical_section();
-
-  CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
-  uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
-
-  // check for empty mailbox
-  CAN_FIFOMailBox_TypeDef to_send;
-  if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
-    // add successfully transmitted message to my fifo
-    if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
-      can_txd_cnt += 1;
-
-      if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
-        CAN_FIFOMailBox_TypeDef to_push;
-        to_push.RIR = CAN->sTxMailBox[0].TIR;
-        to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
-        to_push.RDLR = CAN->sTxMailBox[0].TDLR;
-        to_push.RDHR = CAN->sTxMailBox[0].TDHR;
-        can_push(&can_rx_q, &to_push);
-      }
-
-      if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
-        #ifdef DEBUG
-          puts("CAN TX ERROR!\n");
-        #endif
+  if (can_number != 0xff) {
+
+    enter_critical_section();
+
+    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+    uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
+
+    // check for empty mailbox
+    CAN_FIFOMailBox_TypeDef to_send;
+    if ((CAN->TSR & CAN_TSR_TME0) == CAN_TSR_TME0) {
+      // add successfully transmitted message to my fifo
+      if ((CAN->TSR & CAN_TSR_RQCP0) == CAN_TSR_RQCP0) {
+        can_txd_cnt += 1;
+
+        if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
+          CAN_FIFOMailBox_TypeDef to_push;
+          to_push.RIR = CAN->sTxMailBox[0].TIR;
+          to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((CAN_BUS_RET_FLAG | bus_number) << 4);
+          to_push.RDLR = CAN->sTxMailBox[0].TDLR;
+          to_push.RDHR = CAN->sTxMailBox[0].TDHR;
+          can_push(&can_rx_q, &to_push);
+        }
+
+        if ((CAN->TSR & CAN_TSR_TERR0) == CAN_TSR_TERR0) {
+          #ifdef DEBUG
+            puts("CAN TX ERROR!\n");
+          #endif
+        }
+
+        if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
+          #ifdef DEBUG
+            puts("CAN TX ARBITRATION LOST!\n");
+          #endif
+        }
+
+        // clear interrupt
+        // careful, this can also be cleared by requesting a transmission
+        CAN->TSR |= CAN_TSR_RQCP0;
       }
 
-      if ((CAN->TSR & CAN_TSR_ALST0) == CAN_TSR_ALST0) {
-        #ifdef DEBUG
-          puts("CAN TX ARBITRATION LOST!\n");
-        #endif
+      if (can_pop(can_queues[bus_number], &to_send)) {
+        can_tx_cnt += 1;
+        // only send if we have received a packet
+        CAN->sTxMailBox[0].TDLR = to_send.RDLR;
+        CAN->sTxMailBox[0].TDHR = to_send.RDHR;
+        CAN->sTxMailBox[0].TDTR = to_send.RDTR;
+        CAN->sTxMailBox[0].TIR = to_send.RIR;
       }
-
-      // clear interrupt
-      // careful, this can also be cleared by requesting a transmission
-      CAN->TSR |= CAN_TSR_RQCP0;
     }
 
-    if (can_pop(can_queues[bus_number], &to_send)) {
-      can_tx_cnt += 1;
-      // only send if we have received a packet
-      CAN->sTxMailBox[0].TDLR = to_send.RDLR;
-      CAN->sTxMailBox[0].TDHR = to_send.RDHR;
-      CAN->sTxMailBox[0].TDTR = to_send.RDTR;
-      CAN->sTxMailBox[0].TIR = to_send.RIR;
-    }
+    exit_critical_section();
   }
-
-  exit_critical_section();
 }
 
 // CAN receive handlers
diff --git a/board/drivers/gmlan_alt.h b/board/drivers/gmlan_alt.h
index 3b7cb828a1c5fd..20cfe8a32e25c0 100644
--- a/board/drivers/gmlan_alt.h
+++ b/board/drivers/gmlan_alt.h
@@ -261,18 +261,19 @@ void TIM4_IRQHandler(void) {
 void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
   gmlan_alt_mode = BITBANG;
   // TODO: make failure less silent
-  if (gmlan_sendmax != -1) return;
+  if (gmlan_sendmax == -1) {
 
-  int len = get_bit_message(pkt_stuffed, to_bang);
-  gmlan_fail_count = 0;
-  gmlan_silent_count = 0;
-  gmlan_sending = 0;
-  gmlan_sendmax = len;
+    int len = get_bit_message(pkt_stuffed, to_bang);
+    gmlan_fail_count = 0;
+    gmlan_silent_count = 0;
+    gmlan_sending = 0;
+    gmlan_sendmax = len;
 
-  // setup for bitbang loop
-  set_bitbanged_gmlan(1); // recessive
-  set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
+    // setup for bitbang loop
+    set_bitbanged_gmlan(1); // recessive
+    set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
 
-  setup_timer4();
+    setup_timer4();
+  }
 }
 
diff --git a/board/drivers/uart.h b/board/drivers/uart.h
index 5df63154d3e0c5..b9dc5ad1472b8d 100644
--- a/board/drivers/uart.h
+++ b/board/drivers/uart.h
@@ -50,18 +50,20 @@ uart_ring debug_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
 
 
 uart_ring *get_ring_by_number(int a) {
+  uart_ring *ring = NULL;
   switch(a) {
     case 0:
-      return &debug_ring;
+      ring = &debug_ring;
     case 1:
-      return &esp_ring;
+      ring = &esp_ring;
     case 2:
-      return &lin1_ring;
+      ring = &lin1_ring;
     case 3:
-      return &lin2_ring;
+      ring = &lin2_ring;
     default:
-      return NULL;
+      ring = NULL;
   }
+  return ring;
 }
 
 // ***************************** serial port *****************************
diff --git a/board/drivers/usb.h b/board/drivers/usb.h
index 84be69fe673638..bc1a1bd37ff841 100644
--- a/board/drivers/usb.h
+++ b/board/drivers/usb.h
@@ -491,11 +491,13 @@ void usb_reset(void) {
 }
 
 char to_hex_char(int a) {
+  char ret;
   if (a < 10) {
-    return '0' + a;
+    ret = '0' + a;
   } else {
-    return 'a' + (a-10);
+    ret = 'a' + (a - 10);
   }
+  return ret;
 }
 
 void usb_setup(void) {
diff --git a/board/main.c b/board/main.c
index 32ea3e8a5bb527..2946dbdd5127fd 100644
--- a/board/main.c
+++ b/board/main.c
@@ -153,11 +153,13 @@ int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
 // send on serial, first byte to select the ring
 void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
   UNUSED(hardwired);
-  if (len == 0) return;
-  uart_ring *ur = get_ring_by_number(usbdata[0]);
-  if (!ur) return;
-  if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
-    for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i]));
+  if (len != 0) {
+    uart_ring *ur = get_ring_by_number(usbdata[0]);
+    if (ur != NULL) {
+      if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
+        for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i]));
+      }
+    }
   }
 }
 
diff --git a/board/power_saving.h b/board/power_saving.h
index 96d33842c38c17..e1b20768f649cd 100644
--- a/board/power_saving.h
+++ b/board/power_saving.h
@@ -4,54 +4,56 @@
 int power_save_status = POWER_SAVE_STATUS_DISABLED;
 
 void power_save_enable(void) {
-  if (power_save_status == POWER_SAVE_STATUS_ENABLED) return;
-  puts("enable power savings\n");
-
-  // turn off can
-  set_can_enable(CAN1, 0);
-  set_can_enable(CAN2, 0);
-  set_can_enable(CAN3, 0);
-
-  // turn off GMLAN
-  set_gpio_output(GPIOB, 14, 0);
-  set_gpio_output(GPIOB, 15, 0);
-
-  // turn off LIN
-  set_gpio_output(GPIOB, 7, 0);
-  set_gpio_output(GPIOA, 14, 0);
-
-  if (is_grey_panda) {
-    char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
-    uart_ring *ur = get_ring_by_number(1);
-    for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG)-1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
-  }
+  if (power_save_status != POWER_SAVE_STATUS_ENABLED) {
+    puts("enable power savings\n");
+
+    // turn off can
+    set_can_enable(CAN1, 0);
+    set_can_enable(CAN2, 0);
+    set_can_enable(CAN3, 0);
+
+    // turn off GMLAN
+    set_gpio_output(GPIOB, 14, 0);
+    set_gpio_output(GPIOB, 15, 0);
 
-  power_save_status = POWER_SAVE_STATUS_ENABLED;
+    // turn off LIN
+    set_gpio_output(GPIOB, 7, 0);
+    set_gpio_output(GPIOA, 14, 0);
+
+    if (is_grey_panda) {
+      char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
+      uart_ring *ur = get_ring_by_number(1);
+      for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG)-1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
+    }
+
+    power_save_status = POWER_SAVE_STATUS_ENABLED;
+  }
 }
 
 void power_save_disable(void) {
-  if (power_save_status == POWER_SAVE_STATUS_DISABLED) return;
-  puts("disable power savings\n");
-
-  // turn on can
-  set_can_enable(CAN1, 1);
-  set_can_enable(CAN2, 1);
-  set_can_enable(CAN3, 1);
-
-  // turn on GMLAN
-  set_gpio_output(GPIOB, 14, 1);
-  set_gpio_output(GPIOB, 15, 1);
-
-  // turn on LIN
-  set_gpio_output(GPIOB, 7, 1);
-  set_gpio_output(GPIOA, 14, 1);
-
-  if (is_grey_panda) {
-    char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
-    uart_ring *ur = get_ring_by_number(1);
-    for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG)-1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
-  }
+  if (power_save_status != POWER_SAVE_STATUS_DISABLED) {
+    puts("disable power savings\n");
+
+    // turn on can
+    set_can_enable(CAN1, 1);
+    set_can_enable(CAN2, 1);
+    set_can_enable(CAN3, 1);
+
+    // turn on GMLAN
+    set_gpio_output(GPIOB, 14, 1);
+    set_gpio_output(GPIOB, 15, 1);
 
-  power_save_status = POWER_SAVE_STATUS_DISABLED;
+    // turn on LIN
+    set_gpio_output(GPIOB, 7, 1);
+    set_gpio_output(GPIOA, 14, 1);
+
+    if (is_grey_panda) {
+      char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
+      uart_ring *ur = get_ring_by_number(1);
+      for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG)-1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
+    }
+
+    power_save_status = POWER_SAVE_STATUS_DISABLED;
+  }
 }
 

From 3fd6b337f9d0b09b693fcc842f0c172573d76e00 Mon Sep 17 00:00:00 2001
From: Riccardo <riccardo.biasini@gmail.com>
Date: Wed, 26 Jun 2019 15:59:32 -0700
Subject: [PATCH 3/4] ops, bug

---
 board/drivers/uart.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/board/drivers/uart.h b/board/drivers/uart.h
index b9dc5ad1472b8d..e0885b098cdd0a 100644
--- a/board/drivers/uart.h
+++ b/board/drivers/uart.h
@@ -54,14 +54,19 @@ uart_ring *get_ring_by_number(int a) {
   switch(a) {
     case 0:
       ring = &debug_ring;
+      break;
     case 1:
       ring = &esp_ring;
+      break;
     case 2:
       ring = &lin1_ring;
+      break;
     case 3:
       ring = &lin2_ring;
+      break;
     default:
       ring = NULL;
+      break;
   }
   return ring;
 }

From ec3d0386c2f6e546cc2e5ee8cc855086e8afdbf6 Mon Sep 17 00:00:00 2001
From: Riccardo <riccardo.biasini@gmail.com>
Date: Wed, 26 Jun 2019 18:24:21 -0700
Subject: [PATCH 4/4] refactor set_power_save_state

---
 board/drivers/llgpio.h |  4 +--
 board/gpio.h           |  2 +-
 board/main.c           | 25 ++++++---------
 board/power_saving.h   | 72 ++++++++++++++++--------------------------
 4 files changed, 41 insertions(+), 62 deletions(-)

diff --git a/board/drivers/llgpio.h b/board/drivers/llgpio.h
index c9f11b432c32bb..172776eb35d3cc 100644
--- a/board/drivers/llgpio.h
+++ b/board/drivers/llgpio.h
@@ -14,8 +14,8 @@ void set_gpio_mode(GPIO_TypeDef *GPIO, int pin, int mode) {
   GPIO->MODER = tmp;
 }
 
-void set_gpio_output(GPIO_TypeDef *GPIO, int pin, int val) {
-  if (val != 0) {
+void set_gpio_output(GPIO_TypeDef *GPIO, int pin, bool enabled) {
+  if (enabled) {
     GPIO->ODR |= (1 << pin);
   } else {
     GPIO->ODR &= ~(1 << pin);
diff --git a/board/gpio.h b/board/gpio.h
index 2894a76399d720..37ff6c3f679b71 100644
--- a/board/gpio.h
+++ b/board/gpio.h
@@ -97,7 +97,7 @@ void periph_init(void) {
 
 // ********************* setters *********************
 
-void set_can_enable(CAN_TypeDef *CAN, int enabled) {
+void set_can_enable(CAN_TypeDef *CAN, bool enabled) {
   // enable CAN busses
   if (CAN == CAN1) {
     #ifdef PANDA
diff --git a/board/main.c b/board/main.c
index c56577cc8426f3..e12f4495fa1f25 100644
--- a/board/main.c
+++ b/board/main.c
@@ -64,7 +64,7 @@ void debug_ring_callback(uart_ring *ring) {
 
 // ***************************** started logic *****************************
 
-int is_gpio_started(void) {
+bool is_gpio_started(void) {
   // ignition is on PA1
   return (GPIOA->IDR & (1U << 1)) == 0;
 }
@@ -80,11 +80,8 @@ void EXTI1_IRQHandler(void) {
     delay(100000);
 
     // set power savings mode here
-    if (is_gpio_started() == 1) {
-      power_save_disable();
-    } else {
-      power_save_enable();
-    }
+    int power_save_state = is_gpio_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
+    set_power_save_state(power_save_state);
     EXTI->PR = (1U << 1);
   }
 }
@@ -153,12 +150,10 @@ int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
 // send on serial, first byte to select the ring
 void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
   UNUSED(hardwired);
-  if (len != 0) {
-    uart_ring *ur = get_ring_by_number(usbdata[0]);
-    if (ur != NULL) {
-      if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
-        for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i]));
-      }
+  uart_ring *ur = get_ring_by_number(usbdata[0]);
+  if ((len != 0) && (ur != NULL)) {
+    if ((usbdata[0] < 2) || safety_tx_lin_hook(usbdata[0]-2, usbdata+1, len-1)) {
+      for (int i = 1; i < len; i++) while (!putc(ur, usbdata[i]));
     }
   }
 }
@@ -296,7 +291,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
         if (safety_ignition_hook() != -1) {
           // if the ignition hook depends on something other than the started GPIO
           // we have to disable power savings (fix for GM and Tesla)
-          power_save_disable();
+          set_power_save_state(POWER_SAVE_STATUS_DISABLED);
         }
         #ifndef EON
           // always LIVE on EON
@@ -678,8 +673,8 @@ int main(void) {
     set_esp_mode(ESP_DISABLED);
   }
   // only enter power save after the first cycle
-  /*if (is_gpio_started() == 0) {
-    power_save_enable();
+  /*if (is_gpio_started()) {
+    set_power_save_state(POWER_SAVE_STATUS_ENABLED);
   }*/
   // interrupt on started line
   started_interrupt_init();
diff --git a/board/power_saving.h b/board/power_saving.h
index e1b20768f649cd..986adf3ce29ff5 100644
--- a/board/power_saving.h
+++ b/board/power_saving.h
@@ -3,57 +3,41 @@
 
 int power_save_status = POWER_SAVE_STATUS_DISABLED;
 
-void power_save_enable(void) {
-  if (power_save_status != POWER_SAVE_STATUS_ENABLED) {
-    puts("enable power savings\n");
-
-    // turn off can
-    set_can_enable(CAN1, 0);
-    set_can_enable(CAN2, 0);
-    set_can_enable(CAN3, 0);
-
-    // turn off GMLAN
-    set_gpio_output(GPIOB, 14, 0);
-    set_gpio_output(GPIOB, 15, 0);
-
-    // turn off LIN
-    set_gpio_output(GPIOB, 7, 0);
-    set_gpio_output(GPIOA, 14, 0);
-
-    if (is_grey_panda) {
-      char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
-      uart_ring *ur = get_ring_by_number(1);
-      for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG)-1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
+void set_power_save_state(int state) {
+
+  bool is_valid_state = (state == POWER_SAVE_STATUS_ENABLED) || (state == POWER_SAVE_STATUS_DISABLED);
+  if (is_valid_state && (state != power_save_status)) {
+    bool enable = false;
+    if (state == POWER_SAVE_STATUS_ENABLED) {
+      puts("enable power savings\n");
+      if (is_grey_panda) {
+        char UBLOX_SLEEP_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x08\x00\x17\x78";
+        uart_ring *ur = get_ring_by_number(1);
+        for (unsigned int i = 0; i < sizeof(UBLOX_SLEEP_MSG) - 1; i++) while (!putc(ur, UBLOX_SLEEP_MSG[i]));
+      }
+    } else {
+      puts("disable power savings\n");
+      if (is_grey_panda) {
+        char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
+        uart_ring *ur = get_ring_by_number(1);
+        for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG) - 1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
+      }
+      enable = true;
     }
 
-    power_save_status = POWER_SAVE_STATUS_ENABLED;
-  }
-}
-
-void power_save_disable(void) {
-  if (power_save_status != POWER_SAVE_STATUS_DISABLED) {
-    puts("disable power savings\n");
-
     // turn on can
-    set_can_enable(CAN1, 1);
-    set_can_enable(CAN2, 1);
-    set_can_enable(CAN3, 1);
+    set_can_enable(CAN1, enable);
+    set_can_enable(CAN2, enable);
+    set_can_enable(CAN3, enable);
 
     // turn on GMLAN
-    set_gpio_output(GPIOB, 14, 1);
-    set_gpio_output(GPIOB, 15, 1);
+    set_gpio_output(GPIOB, 14, enable);
+    set_gpio_output(GPIOB, 15, enable);
 
     // turn on LIN
-    set_gpio_output(GPIOB, 7, 1);
-    set_gpio_output(GPIOA, 14, 1);
+    set_gpio_output(GPIOB, 7, enable);
+    set_gpio_output(GPIOA, 14, enable);
 
-    if (is_grey_panda) {
-      char UBLOX_WAKE_MSG[] = "\xb5\x62\x06\x04\x04\x00\x01\x00\x09\x00\x18\x7a";
-      uart_ring *ur = get_ring_by_number(1);
-      for (unsigned int i = 0; i < sizeof(UBLOX_WAKE_MSG)-1; i++) while (!putc(ur, UBLOX_WAKE_MSG[i]));
-    }
-
-    power_save_status = POWER_SAVE_STATUS_DISABLED;
+    power_save_status = state;
   }
 }
-