Squashed 'panda/' changes from 8843af7de..9881e6118

9881e6118 Panda for Mazda (#165) 9a15d2f5b fix version newline a8ed7d219 add subaru outback/legacy to subaru safety (#259) bdeb1c953 mazda is #12 14ea4d2e0 merge safety gm in a single file bf1ef875e Add GM passive safety mode (#266) c131fffae fix canflash for pedal (#267) 3397b1527 only allow bootloader entry on debug builds d68356b92 Honda Nidec: fwd stock AEB (#257) 6f532c6d5 Black panda Jenkins (#256) d68508c79 Gpio race condition fix (#263) d69d05fc0 Fixed pedal not initializing (#262) 36067e01c Honda safety: fixed incorrect brake decoding. Due to the specific limit of 255, this change does not affect the safety behavior git-subtree-dir: panda git-subtree-split: 9881e61184ad0417e9e080767f09585a9c777621 old-commit-hash: 876256a268
2026-06-29 10:32:10 +08:00 · 2019-09-09 23:00:23 +00:00
parent 229b51e5d0
commit 4efb208892
38 changed files with 1312 additions and 447 deletions
@@ -4,6 +4,7 @@
 *.o
 *.so
 *.d
+*.dump
 a.out
 *~
 .#*
@@ -12,4 +13,5 @@ pandacan.egg-info/
 board/obj/
 examples/output.csv
 .DS_Store
+.vscode
 nosetests.xml
@@ -1 +1 @@
-v1.4.3
+v1.4.7
@@ -48,8 +48,12 @@ void detect_configuration(void) {
  has_external_debug_serial = detect_with_pull(GPIOA, 3, PULL_DOWN);

  #ifdef PANDA
-    // check if the ESP is trying to put me in boot mode
-    is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
+    if(hw_type == HW_TYPE_WHITE_PANDA) {
+      // check if the ESP is trying to put me in boot mode
+      is_entering_bootmode = !detect_with_pull(GPIOB, 0, PULL_UP);
+    } else {
+      is_entering_bootmode = 0;
+    }
  #else
    is_entering_bootmode = 0;
  #endif
@@ -58,4 +62,4 @@ void detect_configuration(void) {
 // ///// Board functions ///// //
 bool board_has_gps(void) {
  return ((hw_type == HW_TYPE_GREY_PANDA) || (hw_type == HW_TYPE_BLACK_PANDA));
-}
+}
@@ -24,7 +24,7 @@ struct board {
 };

 // ******************* Definitions ********************
-// These should match the enum in cereal/log.capnp
+// These should match the enums in cereal/log.capnp and __init__.py
 #define HW_TYPE_UNKNOWN 0U
 #define HW_TYPE_WHITE_PANDA 1U
 #define HW_TYPE_GREY_PANDA 2U
@@ -54,4 +54,4 @@ struct board {
 #define CAN_MODE_OBD_CAN2 3U

 // ********************* Globals **********************
-uint8_t usb_power_mode = USB_POWER_NONE;
+uint8_t usb_power_mode = USB_POWER_NONE;
@@ -23,8 +23,9 @@ void black_enable_can_transciever(uint8_t transciever, bool enabled) {
 }

 void black_enable_can_transcievers(bool enabled) {
-  for(uint8_t i=1; i<=4U; i++)
+  for(uint8_t i=1U; i<=4U; i++){
    black_enable_can_transciever(i, enabled);
+  }
 }

 void black_set_led(uint8_t color, bool enabled) {
@@ -43,26 +44,41 @@ void black_set_led(uint8_t color, bool enabled) {
  }
 }

-void black_set_usb_power_mode(uint8_t mode){
+void black_set_gps_load_switch(bool enabled) {
+  set_gpio_output(GPIOC, 12, enabled);
+}
+
+void black_set_usb_load_switch(bool enabled) {
+  set_gpio_output(GPIOB, 1, !enabled);
+}
+
+void black_set_usb_power_mode(uint8_t mode) {
  usb_power_mode = mode;
-  puts("Trying to set USB power mode on black panda. This is not supported.\n");
+  if (mode == USB_POWER_NONE) {
+    black_set_usb_load_switch(false);
+  } else {
+    black_set_usb_load_switch(true);
+  }
 }

 void black_set_esp_gps_mode(uint8_t mode) {
  switch (mode) {
    case ESP_GPS_DISABLED:
-      // ESP OFF
+      // GPS OFF
      set_gpio_output(GPIOC, 14, 0);
      set_gpio_output(GPIOC, 5, 0);
+      black_set_gps_load_switch(false);
      break;
    case ESP_GPS_ENABLED:
-      // ESP ON
+      // GPS ON
      set_gpio_output(GPIOC, 14, 1);
      set_gpio_output(GPIOC, 5, 1);
+      black_set_gps_load_switch(true);
      break;
    case ESP_GPS_BOOTMODE:
      set_gpio_output(GPIOC, 14, 1);
      set_gpio_output(GPIOC, 5, 0);
+      black_set_gps_load_switch(true);
      break;
    default:
      puts("Invalid ESP/GPS mode\n");
@@ -132,9 +148,11 @@ void black_init(void) {
  // C8: FAN aka TIM3_CH3
  set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);

-  // C12: GPS load switch. Turn on permanently for now
-  set_gpio_output(GPIOC, 12, true);
-  //set_gpio_output(GPIOC, 12, false); //TODO: stupid inverted switch on prototype
+  // Turn on GPS load switch.
+  black_set_gps_load_switch(true);
+
+  // Turn on USB load switch.
+  black_set_usb_load_switch(true);

  // Initialize harness
  harness_init();
@@ -285,6 +285,13 @@ void white_init(void) {

  // Set normal CAN mode
  white_set_can_mode(CAN_MODE_NORMAL);
+
+  // Setup ignition interrupts
+  SYSCFG->EXTICR[1] = SYSCFG_EXTICR1_EXTI1_PA;
+  EXTI->IMR |= (1U << 1);
+  EXTI->RTSR |= (1U << 1);
+  EXTI->FTSR |= (1U << 1);
+  NVIC_EnableIRQ(EXTI1_IRQn);
 }

 const harness_configuration white_harness_config = {
@@ -63,7 +63,7 @@ extern void *_app_start[];
 // BOUNTY: $200 coupon on shop.comma.ai or $100 check.

 int main(void) {
-  __disable_irq();
+  disable_interrupts();
  clock_init();
  detect_configuration();
  detect_board_type();
@@ -62,7 +62,7 @@ int can_overflow_cnt = 0;
 bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
  bool ret = 0;

-  enter_critical_section();
+  ENTER_CRITICAL();
  if (q->w_ptr != q->r_ptr) {
    *elem = q->elems[q->r_ptr];
    if ((q->r_ptr + 1U) == q->fifo_size) {
@@ -72,7 +72,7 @@ bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
    }
    ret = 1;
  }
-  exit_critical_section();
+  EXIT_CRITICAL();

  return ret;
 }
@@ -81,7 +81,7 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
  bool ret = false;
  uint32_t next_w_ptr;

-  enter_critical_section();
+  ENTER_CRITICAL();
  if ((q->w_ptr + 1U) == q->fifo_size) {
    next_w_ptr = 0;
  } else {
@@ -92,7 +92,7 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
    q->w_ptr = next_w_ptr;
    ret = true;
  }
-  exit_critical_section();
+  EXIT_CRITICAL();
  if (!ret) {
    can_overflow_cnt++;
    #ifdef DEBUG
@@ -103,10 +103,10 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
 }

 void can_clear(can_ring *q) {
-  enter_critical_section();
+  ENTER_CRITICAL();
  q->w_ptr = 0;
  q->r_ptr = 0;
-  exit_critical_section();
+  EXIT_CRITICAL();
 }

 // assign CAN numbering
@@ -124,7 +124,7 @@ uint8_t bus_lookup[] = {0,1,2};
 uint8_t can_num_lookup[] = {0,1,2,-1};
 int8_t can_forwarding[] = {-1,-1,-1,-1};
 uint32_t can_speed[] = {5000, 5000, 5000, 333};
-#define CAN_MAX 3
+#define CAN_MAX 3U

 #define CANIF_FROM_CAN_NUM(num) (cans[num])
 #define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2))
@@ -158,9 +158,10 @@ void can_init(uint8_t can_number) {
 }

 void can_init_all(void) {
-  for (int i=0; i < CAN_MAX; i++) {
+  for (uint8_t i=0U; i < CAN_MAX; i++) {
    can_init(i);
  }
+  current_board->enable_can_transcievers(true);
 }

 void can_flip_buses(uint8_t bus1, uint8_t bus2){
@@ -248,7 +249,7 @@ void can_set_obd(uint8_t harness_orientation, bool obd){

 // CAN error
 void can_sce(CAN_TypeDef *CAN) {
-  enter_critical_section();
+  ENTER_CRITICAL();

  #ifdef DEBUG
    if (CAN==CAN1) puts("CAN1:  ");
@@ -271,7 +272,7 @@ void can_sce(CAN_TypeDef *CAN) {

  can_err_cnt += 1;
  llcan_clear_send(CAN);
-  exit_critical_section();
+  EXIT_CRITICAL();
 }

 // ***************************** CAN *****************************
@@ -279,7 +280,7 @@ void can_sce(CAN_TypeDef *CAN) {
 void process_can(uint8_t can_number) {
  if (can_number != 0xffU) {

-    enter_critical_section();
+    ENTER_CRITICAL();

    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
    uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
@@ -327,7 +328,7 @@ void process_can(uint8_t can_number) {
      }
    }

-    exit_critical_section();
+    EXIT_CRITICAL();
  }
 }

@@ -11,42 +11,52 @@
 #define OUTPUT_TYPE_OPEN_DRAIN 1U

 void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
+  ENTER_CRITICAL();
  uint32_t tmp = GPIO->MODER;
  tmp &= ~(3U << (pin * 2U));
  tmp |= (mode << (pin * 2U));
  GPIO->MODER = tmp;
+  EXIT_CRITICAL();
 }

 void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) {
+  ENTER_CRITICAL();
  if (enabled) {
    GPIO->ODR |= (1U << pin);
  } else {
    GPIO->ODR &= ~(1U << pin);
  }
  set_gpio_mode(GPIO, pin, MODE_OUTPUT);
+  EXIT_CRITICAL();
 }

 void set_gpio_output_type(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int output_type){
+  ENTER_CRITICAL();
  if(output_type == OUTPUT_TYPE_OPEN_DRAIN) {
    GPIO->OTYPER |= (1U << pin);
  } else {
    GPIO->OTYPER &= ~(1U << pin);
  }
+  EXIT_CRITICAL();
 }

 void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
+  ENTER_CRITICAL();
  uint32_t tmp = GPIO->AFR[pin >> 3U];
  tmp &= ~(0xFU << ((pin & 7U) * 4U));
  tmp |= mode << ((pin & 7U) * 4U);
  GPIO->AFR[pin >> 3] = tmp;
  set_gpio_mode(GPIO, pin, MODE_ALTERNATE);
+  EXIT_CRITICAL();
 }

 void set_gpio_pullup(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
+  ENTER_CRITICAL();
  uint32_t tmp = GPIO->PUPDR;
  tmp &= ~(3U << (pin * 2U));
  tmp |= (mode << (pin * 2U));
  GPIO->PUPDR = tmp;
+  EXIT_CRITICAL();
 }

 int get_gpio_input(GPIO_TypeDef *GPIO, unsigned int pin) {
@@ -74,7 +74,7 @@ uart_ring *get_ring_by_number(int a) {
 // ***************************** serial port *****************************

 void uart_ring_process(uart_ring *q) {
-  enter_critical_section();
+  ENTER_CRITICAL();
  // TODO: check if external serial is connected
  int sr = q->uart->SR;

@@ -108,7 +108,7 @@ void uart_ring_process(uart_ring *q) {
    // set dropped packet flag?
  }

-  exit_critical_section();
+  EXIT_CRITICAL();
 }

 // interrupt boilerplate
@@ -121,13 +121,13 @@ void UART5_IRQHandler(void) { uart_ring_process(&lin1_ring); }
 bool getc(uart_ring *q, char *elem) {
  bool ret = false;

-  enter_critical_section();
+  ENTER_CRITICAL();
  if (q->w_ptr_rx != q->r_ptr_rx) {
    if (elem != NULL) *elem = q->elems_rx[q->r_ptr_rx];
    q->r_ptr_rx = (q->r_ptr_rx + 1U) % FIFO_SIZE;
    ret = true;
  }
-  exit_critical_section();
+  EXIT_CRITICAL();

  return ret;
 }
@@ -136,14 +136,14 @@ bool injectc(uart_ring *q, char elem) {
  int ret = false;
  uint16_t next_w_ptr;

-  enter_critical_section();
+  ENTER_CRITICAL();
  next_w_ptr = (q->w_ptr_rx + 1U) % FIFO_SIZE;
  if (next_w_ptr != q->r_ptr_rx) {
    q->elems_rx[q->w_ptr_rx] = elem;
    q->w_ptr_rx = next_w_ptr;
    ret = true;
  }
-  exit_critical_section();
+  EXIT_CRITICAL();

  return ret;
 }
@@ -152,14 +152,14 @@ bool putc(uart_ring *q, char elem) {
  bool ret = false;
  uint16_t next_w_ptr;

-  enter_critical_section();
+  ENTER_CRITICAL();
  next_w_ptr = (q->w_ptr_tx + 1U) % FIFO_SIZE;
  if (next_w_ptr != q->r_ptr_tx) {
    q->elems_tx[q->w_ptr_tx] = elem;
    q->w_ptr_tx = next_w_ptr;
    ret = true;
  }
-  exit_critical_section();
+  EXIT_CRITICAL();

  uart_ring_process(q);

@@ -185,12 +185,12 @@ void uart_send_break(uart_ring *u) {
 }

 void clear_uart_buff(uart_ring *q) {
-  enter_critical_section();
+  ENTER_CRITICAL();
  q->w_ptr_tx = 0;
  q->r_ptr_tx = 0;
  q->w_ptr_rx = 0;
  q->r_ptr_rx = 0;
-  exit_critical_section();
+  EXIT_CRITICAL();
 }

 // ***************************** start UART code *****************************
@@ -215,7 +215,7 @@ char usart1_dma[USART1_DMA_LEN];
 void uart_dma_drain(void) {
  uart_ring *q = &esp_ring;

-  enter_critical_section();
+  ENTER_CRITICAL();

  if ((DMA2->HISR & DMA_HISR_TCIF5) || (DMA2->HISR & DMA_HISR_HTIF5) || (DMA2_Stream5->NDTR != USART1_DMA_LEN)) {
    // disable DMA
@@ -245,7 +245,7 @@ void uart_dma_drain(void) {
    q->uart->CR3 |= USART_CR3_DMAR;
  }

-  exit_critical_section();
+  EXIT_CRITICAL();
 }

 void DMA2_Stream5_IRQHandler(void) {
@@ -20,6 +20,12 @@ void jump_to_bootloader(void) {
 }

 void early(void) {
+  // Reset global critical depth
+  global_critical_depth = 0;
+
+  // neccesary for DFU flashing on a non-power cycled white panda
+  enable_interrupts();
+
  // after it's been in the bootloader, things are initted differently, so we reset
  if ((enter_bootloader_mode != BOOT_NORMAL) &&
      (enter_bootloader_mode != ENTER_BOOTLOADER_MAGIC) &&
@@ -42,24 +42,26 @@ int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {

 // ********************* IRQ helpers *********************

-int interrupts_enabled = 0;
+volatile bool interrupts_enabled = false;
+
 void enable_interrupts(void) {
-  interrupts_enabled = 1;
+  interrupts_enabled = true;
  __enable_irq();
 }

-int critical_depth = 0;
-void enter_critical_section(void) {
+void disable_interrupts(void) {
+  interrupts_enabled = false;
  __disable_irq();
-  // this is safe because interrupts are disabled
-  critical_depth += 1;
 }

-void exit_critical_section(void) {
-  // this is safe because interrupts are disabled
-  critical_depth -= 1;
-  if ((critical_depth == 0) && interrupts_enabled) {
-    __enable_irq();
+uint8_t global_critical_depth = 0U;
+#define ENTER_CRITICAL()                                      \
+  __disable_irq();                                            \
+  global_critical_depth += 1U;
+
+#define EXIT_CRITICAL()                                       \
+  global_critical_depth -= 1U;                                \
+  if ((global_critical_depth == 0U) && interrupts_enabled) {  \
+    __enable_irq();                                           \
  }
-}

@@ -78,9 +78,11 @@ void started_interrupt_handler(uint8_t interrupt_line) {
    // jenky debounce
    delay(100000);

-    // set power savings mode here
-    int power_save_state = current_board->check_ignition() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
-    set_power_save_state(power_save_state);
+    // set power savings mode here if on EON build
+    #ifdef EON
+      int power_save_state = current_board->check_ignition() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
+      set_power_save_state(power_save_state);
+    #endif
  }
  EXTI->PR = (1U << interrupt_line);
 }
@@ -100,14 +102,6 @@ void EXTI3_IRQHandler(void) {
  started_interrupt_handler(3);
 }

-void started_interrupt_init(void) {
-  SYSCFG->EXTICR[1] = SYSCFG_EXTICR1_EXTI1_PA;
-  EXTI->IMR |= (1U << 1);
-  EXTI->RTSR |= (1U << 1);
-  EXTI->FTSR |= (1U << 1);
-  NVIC_EnableIRQ(EXTI1_IRQn);
-}
-
 // ****************************** safety mode ******************************

 // this is the only way to leave silent mode
@@ -116,30 +110,29 @@ void set_safety_mode(uint16_t mode, int16_t param) {
  if (err == -1) {
    puts("Error: safety set mode failed\n");
  } else {
-    if (mode == SAFETY_NOOUTPUT) {
-      can_silent = ALL_CAN_SILENT;
-    } else {
-      can_silent = ALL_CAN_LIVE;
-    }
-
    switch (mode) {
        case SAFETY_NOOUTPUT:
          set_intercept_relay(false);
          if(hw_type == HW_TYPE_BLACK_PANDA){
            current_board->set_can_mode(CAN_MODE_NORMAL);
          }
+          can_silent = ALL_CAN_SILENT;
          break;
        case SAFETY_ELM327:
          set_intercept_relay(false);
+          heartbeat_counter = 0U;
          if(hw_type == HW_TYPE_BLACK_PANDA){
            current_board->set_can_mode(CAN_MODE_OBD_CAN2);
          }
+          can_silent = ALL_CAN_LIVE;
          break;
        default:
          set_intercept_relay(true);
+          heartbeat_counter = 0U;
          if(hw_type == HW_TYPE_BLACK_PANDA){
            current_board->set_can_mode(CAN_MODE_NORMAL);
          }
+          can_silent = ALL_CAN_LIVE;
          break;
      }          
    if (safety_ignition_hook() != -1) {
@@ -289,11 +282,14 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
      // so it's blocked over wifi
      switch (setup->b.wValue.w) {
        case 0:
-          if (hardwired) {
-            puts("-> entering bootloader\n");
-            enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
-            NVIC_SystemReset();
-          }
+          // only allow bootloader entry on debug builds
+          #ifdef ALLOW_DEBUG
+            if (hardwired) {
+              puts("-> entering bootloader\n");
+              enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
+              NVIC_SystemReset();
+            }
+          #endif
          break;
        case 1:
          puts("-> entering softloader\n");
@@ -464,7 +460,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
      break;
    // **** 0xe6: set USB power
    case 0xe6:
-      if (setup->b.wValue.w == 1U) {
+      if (setup->b.wValue.w == 0U) {
+        puts("user setting NONE mode\n");
+        current_board->set_usb_power_mode(USB_POWER_NONE);
+      } else if (setup->b.wValue.w == 1U) {
        puts("user setting CDP mode\n");
        current_board->set_usb_power_mode(USB_POWER_CDP);
      } else if (setup->b.wValue.w == 2U) {
@@ -610,11 +609,10 @@ void TIM3_IRQHandler(void) {
      pending_can_live = 0;
    }
    #ifdef DEBUG
-      //TODO: re-enable
-      //puts("** blink ");
-      //puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts("  ");
-      //puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts("  ");
-      //puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
+      puts("** blink ");
+      puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts("  ");
+      puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts("  ");
+      puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
    #endif

    // set green LED to be controls allowed
@@ -645,7 +643,7 @@ void TIM3_IRQHandler(void) {

 int main(void) {
  // shouldn't have interrupts here, but just in case
-  __disable_irq();
+  disable_interrupts();

  // init early devices
  clock_init();
@@ -730,11 +728,6 @@ int main(void) {
  /*if (current_board->check_ignition()) {
    set_power_save_state(POWER_SAVE_STATUS_ENABLED);
  }*/
-
-  if (hw_type != HW_TYPE_BLACK_PANDA) {
-    // interrupt on started line
-    started_interrupt_init();
-  }
 #endif

  // 48mhz / 65536 ~= 732 / 732 = 1
@@ -55,18 +55,18 @@ void debug_ring_callback(uart_ring *ring) {
  }
 }

-int usb_cb_ep1_in(uint8_t *usbdata, int len, bool hardwired) {
+int usb_cb_ep1_in(void *usbdata, int len, bool hardwired) {
  UNUSED(usbdata);
  UNUSED(len);
  UNUSED(hardwired);
  return 0;
 }
-void usb_cb_ep2_out(uint8_t *usbdata, int len, bool hardwired) {
+void usb_cb_ep2_out(void *usbdata, int len, bool hardwired) {
  UNUSED(usbdata);
  UNUSED(len);
  UNUSED(hardwired);
 }
-void usb_cb_ep3_out(uint8_t *usbdata, int len, bool hardwired) {
+void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
  UNUSED(usbdata);
  UNUSED(len);
  UNUSED(hardwired);
@@ -299,7 +299,7 @@ void pedal(void) {
 }

 int main(void) {
-  __disable_irq();
+  disable_interrupts();

  // init devices
  clock_init();
@@ -307,6 +307,9 @@ int main(void) {
  detect_configuration();
  detect_board_type();

+  // init board
+  current_board->init();
+
 #ifdef PEDAL_USB
  // enable USB
  usb_init();
@@ -331,7 +334,7 @@ int main(void) {
  watchdog_init();

  puts("**** INTERRUPTS ON ****\n");
-  __enable_irq();
+  enable_interrupts();

  // main pedal loop
  while (1) {
@@ -28,6 +28,13 @@ void set_power_save_state(int state) {
    // Switch CAN transcievers
    current_board->enable_can_transcievers(enable);

+    // Switch EPS/GPS
+    if (enable) {
+      current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+    } else {
+      current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
+    }
+    
    if(hw_type != HW_TYPE_BLACK_PANDA){
      // turn on GMLAN
      set_gpio_output(GPIOB, 14, enable);
@@ -13,6 +13,7 @@
 #include "safety/safety_hyundai.h"
 #include "safety/safety_chrysler.h"
 #include "safety/safety_subaru.h"
+#include "safety/safety_mazda.h"
 #include "safety/safety_elm327.h"

 const safety_hooks *current_hooks = &nooutput_hooks;
@@ -55,6 +56,8 @@ typedef struct {
 #define SAFETY_TESLA 8U
 #define SAFETY_CHRYSLER 9U
 #define SAFETY_SUBARU 10U
+#define SAFETY_GM_PASSIVE 11U
+#define SAFETY_MAZDA 12U
 #define SAFETY_GM_ASCM 0x1334U
 #define SAFETY_TOYOTA_IPAS 0x1335U
 #define SAFETY_ALLOUTPUT 0x1337U
@@ -71,7 +74,9 @@ const safety_hook_config safety_hook_registry[] = {
  {SAFETY_HYUNDAI, &hyundai_hooks},
  {SAFETY_CHRYSLER, &chrysler_hooks},
  {SAFETY_SUBARU, &subaru_hooks},
+  {SAFETY_MAZDA, &mazda_hooks},
  {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
+  {SAFETY_GM_PASSIVE, &gm_passive_hooks},
  {SAFETY_GM_ASCM, &gm_ascm_hooks},
  {SAFETY_TESLA, &tesla_hooks},
  {SAFETY_ALLOUTPUT, &alloutput_hooks},
@@ -231,6 +231,20 @@ static int gm_ign_hook(void) {
  return gm_ignition_started;
 }

+// All sending is disallowed.
+// The only difference from "no output" model
+// is using GM ignition hook.
+
+static void gm_passive_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+  int bus_number = GET_BUS(to_push);
+  int addr = GET_ADDR(to_push);
+
+  if ((addr == 0x1F1) && (bus_number == 0)) {
+    bool ign = (GET_BYTE(to_push, 0) & 0x20) != 0;
+    gm_ignition_started = ign;
+  }
+}
+
 const safety_hooks gm_hooks = {
  .init = gm_init,
  .rx = gm_rx_hook,
@@ -239,3 +253,12 @@ const safety_hooks gm_hooks = {
  .ignition = gm_ign_hook,
  .fwd = default_fwd_hook,
 };
+
+const safety_hooks gm_passive_hooks = {
+  .init = gm_init,
+  .rx = gm_passive_rx_hook,
+  .tx = nooutput_tx_hook,
+  .tx_lin = nooutput_tx_lin_hook,
+  .ignition = gm_ign_hook,
+  .fwd = default_fwd_hook,
+};
@@ -8,16 +8,19 @@
 //      brake > 0mph

 const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328;  // ratio between offset and gain from dbc file
-int honda_brake_prev = 0;
+int honda_brake = 0;
 int honda_gas_prev = 0;
+bool honda_brake_pressed_prev = false;
 bool honda_moving = false;
 bool honda_bosch_hardware = false;
 bool honda_alt_brake_msg = false;
+bool honda_fwd_brake = false;

 static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {

  int addr = GET_ADDR(to_push);
  int len = GET_LEN(to_push);
+  int bus = GET_BUS(to_push);

  // sample speed
  if (addr == 0x158) {
@@ -51,11 +54,11 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
  // exit controls on rising edge of brake press or on brake press when speed > 0
  bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
  if (is_user_brake_msg) {
-    int brake = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
-    if (brake && (!(honda_brake_prev) || honda_moving)) {
+    bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
+    if (brake_pressed && (!(honda_brake_pressed_prev) || honda_moving)) {
      controls_allowed = 0;
    }
-    honda_brake_prev = brake;
+    honda_brake_pressed_prev = brake_pressed;
  }

  // exit controls on rising edge of gas press if interceptor (0x201 w/ len = 6)
@@ -81,6 +84,20 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
      honda_gas_prev = gas;
    }
  }
+  if ((bus == 2) && (addr == 0x1FA)) {
+    bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20;
+    int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3);
+
+    // Forward AEB when stock braking is higher than openpilot braking
+    // only stop forwarding when AEB event is over
+    if (!honda_stock_aeb) {
+      honda_fwd_brake = false;
+    } else if (honda_stock_brake >= honda_brake) {
+      honda_fwd_brake = true;
+    } else {
+      // Leave honda forward brake as is
+    }
+  }
 }

 // all commands: gas, brake and steering
@@ -98,18 +115,21 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
  // disallow actuator commands if gas or brake (with vehicle moving) are pressed
  // and the the latching controls_allowed flag is True
  int pedal_pressed = honda_gas_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) ||
-                      (honda_brake_prev && honda_moving);
+                      (honda_brake_pressed_prev && honda_moving);
  bool current_controls_allowed = controls_allowed && !(pedal_pressed);

  // BRAKE: safety check
-  if (addr == 0x1FA) {
-    int brake = (GET_BYTE(to_send, 0) << 2) + (GET_BYTE(to_send, 1) & 0x3);
+  if ((addr == 0x1FA) && (bus == 0)) {
+    honda_brake = (GET_BYTE(to_send, 0) << 2) + ((GET_BYTE(to_send, 1) >> 6) & 0x3);
    if (!current_controls_allowed || !long_controls_allowed) {
-      if (brake != 0) {
+      if (honda_brake != 0) {
        tx = 0;
      }
    }
-    if (brake > 255) {
+    if (honda_brake > 255) {
+      tx = 0;
+    }
+    if (honda_fwd_brake) {
      tx = 0;
    }
  }
@@ -175,9 +195,12 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
  if (bus_num == 2) {
    // block stock lkas messages and stock acc messages (if OP is doing ACC)
    int addr = GET_ADDR(to_fwd);
-    int is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
-    int is_acc_msg = (addr == 0x1FA) || (addr == 0x30C) || (addr == 0x39F);
-    int block_fwd = is_lkas_msg || (is_acc_msg && long_controls_allowed);
+    bool is_lkas_msg = (addr == 0xE4) || (addr == 0x194) || (addr == 0x33D);
+    bool is_acc_hud_msg = (addr == 0x30C) || (addr == 0x39F);
+    bool is_brake_msg = addr == 0x1FA;
+    bool block_fwd = is_lkas_msg ||
+                     (is_acc_hud_msg && long_controls_allowed) ||
+                     (is_brake_msg && long_controls_allowed && !honda_fwd_brake);
    if (!block_fwd) {
      bus_fwd = 0;
    }
@@ -0,0 +1,169 @@
+
+// CAN msgs we care about
+#define MAZDA_LKAS 0x243
+#define MAZDA_LANEINFO 0x440
+#define MAZDA_CRZ_CTRL 0x21c
+#define MAZDA_WHEEL_SPEED 0x215
+#define MAZDA_STEER_TORQUE 0x240
+
+// CAN bus numbers
+#define MAZDA_MAIN 0
+#define MAZDA_AUX 1
+#define MAZDA_CAM 2
+
+#define MAZDA_MAX_STEER 2048
+
+// max delta torque allowed for real time checks
+#define MAZDA_MAX_RT_DELTA 940
+// 250ms between real time checks
+#define MAZDA_RT_INTERVAL 250000
+#define MAZDA_MAX_RATE_UP 10
+#define MAZDA_MAX_RATE_DOWN 25
+#define MAZDA_DRIVER_TORQUE_ALLOWANCE 15
+#define MAZDA_DRIVER_TORQUE_FACTOR 1
+
+
+int mazda_cruise_engaged_last = 0;
+int mazda_rt_torque_last = 0;
+int mazda_desired_torque_last = 0;
+uint32_t mazda_ts_last = 0;
+struct sample_t mazda_torque_driver;         // last few driver torques measured
+
+// track msgs coming from OP so that we know what CAM msgs to drop and what to forward
+int mazda_op_lkas_detected = 0;
+int mazda_op_laneinfo_detected = 0;
+
+int mazda_forward_cam = 0;
+int mazda_giraffe_switch_2_on = 0;
+
+void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
+  int bus = GET_BUS(to_push);
+  int addr = GET_ADDR(to_push);
+
+  if ((addr == MAZDA_STEER_TORQUE) && (bus == MAZDA_MAIN)) {
+    int torque_driver_new = GET_BYTE(to_push, 0) - 127;
+    // update array of samples
+    update_sample(&mazda_torque_driver, torque_driver_new);
+  }
+
+  // enter controls on rising edge of ACC, exit controls on ACC off
+  if ((addr == MAZDA_CRZ_CTRL) && (bus == MAZDA_MAIN)) {
+    int cruise_engaged = GET_BYTE(to_push, 0) & 8;
+    if (cruise_engaged != 0) {
+      if (!mazda_cruise_engaged_last) {
+	controls_allowed = 1;
+      }
+    }
+    else {
+      controls_allowed = 0;
+    }
+    mazda_cruise_engaged_last = cruise_engaged;
+  }
+
+  // we have msgs on bus MAZDA_CAM
+  if (bus == MAZDA_CAM) {
+    // the stock CAM is connected
+    if (addr == MAZDA_LKAS) {
+      mazda_forward_cam = 1;
+    }
+    // if we see wheel speed msgs on MAZDA_CAM bus then giraffe switch 2 is high
+    // (hardware passthru)
+    if (addr == MAZDA_WHEEL_SPEED) {
+      mazda_giraffe_switch_2_on = 1;
+    }
+  }
+}
+
+static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
+  int tx = 1;
+  int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
+
+  // Check if msg is sent on the main BUS
+  if (bus == MAZDA_MAIN) {
+    if ((addr == MAZDA_LKAS) && !mazda_op_lkas_detected){
+      mazda_op_lkas_detected = 1;
+    }
+    if ((addr == MAZDA_LANEINFO) && !mazda_op_laneinfo_detected){
+      mazda_op_laneinfo_detected = 1;
+    }
+
+    // steer cmd checks
+    if (addr == MAZDA_LKAS) {
+      int desired_torque = (((GET_BYTE(to_send, 0) & 0x0f) << 8) | GET_BYTE(to_send, 1)) - MAZDA_MAX_STEER;
+      bool violation = 0;
+      uint32_t ts = TIM2->CNT;
+
+      if (controls_allowed) {
+
+	// *** global torque limit check ***
+	violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER);
+
+	// *** torque rate limit check ***
+	int desired_torque_last = mazda_desired_torque_last;
+	violation |= driver_limit_check(desired_torque, desired_torque_last, &mazda_torque_driver,
+					MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN,
+					MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR);
+	// used next time
+	mazda_desired_torque_last = desired_torque;
+
+	// *** torque real time rate limit check ***
+	violation |= rt_rate_limit_check(desired_torque, mazda_rt_torque_last, MAZDA_MAX_RT_DELTA);
+
+	// every RT_INTERVAL set the new limits
+	uint32_t ts_elapsed = get_ts_elapsed(ts, mazda_ts_last);
+	if (ts_elapsed > ((uint32_t) MAZDA_RT_INTERVAL)) {
+	  mazda_rt_torque_last = desired_torque;
+	  mazda_ts_last = ts;
+	}
+      }
+
+      // no torque if controls is not allowed
+      if (!controls_allowed && (desired_torque != 0)) {
+	violation = 1;
+      }
+
+      // reset to 0 if either controls is not allowed or there's a violation
+      if (violation || !controls_allowed) {
+	mazda_desired_torque_last = 0;
+	mazda_rt_torque_last = 0;
+	mazda_ts_last = ts;
+      }
+
+      if (violation) {
+	tx = 0;
+      }
+    }
+  }
+  return tx;
+}
+
+static int mazda_fwd_hook(int bus, CAN_FIFOMailBox_TypeDef *to_fwd) {
+  int bus_fwd = -1;
+  if (mazda_forward_cam && !mazda_giraffe_switch_2_on) {
+    int addr = GET_ADDR(to_fwd);
+    if (bus == MAZDA_MAIN) {
+      bus_fwd = MAZDA_CAM;
+    }
+    else if (bus == MAZDA_CAM) {
+      // drop stock CAM_LKAS and CAM_LANEINFI if OP is sending them
+      if (!((addr == MAZDA_LKAS) && mazda_op_lkas_detected) &&
+          !((addr == MAZDA_LANEINFO) && mazda_op_laneinfo_detected)) {
+	bus_fwd = MAZDA_MAIN;
+      }
+    }
+    else {
+      bus_fwd = -1;
+    }
+  }
+  return bus_fwd;
+}
+
+const safety_hooks mazda_hooks = {
+  .init = nooutput_init,
+  .rx = mazda_rx_hook,
+  .tx = mazda_tx_hook,
+  .tx_lin = nooutput_tx_lin_hook,
+  .ignition = default_ign_hook,
+  .fwd = mazda_fwd_hook,
+};
@@ -14,21 +14,22 @@ int subaru_desired_torque_last = 0;
 uint32_t subaru_ts_last = 0;
 struct sample_t subaru_torque_driver;         // last few driver torques measured

-
 static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
  int bus = GET_BUS(to_push);
  int addr = GET_ADDR(to_push);

-  if ((addr == 0x119) && (bus == 0)){
-    int torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
+  if (((addr == 0x119) || (addr == 0x371)) && (bus == 0)){
+    int bit_shift = (addr == 0x119) ? 16 : 29;
+    int torque_driver_new = ((GET_BYTES_04(to_push) >> bit_shift) & 0x7FF);
    torque_driver_new = to_signed(torque_driver_new, 11);
    // update array of samples
    update_sample(&subaru_torque_driver, torque_driver_new);
  }

  // enter controls on rising edge of ACC, exit controls on ACC off
-  if ((addr == 0x240) && (bus == 0)) {
-    int cruise_engaged = GET_BYTE(to_push, 5) & 2;
+  if (((addr == 0x240) || (addr == 0x144)) && (bus == 0)) {
+    int bit_shift = (addr == 0x240) ? 9 : 17;
+    int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
    if (cruise_engaged && !subaru_cruise_engaged_last) {
      controls_allowed = 1;
    }
@@ -44,8 +45,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
  int addr = GET_ADDR(to_send);

  // steer cmd checks
-  if (addr == 0x122) {
-    int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x1FFF);
+  if ((addr == 0x122) || (addr == 0x164)) {
+    int bit_shift = (addr == 0x122) ? 16 : 8;
+    int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF);
    bool violation = 0;
    uint32_t ts = TIM2->CNT;
    desired_torque = to_signed(desired_torque, 13);
@@ -102,8 +104,8 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
    bus_fwd = 2;  // Camera CAN
  }
  if (bus_num == 2) {
+    // 290 is LKAS for Global Platform
    // 356 is LKAS for outback 2015
-    // 356 is LKAS for Global Platform
    // 545 is ES_Distance
    // 802 is ES_LKAS
    int addr = GET_ADDR(to_fwd);
@@ -183,7 +183,7 @@ void CAN1_RX0_IRQHandler(void) {
    if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
      uint8_t dat[8];
      for (int i = 0; i < 8; i++) {
-        dat[0] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
+        dat[i] = GET_BYTE(&CAN->sFIFOMailBox[0], i);
      }
      uint8_t odat[8];
      uint8_t type = dat[0] & 0xF0;
@@ -307,7 +307,7 @@ void soft_flasher_start(void) {
  // green LED on for flashing
  current_board->set_led(LED_GREEN, 1);

-  __enable_irq();
+  enable_interrupts();

  uint64_t cnt = 0;

@@ -135,6 +135,12 @@ class Panda(object):
  REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE
  REQUEST_OUT = usb1.ENDPOINT_OUT | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE

+  HW_TYPE_UNKNOWN = '\x00'
+  HW_TYPE_WHITE_PANDA = '\x01'
+  HW_TYPE_GREY_PANDA = '\x02'
+  HW_TYPE_BLACK_PANDA = '\x03'
+  HW_TYPE_PEDAL = '\x04'
+
  def __init__(self, serial=None, claim=True):
    self._serial = serial
    self._handle = None
@@ -363,11 +369,14 @@ class Panda(object):
  def get_type(self):
    return self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)

+  def is_white(self):
+    return self.get_type() == Panda.HW_TYPE_WHITE_PANDA
+
  def is_grey(self):
-    return self.get_type() == "\x02"
+    return self.get_type() == Panda.HW_TYPE_GREY_PANDA

  def is_black(self):
-    return self.get_type() == "\x03"
+    return self.get_type() == Panda.HW_TYPE_BLACK_PANDA

  def get_serial(self):
    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
@@ -470,6 +479,7 @@ class Panda(object):
        break
      except (usb1.USBErrorIO, usb1.USBErrorOverflow):
        print("CAN: BAD RECV, RETRYING")
+	time.sleep(0.1)
    return parse_can_buffer(dat)

  def can_clear(self, bus):
@@ -1,15 +1,13 @@
 import os
 from panda import Panda
-from helpers import panda_color_to_serial, test_white_and_grey
+from helpers import panda_type_to_serial, test_white_and_grey, test_all_pandas, panda_connect_and_init

-@test_white_and_grey
-@panda_color_to_serial
-def test_recover(serial=None):
-  p = Panda(serial=serial)
+@test_all_pandas
+@panda_connect_and_init
+def test_recover(p):
  assert p.recover(timeout=30)

-@test_white_and_grey
-@panda_color_to_serial
-def test_flash(serial=None):
-  p = Panda(serial=serial)
+@test_all_pandas
+@panda_connect_and_init
+def test_flash(p):
  p.flash()
@@ -4,16 +4,11 @@ import sys
 import time
 from panda import Panda
 from nose.tools import assert_equal, assert_less, assert_greater
-from helpers import time_many_sends, connect_wo_esp, test_white_and_grey, panda_color_to_serial
-
-SPEED_NORMAL = 500
-SPEED_GMLAN = 33.3
-
-@test_white_and_grey
-@panda_color_to_serial
-def test_can_loopback(serial=None):
-  p = connect_wo_esp(serial)
+from helpers import SPEED_NORMAL, SPEED_GMLAN, time_many_sends, test_white_and_grey, panda_type_to_serial, test_all_pandas, panda_connect_and_init

+@test_all_pandas
+@panda_connect_and_init
+def test_can_loopback(p):
  # enable output mode
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

@@ -26,9 +21,6 @@ def test_can_loopback(serial=None):
    busses = [0,1,2]

  for bus in busses:
-    # send heartbeat
-    p.send_heartbeat()
-
    # set bus 0 speed to 250
    p.set_can_speed_kbps(bus, 250)

@@ -47,17 +39,12 @@ def test_can_loopback(serial=None):
    assert 0x1aa == sr[0][0] == lb[0][0]
    assert "message" == sr[0][2] == lb[0][2]

-@test_white_and_grey
-@panda_color_to_serial
-def test_safety_nooutput(serial=None):
-  p = connect_wo_esp(serial)
-
+@test_all_pandas
+@panda_connect_and_init
+def test_safety_nooutput(p):
  # enable output mode
  p.set_safety_mode(Panda.SAFETY_NOOUTPUT)

-  # send heartbeat
-  p.send_heartbeat()
-
  # enable CAN loopback mode
  p.set_can_loopback(True)

@@ -69,11 +56,9 @@ def test_safety_nooutput(serial=None):
  r = p.can_recv()
  assert len(r) == 0

-@test_white_and_grey
-@panda_color_to_serial
-def test_reliability(serial=None):
-  p = connect_wo_esp(serial)
-
+@test_all_pandas
+@panda_connect_and_init
+def test_reliability(p):
  LOOP_COUNT = 100
  MSG_COUNT = 100

@@ -82,17 +67,11 @@ def test_reliability(serial=None):
  p.set_can_loopback(True)
  p.set_can_speed_kbps(0, 1000)

-  # send heartbeat
-  p.send_heartbeat()
-
  addrs = range(100, 100+MSG_COUNT)
  ts = [(j, 0, "\xaa"*8, 0) for j in addrs]

  # 100 loops
  for i in range(LOOP_COUNT):
-    # send heartbeat
-    p.send_heartbeat()
-
    st = time.time()

    p.can_send_many(ts)
@@ -115,17 +94,12 @@ def test_reliability(serial=None):
    sys.stdout.write("P")
    sys.stdout.flush()

-@test_white_and_grey
-@panda_color_to_serial
-def test_throughput(serial=None):
-  p = connect_wo_esp(serial)
-
+@test_all_pandas
+@panda_connect_and_init
+def test_throughput(p):
  # enable output mode
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

-  # send heartbeat
-  p.send_heartbeat()
-
  # enable CAN loopback mode
  p.set_can_loopback(True)

@@ -134,9 +108,6 @@ def test_throughput(serial=None):
    p.set_can_speed_kbps(0, speed)
    time.sleep(0.05)

-    # send heartbeat
-    p.send_heartbeat()
-
    comp_kbps = time_many_sends(p, 0)

    # bit count from https://en.wikipedia.org/wiki/CAN_bus
@@ -147,19 +118,15 @@ def test_throughput(serial=None):
    print("loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct))

@test_white_and_grey
-@panda_color_to_serial
-def test_gmlan(serial=None):
-  p = connect_wo_esp(serial)
-
+@panda_type_to_serial
+@panda_connect_and_init
+def test_gmlan(p):
  if p.legacy:
    return

  # enable output mode
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

-  # send heartbeat
-  p.send_heartbeat()
-
  # enable CAN loopback mode
  p.set_can_loopback(True)

@@ -169,9 +136,6 @@ def test_gmlan(serial=None):

  # set gmlan on CAN2
  for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
-    # send heartbeat
-    p.send_heartbeat()
-
    p.set_gmlan(bus)
    comp_kbps_gmlan = time_many_sends(p, 3)
    assert_greater(comp_kbps_gmlan, 0.8 * SPEED_GMLAN)
@@ -185,27 +149,20 @@ def test_gmlan(serial=None):
    print("%d: %.2f kbps vs %.2f kbps" % (bus, comp_kbps_gmlan, comp_kbps_normal))

@test_white_and_grey
-@panda_color_to_serial
-def test_gmlan_bad_toggle(serial=None):
-  p = connect_wo_esp(serial)
-
+@panda_type_to_serial
+@panda_connect_and_init
+def test_gmlan_bad_toggle(p):
  if p.legacy:
    return

  # enable output mode
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

-  # send heartbeat
-  p.send_heartbeat()
-
  # enable CAN loopback mode
  p.set_can_loopback(True)

  # GMLAN_CAN2
  for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
-    # send heartbeat
-    p.send_heartbeat()
-
    p.set_gmlan(bus)
    comp_kbps_gmlan = time_many_sends(p, 3)
    assert_greater(comp_kbps_gmlan, 0.6 * SPEED_GMLAN)
@@ -213,9 +170,6 @@ def test_gmlan_bad_toggle(serial=None):

  # normal
  for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]:
-    # send heartbeat
-    p.send_heartbeat()
-
    p.set_gmlan(None)
    comp_kbps_normal = time_many_sends(p, bus)
    assert_greater(comp_kbps_normal, 0.6 * SPEED_NORMAL)
@@ -223,10 +177,9 @@ def test_gmlan_bad_toggle(serial=None):


 # this will fail if you have hardware serial connected
-@test_white_and_grey
-@panda_color_to_serial
-def test_serial_debug(serial=None):
-  p = connect_wo_esp(serial)
+@test_all_pandas
+@panda_connect_and_init
+def test_serial_debug(p):
  junk = p.serial_read(Panda.SERIAL_DEBUG)
  p.call_control_api(0xc0)
  assert(p.serial_read(Panda.SERIAL_DEBUG).startswith("can "))
@@ -2,46 +2,44 @@ from __future__ import print_function
 import os
 import time
 from panda import Panda
-from helpers import connect_wifi, test_white, test_white_and_grey, panda_color_to_serial
+from helpers import connect_wifi, test_white, test_all_pandas, panda_type_to_serial, panda_connect_and_init
 import requests

-@test_white_and_grey
-@panda_color_to_serial
-def test_get_serial(serial=None):
-  p = Panda(serial)
+@test_all_pandas
+@panda_connect_and_init
+def test_get_serial(p):
  print(p.get_serial())

-@test_white_and_grey
-@panda_color_to_serial
-def test_get_serial_in_flash_mode(serial=None):
-  p = Panda(serial)
+@test_all_pandas
+@panda_connect_and_init
+def test_get_serial_in_flash_mode(p):
  p.reset(enter_bootstub=True)
  assert(p.bootstub)
  print(p.get_serial())
  p.reset()

@test_white
-@panda_color_to_serial
-def test_connect_wifi(serial=None):
-  connect_wifi(serial)
+@panda_type_to_serial
+def test_connect_wifi(serials=None):
+  connect_wifi(serials[0])

@test_white
-@panda_color_to_serial
-def test_flash_wifi(serial=None):
-  connect_wifi(serial)
+@panda_type_to_serial
+def test_flash_wifi(serials=None):
+  connect_wifi(serials[0])
  assert Panda.flash_ota_wifi(release=False), "OTA Wifi Flash Failed"
-  connect_wifi(serial)
+  connect_wifi(serials[0])

@test_white
-@panda_color_to_serial
-def test_wifi_flash_st(serial=None):
-  connect_wifi(serial)
+@panda_type_to_serial
+def test_wifi_flash_st(serials=None):
+  connect_wifi(serials[0])
  assert Panda.flash_ota_st(), "OTA ST Flash Failed"
  connected = False
  st = time.time()
  while not connected and (time.time() - st) < 20:
    try:
-      p = Panda(serial=serial)
+      p = Panda(serial=serials[0])
      p.get_serial()
      connected = True
    except:
@@ -51,9 +49,9 @@ def test_wifi_flash_st(serial=None):
    assert False, "Panda failed to connect on USB after flashing"

@test_white
-@panda_color_to_serial
-def test_webpage_fetch(serial=None):
-  connect_wifi(serial)
+@panda_type_to_serial
+def test_webpage_fetch(serials=None):
+  connect_wifi(serials[0])
  r = requests.get("http://192.168.0.10/")
  print(r.text)

@@ -1,38 +1,32 @@
 from __future__ import print_function
 import time
 from panda import Panda
-from helpers import time_many_sends, connect_wifi, test_white, panda_color_to_serial
+from helpers import time_many_sends, connect_wifi, test_white, panda_type_to_serial
 from nose.tools import timed, assert_equal, assert_less, assert_greater

@test_white
-@panda_color_to_serial
-def test_get_serial_wifi(serial=None):
-  connect_wifi(serial)
+@panda_type_to_serial
+def test_get_serial_wifi(serials=None):
+  connect_wifi(serials[0])

  p = Panda("WIFI")
  print(p.get_serial())

@test_white
-@panda_color_to_serial
-def test_throughput(serial=None):
-  connect_wifi(serial)
-  p = Panda(serial)
+@panda_type_to_serial
+def test_throughput(serials=None):
+  connect_wifi(serials[0])
+  p = Panda(serials[0])

  # enable output mode
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

-  # send heartbeat
-  p.send_heartbeat()
-
  # enable CAN loopback mode
  p.set_can_loopback(True)

  p = Panda("WIFI")

  for speed in [100,250,500,750,1000]:
-    # send heartbeat
-    p.send_heartbeat()
-
    # set bus 0 speed to speed
    p.set_can_speed_kbps(0, speed)
    time.sleep(0.1)
@@ -47,23 +41,17 @@ def test_throughput(serial=None):
    print("WIFI loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct))

@test_white
-@panda_color_to_serial
-def test_recv_only(serial=None):
-  connect_wifi(serial)
-  p = Panda(serial)
+@panda_type_to_serial
+def test_recv_only(serials=None):
+  connect_wifi(serials[0])
+  p = Panda(serials[0])
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

-  # send heartbeat
-  p.send_heartbeat()
-
  p.set_can_loopback(True)
  pwifi = Panda("WIFI")

  # TODO: msg_count=1000 drops packets, is this fixable?
  for msg_count in [10,100,200]:
-    # send heartbeat
-    p.send_heartbeat()
-
    speed = 500
    p.set_can_speed_kbps(0, speed)
    comp_kbps = time_many_sends(p, 0, pwifi, msg_count)
@@ -1,16 +1,16 @@
 from __future__ import print_function
 import sys
 import time
-from helpers import time_many_sends, connect_wifi, test_white, panda_color_to_serial
+from helpers import time_many_sends, connect_wifi, test_white, panda_type_to_serial
 from panda import Panda, PandaWifiStreaming
 from nose.tools import timed, assert_equal, assert_less, assert_greater

@test_white
-@panda_color_to_serial
-def test_udp_doesnt_drop(serial=None):
-  connect_wifi(serial)
+@panda_type_to_serial
+def test_udp_doesnt_drop(serials=None):
+  connect_wifi(serials[0])

-  p = Panda(serial)
+  p = Panda(serials[0])
  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
  p.set_can_loopback(True)

@@ -1,21 +1,18 @@
 from __future__ import print_function
+import os
 import time
+import random
 from panda import Panda
 from nose.tools import assert_equal, assert_less, assert_greater
-from helpers import time_many_sends, test_two_panda, panda_color_to_serial
+from helpers import time_many_sends, test_two_panda, test_two_black_panda, panda_type_to_serial, clear_can_buffers, panda_connect_and_init

@test_two_panda
-@panda_color_to_serial
-def test_send_recv(serial_sender=None, serial_reciever=None):
-  p_send = Panda(serial_sender)
-  p_recv = Panda(serial_reciever)
-
+@panda_type_to_serial
+@panda_connect_and_init
+def test_send_recv(p_send, p_recv):
  p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  p_recv.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
  p_send.set_can_loopback(False)
-
-  # send heartbeat
-  p_send.send_heartbeat()
-
  p_recv.set_can_loopback(False)

  assert not p_send.legacy
@@ -30,9 +27,6 @@ def test_send_recv(serial_sender=None, serial_reciever=None):

  for bus in busses:
    for speed in [100, 250, 500, 750, 1000]:
-      # send heartbeat
-      p_send.send_heartbeat()
-
      p_send.set_can_speed_kbps(bus, speed)
      p_recv.set_can_speed_kbps(bus, speed)
      time.sleep(0.05)
@@ -46,18 +40,12 @@ def test_send_recv(serial_sender=None, serial_reciever=None):
      print("two pandas bus {}, 100 messages at speed {:4d}, comp speed is {:7.2f}, percent {:6.2f}".format(bus, speed, comp_kbps, saturation_pct))

@test_two_panda
-@panda_color_to_serial
-def test_latency(serial_sender=None, serial_reciever=None):
-  p_send = Panda(serial_sender)
-  p_recv = Panda(serial_reciever)
-
-  # send heartbeat
-  p_send.send_heartbeat()
-  p_recv.send_heartbeat()
-
+@panda_type_to_serial
+@panda_connect_and_init
+def test_latency(p_send, p_recv):
  p_send.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  p_recv.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
  p_send.set_can_loopback(False)
-
  p_recv.set_can_loopback(False)

  assert not p_send.legacy
@@ -72,29 +60,17 @@ def test_latency(serial_sender=None, serial_reciever=None):
  p_recv.can_recv()
  p_send.can_recv()

-  # send heartbeat
-  p_send.send_heartbeat()  
-  p_recv.send_heartbeat()
-
  busses = [0,1,2]

  for bus in busses:
    for speed in [100, 250, 500, 750, 1000]:
-      # send heartbeat
-      p_send.send_heartbeat() 
-      p_recv.send_heartbeat()
-
      p_send.set_can_speed_kbps(bus, speed)
      p_recv.set_can_speed_kbps(bus, speed)
      time.sleep(0.1)
+
      #clear can buffers
-      r = [1]
-      while len(r) > 0:
-        r = p_send.can_recv()
-      r = [1]
-      while len(r) > 0:
-        r = p_recv.can_recv()
-      time.sleep(0.05)
+      clear_can_buffers(p_send)
+      clear_can_buffers(p_recv)

      latencies = []
      comp_kbps_list = []
@@ -137,3 +113,83 @@ def test_latency(serial_sender=None, serial_reciever=None):

      print("two pandas bus {}, {} message average at speed {:4d}, latency is {:5.3f}ms, comp speed is {:7.2f}, percent {:6.2f}"\
            .format(bus, num_messages, speed, average_latency, average_comp_kbps, average_saturation_pct))
+
+@test_two_black_panda
+@panda_type_to_serial
+@panda_connect_and_init
+def test_black_loopback(panda0, panda1):
+  # disable safety modes
+  panda0.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  panda1.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # disable loopback
+  panda0.set_can_loopback(False)
+  panda1.set_can_loopback(False)
+
+  # clear stuff
+  panda0.can_send_many([(0x1ba, 0, "testmsg", 0)]*10)
+  time.sleep(0.05)
+  panda0.can_recv()
+  panda1.can_recv()
+
+  # test array (send bus, sender obd, reciever obd, expected busses)
+  test_array = [
+    (0, False, False, [0]),
+    (1, False, False, [1]),
+    (2, False, False, [2]),
+    (0, False, True, [0, 1]),
+    (1, False, True, []),
+    (2, False, True, [2]),
+    (0, True, False, [0]),
+    (1, True, False, [0]),
+    (2, True, False, [2]),
+    (0, True, True, [0, 1]),
+    (1, True, True, [0, 1]),
+    (2, True, True, [2])
+  ]
+
+  # test functions
+  def get_test_string():
+      return b"test"+os.urandom(10)
+
+  def _test_buses(send_panda, recv_panda, _test_array):
+    for send_bus, send_obd, recv_obd, recv_buses in _test_array:
+      print("\nSend bus:", send_bus, " Send OBD:", send_obd, " Recv OBD:", recv_obd)
+      
+      # set OBD on pandas
+      send_panda.set_gmlan(True if send_obd else None)
+      recv_panda.set_gmlan(True if recv_obd else None)
+
+      # clear buffers
+      clear_can_buffers(send_panda)
+      clear_can_buffers(recv_panda)
+
+      # send the characters
+      at = random.randint(1, 2000)
+      st = get_test_string()[0:8]
+      send_panda.can_send(at, st, send_bus)
+      time.sleep(0.1)
+
+      # check for receive
+      cans_echo = send_panda.can_recv()
+      cans_loop = recv_panda.can_recv()
+
+      loop_buses = []
+      for loop in cans_loop:
+        print("  Loop on bus", str(loop[3]))
+        loop_buses.append(loop[3])
+      if len(cans_loop) == 0:
+        print("  No loop")
+      
+      # test loop buses
+      recv_buses.sort()
+      loop_buses.sort()
+      assert recv_buses == loop_buses
+      print("  TEST PASSED")
+    print("\n")
+
+  # test both orientations
+  print("***************** TESTING (0 --> 1) *****************")
+  _test_buses(panda0, panda1, test_array)
+  print("***************** TESTING (1 --> 0) *****************")
+  _test_buses(panda1, panda0, test_array)
@@ -4,43 +4,41 @@ import time
 import random
 import subprocess
 import requests
+import thread
 from functools import wraps
 from panda import Panda
 from nose.tools import timed, assert_equal, assert_less, assert_greater
 from parameterized import parameterized, param

-test_white_and_grey = parameterized([param(panda_color="White"),
-                                     param(panda_color="Grey")])
-test_white = parameterized([param(panda_color="White")])
-test_grey = parameterized([param(panda_color="Grey")])
-test_two_panda = parameterized([param(panda_color=["Grey", "White"]),
-                                param(panda_color=["White", "Grey"])])
+SPEED_NORMAL = 500
+SPEED_GMLAN = 33.3

-_serials = {}
-def get_panda_serial(is_grey=None):
-  global _serials
-  if is_grey not in _serials:
-    for serial in Panda.list():
-      p = Panda(serial=serial)
-      if is_grey is None or p.is_grey() == is_grey:
-        _serials[is_grey] = serial
-        return serial
-    raise IOError("Panda not found. is_grey: {}".format(is_grey))
-  else:
-    return _serials[is_grey]
-
-def connect_wo_esp(serial=None):
-  # connect to the panda
-  p = Panda(serial=serial)
-
-  # power down the ESP
-  p.set_esp_power(False)
-
-  # clear old junk
-  while len(p.can_recv()) > 0:
-    pass
-
-  return p
+test_all_types = parameterized([
+    param(panda_type=Panda.HW_TYPE_WHITE_PANDA),
+    param(panda_type=Panda.HW_TYPE_GREY_PANDA),
+    param(panda_type=Panda.HW_TYPE_BLACK_PANDA)
+  ])
+test_all_pandas = parameterized(
+    Panda.list()
+  )
+test_white_and_grey = parameterized([
+    param(panda_type=Panda.HW_TYPE_WHITE_PANDA),
+    param(panda_type=Panda.HW_TYPE_GREY_PANDA)
+  ])
+test_white = parameterized([
+    param(panda_type=Panda.HW_TYPE_WHITE_PANDA)
+  ])
+test_grey = parameterized([
+    param(panda_type=Panda.HW_TYPE_GREY_PANDA)
+  ])
+test_two_panda = parameterized([
+    param(panda_type=[Panda.HW_TYPE_GREY_PANDA, Panda.HW_TYPE_WHITE_PANDA]),
+    param(panda_type=[Panda.HW_TYPE_WHITE_PANDA, Panda.HW_TYPE_GREY_PANDA]),
+    param(panda_type=[Panda.HW_TYPE_BLACK_PANDA, Panda.HW_TYPE_BLACK_PANDA])
+  ])
+test_two_black_panda = parameterized([
+    param(panda_type=[Panda.HW_TYPE_BLACK_PANDA, Panda.HW_TYPE_BLACK_PANDA])
+  ])

 def connect_wifi(serial=None):
  p = Panda(serial=serial)
@@ -170,23 +168,93 @@ def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None, two_pandas=F

  return comp_kbps

-
-def panda_color_to_serial(fn):
+_panda_serials = None
+def panda_type_to_serial(fn):
  @wraps(fn)
-  def wrapper(panda_color=None, **kwargs):
-    pandas_is_grey = []
-    if panda_color is not None:
-      if not isinstance(panda_color, list):
-        panda_color = [panda_color]
-      panda_color = [s.lower() for s in panda_color]
-    for p in panda_color:
-      if p is None:
-        pandas_is_grey.append(None)
-      elif p in ["grey", "gray"]:
-        pandas_is_grey.append(True)
-      elif p in ["white"]:
-        pandas_is_grey.append(False)
-      else:
-        raise ValueError("Invalid Panda Color {}".format(p))
-    return fn(*[get_panda_serial(is_grey) for is_grey in pandas_is_grey], **kwargs)
+  def wrapper(panda_type=None, **kwargs):
+    # Change panda_types to a list
+    if panda_type is not None:
+      if not isinstance(panda_type, list):
+        panda_type = [panda_type]
+    
+    # If not done already, get panda serials and their type
+    global _panda_serials
+    if _panda_serials == None:
+      _panda_serials = []
+      for serial in Panda.list():
+        p = Panda(serial=serial)
+        _panda_serials.append((serial, p.get_type()))
+        p.close()
+    
+    # Find a panda with the correct types and add the corresponding serial
+    serials = []
+    for p_type in panda_type:
+      found = False
+      for serial, pt in _panda_serials:
+        # Never take the same panda twice
+        if (pt == p_type) and (serial not in serials):
+          serials.append(serial)
+          found = True
+          break
+      if not found:
+        raise IOError("No unused panda found for type: {}".format(p_type))
+    return fn(serials, **kwargs)
  return wrapper
+
+def heartbeat_thread(p):
+  while True:
+    try:
+      p.send_heartbeat()
+      time.sleep(1)
+    except:
+      break
+
+def panda_connect_and_init(fn):
+  @wraps(fn)
+  def wrapper(panda_serials=None, **kwargs):
+    # Change panda_serials to a list
+    if panda_serials is not None:
+      if not isinstance(panda_serials, list):
+        panda_serials = [panda_serials]
+    
+    # Connect to pandas
+    pandas = []
+    for panda_serial in panda_serials:
+      pandas.append(Panda(serial=panda_serial))
+
+    # Initialize pandas
+    for panda in pandas:
+      panda.set_can_loopback(False)
+      panda.set_gmlan(None)
+      panda.set_esp_power(False)
+      for bus, speed in [(0, SPEED_NORMAL), (1, SPEED_NORMAL), (2, SPEED_NORMAL), (3, SPEED_GMLAN)]:
+        panda.set_can_speed_kbps(bus, speed)
+      clear_can_buffers(panda)
+      thread.start_new_thread(heartbeat_thread, (panda,))
+
+    # Run test function
+    ret = fn(*pandas, **kwargs)
+
+    # Close all connections
+    for panda in pandas:
+      panda.close()
+
+    # Return test function result
+    return ret
+  return wrapper
+
+def clear_can_buffers(panda):
+  # clear tx buffers
+  for i in range(4):
+    panda.can_clear(i)
+  
+  # clear rx buffers
+  panda.can_clear(0xFFFF)
+  r = [1]
+  st = time.time()
+  while len(r) > 0:
+    r = panda.can_recv()
+    time.sleep(0.05)
+    if (time.time() - st) > 10:
+      print("Unable to clear can buffers for panda ", panda.get_serial())
+      assert False
@@ -1,6 +1,6 @@
 #!/usr/bin/env python

-# Loopback test between black panda (+ harness and power) and white/grey panda
+# Loopback test between two black pandas (+ harness and power)
 # Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test.
 # To be sure, the test should be run with both harness orientations

@@ -33,81 +33,70 @@ def run_test(sleep_duration):
  pandas[0] = Panda(pandas[0])
  pandas[1] = Panda(pandas[1])

-  # find out which one is black
+  # find out the hardware types
  type0 = pandas[0].get_type()
  type1 = pandas[1].get_type()
-
-  black_panda = None
-  other_panda = None
  
-  if type0 == "\x03" and type1 != "\x03":
-    black_panda = pandas[0]
-    other_panda = pandas[1]
-  elif type0 != "\x03" and type1 == "\x03":
-    black_panda = pandas[1]
-    other_panda = pandas[0]
-  else:
-    print("Connect white/grey and black panda to run this test!")
+  if type0 != "\x03" or type1 != "\x03":
+    print("Connect two black pandas to run this test!")
    assert False

-  # disable safety modes
-  black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
-  other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  for panda in pandas:
+    # disable safety modes
+    panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)

-  # test health packet
-  print("black panda health", black_panda.health())
-  print("other panda health", other_panda.health())
+    # test health packet
+    print("panda health", panda.health())

-  # test black -> other
-  test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration)
-  test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration)
+  # setup test array (send bus, sender obd, reciever obd, expected busses)
+  test_array = [
+    (0, False, False, [0]),
+    (1, False, False, [1]),
+    (2, False, False, [2]),
+    (0, False, True, [0, 1]),
+    (1, False, True, []),
+    (2, False, True, [2]),
+    (0, True, False, [0]),
+    (1, True, False, [0]),
+    (2, True, False, [2]),
+    (0, True, True, [0, 1]),
+    (1, True, True, [0, 1]),
+    (2, True, True, [2])
+  ]
+
+  # test both orientations
+  print("***************** TESTING (0 --> 1) *****************")
+  test_buses(pandas[0], pandas[1], test_array, sleep_duration)
+  print("***************** TESTING (1 --> 0) *****************")
+  test_buses(pandas[1], pandas[0], test_array, sleep_duration)
 	

-def test_buses(black_panda, other_panda, direction, test_array, sleep_duration):
-  if direction:
-    print("***************** TESTING (BLACK --> OTHER) *****************")
-  else:
-    print("***************** TESTING (OTHER --> BLACK) *****************")
-
-  for send_bus, obd, recv_buses in test_array:
-    black_panda.send_heartbeat()
-    other_panda.send_heartbeat()
-    print("\ntest can: ", send_bus, " OBD: ", obd)
+def test_buses(send_panda, recv_panda, test_array, sleep_duration):
+  for send_bus, send_obd, recv_obd, recv_buses in test_array:
+    send_panda.send_heartbeat()
+    recv_panda.send_heartbeat()
+    print("\nSend bus:", send_bus, " Send OBD:", send_obd, " Recv OBD:", recv_obd)
    
-    # set OBD on black panda
-    black_panda.set_gmlan(True if obd else None)
+    # set OBD on pandas
+    send_panda.set_gmlan(True if send_obd else None)
+    recv_panda.set_gmlan(True if recv_obd else None)

    # clear and flush
-    if direction:
-      black_panda.can_clear(send_bus)
-    else:
-      other_panda.can_clear(send_bus)
-
+    send_panda.can_clear(send_bus)
    for recv_bus in recv_buses:
-      if direction:
-        other_panda.can_clear(recv_bus)
-      else:
-	black_panda.can_clear(recv_bus)
-    
-    black_panda.can_recv()
-    other_panda.can_recv()
+      recv_panda.can_clear(recv_bus)
+    send_panda.can_recv()
+    recv_panda.can_recv()

    # send the characters
    at = random.randint(1, 2000)
    st = get_test_string()[0:8]
-    if direction:
-      black_panda.can_send(at, st, send_bus)
-    else:
-      other_panda.can_send(at, st, send_bus)
+    send_panda.can_send(at, st, send_bus)
    time.sleep(0.1)

    # check for receive
-    if direction:
-      cans_echo = black_panda.can_recv()
-      cans_loop = other_panda.can_recv()
-    else:
-      cans_echo = other_panda.can_recv()
-      cans_loop = black_panda.can_recv()
+    cans_echo = send_panda.can_recv()
+    cans_loop = recv_panda.can_recv()

    loop_buses = []
    for loop in cans_loop:
@@ -0,0 +1,169 @@
+#!/usr/bin/env python
+
+# Loopback test between black panda (+ harness and power) and white/grey panda
+# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test.
+# To be sure, the test should be run with both harness orientations
+
+from __future__ import print_function
+import os
+import sys
+import time
+import random
+import argparse
+
+from hexdump import hexdump
+from itertools import permutations
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+
+def get_test_string():
+  return b"test"+os.urandom(10)
+
+counter = 0
+nonzero_bus_errors = 0
+zero_bus_errors = 0
+content_errors = 0
+
+def run_test(sleep_duration):
+  global counter, nonzero_bus_errors, zero_bus_errors, content_errors
+
+  pandas = Panda.list()
+  print(pandas)
+
+  # make sure two pandas are connected
+  if len(pandas) != 2:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # connect
+  pandas[0] = Panda(pandas[0])
+  pandas[1] = Panda(pandas[1])
+
+  # find out which one is black
+  type0 = pandas[0].get_type()
+  type1 = pandas[1].get_type()
+
+  black_panda = None
+  other_panda = None
+  
+  if type0 == "\x03" and type1 != "\x03":
+    black_panda = pandas[0]
+    other_panda = pandas[1]
+  elif type0 != "\x03" and type1 == "\x03":
+    black_panda = pandas[1]
+    other_panda = pandas[0]
+  else:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # disable safety modes
+  black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # test health packet
+  print("black panda health", black_panda.health())
+  print("other panda health", other_panda.health())
+
+  # test black -> other
+  while True:
+    test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration)
+    test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration)
+    counter += 1
+    print("Number of cycles:", counter, "Non-zero bus errors:", nonzero_bus_errors, "Zero bus errors:", zero_bus_errors, "Content errors:", content_errors)
+    
+    # Toggle relay
+    black_panda.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+    time.sleep(1)
+    black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+    time.sleep(1)
+	
+
+def test_buses(black_panda, other_panda, direction, test_array, sleep_duration):
+  global nonzero_bus_errors, zero_bus_errors, content_errors
+
+  if direction:
+    print("***************** TESTING (BLACK --> OTHER) *****************")
+  else:
+    print("***************** TESTING (OTHER --> BLACK) *****************")
+
+  for send_bus, obd, recv_buses in test_array:
+    black_panda.send_heartbeat()
+    other_panda.send_heartbeat()
+    print("\ntest can: ", send_bus, " OBD: ", obd)
+    
+    # set OBD on black panda
+    black_panda.set_gmlan(True if obd else None)
+
+    # clear and flush
+    if direction:
+      black_panda.can_clear(send_bus)
+    else:
+      other_panda.can_clear(send_bus)
+
+    for recv_bus in recv_buses:
+      if direction:
+        other_panda.can_clear(recv_bus)
+      else:
+	black_panda.can_clear(recv_bus)
+    
+    black_panda.can_recv()
+    other_panda.can_recv()
+
+    # send the characters
+    at = random.randint(1, 2000)
+    st = get_test_string()[0:8]
+    if direction:
+      black_panda.can_send(at, st, send_bus)
+    else:
+      other_panda.can_send(at, st, send_bus)
+    time.sleep(0.1)
+
+    # check for receive
+    if direction:
+      cans_echo = black_panda.can_recv()
+      cans_loop = other_panda.can_recv()
+    else:
+      cans_echo = other_panda.can_recv()
+      cans_loop = black_panda.can_recv()
+
+    loop_buses = []
+    for loop in cans_loop:
+      if (loop[0] != at) or (loop[2] != st):
+	      content_errors += 1
+
+      print("  Loop on bus", str(loop[3]))
+      loop_buses.append(loop[3])
+    if len(cans_loop) == 0:
+      print("  No loop")
+      if not os.getenv("NOASSERT"):
+        assert False
+    
+    # test loop buses
+    recv_buses.sort()
+    loop_buses.sort()
+    if(recv_buses != loop_buses):
+      if len(loop_buses) == 0:
+	      zero_bus_errors += 1
+      else:
+	      nonzero_bus_errors += 1
+      if not os.getenv("NOASSERT"):
+        assert False
+    else:
+      print("  TEST PASSED")
+
+    time.sleep(sleep_duration)
+  print("\n")
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("-n", type=int, help="Number of test iterations to run")
+  parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
+  args = parser.parse_args()
+
+  if args.n is None:
+    while True:
+      run_test(sleep_duration=args.sleep)
+  else:
+    for i in range(args.n):
+      run_test(sleep_duration=args.sleep)
@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+
+# Loopback test between black panda (+ harness and power) and white/grey panda
+# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test.
+# To be sure, the test should be run with both harness orientations
+
+from __future__ import print_function
+import os
+import sys
+import time
+import random
+import argparse
+
+from hexdump import hexdump
+from itertools import permutations
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+
+def get_test_string():
+  return b"test"+os.urandom(10)
+
+counter = 0
+nonzero_bus_errors = 0
+zero_bus_errors = 0
+content_errors = 0
+
+def run_test(sleep_duration):
+  global counter, nonzero_bus_errors, zero_bus_errors, content_errors
+
+  pandas = Panda.list()
+  print(pandas)
+
+  # make sure two pandas are connected
+  if len(pandas) != 2:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # connect
+  pandas[0] = Panda(pandas[0])
+  pandas[1] = Panda(pandas[1])
+
+  # find out which one is black
+  type0 = pandas[0].get_type()
+  type1 = pandas[1].get_type()
+
+  black_panda = None
+  other_panda = None
+  
+  if type0 == "\x03" and type1 != "\x03":
+    black_panda = pandas[0]
+    other_panda = pandas[1]
+  elif type0 != "\x03" and type1 == "\x03":
+    black_panda = pandas[1]
+    other_panda = pandas[0]
+  else:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # disable safety modes
+  black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # test health packet
+  print("black panda health", black_panda.health())
+  print("other panda health", other_panda.health())
+
+  # test black -> other
+  start_time = time.time()
+  temp_start_time = start_time
+  while True:
+    test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration)
+    test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration)
+    counter += 1
+    
+    runtime = time.time() - start_time
+    print("Number of cycles:", counter, "Non-zero bus errors:", nonzero_bus_errors, "Zero bus errors:", zero_bus_errors, "Content errors:", content_errors, "Runtime: ", runtime)
+
+    if (time.time() - temp_start_time) > 3600*6:
+    	# Toggle relay
+    	black_panda.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+    	time.sleep(1)
+	black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+    	time.sleep(1)
+	temp_start_time = time.time()
+	
+
+def test_buses(black_panda, other_panda, direction, test_array, sleep_duration):
+  global nonzero_bus_errors, zero_bus_errors, content_errors
+
+  if direction:
+    print("***************** TESTING (BLACK --> OTHER) *****************")
+  else:
+    print("***************** TESTING (OTHER --> BLACK) *****************")
+
+  for send_bus, obd, recv_buses in test_array:
+    black_panda.send_heartbeat()
+    other_panda.send_heartbeat()
+    print("\ntest can: ", send_bus, " OBD: ", obd)
+    
+    # set OBD on black panda
+    black_panda.set_gmlan(True if obd else None)
+
+    # clear and flush
+    if direction:
+      black_panda.can_clear(send_bus)
+    else:
+      other_panda.can_clear(send_bus)
+
+    for recv_bus in recv_buses:
+      if direction:
+        other_panda.can_clear(recv_bus)
+      else:
+	black_panda.can_clear(recv_bus)
+    
+    black_panda.can_recv()
+    other_panda.can_recv()
+
+    # send the characters
+    at = random.randint(1, 2000)
+    st = get_test_string()[0:8]
+    if direction:
+      black_panda.can_send(at, st, send_bus)
+    else:
+      other_panda.can_send(at, st, send_bus)
+    time.sleep(0.1)
+
+    # check for receive
+    if direction:
+      cans_echo = black_panda.can_recv()
+      cans_loop = other_panda.can_recv()
+    else:
+      cans_echo = other_panda.can_recv()
+      cans_loop = black_panda.can_recv()
+
+    loop_buses = []
+    for loop in cans_loop:
+      if (loop[0] != at) or (loop[2] != st):
+	      content_errors += 1
+
+      print("  Loop on bus", str(loop[3]))
+      loop_buses.append(loop[3])
+    if len(cans_loop) == 0:
+      print("  No loop")
+      if not os.getenv("NOASSERT"):
+        assert False
+    
+    # test loop buses
+    recv_buses.sort()
+    loop_buses.sort()
+    if(recv_buses != loop_buses):
+      if len(loop_buses) == 0:
+	      zero_bus_errors += 1
+      else:
+	      nonzero_bus_errors += 1
+      if not os.getenv("NOASSERT"):
+        assert False
+    else:
+      print("  TEST PASSED")
+
+    time.sleep(sleep_duration)
+  print("\n")
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("-n", type=int, help="Number of test iterations to run")
+  parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
+  args = parser.parse_args()
+
+  if args.n is None:
+    while True:
+      run_test(sleep_duration=args.sleep)
+  else:
+    for i in range(args.n):
+      run_test(sleep_duration=args.sleep)
@@ -0,0 +1,145 @@
+#!/usr/bin/env python
+
+# Relay test with loopback between black panda (+ harness and power) and white/grey panda
+# Tests the relay switching multiple times / second by looking at the buses on which loop occurs.
+
+from __future__ import print_function
+import os
+import sys
+import time
+import random
+import argparse
+
+from hexdump import hexdump
+from itertools import permutations
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+
+def get_test_string():
+  return b"test"+os.urandom(10)
+
+counter = 0
+open_errors = 0
+closed_errors = 0
+content_errors = 0
+
+def run_test(sleep_duration):
+  global counter, open_errors, closed_errors, content_errors
+
+  pandas = Panda.list()
+  #pandas = ["540046000c51363338383037", "07801b800f51363038363036"]
+  print(pandas)
+
+  # make sure two pandas are connected
+  if len(pandas) != 2:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # connect
+  pandas[0] = Panda(pandas[0])
+  pandas[1] = Panda(pandas[1])
+
+  # find out which one is black
+  type0 = pandas[0].get_type()
+  type1 = pandas[1].get_type()
+
+  black_panda = None
+  other_panda = None
+  
+  if type0 == "\x03" and type1 != "\x03":
+    black_panda = pandas[0]
+    other_panda = pandas[1]
+  elif type0 != "\x03" and type1 == "\x03":
+    black_panda = pandas[1]
+    other_panda = pandas[0]
+  else:
+    print("Connect white/grey and black panda to run this test!")
+    assert False
+
+  # disable safety modes
+  black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+
+  # test health packet
+  print("black panda health", black_panda.health())
+  print("other panda health", other_panda.health())
+
+  # test black -> other
+  while True:
+    # Switch on relay
+    black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+    time.sleep(0.05)
+
+    if not test_buses(black_panda, other_panda, (0, False, [0])):
+      open_errors += 1
+      print("Open error")
+      assert False
+
+    # Switch off relay
+    black_panda.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+    time.sleep(0.05)
+
+    if not test_buses(black_panda, other_panda, (0, False, [0, 2])):
+      closed_errors += 1
+      print("Close error")
+      assert False
+
+    counter += 1
+    print("Number of cycles:", counter, "Open errors:", open_errors, "Closed errors:", closed_errors, "Content errors:", content_errors)	
+
+def test_buses(black_panda, other_panda, test_obj):
+  global content_errors
+  send_bus, obd, recv_buses = test_obj
+    
+  black_panda.send_heartbeat()
+  other_panda.send_heartbeat()
+    
+  # Set OBD on send panda
+  other_panda.set_gmlan(True if obd else None)
+
+  # clear and flush
+  other_panda.can_clear(send_bus)
+
+  for recv_bus in recv_buses:
+    black_panda.can_clear(recv_bus)
+    
+  black_panda.can_recv()
+  other_panda.can_recv()
+
+  # send the characters
+  at = random.randint(1, 2000)
+  st = get_test_string()[0:8]
+  other_panda.can_send(at, st, send_bus)
+  time.sleep(0.05)
+
+  # check for receive
+  cans_echo = other_panda.can_recv()
+  cans_loop = black_panda.can_recv()
+
+  loop_buses = []
+  for loop in cans_loop:
+    if (loop[0] != at) or (loop[2] != st):
+      content_errors += 1
+    loop_buses.append(loop[3])
+    
+  # test loop buses
+  recv_buses.sort()
+  loop_buses.sort()
+  if(recv_buses != loop_buses):
+    return False
+  else:
+    return True
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("-n", type=int, help="Number of test iterations to run")
+  parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0)
+  args = parser.parse_args()
+
+  if args.n is None:
+    while True:
+      run_test(sleep_duration=args.sleep)
+  else:
+    for i in range(args.n):
+      run_test(sleep_duration=args.sleep)
@@ -12,33 +12,38 @@ setcolor = ["\033[1;32;40m", "\033[1;31;40m"]
 unsetcolor = "\033[00m"

 if __name__ == "__main__":
-  port_number = int(os.getenv("PORT", 0))
-  claim = os.getenv("CLAIM") is not None
-
-  serials = Panda.list()
-  if os.getenv("SERIAL"):
-    serials = filter(lambda x: x==os.getenv("SERIAL"), serials)
-
-  pandas = list(map(lambda x: Panda(x, claim=claim), serials))
-
-  if not len(pandas):
-    sys.exit("no pandas found")
-
-  if os.getenv("BAUD") is not None:
-    for panda in pandas:
-      panda.set_uart_baud(port_number, int(os.getenv("BAUD")))
-
  while True:
-    for i, panda in enumerate(pandas):
+    try:
+      port_number = int(os.getenv("PORT", 0))
+      claim = os.getenv("CLAIM") is not None
+
+      serials = Panda.list()
+      if os.getenv("SERIAL"):
+        serials = filter(lambda x: x==os.getenv("SERIAL"), serials)
+
+      pandas = list(map(lambda x: Panda(x, claim=claim), serials))
+
+      if not len(pandas):
+        sys.exit("no pandas found")
+
+      if os.getenv("BAUD") is not None:
+        for panda in pandas:
+          panda.set_uart_baud(port_number, int(os.getenv("BAUD")))
+
      while True:
-        ret = panda.serial_read(port_number)
-        if len(ret) > 0:
-          sys.stdout.write(setcolor[i] + str(ret) + unsetcolor)
-          sys.stdout.flush()
-        else:
-          break
-      if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
-        ln = sys.stdin.readline()
-        if claim:
-          panda.serial_write(port_number, ln)
-      time.sleep(0.01)
+        for i, panda in enumerate(pandas):
+          while True:
+      	    ret = panda.serial_read(port_number)
+  	    if len(ret) > 0:
+	      sys.stdout.write(setcolor[i] + str(ret) + unsetcolor)
+	      sys.stdout.flush()
+	    else:
+	      break
+          if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []):
+	    ln = sys.stdin.readline()
+	    if claim:
+	      panda.serial_write(port_number, ln)
+          time.sleep(0.01)
+    except:
+      print("panda disconnected!")
+      time.sleep(0.5);
@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+import time
+from panda import Panda
+	
+if __name__ == "__main__":
+  panda_serials = Panda.list()
+  pandas = []
+  for ps in panda_serials:
+    pandas.append(Panda(serial=ps))
+  if len(pandas) == 0:
+    print("No pandas connected")
+    assert False
+
+  while True:
+    for panda in pandas:
+      print(panda.health())
+    print("\n")
+    time.sleep(0.5)
+
@@ -57,8 +57,9 @@ void set_toyota_rt_torque_last(int t);

 void init_tests_honda(void);
 bool get_honda_moving(void);
-int get_honda_brake_prev(void);
+bool get_honda_brake_pressed_prev(void);
 int get_honda_gas_prev(void);
+void set_honda_fwd_brake(bool);
 void set_honda_alt_brake_msg(bool);
 void set_honda_bosch_hardware(bool);
 int get_honda_bosch_hardware(void);
@@ -227,8 +227,8 @@ bool get_honda_moving(void){
  return honda_moving;
 }

-int get_honda_brake_prev(void){
-  return honda_brake_prev;
+bool get_honda_brake_pressed_prev(void){
+  return honda_brake_pressed_prev;
 }

 int get_honda_gas_prev(void){
@@ -247,6 +247,10 @@ int get_honda_bosch_hardware(void) {
  return honda_bosch_hardware;
 }

+void set_honda_fwd_brake(bool c){
+  honda_fwd_brake = c;
+}
+
 void init_tests(void){
  // get HW_TYPE from env variable set in test.sh
  hw_type = atoi(getenv("HW_TYPE"));
@@ -315,8 +319,9 @@ void init_tests_subaru(void){
 void init_tests_honda(void){
  init_tests();
  honda_moving = false;
-  honda_brake_prev = 0;
+  honda_brake_pressed_prev = false;
  honda_gas_prev = 0;
+  honda_fwd_brake = false;
 }

 void set_gmlan_digital_output(int to_set){
@@ -64,7 +64,7 @@ class TestHondaSafety(unittest.TestCase):
  def _send_brake_msg(self, brake):
    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
    to_send[0].RIR = 0x1FA << 21
-    to_send[0].RDLR = ((brake & 0x3) << 8) | ((brake & 0x3FF) >> 2)
+    to_send[0].RDLR = ((brake & 0x3) << 14) | ((brake & 0x3FF) >> 2)

    return to_send

@@ -112,9 +112,9 @@ class TestHondaSafety(unittest.TestCase):
    self.assertEqual(1, self.safety.get_honda_moving())

  def test_prev_brake(self):
-    self.assertFalse(self.safety.get_honda_brake_prev())
+    self.assertFalse(self.safety.get_honda_brake_pressed_prev())
    self.safety.safety_rx_hook(self._brake_msg(True))
-    self.assertTrue(self.safety.get_honda_brake_prev())
+    self.assertTrue(self.safety.get_honda_brake_pressed_prev())

  def test_disengage_on_brake(self):
    self.safety.set_controls_allowed(1)
@@ -204,17 +204,22 @@ class TestHondaSafety(unittest.TestCase):
    self.safety.set_gas_interceptor_detected(False)

  def test_brake_safety_check(self):
-    for long_controls_allowed in [0, 1]:
-      self.safety.set_long_controls_allowed(long_controls_allowed)
-      for brake in np.arange(0, MAX_BRAKE + 10, 1):
-        for controls_allowed in [True, False]:
-          self.safety.set_controls_allowed(controls_allowed)
-          if controls_allowed and long_controls_allowed:
-            send = MAX_BRAKE >= brake >= 0
-          else:
-            send = brake == 0
-          self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
+    for fwd_brake in [False, True]:
+      self.safety.set_honda_fwd_brake(fwd_brake)
+      for long_controls_allowed in [0, 1]:
+        self.safety.set_long_controls_allowed(long_controls_allowed)
+        for brake in np.arange(0, MAX_BRAKE + 10, 1):
+          for controls_allowed in [True, False]:
+            self.safety.set_controls_allowed(controls_allowed)
+            if fwd_brake:
+              send = False  # block openpilot brake msg when fwd'ing stock msg
+            elif controls_allowed and long_controls_allowed:
+              send = MAX_BRAKE >= brake >= 0
+            else:
+              send = brake == 0
+            self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
    self.safety.set_long_controls_allowed(True)
+    self.safety.set_honda_fwd_brake(False)

  def test_gas_interceptor_safety_check(self):
    for long_controls_allowed in [0, 1]:
@@ -252,27 +257,33 @@ class TestHondaSafety(unittest.TestCase):
    buss = range(0x0, 0x3)
    msgs = range(0x1, 0x800)
    long_controls_allowed = [0, 1]
+    fwd_brake = [False, True]

    self.safety.set_honda_bosch_hardware(0)

-    for l in long_controls_allowed:
-      self.safety.set_long_controls_allowed(l)
-      blocked_msgs = [0xE4, 0x194, 0x33D]
-      if l:
-        blocked_msgs += [0x1FA ,0x30C, 0x39F]
-      for b in buss:
-        for m in msgs:
-          if b == 0:
-            fwd_bus = 2
-          elif b == 1:
-            fwd_bus = -1
-          elif b == 2:
-            fwd_bus = -1 if m in blocked_msgs else 0
+    for f in fwd_brake:
+      self.safety.set_honda_fwd_brake(f)
+      for l in long_controls_allowed:
+        self.safety.set_long_controls_allowed(l)
+        blocked_msgs = [0xE4, 0x194, 0x33D]
+        if l:
+          blocked_msgs += [0x30C, 0x39F]
+          if not f:
+            blocked_msgs += [0x1FA]
+        for b in buss:
+          for m in msgs:
+            if b == 0:
+              fwd_bus = 2
+            elif b == 1:
+              fwd_bus = -1
+            elif b == 2:
+              fwd_bus = -1 if m in blocked_msgs else 0

-          # assume len 8
-          self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+            # assume len 8
+            self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))

    self.safety.set_long_controls_allowed(True)
+    self.safety.set_honda_fwd_brake(False)