diff --git a/panda/.circleci/config.yml b/panda/.circleci/config.yml
index 58fc1d13bc..a4ae26efe5 100644
--- a/panda/.circleci/config.yml
+++ b/panda/.circleci/config.yml
@@ -64,7 +64,7 @@ jobs:
       - run:
           name: Build ESP image
           command: |
-            docker run panda_build /bin/bash -c "cd /panda/boardesp; make user1.bin"
+            docker run panda_build /bin/bash -c "cd /panda/boardesp; ./get_sdk.sh; make user1.bin"
 
   safety_replay:
     machine:
diff --git a/panda/Dockerfile b/panda/Dockerfile
index a029a5ffa4..fa020ad059 100644
--- a/panda/Dockerfile
+++ b/panda/Dockerfile
@@ -71,6 +71,8 @@ ENV PYTHONPATH /tmp:$PYTHONPATH
 COPY ./boardesp/get_sdk_ci.sh /tmp/panda/boardesp/
 COPY ./boardesp/python2_make.py /tmp/panda/boardesp/
 
+COPY ./panda_jungle /tmp/panda_jungle
+
 RUN useradd --system -s /sbin/nologin pandauser
 RUN mkdir -p /tmp/panda/boardesp/esp-open-sdk
 RUN chown pandauser /tmp/panda/boardesp/esp-open-sdk
diff --git a/panda/Jenkinsfile b/panda/Jenkinsfile
index 204b5beb3d..61068a4865 100644
--- a/panda/Jenkinsfile
+++ b/panda/Jenkinsfile
@@ -14,6 +14,11 @@ pipeline {
       steps {
         timeout(time: 60, unit: 'MINUTES') {
           script {
+            try {
+              sh 'cp -R /home/batman/panda_jungle .'
+            } catch (err) {
+              echo "Folder already exists"
+            }
             sh 'git archive -v -o panda.tar.gz --format=tar.gz HEAD'
             dockerImage = docker.build("${env.DOCKER_IMAGE_TAG}")
           }
diff --git a/panda/README.md b/panda/README.md
index 634db4c120..ea0df2a19c 100644
--- a/panda/README.md
+++ b/panda/README.md
@@ -87,7 +87,7 @@ To print out the serial console from the ESP8266, run PORT=1 tests/debug_console
 Safety Model
 ------
 
-When a panda powers up, by default it's in `SAFETY_NOOUTPUT` mode. While in no output mode, the buses are also forced to be silent. In order to send messages, you have to select a safety mode. Currently, setting safety modes is only supported over USB.
+When a panda powers up, by default it's in `SAFETY_SILENT` mode. While in `SAFETY_SILENT` mode, the buses are also forced to be silent. In order to send messages, you have to select a safety mode. Currently, setting safety modes is only supported over USB.
 
 Safety modes optionally supports `controls_allowed`, which allows or blocks a subset of messages based on a customizable state in the board.
 
diff --git a/panda/VERSION b/panda/VERSION
index 4279ff22f4..538a65cc82 100644
--- a/panda/VERSION
+++ b/panda/VERSION
@@ -1 +1 @@
-v1.5.9
\ No newline at end of file
+v1.6.9
\ No newline at end of file
diff --git a/panda/board/board.h b/panda/board/board.h
index 5629a841d3..30e1fa4d17 100644
--- a/panda/board/board.h
+++ b/panda/board/board.h
@@ -19,7 +19,9 @@
 
 void detect_board_type(void) {
   #ifdef PANDA
-    // SPI lines floating: white (TODO: is this reliable?)
+    // SPI lines floating: white (TODO: is this reliable? Not really, we have to enable ESP/GPS to be able to detect this on the UART)
+    set_gpio_output(GPIOC, 14, 1);
+    set_gpio_output(GPIOC, 5, 1);
     if((detect_with_pull(GPIOA, 4, PULL_DOWN)) || (detect_with_pull(GPIOA, 5, PULL_DOWN)) || (detect_with_pull(GPIOA, 6, PULL_DOWN)) || (detect_with_pull(GPIOA, 7, PULL_DOWN))){
       hw_type = HW_TYPE_WHITE_PANDA;
       current_board = &board_white;
diff --git a/panda/board/board_declarations.h b/panda/board/board_declarations.h
index 2fd3976a0d..d973551baf 100644
--- a/panda/board/board_declarations.h
+++ b/panda/board/board_declarations.h
@@ -6,11 +6,12 @@ typedef void (*board_set_led)(uint8_t color, bool enabled);
 typedef void (*board_set_usb_power_mode)(uint8_t mode);
 typedef void (*board_set_esp_gps_mode)(uint8_t mode);
 typedef void (*board_set_can_mode)(uint8_t mode);
-typedef void (*board_usb_power_mode_tick)(uint64_t tcnt);
+typedef void (*board_usb_power_mode_tick)(uint32_t uptime);
 typedef bool (*board_check_ignition)(void);
 typedef uint32_t (*board_read_current)(void);
 typedef void (*board_set_ir_power)(uint8_t percentage);
 typedef void (*board_set_fan_power)(uint8_t percentage);
+typedef void (*board_set_phone_power)(bool enabled);
 
 struct board {
   const char *board_type;
@@ -27,6 +28,7 @@ struct board {
   board_read_current read_current;
   board_set_ir_power set_ir_power;
   board_set_fan_power set_fan_power;
+  board_set_phone_power set_phone_power;
 };
 
 // ******************* Definitions ********************
diff --git a/panda/board/boards/black.h b/panda/board/boards/black.h
index f033e82b23..7165aa6cbb 100644
--- a/panda/board/boards/black.h
+++ b/panda/board/boards/black.h
@@ -123,8 +123,8 @@ void black_set_can_mode(uint8_t mode){
   }
 }
 
-void black_usb_power_mode_tick(uint64_t tcnt){
-  UNUSED(tcnt);
+void black_usb_power_mode_tick(uint32_t uptime){
+  UNUSED(uptime);
   // Not applicable
 }
 
@@ -146,6 +146,10 @@ void black_set_fan_power(uint8_t percentage){
   UNUSED(percentage);
 }
 
+void black_set_phone_power(bool enabled){
+  UNUSED(enabled);
+}
+
 void black_init(void) {
   common_init_gpio();
 
@@ -158,6 +162,9 @@ void black_init(void) {
   set_gpio_mode(GPIOC, 0, MODE_ANALOG);
   set_gpio_mode(GPIOC, 3, MODE_ANALOG);
 
+  // Set default state of GPS
+  current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+
   // C10: OBD_SBU1_RELAY (harness relay driving output)
   // C11: OBD_SBU2_RELAY (harness relay driving output)
   set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
@@ -227,5 +234,6 @@ const board board_black = {
   .check_ignition = black_check_ignition,
   .read_current = black_read_current,
   .set_fan_power = black_set_fan_power,
-  .set_ir_power = black_set_ir_power
+  .set_ir_power = black_set_ir_power,
+  .set_phone_power = black_set_phone_power
 };
diff --git a/panda/board/boards/common.h b/panda/board/boards/common.h
index e33b2a2f04..d0a4908763 100644
--- a/panda/board/boards/common.h
+++ b/panda/board/boards/common.h
@@ -61,7 +61,7 @@ void peripherals_init(void){
   RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;  // pedal and fan PWM
   RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;  // gmlan_alt and IR PWM
   //RCC->APB1ENR |= RCC_APB1ENR_TIM5EN;
-  //RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;
+  RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;  // interrupt timer
   RCC->APB1ENR |= RCC_APB1ENR_PWREN;   // for RTC config
   RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
   RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
diff --git a/panda/board/boards/grey.h b/panda/board/boards/grey.h
index 1a39bce07f..3dcb730a58 100644
--- a/panda/board/boards/grey.h
+++ b/panda/board/boards/grey.h
@@ -3,10 +3,18 @@
 // ////////// //
 
 // Most hardware functionality is similar to white panda
+
+void grey_init(void) {
+  white_grey_common_init();
+
+  // Set default state of GPS
+  current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+}
+
 const board board_grey = {
   .board_type = "Grey",
   .harness_config = &white_harness_config,
-  .init = white_init,
+  .init = grey_init,
   .enable_can_transciever = white_enable_can_transciever,
   .enable_can_transcievers = white_enable_can_transcievers,
   .set_led = white_set_led,
@@ -17,5 +25,6 @@ const board board_grey = {
   .check_ignition = white_check_ignition,
   .read_current = white_read_current,
   .set_fan_power = white_set_fan_power,
-  .set_ir_power = white_set_ir_power
+  .set_ir_power = white_set_ir_power,
+  .set_phone_power = white_set_phone_power
 };
\ No newline at end of file
diff --git a/panda/board/boards/pedal.h b/panda/board/boards/pedal.h
index 02612d3f09..c67d39151d 100644
--- a/panda/board/boards/pedal.h
+++ b/panda/board/boards/pedal.h
@@ -50,8 +50,8 @@ void pedal_set_can_mode(uint8_t mode){
   }
 }
 
-void pedal_usb_power_mode_tick(uint64_t tcnt){
-  UNUSED(tcnt);
+void pedal_usb_power_mode_tick(uint32_t uptime){
+  UNUSED(uptime);
   // Not applicable
 }
 
@@ -73,6 +73,10 @@ void pedal_set_fan_power(uint8_t percentage){
   UNUSED(percentage);
 }
 
+void pedal_set_phone_power(bool enabled){
+  UNUSED(enabled);
+}
+
 void pedal_init(void) {
   common_init_gpio();
 
@@ -108,5 +112,6 @@ const board board_pedal = {
   .check_ignition = pedal_check_ignition,
   .read_current = pedal_read_current,
   .set_fan_power = pedal_set_fan_power,
-  .set_ir_power = pedal_set_ir_power
+  .set_ir_power = pedal_set_ir_power,
+  .set_phone_power = pedal_set_phone_power
 };
\ No newline at end of file
diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h
index f5765c226f..1545a3f2db 100644
--- a/panda/board/boards/uno.h
+++ b/panda/board/boards/uno.h
@@ -1,6 +1,8 @@
 // ///////////// //
 // Uno + Harness //
 // ///////////// //
+#define BOOTKICK_TIME 3U
+uint8_t bootkick_timer = 0U;
 
 void uno_enable_can_transciever(uint8_t transciever, bool enabled) {
   switch (transciever){
@@ -48,9 +50,38 @@ void uno_set_gps_load_switch(bool enabled) {
   set_gpio_output(GPIOC, 12, enabled);
 }
 
+void uno_set_bootkick(bool enabled){
+  set_gpio_output(GPIOB, 14, !enabled);
+}
+
+void uno_bootkick(void) {
+  bootkick_timer = BOOTKICK_TIME;
+  uno_set_bootkick(true);
+}
+
+void uno_set_phone_power(bool enabled){
+  set_gpio_output(GPIOB, 4, enabled);
+}
+
 void uno_set_usb_power_mode(uint8_t mode) {
-  UNUSED(mode);
-  puts("Setting USB mode makes no sense on UNO\n");
+  bool valid = false;
+  switch (mode) {
+    case USB_POWER_CLIENT:
+      uno_set_phone_power(false);
+      valid = true;
+      break;
+    case USB_POWER_CDP:
+      uno_set_phone_power(true);
+      uno_bootkick();
+      valid = true;
+      break;
+    default:
+      puts("Invalid USB power mode\n");
+      break;
+  }
+  if (valid) {
+    usb_power_mode = mode;
+  }
 }
 
 void uno_set_esp_gps_mode(uint8_t mode) {
@@ -106,12 +137,11 @@ void uno_set_can_mode(uint8_t mode){
   }
 }
 
-void uno_set_bootkick(bool enabled){
-  set_gpio_output(GPIOB, 14, !enabled);
-}
-
-void uno_usb_power_mode_tick(uint64_t tcnt){
-  if(tcnt == 3U){
+void uno_usb_power_mode_tick(uint32_t uptime){
+  UNUSED(uptime);
+  if(bootkick_timer != 0U){
+    bootkick_timer--;
+  } else {
     uno_set_bootkick(false);
   }
 }
@@ -152,6 +182,9 @@ void uno_init(void) {
   set_gpio_mode(GPIOC, 0, MODE_ANALOG);
   set_gpio_mode(GPIOC, 3, MODE_ANALOG);
 
+  // Set default state of GPS
+  current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+
   // C10: OBD_SBU1_RELAY (harness relay driving output)
   // C11: OBD_SBU2_RELAY (harness relay driving output)
   set_gpio_mode(GPIOC, 10, MODE_OUTPUT);
@@ -168,7 +201,7 @@ void uno_init(void) {
   uno_set_gps_load_switch(true);
 
   // Turn on phone regulator
-  set_gpio_output(GPIOB, 4, 1);
+  uno_set_phone_power(true);
 
   // Initialize IR PWM and set to 0%
   set_gpio_alternate(GPIOB, 7, GPIO_AF2_TIM4);
@@ -212,7 +245,7 @@ void uno_init(void) {
   }
 
   // Bootkick phone
-  uno_set_bootkick(true);
+  uno_bootkick();
 }
 
 const harness_configuration uno_harness_config = {
@@ -243,5 +276,6 @@ const board board_uno = {
   .check_ignition = uno_check_ignition,
   .read_current = uno_read_current,
   .set_fan_power = uno_set_fan_power,
-  .set_ir_power = uno_set_ir_power
+  .set_ir_power = uno_set_ir_power,
+  .set_phone_power = uno_set_phone_power
 };
diff --git a/panda/board/boards/white.h b/panda/board/boards/white.h
index 899ba8d4fe..1be0702f94 100644
--- a/panda/board/boards/white.h
+++ b/panda/board/boards/white.h
@@ -78,11 +78,13 @@ void white_set_esp_gps_mode(uint8_t mode) {
       set_gpio_output(GPIOC, 14, 0);
       set_gpio_output(GPIOC, 5, 0);
       break;
+#ifndef EON
     case ESP_GPS_ENABLED:
       // ESP ON
       set_gpio_output(GPIOC, 14, 1);
       set_gpio_output(GPIOC, 5, 1);
       break;
+#endif
     case ESP_GPS_BOOTMODE:
       set_gpio_output(GPIOC, 14, 1);
       set_gpio_output(GPIOC, 5, 0);
@@ -156,8 +158,8 @@ uint32_t white_read_current(void){
   return adc_get(ADCCHAN_CURRENT);
 }
 
-uint64_t marker = 0;
-void white_usb_power_mode_tick(uint64_t tcnt){
+uint32_t marker = 0;
+void white_usb_power_mode_tick(uint32_t uptime){
 
   // on EON or BOOTSTUB, no state machine
 #if !defined(BOOTSTUB) && !defined(EON)
@@ -171,47 +173,47 @@ void white_usb_power_mode_tick(uint64_t tcnt){
 
   switch (usb_power_mode) {
     case USB_POWER_CLIENT:
-      if ((tcnt - marker) >= CLICKS) {
+      if ((uptime - marker) >= CLICKS) {
         if (!is_enumerated) {
           puts("USBP: didn't enumerate, switching to CDP mode\n");
           // switch to CDP
           white_set_usb_power_mode(USB_POWER_CDP);
-          marker = tcnt;
+          marker = uptime;
         }
       }
       // keep resetting the timer if it's enumerated
       if (is_enumerated) {
-        marker = tcnt;
+        marker = uptime;
       }
       break;
     case USB_POWER_CDP:
       // been CLICKS clicks since we switched to CDP
-      if ((tcnt-marker) >= CLICKS) {
+      if ((uptime - marker) >= CLICKS) {
         // measure current draw, if positive and no enumeration, switch to DCP
         if (!is_enumerated && (current < CURRENT_THRESHOLD)) {
           puts("USBP: no enumeration with current draw, switching to DCP mode\n");
           white_set_usb_power_mode(USB_POWER_DCP);
-          marker = tcnt;
+          marker = uptime;
         }
       }
       // keep resetting the timer if there's no current draw in CDP
       if (current >= CURRENT_THRESHOLD) {
-        marker = tcnt;
+        marker = uptime;
       }
       break;
     case USB_POWER_DCP:
       // been at least CLICKS clicks since we switched to DCP
-      if ((tcnt-marker) >= CLICKS) {
+      if ((uptime - marker) >= CLICKS) {
         // if no current draw, switch back to CDP
         if (current >= CURRENT_THRESHOLD) {
           puts("USBP: no current draw, switching back to CDP mode\n");
           white_set_usb_power_mode(USB_POWER_CDP);
-          marker = tcnt;
+          marker = uptime;
         }
       }
       // keep resetting the timer if there's current draw in DCP
       if (current < CURRENT_THRESHOLD) {
-        marker = tcnt;
+        marker = uptime;
       }
       break;
     default:
@@ -219,7 +221,7 @@ void white_usb_power_mode_tick(uint64_t tcnt){
       break;
   }
 #else
-  UNUSED(tcnt);
+  UNUSED(uptime);
 #endif
 }
 
@@ -236,7 +238,11 @@ bool white_check_ignition(void){
   return !get_gpio_input(GPIOA, 1);
 }
 
-void white_init(void) {
+void white_set_phone_power(bool enabled){
+  UNUSED(enabled);
+}
+
+void white_grey_common_init(void) {
   common_init_gpio();
 
   // C3: current sense
@@ -296,13 +302,6 @@ 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);
-
   // Init usb power mode
   uint32_t voltage = adc_get_voltage();
   // Init in CDP mode only if panda is powered by 12V.
@@ -314,6 +313,17 @@ void white_init(void) {
   }
 }
 
+void white_init(void) {
+  white_grey_common_init();
+
+  // Set default state of ESP
+  #ifdef EON
+    current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
+  #else
+    current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
+  #endif
+}
+
 const harness_configuration white_harness_config = {
   .has_harness = false
 };
@@ -332,5 +342,6 @@ const board board_white = {
   .check_ignition = white_check_ignition,
   .read_current = white_read_current,
   .set_fan_power = white_set_fan_power,
-  .set_ir_power = white_set_ir_power
+  .set_ir_power = white_set_ir_power,
+  .set_phone_power = white_set_phone_power
 };
diff --git a/panda/board/bootstub.c b/panda/board/bootstub.c
index 8ada20c738..1521b53240 100644
--- a/panda/board/bootstub.c
+++ b/panda/board/bootstub.c
@@ -1,5 +1,8 @@
 #define BOOTSTUB
 
+#define VERS_TAG 0x53524556
+#define MIN_VERSION 2
+
 #include "config.h"
 #include "obj/gitversion.h"
 
@@ -29,7 +32,11 @@ const board *current_board;
 // ********************* Includes *********************
 #include "libc.h"
 #include "provision.h"
+#include "critical.h"
+#include "faults.h"
 
+#include "drivers/registers.h"
+#include "drivers/interrupts.h"
 #include "drivers/clock.h"
 #include "drivers/llgpio.h"
 #include "drivers/adc.h"
@@ -65,6 +72,9 @@ extern void *_app_start[];
 // BOUNTY: $200 coupon on shop.comma.ai or $100 check.
 
 int main(void) {
+  // Init interrupt table
+  init_interrupts(true);
+
   disable_interrupts();
   clock_init();
   detect_configuration();
@@ -83,6 +93,13 @@ int main(void) {
   uint8_t digest[SHA_DIGEST_SIZE];
   SHA_hash(&_app_start[1], len-4, digest);
 
+  // verify version, last bytes in the signed area
+  uint32_t vers[2] = {0};
+  memcpy(&vers, ((void*)&_app_start[0]) + len - sizeof(vers), sizeof(vers));
+  if (vers[0] != VERS_TAG || vers[1] < MIN_VERSION) {
+    goto fail;
+  }
+
   // verify RSA signature
   if (RSA_verify(&release_rsa_key, ((void*)&_app_start[0]) + len, RSANUMBYTES, digest, SHA_DIGEST_SIZE)) {
     goto good;
diff --git a/panda/board/build.mk b/panda/board/build.mk
index acac03095b..21daf53ad0 100644
--- a/panda/board/build.mk
+++ b/panda/board/build.mk
@@ -33,7 +33,7 @@ POSTCOMPILE = @mv -f $(DEPDIR)/$*.Td $(DEPDIR)/$*.d && touch $@
 
 # this no longer pushes the bootstub
 flash: obj/$(PROJ_NAME).bin
-	PYTHONPATH=../ python -c "from python import Panda; Panda().flash('obj/$(PROJ_NAME).bin')"
+	PYTHONPATH=../ python3 -c "from python import Panda; Panda().flash('obj/$(PROJ_NAME).bin')"
 
 ota: obj/$(PROJ_NAME).bin
 	curl http://192.168.0.10/stupdate --upload-file $<
@@ -42,7 +42,7 @@ bin: obj/$(PROJ_NAME).bin
 
 # this flashes everything
 recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin
-	-PYTHONPATH=../ python -c "from python import Panda; Panda().reset(enter_bootloader=True)"
+	-PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootloader=True)"
 	sleep 1.0
 	$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin
 	$(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin
diff --git a/panda/board/config.h b/panda/board/config.h
index c2eb412e96..15096b69fa 100644
--- a/panda/board/config.h
+++ b/panda/board/config.h
@@ -5,6 +5,7 @@
 //#define DEBUG_UART
 //#define DEBUG_USB
 //#define DEBUG_SPI
+//#define DEBUG_FAULTS
 
 #ifdef STM32F4
   #define PANDA
@@ -37,5 +38,8 @@
 
 #define MAX_RESP_LEN 0x40U
 
+// Around (1Mbps / 8 bits/byte / 12 bytes per message)
+#define CAN_INTERRUPT_RATE 12000U
+
 #endif
 
diff --git a/panda/board/critical.h b/panda/board/critical.h
new file mode 100644
index 0000000000..c8cf52c7a1
--- /dev/null
+++ b/panda/board/critical.h
@@ -0,0 +1,23 @@
+// ********************* Critical section helpers *********************
+volatile bool interrupts_enabled = false;
+
+void enable_interrupts(void) {
+  interrupts_enabled = true;
+  __enable_irq();
+}
+
+void disable_interrupts(void) {
+  interrupts_enabled = false;
+  __disable_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();                                           \
+  }
diff --git a/panda/board/drivers/adc.h b/panda/board/drivers/adc.h
index 2a91fef8dc..358497adbf 100644
--- a/panda/board/drivers/adc.h
+++ b/panda/board/drivers/adc.h
@@ -9,26 +9,16 @@
 #define ADCCHAN_CURRENT 13
 
 void adc_init(void) {
-  // global setup
-  ADC->CCR = ADC_CCR_TSVREFE | ADC_CCR_VBATE;
-  //ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_EOCS | ADC_CR2_DDS;
-  ADC1->CR2 = ADC_CR2_ADON;
-
-  // long
-  //ADC1->SMPR1 = ADC_SMPR1_SMP10 | ADC_SMPR1_SMP11 | ADC_SMPR1_SMP12 | ADC_SMPR1_SMP13;
-  ADC1->SMPR1 = ADC_SMPR1_SMP12 | ADC_SMPR1_SMP13;
+  register_set(&(ADC->CCR), ADC_CCR_TSVREFE | ADC_CCR_VBATE, 0xC30000U);
+  register_set(&(ADC1->CR2), ADC_CR2_ADON, 0xFF7F0F03U);
+  register_set(&(ADC1->SMPR1), ADC_SMPR1_SMP12 | ADC_SMPR1_SMP13, 0x7FFFFFFU);
 }
 
 uint32_t adc_get(unsigned int channel) {
-  // includes length
-  //ADC1->SQR1 = 0;
-
-  // select channel
-  ADC1->JSQR = channel << 15;
-
-  //ADC1->CR1 = ADC_CR1_DISCNUM_0;
-  //ADC1->CR1 = ADC_CR1_EOCIE;
+  // Select channel
+  register_set(&(ADC1->JSQR), (channel << 15U), 0x3FFFFFU);
 
+  // Start conversion
   ADC1->SR &= ~(ADC_SR_JEOC);
   ADC1->CR2 |= ADC_CR2_JSWSTART;
   while (!(ADC1->SR & ADC_SR_JEOC));
diff --git a/panda/board/drivers/can.h b/panda/board/drivers/can.h
index c9bf2d2543..07895e3e08 100644
--- a/panda/board/drivers/can.h
+++ b/panda/board/drivers/can.h
@@ -27,11 +27,12 @@ void can_set_forwarding(int from, int to);
 
 void can_init(uint8_t can_number);
 void can_init_all(void);
-void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number);
+void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook);
 bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
 
 // Ignition detected from CAN meessages
 bool ignition_can = false;
+uint32_t ignition_can_cnt = 0U;
 
 // end API
 
@@ -148,23 +149,10 @@ void can_set_speed(uint8_t can_number) {
   }
 }
 
-void can_init(uint8_t can_number) {
-  if (can_number != 0xffU) {
-    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
-    can_set_speed(can_number);
-
-    llcan_init(CAN);
-
-    // in case there are queued up messages
-    process_can(can_number);
-  }
-}
-
 void can_init_all(void) {
   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){
@@ -336,11 +324,12 @@ void process_can(uint8_t can_number) {
 }
 
 void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
-
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
   int len = GET_LEN(to_push);
 
+  ignition_can_cnt = 0U;  // reset counter
+
   if (bus == 0) {
     // GM exception
     if ((addr == 0x1F1) && (len == 8)) {
@@ -357,11 +346,6 @@ void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       // this message isn't all zeros when ignition is on
       ignition_can = GET_BYTES_04(to_push) != 0;
     }
-    // VW exception
-    if ((addr == 0x3C0) && (len == 4)) {
-     // VW Terminal 15 (ignition-on) state
-     ignition_can  = (GET_BYTE(to_push, 2) & 0x2) != 0;
-    }
   }
 }
 
@@ -394,7 +378,7 @@ void can_rx(uint8_t can_number) {
       to_send.RDTR = to_push.RDTR;
       to_send.RDLR = to_push.RDLR;
       to_send.RDHR = to_push.RDHR;
-      can_send(&to_send, bus_fwd_num);
+      can_send(&to_send, bus_fwd_num, true);
     }
 
     safety_rx_hook(&to_push);
@@ -408,20 +392,20 @@ void can_rx(uint8_t can_number) {
   }
 }
 
-void CAN1_TX_IRQHandler(void) { process_can(0); }
-void CAN1_RX0_IRQHandler(void) { can_rx(0); }
-void CAN1_SCE_IRQHandler(void) { can_sce(CAN1); }
+void CAN1_TX_IRQ_Handler(void) { process_can(0); }
+void CAN1_RX0_IRQ_Handler(void) { can_rx(0); }
+void CAN1_SCE_IRQ_Handler(void) { can_sce(CAN1); }
 
-void CAN2_TX_IRQHandler(void) { process_can(1); }
-void CAN2_RX0_IRQHandler(void) { can_rx(1); }
-void CAN2_SCE_IRQHandler(void) { can_sce(CAN2); }
+void CAN2_TX_IRQ_Handler(void) { process_can(1); }
+void CAN2_RX0_IRQ_Handler(void) { can_rx(1); }
+void CAN2_SCE_IRQ_Handler(void) { can_sce(CAN2); }
 
-void CAN3_TX_IRQHandler(void) { process_can(2); }
-void CAN3_RX0_IRQHandler(void) { can_rx(2); }
-void CAN3_SCE_IRQHandler(void) { can_sce(CAN3); }
+void CAN3_TX_IRQ_Handler(void) { process_can(2); }
+void CAN3_RX0_IRQ_Handler(void) { can_rx(2); }
+void CAN3_SCE_IRQ_Handler(void) { can_sce(CAN3); }
 
-void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
-  if (safety_tx_hook(to_push) != 0) {
+void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) {
+  if (skip_tx_hook || safety_tx_hook(to_push) != 0) {
     if (bus_number < BUS_MAX) {
       // add CAN packet to send queue
       // bus number isn't passed through
@@ -440,3 +424,25 @@ void can_set_forwarding(int from, int to) {
   can_forwarding[from] = to;
 }
 
+void can_init(uint8_t can_number) {
+  REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+  REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+  REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+  REGISTER_INTERRUPT(CAN2_TX_IRQn, CAN2_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
+  REGISTER_INTERRUPT(CAN2_RX0_IRQn, CAN2_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
+  REGISTER_INTERRUPT(CAN2_SCE_IRQn, CAN2_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_2)
+  REGISTER_INTERRUPT(CAN3_TX_IRQn, CAN3_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
+  REGISTER_INTERRUPT(CAN3_RX0_IRQn, CAN3_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
+  REGISTER_INTERRUPT(CAN3_SCE_IRQn, CAN3_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_3)
+
+  if (can_number != 0xffU) {
+    CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number);
+    can_set_speed(can_number);
+
+    llcan_init(CAN);
+
+    // in case there are queued up messages
+    process_can(can_number);
+  }
+}
+
diff --git a/panda/board/drivers/clock.h b/panda/board/drivers/clock.h
index d564c7f01d..b75692438e 100644
--- a/panda/board/drivers/clock.h
+++ b/panda/board/drivers/clock.h
@@ -1,25 +1,24 @@
 void clock_init(void) {
   // enable external oscillator
-  RCC->CR |= RCC_CR_HSEON;
+  register_set_bits(&(RCC->CR), RCC_CR_HSEON);
   while ((RCC->CR & RCC_CR_HSERDY) == 0);
 
   // divide things
-  RCC->CFGR = RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4;
+  register_set(&(RCC->CFGR), RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4, 0xFF7FFCF3U);
 
   // 16mhz crystal
-  RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
-                 RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE;
+  register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE, 0x7F437FFFU);
 
   // start PLL
-  RCC->CR |= RCC_CR_PLLON;
+  register_set_bits(&(RCC->CR), RCC_CR_PLLON);
   while ((RCC->CR & RCC_CR_PLLRDY) == 0);
 
   // Configure Flash prefetch, Instruction cache, Data cache and wait state
   // *** without this, it breaks ***
-  FLASH->ACR = FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_5WS;
+  register_set(&(FLASH->ACR), FLASH_ACR_ICEN | FLASH_ACR_DCEN | FLASH_ACR_LATENCY_5WS, 0x1F0FU);
 
   // switch to PLL
-  RCC->CFGR |= RCC_CFGR_SW_PLL;
+  register_set_bits(&(RCC->CFGR), RCC_CFGR_SW_PLL);
   while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
 
   // *** running on PLL ***
@@ -27,14 +26,15 @@ void clock_init(void) {
 
 void watchdog_init(void) {
   // setup watchdog
-  IWDG->KR = 0x5555;
-  IWDG->PR = 0;          // divider /4
+  IWDG->KR = 0x5555U;
+  register_set(&(IWDG->PR), 0x0U, 0x7U);  // divider/4
+
   // 0 = 0.125 ms, let's have a 50ms watchdog
-  IWDG->RLR = 400 - 1;
-  IWDG->KR = 0xCCCC;
+  register_set(&(IWDG->RLR), (400U-1U), 0xFFFU);
+  IWDG->KR = 0xCCCCU;
 }
 
 void watchdog_feed(void) {
-  IWDG->KR = 0xAAAA;
+  IWDG->KR = 0xAAAAU;
 }
 
diff --git a/panda/board/drivers/dac.h b/panda/board/drivers/dac.h
index ac565eb221..4bdb16100c 100644
--- a/panda/board/drivers/dac.h
+++ b/panda/board/drivers/dac.h
@@ -2,22 +2,19 @@ void puth(unsigned int i);
 void puts(const char *a);
 
 void dac_init(void) {
-  // no buffers required since we have an opamp
-  //DAC->CR = DAC_CR_EN1 | DAC_CR_BOFF1 | DAC_CR_EN2 | DAC_CR_BOFF2;
-  DAC->DHR12R1 = 0;
-  DAC->DHR12R2 = 0;
-  DAC->CR = DAC_CR_EN1 | DAC_CR_EN2;
+  // No buffers required since we have an opamp
+  register_set(&(DAC->DHR12R1), 0U, 0xFFFU);
+  register_set(&(DAC->DHR12R2), 0U, 0xFFFU);
+  register_set(&(DAC->CR), DAC_CR_EN1 | DAC_CR_EN2, 0x3FFF3FFFU);
 }
 
 void dac_set(int channel, uint32_t value) {
   if (channel == 0) {
-    DAC->DHR12R1 = value;
+    register_set(&(DAC->DHR12R1), value, 0xFFFU);
   } else if (channel == 1) {
-    DAC->DHR12R2 = value;
+    register_set(&(DAC->DHR12R2), value, 0xFFFU);
   } else {
-    puts("Failed to set DAC: invalid channel value: ");
-    puth(value);
-    puts("\n");
+    puts("Failed to set DAC: invalid channel value: 0x"); puth(value); puts("\n");
   }
 }
 
diff --git a/panda/board/drivers/fan.h b/panda/board/drivers/fan.h
index d7326ec0b3..2f10e5ca8d 100644
--- a/panda/board/drivers/fan.h
+++ b/panda/board/drivers/fan.h
@@ -1,15 +1,3 @@
-void fan_init(void){
-    // Init PWM speed control
-    pwm_init(TIM3, 3);
-
-    // Init TACH interrupt
-    SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI2_PD;
-    EXTI->IMR |= (1U << 2);
-    EXTI->RTSR |= (1U << 2);
-    EXTI->FTSR |= (1U << 2);
-    NVIC_EnableIRQ(EXTI2_IRQn);
-}
-
 void fan_set_power(uint8_t percentage){
   pwm_set(TIM3, 3, percentage);
 }
@@ -27,10 +15,25 @@ void fan_tick(void){
 }
 
 // TACH interrupt handler
-void EXTI2_IRQHandler(void) {
+void EXTI2_IRQ_Handler(void) {
     volatile unsigned int pr = EXTI->PR & (1U << 2);
     if ((pr & (1U << 2)) != 0U) {
         fan_tach_counter++;
     }
     EXTI->PR = (1U << 2);
+}
+
+void fan_init(void){
+    // 5000RPM * 4 tach edges / 60 seconds
+    REGISTER_INTERRUPT(EXTI2_IRQn, EXTI2_IRQ_Handler, 700U, FAULT_INTERRUPT_RATE_TACH)
+
+    // Init PWM speed control
+    pwm_init(TIM3, 3);
+
+    // Init TACH interrupt
+    register_set(&(SYSCFG->EXTICR[0]), SYSCFG_EXTICR1_EXTI2_PD, 0xF00U);
+    register_set_bits(&(EXTI->IMR), (1U << 2));
+    register_set_bits(&(EXTI->RTSR), (1U << 2));
+    register_set_bits(&(EXTI->FTSR), (1U << 2));
+    NVIC_EnableIRQ(EXTI2_IRQn);
 }
\ No newline at end of file
diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h
index c697a21b41..6d4ba12b8c 100644
--- a/panda/board/drivers/gmlan_alt.h
+++ b/panda/board/drivers/gmlan_alt.h
@@ -124,15 +124,15 @@ int get_bit_message(char *out, CAN_FIFOMailBox_TypeDef *to_bang) {
 
 void setup_timer4(void) {
   // setup
-  TIM4->PSC = 48-1;          // tick on 1 us
-  TIM4->CR1 = TIM_CR1_CEN;   // enable
-  TIM4->ARR = 30-1;          // 33.3 kbps
+  register_set(&(TIM4->PSC), (48-1), 0xFFFFU);    // Tick on 1 us
+  register_set(&(TIM4->CR1), TIM_CR1_CEN, 0x3FU); // Enable
+  register_set(&(TIM4->ARR), (30-1), 0xFFFFU);   // 33.3 kbps
 
   // in case it's disabled
   NVIC_EnableIRQ(TIM4_IRQn);
 
   // run the interrupt
-  TIM4->DIER = TIM_DIER_UIE; // update interrupt
+  register_set(&(TIM4->DIER), TIM_DIER_UIE, 0x5F5FU); // Update interrupt
   TIM4->SR = 0;
 }
 
@@ -171,9 +171,9 @@ void reset_gmlan_switch_timeout(void) {
 
 void set_bitbanged_gmlan(int val) {
   if (val != 0) {
-    GPIOB->ODR |= (1U << 13);
+    register_set_bits(&(GPIOB->ODR), (1U << 13));
   } else {
-    GPIOB->ODR &= ~(1U << 13);
+    register_clear_bits(&(GPIOB->ODR), (1U << 13));
   }
 }
 
@@ -187,7 +187,7 @@ int gmlan_fail_count = 0;
 #define REQUIRED_SILENT_TIME 10
 #define MAX_FAIL_COUNT 10
 
-void TIM4_IRQHandler(void) {
+void TIM4_IRQ_Handler(void) {
   if (gmlan_alt_mode == BITBANG) {
     if ((TIM4->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) {
       int read = get_gpio_input(GPIOB, 12);
@@ -231,8 +231,8 @@ void TIM4_IRQHandler(void) {
       if ((gmlan_sending == gmlan_sendmax) || (gmlan_fail_count == MAX_FAIL_COUNT)) {
         set_bitbanged_gmlan(1); // recessive
         set_gpio_mode(GPIOB, 13, MODE_INPUT);
-        TIM4->DIER = 0;  // no update interrupt
-        TIM4->CR1 = 0;   // disable timer
+        register_clear_bits(&(TIM4->DIER), TIM_DIER_UIE); // No update interrupt
+        register_set(&(TIM4->CR1), 0U, 0x3FU); // Disable timer
         gmlan_sendmax = -1;   // exit
       }
     }
@@ -279,6 +279,8 @@ bool bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
     set_bitbanged_gmlan(1); // recessive
     set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
 
+    // 33kbps
+    REGISTER_INTERRUPT(TIM4_IRQn, TIM4_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN)
     setup_timer4();
   }
   return gmlan_send_ok;
diff --git a/panda/board/drivers/harness.h b/panda/board/drivers/harness.h
index 17520a4bab..c996bff0b2 100644
--- a/panda/board/drivers/harness.h
+++ b/panda/board/drivers/harness.h
@@ -8,7 +8,7 @@ uint8_t car_harness_status = 0U;
 
 struct harness_configuration {
   const bool has_harness;
-  GPIO_TypeDef *GPIO_SBU1;  
+  GPIO_TypeDef *GPIO_SBU1;
   GPIO_TypeDef *GPIO_SBU2;
   GPIO_TypeDef *GPIO_relay_normal;
   GPIO_TypeDef *GPIO_relay_flipped;
@@ -52,28 +52,6 @@ bool harness_check_ignition(void) {
   return ret;
 }
 
-// TODO: refactor to use harness config
-void harness_setup_ignition_interrupts(void){
-  if(car_harness_status == HARNESS_STATUS_NORMAL){
-    SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI3_PC;
-    EXTI->IMR |= (1U << 3);
-    EXTI->RTSR |= (1U << 3);
-    EXTI->FTSR |= (1U << 3);
-    puts("setup interrupts: normal\n");
-  } else if(car_harness_status == HARNESS_STATUS_FLIPPED) {
-    SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI0_PC;
-    EXTI->IMR |= (1U << 0);
-    EXTI->RTSR |= (1U << 0);
-    EXTI->FTSR |= (1U << 0);
-    NVIC_EnableIRQ(EXTI1_IRQn);
-    puts("setup interrupts: flipped\n");
-  } else {
-    puts("tried to setup ignition interrupts without harness connected\n");
-  }
-  NVIC_EnableIRQ(EXTI0_IRQn);
-  NVIC_EnableIRQ(EXTI3_IRQn);
-}
-
 uint8_t harness_detect_orientation(void) {
   uint8_t ret = HARNESS_STATUS_NC;
 
@@ -117,14 +95,11 @@ void harness_init(void) {
       set_gpio_mode(current_board->harness_config->GPIO_SBU2, current_board->harness_config->pin_SBU2, MODE_INPUT);
     } else {
       set_gpio_mode(current_board->harness_config->GPIO_SBU1, current_board->harness_config->pin_SBU1, MODE_INPUT);
-    }      
+    }
 
     // keep busses connected by default
     set_intercept_relay(false);
-
-    // setup ignition interrupts
-    harness_setup_ignition_interrupts();
   } else {
     puts("failed to detect car harness!\n");
   }
-}
\ No newline at end of file
+}
diff --git a/panda/board/drivers/interrupts.h b/panda/board/drivers/interrupts.h
new file mode 100644
index 0000000000..f15c441ab0
--- /dev/null
+++ b/panda/board/drivers/interrupts.h
@@ -0,0 +1,164 @@
+typedef struct interrupt {
+  IRQn_Type irq_type;
+  void (*handler)(void);
+  uint32_t call_counter;
+  uint32_t max_call_rate;   // Call rate is defined as the amount of calls each second
+  uint32_t call_rate_fault;
+} interrupt;
+
+void unused_interrupt_handler(void) {
+  // Something is wrong if this handler is called!
+  puts("Unused interrupt handler called!\n");
+  fault_occurred(FAULT_UNUSED_INTERRUPT_HANDLED);
+}
+
+#define NUM_INTERRUPTS 102U                // There are 102 external interrupt sources (see stm32f413.h)
+interrupt interrupts[NUM_INTERRUPTS];
+
+#define REGISTER_INTERRUPT(irq_num, func_ptr, call_rate, rate_fault) \
+  interrupts[irq_num].irq_type = irq_num; \
+  interrupts[irq_num].handler = func_ptr;  \
+  interrupts[irq_num].call_counter = 0U;   \
+  interrupts[irq_num].max_call_rate = call_rate; \
+  interrupts[irq_num].call_rate_fault = rate_fault;
+
+bool check_interrupt_rate = false;
+
+void handle_interrupt(IRQn_Type irq_type){
+  interrupts[irq_type].call_counter++;
+  interrupts[irq_type].handler();
+
+  // Check that the interrupts don't fire too often
+  if(check_interrupt_rate && (interrupts[irq_type].call_counter > interrupts[irq_type].max_call_rate)){
+    puts("Interrupt 0x"); puth(irq_type); puts(" fired too often (0x"); puth(interrupts[irq_type].call_counter); puts("/s)!\n");
+    fault_occurred(interrupts[irq_type].call_rate_fault);
+  }
+}
+
+// Reset interrupt counter every second
+void TIM6_DAC_IRQ_Handler(void) {
+  if (TIM6->SR != 0) {
+    for(uint16_t i=0U; i<NUM_INTERRUPTS; i++){
+      interrupts[i].call_counter = 0U;
+    }
+  }
+  TIM6->SR = 0;
+}
+
+void init_interrupts(bool check_rate_limit){
+  check_interrupt_rate = check_rate_limit;
+
+  for(uint16_t i=0U; i<NUM_INTERRUPTS; i++){
+    interrupts[i].handler = unused_interrupt_handler;
+  }
+
+  // Init timer 10 for a 1s interval
+  register_set_bits(&(RCC->APB1ENR), RCC_APB1ENR_TIM6EN);  // Enable interrupt timer peripheral
+  REGISTER_INTERRUPT(TIM6_DAC_IRQn, TIM6_DAC_IRQ_Handler, 1, FAULT_INTERRUPT_RATE_INTERRUPTS)
+  register_set(&(TIM6->PSC), (732-1), 0xFFFFU);
+  register_set(&(TIM6->DIER), TIM_DIER_UIE, 0x5F5FU);
+  register_set(&(TIM6->CR1), TIM_CR1_CEN, 0x3FU);
+  TIM6->SR = 0;
+  NVIC_EnableIRQ(TIM6_DAC_IRQn);
+}
+
+// ********************* Bare interrupt handlers *********************
+// Only implemented the STM32F413 interrupts for now, the STM32F203 specific ones do not fall into the scope of SIL2
+
+void WWDG_IRQHandler(void) {handle_interrupt(WWDG_IRQn);}
+void PVD_IRQHandler(void) {handle_interrupt(PVD_IRQn);}
+void TAMP_STAMP_IRQHandler(void) {handle_interrupt(TAMP_STAMP_IRQn);}
+void RTC_WKUP_IRQHandler(void) {handle_interrupt(RTC_WKUP_IRQn);}
+void FLASH_IRQHandler(void) {handle_interrupt(FLASH_IRQn);}
+void RCC_IRQHandler(void) {handle_interrupt(RCC_IRQn);}
+void EXTI0_IRQHandler(void) {handle_interrupt(EXTI0_IRQn);}
+void EXTI1_IRQHandler(void) {handle_interrupt(EXTI1_IRQn);}
+void EXTI2_IRQHandler(void) {handle_interrupt(EXTI2_IRQn);}
+void EXTI3_IRQHandler(void) {handle_interrupt(EXTI3_IRQn);}
+void EXTI4_IRQHandler(void) {handle_interrupt(EXTI4_IRQn);}
+void DMA1_Stream0_IRQHandler(void) {handle_interrupt(DMA1_Stream0_IRQn);}
+void DMA1_Stream1_IRQHandler(void) {handle_interrupt(DMA1_Stream1_IRQn);}
+void DMA1_Stream2_IRQHandler(void) {handle_interrupt(DMA1_Stream2_IRQn);}
+void DMA1_Stream3_IRQHandler(void) {handle_interrupt(DMA1_Stream3_IRQn);}
+void DMA1_Stream4_IRQHandler(void) {handle_interrupt(DMA1_Stream4_IRQn);}
+void DMA1_Stream5_IRQHandler(void) {handle_interrupt(DMA1_Stream5_IRQn);}
+void DMA1_Stream6_IRQHandler(void) {handle_interrupt(DMA1_Stream6_IRQn);}
+void ADC_IRQHandler(void) {handle_interrupt(ADC_IRQn);}
+void CAN1_TX_IRQHandler(void) {handle_interrupt(CAN1_TX_IRQn);}
+void CAN1_RX0_IRQHandler(void) {handle_interrupt(CAN1_RX0_IRQn);}
+void CAN1_RX1_IRQHandler(void) {handle_interrupt(CAN1_RX1_IRQn);}
+void CAN1_SCE_IRQHandler(void) {handle_interrupt(CAN1_SCE_IRQn);}
+void EXTI9_5_IRQHandler(void) {handle_interrupt(EXTI9_5_IRQn);}
+void TIM1_BRK_TIM9_IRQHandler(void) {handle_interrupt(TIM1_BRK_TIM9_IRQn);}
+void TIM1_UP_TIM10_IRQHandler(void) {handle_interrupt(TIM1_UP_TIM10_IRQn);}
+void TIM1_TRG_COM_TIM11_IRQHandler(void) {handle_interrupt(TIM1_TRG_COM_TIM11_IRQn);}
+void TIM1_CC_IRQHandler(void) {handle_interrupt(TIM1_CC_IRQn);}
+void TIM2_IRQHandler(void) {handle_interrupt(TIM2_IRQn);}
+void TIM3_IRQHandler(void) {handle_interrupt(TIM3_IRQn);}
+void TIM4_IRQHandler(void) {handle_interrupt(TIM4_IRQn);}
+void I2C1_EV_IRQHandler(void) {handle_interrupt(I2C1_EV_IRQn);}
+void I2C1_ER_IRQHandler(void) {handle_interrupt(I2C1_ER_IRQn);}
+void I2C2_EV_IRQHandler(void) {handle_interrupt(I2C2_EV_IRQn);}
+void I2C2_ER_IRQHandler(void) {handle_interrupt(I2C2_ER_IRQn);}
+void SPI1_IRQHandler(void) {handle_interrupt(SPI1_IRQn);}
+void SPI2_IRQHandler(void) {handle_interrupt(SPI2_IRQn);}
+void USART1_IRQHandler(void) {handle_interrupt(USART1_IRQn);}
+void USART2_IRQHandler(void) {handle_interrupt(USART2_IRQn);}
+void USART3_IRQHandler(void) {handle_interrupt(USART3_IRQn);}
+void EXTI15_10_IRQHandler(void) {handle_interrupt(EXTI15_10_IRQn);}
+void RTC_Alarm_IRQHandler(void) {handle_interrupt(RTC_Alarm_IRQn);}
+void OTG_FS_WKUP_IRQHandler(void) {handle_interrupt(OTG_FS_WKUP_IRQn);}
+void TIM8_BRK_TIM12_IRQHandler(void) {handle_interrupt(TIM8_BRK_TIM12_IRQn);}
+void TIM8_UP_TIM13_IRQHandler(void) {handle_interrupt(TIM8_UP_TIM13_IRQn);}
+void TIM8_TRG_COM_TIM14_IRQHandler(void) {handle_interrupt(TIM8_TRG_COM_TIM14_IRQn);}
+void TIM8_CC_IRQHandler(void) {handle_interrupt(TIM8_CC_IRQn);}
+void DMA1_Stream7_IRQHandler(void) {handle_interrupt(DMA1_Stream7_IRQn);}
+void FSMC_IRQHandler(void) {handle_interrupt(FSMC_IRQn);}
+void SDIO_IRQHandler(void) {handle_interrupt(SDIO_IRQn);}
+void TIM5_IRQHandler(void) {handle_interrupt(TIM5_IRQn);}
+void SPI3_IRQHandler(void) {handle_interrupt(SPI3_IRQn);}
+void UART4_IRQHandler(void) {handle_interrupt(UART4_IRQn);}
+void UART5_IRQHandler(void) {handle_interrupt(UART5_IRQn);}
+void TIM6_DAC_IRQHandler(void) {handle_interrupt(TIM6_DAC_IRQn);}
+void TIM7_IRQHandler(void) {handle_interrupt(TIM7_IRQn);}
+void DMA2_Stream0_IRQHandler(void) {handle_interrupt(DMA2_Stream0_IRQn);}
+void DMA2_Stream1_IRQHandler(void) {handle_interrupt(DMA2_Stream1_IRQn);}
+void DMA2_Stream2_IRQHandler(void) {handle_interrupt(DMA2_Stream2_IRQn);}
+void DMA2_Stream3_IRQHandler(void) {handle_interrupt(DMA2_Stream3_IRQn);}
+void DMA2_Stream4_IRQHandler(void) {handle_interrupt(DMA2_Stream4_IRQn);}
+void CAN2_TX_IRQHandler(void) {handle_interrupt(CAN2_TX_IRQn);}
+void CAN2_RX0_IRQHandler(void) {handle_interrupt(CAN2_RX0_IRQn);}
+void CAN2_RX1_IRQHandler(void) {handle_interrupt(CAN2_RX1_IRQn);}
+void CAN2_SCE_IRQHandler(void) {handle_interrupt(CAN2_SCE_IRQn);}
+void OTG_FS_IRQHandler(void) {handle_interrupt(OTG_FS_IRQn);}
+void DMA2_Stream5_IRQHandler(void) {handle_interrupt(DMA2_Stream5_IRQn);}
+void DMA2_Stream6_IRQHandler(void) {handle_interrupt(DMA2_Stream6_IRQn);}
+void DMA2_Stream7_IRQHandler(void) {handle_interrupt(DMA2_Stream7_IRQn);}
+void USART6_IRQHandler(void) {handle_interrupt(USART6_IRQn);}
+void I2C3_EV_IRQHandler(void) {handle_interrupt(I2C3_EV_IRQn);}
+void I2C3_ER_IRQHandler(void) {handle_interrupt(I2C3_ER_IRQn);}
+#ifdef STM32F4
+  void DFSDM1_FLT0_IRQHandler(void) {handle_interrupt(DFSDM1_FLT0_IRQn);}
+  void DFSDM1_FLT1_IRQHandler(void) {handle_interrupt(DFSDM1_FLT1_IRQn);}
+  void CAN3_TX_IRQHandler(void) {handle_interrupt(CAN3_TX_IRQn);}
+  void CAN3_RX0_IRQHandler(void) {handle_interrupt(CAN3_RX0_IRQn);}
+  void CAN3_RX1_IRQHandler(void) {handle_interrupt(CAN3_RX1_IRQn);}
+  void CAN3_SCE_IRQHandler(void) {handle_interrupt(CAN3_SCE_IRQn);}
+  void RNG_IRQHandler(void) {handle_interrupt(RNG_IRQn);}
+  void FPU_IRQHandler(void) {handle_interrupt(FPU_IRQn);}
+  void UART7_IRQHandler(void) {handle_interrupt(UART7_IRQn);}
+  void UART8_IRQHandler(void) {handle_interrupt(UART8_IRQn);}
+  void SPI4_IRQHandler(void) {handle_interrupt(SPI4_IRQn);}
+  void SPI5_IRQHandler(void) {handle_interrupt(SPI5_IRQn);}
+  void SAI1_IRQHandler(void) {handle_interrupt(SAI1_IRQn);}
+  void UART9_IRQHandler(void) {handle_interrupt(UART9_IRQn);}
+  void UART10_IRQHandler(void) {handle_interrupt(UART10_IRQn);}
+  void QUADSPI_IRQHandler(void) {handle_interrupt(QUADSPI_IRQn);}
+  void FMPI2C1_EV_IRQHandler(void) {handle_interrupt(FMPI2C1_EV_IRQn);}
+  void FMPI2C1_ER_IRQHandler(void) {handle_interrupt(FMPI2C1_ER_IRQn);}
+  void LPTIM1_IRQHandler(void) {handle_interrupt(LPTIM1_IRQn);}
+  void DFSDM2_FLT0_IRQHandler(void) {handle_interrupt(DFSDM2_FLT0_IRQn);}
+  void DFSDM2_FLT1_IRQHandler(void) {handle_interrupt(DFSDM2_FLT1_IRQn);}
+  void DFSDM2_FLT2_IRQHandler(void) {handle_interrupt(DFSDM2_FLT2_IRQn);}
+  void DFSDM2_FLT3_IRQHandler(void) {handle_interrupt(DFSDM2_FLT3_IRQn);}
+#endif
\ No newline at end of file
diff --git a/panda/board/drivers/llcan.h b/panda/board/drivers/llcan.h
index 0a698d4e8d..4cd9b4b5ab 100644
--- a/panda/board/drivers/llcan.h
+++ b/panda/board/drivers/llcan.h
@@ -19,25 +19,24 @@ void puts(const char *a);
 
 bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool silent) {
   // initialization mode
-  CAN_obj->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
+  register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_INRQ, 0x180FFU);
   while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
 
   // set time quanta from defines
-  CAN_obj->BTR = (CAN_BTR_TS1_0 * (CAN_SEQ1-1)) |
+  register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1)) |
             (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) |
-            (can_speed_to_prescaler(speed) - 1U);
+            (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU);
 
   // silent loopback mode for debugging
   if (loopback) {
-    CAN_obj->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM;
+    register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM | CAN_BTR_LBKM);
   }
   if (silent) {
-    CAN_obj->BTR |= CAN_BTR_SILM;
+    register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM);
   }
 
   // reset
-  // cppcheck-suppress redundantAssignment ; it's a register
-  CAN_obj->MCR = CAN_MCR_TTCM | CAN_MCR_ABOM;
+  register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU);
 
   #define CAN_TIMEOUT 1000000
   int tmp = 0;
@@ -51,20 +50,25 @@ bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool s
 }
 
 void llcan_init(CAN_TypeDef *CAN_obj) {
-  // accept all filter
-  CAN_obj->FMR |= CAN_FMR_FINIT;
+  // Enter init mode
+  register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
+  
+  // Wait for INAK bit to be set
+  while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {}
 
   // no mask
-  CAN_obj->sFilterRegister[0].FR1 = 0;
-  CAN_obj->sFilterRegister[0].FR2 = 0;
-  CAN_obj->sFilterRegister[14].FR1 = 0;
-  CAN_obj->sFilterRegister[14].FR2 = 0;
+  // For some weird reason some of these registers do not want to set properly on CAN2 and CAN3. Probably something to do with the single/dual mode and their different filters.
+  CAN_obj->sFilterRegister[0].FR1 = 0U;
+  CAN_obj->sFilterRegister[0].FR2 = 0U;
+  CAN_obj->sFilterRegister[14].FR1 = 0U;
+  CAN_obj->sFilterRegister[14].FR2 = 0U;
   CAN_obj->FA1R |= 1U | (1U << 14);
 
-  CAN_obj->FMR &= ~(CAN_FMR_FINIT);
+  // Exit init mode, do not wait
+  register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT);
 
   // enable certain CAN interrupts
-  CAN_obj->IER |= CAN_IER_TMEIE | CAN_IER_FMPIE0 |  CAN_IER_WKUIE;
+  register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 |  CAN_IER_WKUIE);
 
   if (CAN_obj == CAN1) {
     NVIC_EnableIRQ(CAN1_TX_IRQn);
@@ -87,7 +91,7 @@ void llcan_init(CAN_TypeDef *CAN_obj) {
 
 void llcan_clear_send(CAN_TypeDef *CAN_obj) {
   CAN_obj->TSR |= CAN_TSR_ABRQ0;
-  CAN_obj->MSR &= ~(CAN_MSR_ERRI);
+  register_clear_bits(&(CAN_obj->MSR), CAN_MSR_ERRI);
   // cppcheck-suppress selfAssignment ; needed to clear the register
   CAN_obj->MSR = CAN_obj->MSR;
 }
diff --git a/panda/board/drivers/llgpio.h b/panda/board/drivers/llgpio.h
index 9304cbe010..0bd58c3b8a 100644
--- a/panda/board/drivers/llgpio.h
+++ b/panda/board/drivers/llgpio.h
@@ -15,16 +15,16 @@ void set_gpio_mode(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
   uint32_t tmp = GPIO->MODER;
   tmp &= ~(3U << (pin * 2U));
   tmp |= (mode << (pin * 2U));
-  GPIO->MODER = tmp;
+  register_set(&(GPIO->MODER), tmp, 0xFFFFFFFFU);
   EXIT_CRITICAL();
 }
 
 void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) {
   ENTER_CRITICAL();
   if (enabled) {
-    GPIO->ODR |= (1U << pin);
+    register_set_bits(&(GPIO->ODR), (1U << pin));
   } else {
-    GPIO->ODR &= ~(1U << pin);
+    register_clear_bits(&(GPIO->ODR), (1U << pin));
   }
   set_gpio_mode(GPIO, pin, MODE_OUTPUT);
   EXIT_CRITICAL();
@@ -33,9 +33,9 @@ void set_gpio_output(GPIO_TypeDef *GPIO, unsigned int pin, bool enabled) {
 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);
+    register_set_bits(&(GPIO->OTYPER), (1U << pin));
   } else {
-    GPIO->OTYPER &= ~(1U << pin);
+    register_clear_bits(&(GPIO->OTYPER), (1U << pin));
   }
   EXIT_CRITICAL();
 }
@@ -45,7 +45,7 @@ void set_gpio_alternate(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode)
   uint32_t tmp = GPIO->AFR[pin >> 3U];
   tmp &= ~(0xFU << ((pin & 7U) * 4U));
   tmp |= mode << ((pin & 7U) * 4U);
-  GPIO->AFR[pin >> 3] = tmp;
+  register_set(&(GPIO->AFR[pin >> 3]), tmp, 0xFFFFFFFFU);
   set_gpio_mode(GPIO, pin, MODE_ALTERNATE);
   EXIT_CRITICAL();
 }
@@ -55,7 +55,7 @@ void set_gpio_pullup(GPIO_TypeDef *GPIO, unsigned int pin, unsigned int mode) {
   uint32_t tmp = GPIO->PUPDR;
   tmp &= ~(3U << (pin * 2U));
   tmp |= (mode << (pin * 2U));
-  GPIO->PUPDR = tmp;
+  register_set(&(GPIO->PUPDR), tmp, 0xFFFFFFFFU);
   EXIT_CRITICAL();
 }
 
diff --git a/panda/board/drivers/pwm.h b/panda/board/drivers/pwm.h
index d2e1652c1c..c3709200c1 100644
--- a/panda/board/drivers/pwm.h
+++ b/panda/board/drivers/pwm.h
@@ -1,53 +1,54 @@
 #define PWM_COUNTER_OVERFLOW 2000U // To get ~50kHz
 
+// TODO: Implement for 32-bit timers
+
 void pwm_init(TIM_TypeDef *TIM, uint8_t channel){
     // Enable timer and auto-reload
-    TIM->CR1 = TIM_CR1_CEN | TIM_CR1_ARPE;
+    register_set(&(TIM->CR1), TIM_CR1_CEN | TIM_CR1_ARPE, 0x3FU);
 
     // Set channel as PWM mode 1 and enable output
     switch(channel){
         case 1U:
-            TIM->CCMR1 |= (TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE);
-            TIM->CCER |= TIM_CCER_CC1E;
+            register_set_bits(&(TIM->CCMR1), (TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE));
+            register_set_bits(&(TIM->CCER), TIM_CCER_CC1E);
             break;
         case 2U:
-            TIM->CCMR1 |= (TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE);
-            TIM->CCER |= TIM_CCER_CC2E;
+            register_set_bits(&(TIM->CCMR1), (TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE));
+            register_set_bits(&(TIM->CCER), TIM_CCER_CC2E);
             break;
         case 3U:
-            TIM->CCMR2 |= (TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE);
-            TIM->CCER |= TIM_CCER_CC3E;
+            register_set_bits(&(TIM->CCMR2), (TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE));
+            register_set_bits(&(TIM->CCER), TIM_CCER_CC3E);
             break;
         case 4U:
-            TIM->CCMR2 |= (TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4PE);
-            TIM->CCER |= TIM_CCER_CC4E;
+            register_set_bits(&(TIM->CCMR2), (TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4PE));
+            register_set_bits(&(TIM->CCER), TIM_CCER_CC4E);
             break;
         default:
             break;
     }
 
     // Set max counter value
-    TIM->ARR = PWM_COUNTER_OVERFLOW;
+    register_set(&(TIM->ARR), PWM_COUNTER_OVERFLOW, 0xFFFFU);
 
     // Update registers and clear counter
     TIM->EGR |= TIM_EGR_UG;
 }
 
-// TODO: Implement for 32-bit timers
 void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage){
     uint16_t comp_value = (((uint16_t) percentage * PWM_COUNTER_OVERFLOW) / 100U);
     switch(channel){
         case 1U:
-            TIM->CCR1 = comp_value;
+            register_set(&(TIM->CCR1), comp_value, 0xFFFFU);
             break;
         case 2U:
-            TIM->CCR2 = comp_value;
+            register_set(&(TIM->CCR2), comp_value, 0xFFFFU);
             break;
         case 3U:
-            TIM->CCR3 = comp_value;
+            register_set(&(TIM->CCR3), comp_value, 0xFFFFU);
             break;
         case 4U:
-            TIM->CCR4 = comp_value;
+            register_set(&(TIM->CCR4), comp_value, 0xFFFFU);
             break;
         default:
             break;
diff --git a/panda/board/drivers/registers.h b/panda/board/drivers/registers.h
new file mode 100644
index 0000000000..76748295d9
--- /dev/null
+++ b/panda/board/drivers/registers.h
@@ -0,0 +1,81 @@
+
+typedef struct reg {
+  volatile uint32_t *address;
+  uint32_t value;
+  uint32_t check_mask;
+} reg;
+
+// 10 bit hash with 23 as a prime
+#define REGISTER_MAP_SIZE 0x3FFU
+#define HASHING_PRIME 23U
+#define CHECK_COLLISION(hash, addr) (((uint32_t) register_map[hash].address != 0U) && (register_map[hash].address != addr))
+
+reg register_map[REGISTER_MAP_SIZE];
+
+// Hash spread in first and second iterations seems to be reasonable.
+// See: tests/development/register_hashmap_spread.py
+// Also, check the collision warnings in the debug output, and minimize those.
+uint16_t hash_addr(uint32_t input){
+  return (((input >> 16U) ^ ((((input + 1U) & 0xFFFFU) * HASHING_PRIME) & 0xFFFFU)) & REGISTER_MAP_SIZE);
+}
+
+// Do not put bits in the check mask that get changed by the hardware
+void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask){
+  ENTER_CRITICAL()
+  // Set bits in register that are also in the mask
+  (*addr) = ((*addr) & (~mask)) | (val & mask);
+
+  // Add these values to the map
+  uint16_t hash = hash_addr((uint32_t) addr);
+  uint16_t tries = REGISTER_MAP_SIZE;
+  while(CHECK_COLLISION(hash, addr) && (tries > 0U)) { hash = hash_addr((uint32_t) hash); tries--;}
+  if (tries != 0U){
+    register_map[hash].address = addr;
+    register_map[hash].value = (register_map[hash].value & (~mask)) | (val & mask);
+    register_map[hash].check_mask |= mask;
+  } else {
+    #ifdef DEBUG_FAULTS
+      puts("Hash collision: address 0x"); puth((uint32_t) addr); puts("!\n");
+    #endif
+  }
+  EXIT_CRITICAL()
+}
+
+// Set individual bits. Also add them to the check_mask.
+// Do not use this to change bits that get reset by the hardware
+void register_set_bits(volatile uint32_t *addr, uint32_t val) {
+  return register_set(addr, val, val);
+}
+
+// Clear individual bits. Also add them to the check_mask.
+// Do not use this to clear bits that get set by the hardware
+void register_clear_bits(volatile uint32_t *addr, uint32_t val) {
+  return register_set(addr, (~val), val);
+}
+
+// To be called periodically
+void check_registers(void){
+  for(uint16_t i=0U; i<REGISTER_MAP_SIZE; i++){
+    if((uint32_t) register_map[i].address != 0U){
+      ENTER_CRITICAL()
+      if((*(register_map[i].address) & register_map[i].check_mask) != (register_map[i].value & register_map[i].check_mask)){
+        #ifdef DEBUG_FAULTS
+          puts("Register at address 0x"); puth((uint32_t) register_map[i].address); puts(" is divergent!");
+          puts("   Map: 0x"); puth(register_map[i].value);
+          puts("   Register: 0x"); puth(*(register_map[i].address));
+          puts("   Mask: 0x"); puth(register_map[i].check_mask);
+          puts("\n");
+        #endif
+        fault_occurred(FAULT_REGISTER_DIVERGENT);
+      }
+      EXIT_CRITICAL()
+    }
+  }
+}
+
+void init_registers(void) {
+  for(uint16_t i=0U; i<REGISTER_MAP_SIZE; i++){
+    register_map[i].address = (volatile uint32_t *) 0U;
+    register_map[i].check_mask = 0U;
+  }
+}
\ No newline at end of file
diff --git a/panda/board/drivers/rtc.h b/panda/board/drivers/rtc.h
index 9cf113a02e..301c4de770 100644
--- a/panda/board/drivers/rtc.h
+++ b/panda/board/drivers/rtc.h
@@ -27,19 +27,19 @@ void rtc_init(void){
         if((RCC->BDCR & RCC_BDCR_MASK) != RCC_BDCR_OPTIONS){
             puts("Initializing RTC\n");
             // Reset backup domain
-            RCC->BDCR |= RCC_BDCR_BDRST;
+            register_set_bits(&(RCC->BDCR), RCC_BDCR_BDRST);
 
             // Disable write protection
-            PWR->CR |= PWR_CR_DBP;
+            register_set_bits(&(PWR->CR), PWR_CR_DBP);
 
             // Clear backup domain reset
-            RCC->BDCR &= ~(RCC_BDCR_BDRST);
+            register_clear_bits(&(RCC->BDCR), RCC_BDCR_BDRST);
 
             // Set RTC options
-            RCC->BDCR = RCC_BDCR_OPTIONS | (RCC->BDCR & (~RCC_BDCR_MASK));
+            register_set(&(RCC->BDCR), RCC_BDCR_OPTIONS, RCC_BDCR_MASK);
 
             // Enable write protection
-            PWR->CR &= ~(PWR_CR_DBP);
+            register_clear_bits(&(PWR->CR), PWR_CR_DBP);
         }
     }
 }
@@ -49,12 +49,12 @@ void rtc_set_time(timestamp_t time){
         puts("Setting RTC time\n");
 
         // Disable write protection
-        PWR->CR |= PWR_CR_DBP;
+        register_set_bits(&(PWR->CR), PWR_CR_DBP);
         RTC->WPR = 0xCA;
         RTC->WPR = 0x53;
 
         // Enable initialization mode
-        RTC->ISR |= RTC_ISR_INIT;
+        register_set_bits(&(RTC->ISR), RTC_ISR_INIT);
         while((RTC->ISR & RTC_ISR_INITF) == 0){}
         
         // Set time
@@ -62,17 +62,17 @@ void rtc_set_time(timestamp_t time){
         RTC->DR = (to_bcd(time.year - YEAR_OFFSET) << RTC_DR_YU_Pos) | (time.weekday << RTC_DR_WDU_Pos) | (to_bcd(time.month) << RTC_DR_MU_Pos) | (to_bcd(time.day) << RTC_DR_DU_Pos);
 
         // Set options
-        RTC->CR = 0U;
+        register_set(&(RTC->CR), 0U, 0xFCFFFFU);
         
         // Disable initalization mode
-        RTC->ISR &= ~(RTC_ISR_INIT);
+        register_clear_bits(&(RTC->ISR), RTC_ISR_INIT);
 
         // Wait for synchronization
         while((RTC->ISR & RTC_ISR_RSF) == 0){}
 
         // Re-enable write protection
         RTC->WPR = 0x00;
-        PWR->CR &= ~(PWR_CR_DBP);
+        register_clear_bits(&(PWR->CR), PWR_CR_DBP);
     }
 }
 
diff --git a/panda/board/drivers/spi.h b/panda/board/drivers/spi.h
index 26690984de..29963b6bdf 100644
--- a/panda/board/drivers/spi.h
+++ b/panda/board/drivers/spi.h
@@ -10,45 +10,22 @@ uint8_t spi_buf[SPI_BUF_SIZE];
 int spi_buf_count = 0;
 int spi_total_count = 0;
 
-void spi_init(void) {
-  //puts("SPI init\n");
-  SPI1->CR1 = SPI_CR1_SPE;
-
-  // enable SPI interrupts
-  //SPI1->CR2 = SPI_CR2_RXNEIE | SPI_CR2_ERRIE | SPI_CR2_TXEIE;
-  SPI1->CR2 = SPI_CR2_RXNEIE;
-
-  NVIC_EnableIRQ(DMA2_Stream2_IRQn);
-  NVIC_EnableIRQ(DMA2_Stream3_IRQn);
-  //NVIC_EnableIRQ(SPI1_IRQn);
-
-  // reset handshake back to pull up
-  set_gpio_mode(GPIOB, 0, MODE_INPUT);
-  set_gpio_pullup(GPIOB, 0, PULL_UP);
-
-  // setup interrupt on falling edge of SPI enable (on PA4)
-  SYSCFG->EXTICR[2] = SYSCFG_EXTICR2_EXTI4_PA;
-  EXTI->IMR |= (1U << 4);
-  EXTI->FTSR |= (1U << 4);
-  NVIC_EnableIRQ(EXTI4_IRQn);
-}
-
 void spi_tx_dma(void *addr, int len) {
   // disable DMA
-  SPI1->CR2 &= ~SPI_CR2_TXDMAEN;
-  DMA2_Stream3->CR &= ~DMA_SxCR_EN;
+  register_clear_bits(&(SPI1->CR2), SPI_CR2_TXDMAEN);
+  register_clear_bits(&(DMA2_Stream3->CR), DMA_SxCR_EN);
 
   // DMA2, stream 3, channel 3
-  DMA2_Stream3->M0AR = (uint32_t)addr;
+  register_set(&(DMA2_Stream3->M0AR), (uint32_t)addr, 0xFFFFFFFFU);
   DMA2_Stream3->NDTR = len;
-  DMA2_Stream3->PAR = (uint32_t)&(SPI1->DR);
+  register_set(&(DMA2_Stream3->PAR), (uint32_t)&(SPI1->DR), 0xFFFFFFFFU);
 
   // channel3, increment memory, memory -> periph, enable
-  DMA2_Stream3->CR = DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_EN;
+  register_set(&(DMA2_Stream3->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_DIR_0 | DMA_SxCR_EN), 0x1E077EFEU);
   delay(0);
-  DMA2_Stream3->CR |= DMA_SxCR_TCIE;
+  register_set_bits(&(DMA2_Stream3->CR), DMA_SxCR_TCIE);
 
-  SPI1->CR2 |= SPI_CR2_TXDMAEN;
+  register_set_bits(&(SPI1->CR2), SPI_CR2_TXDMAEN);
 
   // signal data is ready by driving low
   // esp must be configured as input by this point
@@ -57,33 +34,32 @@ void spi_tx_dma(void *addr, int len) {
 
 void spi_rx_dma(void *addr, int len) {
   // disable DMA
-  SPI1->CR2 &= ~SPI_CR2_RXDMAEN;
-  DMA2_Stream2->CR &= ~DMA_SxCR_EN;
+  register_clear_bits(&(SPI1->CR2), SPI_CR2_RXDMAEN);
+  register_clear_bits(&(DMA2_Stream2->CR), DMA_SxCR_EN);
 
   // drain the bus
   volatile uint8_t dat = SPI1->DR;
   (void)dat;
 
   // DMA2, stream 2, channel 3
-  DMA2_Stream2->M0AR = (uint32_t)addr;
+  register_set(&(DMA2_Stream2->M0AR), (uint32_t)addr, 0xFFFFFFFFU);
   DMA2_Stream2->NDTR = len;
-  DMA2_Stream2->PAR = (uint32_t)&(SPI1->DR);
+  register_set(&(DMA2_Stream2->PAR), (uint32_t)&(SPI1->DR), 0xFFFFFFFFU);
 
   // channel3, increment memory, periph -> memory, enable
-  DMA2_Stream2->CR = DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_EN;
+  register_set(&(DMA2_Stream2->CR), (DMA_SxCR_CHSEL_1 | DMA_SxCR_CHSEL_0 | DMA_SxCR_MINC | DMA_SxCR_EN), 0x1E077EFEU);
   delay(0);
-  DMA2_Stream2->CR |= DMA_SxCR_TCIE;
+  register_set_bits(&(DMA2_Stream2->CR), DMA_SxCR_TCIE);
 
-  SPI1->CR2 |= SPI_CR2_RXDMAEN;
+  register_set_bits(&(SPI1->CR2), SPI_CR2_RXDMAEN);
 }
 
 // ***************************** SPI IRQs *****************************
-
 // can't go on the stack cause it's DMAed
 uint8_t spi_tx_buf[0x44];
 
 // SPI RX
-void DMA2_Stream2_IRQHandler(void) {
+void DMA2_Stream2_IRQ_Handler(void) {
   int *resp_len = (int*)spi_tx_buf;
   (void)memset(spi_tx_buf, 0xaa, 0x44);
   *resp_len = spi_cb_rx(spi_buf, 0x14, spi_tx_buf+4);
@@ -99,7 +75,7 @@ void DMA2_Stream2_IRQHandler(void) {
 }
 
 // SPI TX
-void DMA2_Stream3_IRQHandler(void) {
+void DMA2_Stream3_IRQ_Handler(void) {
   #ifdef DEBUG_SPI
     puts("SPI handshake\n");
   #endif
@@ -112,7 +88,7 @@ void DMA2_Stream3_IRQHandler(void) {
   DMA2->LIFCR = DMA_LIFCR_CTCIF3;
 }
 
-void EXTI4_IRQHandler(void) {
+void EXTI4_IRQ_Handler(void) {
   volatile unsigned int pr = EXTI->PR & (1U << 4);
   #ifdef DEBUG_SPI
     puts("exti4\n");
@@ -125,3 +101,31 @@ void EXTI4_IRQHandler(void) {
   EXTI->PR = pr;
 }
 
+// ***************************** SPI init *****************************
+void spi_init(void) {
+  // Max SPI clock the ESP can produce is 80MHz. At buffer size of 256 bytes, that's a max of about 40k buffers per second
+  REGISTER_INTERRUPT(DMA2_Stream2_IRQn, DMA2_Stream2_IRQ_Handler, 50000U, FAULT_INTERRUPT_RATE_SPI_DMA)
+  REGISTER_INTERRUPT(DMA2_Stream3_IRQn, DMA2_Stream3_IRQ_Handler, 50000U, FAULT_INTERRUPT_RATE_SPI_DMA)
+  REGISTER_INTERRUPT(EXTI4_IRQn, EXTI4_IRQ_Handler, 50000U, FAULT_INTERRUPT_RATE_SPI_CS) // TODO: Figure out if this is a reasonable limit
+
+  //puts("SPI init\n");
+  register_set(&(SPI1->CR1), SPI_CR1_SPE, 0xFFFFU);
+
+  // enable SPI interrupts
+  //SPI1->CR2 = SPI_CR2_RXNEIE | SPI_CR2_ERRIE | SPI_CR2_TXEIE;
+  register_set(&(SPI1->CR2), SPI_CR2_RXNEIE, 0xF7U);
+
+  NVIC_EnableIRQ(DMA2_Stream2_IRQn);
+  NVIC_EnableIRQ(DMA2_Stream3_IRQn);
+  //NVIC_EnableIRQ(SPI1_IRQn);
+
+  // reset handshake back to pull up
+  set_gpio_mode(GPIOB, 0, MODE_INPUT);
+  set_gpio_pullup(GPIOB, 0, PULL_UP);
+
+  // setup interrupt on falling edge of SPI enable (on PA4)
+  register_set(&(SYSCFG->EXTICR[2]), SYSCFG_EXTICR2_EXTI4_PA, 0xFFFFU);
+  register_set_bits(&(EXTI->IMR), (1U << 4));
+  register_set_bits(&(EXTI->FTSR), (1U << 4));
+  NVIC_EnableIRQ(EXTI4_IRQn);
+}
\ No newline at end of file
diff --git a/panda/board/drivers/timer.h b/panda/board/drivers/timer.h
index a14b619e4b..d7aa7e8811 100644
--- a/panda/board/drivers/timer.h
+++ b/panda/board/drivers/timer.h
@@ -1,7 +1,7 @@
 void timer_init(TIM_TypeDef *TIM, int psc) {
-  TIM->PSC = psc-1;
-  TIM->DIER = TIM_DIER_UIE;
-  TIM->CR1 = TIM_CR1_CEN;
+  register_set(&(TIM->PSC), (psc-1), 0xFFFFU);
+  register_set(&(TIM->DIER), TIM_DIER_UIE, 0x5F5FU);
+  register_set(&(TIM->CR1), TIM_CR1_CEN, 0x3FU);
   TIM->SR = 0;
 }
 
diff --git a/panda/board/drivers/uart.h b/panda/board/drivers/uart.h
index 3a095f6726..da45803001 100644
--- a/panda/board/drivers/uart.h
+++ b/panda/board/drivers/uart.h
@@ -197,12 +197,12 @@ void uart_interrupt_handler(uart_ring *q) {
   EXIT_CRITICAL();
 }
 
-void USART1_IRQHandler(void) { uart_interrupt_handler(&uart_ring_esp_gps); }
-void USART2_IRQHandler(void) { uart_interrupt_handler(&uart_ring_debug); }
-void USART3_IRQHandler(void) { uart_interrupt_handler(&uart_ring_lin2); }
-void UART5_IRQHandler(void) { uart_interrupt_handler(&uart_ring_lin1); }
+void USART1_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_esp_gps); }
+void USART2_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_debug); }
+void USART3_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_lin2); }
+void UART5_IRQ_Handler(void) { uart_interrupt_handler(&uart_ring_lin1); }
 
-void DMA2_Stream5_IRQHandler(void) {
+void DMA2_Stream5_IRQ_Handler(void) {
   ENTER_CRITICAL();
 
   // Handle errors
@@ -272,6 +272,13 @@ void uart_set_baud(USART_TypeDef *u, unsigned int baud) {
 }
 
 void uart_init(uart_ring *q, int baud) {
+  // Register interrupts (max data rate: 115200 baud)
+  REGISTER_INTERRUPT(USART1_IRQn, USART1_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_1)
+  REGISTER_INTERRUPT(USART2_IRQn, USART2_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_2)
+  REGISTER_INTERRUPT(USART3_IRQn, USART3_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_3)
+  REGISTER_INTERRUPT(UART5_IRQn, UART5_IRQ_Handler, 150000U, FAULT_INTERRUPT_RATE_UART_5)
+  REGISTER_INTERRUPT(DMA2_Stream5_IRQn, DMA2_Stream5_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_UART_DMA)   // Called twice per buffer
+
   // Set baud and enable peripheral with TX and RX mode
   uart_set_baud(q->uart, baud);
   q->uart->CR1 = USART_CR1_UE | USART_CR1_TE | USART_CR1_RE;
diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h
index 88455c71ca..a970194ffd 100644
--- a/panda/board/drivers/usb.h
+++ b/panda/board/drivers/usb.h
@@ -281,15 +281,8 @@ uint8_t binary_object_store_desc[] = {
   // BOS header
   BINARY_OBJECT_STORE_DESCRIPTOR_LENGTH, // bLength, this is only the length of the header
   BINARY_OBJECT_STORE_DESCRIPTOR, // bDescriptorType
-  0x40, 0x00, // wTotalLength (LSB, MSB)
-  0x03, // bNumDeviceCaps (USB 2.0 + WebUSB + WinUSB)
-
-  // -------------------------------------------------
-  // USB 2.0 extension descriptor
-  0x07, // bLength, Descriptor size
-  0x10, // bDescriptorType, Device Capability Descriptor Type
-  0x02, // bDevCapabilityType, USB 2.0 extension capability type
-  0x00, 0x00, 0x00, 0x00, // bmAttributes, LIBUSB_BM_LPM_SUPPORT = 2 and its the only option
+  0x39, 0x00, // wTotalLength (LSB, MSB)
+  0x02, // bNumDeviceCaps (WebUSB + WinUSB)
 
   // -------------------------------------------------
   // WebUSB descriptor
@@ -667,76 +660,7 @@ void usb_setup(void) {
   }
 }
 
-void usb_init(void) {
-  // full speed PHY, do reset and remove power down
-  /*puth(USBx->GRSTCTL);
-  puts(" resetting PHY\n");*/
-  while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
-  //puts("AHB idle\n");
-
-  // reset PHY here
-  USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
-  while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST);
-  //puts("reset done\n");
-
-  // internal PHY, force device mode
-  USBx->GUSBCFG = USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_FDMOD;
-
-  // slowest timings
-  USBx->GUSBCFG |= ((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT);
-
-  // power up the PHY
-#ifdef STM32F4
-  USBx->GCCFG = USB_OTG_GCCFG_PWRDWN;
-
-  //USBx->GCCFG |= USB_OTG_GCCFG_VBDEN | USB_OTG_GCCFG_SDEN |USB_OTG_GCCFG_PDEN | USB_OTG_GCCFG_DCDEN;
-
-  /* B-peripheral session valid override enable*/
-  USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
-  USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN;
-#else
-  USBx->GCCFG = USB_OTG_GCCFG_PWRDWN | USB_OTG_GCCFG_NOVBUSSENS;
-#endif
 
-  // be a device, slowest timings
-  //USBx->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_TRDT | USB_OTG_GUSBCFG_TOCAL;
-  //USBx->GUSBCFG |= (uint32_t)((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT);
-  //USBx->GUSBCFG = USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_TRDT | USB_OTG_GUSBCFG_TOCAL;
-
-  // **** for debugging, doesn't seem to work ****
-  //USBx->GUSBCFG |= USB_OTG_GUSBCFG_CTXPKT;
-
-  // reset PHY clock
-  USBx_PCGCCTL = 0;
-
-  // enable the fancy OTG things
-  // DCFG_FRAME_INTERVAL_80 is 0
-  //USBx->GUSBCFG |= USB_OTG_GUSBCFG_HNPCAP | USB_OTG_GUSBCFG_SRPCAP;
-  USBx_DEVICE->DCFG |= USB_OTG_SPEED_FULL | USB_OTG_DCFG_NZLSOHSK;
-
-  //USBx_DEVICE->DCFG = USB_OTG_DCFG_NZLSOHSK | USB_OTG_DCFG_DSPD;
-  //USBx_DEVICE->DCFG = USB_OTG_DCFG_DSPD;
-
-  // clear pending interrupts
-  USBx->GINTSTS = 0xBFFFFFFFU;
-
-  // setup USB interrupts
-  // all interrupts except TXFIFO EMPTY
-  //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF);
-  //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM);
-  USBx->GINTMSK = USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_OTGINT |
-                  USB_OTG_GINTMSK_RXFLVLM | USB_OTG_GINTMSK_GONAKEFFM | USB_OTG_GINTMSK_GINAKEFFM |
-                  USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_USBSUSPM |
-                  USB_OTG_GINTMSK_CIDSCHGM | USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_MMISM;
-
-  USBx->GAHBCFG = USB_OTG_GAHBCFG_GINT;
-
-  // DCTL startup value is 2 on new chip, 0 on old chip
-  USBx_DEVICE->DCTL = 0;
-
-  // enable the IRQ
-  NVIC_EnableIRQ(OTG_FS_IRQn);
-}
 
 // ***************************** USB port *****************************
 
@@ -1008,7 +932,7 @@ void usb_irqhandler(void) {
   //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF);
 }
 
-void OTG_FS_IRQHandler(void) {
+void OTG_FS_IRQ_Handler(void) {
   NVIC_DisableIRQ(OTG_FS_IRQn);
   //__disable_irq();
   usb_irqhandler();
@@ -1016,3 +940,77 @@ void OTG_FS_IRQHandler(void) {
   NVIC_EnableIRQ(OTG_FS_IRQn);
 }
 
+// ***************************** USB init *****************************
+
+void usb_init(void) {
+  REGISTER_INTERRUPT(OTG_FS_IRQn, OTG_FS_IRQ_Handler, 1500000U, FAULT_INTERRUPT_RATE_USB) //TODO: Find out a better rate limit for USB. Now it's the 1.5MB/s rate
+
+  // full speed PHY, do reset and remove power down
+  /*puth(USBx->GRSTCTL);
+  puts(" resetting PHY\n");*/
+  while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0);
+  //puts("AHB idle\n");
+
+  // reset PHY here
+  USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
+  while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST);
+  //puts("reset done\n");
+
+  // internal PHY, force device mode
+  USBx->GUSBCFG = USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_FDMOD;
+
+  // slowest timings
+  USBx->GUSBCFG |= ((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT);
+
+  // power up the PHY
+#ifdef STM32F4
+  USBx->GCCFG = USB_OTG_GCCFG_PWRDWN;
+
+  //USBx->GCCFG |= USB_OTG_GCCFG_VBDEN | USB_OTG_GCCFG_SDEN |USB_OTG_GCCFG_PDEN | USB_OTG_GCCFG_DCDEN;
+
+  /* B-peripheral session valid override enable*/
+  USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOVAL;
+  USBx->GOTGCTL |= USB_OTG_GOTGCTL_BVALOEN;
+#else
+  USBx->GCCFG = USB_OTG_GCCFG_PWRDWN | USB_OTG_GCCFG_NOVBUSSENS;
+#endif
+
+  // be a device, slowest timings
+  //USBx->GUSBCFG = USB_OTG_GUSBCFG_FDMOD | USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_TRDT | USB_OTG_GUSBCFG_TOCAL;
+  //USBx->GUSBCFG |= (uint32_t)((USBD_FS_TRDT_VALUE << 10) & USB_OTG_GUSBCFG_TRDT);
+  //USBx->GUSBCFG = USB_OTG_GUSBCFG_PHYSEL | USB_OTG_GUSBCFG_TRDT | USB_OTG_GUSBCFG_TOCAL;
+
+  // **** for debugging, doesn't seem to work ****
+  //USBx->GUSBCFG |= USB_OTG_GUSBCFG_CTXPKT;
+
+  // reset PHY clock
+  USBx_PCGCCTL = 0;
+
+  // enable the fancy OTG things
+  // DCFG_FRAME_INTERVAL_80 is 0
+  //USBx->GUSBCFG |= USB_OTG_GUSBCFG_HNPCAP | USB_OTG_GUSBCFG_SRPCAP;
+  USBx_DEVICE->DCFG |= USB_OTG_SPEED_FULL | USB_OTG_DCFG_NZLSOHSK;
+
+  //USBx_DEVICE->DCFG = USB_OTG_DCFG_NZLSOHSK | USB_OTG_DCFG_DSPD;
+  //USBx_DEVICE->DCFG = USB_OTG_DCFG_DSPD;
+
+  // clear pending interrupts
+  USBx->GINTSTS = 0xBFFFFFFFU;
+
+  // setup USB interrupts
+  // all interrupts except TXFIFO EMPTY
+  //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF);
+  //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM);
+  USBx->GINTMSK = USB_OTG_GINTMSK_USBRST | USB_OTG_GINTMSK_ENUMDNEM | USB_OTG_GINTMSK_OTGINT |
+                  USB_OTG_GINTMSK_RXFLVLM | USB_OTG_GINTMSK_GONAKEFFM | USB_OTG_GINTMSK_GINAKEFFM |
+                  USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT | USB_OTG_GINTMSK_USBSUSPM |
+                  USB_OTG_GINTMSK_CIDSCHGM | USB_OTG_GINTMSK_SRQIM | USB_OTG_GINTMSK_MMISM;
+
+  USBx->GAHBCFG = USB_OTG_GAHBCFG_GINT;
+
+  // DCTL startup value is 2 on new chip, 0 on old chip
+  USBx_DEVICE->DCTL = 0;
+
+  // enable the IRQ
+  NVIC_EnableIRQ(OTG_FS_IRQn);
+}
\ No newline at end of file
diff --git a/panda/board/faults.h b/panda/board/faults.h
new file mode 100644
index 0000000000..aa5db341b5
--- /dev/null
+++ b/panda/board/faults.h
@@ -0,0 +1,49 @@
+#define FAULT_STATUS_NONE 0U
+#define FAULT_STATUS_TEMPORARY 1U
+#define FAULT_STATUS_PERMANENT 2U
+
+// Fault types
+#define FAULT_RELAY_MALFUNCTION         (1U << 0)
+#define FAULT_UNUSED_INTERRUPT_HANDLED  (1U << 1)
+#define FAULT_INTERRUPT_RATE_CAN_1      (1U << 2)
+#define FAULT_INTERRUPT_RATE_CAN_2      (1U << 3)
+#define FAULT_INTERRUPT_RATE_CAN_3      (1U << 4)
+#define FAULT_INTERRUPT_RATE_TACH       (1U << 5)
+#define FAULT_INTERRUPT_RATE_GMLAN      (1U << 6)
+#define FAULT_INTERRUPT_RATE_INTERRUPTS (1U << 7)
+#define FAULT_INTERRUPT_RATE_SPI_DMA    (1U << 8)
+#define FAULT_INTERRUPT_RATE_SPI_CS     (1U << 9)
+#define FAULT_INTERRUPT_RATE_UART_1     (1U << 10)
+#define FAULT_INTERRUPT_RATE_UART_2     (1U << 11)
+#define FAULT_INTERRUPT_RATE_UART_3     (1U << 12)
+#define FAULT_INTERRUPT_RATE_UART_5     (1U << 13)
+#define FAULT_INTERRUPT_RATE_UART_DMA   (1U << 14)
+#define FAULT_INTERRUPT_RATE_USB        (1U << 15)
+#define FAULT_INTERRUPT_RATE_TIM1       (1U << 16)
+#define FAULT_INTERRUPT_RATE_TIM3       (1U << 17)
+#define FAULT_REGISTER_DIVERGENT        (1U << 18)
+
+// Permanent faults
+#define PERMANENT_FAULTS 0U
+
+uint8_t fault_status = FAULT_STATUS_NONE;
+uint32_t faults = 0U;
+
+void fault_occurred(uint32_t fault) {
+  faults |= fault;
+  if((PERMANENT_FAULTS & fault) != 0U){
+    puts("Permanent fault occurred: 0x"); puth(fault); puts("\n");
+    fault_status = FAULT_STATUS_PERMANENT;
+  } else {
+    puts("Temporary fault occurred: 0x"); puth(fault); puts("\n");
+    fault_status = FAULT_STATUS_TEMPORARY;
+  }
+}
+
+void fault_recovered(uint32_t fault) {
+  if((PERMANENT_FAULTS & fault) == 0U){
+    faults &= ~fault;
+  } else {
+    puts("Cannot recover from a permanent fault!\n");
+  }
+}
\ No newline at end of file
diff --git a/panda/board/gpio.h b/panda/board/gpio.h
index f33196e8fa..2b937eced4 100644
--- a/panda/board/gpio.h
+++ b/panda/board/gpio.h
@@ -23,6 +23,9 @@ void early(void) {
   // Reset global critical depth
   global_critical_depth = 0;
 
+  // Init register and interrupt tables
+  init_registers();
+
   // neccesary for DFU flashing on a non-power cycled white panda
   enable_interrupts();
 
@@ -59,13 +62,6 @@ void early(void) {
   detect_configuration();
   detect_board_type();
 
-  #ifdef PANDA
-    // enable the ESP, disable ESP boot mode
-    // dont disable on grey panda
-    current_board->set_esp_gps_mode(ESP_GPS_ENABLED);
-  #endif
-
-
   if (enter_bootloader_mode == ENTER_BOOTLOADER_MAGIC) {
   #ifdef PANDA
     current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
diff --git a/panda/board/inc/stm32f413xx.h b/panda/board/inc/stm32f413xx.h
index 0fd9f4c387..0962a8def1 100644
--- a/panda/board/inc/stm32f413xx.h
+++ b/panda/board/inc/stm32f413xx.h
@@ -141,6 +141,7 @@ typedef enum
   TIM8_TRG_COM_TIM14_IRQn     = 45,     /*!< TIM8 Trigger and Commutation Interrupt and TIM14 global interrupt */
   TIM8_CC_IRQn                = 46,     /*!< TIM8 Capture Compare global interrupt                             */
   DMA1_Stream7_IRQn           = 47,     /*!< DMA1 Stream7 Interrupt                                            */
+  FSMC_IRQn                   = 48,     /*!< FSMC global Interrupt                                             */
   SDIO_IRQn                   = 49,     /*!< SDIO global Interrupt                                             */
   TIM5_IRQn                   = 50,     /*!< TIM5 global Interrupt                                             */
   SPI3_IRQn                   = 51,     /*!< SPI3 global Interrupt                                             */
diff --git a/panda/board/libc.h b/panda/board/libc.h
index f8d9dbce2a..67411fbb31 100644
--- a/panda/board/libc.h
+++ b/panda/board/libc.h
@@ -40,28 +40,3 @@ int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {
   return ret;
 }
 
-// ********************* IRQ helpers *********************
-
-volatile bool interrupts_enabled = false;
-
-void enable_interrupts(void) {
-  interrupts_enabled = true;
-  __enable_irq();
-}
-
-void disable_interrupts(void) {
-  interrupts_enabled = false;
-  __disable_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();                                           \
-  }
-
diff --git a/panda/board/main.c b/panda/board/main.c
index 3bcae876e2..dd4edb33d1 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -5,10 +5,15 @@
 #include "config.h"
 #include "obj/gitversion.h"
 
+#include "main_declarations.h"
+#include "critical.h"
+
 #include "libc.h"
 #include "provision.h"
+#include "faults.h"
 
-#include "main_declarations.h"
+#include "drivers/registers.h"
+#include "drivers/interrupts.h"
 
 #include "drivers/llcan.h"
 #include "drivers/llgpio.h"
@@ -34,6 +39,28 @@
 
 #include "drivers/can.h"
 
+extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used
+
+struct __attribute__((packed)) health_t {
+  uint32_t uptime_pkt;
+  uint32_t voltage_pkt;
+  uint32_t current_pkt;
+  uint32_t can_send_errs_pkt;
+  uint32_t can_fwd_errs_pkt;
+  uint32_t gmlan_send_errs_pkt;
+  uint32_t faults_pkt;
+  uint8_t ignition_line_pkt;
+  uint8_t ignition_can_pkt;
+  uint8_t controls_allowed_pkt;
+  uint8_t gas_interceptor_detected_pkt;
+  uint8_t car_harness_status_pkt;
+  uint8_t usb_power_mode_pkt;
+  uint8_t safety_mode_pkt;
+  uint8_t fault_status_pkt;
+  uint8_t power_save_enabled_pkt;
+};
+
+
 // ********************* Serial debugging *********************
 
 bool check_started(void) {
@@ -45,11 +72,14 @@ void debug_ring_callback(uart_ring *ring) {
   while (getc(ring, &rcv)) {
     (void)putc(ring, rcv);  // misra-c2012-17.7: cast to void is ok: debug function
 
-    // jump to DFU flash
-    if (rcv == 'z') {
-      enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
-      NVIC_SystemReset();
-    }
+    // only allow bootloader entry on debug builds
+    #ifdef ALLOW_DEBUG
+      // jump to DFU flash
+      if (rcv == 'z') {
+        enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
+        NVIC_SystemReset();
+      }
+    #endif
 
     // normal reset
     if (rcv == 'x') {
@@ -72,100 +102,65 @@ void debug_ring_callback(uart_ring *ring) {
   }
 }
 
-// ***************************** started logic *****************************
-void started_interrupt_handler(uint8_t interrupt_line) {
-  volatile unsigned int pr = EXTI->PR & (1U << interrupt_line);
-  if ((pr & (1U << interrupt_line)) != 0U) {
-    #ifdef DEBUG
-      puts("got started interrupt\n");
-    #endif
-
-    // jenky debounce
-    delay(100000);
-
-    #ifdef EON
-      // set power savings mode here if on EON build
-      int power_save_state = check_started() ? POWER_SAVE_STATUS_DISABLED : POWER_SAVE_STATUS_ENABLED;
-      set_power_save_state(power_save_state);
-      // set CDP usb power mode everytime that the car starts to make sure EON is charging
-      if (check_started()) {
-        current_board->set_usb_power_mode(USB_POWER_CDP);
-      }
-    #endif
-  }
-  EXTI->PR = (1U << interrupt_line);
-}
-
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void EXTI0_IRQHandler(void) {
-  started_interrupt_handler(0);
-}
-
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void EXTI1_IRQHandler(void) {
-  started_interrupt_handler(1);
-}
-
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void EXTI3_IRQHandler(void) {
-  started_interrupt_handler(3);
-}
-
 // ****************************** safety mode ******************************
 
 // this is the only way to leave silent mode
 void set_safety_mode(uint16_t mode, int16_t param) {
-  int err = safety_set_mode(mode, param);
+  uint16_t mode_copy = mode;
+  int err = set_safety_hooks(mode_copy, param);
   if (err == -1) {
-    puts("Error: safety set mode failed\n");
-  } else {
-    switch (mode) {
-        case SAFETY_NOOUTPUT:
-          set_intercept_relay(false);
-          if(board_has_obd()){
-            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(board_has_obd()){
-            current_board->set_can_mode(CAN_MODE_OBD_CAN2);
-          }
-          can_silent = ALL_CAN_LIVE;
-          break;
-        default:
-          set_intercept_relay(true);
-          heartbeat_counter = 0U;
-          if(board_has_obd()){
-            current_board->set_can_mode(CAN_MODE_NORMAL);
-          }
-          can_silent = ALL_CAN_LIVE;
-          break;
+    puts("Error: safety set mode failed. Falling back to SILENT\n");
+    mode_copy = SAFETY_SILENT;
+    err = set_safety_hooks(mode_copy, 0);
+    if (err == -1) {
+      puts("Error: Failed setting SILENT mode. Hanging\n");
+      while (true) {
+        // TERMINAL ERROR: we can't continue if SILENT safety mode isn't succesfully set
+      }
+    }
+  }
+  switch (mode_copy) {
+    case SAFETY_SILENT:
+      set_intercept_relay(false);
+      if (board_has_obd()) {
+        current_board->set_can_mode(CAN_MODE_NORMAL);
       }
-    can_init_all();
+      can_silent = ALL_CAN_SILENT;
+      break;
+    case SAFETY_NOOUTPUT:
+      set_intercept_relay(false);
+      if (board_has_obd()) {
+        current_board->set_can_mode(CAN_MODE_NORMAL);
+      }
+      can_silent = ALL_CAN_LIVE;
+      break;
+    case SAFETY_ELM327:
+      set_intercept_relay(false);
+      heartbeat_counter = 0U;
+      if (board_has_obd()) {
+        current_board->set_can_mode(CAN_MODE_OBD_CAN2);
+      }
+      can_silent = ALL_CAN_LIVE;
+      break;
+    default:
+      set_intercept_relay(true);
+      heartbeat_counter = 0U;
+      if (board_has_obd()) {
+        current_board->set_can_mode(CAN_MODE_NORMAL);
+      }
+      can_silent = ALL_CAN_LIVE;
+      break;
   }
+  can_init_all();
 }
 
 // ***************************** USB port *****************************
 
 int get_health_pkt(void *dat) {
-  struct __attribute__((packed)) {
-    uint32_t voltage_pkt;
-    uint32_t current_pkt;
-    uint32_t can_send_errs_pkt;
-    uint32_t can_fwd_errs_pkt;
-    uint32_t gmlan_send_errs_pkt;
-    uint8_t ignition_line_pkt;
-    uint8_t ignition_can_pkt;
-    uint8_t controls_allowed_pkt;
-    uint8_t gas_interceptor_detected_pkt;
-    uint8_t car_harness_status_pkt;
-    uint8_t usb_power_mode_pkt;
-    uint8_t safety_mode_pkt;
-  } *health = dat;
+  COMPILE_TIME_ASSERT(sizeof(struct health_t) <= MAX_RESP_LEN);
+  struct health_t * health = (struct health_t*)dat;
 
+  health->uptime_pkt = uptime_cnt;
   health->voltage_pkt = adc_get_voltage();
   health->current_pkt = current_board->read_current();
 
@@ -181,6 +176,10 @@ int get_health_pkt(void *dat) {
   health->car_harness_status_pkt = car_harness_status;
   health->usb_power_mode_pkt = usb_power_mode;
   health->safety_mode_pkt = (uint8_t)(current_safety_mode);
+  health->power_save_enabled_pkt = (uint8_t)(power_save_status == POWER_SAVE_STATUS_ENABLED);
+
+  health->fault_status_pkt = fault_status;
+  health->faults_pkt = faults;
 
   return sizeof(*health);
 }
@@ -230,7 +229,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) {
     to_push.RIR = d32[dpkt];
 
     uint8_t bus_number = (to_push.RDTR >> 4) & CAN_BUS_NUM_MASK;
-    can_send(&to_push, bus_number);
+    can_send(&to_push, bus_number, false);
   }
 }
 
@@ -313,6 +312,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
       resp[1] = ((fan_rpm & 0xFF00U) >> 8U);
       resp_len = 2;
       break;
+    // **** 0xb3: set phone power
+    case 0xb3:
+      current_board->set_phone_power(setup->b.wValue.w > 0U);
+      break;
     // **** 0xc0: get CAN debug info
     case 0xc0:
       puts("can tx: "); puth(can_tx_cnt);
@@ -366,6 +369,24 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
     case 0xd2:
       resp_len = get_health_pkt(resp);
       break;
+    // **** 0xd3: get first 64 bytes of signature
+    case 0xd3:
+      {
+        resp_len = 64;
+        char * code = (char*)_app_start;
+        int code_len = _app_start[0];
+        (void)memcpy(resp, &code[code_len], resp_len);
+      }
+      break;
+    // **** 0xd4: get second 64 bytes of signature
+    case 0xd4:
+      {
+        resp_len = 64;
+        char * code = (char*)_app_start;
+        int code_len = _app_start[0];
+        (void)memcpy(resp, &code[code_len + 64], resp_len);
+      }
+      break;
     // **** 0xd6: get version
     case 0xd6:
       COMPILE_TIME_ASSERT(sizeof(gitversion) <= MAX_RESP_LEN);
@@ -427,8 +448,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
     // **** 0xdc: set safety mode
     case 0xdc:
       // Blocked over WiFi.
-      // Allow NOOUTPUT and ELM security mode to be set over wifi.
-      if (hardwired || (setup->b.wValue.w == SAFETY_NOOUTPUT) || (setup->b.wValue.w == SAFETY_ELM327)) {
+      // Allow SILENT, NOOUTPUT and ELM security mode to be set over wifi.
+      if (hardwired || (setup->b.wValue.w == SAFETY_SILENT) ||
+                       (setup->b.wValue.w == SAFETY_NOOUTPUT) ||
+                       (setup->b.wValue.w == SAFETY_ELM327)) {
         set_safety_mode(setup->b.wValue.w, (uint16_t) setup->b.wIndex.w);
       }
       break;
@@ -526,6 +549,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired)
     case 0xe6:
       current_board->set_usb_power_mode(setup->b.wValue.w);
       break;
+    // **** 0xe7: set power save state
+    case 0xe7:
+      set_power_save_state(setup->b.wValue.w);
+      break;
     // **** 0xf0: do k-line wValue pulse on uart2 for Acura
     case 0xf0:
       if (setup->b.wValue.w == 1U) {
@@ -640,24 +667,21 @@ void __attribute__ ((noinline)) enable_fpu(void) {
   SCB->CPACR |= ((3UL << (10U * 2U)) | (3UL << (11U * 2U)));
 }
 
-uint64_t tcnt = 0;
-
-// go into NOOUTPUT when the EON does not send a heartbeat for this amount of seconds.
+// go into SILENT when the EON does not send a heartbeat for this amount of seconds.
 #define EON_HEARTBEAT_IGNITION_CNT_ON 5U
 #define EON_HEARTBEAT_IGNITION_CNT_OFF 2U
 
 // called once per second
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void TIM1_BRK_TIM9_IRQHandler(void) {
+void TIM1_BRK_TIM9_IRQ_Handler(void) {
   if (TIM9->SR != 0) {
     can_live = pending_can_live;
 
-    current_board->usb_power_mode_tick(tcnt);
+    current_board->usb_power_mode_tick(uptime_cnt);
 
     //puth(usart1_dma); puts(" "); puth(DMA2_Stream5->M0AR); puts(" "); puth(DMA2_Stream5->NDTR); puts("\n");
 
     // reset this every 16th pass
-    if ((tcnt & 0xFU) == 0U) {
+    if ((uptime_cnt & 0xFU) == 0U) {
       pending_can_live = 0;
     }
     #ifdef DEBUG
@@ -676,30 +700,57 @@ void TIM1_BRK_TIM9_IRQHandler(void) {
 
     // turn off the blue LED, turned on by CAN
     // unless we are in power saving mode
-    current_board->set_led(LED_BLUE, (tcnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
+    current_board->set_led(LED_BLUE, (uptime_cnt & 1U) && (power_save_status == POWER_SAVE_STATUS_ENABLED));
 
     // increase heartbeat counter and cap it at the uint32 limit
     if (heartbeat_counter < __UINT32_MAX__) {
       heartbeat_counter += 1U;
     }
 
-    // check heartbeat counter if we are running EON code. If the heartbeat has been gone for a while, go to NOOUTPUT safety mode.
     #ifdef EON
+    // check heartbeat counter if we are running EON code.
+    // if the heartbeat has been gone for a while, go to SILENT safety mode and enter power save
     if (heartbeat_counter >= (check_started() ? EON_HEARTBEAT_IGNITION_CNT_ON : EON_HEARTBEAT_IGNITION_CNT_OFF)) {
-      puts("EON hasn't sent a heartbeat for 0x"); puth(heartbeat_counter); puts(" seconds. Safety is set to NOOUTPUT mode.\n");
-      if(current_safety_mode != SAFETY_NOOUTPUT){
-        set_safety_mode(SAFETY_NOOUTPUT, 0U);
+      puts("EON hasn't sent a heartbeat for 0x");
+      puth(heartbeat_counter);
+      puts(" seconds. Safety is set to SILENT mode.\n");
+      if (current_safety_mode != SAFETY_SILENT) {
+        set_safety_mode(SAFETY_SILENT, 0U);
+      }
+      if (power_save_status != POWER_SAVE_STATUS_ENABLED) {
+        set_power_save_state(POWER_SAVE_STATUS_ENABLED);
       }
     }
+
+    // enter CDP mode when car starts to ensure we are charging a turned off EON
+    if (check_started() && (usb_power_mode != USB_POWER_CDP)) {
+      current_board->set_usb_power_mode(USB_POWER_CDP);
+    }
     #endif
 
+    // check registers
+    check_registers();
+
+    // set ignition_can to false after 2s of no CAN seen
+    if (ignition_can_cnt > 2U) {
+      ignition_can = false;
+    };
+
     // on to the next one
-    tcnt += 1U;
+    uptime_cnt += 1U;
+    safety_mode_cnt += 1U;
+    ignition_can_cnt += 1U;
   }
   TIM9->SR = 0;
 }
 
 int main(void) {
+  // Init interrupt table
+  init_interrupts(true);
+
+  // 1s timer
+  REGISTER_INTERRUPT(TIM1_BRK_TIM9_IRQn, TIM1_BRK_TIM9_IRQ_Handler, 2U, FAULT_INTERRUPT_RATE_TIM1)
+
   // shouldn't have interrupts here, but just in case
   disable_interrupts();
 
@@ -760,32 +811,16 @@ int main(void) {
   TIM2->EGR = TIM_EGR_UG;
   // use TIM2->CNT to read
 
-  // default to silent mode to prevent issues with Ford
-  // hardcode a specific safety mode if you want to force the panda to be in a specific mode
-  int err = safety_set_mode(SAFETY_NOOUTPUT, 0);
-  if (err == -1) {
-    puts("Failed to set safety mode\n");
-    while (true) {
-      // if SAFETY_NOOUTPUT isn't succesfully set, we can't continue
-    }
-  }
-  can_silent = ALL_CAN_SILENT;
-  can_init_all();
+  // init to SILENT and can silent
+  set_safety_mode(SAFETY_SILENT, 0);
+
+  // enable CAN TXs
+  current_board->enable_can_transcievers(true);
 
 #ifndef EON
   spi_init();
 #endif
 
-#ifdef EON
-  // have to save power
-  if (hw_type == HW_TYPE_WHITE_PANDA) {
-    current_board->set_esp_gps_mode(ESP_GPS_DISABLED);
-  }
-  // only enter power save after the first cycle
-  /*if (check_started()) {
-    set_power_save_state(POWER_SAVE_STATUS_ENABLED);
-  }*/
-#endif
   // 1hz
   timer_init(TIM9, 1464);
   NVIC_EnableIRQ(TIM1_BRK_TIM9_IRQn);
@@ -804,19 +839,30 @@ int main(void) {
 
   for (cnt=0;;cnt++) {
     if (power_save_status == POWER_SAVE_STATUS_DISABLED) {
-      int div_mode = ((usb_power_mode == USB_POWER_DCP) ? 4 : 1);
-
-      // useful for debugging, fade breaks = panda is overloaded
-      for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) {
-        for (int fade = 0; fade < 1024; fade += 8) {
-          for (int i = 0; i < (128/div_mode); i++) {
-            current_board->set_led(LED_RED, 1);
-            if (fade < 512) { delay(fade); } else { delay(1024-fade); }
-            current_board->set_led(LED_RED, 0);
-            if (fade < 512) { delay(512-fade); } else { delay(fade-512); }
+      #ifdef DEBUG_FAULTS
+      if(fault_status == FAULT_STATUS_NONE){
+      #endif
+        int div_mode = ((usb_power_mode == USB_POWER_DCP) ? 4 : 1);
+
+        // useful for debugging, fade breaks = panda is overloaded
+        for (int div_mode_loop = 0; div_mode_loop < div_mode; div_mode_loop++) {
+          for (int fade = 0; fade < 1024; fade += 8) {
+            for (int i = 0; i < (128/div_mode); i++) {
+              current_board->set_led(LED_RED, 1);
+              if (fade < 512) { delay(fade); } else { delay(1024-fade); }
+              current_board->set_led(LED_RED, 0);
+              if (fade < 512) { delay(512-fade); } else { delay(fade-512); }
+            }
           }
         }
-      }
+      #ifdef DEBUG_FAULTS
+      } else {
+          current_board->set_led(LED_RED, 1);
+          delay(512000U);
+          current_board->set_led(LED_RED, 0);
+          delay(512000U);
+        }
+      #endif
     } else {
       __WFI();
     }
diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h
index 8929e9ac0e..363a992dbf 100644
--- a/panda/board/main_declarations.h
+++ b/panda/board/main_declarations.h
@@ -11,4 +11,5 @@ void can_set_obd(uint8_t harness_orientation, bool obd);
 uint8_t hw_type = 0;
 const board *current_board;
 bool is_enumerated = 0;
-uint32_t heartbeat_counter = 0;
\ No newline at end of file
+uint32_t heartbeat_counter = 0;
+uint32_t uptime_cnt = 0;
diff --git a/panda/board/pedal/main.c b/panda/board/pedal/main.c
index e7642f0c00..667d27bf0f 100644
--- a/panda/board/pedal/main.c
+++ b/panda/board/pedal/main.c
@@ -3,7 +3,11 @@
 #include "libc.h"
 
 #include "main_declarations.h"
+#include "critical.h"
+#include "faults.h"
 
+#include "drivers/registers.h"
+#include "drivers/interrupts.h"
 #include "drivers/llcan.h"
 #include "drivers/llgpio.h"
 #include "drivers/adc.h"
@@ -129,8 +133,7 @@ uint8_t pedal_checksum(uint8_t *dat, int len) {
 #define CAN_GAS_SIZE 6
 #define COUNTER_CYCLE 0xFU
 
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void CAN1_TX_IRQHandler(void) {
+void CAN1_TX_IRQ_Handler(void) {
   // clear interrupt
   CAN->TSR |= CAN_TSR_RQCP0;
 }
@@ -152,8 +155,7 @@ uint32_t current_index = 0;
 #define FAULT_INVALID 6U
 uint8_t state = FAULT_STARTUP;
 
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void CAN1_RX0_IRQHandler(void) {
+void CAN1_RX0_IRQ_Handler(void) {
   while ((CAN->RF0R & CAN_RF0R_FMP0) != 0) {
     #ifdef DEBUG
       puts("CAN RX\n");
@@ -216,8 +218,7 @@ void CAN1_RX0_IRQHandler(void) {
   }
 }
 
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void CAN1_SCE_IRQHandler(void) {
+void CAN1_SCE_IRQ_Handler(void) {
   state = FAULT_SCE;
   llcan_clear_send(CAN);
 }
@@ -228,8 +229,7 @@ unsigned int pkt_idx = 0;
 
 int led_value = 0;
 
-// cppcheck-suppress unusedFunction ; used in headers not included in cppcheck
-void TIM3_IRQHandler(void) {
+void TIM3_IRQ_Handler(void) {
   #ifdef DEBUG
     puth(TIM3->CNT);
     puts(" ");
@@ -296,6 +296,16 @@ void pedal(void) {
 }
 
 int main(void) {
+  // Init interrupt table
+  init_interrupts(true);
+
+  REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+  REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+  REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+
+  // Should run at around 732Hz (see init below)
+  REGISTER_INTERRUPT(TIM3_IRQn, TIM3_IRQ_Handler, 1000U, FAULT_INTERRUPT_RATE_TIM3)
+
   disable_interrupts();
 
   // init devices
diff --git a/panda/board/power_saving.h b/panda/board/power_saving.h
index d397884f6e..3ee2170d71 100644
--- a/panda/board/power_saving.h
+++ b/panda/board/power_saving.h
@@ -1,3 +1,6 @@
+// WARNING: To stay in compliance with the SIL2 rules laid out in STM UM1840, we should never implement any of the available hardware low power modes.
+// See rule: CoU_3
+
 #define POWER_SAVE_STATUS_DISABLED 0
 #define POWER_SAVE_STATUS_ENABLED 1
 
diff --git a/panda/board/safety.h b/panda/board/safety.h
index c68eda2c4a..0a216e5b1f 100644
--- a/panda/board/safety.h
+++ b/panda/board/safety.h
@@ -18,7 +18,7 @@
 #include "safety/safety_elm327.h"
 
 // from cereal.car.CarParams.SafetyModel
-#define SAFETY_NOOUTPUT 0U
+#define SAFETY_SILENT 0U
 #define SAFETY_HONDA 1U
 #define SAFETY_TOYOTA 2U
 #define SAFETY_ELM327 3U
@@ -35,8 +35,9 @@
 #define SAFETY_TOYOTA_IPAS 16U
 #define SAFETY_ALLOUTPUT 17U
 #define SAFETY_GM_ASCM 18U
+#define SAFETY_NOOUTPUT 19U
 
-uint16_t current_safety_mode = SAFETY_NOOUTPUT;
+uint16_t current_safety_mode = SAFETY_SILENT;
 const safety_hooks *current_hooks = &nooutput_hooks;
 
 void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push){
@@ -55,39 +56,54 @@ int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   return current_hooks->fwd(bus_num, to_fwd);
 }
 
+bool addr_allowed(int addr, int bus, const AddrBus addr_list[], int len) {
+  bool allowed = false;
+  for (int i = 0; i < len; i++) {
+    if ((addr == addr_list[i].addr) && (bus == addr_list[i].bus)) {
+      allowed = true;
+      break;
+    }
+  }
+  return allowed;
+}
+
 typedef struct {
   uint16_t id;
   const safety_hooks *hooks;
 } safety_hook_config;
 
 const safety_hook_config safety_hook_registry[] = {
-  {SAFETY_NOOUTPUT, &nooutput_hooks},
+  {SAFETY_SILENT, &nooutput_hooks},
   {SAFETY_HONDA, &honda_hooks},
   {SAFETY_TOYOTA, &toyota_hooks},
   {SAFETY_ELM327, &elm327_hooks},
   {SAFETY_GM, &gm_hooks},
   {SAFETY_HONDA_BOSCH, &honda_bosch_hooks},
-  {SAFETY_FORD, &ford_hooks},
-  {SAFETY_CADILLAC, &cadillac_hooks},
   {SAFETY_HYUNDAI, &hyundai_hooks},
   {SAFETY_CHRYSLER, &chrysler_hooks},
-  {SAFETY_TESLA, &tesla_hooks},
   {SAFETY_SUBARU, &subaru_hooks},
   {SAFETY_MAZDA, &mazda_hooks},
   {SAFETY_VOLKSWAGEN, &volkswagen_hooks},
+  {SAFETY_NOOUTPUT, &nooutput_hooks},
+#ifdef ALLOW_DEBUG
+  {SAFETY_CADILLAC, &cadillac_hooks},
   {SAFETY_TOYOTA_IPAS, &toyota_ipas_hooks},
+  {SAFETY_TESLA, &tesla_hooks},
   {SAFETY_ALLOUTPUT, &alloutput_hooks},
   {SAFETY_GM_ASCM, &gm_ascm_hooks},
+  {SAFETY_FORD, &ford_hooks},
+#endif
 };
 
-int safety_set_mode(uint16_t mode, int16_t param) {
-  int set_status = -1;   // not set
+int set_safety_hooks(uint16_t mode, int16_t param) {
+  safety_mode_cnt = 0U;  // reset safety mode timer
+  int set_status = -1;  // not set
   int hook_config_count = sizeof(safety_hook_registry) / sizeof(safety_hook_config);
   for (int i = 0; i < hook_config_count; i++) {
     if (safety_hook_registry[i].id == mode) {
       current_hooks = safety_hook_registry[i].hooks;
       current_safety_mode = safety_hook_registry[i].id;
-      set_status = 0;    // set
+      set_status = 0;  // set
       break;
     }
   }
@@ -205,7 +221,7 @@ float interpolate(struct lookup_t xy, float x) {
         float y0 = xy.y[i];
         float dx = xy.x[i+1] - x0;
         float dy = xy.y[i+1] - y0;
-        // dx should not be zero as xy.x is supposed ot be monotonic
+        // dx should not be zero as xy.x is supposed to be monotonic
         if (dx <= 0.) {
           dx = 0.0001;
         }
diff --git a/panda/board/safety/safety_cadillac.h b/panda/board/safety/safety_cadillac.h
index 4ae2045505..ccfa78d3f7 100644
--- a/panda/board/safety/safety_cadillac.h
+++ b/panda/board/safety/safety_cadillac.h
@@ -1,5 +1,6 @@
 #define CADILLAC_TORQUE_MSG_N 4      // 4 torque messages: 0x151, 0x152, 0x153, 0x154
 
+const AddrBus CADILLAC_TX_MSGS[] = {{0x151, 2}, {0x152, 0}, {0x153, 2}, {0x154, 0}};
 const int CADILLAC_MAX_STEER = 150; // 1s
 // real time torque limit to prevent controls spamming
 // the real time limit is 1500/sec
@@ -55,6 +56,11 @@ static void cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
+
+  if (!addr_allowed(addr, bus, CADILLAC_TX_MSGS, sizeof(CADILLAC_TX_MSGS) / sizeof(CADILLAC_TX_MSGS[0]))) {
+    tx = 0;
+  }
 
   // steer cmd checks
   if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) {
@@ -109,13 +115,8 @@ static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static void cadillac_init(int16_t param) {
-  UNUSED(param);
-  controls_allowed = 0;
-}
-
 const safety_hooks cadillac_hooks = {
-  .init = cadillac_init,
+  .init = nooutput_init,
   .rx = cadillac_rx_hook,
   .tx = cadillac_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h
index 9b8fc36154..ce9f65f2af 100644
--- a/panda/board/safety/safety_chrysler.h
+++ b/panda/board/safety/safety_chrysler.h
@@ -4,8 +4,8 @@ const uint32_t CHRYSLER_RT_INTERVAL = 250000;  // 250ms between real time checks
 const int CHRYSLER_MAX_RATE_UP = 3;
 const int CHRYSLER_MAX_RATE_DOWN = 3;
 const int CHRYSLER_MAX_TORQUE_ERROR = 80;    // max torque cmd in excess of torque motor
+const AddrBus CHRYSLER_TX_MSGS[] = {{571, 0}, {658, 0}, {678, 0}};
 
-bool chrysler_camera_detected = 0;             // is giraffe switch 2 high?
 int chrysler_rt_torque_last = 0;
 int chrysler_desired_torque_last = 0;
 int chrysler_cruise_engaged_last = 0;
@@ -36,23 +36,25 @@ static void chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     chrysler_cruise_engaged_last = cruise_engaged;
   }
 
-  // check if stock camera ECU is still online
-  if ((bus == 0) && (addr == 0x292)) {
-    chrysler_camera_detected = 1;
-    controls_allowed = 0;
+  // check if stock camera ECU is on bus 0
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x292)) {
+    relay_malfunction = true;
   }
 }
 
 static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   int tx = 1;
+  int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
 
-  // If camera is on bus 0, then nothing can be sent
-  if (chrysler_camera_detected) {
+  if (!addr_allowed(addr, bus, CHRYSLER_TX_MSGS, sizeof(CHRYSLER_TX_MSGS) / sizeof(CHRYSLER_TX_MSGS[0]))) {
     tx = 0;
   }
 
-  int addr = GET_ADDR(to_send);
+  if (relay_malfunction) {
+    tx = 0;
+  }
 
   // LKA STEER
   if (addr == 0x292) {
@@ -100,39 +102,37 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     }
   }
 
-  // FORCE CANCEL: safety check only relevant when spamming the cancel button.
-  // ensuring that only the cancel button press is sent when controls are off.
-  // This avoids unintended engagements while still allowing resume spam
-  // TODO: fix bug preventing the button msg to be fwd'd on bus 2
+  // FORCE CANCEL: only the cancel button press is allowed
+  if (addr == 571) {
+    if (GET_BYTE(to_send, 0) != 1) {
+      tx = 0;
+    }
+  }
 
-  // 1 allows the message through
   return tx;
 }
 
-static void chrysler_init(int16_t param) {
-  UNUSED(param);
-  controls_allowed = 0;
-  chrysler_camera_detected = 0;
-}
-
 static int chrysler_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   int bus_fwd = -1;
   int addr = GET_ADDR(to_fwd);
-  // forward CAN 0 -> 2 so stock LKAS camera sees messages
-  if ((bus_num == 0) && !chrysler_camera_detected) {
-    bus_fwd = 2;
-  }
-  // forward all messages from camera except LKAS_COMMAND and LKAS_HUD
-  if ((bus_num == 2) && !chrysler_camera_detected && (addr != 658) && (addr != 678)) {
-    bus_fwd = 0;
+
+  if (!relay_malfunction) {
+    // forward CAN 0 -> 2 so stock LKAS camera sees messages
+    if (bus_num == 0) {
+      bus_fwd = 2;
+    }
+    // forward all messages from camera except LKAS_COMMAND and LKAS_HUD
+    if ((bus_num == 2) && (addr != 658) && (addr != 678)) {
+      bus_fwd = 0;
+    }
   }
   return bus_fwd;
 }
 
 
 const safety_hooks chrysler_hooks = {
-  .init = chrysler_init,
+  .init = nooutput_init,
   .rx = chrysler_rx_hook,
   .tx = chrysler_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
diff --git a/panda/board/safety/safety_defaults.h b/panda/board/safety/safety_defaults.h
index 2743db96fe..4733438c9c 100644
--- a/panda/board/safety/safety_defaults.h
+++ b/panda/board/safety/safety_defaults.h
@@ -6,7 +6,8 @@ void default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
 static void nooutput_init(int16_t param) {
   UNUSED(param);
-  controls_allowed = 0;
+  controls_allowed = false;
+  relay_malfunction = false;
 }
 
 static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@@ -39,7 +40,8 @@ const safety_hooks nooutput_hooks = {
 
 static void alloutput_init(int16_t param) {
   UNUSED(param);
-  controls_allowed = 1;
+  controls_allowed = true;
+  relay_malfunction = false;
 }
 
 static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
diff --git a/panda/board/safety/safety_elm327.h b/panda/board/safety/safety_elm327.h
index ce405b4767..df515b05d6 100644
--- a/panda/board/safety/safety_elm327.h
+++ b/panda/board/safety/safety_elm327.h
@@ -12,7 +12,7 @@ static int elm327_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   //Check valid 29 bit send addresses for ISO 15765-4
   //Check valid 11 bit send addresses for ISO 15765-4
   if ((addr != 0x18DB33F1) && ((addr & 0x1FFF00FF) != 0x18DA00F1) &&
-      ((addr != 0x7DF) && ((addr & 0x7F8) != 0x7E0))) {
+      ((addr != 0x7DF) && ((addr & 0x1FFFFFF8) != 0x7E0))) {
     tx = 0;
   }
   return tx;
diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h
index 22fc604ff1..47c7342a44 100644
--- a/panda/board/safety/safety_ford.h
+++ b/panda/board/safety/safety_ford.h
@@ -14,6 +14,7 @@ bool ford_moving = false;
 static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 
   int addr = GET_ADDR(to_push);
+  int bus = GET_BUS(to_push);
 
   if (addr == 0x217) {
     // wheel speeds are 14 bits every 16
@@ -53,6 +54,10 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     }
     ford_gas_prev = gas;
   }
+
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) {
+    relay_malfunction = true;
+  }
 }
 
 // all commands: just steering
@@ -64,11 +69,16 @@ static void ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   int tx = 1;
+  int addr = GET_ADDR(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 = ford_gas_prev || (ford_brake_prev && ford_moving);
   bool current_controls_allowed = controls_allowed && !(pedal_pressed);
-  int addr = GET_ADDR(to_send);
+
+  if (relay_malfunction) {
+    tx = 0;
+  }
 
   // STEER: safety check
   if (addr == 0x3CA) {
@@ -92,6 +102,8 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
+// TODO: keep camera on bus 2 and make a fwd_hook
+
 const safety_hooks ford_hooks = {
   .init = nooutput_init,
   .rx = ford_rx_hook,
diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h
index 452f70953d..92c32ad808 100644
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@@ -18,19 +18,21 @@ const int GM_DRIVER_TORQUE_FACTOR = 4;
 const int GM_MAX_GAS = 3072;
 const int GM_MAX_REGEN = 1404;
 const int GM_MAX_BRAKE = 350;
+const AddrBus GM_TX_MSGS[] = {{384, 0}, {1033, 0}, {1034, 0}, {715, 0}, {880, 0},  // pt bus
+                              {161, 1}, {774, 1}, {776, 1}, {784, 1},   // obs bus
+                              {789, 2},  // ch bus
+                              {0x104c006c, 3}, {0x10400060, 3}};  // gmlan
 
 int gm_brake_prev = 0;
 int gm_gas_prev = 0;
 bool gm_moving = false;
-// silence everything if stock car control ECUs are still online
-bool gm_ascm_detected = 0;
 int gm_rt_torque_last = 0;
 int gm_desired_torque_last = 0;
 uint32_t gm_ts_last = 0;
 struct sample_t gm_torque_driver;         // last few driver torques measured
 
 static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
-  int bus_number = GET_BUS(to_push);
+  int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
 
   if (addr == 388) {
@@ -46,15 +48,6 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
   }
 
-  // Check if ASCM or LKA camera are online
-  // on powertrain bus.
-  // 384 = ASCMLKASteeringCmd
-  // 715 = ASCMGasRegenCmd
-  if ((bus_number == 0) && ((addr == 384) || (addr == 715))) {
-    gm_ascm_detected = 1;
-    controls_allowed = 0;
-  }
-
   // ACC steering wheel buttons
   if (addr == 481) {
     int button = (GET_BYTE(to_push, 5) & 0x70) >> 4;
@@ -102,6 +95,14 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       controls_allowed = 0;
     }
   }
+
+  // Check if ASCM or LKA camera are online
+  // on powertrain bus.
+  // 384 = ASCMLKASteeringCmd
+  // 715 = ASCMGasRegenCmd
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 384) || (addr == 715))) {
+    relay_malfunction = true;
+  }
 }
 
 // all commands: gas/regen, friction brake and steering
@@ -113,9 +114,14 @@ static void gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
 static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   int tx = 1;
+  int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
+
+  if (!addr_allowed(addr, bus, GM_TX_MSGS, sizeof(GM_TX_MSGS)/sizeof(GM_TX_MSGS[0]))) {
+    tx = 0;
+  }
 
-  // There can be only one! (ASCM)
-  if (gm_ascm_detected) {
+  if (relay_malfunction) {
     tx = 0;
   }
 
@@ -124,8 +130,6 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int pedal_pressed = gm_gas_prev || (gm_brake_prev && gm_moving);
   bool current_controls_allowed = controls_allowed && !pedal_pressed;
 
-  int addr = GET_ADDR(to_send);
-
   // BRAKE: safety check
   if (addr == 789) {
     int brake = ((GET_BYTE(to_send, 0) & 0xFU) << 8) + GET_BYTE(to_send, 1);
@@ -188,11 +192,6 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     }
   }
 
-  // PARK ASSIST STEER: unlimited torque, no thanks
-  if (addr == 823) {
-    tx = 0;
-  }
-
   // GAS/REGEN: safety check
   if (addr == 715) {
     int gas_regen = ((GET_BYTE(to_send, 2) & 0x7FU) << 5) + ((GET_BYTE(to_send, 3) & 0xF8U) >> 3);
@@ -213,14 +212,9 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   return tx;
 }
 
-static void gm_init(int16_t param) {
-  UNUSED(param);
-  controls_allowed = 0;
-}
-
 
 const safety_hooks gm_hooks = {
-  .init = gm_init,
+  .init = nooutput_init,
   .rx = gm_rx_hook,
   .tx = gm_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index f59e288812..4cef17f79d 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -6,7 +6,9 @@
 //      accel rising edge
 //      brake rising edge
 //      brake > 0mph
-
+const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}, {0x39F, 0}};
+const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}};  // Bosch Harness
+const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0x296, 0}, {0x33D, 2}};  // Bosch Giraffe
 const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328;  // ratio between offset and gain from dbc file
 int honda_brake = 0;
 int honda_gas_prev = 0;
@@ -95,7 +97,17 @@ static void honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     } else if (honda_stock_brake >= honda_brake) {
       honda_fwd_brake = true;
     } else {
-      // Leave honda forward brake as is
+      // Leave Honda forward brake as is
+    }
+  }
+
+  // if steering controls messages are received on the destination bus, it's an indication
+  // that the relay might be malfunctioning
+  int bus_rdr_car = (board_has_relay()) ? 0 : 2;  // radar bus, car side
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 0xE4) || (addr == 0x194))) {
+    if ((honda_bosch_hardware && (bus == bus_rdr_car)) ||
+      (!honda_bosch_hardware && (bus == 0))) {
+      relay_malfunction = true;
     }
   }
 }
@@ -112,6 +124,22 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int addr = GET_ADDR(to_send);
   int bus = GET_BUS(to_send);
 
+  if (honda_bosch_hardware) {
+    if (board_has_relay() && !addr_allowed(addr, bus, HONDA_BH_TX_MSGS, sizeof(HONDA_BH_TX_MSGS)/sizeof(HONDA_BH_TX_MSGS[0]))) {
+      tx = 0;
+    }
+    if (!board_has_relay() && !addr_allowed(addr, bus, HONDA_BG_TX_MSGS, sizeof(HONDA_BG_TX_MSGS)/sizeof(HONDA_BG_TX_MSGS[0]))) {
+      tx = 0;
+    }
+  }
+  if (!honda_bosch_hardware && !addr_allowed(addr, bus, HONDA_N_TX_MSGS, sizeof(HONDA_N_TX_MSGS)/sizeof(HONDA_N_TX_MSGS[0]))) {
+    tx = 0;
+  }
+
+  if (relay_malfunction) {
+    tx = 0;
+  }
+
   // 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) ||
@@ -170,13 +198,15 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
 static void honda_init(int16_t param) {
   UNUSED(param);
-  controls_allowed = 0;
+  controls_allowed = false;
+  relay_malfunction = false;
   honda_bosch_hardware = false;
   honda_alt_brake_msg = false;
 }
 
 static void honda_bosch_init(int16_t param) {
-  controls_allowed = 0;
+  controls_allowed = false;
+  relay_malfunction = false;
   honda_bosch_hardware = true;
   // Checking for alternate brake override from safety parameter
   honda_alt_brake_msg = (param == 1) ? true : false;
@@ -189,20 +219,22 @@ static int honda_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   // 0x39f is radar hud
   int bus_fwd = -1;
 
-  if (bus_num == 0) {
-    bus_fwd = 2;
-  }
-  if (bus_num == 2) {
-    // block stock lkas messages and stock acc messages (if OP is doing ACC)
-    int addr = GET_ADDR(to_fwd);
-    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;
+  if (!relay_malfunction) {
+    if (bus_num == 0) {
+      bus_fwd = 2;
+    }
+    if (bus_num == 2) {
+      // block stock lkas messages and stock acc messages (if OP is doing ACC)
+      int addr = GET_ADDR(to_fwd);
+      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;
+      }
     }
   }
   return bus_fwd;
@@ -213,14 +245,16 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
   int bus_rdr_cam = (board_has_relay()) ? 2 : 1;  // radar bus, camera side
   int bus_rdr_car = (board_has_relay()) ? 0 : 2;  // radar bus, car side
 
-  if (bus_num == bus_rdr_car) {
-    bus_fwd = bus_rdr_cam;
-  }
-  if (bus_num == bus_rdr_cam)  {
-    int addr = GET_ADDR(to_fwd);
-    int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
-    if (!is_lkas_msg) {
-      bus_fwd = bus_rdr_car;
+  if (!relay_malfunction) {
+    if (bus_num == bus_rdr_car) {
+      bus_fwd = bus_rdr_cam;
+    }
+    if (bus_num == bus_rdr_cam)  {
+      int addr = GET_ADDR(to_fwd);
+      int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
+      if (!is_lkas_msg) {
+        bus_fwd = bus_rdr_car;
+      }
     }
   }
   return bus_fwd;
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index 21796e4caa..b2d215f687 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -6,9 +6,8 @@ const int HYUNDAI_MAX_RATE_DOWN = 7;
 const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
 const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
 
-bool hyundai_camera_detected = 0;
-bool hyundai_giraffe_switch_2 = 0;          // is giraffe switch 2 high?
-int hyundai_camera_bus = 0;
+const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}};
+
 int hyundai_rt_torque_last = 0;
 int hyundai_desired_torque_last = 0;
 int hyundai_cruise_engaged_last = 0;
@@ -25,15 +24,9 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     update_sample(&hyundai_torque_driver, torque_driver_new);
   }
 
-  // check if stock camera ECU is still online
-  if ((bus == 0) && (addr == 832)) {
-    hyundai_camera_detected = 1;
-    controls_allowed = 0;
-  }
-
-  // Find out which bus the camera is on
-  if (addr == 832) {
-    hyundai_camera_bus = bus;
+  // check if stock camera ECU is on bus 0
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 832)) {
+    relay_malfunction = true;
   }
 
   // enter controls on rising edge of ACC, exit controls on ACC off
@@ -48,20 +41,19 @@ static void hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     }
     hyundai_cruise_engaged_last = cruise_engaged;
   }
-
-  // 832 is lkas cmd. If it is on camera bus, then giraffe switch 2 is high
-  if ((addr == 832) && (bus == hyundai_camera_bus) && (hyundai_camera_bus != 0)) {
-    hyundai_giraffe_switch_2 = 1;
-  }
 }
 
 static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   int tx = 1;
   int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
+
+  if (!addr_allowed(addr, bus, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]))) {
+    tx = 0;
+  }
 
-  // There can be only one! (camera)
-  if (hyundai_camera_detected) {
+  if (relay_malfunction) {
     tx = 0;
   }
 
@@ -115,12 +107,11 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   // FORCE CANCEL: safety check only relevant when spamming the cancel button.
   // ensuring that only the cancel button press is sent (VAL 4) when controls are off.
   // This avoids unintended engagements while still allowing resume spam
-  // TODO: fix bug preventing the button msg to be fwd'd on bus 2
-  //if ((addr == 1265) && !controls_allowed && (bus == 0) {
-  //  if ((GET_BYTES_04(to_send) & 0x7) != 4) {
-  //    tx = 0;
-  //  }
-  //}
+  if ((addr == 1265) && !controls_allowed) {
+    if ((GET_BYTES_04(to_send) & 0x7) != 4) {
+      tx = 0;
+    }
+  }
 
   // 1 allows the message through
   return tx;
@@ -129,29 +120,22 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   int bus_fwd = -1;
+  int addr = GET_ADDR(to_fwd);
   // forward cam to ccan and viceversa, except lkas cmd
-  if (hyundai_giraffe_switch_2) {
+  if (!relay_malfunction) {
     if (bus_num == 0) {
-      bus_fwd = hyundai_camera_bus;
+      bus_fwd = 2;
     }
-    if (bus_num == hyundai_camera_bus) {
-      int addr = GET_ADDR(to_fwd);
-      if (addr != 832) {
-        bus_fwd = 0;
-      }
+    if ((bus_num == 2) && (addr != 832)) {
+      bus_fwd = 0;
     }
   }
   return bus_fwd;
 }
 
-static void hyundai_init(int16_t param) {
-  UNUSED(param);
-  controls_allowed = 0;
-  hyundai_giraffe_switch_2 = 0;
-}
 
 const safety_hooks hyundai_hooks = {
-  .init = hyundai_init,
+  .init = nooutput_init,
   .rx = hyundai_rx_hook,
   .tx = hyundai_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h
index 60d8b2bda9..f1215f23da 100644
--- a/panda/board/safety/safety_mazda.h
+++ b/panda/board/safety/safety_mazda.h
@@ -30,12 +30,6 @@ 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);
@@ -51,7 +45,7 @@ void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     int cruise_engaged = GET_BYTE(to_push, 0) & 8;
     if (cruise_engaged != 0) {
       if (!mazda_cruise_engaged_last) {
-	controls_allowed = 1;
+        controls_allowed = 1;
       }
     }
     else {
@@ -60,17 +54,9 @@ void mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     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;
-    }
+  // if we see wheel speed msgs on MAZDA_CAM bus then relay is closed
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == MAZDA_CAM) && (addr == MAZDA_WHEEL_SPEED)) {
+    relay_malfunction = true;
   }
 }
 
@@ -79,15 +65,12 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int addr = GET_ADDR(to_send);
   int bus = GET_BUS(to_send);
 
+  if (relay_malfunction) {
+    tx = 0;
+  }
+
   // 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;
@@ -96,42 +79,42 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
       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;
-	}
+        // *** 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;
+        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;
+        mazda_desired_torque_last = 0;
+        mazda_rt_torque_last = 0;
+        mazda_ts_last = ts;
       }
 
       if (violation) {
-	tx = 0;
+        tx = 0;
       }
     }
   }
@@ -140,16 +123,14 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
 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) {
+  if (!relay_malfunction) {
     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;
+      if (!(addr == MAZDA_LKAS)) {
+        bus_fwd = MAZDA_MAIN;
       }
     }
     else {
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index c09b9d6225..66e5947f85 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -8,6 +8,8 @@ const int SUBARU_MAX_RATE_DOWN = 70;
 const int SUBARU_DRIVER_TORQUE_ALLOWANCE = 60;
 const int SUBARU_DRIVER_TORQUE_FACTOR = 10;
 
+const AddrBus SUBARU_TX_MSGS[] = {{0x122, 0}, {0x164, 0}, {0x221, 0}, {0x322, 0}};
+
 int subaru_cruise_engaged_last = 0;
 int subaru_rt_torque_last = 0;
 int subaru_desired_torque_last = 0;
@@ -38,11 +40,24 @@ static void subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     }
     subaru_cruise_engaged_last = cruise_engaged;
   }
+
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && ((addr == 0x122) || (addr == 0x164))) {
+    relay_malfunction = true;
+  }
 }
 
 static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
+  int bus = GET_BUS(to_send);
+
+  if (!addr_allowed(addr, bus, SUBARU_TX_MSGS, sizeof(SUBARU_TX_MSGS) / sizeof(SUBARU_TX_MSGS[0]))) {
+    tx = 0;
+  }
+
+  if (relay_malfunction) {
+    tx = 0;
+  }
 
   // steer cmd checks
   if ((addr == 0x122) || (addr == 0x164)) {
@@ -98,23 +113,24 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 }
 
 static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
-
   int bus_fwd = -1;
-  if (bus_num == 0) {
-    bus_fwd = 2;  // Camera CAN
-  }
-  if (bus_num == 2) {
-    // 290 is LKAS for Global Platform
-    // 356 is LKAS for outback 2015
-    // 545 is ES_Distance
-    // 802 is ES_LKAS
-    int addr = GET_ADDR(to_fwd);
-    int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
-    if (!block_msg) {
-      bus_fwd = 0;  // Main CAN
+
+  if (!relay_malfunction) {
+    if (bus_num == 0) {
+      bus_fwd = 2;  // Camera CAN
+    }
+    if (bus_num == 2) {
+      // 290 is LKAS for Global Platform
+      // 356 is LKAS for outback 2015
+      // 545 is ES_Distance
+      // 802 is ES_LKAS
+      int addr = GET_ADDR(to_fwd);
+      int block_msg = (addr == 290) || (addr == 356) || (addr == 545) || (addr == 802);
+      if (!block_msg) {
+        bus_fwd = 0;  // Main CAN
+      }
     }
   }
-
   // fallback to do not forward
   return bus_fwd;
 }
diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index 134cc845c9..0576a46381 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -18,12 +18,16 @@ const int TOYOTA_MIN_ACCEL = -3000;       // 3.0 m/s2
 
 const int TOYOTA_GAS_INTERCEPTOR_THRESHOLD = 475;  // ratio between offset and gain from dbc file
 
+// allowed DSU messages on bus 0 and 1
+const AddrBus TOYOTA_TX_MSGS[] = {{0x283, 0}, {0x2E6, 0}, {0x2E7, 0}, {0x33E, 0}, {0x344, 0}, {0x365, 0}, {0x366, 0}, {0x4CB, 0},  // DSU bus 0
+                                         {0x128, 1}, {0x141, 1}, {0x160, 1}, {0x161, 1}, {0x470, 1},  // DSU bus 1
+                                         {0x2E4, 0}, {0x411, 0}, {0x412, 0}, {0x343, 0}, {0x1D2, 0}, // LKAS + ACC
+                                         {0x200, 0}};  // interceptor
+
 // global actuation limit states
 int toyota_dbc_eps_torque_factor = 100;   // conversion factor for STEER_TORQUE_EPS in %: see dbc file
 
 // states
-int toyota_giraffe_switch_1 = 0;          // is giraffe switch 1 high?
-int toyota_camera_forwarded = 0;          // should we forward the camera bus?
 int toyota_desired_torque_last = 0;       // last desired steer torque
 int toyota_rt_torque_last = 0;            // last desired torque for real time check
 uint32_t toyota_ts_last = 0;
@@ -87,14 +91,9 @@ static void toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     toyota_gas_prev = gas;
   }
 
-  // msgs are only on bus 2 if panda is connected to frc
-  if (bus == 2) {
-    toyota_camera_forwarded = 1;
-  }
-
-  // 0x2E4 is lkas cmd. If it is on bus 0, then giraffe switch 1 is high
-  if ((addr == 0x2E4) && (bus == 0)) {
-    toyota_giraffe_switch_1 = 1;
+  // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4) && (bus == 0)) {
+    relay_malfunction = true;
   }
 }
 
@@ -104,14 +103,17 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int addr = GET_ADDR(to_send);
   int bus = GET_BUS(to_send);
 
+  if (!addr_allowed(addr, bus, TOYOTA_TX_MSGS, sizeof(TOYOTA_TX_MSGS)/sizeof(TOYOTA_TX_MSGS[0]))) {
+    tx = 0;
+  }
+
+  if (relay_malfunction) {
+    tx = 0;
+  }
+
   // Check if msg is sent on BUS 0
   if (bus == 0) {
 
-    // no IPAS in non IPAS mode
-    if ((addr == 0x266) || (addr == 0x167)) {
-      tx = 0;
-    }
-
     // GAS PEDAL: safety check
     if (addr == 0x200) {
       if (!controls_allowed || !long_controls_allowed) {
@@ -185,28 +187,26 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     }
   }
 
-  // 1 allows the message through
   return tx;
 }
 
 static void toyota_init(int16_t param) {
   controls_allowed = 0;
-  toyota_giraffe_switch_1 = 0;
-  toyota_camera_forwarded = 0;
+  relay_malfunction = 0;
   toyota_dbc_eps_torque_factor = param;
 }
 
 static int toyota_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   int bus_fwd = -1;
-  if (toyota_camera_forwarded && !toyota_giraffe_switch_1) {
+  if (!relay_malfunction) {
     if (bus_num == 0) {
       bus_fwd = 2;
     }
     if (bus_num == 2) {
       int addr = GET_ADDR(to_fwd);
       // block stock lkas messages and stock acc messages (if OP is doing ACC)
-      // in TSS2, 0.191 is LTA which we need to block to avoid controls collision
+      // in TSS2, 0x191 is LTA which we need to block to avoid controls collision
       int is_lkas_msg = ((addr == 0x2E4) || (addr == 0x412) || (addr == 0x191));
       // in TSS2 the camera does ACC as well, so filter 0x343
       int is_acc_msg = (addr == 0x343);
diff --git a/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h
index bb184cd91f..c16e559ce1 100644
--- a/panda/board/safety/safety_volkswagen.h
+++ b/panda/board/safety/safety_volkswagen.h
@@ -1,3 +1,11 @@
+// Safety-relevant CAN messages for the Volkswagen MQB platform.
+#define MSG_EPS_01              0x09F
+#define MSG_MOTOR_20            0x121
+#define MSG_ACC_06              0x122
+#define MSG_HCA_01              0x126
+#define MSG_GRA_ACC_01          0x12B
+#define MSG_LDW_02              0x397
+
 const int VOLKSWAGEN_MAX_STEER = 250;               // 2.5 Nm (EPS side max of 3.0Nm with fault if violated)
 const int VOLKSWAGEN_MAX_RT_DELTA = 75;             // 4 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 50 ; 50 * 1.5 for safety pad = 75
 const uint32_t VOLKSWAGEN_RT_INTERVAL = 250000;     // 250ms between real time checks
@@ -6,26 +14,15 @@ const int VOLKSWAGEN_MAX_RATE_DOWN = 10;            // 5.0 Nm/s available rate o
 const int VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE = 80;
 const int VOLKSWAGEN_DRIVER_TORQUE_FACTOR = 3;
 
+// MSG_GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
+const AddrBus VOLKSWAGEN_TX_MSGS[] = {{MSG_HCA_01, 0}, {MSG_GRA_ACC_01, 0}, {MSG_GRA_ACC_01, 2}, {MSG_LDW_02, 0}};
+
 struct sample_t volkswagen_torque_driver;           // last few driver torques measured
 int volkswagen_rt_torque_last = 0;
 int volkswagen_desired_torque_last = 0;
 uint32_t volkswagen_ts_last = 0;
 int volkswagen_gas_prev = 0;
 
-// Safety-relevant CAN messages for the Volkswagen MQB platform.
-#define MSG_EPS_01              0x09F
-#define MSG_MOTOR_20            0x121
-#define MSG_ACC_06              0x122
-#define MSG_HCA_01              0x126
-#define MSG_GRA_ACC_01          0x12B
-#define MSG_LDW_02              0x397
-#define MSG_KLEMMEN_STATUS_01   0x3C0
-
-static void volkswagen_init(int16_t param) {
-  UNUSED(param); // May use param in the future to indicate MQB vs PQ35/PQ46/NMS vs MLB, or wiring configuration.
-  controls_allowed = 0;
-}
-
 static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   int bus = GET_BUS(to_push);
   int addr = GET_ADDR(to_push);
@@ -58,6 +55,10 @@ static void volkswagen_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
     }
     volkswagen_gas_prev = gas;
   }
+
+  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_01)) {
+    relay_malfunction = true;
+  }
 }
 
 static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
@@ -65,6 +66,14 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int bus = GET_BUS(to_send);
   int tx = 1;
 
+  if (!addr_allowed(addr, bus, VOLKSWAGEN_TX_MSGS, sizeof(VOLKSWAGEN_TX_MSGS)/sizeof(VOLKSWAGEN_TX_MSGS[0]))) {
+    tx = 0;
+  }
+
+  if (relay_malfunction) {
+    tx = 0;
+  }
+
   // Safety check for HCA_01 Heading Control Assist torque.
   if (addr == MSG_HCA_01) {
     bool violation = false;
@@ -118,7 +127,7 @@ static int volkswagen_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
 
   // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
   // This avoids unintended engagements while still allowing resume spam
-  if ((bus == 2) && (addr == MSG_GRA_ACC_01) && !controls_allowed) {
+  if ((addr == MSG_GRA_ACC_01) && !controls_allowed) {
     // disallow resume and set: bits 16 and 19
     if ((GET_BYTE(to_send, 2) & 0x9) != 0) {
       tx = 0;
@@ -135,31 +144,32 @@ static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
 
   // NOTE: Will need refactoring for other bus layouts, such as no-forwarding at camera or J533 running-gear CAN
 
-  switch (bus_num) {
-    case 0:
-      // Forward all traffic from J533 gateway to Extended CAN devices.
-      bus_fwd = 2;
-      break;
-    case 2:
-      if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
-        // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera.
+  if (!relay_malfunction) {
+    switch (bus_num) {
+      case 0:
+        // Forward all traffic from J533 gateway to Extended CAN devices.
+        bus_fwd = 2;
+        break;
+      case 2:
+        if ((addr == MSG_HCA_01) || (addr == MSG_LDW_02)) {
+          // OP takes control of the Heading Control Assist and Lane Departure Warning messages from the camera.
+          bus_fwd = -1;
+        } else {
+          // Forward all remaining traffic from Extended CAN devices to J533 gateway.
+          bus_fwd = 0;
+        }
+        break;
+      default:
+        // No other buses should be in use; fallback to do-not-forward.
         bus_fwd = -1;
-      } else {
-        // Forward all remaining traffic from Extended CAN devices to J533 gateway.
-        bus_fwd = 0;
-      }
-      break;
-    default:
-      // No other buses should be in use; fallback to do-not-forward.
-      bus_fwd = -1;
-      break;
+        break;
+    }
   }
-
   return bus_fwd;
 }
 
 const safety_hooks volkswagen_hooks = {
-  .init = volkswagen_init,
+  .init = nooutput_init,
   .rx = volkswagen_rx_hook,
   .tx = volkswagen_tx_hook,
   .tx_lin = nooutput_tx_lin_hook,
diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h
index efa6a4e2b8..e192d74fb7 100644
--- a/panda/board/safety_declarations.h
+++ b/panda/board/safety_declarations.h
@@ -11,6 +11,11 @@ struct lookup_t {
   float y[3];
 };
 
+typedef struct {
+  int addr;
+  int bus;
+} AddrBus;
+
 void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
 int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_send);
 int safety_tx_lin_hook(int lin_num, uint8_t *data, int len);
@@ -25,6 +30,7 @@ bool driver_limit_check(int val, int val_last, struct sample_t *val_driver,
   const int MAX_ALLOWANCE, const int DRIVER_FACTOR);
 bool rt_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA);
 float interpolate(struct lookup_t xy, float x);
+bool addr_allowed(int addr, int bus, const AddrBus addr_list[], int len);
 
 typedef void (*safety_hook_init)(int16_t param);
 typedef void (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push);
@@ -40,13 +46,19 @@ typedef struct {
   fwd_hook fwd;
 } safety_hooks;
 
-// This can be set by the safety hooks.
-bool controls_allowed = 0;
-bool gas_interceptor_detected = 0;
+// This can be set by the safety hooks
+bool controls_allowed = false;
+bool relay_malfunction = false;
+bool gas_interceptor_detected = false;
 int gas_interceptor_prev = 0;
 
 // This is set by USB command 0xdf
-bool long_controls_allowed = 1;
+bool long_controls_allowed = true;
+
+// time since safety mode has been changed
+uint32_t safety_mode_cnt = 0U;
+// allow 1s of transition timeout after relay changes state before assessing malfunctioning
+const uint32_t RELAY_TRNS_TIMEOUT = 1U;
 
 // avg between 2 tracks
 #define GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2)
diff --git a/panda/board/spi_flasher.h b/panda/board/spi_flasher.h
index b1bba71d46..fbdbab8a61 100644
--- a/panda/board/spi_flasher.h
+++ b/panda/board/spi_flasher.h
@@ -65,7 +65,12 @@ 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:
+          #ifdef ALLOW_DEBUG
           if (hardwired) {
+          #else
+          // no more bootstub on UNO
+          if (hardwired && hw_type != HW_TYPE_UNO) {
+          #endif
             puts("-> entering bootloader\n");
             enter_bootloader_mode = ENTER_BOOTLOADER_MAGIC;
             NVIC_SystemReset();
@@ -151,7 +156,7 @@ int spi_cb_rx(uint8_t *data, int len, uint8_t *data_out) {
 #define CAN_BL_INPUT 0x1
 #define CAN_BL_OUTPUT 0x2
 
-void CAN1_TX_IRQHandler(void) {
+void CAN1_TX_IRQ_Handler(void) {
   // clear interrupt
   CAN->TSR |= CAN_TSR_RQCP0;
 }
@@ -178,7 +183,7 @@ void bl_can_send(uint8_t *odat) {
   CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
 }
 
-void CAN1_RX0_IRQHandler(void) {
+void CAN1_RX0_IRQ_Handler(void) {
   while (CAN->RF0R & CAN_RF0R_FMP0) {
     if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
       uint8_t dat[8];
@@ -253,13 +258,19 @@ void CAN1_RX0_IRQHandler(void) {
   }
 }
 
-void CAN1_SCE_IRQHandler(void) {
+void CAN1_SCE_IRQ_Handler(void) {
   llcan_clear_send(CAN);
 }
 
 #endif
 
 void soft_flasher_start(void) {
+  #ifdef PEDAL
+    REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+    REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+    REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1)
+  #endif
+
   puts("\n\n\n************************ FLASHER START ************************\n");
 
   enter_bootloader_mode = 0;
diff --git a/panda/crypto/sign.py b/panda/crypto/sign.py
index 0f1ce03081..8cbe6f4195 100755
--- a/panda/crypto/sign.py
+++ b/panda/crypto/sign.py
@@ -6,6 +6,9 @@ import hashlib
 from Crypto.PublicKey import RSA
 import binascii
 
+# increment this to make new hardware not run old versions
+VERSION = 2
+
 rsa = RSA.importKey(open(sys.argv[3]).read())
 
 with open(sys.argv[1], "rb") as f:
@@ -15,6 +18,9 @@ print("signing", len(dat), "bytes")
 
 with open(sys.argv[2], "wb") as f:
   if os.getenv("SETLEN") is not None:
+    # add the version at the end
+    dat += b"VERS" + struct.pack("I", VERSION)
+    # add the length at the beginning
     x = struct.pack("I", len(dat)) + dat[4:]
     # mock signature of dat[4:]
     dd = hashlib.sha1(dat[4:]).digest()
diff --git a/panda/drivers/linux/panda.c b/panda/drivers/linux/panda.c
index 3d4f957f9c..c0aa44be18 100644
--- a/panda/drivers/linux/panda.c
+++ b/panda/drivers/linux/panda.c
@@ -39,7 +39,7 @@
 #define PANDA_DLC_MASK  0x0F
 
 #define SAFETY_ALLOUTPUT 17
-#define SAFETY_NOOUTPUT 0
+#define SAFETY_SILENT 0
 
 struct panda_usb_ctx {
   struct panda_inf_priv *priv;
@@ -159,7 +159,7 @@ static int panda_set_output_enable(struct panda_inf_priv* priv, bool enable){
   return usb_control_msg(priv->priv_dev->udev,
 			 usb_sndctrlpipe(priv->priv_dev->udev, 0),
 			 0xDC, USB_TYPE_VENDOR | USB_RECIP_DEVICE,
-			 enable ? SAFETY_ALLOUTPUT : SAFETY_NOOUTPUT, 0, NULL, 0, USB_CTRL_SET_TIMEOUT);
+			 enable ? SAFETY_ALLOUTPUT : SAFETY_SILENT, 0, NULL, 0, USB_CTRL_SET_TIMEOUT);
 }
 
 static void panda_usb_write_bulk_callback(struct urb *urb)
diff --git a/panda/drivers/windows/panda_shared/panda.cpp b/panda/drivers/windows/panda_shared/panda.cpp
index 5f711b02aa..8425ec9da2 100644
--- a/panda/drivers/windows/panda_shared/panda.cpp
+++ b/panda/drivers/windows/panda_shared/panda.cpp
@@ -277,7 +277,7 @@ bool Panda::esp_reset(uint16_t bootmode = 0) {
 	return this->control_transfer(REQUEST_OUT, 0xda, bootmode, 0, NULL, 0, 0) != -1;
 }
 
-bool Panda::set_safety_mode(PANDA_SAFETY_MODE mode = SAFETY_NOOUTPUT) {
+bool Panda::set_safety_mode(PANDA_SAFETY_MODE mode = SAFETY_SILENT) {
 	return this->control_transfer(REQUEST_OUT, 0xdc, mode, 0, NULL, 0, 0) != -1;
 }
 
diff --git a/panda/drivers/windows/panda_shared/panda.h b/panda/drivers/windows/panda_shared/panda.h
index 8d98b08b71..117fc92760 100644
--- a/panda/drivers/windows/panda_shared/panda.h
+++ b/panda/drivers/windows/panda_shared/panda.h
@@ -38,7 +38,7 @@
 
 namespace panda {
 	typedef enum _PANDA_SAFETY_MODE : uint16_t {
-		SAFETY_NOOUTPUT = 0,
+		SAFETY_SILENT = 0,
 		SAFETY_HONDA = 1,
 		SAFETY_ALLOUTPUT = 17,
 	} PANDA_SAFETY_MODE;
diff --git a/panda/examples/can_logger.py b/panda/examples/can_logger.py
index 203023dc92..12dcf58491 100755
--- a/panda/examples/can_logger.py
+++ b/panda/examples/can_logger.py
@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 
-import binascii
 import csv
 import sys
 from panda import Panda
@@ -21,7 +20,7 @@ def can_logger():
       sys.exit(0)
 
   try:
-    outputfile = open('output.csv', 'wb')
+    outputfile = open('output.csv', 'w')
     csvwriter = csv.writer(outputfile)
     #Write Header
     csvwriter.writerow(['Bus', 'MessageID', 'Message', 'MessageLength'])
@@ -35,7 +34,7 @@ def can_logger():
       can_recv = p.can_recv()
 
       for address, _, dat, src  in can_recv:
-        csvwriter.writerow([str(src), str(hex(address)), "0x" + binascii.hexlify(dat), len(dat)])
+        csvwriter.writerow([str(src), str(hex(address)), f"0x{dat.hex()}", len(dat)])
 
         if src == 0:
           bus0_msg_cnt += 1
@@ -44,10 +43,10 @@ def can_logger():
         elif src == 2:
           bus2_msg_cnt += 1
 
-        print("Message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt), end='\r')
+        print(f"Message Counts... Bus 0: {bus0_msg_cnt} Bus 1: {bus1_msg_cnt} Bus 2: {bus2_msg_cnt}", end='\r')
 
   except KeyboardInterrupt:
-    print("\nNow exiting. Final message Counts... Bus 0: " + str(bus0_msg_cnt) + " Bus 1: " + str(bus1_msg_cnt) + " Bus 2: " + str(bus2_msg_cnt))
+    print(f"\nNow exiting. Final message Counts... Bus 0: {bus0_msg_cnt} Bus 1: {bus1_msg_cnt} Bus 2: {bus2_msg_cnt}")
     outputfile.close()
 
 if __name__ == "__main__":
diff --git a/panda/examples/eps_read_software_ids.py b/panda/examples/eps_read_software_ids.py
deleted file mode 100755
index a3292207c7..0000000000
--- a/panda/examples/eps_read_software_ids.py
+++ /dev/null
@@ -1,59 +0,0 @@
-#!/usr/bin/env python3
-from panda import Panda
-from panda.python.uds import UdsClient, NegativeResponseError, DATA_IDENTIFIER_TYPE
-
-if __name__ == "__main__":
-  address = 0x18da30f1 # Honda EPS
-  panda = Panda()
-  uds_client = UdsClient(panda, address, debug=False)
-
-  print("tester present ...")
-  uds_client.tester_present()
-
-  try:
-    print("")
-    print("read data by id: boot software id ...")
-    data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.BOOT_SOFTWARE_IDENTIFICATION)
-    print(data.decode('utf-8'))
-  except NegativeResponseError as e:
-    print(e)
-
-  try:
-    print("")
-    print("read data by id: application software id ...")
-    data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
-    print(data.decode('utf-8'))
-  except NegativeResponseError as e:
-    print(e)
-
-  try:
-    print("")
-    print("read data by id: application data id ...")
-    data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
-    print(data.decode('utf-8'))
-  except NegativeResponseError as e:
-    print(e)
-
-  try:
-    print("")
-    print("read data by id: boot software fingerprint ...")
-    data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.BOOT_SOFTWARE_FINGERPRINT)
-    print(data.decode('utf-8'))
-  except NegativeResponseError as e:
-    print(e)
-
-  try:
-    print("")
-    print("read data by id: application software fingerprint ...")
-    data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_FINGERPRINT)
-    print(data.decode('utf-8'))
-  except NegativeResponseError as e:
-    print(e)
-
-  try:
-    print("")
-    print("read data by id: application data fingerprint ...")
-    data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_FINGERPRINT)
-    print(data.decode('utf-8'))
-  except NegativeResponseError as e:
-    print(e)
diff --git a/panda/examples/query_fw_versions.py b/panda/examples/query_fw_versions.py
new file mode 100755
index 0000000000..2177335f6e
--- /dev/null
+++ b/panda/examples/query_fw_versions.py
@@ -0,0 +1,77 @@
+#!/usr/bin/env python3
+from tqdm import tqdm
+from panda import Panda
+from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE, DATA_IDENTIFIER_TYPE
+
+if __name__ == "__main__":
+  addrs = [0x700 + i for i in range(256)]
+  addrs += [0x18da0000 + (i<<8) + 0xf1 for i in range(256)]
+  results = {}
+
+  panda = Panda()
+  panda.set_safety_mode(Panda.SAFETY_ELM327)
+  print("querying addresses ...")
+  with tqdm(addrs) as t:
+    for addr in t:
+      # skip functional broadcast addrs
+      if addr == 0x7df or addr == 0x18db33f1:
+        continue
+      t.set_description(hex(addr))
+
+      uds_client = UdsClient(panda, addr, bus=1 if panda.has_obd() else 0, timeout=0.1, debug=False)
+      try:
+        uds_client.tester_present()
+        uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC)
+      except NegativeResponseError:
+        pass
+      except MessageTimeoutError:
+        continue
+
+      resp = {}
+
+      try:
+        data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.BOOT_SOFTWARE_IDENTIFICATION)
+        if data: resp[DATA_IDENTIFIER_TYPE.BOOT_SOFTWARE_IDENTIFICATION] = data
+      except NegativeResponseError:
+        pass
+
+      try:
+        data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
+        if data: resp[DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION] = data
+      except NegativeResponseError:
+        pass
+
+      try:
+        data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
+        if data: resp[DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION] = data
+      except NegativeResponseError:
+        pass
+
+      try:
+        data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.BOOT_SOFTWARE_FINGERPRINT)
+        if data: resp[DATA_IDENTIFIER_TYPE.BOOT_SOFTWARE_FINGERPRINT] = data
+      except NegativeResponseError:
+        pass
+
+      try:
+        data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_FINGERPRINT)
+        if data: resp[DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_FINGERPRINT] = data
+      except NegativeResponseError:
+        pass
+
+      try:
+        data = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_FINGERPRINT)
+        if data: resp[DATA_IDENTIFIER_TYPE.APPLICATION_DATA_FINGERPRINT] = data
+      except NegativeResponseError:
+        pass
+
+      if resp.keys():
+        results[addr] = resp
+
+    print("results:")
+    if len(results.items()):
+      for addr, resp in results.items():
+        for rid, dat in resp.items():
+          print(hex(addr), hex(rid), dat.decode())
+    else:
+      print("no fw versions found!")
diff --git a/panda/examples/tesla_tester.py b/panda/examples/tesla_tester.py
index 9a77eb4edf..57079f56eb 100644
--- a/panda/examples/tesla_tester.py
+++ b/panda/examples/tesla_tester.py
@@ -22,7 +22,7 @@ def tesla_tester():
   body_bus_num = 1 # My TDC to OBD adapter has PT on bus0 BDY on bus1 and CH on bus2
   p.set_can_speed_kbps(body_bus_num, body_bus_speed)
 
-  # Now set the panda from its default of SAFETY_NOOUTPUT (read only) to SAFETY_ALLOUTPUT
+  # Now set the panda from its default of SAFETY_SILENT (read only) to SAFETY_ALLOUTPUT
   # Careful, as this will let us send any CAN messages we want (which could be very bad!)
   print("Setting Panda to output mode...")
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
@@ -37,7 +37,7 @@ def tesla_tester():
 
   #Back to safety...
   print("Disabling output on Panda...")
-  p.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+  p.set_safety_mode(Panda.SAFETY_SILENT)
 
   print("Reading VIN from 0x568. This is painfully slow and can take up to 3 minutes (1 minute per message; 3 messages needed for full VIN)...")
 
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 326128d3ec..8f4ae29181 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -110,7 +110,7 @@ class WifiHandle(object):
 class Panda(object):
 
   # matches cereal.car.CarParams.SafetyModel
-  SAFETY_NOOUTPUT = 0
+  SAFETY_SILENT = 0
   SAFETY_HONDA = 1
   SAFETY_TOYOTA = 2
   SAFETY_ELM327 = 3
@@ -127,6 +127,7 @@ class Panda(object):
   SAFETY_TOYOTA_IPAS = 16
   SAFETY_ALLOUTPUT = 17
   SAFETY_GM_ASCM = 18
+  SAFETY_NOOUTPUT = 19
 
   SERIAL_DEBUG = 0
   SERIAL_ESP = 1
@@ -345,18 +346,25 @@ class Panda(object):
   # ******************* health *******************
 
   def health(self):
-    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 24)
-    a = struct.unpack("IIIIIBBBB", dat)
+    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 37)
+    a = struct.unpack("IIIIIIIBBBBBBBBB", dat)
     return {
-      "voltage": a[0],
-      "current": a[1],
-      "can_send_errs": a[2],
-      "can_fwd_errs": a[3],
-      "gmlan_send_errs": a[4],
-      "started": a[5],
-      "controls_allowed": a[6],
-      "gas_interceptor_detected": a[7],
-      "car_harness_status": a[8]
+      "uptime": a[0],
+      "voltage": a[1],
+      "current": a[2],
+      "can_send_errs": a[3],
+      "can_fwd_errs": a[4],
+      "gmlan_send_errs": a[5],
+      "faults": a[6],
+      "ignition_line": a[7],
+      "ignition_can": a[8],
+      "controls_allowed": a[9],
+      "gas_interceptor_detected": a[10],
+      "car_harness_status": a[11],
+      "usb_power_mode": a[12],
+      "safety_mode": a[13],
+      "fault_status": a[14],
+      "power_save_enabled": a[15]
     }
 
   # ******************* control *******************
@@ -371,6 +379,17 @@ class Panda(object):
   def get_version(self):
     return self._handle.controlRead(Panda.REQUEST_IN, 0xd6, 0, 0, 0x40).decode('utf8')
 
+  @staticmethod
+  def get_signature_from_firmware(fn):
+    f = open(fn, 'rb')
+    f.seek(-128, 2)  # Seek from end of file
+    return f.read(128)
+
+  def get_signature(self):
+    part_1 = self._handle.controlRead(Panda.REQUEST_IN, 0xd3, 0, 0, 0x40)
+    part_2 = self._handle.controlRead(Panda.REQUEST_IN, 0xd4, 0, 0, 0x40)
+    return part_1 + part_2
+
   def get_type(self):
     return self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
 
@@ -386,6 +405,9 @@ class Panda(object):
   def is_uno(self):
     return self.get_type() == Panda.HW_TYPE_UNO
 
+  def has_obd(self):
+    return (self.is_uno() or self.is_black())
+
   def get_serial(self):
     dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
     hashsig, calc_hash = dat[0x1c:], hashlib.sha1(dat[0:0x1c]).digest()[0:4]
@@ -400,6 +422,9 @@ class Panda(object):
   def set_usb_power(self, on):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xe6, int(on), 0, b'')
 
+  def set_power_save(self, power_save_enabled=0):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe7, int(power_save_enabled), 0, b'')
+
   def set_esp_power(self, on):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xd9, int(on), 0, b'')
 
@@ -407,7 +432,7 @@ class Panda(object):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xda, int(bootmode), 0, b'')
     time.sleep(0.2)
 
-  def set_safety_mode(self, mode=SAFETY_NOOUTPUT):
+  def set_safety_mode(self, mode=SAFETY_SILENT):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, 0, b'')
 
   def set_can_forwarding(self, from_bus, to_bus):
@@ -623,4 +648,8 @@ class Panda(object):
   def get_fan_rpm(self):
     dat = self._handle.controlRead(Panda.REQUEST_IN, 0xb2, 0, 0, 2)
     a = struct.unpack("H", dat)
-    return a[0]
\ No newline at end of file
+    return a[0]
+
+# ****************** Phone *****************
+  def set_phone_power(self, enabled):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, int(enabled), 0, b'')
diff --git a/panda/python/uds.py b/panda/python/uds.py
index 5f5ae53bfb..ee9e9ea667 100644
--- a/panda/python/uds.py
+++ b/panda/python/uds.py
@@ -1,11 +1,8 @@
 #!/usr/bin/env python3
 import time
 import struct
-from typing import NamedTuple, List
+from typing import Callable, NamedTuple, Tuple, List
 from enum import IntEnum
-from queue import Queue, Empty
-from threading import Thread
-from binascii import hexlify
 
 class SERVICE_TYPE(IntEnum):
   DIAGNOSTIC_SESSION_CONTROL         = 0x10
@@ -213,6 +210,7 @@ class MessageTimeoutError(Exception):
 
 class NegativeResponseError(Exception):
   def __init__(self, message, service_id, error_code):
+    super().__init__()
     self.message = message
     self.service_id = service_id
     self.error_code = error_code
@@ -270,20 +268,79 @@ _negative_response_codes = {
     0x93: 'voltage too low',
 }
 
+class CanClient():
+  def __init__(self, can_send: Callable[[Tuple[int, bytes, int]], None], can_recv: Callable[[], List[Tuple[int, int, bytes, int]]], tx_addr: int, rx_addr: int, bus: int, debug: bool=False):
+    self.tx = can_send
+    self.rx = can_recv
+    self.tx_addr = tx_addr
+    self.rx_addr = rx_addr
+    self.bus = bus
+    self.debug = debug
+
+  def _recv_filter(self, bus, addr):
+    # handle functionl addresses (switch to first addr to respond)
+    if self.tx_addr == 0x7DF:
+      is_response = addr >= 0x7E8 and addr <= 0x7EF
+      if is_response:
+        if self.debug: print(f"switch to physical addr {hex(addr)}")
+        self.tx_addr = addr-8
+        self.rx_addr = addr
+      return is_response
+    if self.tx_addr == 0x18DB33F1:
+      is_response = addr >= 0x18DAF100 and addr <= 0x18DAF1FF
+      if is_response:
+        if self.debug: print(f"switch to physical addr {hex(addr)}")
+        self.tx_addr = 0x18DA00F1 + (addr<<8 & 0xFF00)
+        self.rx_addr = addr
+    return bus == self.bus and addr == self.rx_addr
+
+  def recv(self, drain=False) -> List[bytes]:
+    msg_array = []
+    while True:
+      msgs = self.rx()
+      if drain:
+        if self.debug: print("CAN-RX: drain - {}".format(len(msgs)))
+      else:
+        for rx_addr, rx_ts, rx_data, rx_bus in msgs or []:
+          if self._recv_filter(rx_bus, rx_addr) and len(rx_data) > 0:
+            rx_data = bytes(rx_data) # convert bytearray to bytes
+            if self.debug: print(f"CAN-RX: {hex(rx_addr)} - 0x{bytes.hex(rx_data)}")
+            msg_array.append(rx_data)
+      # break when non-full buffer is processed
+      if len(msgs) < 254:
+        return msg_array
+
+  def send(self, msgs: List[bytes], delay: float=0) -> None:
+    first = True
+    for msg in msgs:
+      if delay and not first:
+        if self.debug: print(f"CAN-TX: delay - {delay}")
+        time.sleep(delay)
+      if self.debug: print(f"CAN-TX: {hex(self.tx_addr)} - 0x{bytes.hex(msg)}")
+      self.tx(self.tx_addr, msg, self.bus)
+      first = False
+
 class IsoTpMessage():
-  def __init__(self, can_tx_queue: Queue, can_rx_queue: Queue, timeout: float, debug: bool=False):
-    self.can_tx_queue = can_tx_queue
-    self.can_rx_queue = can_rx_queue
+  def __init__(self, can_client: CanClient, timeout: float=1, debug: bool=False):
+    self._can_client = can_client
     self.timeout = timeout
     self.debug = debug
 
   def send(self, dat: bytes) -> None:
+    # throw away any stale data
+    self._can_client.recv(drain=True)
+
     self.tx_dat = dat
     self.tx_len = len(dat)
     self.tx_idx = 0
     self.tx_done = False
 
-    if self.debug: print(f"ISO-TP: REQUEST - {hexlify(self.tx_dat)}")
+    self.rx_dat = b""
+    self.rx_len = 0
+    self.rx_idx = 0
+    self.rx_done = False
+
+    if self.debug: print(f"ISO-TP: REQUEST - 0x{bytes.hex(self.tx_dat)}")
     self._tx_first_frame()
 
   def _tx_first_frame(self) -> None:
@@ -296,27 +353,25 @@ class IsoTpMessage():
       # first frame (send first 6 bytes)
       if self.debug: print("ISO-TP: TX - first frame")
       msg = (struct.pack("!H", 0x1000 | self.tx_len) + self.tx_dat[:6]).ljust(8, b"\x00")
-    self.can_tx_queue.put(msg)
+    self._can_client.send([msg])
 
   def recv(self) -> bytes:
-    self.rx_dat = b""
-    self.rx_len = 0
-    self.rx_idx = 0
-    self.rx_done = False
-
+    start_time = time.time()
     try:
       while True:
-        self._isotp_rx_next()
-        if self.tx_done and self.rx_done:
-          return self.rx_dat
-    except Empty:
-      raise MessageTimeoutError("timeout waiting for response")
+        for msg in self._can_client.recv():
+          self._isotp_rx_next(msg)
+          if self.tx_done and self.rx_done:
+            return self.rx_dat
+        # no timeout indicates non-blocking
+        if self.timeout == 0:
+          return None
+        if time.time() - start_time > self.timeout:
+          raise MessageTimeoutError("timeout waiting for response")
     finally:
-      if self.debug: print(f"ISO-TP: RESPONSE - {hexlify(self.rx_dat)}")
-
-  def _isotp_rx_next(self) -> None:
-    rx_data = self.can_rx_queue.get(block=True, timeout=self.timeout)
+      if self.debug and self.rx_dat: print(f"ISO-TP: RESPONSE - 0x{bytes.hex(self.rx_dat)}")
 
+  def _isotp_rx_next(self, rx_data: bytes) -> None:
     # single rx_frame
     if rx_data[0] >> 4 == 0x0:
       self.rx_len = rx_data[0] & 0xFF
@@ -336,9 +391,9 @@ class IsoTpMessage():
       if self.debug: print(f"ISO-TP: TX - flow control continue")
       # send flow control message (send all bytes)
       msg = b"\x30\x00\x00".ljust(8, b"\x00")
-      self.can_tx_queue.put(msg)
+      self._can_client.send([msg])
       return
-    
+
     # consecutive rx frame
     if rx_data[0] >> 4 == 0x2:
       assert self.rx_done == False, "isotp - rx: consecutive frame with no active frame"
@@ -361,19 +416,19 @@ class IsoTpMessage():
         delay_ts = rx_data[2] & 0x7F
         # scale is 1 milliseconds if first bit == 0, 100 micro seconds if first bit == 1
         delay_div = 1000. if rx_data[2] & 0x80 == 0 else 10000.
+        delay_sec = delay_ts / delay_div
         # first frame = 6 bytes, each consecutive frame = 7 bytes
         start = 6 + self.tx_idx * 7
         count = rx_data[1]
         end = start + count * 7 if count > 0 else self.tx_len
+        tx_msgs = []
         for i in range(start, end, 7):
-          if delay_ts > 0 and i > start:
-            delay_s = delay_ts / delay_div
-            if self.debug: print(f"ISO-TP: TX - delay - seconds={delay_s}")
-            time.sleep(delay_s)
           self.tx_idx += 1
-          # consecutive tx frames
+          # consecutive tx messages
           msg = (bytes([0x20 | (self.tx_idx & 0xF)]) + self.tx_dat[i:i+7]).ljust(8, b"\x00")
-          self.can_tx_queue.put(msg)
+          tx_msgs.append(msg)
+        # send consecutive tx messages
+        self._can_client.send(tx_msgs, delay=delay_sec)
         if end >= self.tx_len:
           self.tx_done = True
         if self.debug: print(f"ISO-TP: TX - consecutive frame - idx={self.tx_idx} done={self.tx_done}")
@@ -381,57 +436,30 @@ class IsoTpMessage():
         # wait (do nothing until next flow control message)
         if self.debug: print("ISO-TP: TX - flow control wait")
 
+FUNCTIONAL_ADDRS = [0x7DF, 0x18DB33F1]
+
+def get_rx_addr_for_tx_addr(tx_addr):
+  if tx_addr in FUNCTIONAL_ADDRS:
+    return None
+
+  if tx_addr < 0xFFF8:
+    # standard 11 bit response addr (add 8)
+    return tx_addr + 8
+  
+  if tx_addr > 0x10000000 and tx_addr < 0xFFFFFFFF:
+    # standard 29 bit response addr (flip last two bytes)
+    return (tx_addr & 0xFFFF0000) + (tx_addr<<8 & 0xFF00) + (tx_addr>>8 & 0xFF)
+
+  raise ValueError("invalid tx_addr: {}".format(tx_addr))
+
 class UdsClient():
-  def __init__(self, panda, tx_addr: int, rx_addr: int=None, bus: int=0, timeout: float=10, debug: bool=False):
-    self.panda = panda
+  def __init__(self, panda, tx_addr: int, rx_addr: int=None, bus: int=0, timeout: float=1, debug: bool=False):
     self.bus = bus
     self.tx_addr = tx_addr
-    if rx_addr == None:
-      if tx_addr < 0xFFF8:
-        # standard 11 bit response addr (add 8)
-        self.rx_addr = tx_addr+8
-      elif tx_addr > 0x10000000 and tx_addr < 0xFFFFFFFF:
-        # standard 29 bit response addr (flip last two bytes)
-        self.rx_addr = (tx_addr & 0xFFFF0000) + (tx_addr<<8 & 0xFF00) + (tx_addr>>8 & 0xFF)
-      else:
-        raise ValueError("invalid tx_addr: {}".format(tx_addr))
-
-    self.can_tx_queue = Queue()
-    self.can_rx_queue = Queue()
+    self.rx_addr = rx_addr if rx_addr is not None else get_rx_addr_for_tx_addr(tx_addr)
     self.timeout = timeout
     self.debug = debug
-
-    self.can_thread = Thread(target=self._can_thread, args=(self.debug,))
-    self.can_thread.daemon = True
-    self.can_thread.start()
-
-  def _can_thread(self, debug: bool=False):
-    try:
-      # allow all output
-      self.panda.set_safety_mode(0x1337)
-      # clear tx buffer
-      self.panda.can_clear(self.bus)
-      # clear rx buffer
-      self.panda.can_clear(0xFFFF)
-
-      while True:
-        # send
-        while not self.can_tx_queue.empty():
-          msg = self.can_tx_queue.get(block=False)
-          if debug: print("CAN-TX: {} - {}".format(hex(self.tx_addr), hexlify(msg)))
-          self.panda.can_send(self.tx_addr, msg, self.bus)
-
-        # receive
-        msgs = self.panda.can_recv()
-        for rx_addr, rx_ts, rx_data, rx_bus in msgs:
-          if rx_bus != self.bus or rx_addr != self.rx_addr or len(rx_data) == 0:
-            continue
-          if debug: print("CAN-RX: {} - {}".format(hex(self.rx_addr), hexlify(rx_data)))
-          self.can_rx_queue.put(rx_data)
-        else:
-          time.sleep(0.01)
-    finally:
-      self.panda.close()
+    self._can_client = CanClient(panda.can_send, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, debug=self.debug)
 
   # generic uds request
   def _uds_request(self, service_type: SERVICE_TYPE, subfunction: int=None, data: bytes=None) -> bytes:
@@ -442,7 +470,7 @@ class UdsClient():
       req += data
 
     # send request, wait for response
-    isotp_msg = IsoTpMessage(self.can_tx_queue, self.can_rx_queue, self.timeout, self.debug)
+    isotp_msg = IsoTpMessage(self._can_client, self.timeout, self.debug)
     isotp_msg.send(req)
     while True:
       resp = isotp_msg.recv()
@@ -453,12 +481,12 @@ class UdsClient():
         service_id = resp[1] if len(resp) > 1 else -1
         try:
           service_desc = SERVICE_TYPE(service_id).name
-        except Exception:
+        except BaseException:
           service_desc = 'NON_STANDARD_SERVICE'
         error_code = resp[2] if len(resp) > 2 else -1
         try:
           error_desc = _negative_response_codes[error_code]
-        except Exception:
+        except BaseException:
           error_desc = resp[3:]
         # wait for another message if response pending
         if error_code == 0x78:
diff --git a/panda/requirements.txt b/panda/requirements.txt
index e7cf9931d5..5ceb3cf6e7 100644
--- a/panda/requirements.txt
+++ b/panda/requirements.txt
@@ -6,6 +6,6 @@ tqdm>=4.14.0
 nose
 parameterized
 requests
-flake8==3.7.8
-pylint==2.4.2
+flake8==3.7.9
+pylint==2.4.3
 cffi==1.11.4
diff --git a/panda/tests/automated/1_program.py b/panda/tests/automated/1_program.py
index 538f18c0c7..98c32114fe 100644
--- a/panda/tests/automated/1_program.py
+++ b/panda/tests/automated/1_program.py
@@ -1,11 +1,34 @@
-from .helpers import test_all_pandas, panda_connect_and_init
+import os
+
+from nose.tools import assert_equal
+
+from panda import Panda, BASEDIR
+from .helpers import reset_pandas, test_all_pandas, panda_connect_and_init
+
+
+# Reset the pandas before flashing them
+def aaaa_reset_before_tests():
+  reset_pandas()
+
 
 @test_all_pandas
 @panda_connect_and_init
 def test_recover(p):
   assert p.recover(timeout=30)
 
+
 @test_all_pandas
 @panda_connect_and_init
 def test_flash(p):
   p.flash()
+
+
+@test_all_pandas
+@panda_connect_and_init
+def test_get_signature(p):
+  fn = os.path.join(BASEDIR, "board/obj/panda.bin")
+
+  firmware_sig = Panda.get_signature_from_firmware(fn)
+  panda_sig = p.get_signature()
+
+  assert_equal(panda_sig, firmware_sig)
diff --git a/panda/tests/automated/2_health.py b/panda/tests/automated/2_health.py
new file mode 100644
index 0000000000..da5eaec9d9
--- /dev/null
+++ b/panda/tests/automated/2_health.py
@@ -0,0 +1,44 @@
+import time
+from panda_jungle import PandaJungle  # pylint: disable=import-error
+from .helpers import panda_jungle, reset_pandas, test_all_pandas, test_all_gen2_pandas, panda_connect_and_init
+
+# Reset the pandas before running tests
+def aaaa_reset_before_tests():
+  reset_pandas()
+
+@test_all_pandas
+@panda_connect_and_init
+def test_ignition(p):
+  try:
+    # Set harness orientation to #2, since the ignition line is on the wrong SBU bus :/
+    panda_jungle.set_harness_orientation(PandaJungle.HARNESS_ORIENTATION_2)
+    reset_pandas()
+    p.reconnect()
+    panda_jungle.set_ignition(False)
+    time.sleep(2)
+    assert p.health()['ignition_line'] == False
+    panda_jungle.set_ignition(True)
+    time.sleep(2)
+    assert p.health()['ignition_line'] == True
+  finally:
+    panda_jungle.set_harness_orientation(PandaJungle.HARNESS_ORIENTATION_1)
+
+@test_all_gen2_pandas
+@panda_connect_and_init
+def test_orientation_detection(p):
+  seen_orientations = []
+  for i in range(3):
+    panda_jungle.set_harness_orientation(i)
+    reset_pandas()
+    p.reconnect()
+    detected_harness_orientation = p.health()['car_harness_status']
+    if (i == 0 and detected_harness_orientation != 0) or detected_harness_orientation in seen_orientations:
+      assert False
+    seen_orientations.append(detected_harness_orientation)
+
+
+@test_all_pandas
+@panda_connect_and_init
+def test_voltage(p):
+  voltage = p.health()['voltage']
+  assert ((voltage > 10000) and (voltage < 14000))
\ No newline at end of file
diff --git a/panda/tests/automated/2_usb_to_can.py b/panda/tests/automated/3_usb_to_can.py
similarity index 88%
rename from panda/tests/automated/2_usb_to_can.py
rename to panda/tests/automated/3_usb_to_can.py
index 32ef558cfc..282a37ef1c 100644
--- a/panda/tests/automated/2_usb_to_can.py
+++ b/panda/tests/automated/3_usb_to_can.py
@@ -2,11 +2,18 @@ import sys
 import time
 from panda import Panda
 from nose.tools import assert_equal, assert_less, assert_greater
-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
+from .helpers import start_heartbeat_thread, reset_pandas, SPEED_NORMAL, SPEED_GMLAN, time_many_sends, test_white_and_grey, panda_type_to_serial, test_all_pandas, panda_connect_and_init
+
+# Reset the pandas before running tests
+def aaaa_reset_before_tests():
+  reset_pandas()
 
 @test_all_pandas
 @panda_connect_and_init
 def test_can_loopback(p):
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
@@ -40,8 +47,11 @@ def test_can_loopback(p):
 @test_all_pandas
 @panda_connect_and_init
 def test_safety_nooutput(p):
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
-  p.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+  p.set_safety_mode(Panda.SAFETY_SILENT)
 
   # enable CAN loopback mode
   p.set_can_loopback(True)
@@ -60,6 +70,9 @@ def test_reliability(p):
   LOOP_COUNT = 100
   MSG_COUNT = 100
 
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
   p.set_can_loopback(True)
@@ -95,6 +108,9 @@ def test_reliability(p):
 @test_all_pandas
 @panda_connect_and_init
 def test_throughput(p):
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
@@ -122,6 +138,9 @@ def test_gmlan(p):
   if p.legacy:
     return
 
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
@@ -153,6 +172,9 @@ def test_gmlan_bad_toggle(p):
   if p.legacy:
     return
 
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
diff --git a/panda/tests/automated/3_wifi.py b/panda/tests/automated/4_wifi.py
similarity index 85%
rename from panda/tests/automated/3_wifi.py
rename to panda/tests/automated/4_wifi.py
index df66d6c0f3..15b8dc6c9e 100644
--- a/panda/tests/automated/3_wifi.py
+++ b/panda/tests/automated/4_wifi.py
@@ -1,8 +1,12 @@
 import time
 from panda import Panda
-from .helpers import connect_wifi, test_white, test_all_pandas, panda_type_to_serial, panda_connect_and_init
+from .helpers import reset_pandas, connect_wifi, test_white, test_all_pandas, panda_type_to_serial, panda_connect_and_init
 import requests
 
+# Reset the pandas before running tests
+def aaaa_reset_before_tests():
+  reset_pandas()
+
 @test_all_pandas
 @panda_connect_and_init
 def test_get_serial(p):
diff --git a/panda/tests/automated/4_wifi_functionality.py b/panda/tests/automated/5_wifi_functionality.py
similarity index 83%
rename from panda/tests/automated/4_wifi_functionality.py
rename to panda/tests/automated/5_wifi_functionality.py
index ee349ddcbf..88c81dcbdc 100644
--- a/panda/tests/automated/4_wifi_functionality.py
+++ b/panda/tests/automated/5_wifi_functionality.py
@@ -1,6 +1,10 @@
 import time
 from panda import Panda
-from .helpers import time_many_sends, connect_wifi, test_white, panda_type_to_serial
+from .helpers import start_heartbeat_thread, reset_pandas, time_many_sends, connect_wifi, test_white, panda_type_to_serial
+
+# Reset the pandas before running tests
+def aaaa_reset_before_tests():
+  reset_pandas()
 
 @test_white
 @panda_type_to_serial
@@ -16,6 +20,9 @@ def test_throughput(serials=None):
   connect_wifi(serials[0])
   p = Panda(serials[0])
 
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   # enable output mode
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
@@ -43,6 +50,10 @@ def test_throughput(serials=None):
 def test_recv_only(serials=None):
   connect_wifi(serials[0])
   p = Panda(serials[0])
+
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
 
   p.set_can_loopback(True)
diff --git a/panda/tests/automated/6_two_panda.py b/panda/tests/automated/6_two_panda.py
deleted file mode 100644
index f91403545f..0000000000
--- a/panda/tests/automated/6_two_panda.py
+++ /dev/null
@@ -1,195 +0,0 @@
-
-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, test_two_black_panda, panda_type_to_serial, clear_can_buffers, panda_connect_and_init
-
-@test_two_panda
-@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)
-  p_recv.set_can_loopback(False)
-
-  assert not p_send.legacy
-  assert not p_recv.legacy
-
-  p_send.can_send_many([(0x1ba, 0, b"message", 0)]*2)
-  time.sleep(0.05)
-  p_recv.can_recv()
-  p_send.can_recv()
-
-  busses = [0,1,2]
-
-  for bus in busses:
-    for speed in [100, 250, 500, 750, 1000]:
-      p_send.set_can_speed_kbps(bus, speed)
-      p_recv.set_can_speed_kbps(bus, speed)
-      time.sleep(0.05)
-
-      comp_kbps = time_many_sends(p_send, bus, p_recv, two_pandas=True)
-
-      saturation_pct = (comp_kbps/speed) * 100.0
-      assert_greater(saturation_pct, 80)
-      assert_less(saturation_pct, 100)
-
-      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_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
-  assert not p_recv.legacy
-
-  p_send.set_can_speed_kbps(0, 100)
-  p_recv.set_can_speed_kbps(0, 100)
-  time.sleep(0.05)
-
-  p_send.can_send_many([(0x1ba, 0, b"testmsg", 0)]*10)
-  time.sleep(0.05)
-  p_recv.can_recv()
-  p_send.can_recv()
-
-  busses = [0,1,2]
-
-  for bus in busses:
-    for speed in [100, 250, 500, 750, 1000]:
-      p_send.set_can_speed_kbps(bus, speed)
-      p_recv.set_can_speed_kbps(bus, speed)
-      time.sleep(0.1)
-
-      #clear can buffers
-      clear_can_buffers(p_send)
-      clear_can_buffers(p_recv)
-
-      latencies = []
-      comp_kbps_list = []
-      saturation_pcts = []
-
-      num_messages = 100
-
-      for i in range(num_messages):
-        st = time.time()
-        p_send.can_send(0x1ab, b"message", bus)
-        r = []
-        while len(r) < 1 and (time.time() - st) < 5:
-          r = p_recv.can_recv()
-        et = time.time()
-        r_echo = []
-        while len(r_echo) < 1 and (time.time() - st) < 10:
-          r_echo = p_send.can_recv()
-
-        if len(r) == 0 or len(r_echo) == 0:
-          print("r: {}, r_echo: {}".format(r, r_echo))
-
-        assert_equal(len(r),1)
-        assert_equal(len(r_echo),1)
-
-        et = (et - st)*1000.0
-        comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7) / et
-        latency = et - ((1+11+1+1+1+4+8*8+15+1+1+1+7) / speed)
-
-        assert_less(latency, 5.0)
-
-        saturation_pct = (comp_kbps/speed) * 100.0
-        latencies.append(latency)
-        comp_kbps_list.append(comp_kbps)
-        saturation_pcts.append(saturation_pct)
-
-      average_latency = sum(latencies)/num_messages
-      assert_less(average_latency, 1.0)
-      average_comp_kbps = sum(comp_kbps_list)/num_messages
-      average_saturation_pct = sum(saturation_pcts)/num_messages
-
-      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, b"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
-      _ = send_panda.can_recv()  # cans echo
-      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)
diff --git a/panda/tests/automated/5_wifi_udp.py b/panda/tests/automated/6_wifi_udp.py
similarity index 88%
rename from panda/tests/automated/5_wifi_udp.py
rename to panda/tests/automated/6_wifi_udp.py
index fd905aa895..197e4d46c7 100644
--- a/panda/tests/automated/5_wifi_udp.py
+++ b/panda/tests/automated/6_wifi_udp.py
@@ -1,16 +1,23 @@
-
 import sys
 import time
-from .helpers import time_many_sends, connect_wifi, test_white, panda_type_to_serial
+from .helpers import start_heartbeat_thread, reset_pandas, time_many_sends, connect_wifi, test_white, panda_type_to_serial
 from panda import Panda, PandaWifiStreaming
 from nose.tools import assert_less, assert_greater
 
+# Reset the pandas before running tests
+def aaaa_reset_before_tests():
+  reset_pandas()
+
 @test_white
 @panda_type_to_serial
 def test_udp_doesnt_drop(serials=None):
   connect_wifi(serials[0])
 
   p = Panda(serials[0])
+
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
   p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
   p.set_can_loopback(True)
 
diff --git a/panda/tests/automated/7_can_loopback.py b/panda/tests/automated/7_can_loopback.py
new file mode 100644
index 0000000000..cb9f5570b5
--- /dev/null
+++ b/panda/tests/automated/7_can_loopback.py
@@ -0,0 +1,202 @@
+import os
+import time
+import random
+from panda import Panda
+from nose.tools import assert_equal, assert_less, assert_greater
+from .helpers import panda_jungle, start_heartbeat_thread, reset_pandas, time_many_sends, test_all_pandas, test_all_gen2_pandas, clear_can_buffers, panda_connect_and_init
+
+# Reset the pandas before running tests
+def aaaa_reset_before_tests():
+  reset_pandas()
+
+@test_all_pandas
+@panda_connect_and_init
+def test_send_recv(p):
+  def test(p_send, p_recv):
+    p_send.set_can_loopback(False)
+    p_recv.set_can_loopback(False)
+
+    p_send.can_send_many([(0x1ba, 0, b"message", 0)]*2)
+    time.sleep(0.05)
+    p_recv.can_recv()
+    p_send.can_recv()
+
+    busses = [0,1,2]
+
+    for bus in busses:
+      for speed in [100, 250, 500, 750, 1000]:
+        p_send.set_can_speed_kbps(bus, speed)
+        p_recv.set_can_speed_kbps(bus, speed)
+        time.sleep(0.05)
+
+        clear_can_buffers(p_send)
+        clear_can_buffers(p_recv)
+
+        comp_kbps = time_many_sends(p_send, bus, p_recv, two_pandas=True)
+
+        saturation_pct = (comp_kbps/speed) * 100.0
+        assert_greater(saturation_pct, 80)
+        assert_less(saturation_pct, 100)
+
+        print("two pandas bus {}, 100 messages at speed {:4d}, comp speed is {:7.2f}, percent {:6.2f}".format(bus, speed, comp_kbps, saturation_pct))
+
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
+  # Set safety mode and power saving
+  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  p.set_power_save(False)
+
+  try:
+    # Run tests in both directions
+    test(p, panda_jungle)
+    test(panda_jungle, p)
+  except Exception as e:
+    # Raise errors again, we don't want them to get lost
+    raise e
+  finally:
+    # Set back to silent mode
+    p.set_safety_mode(Panda.SAFETY_SILENT)
+
+@test_all_pandas
+@panda_connect_and_init
+def test_latency(p):
+  def test(p_send, p_recv):
+    p_send.set_can_loopback(False)
+    p_recv.set_can_loopback(False)
+
+    p_send.set_can_speed_kbps(0, 100)
+    p_recv.set_can_speed_kbps(0, 100)
+    time.sleep(0.05)
+
+    p_send.can_send_many([(0x1ba, 0, b"testmsg", 0)]*10)
+    time.sleep(0.05)
+    p_recv.can_recv()
+    p_send.can_recv()
+
+    busses = [0,1,2]
+
+    for bus in busses:
+      for speed in [100, 250, 500, 750, 1000]:
+        p_send.set_can_speed_kbps(bus, speed)
+        p_recv.set_can_speed_kbps(bus, speed)
+        time.sleep(0.1)
+
+        # clear can buffers
+        clear_can_buffers(p_send)
+        clear_can_buffers(p_recv)
+
+        latencies = []
+        comp_kbps_list = []
+        saturation_pcts = []
+
+        num_messages = 100
+
+        for i in range(num_messages):
+          st = time.time()
+          p_send.can_send(0x1ab, b"message", bus)
+          r = []
+          while len(r) < 1 and (time.time() - st) < 5:
+            r = p_recv.can_recv()
+          et = time.time()
+          r_echo = []
+          while len(r_echo) < 1 and (time.time() - st) < 10:
+            r_echo = p_send.can_recv()
+
+          if len(r) == 0 or len(r_echo) == 0:
+            print("r: {}, r_echo: {}".format(r, r_echo))
+
+          assert_equal(len(r),1)
+          assert_equal(len(r_echo),1)
+
+          et = (et - st)*1000.0
+          comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7) / et
+          latency = et - ((1+11+1+1+1+4+8*8+15+1+1+1+7) / speed)
+
+          assert_less(latency, 5.0)
+
+          saturation_pct = (comp_kbps/speed) * 100.0
+          latencies.append(latency)
+          comp_kbps_list.append(comp_kbps)
+          saturation_pcts.append(saturation_pct)
+
+        average_latency = sum(latencies)/num_messages
+        assert_less(average_latency, 1.0)
+        average_comp_kbps = sum(comp_kbps_list)/num_messages
+        average_saturation_pct = sum(saturation_pcts)/num_messages
+
+        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))
+
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
+  # Set safety mode and power saving
+  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  p.set_power_save(False)
+
+  try:
+    # Run tests in both directions
+    test(p, panda_jungle)
+    test(panda_jungle, p)
+  except Exception as e:
+    # Raise errors again, we don't want them to get lost
+    raise e
+  finally:
+    # Set back to silent mode
+    p.set_safety_mode(Panda.SAFETY_SILENT)
+
+
+@test_all_gen2_pandas
+@panda_connect_and_init
+def test_gen2_loopback(p):
+  def test(p_send, p_recv):
+    for bus in range(4):
+      obd = False
+      if bus == 3:
+        obd = True
+        bus = 1
+      
+      # Clear buses
+      clear_can_buffers(p_send)
+      clear_can_buffers(p_recv)
+
+      # Send a random string
+      addr = random.randint(1, 2000)
+      string = b"test"+os.urandom(4)
+      p_send.set_obd(obd)
+      p_recv.set_obd(obd)
+      time.sleep(0.2)
+      p_send.can_send(addr, string, bus)
+      time.sleep(0.2)
+
+      content = p_recv.can_recv()
+
+      # Check amount of messages
+      assert len(content) == 1
+
+      # Check content
+      assert content[0][0] == addr and content[0][2] == string
+      
+      # Check bus
+      assert content[0][3] == bus
+
+      print("Bus:", bus, "OBD:", obd, "OK")
+
+  # Start heartbeat
+  start_heartbeat_thread(p)
+
+  # Set safety mode and power saving
+  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  p.set_power_save(False)
+
+  try:
+    # Run tests in both directions
+    test(p, panda_jungle)
+    test(panda_jungle, p)
+  except Exception as e:
+    # Raise errors again, we don't want them to get lost
+    raise e
+  finally:
+    # Set back to silent mode
+    p.set_safety_mode(Panda.SAFETY_SILENT)
diff --git a/panda/tests/automated/helpers.py b/panda/tests/automated/helpers.py
index 2642e9156a..78a9287c10 100644
--- a/panda/tests/automated/helpers.py
+++ b/panda/tests/automated/helpers.py
@@ -5,21 +5,52 @@ import random
 import subprocess
 import requests
 import _thread
+import faulthandler
 from functools import wraps
 from panda import Panda
+from panda_jungle import PandaJungle  # pylint: disable=import-error
 from nose.tools import assert_equal
 from parameterized import parameterized, param
+from .timeout import run_with_timeout
+from .wifi_helpers import _connect_wifi
 
 SPEED_NORMAL = 500
 SPEED_GMLAN = 33.3
+BUS_SPEEDS = [(0, SPEED_NORMAL), (1, SPEED_NORMAL), (2, SPEED_NORMAL), (3, SPEED_GMLAN)]
+TIMEOUT = 30
+GEN2_HW_TYPES = [Panda.HW_TYPE_BLACK_PANDA, Panda.HW_TYPE_UNO]
 
+# Enable fault debug
+faulthandler.enable(all_threads=False)
+
+# Connect to Panda Jungle
+panda_jungle = PandaJungle()
+
+# Find all panda's connected
+_panda_serials = None
+def init_panda_serials():
+  global panda_jungle, _panda_serials
+  _panda_serials = []
+  panda_jungle.set_panda_power(True)
+  time.sleep(5)
+  for serial in Panda.list():
+    p = Panda(serial=serial)
+    _panda_serials.append((serial, p.get_type()))
+    p.close()
+  print('Found', str(len(_panda_serials)), 'pandas')
+init_panda_serials()
+
+# Panda providers
 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()
+    list(map(lambda x: x[0], _panda_serials))
+  )
+test_all_gen2_pandas = parameterized(
+    list(map(lambda x: x[0], filter(lambda x: x[1] in GEN2_HW_TYPES, _panda_serials)))
   )
 test_white_and_grey = parameterized([
     param(panda_type=Panda.HW_TYPE_WHITE_PANDA),
@@ -31,13 +62,8 @@ test_white = parameterized([
 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])
+test_black = parameterized([
+    param(panda_type=Panda.HW_TYPE_BLACK_PANDA)
   ])
 
 def connect_wifi(serial=None):
@@ -47,111 +73,26 @@ def connect_wifi(serial=None):
   assert(dongle_id.isalnum())
   _connect_wifi(dongle_id, pw)
 
-FNULL = open(os.devnull, 'w')
-def _connect_wifi(dongle_id, pw, insecure_okay=False):
-  ssid = "panda-" + dongle_id
-
-  r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT)
-  if not r:
-    #Can already ping, try connecting on wifi
-    try:
-      p = Panda("WIFI")
-      p.get_serial()
-      print("Already connected")
-      return
-    except:
-      pass
-
-  print("WIFI: connecting to %s" % ssid)
-
-  while 1:
-    if sys.platform == "darwin":
-      os.system("networksetup -setairportnetwork en0 %s %s" % (ssid, pw))
-    else:
-      wlan_interface = subprocess.check_output(["sh", "-c", "iw dev | awk '/Interface/ {print $2}'"]).strip().decode('utf8')
-      cnt = 0
-      MAX_TRIES = 10
-      while cnt < MAX_TRIES:
-        print("WIFI: scanning %d" % cnt)
-        os.system("iwlist %s scanning > /dev/null" % wlan_interface)
-        os.system("nmcli device wifi rescan")
-        wifi_networks = [x.decode("utf8") for x in subprocess.check_output(["nmcli","dev", "wifi", "list"]).split(b"\n")]
-        wifi_scan = [x for x in wifi_networks if ssid in x]
-        if len(wifi_scan) != 0:
-          break
-        time.sleep(0.1)
-        # MAX_TRIES tries, ~10 seconds max
-        cnt += 1
-      assert cnt < MAX_TRIES
-      if "-pair" in wifi_scan[0]:
-        os.system("nmcli d wifi connect %s-pair" % (ssid))
-        connect_cnt = 0
-        MAX_TRIES = 100
-        while connect_cnt < MAX_TRIES:
-          connect_cnt += 1
-          r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT)
-          if r:
-            print("Waiting for panda to ping...")
-            time.sleep(0.5)
-          else:
-            break
-        if insecure_okay:
-          break
-        # fetch webpage
-        print("connecting to insecure network to secure")
-        try:
-          r = requests.get("http://192.168.0.10/")
-        except requests.ConnectionError:
-          r = requests.get("http://192.168.0.10/")
-        assert r.status_code==200
-
-        print("securing")
-        try:
-          r = requests.get("http://192.168.0.10/secure", timeout=0.01)
-        except requests.exceptions.Timeout:
-          print("timeout http request to secure")
-          pass
-      else:
-        ret = os.system("nmcli d wifi connect %s password %s" % (ssid, pw))
-        if os.WEXITSTATUS(ret) == 0:
-          #check ping too
-          ping_ok = False
-          connect_cnt = 0
-          MAX_TRIES = 10
-          while connect_cnt < MAX_TRIES:
-            connect_cnt += 1
-            r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT)
-            if r:
-              print("Waiting for panda to ping...")
-              time.sleep(0.1)
-            else:
-              ping_ok = True
-              break
-          if ping_ok:
-            break
-
-  # TODO: confirm that it's connected to the right panda
-
-def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None, two_pandas=False):
-  if precv == None:
-    precv = p
+def time_many_sends(p, bus, p_recv=None, msg_count=100, msg_id=None, two_pandas=False):
+  if p_recv == None:
+    p_recv = p
   if msg_id == None:
     msg_id = random.randint(0x100, 0x200)
-  if p == precv and two_pandas:
+  if p == p_recv and two_pandas:
     raise ValueError("Cannot have two pandas that are the same panda")
 
-  st = time.time()
+  start_time = time.time()
   p.can_send_many([(msg_id, 0, b"\xaa"*8, bus)]*msg_count)
   r = []
   r_echo = []
   r_len_expected = msg_count if two_pandas else msg_count*2
   r_echo_len_exected = msg_count if two_pandas else 0
 
-  while len(r) < r_len_expected and (time.time() - st) < 5:
-    r.extend(precv.can_recv())
-  et = time.time()
+  while len(r) < r_len_expected and (time.time() - start_time) < 5:
+    r.extend(p_recv.can_recv())
+  end_time = time.time()
   if two_pandas:
-    while len(r_echo) < r_echo_len_exected and (time.time() - st) < 10:
+    while len(r_echo) < r_echo_len_exected and (time.time() - start_time) < 10:
       r_echo.extend(p.can_recv())
 
   sent_echo = [x for x in r if x[3] == 0x80 | bus and x[0] == msg_id]
@@ -164,12 +105,17 @@ def time_many_sends(p, bus, precv=None, msg_count=100, msg_id=None, two_pandas=F
   assert_equal(len(resp), msg_count)
   assert_equal(len(sent_echo), msg_count)
 
-  et = (et-st)*1000.0
-  comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7)*msg_count / et
+  end_time = (end_time-start_time)*1000.0
+  comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7)*msg_count / end_time
 
   return comp_kbps
 
-_panda_serials = None
+def reset_pandas():
+  panda_jungle.set_panda_power(False)
+  time.sleep(2)
+  panda_jungle.set_panda_power(True)
+  time.sleep(5)
+
 def panda_type_to_serial(fn):
   @wraps(fn)
   def wrapper(panda_type=None, **kwargs):
@@ -181,11 +127,7 @@ def panda_type_to_serial(fn):
     # 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()
+      init_panda_serials()
 
     # Find a panda with the correct types and add the corresponding serial
     serials = []
@@ -202,13 +144,15 @@ def panda_type_to_serial(fn):
     return fn(serials, **kwargs)
   return wrapper
 
-def heartbeat_thread(p):
-  while True:
-    try:
-      p.send_heartbeat()
-      time.sleep(1)
-    except:
-      break
+def start_heartbeat_thread(p):
+  def heartbeat_thread(p):
+    while True:
+      try:
+        p.send_heartbeat()
+        time.sleep(1)
+      except:
+        break
+  _thread.start_new_thread(heartbeat_thread, (p,))
 
 def panda_connect_and_init(fn):
   @wraps(fn)
@@ -223,25 +167,38 @@ def panda_connect_and_init(fn):
     for panda_serial in panda_serials:
       pandas.append(Panda(serial=panda_serial))
 
+    # Initialize jungle
+    clear_can_buffers(panda_jungle)
+    panda_jungle.set_can_loopback(False)
+    panda_jungle.set_obd(False)
+    panda_jungle.set_harness_orientation(PandaJungle.HARNESS_ORIENTATION_1)
+    for bus, speed in BUS_SPEEDS:
+        panda_jungle.set_can_speed_kbps(bus, speed)
+
     # 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_power_save(False)
+      for bus, speed in BUS_SPEEDS:
         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)
+      panda.set_power_save(False)
 
-    # Close all connections
-    for panda in pandas:
-      panda.close()
-
-    # Return test function result
-    return ret
+    try:
+      run_with_timeout(TIMEOUT, fn, *pandas, **kwargs)
+
+      # Check if the pandas did not throw any faults while running test
+      for panda in pandas:
+        panda.reconnect()
+        assert panda.health()['fault_status'] == 0
+    except Exception as e:
+      raise e
+    finally:
+      # Close all connections
+      for panda in pandas:
+        panda.close()        
   return wrapper
 
 def clear_can_buffers(panda):
diff --git a/panda/tests/automated/timeout.py b/panda/tests/automated/timeout.py
new file mode 100644
index 0000000000..f937844f07
--- /dev/null
+++ b/panda/tests/automated/timeout.py
@@ -0,0 +1,25 @@
+import time
+from multiprocessing import Process
+
+# Note: this does not return any return values of the function, just the exit status
+INTERVAL = 0.1
+def run_with_timeout(timeout, fn, *kwargs):
+  def runner(fn, kwargs):
+    try:
+      fn(*kwargs)
+    except Exception as e:
+      print(e)
+      raise e
+  
+  process = Process(target=runner, args=(fn, kwargs))
+  process.start()
+
+  counter = 0
+  while process.is_alive():
+    time.sleep(INTERVAL)
+    counter+=1
+    if (counter * INTERVAL) > timeout:
+      process.terminate()
+      raise TimeoutError("Function timed out!")
+  if process.exitcode != 0:
+    raise RuntimeError("Test failed with exit code: ", str(process.exitcode))
\ No newline at end of file
diff --git a/panda/tests/automated/wifi_helpers.py b/panda/tests/automated/wifi_helpers.py
new file mode 100644
index 0000000000..de9b224a1a
--- /dev/null
+++ b/panda/tests/automated/wifi_helpers.py
@@ -0,0 +1,85 @@
+import os
+FNULL = open(os.devnull, 'w')
+def _connect_wifi(dongle_id, pw, insecure_okay=False):
+  ssid = "panda-" + dongle_id
+
+  r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT)
+  if not r:
+    # Can already ping, try connecting on wifi
+    try:
+      p = Panda("WIFI")
+      p.get_serial()
+      print("Already connected")
+      return
+    except:
+      pass
+
+  print("WIFI: connecting to %s" % ssid)
+
+  while 1:
+    if sys.platform == "darwin":
+      os.system("networksetup -setairportnetwork en0 %s %s" % (ssid, pw))
+    else:
+      wlan_interface = subprocess.check_output(["sh", "-c", "iw dev | awk '/Interface/ {print $2}'"]).strip().decode('utf8')
+      cnt = 0
+      MAX_TRIES = 10
+      while cnt < MAX_TRIES:
+        print("WIFI: scanning %d" % cnt)
+        os.system("iwlist %s scanning > /dev/null" % wlan_interface)
+        os.system("nmcli device wifi rescan")
+        wifi_networks = [x.decode("utf8") for x in subprocess.check_output(["nmcli","dev", "wifi", "list"]).split(b"\n")]
+        wifi_scan = [x for x in wifi_networks if ssid in x]
+        if len(wifi_scan) != 0:
+          break
+        time.sleep(0.1)
+        # MAX_TRIES tries, ~10 seconds max
+        cnt += 1
+      assert cnt < MAX_TRIES
+      if "-pair" in wifi_scan[0]:
+        os.system("nmcli d wifi connect %s-pair" % (ssid))
+        connect_cnt = 0
+        MAX_TRIES = 100
+        while connect_cnt < MAX_TRIES:
+          connect_cnt += 1
+          r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT)
+          if r:
+            print("Waiting for panda to ping...")
+            time.sleep(0.5)
+          else:
+            break
+        if insecure_okay:
+          break
+        # fetch webpage
+        print("connecting to insecure network to secure")
+        try:
+          r = requests.get("http://192.168.0.10/")
+        except requests.ConnectionError:
+          r = requests.get("http://192.168.0.10/")
+        assert r.status_code==200
+
+        print("securing")
+        try:
+          r = requests.get("http://192.168.0.10/secure", timeout=0.01)
+        except requests.exceptions.Timeout:
+          print("timeout http request to secure")
+          pass
+      else:
+        ret = os.system("nmcli d wifi connect %s password %s" % (ssid, pw))
+        if os.WEXITSTATUS(ret) == 0:
+          #check ping too
+          ping_ok = False
+          connect_cnt = 0
+          MAX_TRIES = 10
+          while connect_cnt < MAX_TRIES:
+            connect_cnt += 1
+            r = subprocess.call(["ping", "-W", "4", "-c", "1", "192.168.0.10"], stdout=FNULL, stderr=subprocess.STDOUT)
+            if r:
+              print("Waiting for panda to ping...")
+              time.sleep(0.1)
+            else:
+              ping_ok = True
+              break
+          if ping_ok:
+            break
+
+  # TODO: confirm that it's connected to the right panda
\ No newline at end of file
diff --git a/panda/tests/black_white_loopback_test.py b/panda/tests/black_white_loopback_test.py
index 66c5e80f45..d700068d9e 100755
--- a/panda/tests/black_white_loopback_test.py
+++ b/panda/tests/black_white_loopback_test.py
@@ -67,7 +67,7 @@ def run_test(sleep_duration):
     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)
+    black_panda.set_safety_mode(Panda.SAFETY_SILENT)
     time.sleep(1)
     black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
     time.sleep(1)
diff --git a/panda/tests/black_white_relay_endurance.py b/panda/tests/black_white_relay_endurance.py
index 6970966526..d3d61be661 100755
--- a/panda/tests/black_white_relay_endurance.py
+++ b/panda/tests/black_white_relay_endurance.py
@@ -72,7 +72,7 @@ def run_test(sleep_duration):
 
     if (time.time() - temp_start_time) > 3600*6:
     	# Toggle relay
-      black_panda.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+      black_panda.set_safety_mode(Panda.SAFETY_SILENT)
       time.sleep(1)
       black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
       time.sleep(1)
diff --git a/panda/tests/black_white_relay_test.py b/panda/tests/black_white_relay_test.py
index d80490f7d4..f12e42d39b 100755
--- a/panda/tests/black_white_relay_test.py
+++ b/panda/tests/black_white_relay_test.py
@@ -74,7 +74,7 @@ def run_test(sleep_duration):
       assert False
 
     # Switch off relay
-    black_panda.set_safety_mode(Panda.SAFETY_NOOUTPUT)
+    black_panda.set_safety_mode(Panda.SAFETY_SILENT)
     time.sleep(0.05)
 
     if not test_buses(black_panda, other_panda, (0, False, [0, 2])):
diff --git a/panda/tests/debug_console.py b/panda/tests/debug_console.py
index 8e66946dd4..7b354b1487 100755
--- a/panda/tests/debug_console.py
+++ b/panda/tests/debug_console.py
@@ -44,6 +44,6 @@ if __name__ == "__main__":
             if claim:
               panda.serial_write(port_number, ln)
           time.sleep(0.01)
-    except:
+    except Exception:
       print("panda disconnected!")
       time.sleep(0.5);
diff --git a/panda/tests/development/register_hashmap_spread.py b/panda/tests/development/register_hashmap_spread.py
new file mode 100755
index 0000000000..ba0a0eb29d
--- /dev/null
+++ b/panda/tests/development/register_hashmap_spread.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+import matplotlib.pyplot as plt # pylint: disable=import-error
+
+HASHING_PRIME = 23
+REGISTER_MAP_SIZE = 0x3FF
+BYTES_PER_REG = 4
+
+# From ST32F413 datasheet
+REGISTER_ADDRESS_REGIONS = [
+  (0x40000000, 0x40007FFF),
+  (0x40010000, 0x400107FF),
+  (0x40011000, 0x400123FF),
+  (0x40012C00, 0x40014BFF),
+  (0x40015000, 0x400153FF),
+  (0x40015800, 0x40015BFF),
+  (0x40016000, 0x400167FF),
+  (0x40020000, 0x40021FFF),
+  (0x40023000, 0x400233FF),
+  (0x40023800, 0x40023FFF),
+  (0x40026000, 0x400267FF),
+  (0x50000000, 0x5003FFFF),
+  (0x50060000, 0x500603FF),
+  (0x50060800, 0x50060BFF),
+  (0x50060800, 0x50060BFF),
+  (0xE0000000, 0xE00FFFFF)
+]
+
+def hash(reg_addr):
+  return (((reg_addr >> 16) ^ ((((reg_addr + 1) & 0xFFFF) * HASHING_PRIME) & 0xFFFF)) & REGISTER_MAP_SIZE)
+
+# Calculate hash for each address
+hashes = []
+double_hashes = []
+for (start_addr, stop_addr) in REGISTER_ADDRESS_REGIONS:
+  for addr in range(start_addr, stop_addr+1, BYTES_PER_REG):
+    h = hash(addr)
+    hashes.append(h)
+    double_hashes.append(hash(h))
+
+# Make histograms
+plt.subplot(2, 1, 1)
+plt.hist(hashes, bins=REGISTER_MAP_SIZE)
+plt.title("Number of collisions per hash")
+plt.xlabel("Address")
+
+plt.subplot(2, 1, 2)
+plt.hist(double_hashes, bins=REGISTER_MAP_SIZE)
+plt.title("Number of collisions per double hash")
+plt.xlabel("Address")
+plt.show()
diff --git a/panda/tests/echo.py b/panda/tests/echo.py
new file mode 100755
index 0000000000..9ef0cf1074
--- /dev/null
+++ b/panda/tests/echo.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+import os
+import sys
+import time
+import _thread
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+
+# This script is intended to be used in conjunction with the echo_loopback_test.py test script from panda jungle.
+# It sends a reversed response back for every message received containing b"test".
+
+def heartbeat_thread(p):
+  while True:
+    try:
+      p.send_heartbeat()
+      time.sleep(1)
+    except:
+      break
+
+# Resend every CAN message that has been received on the same bus, but with the data reversed
+if __name__ == "__main__":
+  p = Panda()
+  _thread.start_new_thread(heartbeat_thread, (p,))
+  p.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+  p.set_power_save(False)
+
+  while True:
+    incoming = p.can_recv()
+    for message in incoming:
+      address, notused, data, bus = message
+      if b'test' in data:
+        p.can_send(address, data[::-1], bus)
+
+
diff --git a/panda/tests/ir_test.py b/panda/tests/ir_test.py
index caef9e4909..a65fe06865 100755
--- a/panda/tests/ir_test.py
+++ b/panda/tests/ir_test.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 import os
 import sys
 import time
diff --git a/panda/tests/misra/.gitignore b/panda/tests/misra/.gitignore
new file mode 100644
index 0000000000..fa3aeaa306
--- /dev/null
+++ b/panda/tests/misra/.gitignore
@@ -0,0 +1 @@
+cppcheck/
diff --git a/panda/tests/misra/coverage_table b/panda/tests/misra/coverage_table
index dcb5992bc7..9dc89bd4f5 100644
--- a/panda/tests/misra/coverage_table
+++ b/panda/tests/misra/coverage_table
@@ -7,7 +7,7 @@
 2.4     X (Cppcheck)
 2.5     
 2.6     X (Cppcheck)
-2.7     
+2.7     X (Addon)
 3.1     X (Addon)
 3.2     X (Addon)
 4.1     X (Addon)
diff --git a/panda/tests/misra/test_misra.sh b/panda/tests/misra/test_misra.sh
index 2542290744..47a38dad23 100755
--- a/panda/tests/misra/test_misra.sh
+++ b/panda/tests/misra/test_misra.sh
@@ -4,7 +4,7 @@ mkdir /tmp/misra || true
 git clone https://github.com/danmar/cppcheck.git || true
 cd cppcheck
 git fetch
-git checkout bdd41151ed550e3d240a6dd6847859216b7ad736
+git checkout e46191e6e809272d8b34feca8999ee413f716b80
 make -j4
 cd ../../../
 
diff --git a/panda/tests/safety/common.py b/panda/tests/safety/common.py
new file mode 100644
index 0000000000..99f7237921
--- /dev/null
+++ b/panda/tests/safety/common.py
@@ -0,0 +1,36 @@
+from panda.tests.safety import libpandasafety_py
+
+def make_msg(bus, addr, length=8):
+  to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
+  if addr >= 0x800:
+    to_send[0].RIR = (addr << 3) | 5
+  else:
+    to_send[0].RIR = (addr << 21) | 1
+  to_send[0].RDTR = length
+  to_send[0].RDTR |= bus << 4
+
+  return to_send
+
+def test_relay_malfunction(test, addr):
+  # input is a test class and the address that, if seen on bus 0, triggers
+  # the relay_malfunction protection logic: both tx_hook and fwd_hook are
+  # expected to return failure
+  test.assertFalse(test.safety.get_relay_malfunction())
+  test.safety.safety_rx_hook(make_msg(0, addr, 8))
+  test.assertTrue(test.safety.get_relay_malfunction())
+  for a in range(1, 0x800):
+    for b in range(0, 3):
+      test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8)))
+      test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8)))
+
+def test_manually_enable_controls_allowed(test):
+  test.safety.set_controls_allowed(1)
+  test.assertTrue(test.safety.get_controls_allowed())
+  test.safety.set_controls_allowed(0)
+  test.assertFalse(test.safety.get_controls_allowed())
+
+def test_spam_can_buses(test, TX_MSGS):
+  for addr in range(1, 0x800):
+    for bus in range(0, 4):
+      if all(addr != m[0] or bus != m[1] for m in TX_MSGS):
+        test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8)))
diff --git a/panda/tests/safety/libpandasafety_py.py b/panda/tests/safety/libpandasafety_py.py
index 819fcada37..5b1bd28c9a 100644
--- a/panda/tests/safety/libpandasafety_py.py
+++ b/panda/tests/safety/libpandasafety_py.py
@@ -34,6 +34,8 @@ bool board_has_relay(void);
 
 void set_controls_allowed(bool c);
 bool get_controls_allowed(void);
+void set_relay_malfunction(bool c);
+bool get_relay_malfunction(void);
 void set_long_controls_allowed(bool c);
 bool get_long_controls_allowed(void);
 void set_gas_interceptor_detected(bool c);
@@ -46,7 +48,7 @@ void reset_angle_control(void);
 void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_send);
 int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push);
 int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
-int safety_set_mode(uint16_t  mode, int16_t param);
+int set_safety_hooks(uint16_t  mode, int16_t param);
 
 void init_tests_toyota(void);
 int get_toyota_torque_meas_min(void);
@@ -54,7 +56,6 @@ int get_toyota_torque_meas_max(void);
 int get_toyota_gas_prev(void);
 void set_toyota_torque_meas(int min, int max);
 void set_toyota_desired_torque_last(int t);
-void set_toyota_camera_forwarded(int t);
 void set_toyota_rt_torque_last(int t);
 
 void init_tests_honda(void);
@@ -80,13 +81,10 @@ void init_tests_hyundai(void);
 void set_hyundai_desired_torque_last(int t);
 void set_hyundai_rt_torque_last(int t);
 void set_hyundai_torque_driver(int min, int max);
-void set_hyundai_giraffe_switch_2(int t);
-void set_hyundai_camera_bus(int t);
 
 void init_tests_chrysler(void);
 void set_chrysler_desired_torque_last(int t);
 void set_chrysler_rt_torque_last(int t);
-void set_chrysler_camera_detected(int t);
 int get_chrysler_torque_meas_min(void);
 int get_chrysler_torque_meas_max(void);
 void set_chrysler_torque_meas(int min, int max);
diff --git a/panda/tests/safety/test.c b/panda/tests/safety/test.c
index c61f64077c..de12a4d046 100644
--- a/panda/tests/safety/test.c
+++ b/panda/tests/safety/test.c
@@ -42,6 +42,8 @@ TIM_TypeDef *TIM2 = &timer;
 #define HW_TYPE_PEDAL 4U
 #define HW_TYPE_UNO 5U
 
+#define ALLOW_DEBUG
+
 // from main_declarations.h
 uint8_t hw_type = HW_TYPE_UNKNOWN;
 
@@ -80,6 +82,10 @@ void set_controls_allowed(bool c){
   controls_allowed = c;
 }
 
+void set_relay_malfunction(bool c){
+  relay_malfunction = c;
+}
+
 void set_long_controls_allowed(bool c){
   long_controls_allowed = c;
 }
@@ -96,6 +102,10 @@ bool get_controls_allowed(void){
   return controls_allowed;
 }
 
+bool get_relay_malfunction(void){
+  return relay_malfunction;
+}
+
 bool get_long_controls_allowed(void){
   return long_controls_allowed;
 }
@@ -116,10 +126,6 @@ void set_timer(uint32_t t){
   timer.CNT = t;
 }
 
-void set_toyota_camera_forwarded(int t){
-  toyota_camera_forwarded = t;
-}
-
 void set_toyota_torque_meas(int min, int max){
   toyota_torque_meas.min = min;
   toyota_torque_meas.max = max;
@@ -140,18 +146,6 @@ void set_hyundai_torque_driver(int min, int max){
   hyundai_torque_driver.max = max;
 }
 
-void set_hyundai_camera_bus(int t){
-  hyundai_camera_bus = t;
-}
-
-void set_hyundai_giraffe_switch_2(int t){
-  hyundai_giraffe_switch_2 = t;
-}
-
-void set_chrysler_camera_detected(int t){
-  chrysler_camera_detected = t;
-}
-
 void set_chrysler_torque_meas(int min, int max){
   chrysler_torque_meas.min = min;
   chrysler_torque_meas.max = max;
@@ -278,6 +272,7 @@ void set_honda_fwd_brake(bool c){
 void init_tests(void){
   // get HW_TYPE from env variable set in test.sh
   hw_type = atoi(getenv("HW_TYPE"));
+  safety_mode_cnt = 2U;  // avoid ignoring relay_malfunction logic
 }
 
 void init_tests_toyota(void){
diff --git a/panda/tests/safety/test_cadillac.py b/panda/tests/safety/test_cadillac.py
index f211908b7b..7806fb7b28 100644
--- a/panda/tests/safety/test_cadillac.py
+++ b/panda/tests/safety/test_cadillac.py
@@ -1,8 +1,10 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
+
 
 MAX_RATE_UP = 2
 MAX_RATE_DOWN = 5
@@ -16,6 +18,8 @@ DRIVER_TORQUE_FACTOR = 4;
 
 IPAS_OVERRIDE_THRESHOLD = 200
 
+TX_MSGS = [[0x151, 2], [0x152, 0], [0x153, 2], [0x154, 0]]
+
 def twos_comp(val, bits):
   if val >= 0:
     return val
@@ -32,59 +36,42 @@ class TestCadillacSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_CADILLAC, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_CADILLAC, 0)
     cls.safety.init_tests_cadillac()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _set_prev_torque(self, t):
     self.safety.set_cadillac_desired_torque_last(t)
     self.safety.set_cadillac_rt_torque_last(t)
 
   def _torque_driver_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x164 << 21
-
     t = twos_comp(torque, 11)
+    to_send = make_msg(0, 0x164)
     to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8)
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x151 << 21
-
+    to_send = make_msg(2, 0x151)
     t = twos_comp(torque, 14)
     to_send[0].RDLR = ((t >> 8) & 0x3F) | ((t & 0xFF) << 8)
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.set_controls_allowed(0)
+    test_manually_enable_controls_allowed(self)
 
   def test_enable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x370 << 21
+    to_push = make_msg(0, 0x370)
     to_push[0].RDLR = 0x800000
-    to_push[0].RDTR = 0
-
     self.safety.safety_rx_hook(to_push)
     self.assertTrue(self.safety.get_controls_allowed())
 
   def test_disable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x370 << 21
-    to_push[0].RDLR = 0
-    to_push[0].RDTR = 0
-
+    to_push = make_msg(0, 0x370)
     self.safety.set_controls_allowed(1)
     self.safety.safety_rx_hook(to_push)
     self.assertFalse(self.safety.get_controls_allowed())
@@ -190,7 +177,7 @@ class TestCadillacSafety(unittest.TestCase):
     for b in buss:
       for m in msgs:
         # assume len 8
-        self.assertEqual(-1, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+        self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety/test_chrysler.py b/panda/tests/safety/test_chrysler.py
index 37bffe0eaf..2c472706fe 100755
--- a/panda/tests/safety/test_chrysler.py
+++ b/panda/tests/safety/test_chrysler.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_RATE_UP = 3
 MAX_RATE_DOWN = 3
@@ -13,40 +14,17 @@ RT_INTERVAL = 250000
 
 MAX_TORQUE_ERROR = 80
 
-def twos_comp(val, bits):
-  if val >= 0:
-    return val
-  else:
-    return (2**bits) + val
-
-def sign(a):
-  if a > 0:
-    return 1
-  else:
-    return -1
-
-def swap_bytes(data_str):
-  """Accepts string with hex, returns integer with order swapped for CAN."""
-  a = int(data_str, 16)
-  return ((a & 0xff) << 24) + ((a & 0xff00) << 8) + ((a & 0x00ff0000) >> 8) + ((a & 0xff000000) >> 24)
+TX_MSGS = [[571, 0], [658, 0], [678, 0]]
 
 class TestChryslerSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_CHRYSLER, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0)
     cls.safety.init_tests_chrysler()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _button_msg(self, buttons):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 1265 << 21
+    to_send = make_msg(0, 571)
     to_send[0].RDLR = buttons
     return to_send
 
@@ -56,17 +34,21 @@ class TestChryslerSafety(unittest.TestCase):
     self.safety.set_chrysler_torque_meas(t, t)
 
   def _torque_meas_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 544 << 21
+    to_send = make_msg(0, 544)
     to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8)
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x292 << 21
+    to_send = make_msg(0, 0x292)
     to_send[0].RDLR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8)
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 0x292)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
@@ -81,22 +63,17 @@ class TestChryslerSafety(unittest.TestCase):
           self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.set_controls_allowed(0)
-    self.assertFalse(self.safety.get_controls_allowed())
+    test_manually_enable_controls_allowed(self)
 
   def test_enable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x1f4 << 21
+    to_push = make_msg(0, 0x1F4)
     to_push[0].RDLR = 0x380000
 
     self.safety.safety_rx_hook(to_push)
     self.assertTrue(self.safety.get_controls_allowed())
 
   def test_disable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x1f4 << 21
+    to_push = make_msg(0, 0x1F4)
     to_push[0].RDLR = 0
 
     self.safety.set_controls_allowed(1)
@@ -179,28 +156,30 @@ class TestChryslerSafety(unittest.TestCase):
     self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
     self.assertEqual(0, self.safety.get_chrysler_torque_meas_min())
 
+  def test_cancel_button(self):
+    CANCEL = 1
+    for b in range(0, 0xff):
+      if b == CANCEL:
+        self.assertTrue(self.safety.safety_tx_hook(self._button_msg(b)))
+      else:
+        self.assertFalse(self.safety.safety_tx_hook(self._button_msg(b)))
+
   def test_fwd_hook(self):
     buss = list(range(0x0, 0x3))
     msgs = list(range(0x1, 0x800))
-    chrysler_camera_detected = [0, 1]
-
-    for ccd in chrysler_camera_detected:
-      self.safety.set_chrysler_camera_detected(ccd)
-      blocked_msgs = [658, 678]
-      for b in buss:
-        for m in msgs:
-          if not ccd:
-            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
-          else:
-            fwd_bus = -1
-
-          # assume len 8
-          self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+
+    blocked_msgs = [658, 678]
+    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, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety/test_gm.py b/panda/tests/safety/test_gm.py
index 304dbbe939..99c15750c4 100644
--- a/panda/tests/safety/test_gm.py
+++ b/panda/tests/safety/test_gm.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_RATE_UP = 7
 MAX_RATE_DOWN = 17
@@ -17,6 +18,11 @@ RT_INTERVAL = 250000
 DRIVER_TORQUE_ALLOWANCE = 50;
 DRIVER_TORQUE_FACTOR = 4;
 
+TX_MSGS = [[384, 0], [1033, 0], [1034, 0], [715, 0], [880, 0],  # pt bus
+           [161, 1], [774, 1], [776, 1], [784, 1],  # obs bus
+           [789, 2],  # ch bus
+           [0x104c006c, 3], [0x10400060]]  # gmlan
+
 def twos_comp(val, bits):
   if val >= 0:
     return val
@@ -33,50 +39,37 @@ class TestGmSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_GM, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_GM, 0)
     cls.safety.init_tests_gm()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _speed_msg(self, speed):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 842 << 21
+    to_send = make_msg(0, 842)
     to_send[0].RDLR = speed
     return to_send
 
   def _button_msg(self, buttons):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 481 << 21
+    to_send = make_msg(0, 481)
     to_send[0].RDHR = buttons << 12
     return to_send
 
   def _brake_msg(self, brake):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 241 << 21
+    to_send = make_msg(0, 241)
     to_send[0].RDLR = 0xa00 if brake else 0x900
     return to_send
 
   def _gas_msg(self, gas):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 417 << 21
+    to_send = make_msg(0, 417)
     to_send[0].RDHR = (1 << 16) if gas else 0
     return to_send
 
   def _send_brake_msg(self, brake):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 789 << 21
+    to_send = make_msg(2, 789)
     brake = (-brake) & 0xfff
     to_send[0].RDLR = (brake >> 8) | ((brake &0xff) << 8)
     return to_send
 
   def _send_gas_msg(self, gas):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 715 << 21
+    to_send = make_msg(0, 715)
     to_send[0].RDLR = ((gas & 0x1f) << 27) | ((gas & 0xfe0) << 11)
     return to_send
 
@@ -85,21 +78,23 @@ class TestGmSafety(unittest.TestCase):
     self.safety.set_gm_rt_torque_last(t)
 
   def _torque_driver_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 388 << 21
-
     t = twos_comp(torque, 11)
+    to_send = make_msg(0, 388)
     to_send[0].RDHR = (((t >> 8) & 0x7) << 16) | ((t & 0xFF) << 24)
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 384 << 21
-
     t = twos_comp(torque, 11)
+    to_send = make_msg(0, 384)
     to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8)
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 384)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
@@ -198,10 +193,7 @@ class TestGmSafety(unittest.TestCase):
           self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.set_controls_allowed(0)
-    self.assertFalse(self.safety.get_controls_allowed())
+    test_manually_enable_controls_allowed(self)
 
   def test_non_realtime_limit_up(self):
     self.safety.set_gm_torque_driver(0, 0)
@@ -289,7 +281,7 @@ class TestGmSafety(unittest.TestCase):
     for b in buss:
       for m in msgs:
         # assume len 8
-        self.assertEqual(-1, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+        self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety/test_honda.py b/panda/tests/safety/test_honda.py
index 55a376a2e5..6ab247835a 100755
--- a/panda/tests/safety/test_honda.py
+++ b/panda/tests/safety/test_honda.py
@@ -1,94 +1,79 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_BRAKE = 255
 
 INTERCEPTOR_THRESHOLD = 328
+TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0], [0x39F, 0]]
 
 class TestHondaSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_HONDA, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_HONDA, 0)
     cls.safety.init_tests_honda()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-
-    return to_send
-
   def _speed_msg(self, speed):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x158 << 21
+    to_send = make_msg(0, 0x158)
     to_send[0].RDLR = speed
-
     return to_send
 
   def _button_msg(self, buttons, msg):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = msg << 21
-    to_send[0].RDLR = buttons << 5
     has_relay = self.safety.board_has_relay()
     honda_bosch_hardware = self.safety.get_honda_bosch_hardware()
     bus = 1 if has_relay and honda_bosch_hardware else 0
-    to_send[0].RDTR = bus << 4
-
+    to_send = make_msg(bus, msg)
+    to_send[0].RDLR = buttons << 5
     return to_send
 
   def _brake_msg(self, brake):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x17C << 21
+    to_send = make_msg(0, 0x17C)
     to_send[0].RDHR = 0x200000 if brake else 0
-
     return to_send
 
   def _alt_brake_msg(self, brake):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x1BE << 21
+    to_send = make_msg(0, 0x1BE)
     to_send[0].RDLR = 0x10 if brake else 0
-
     return to_send
 
   def _gas_msg(self, gas):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x17C << 21
+    to_send = make_msg(0, 0x17C)
     to_send[0].RDLR = 1 if gas else 0
-
     return to_send
 
   def _send_brake_msg(self, brake):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x1FA << 21
+    to_send = make_msg(0, 0x1FA)
     to_send[0].RDLR = ((brake & 0x3) << 14) | ((brake & 0x3FF) >> 2)
-
     return to_send
 
   def _send_interceptor_msg(self, gas, addr):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = 6
+    to_send = make_msg(0, addr, 6)
     gas2 = gas * 2
     to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
                       ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
-
     return to_send
 
   def _send_steer_msg(self, steer):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0xE4 << 21
+    to_send = make_msg(0, 0xE4, 6)
     to_send[0].RDLR = steer
-
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 0xE4)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
+  def test_manually_enable_controls_allowed(self):
+    test_manually_enable_controls_allowed(self)
+
   def test_resume_button(self):
     RESUME_BTN = 4
     self.safety.set_controls_allowed(0)
@@ -281,7 +266,7 @@ class TestHondaSafety(unittest.TestCase):
               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)))
+            self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
 
     self.safety.set_long_controls_allowed(True)
     self.safety.set_honda_fwd_brake(False)
diff --git a/panda/tests/safety/test_honda_bosch.py b/panda/tests/safety/test_honda_bosch.py
index 7571e1eddc..eed3622bcd 100755
--- a/panda/tests/safety/test_honda_bosch.py
+++ b/panda/tests/safety/test_honda_bosch.py
@@ -1,29 +1,31 @@
 #!/usr/bin/env python3
 import unittest
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import make_msg, test_spam_can_buses
 
 MAX_BRAKE = 255
 
+H_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]]  # Bosch Harness
+G_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]]  # Bosch Giraffe
+
+
 class TestHondaSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_HONDA_BOSCH, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH, 0)
     cls.safety.init_tests_honda()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-
-    return to_send
+  def test_spam_can_buses(self):
+    if self.safety.board_has_relay():
+      test_spam_can_buses(self, H_TX_MSGS)
+    else:
+      test_spam_can_buses(self, G_TX_MSGS)
 
   def test_fwd_hook(self):
     buss = range(0x0, 0x3)
     msgs = range(0x1, 0x800)
-    #has_relay = self.safety.get_hw_type() == 3  # black panda
     has_relay = self.safety.board_has_relay()
     bus_rdr_cam = 2 if has_relay else 1
     bus_rdr_car = 0 if has_relay else 2
@@ -40,7 +42,7 @@ class TestHondaSafety(unittest.TestCase):
           fwd_bus = bus_rdr_cam
 
         # assume len 8
-        self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+        self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety/test_hyundai.py b/panda/tests/safety/test_hyundai.py
index d8fa02691b..9067ec9842 100644
--- a/panda/tests/safety/test_hyundai.py
+++ b/panda/tests/safety/test_hyundai.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_RATE_UP = 3
 MAX_RATE_DOWN = 7
@@ -14,6 +15,8 @@ RT_INTERVAL = 250000
 DRIVER_TORQUE_ALLOWANCE = 50;
 DRIVER_TORQUE_FACTOR = 2;
 
+TX_MSGS = [[832, 0], [1265, 0]]
+
 def twos_comp(val, bits):
   if val >= 0:
     return val
@@ -30,19 +33,11 @@ class TestHyundaiSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_HYUNDAI, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0)
     cls.safety.init_tests_hyundai()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _button_msg(self, buttons):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 1265 << 21
+    to_send = make_msg(0, 1265)
     to_send[0].RDLR = buttons
     return to_send
 
@@ -51,17 +46,21 @@ class TestHyundaiSafety(unittest.TestCase):
     self.safety.set_hyundai_rt_torque_last(t)
 
   def _torque_driver_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 897 << 21
+    to_send = make_msg(0, 897)
     to_send[0].RDLR = (torque + 2048) << 11
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 832 << 21
+    to_send = make_msg(0, 832)
     to_send[0].RDLR = (torque + 1024) << 16
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 832)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
@@ -76,24 +75,16 @@ class TestHyundaiSafety(unittest.TestCase):
           self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.set_controls_allowed(0)
-    self.assertFalse(self.safety.get_controls_allowed())
+    test_manually_enable_controls_allowed(self)
 
   def test_enable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 1057 << 21
+    to_push = make_msg(0, 1057)
     to_push[0].RDLR = 1 << 13
-
     self.safety.safety_rx_hook(to_push)
     self.assertTrue(self.safety.get_controls_allowed())
 
   def test_disable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 1057 << 21
-    to_push[0].RDLR = 0
-
+    to_push = make_msg(0, 1057)
     self.safety.set_controls_allowed(1)
     self.safety.safety_rx_hook(to_push)
     self.assertFalse(self.safety.get_controls_allowed())
@@ -176,43 +167,35 @@ class TestHyundaiSafety(unittest.TestCase):
       self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
 
 
-  #def test_spam_cancel_safety_check(self):
-  #  RESUME_BTN = 1
-  #  SET_BTN = 2
-  #  CANCEL_BTN = 4
-  #  BUTTON_MSG = 1265
-  #  self.safety.set_controls_allowed(0)
-  #  self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN)))
-  #  self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
-  #  self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN)))
-  #  # do not block resume if we are engaged already
-  #  self.safety.set_controls_allowed(1)
-  #  self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
+  def test_spam_cancel_safety_check(self):
+    RESUME_BTN = 1
+    SET_BTN = 2
+    CANCEL_BTN = 4
+    self.safety.set_controls_allowed(0)
+    self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN)))
+    self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
+    self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN)))
+    # do not block resume if we are engaged already
+    self.safety.set_controls_allowed(1)
+    self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
 
   def test_fwd_hook(self):
 
     buss = list(range(0x0, 0x3))
     msgs = list(range(0x1, 0x800))
-    hyundai_giraffe_switch_2 = [0, 1]
-
-    self.safety.set_hyundai_camera_bus(2)
-    for hgs in hyundai_giraffe_switch_2:
-      self.safety.set_hyundai_giraffe_switch_2(hgs)
-      blocked_msgs = [832]
-      for b in buss:
-        for m in msgs:
-          if hgs:
-            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
-          else:
-            fwd_bus = -1
-
-          # assume len 8
-          self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+
+    blocked_msgs = [832]
+    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, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety/test_subaru.py b/panda/tests/safety/test_subaru.py
index 49933e6636..e18d7515cb 100644
--- a/panda/tests/safety/test_subaru.py
+++ b/panda/tests/safety/test_subaru.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_RATE_UP = 50
 MAX_RATE_DOWN = 70
@@ -14,6 +15,8 @@ RT_INTERVAL = 250000
 DRIVER_TORQUE_ALLOWANCE = 60;
 DRIVER_TORQUE_FACTOR = 10;
 
+TX_MSGS = [[0x122, 0], [0x164, 0], [0x221, 0], [0x322, 0]]
+
 def twos_comp(val, bits):
   if val >= 0:
     return val
@@ -30,52 +33,43 @@ class TestSubaruSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_SUBARU, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0)
     cls.safety.init_tests_subaru()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _set_prev_torque(self, t):
     self.safety.set_subaru_desired_torque_last(t)
     self.safety.set_subaru_rt_torque_last(t)
 
   def _torque_driver_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x119 << 21
-
     t = twos_comp(torque, 11)
+    to_send = make_msg(0, 0x119)
     to_send[0].RDLR = ((t & 0x7FF) << 16)
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x122 << 21
-
+    to_send = make_msg(0, 0x122)
     t = twos_comp(torque, 13)
     to_send[0].RDLR = (t << 16)
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 0x122)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
   def test_enable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x240 << 21
+    to_push = make_msg(0, 0x240)
     to_push[0].RDHR = 1 << 9
-
     self.safety.safety_rx_hook(to_push)
     self.assertTrue(self.safety.get_controls_allowed())
 
   def test_disable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x240 << 21
+    to_push = make_msg(0, 0x240)
     to_push[0].RDHR = 0
-
     self.safety.set_controls_allowed(1)
     self.safety.safety_rx_hook(to_push)
     self.assertFalse(self.safety.get_controls_allowed())
@@ -91,10 +85,7 @@ class TestSubaruSafety(unittest.TestCase):
           self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.set_controls_allowed(0)
-    self.assertFalse(self.safety.get_controls_allowed())
+    test_manually_enable_controls_allowed(self)
 
   def test_non_realtime_limit_up(self):
     self.safety.set_subaru_torque_driver(0, 0)
@@ -188,7 +179,7 @@ class TestSubaruSafety(unittest.TestCase):
           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)))
+        self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety/test_toyota.py b/panda/tests/safety/test_toyota.py
index 15d3b20cc5..58e32f036e 100644
--- a/panda/tests/safety/test_toyota.py
+++ b/panda/tests/safety/test_toyota.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_RATE_UP = 10
 MAX_RATE_DOWN = 25
@@ -15,9 +16,14 @@ MAX_RT_DELTA = 375
 RT_INTERVAL = 250000
 
 MAX_TORQUE_ERROR = 350
-
 INTERCEPTOR_THRESHOLD = 475
 
+TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0],  # DSU bus 0
+           [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1],  # DSU bus 1
+           [0x2E4, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0],  # LKAS + ACC
+           [0x200, 0]];  # interceptor
+
+
 def twos_comp(val, bits):
   if val >= 0:
     return val
@@ -34,75 +40,60 @@ class TestToyotaSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_TOYOTA, 100)
+    cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 100)
     cls.safety.init_tests_toyota()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _set_prev_torque(self, t):
     self.safety.set_toyota_desired_torque_last(t)
     self.safety.set_toyota_rt_torque_last(t)
     self.safety.set_toyota_torque_meas(t, t)
 
   def _torque_meas_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x260 << 21
-
     t = twos_comp(torque, 16)
+    to_send = make_msg(0, 0x260)
     to_send[0].RDHR = t | ((t & 0xFF) << 16)
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x2E4 << 21
-
     t = twos_comp(torque, 16)
+    to_send = make_msg(0, 0x2E4)
     to_send[0].RDLR = t | ((t & 0xFF) << 16)
     return to_send
 
   def _accel_msg(self, accel):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x343 << 21
-
+    to_send = make_msg(0, 0x343)
     a = twos_comp(accel, 16)
     to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8)
     return to_send
 
   def _send_gas_msg(self, gas):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x2C1 << 21
+    to_send = make_msg(0, 0x2C1)
     to_send[0].RDHR = (gas & 0xFF) << 16
-
     return to_send
 
   def _send_interceptor_msg(self, gas, addr):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = 6
     gas2 = gas * 2
+    to_send = make_msg(0, addr, 6)
     to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
                       ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
-
     return to_send
 
   def _pcm_cruise_msg(self, cruise_on):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x1D2 << 21
+    to_send = make_msg(0, 0x1D2)
     to_send[0].RDLR = cruise_on << 5
-
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 0x2E4)
+
   def test_default_controls_not_allowed(self):
     self.assertFalse(self.safety.get_controls_allowed())
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
+    test_manually_enable_controls_allowed(self)
 
   def test_enable_control_allowed_from_cruise(self):
     self.safety.safety_rx_hook(self._pcm_cruise_msg(False))
@@ -285,29 +276,23 @@ class TestToyotaSafety(unittest.TestCase):
     buss = list(range(0x0, 0x3))
     msgs = list(range(0x1, 0x800))
     long_controls_allowed = [0, 1]
-    toyota_camera_forwarded = [0, 1]
-
-    for tcf in toyota_camera_forwarded:
-      self.safety.set_toyota_camera_forwarded(tcf)
-      for lca in long_controls_allowed:
-        self.safety.set_long_controls_allowed(lca)
-        blocked_msgs = [0x2E4, 0x412, 0x191]
-        if lca:
-          blocked_msgs += [0x343]
-        for b in buss:
-          for m in msgs:
-            if tcf:
-              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
-            else:
-              fwd_bus = -1
-
-            # assume len 8
-            self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+
+    for lca in long_controls_allowed:
+      self.safety.set_long_controls_allowed(lca)
+      blocked_msgs = [0x2E4, 0x412, 0x191]
+      if lca:
+        blocked_msgs += [0x343]
+      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, make_msg(b, m, 8)))
 
     self.safety.set_long_controls_allowed(True)
 
diff --git a/panda/tests/safety/test_toyota_ipas.py b/panda/tests/safety/test_toyota_ipas.py
index df0f36ffd7..680fc149b7 100644
--- a/panda/tests/safety/test_toyota_ipas.py
+++ b/panda/tests/safety/test_toyota_ipas.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests. safety.common import make_msg
 
 IPAS_OVERRIDE_THRESHOLD = 200
 
@@ -26,13 +27,11 @@ class TestToyotaSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_TOYOTA_IPAS, 66)
+    cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA_IPAS, 66)
     cls.safety.init_tests_toyota()
 
   def _torque_driver_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x260 << 21
-
+    to_send = make_msg(0, 0x260)
     t = twos_comp(torque, 16)
     to_send[0].RDLR = t | ((t & 0xFF) << 16)
     return to_send
@@ -42,9 +41,7 @@ class TestToyotaSafety(unittest.TestCase):
       self.safety.safety_rx_hook(self._torque_driver_msg(torque))
 
   def _angle_meas_msg(self, angle):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x25 << 21
-
+    to_send = make_msg(0, 0x25)
     t = twos_comp(angle, 12)
     to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
     return to_send
@@ -54,17 +51,13 @@ class TestToyotaSafety(unittest.TestCase):
       self.safety.safety_rx_hook(self._angle_meas_msg(angle))
 
   def _ipas_state_msg(self, state):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x262 << 21
-
+    to_send = make_msg(0, 0x262)
     to_send[0].RDLR = state & 0xF
     return to_send
 
   def _ipas_control_msg(self, angle, state):
     # note: we command 2/3 of the angle due to CAN conversion
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x266 << 21
-
+    to_send = make_msg(0, 0x266)
     t = twos_comp(angle, 12)
     to_send[0].RDLR = ((t & 0xF00) >> 8) | ((t & 0xFF) << 8)
     to_send[0].RDLR |= ((state & 0xf) << 4)
@@ -72,8 +65,7 @@ class TestToyotaSafety(unittest.TestCase):
     return to_send
 
   def _speed_msg(self, speed):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0xb4 << 21
+    to_send = make_msg(0, 0xB4)
     speed = int(speed * 100 * 3.6)
 
     to_send[0].RDHR = ((speed & 0xFF) << 16) | (speed & 0xFF00)
diff --git a/panda/tests/safety/test_volkswagen.py b/panda/tests/safety/test_volkswagen.py
index aa535cdac9..99d0916e46 100644
--- a/panda/tests/safety/test_volkswagen.py
+++ b/panda/tests/safety/test_volkswagen.py
@@ -1,8 +1,9 @@
 #!/usr/bin/env python3
 import unittest
 import numpy as np
-import libpandasafety_py  # pylint: disable=import-error
 from panda import Panda
+from panda.tests.safety import libpandasafety_py
+from panda.tests.safety.common import test_relay_malfunction, make_msg, test_manually_enable_controls_allowed, test_spam_can_buses
 
 MAX_RATE_UP = 4
 MAX_RATE_DOWN = 10
@@ -14,6 +15,8 @@ RT_INTERVAL = 250000
 DRIVER_TORQUE_ALLOWANCE = 80
 DRIVER_TORQUE_FACTOR = 3
 
+TX_MSGS = [[0x126, 0], [0x12B, 0], [0x12B, 2], [0x397, 0]]
+
 def sign(a):
   if a > 0:
     return 1
@@ -24,24 +27,15 @@ class TestVolkswagenSafety(unittest.TestCase):
   @classmethod
   def setUp(cls):
     cls.safety = libpandasafety_py.libpandasafety
-    cls.safety.safety_set_mode(Panda.SAFETY_VOLKSWAGEN, 0)
+    cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN, 0)
     cls.safety.init_tests_volkswagen()
 
-  def _send_msg(self, bus, addr, length):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = addr << 21
-    to_send[0].RDTR = length
-    to_send[0].RDTR = bus << 4
-    return to_send
-
   def _set_prev_torque(self, t):
     self.safety.set_volkswagen_desired_torque_last(t)
     self.safety.set_volkswagen_rt_torque_last(t)
 
   def _torque_driver_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x9F << 21
-
+    to_send = make_msg(0, 0x9F)
     t = abs(torque)
     to_send[0].RDHR = ((t & 0x1FFF) << 8)
     if torque < 0:
@@ -49,9 +43,7 @@ class TestVolkswagenSafety(unittest.TestCase):
     return to_send
 
   def _torque_msg(self, torque):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x126 << 21
-
+    to_send = make_msg(0, 0x126)
     t = abs(torque)
     to_send[0].RDLR = (t & 0xFFF) << 16
     if torque < 0:
@@ -59,20 +51,21 @@ class TestVolkswagenSafety(unittest.TestCase):
     return to_send
 
   def _gas_msg(self, gas):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x121 << 21
+    to_send = make_msg(0, 0x121)
     to_send[0].RDLR = (gas & 0xFF) << 12
-
     return to_send
 
   def _button_msg(self, bit):
-    to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_send[0].RIR = 0x12B << 21
+    to_send = make_msg(2, 0x12B)
     to_send[0].RDLR = 1 << bit
-    to_send[0].RDTR = 2 << 4
-
     return to_send
 
+  def test_spam_can_buses(self):
+    test_spam_can_buses(self, TX_MSGS)
+
+  def test_relay_malfunction(self):
+    test_relay_malfunction(self, 0x126)
+
   def test_prev_gas(self):
     for g in range(0, 256):
       self.safety.safety_rx_hook(self._gas_msg(g))
@@ -82,18 +75,13 @@ class TestVolkswagenSafety(unittest.TestCase):
     self.assertFalse(self.safety.get_controls_allowed())
 
   def test_enable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x122 << 21
+    to_push = make_msg(0, 0x122)
     to_push[0].RDHR = 0x30000000
-
     self.safety.safety_rx_hook(to_push)
     self.assertTrue(self.safety.get_controls_allowed())
 
   def test_disable_control_allowed_from_cruise(self):
-    to_push = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-    to_push[0].RIR = 0x122 << 21
-    to_push[0].RDHR = 0
-
+    to_push = make_msg(0, 0x122)
     self.safety.set_controls_allowed(1)
     self.safety.safety_rx_hook(to_push)
     self.assertFalse(self.safety.get_controls_allowed())
@@ -130,10 +118,7 @@ class TestVolkswagenSafety(unittest.TestCase):
           self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t)))
 
   def test_manually_enable_controls_allowed(self):
-    self.safety.set_controls_allowed(1)
-    self.assertTrue(self.safety.get_controls_allowed())
-    self.safety.set_controls_allowed(0)
-    self.assertFalse(self.safety.get_controls_allowed())
+    test_manually_enable_controls_allowed(self)
 
   def test_spam_cancel_safety_check(self):
     BIT_CANCEL = 13
@@ -226,10 +211,10 @@ class TestVolkswagenSafety(unittest.TestCase):
 
 
   def test_fwd_hook(self):
-    buss = list(range(0x0, 0x2))
+    buss = list(range(0x0, 0x3))
     msgs = list(range(0x1, 0x800))
     blocked_msgs_0to2 = []
-    blocked_msgs_2to0 = [0x122, 0x397]
+    blocked_msgs_2to0 = [0x126, 0x397]
     for b in buss:
       for m in msgs:
         if b == 0:
@@ -240,7 +225,7 @@ class TestVolkswagenSafety(unittest.TestCase):
           fwd_bus = -1 if m in blocked_msgs_2to0 else 0
 
         # assume len 8
-        self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, self._send_msg(b, m, 8)))
+        self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
 
 
 if __name__ == "__main__":
diff --git a/panda/tests/safety_replay/helpers.py b/panda/tests/safety_replay/helpers.py
index 6f4e63c325..8fa1d3a1e9 100644
--- a/panda/tests/safety_replay/helpers.py
+++ b/panda/tests/safety_replay/helpers.py
@@ -59,11 +59,13 @@ def set_desired_torque_last(safety, mode, torque):
     safety.set_subaru_desired_torque_last(torque)
 
 def package_can_msg(msg):
-  addr_shift = 3 if msg.address >= 0x800 else 21
   rdlr, rdhr = struct.unpack('II', msg.dat.ljust(8, b'\x00'))
 
   ret = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *')
-  ret[0].RIR = msg.address << addr_shift
+  if msg.address >= 0x800:
+    ret[0].RIR = (msg.address << 3) | 5
+  else:
+    ret[0].RIR = (msg.address << 21) | 1
   ret[0].RDTR = len(msg.dat) | ((msg.src & 0xF) << 4)
   ret[0].RDHR = rdhr
   ret[0].RDLR = rdlr
diff --git a/panda/tests/safety_replay/replay_drive.py b/panda/tests/safety_replay/replay_drive.py
index 3db57dfd16..09c677cacb 100755
--- a/panda/tests/safety_replay/replay_drive.py
+++ b/panda/tests/safety_replay/replay_drive.py
@@ -2,7 +2,7 @@
 
 import os
 import sys
-import panda.tests.safety.libpandasafety_py as libpandasafety_py
+from panda.tests.safety import libpandasafety_py
 from panda.tests.safety_replay.helpers import package_can_msg, init_segment
 from tools.lib.logreader import LogReader  # pylint: disable=import-error
 
@@ -10,7 +10,7 @@ from tools.lib.logreader import LogReader  # pylint: disable=import-error
 def replay_drive(lr, safety_mode, param):
   safety = libpandasafety_py.libpandasafety
 
-  err = safety.safety_set_mode(safety_mode, param)
+  err = safety.set_safety_hooks(safety_mode, param)
   assert err == 0, "invalid safety mode: %d" % safety_mode
 
   if "SEGMENT" in os.environ:
@@ -35,7 +35,7 @@ def replay_drive(lr, safety_mode, param):
           blocked_addrs.add(canmsg.address)
 
           if "DEBUG" in os.environ:
-            print("blocked %d at %f" % (canmsg.address, (msg.logMonoTime - start_t)/(1e9)))
+            print("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t)/(1e9)))
         tx_controls += safety.get_controls_allowed()
         tx_tot += 1
     elif msg.which() == 'can':
diff --git a/panda/tests/safety_replay/test_safety_replay.py b/panda/tests/safety_replay/test_safety_replay.py
index cb38a94edf..b4278351fe 100755
--- a/panda/tests/safety_replay/test_safety_replay.py
+++ b/panda/tests/safety_replay/test_safety_replay.py
@@ -16,8 +16,9 @@ logs = [
   ("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", Panda.SAFETY_GM, 0), # GM.VOLT
   ("0375fdf7b1ce594d|2019-05-21--20-10-33.bz2", Panda.SAFETY_HONDA_BOSCH, 1), # HONDA.ACCORD
   ("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SANTA_FE
-  ("03efb1fda29e30fe|2019-02-21--18-03-45.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID
+  ("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID
   ("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, 0), # SUBARU.IMPREZA
+  ("b0c9d2329ad1606b|2019-11-17--17-06-13.bz2", Panda.SAFETY_VOLKSWAGEN, 0), # VOLKSWAGEN.GOLF
 ]
 
 if __name__ == "__main__":