diff --git a/board/can.c b/board/can.c
index 74c2ca0d..129f9af8 100644
--- a/board/can.c
+++ b/board/can.c
@@ -3,52 +3,51 @@
 #include "can.h"
 #include "uart.h"
 #include "gpio.h"
+#include "llgpio.h"
 #include "libc.h"
+#include "rev.h"
 
 // assign CAN numbering
 #ifdef PANDA
-// panda:       CAN1 = 0   CAN2 = 1   CAN3 = 2
+// panda: CAN1 = 0   CAN2 = 1   CAN3 = 2
   can_port_desc can_ports[] = {
-    {.CAN=CAN1,
-     .forwarding=-1,
-     .bitrate=CAN_DEFAULT_BITRATE,
-     .gmlan_support=false,
-     .gmlan=false,
-     .safety_mode=0,
-     .pin={GPIOC, 1, 0}},
-    {.CAN=CAN2,
-     .forwarding=-1,
-     .bitrate=CAN_DEFAULT_BITRATE,
-     .gmlan_support=true,
-     .gmlan=false,
-     .safety_mode=0,
-     .pin={GPIOC, 13, 0}},
+    {CAN_PORT_DESC_INITIALIZER,
+     .CAN = CAN1,
+     .can_pins = {{GPIOB, 8, GPIO_AF8_CAN1}, {GPIOB, 9, GPIO_AF8_CAN1}},
+     .enable_pin = {GPIOC, 1, 0},
+     .gmlan_support = false,
+    },
+    {CAN_PORT_DESC_INITIALIZER,
+     .CAN = CAN2,
+     .can_pins = {{GPIOB, 5, GPIO_AF9_CAN2}, {GPIOB, 6, GPIO_AF9_CAN2}},
+     .enable_pin = {GPIOC, 13, 0},
+     .gmlan_support = true,
+     .gmlan_pins = {{GPIOB, 12, GPIO_AF9_CAN2}, {GPIOB, 13, GPIO_AF9_CAN2}},
+    },
     //TODO Make gmlan support correct for REV B
-    {.CAN=CAN3,
-     .forwarding=-1,
-     .bitrate=CAN_DEFAULT_BITRATE,
-     .gmlan_support=true,
-     .gmlan=false,
-     .safety_mode=0,
-     .pin={GPIOA, 0, 0}},
+    {CAN_PORT_DESC_INITIALIZER,
+     .CAN = CAN3,
+     .can_pins = {{GPIOA, 8, GPIO_AF11_CAN3}, {GPIOA, 15, GPIO_AF11_CAN3}},
+     .enable_pin = {GPIOA, 0, 0},
+     .gmlan_support = true,
+     .gmlan_pins = {{GPIOB, 3, GPIO_AF11_CAN3}, {GPIOB, 4, GPIO_AF11_CAN3}},
+    }
   };
 #else
-// old:         CAN1 = 1   CAN2 = 0
+// old:   CAN1 = 1   CAN2 = 0
   can_port_desc can_ports[] = {
-    {.CAN=CAN2,
-     .forwarding=-1,
-     .bitrate=CAN_DEFAULT_BITRATE,
-     .gmlan_support=false,
-     .gmlan=false,
-     .safety_mode=0,
-     .pin={GPIOB, 3, 1}},
-    {.CAN=CAN1,
-     .forwarding=-1,
-     .bitrate=CAN_DEFAULT_BITRATE,
-     .gmlan_support=false,
-     .gmlan=false,
-     .safety_mode=0,
-     .pin={GPIOB, 4, 1}},
+    {CAN_PORT_DESC_INITIALIZER,
+     .CAN = CAN2,
+     .can_pins = {{GPIOB, 5, GPIO_AF9_CAN2}, {GPIOB, 6, GPIO_AF9_CAN2}},
+     .enable_pin = {GPIOB, 4, 1},
+     .gmlan_support = false,
+    },
+    {CAN_PORT_DESC_INITIALIZER,
+     .CAN = CAN1,
+     .can_pins = {{GPIOB, 8, GPIO_AF9_CAN1}, {GPIOB, 9, GPIO_AF9_CAN1}},
+     .enable_pin = {GPIOB, 3, 1},
+     .gmlan_support = false,
+    }
   };
 #endif
 
@@ -80,24 +79,75 @@ int push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
 // ********************* CAN Functions *********************
 
 void can_init(uint8_t canid) {
+  int i;
   uint32_t bitrate;
   CAN_TypeDef *CAN;
   uint8_t quanta;
   uint16_t prescaler;
   uint8_t seq1, seq2;
 
+  can_port_desc *port;
+  gpio_alt_setting *disable_pins;
+  gpio_alt_setting *enable_pins;
+
   puts("Can init: ");
   puth(canid);
   puts("\n");
 
   if(canid >= CAN_MAX) return;
+  port = &can_ports[canid];
 
-  bitrate = can_ports[canid].bitrate;
-  CAN = can_ports[canid].CAN;
+  //////////// Set MCU pin modes
+  if (port->gmlan_support) {
+    disable_pins = port->gmlan ? port->can_pins : port->gmlan_pins;
+    enable_pins =  port->gmlan ? port->gmlan_pins : port->can_pins;
+  } else {
+    disable_pins = 0;
+    enable_pins =  port->can_pins;
+  }
 
-  //MAX 1 Megabaud
-  if(bitrate > 1000000)
-    bitrate = 1000000;
+  // Disable output on either CAN or GMLAN pins
+  if (disable_pins) {
+    set_gpio_mode(disable_pins[0].port, disable_pins[0].num, MODE_INPUT);
+    set_gpio_mode(disable_pins[1].port, disable_pins[1].num, MODE_INPUT);
+  }
+
+  // Enable output on either CAN or GMLAN pins
+  if (enable_pins) {
+    set_gpio_alternate(enable_pins[0].port, enable_pins[0].num, enable_pins[0].setting);
+    set_gpio_alternate(enable_pins[1].port, enable_pins[1].num, enable_pins[1].setting);
+  }
+
+  /* GMLAN mode pins:
+     M0(B15)  M1(B14)  mode
+     =======================
+     0        0        sleep
+     1        0        100kbit
+     0        1        high voltage wakeup
+     1        1        33kbit (normal)
+  */
+  if (port->gmlan) {
+    set_gpio_output(GPIOB, 14, 1);
+    set_gpio_output(GPIOB, 15, 1);
+  } else {
+    for (i = 0; i < CAN_MAX; i++)
+      if (can_ports[i].gmlan)
+	break;
+    if (i == CAN_MAX){
+      set_gpio_output(GPIOB, 14, 0);
+      set_gpio_output(GPIOB, 15, 0);
+      puts("Disabling GMLAN output\n");
+    }
+  }
+
+  //////////// Calculate and set CAN bitrate
+  if (port->gmlan) {
+    bitrate = (port->gmlan_bitrate) < 58333 ? 33333 : 83333;
+  } else {
+    bitrate = port->bitrate;
+    if(bitrate > 1000000) //MAX 1 Megabaud
+      bitrate = 1000000;
+  }
 
   //puts("  Speed req: ");
   //puth(bitrate);
@@ -121,18 +171,33 @@ void can_init(uint8_t canid) {
   if(prescaler > 0x3FF)
     prescaler = 0x3FF;
 
-  can_ports[canid].bitrate = FREQ/quanta/prescaler;
+  bitrate = FREQ/quanta/prescaler;
+  if (port->gmlan) {
+    port->gmlan_bitrate = bitrate;
+  } else {
+    port->bitrate = bitrate;
+  }
 
   puts("  Speed: ");
-  puth(can_ports[canid].bitrate);
+  puth(bitrate);
   puts("\n");
+
+  if (port->gmlan)
+    puts("  Type GMLAN\n");
+  else
+    puts("  Type CAN\n");
+
+  //////////////// Enable CAN port
   if (controls_allowed)
     puts("  Output Enabled\n");
   else
     puts("  Output Disabled\n");
 
+  //TODO: Eddie, check if disabling output causes correct behavior.
+  //set_can_enable(canid, controls_allowed);
   set_can_enable(canid, 1);
 
+  CAN = port->CAN;
   // Move CAN to initialization mode and sync.
   CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
   while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
@@ -181,6 +246,22 @@ void can_init(uint8_t canid) {
   //CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_FMPIE1;
 }
 
+void set_can_mode(int canid, int use_gmlan) {
+  int i;
+
+  if (canid >= CAN_MAX) return;
+
+  if (use_gmlan)
+    for (i = 0; i < CAN_MAX; i++)
+      if (can_ports[i].gmlan)
+	set_can_mode(i, 0);
+
+  if (!can_ports[canid].gmlan_support) use_gmlan = 0;
+  can_ports[canid].gmlan = use_gmlan;
+
+  can_init(canid);
+}
+
 // CAN error
 void can_sce(CAN_TypeDef *CAN) {
   #ifdef DEBUG
diff --git a/board/can.h b/board/can.h
index cb335257..65dfaf3d 100644
--- a/board/can.h
+++ b/board/can.h
@@ -4,20 +4,37 @@
 #include <stdbool.h>
 #define CAN_TIMEOUT 1000000
 
+#define CAN_PORT_DESC_INITIALIZER               \
+  .forwarding=-1,				\
+  .bitrate=CAN_DEFAULT_BITRATE,                 \
+  .bitrate=CAN_DEFAULT_BITRATE,                 \
+  .gmlan=false,                                 \
+  .gmlan_bitrate=GMLAN_DEFAULT_BITRATE,         \
+  .safety_mode=0
+
 typedef struct {
   GPIO_TypeDef* port;
   uint8_t num;
   bool high_val;
 } gpio_pin;
 
+typedef struct {
+  GPIO_TypeDef* port;
+  uint8_t num;
+  uint32_t setting;
+} gpio_alt_setting;
+
 typedef struct {
   CAN_TypeDef *CAN;
   int8_t forwarding;
   uint32_t bitrate;
   bool gmlan;
   bool gmlan_support;
+  uint32_t gmlan_bitrate;
   uint8_t safety_mode;
-  gpio_pin pin;
+  gpio_pin enable_pin;
+  gpio_alt_setting can_pins[2];
+  gpio_alt_setting gmlan_pins[2];
 } can_port_desc;
 
 extern can_port_desc can_ports[];
@@ -51,6 +68,8 @@ int push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
 
 void can_init(uint8_t canid);
 
+void set_can_mode(int can, int use_gmlan);
+
 // CAN error
 void can_sce(CAN_TypeDef *CAN);
 
diff --git a/board/config.h b/board/config.h
index 58738ad8..5208a84b 100644
--- a/board/config.h
+++ b/board/config.h
@@ -1,48 +1,22 @@
 #ifndef PANDA_CONFIG_H
 #define PANDA_CONFIG_H
 
+#include "rev.h"
+
 //#define DEBUG
 //#define DEBUG_USB
 //#define CAN_LOOPBACK_MODE
 
-#ifdef STM32F4
-  #define PANDA
-  #include "stm32f4xx.h"
-#else
-  #include "stm32f2xx.h"
-#endif
-
-#ifdef PANDA
-  #define ENABLE_CURRENT_SENSOR
-  #define ENABLE_SPI
-#endif
-
-#ifdef PANDA
-  #define LED_RED 9
-  #define LED_GREEN 7
-  #define LED_BLUE 6
-#else
-  #define LED_RED 10
-  #define LED_GREEN 11
-  #define LED_BLUE -1
-#endif
-
 #define USB_VID 0xbbaa
 #define USB_PID 0xddcc
 
-#define FREQ 24000000
-
-// 500 khz
-#define CAN_DEFAULT_BITRATE 500000
-#define GMLAN_DEFAULT_BITRATE 33333
+#define CAN_DEFAULT_BITRATE 500000 // 500 khz
+#define GMLAN_DEFAULT_BITRATE 33333 // 33.333 khz
 
 #define FIFO_SIZE 0x100
 
 #define NULL ((void*)0)
 
-#define PANDA_REV_AB 0
-#define PANDA_REV_C 1
-
 #define PANDA_SAFETY
 
 #endif
diff --git a/board/early.c b/board/early.c
index 30bd7b80..65021d39 100644
--- a/board/early.c
+++ b/board/early.c
@@ -2,10 +2,11 @@
 #include "early.h"
 #include "llgpio.h"
 #include "uart.h"
+#include "rev.h"
 
 int has_external_debug_serial = 0;
 int is_giant_panda = 0;
-int revision = PANDA_REV_AB;
+enum rev revision = REV_A;
 void *g_pfnVectors;
 
 // must call again from main because BSS is zeroed
@@ -33,7 +34,9 @@ void detect() {
   set_gpio_pullup(GPIOA, 13, PULL_DOWN);
   for (i=0;i<PULL_EFFECTIVE_DELAY;i++);
   if(get_gpio_input(GPIOA, 13))
-    revision = PANDA_REV_C;
+    revision = REV_C;
+  else
+    revision = REV_B;
 
   // RESET pull up/down
   set_gpio_pullup(GPIOA, 13, PULL_NONE);
diff --git a/board/early.h b/board/early.h
index 5c445251..6727f201 100644
--- a/board/early.h
+++ b/board/early.h
@@ -1,3 +1,8 @@
+#ifndef PANDA_EARLY_H
+#define PANDA_EARLY_H
+
+#include "rev.h"
+
 #define ENTER_BOOTLOADER_MAGIC 0xdeadbeef
 #define POST_BOOTLOADER_MAGIC 0xdeadb111
 #define PULL_EFFECTIVE_DELAY 10
@@ -7,8 +12,10 @@ extern void *_app_start[];
 extern void *g_pfnVectors;
 extern int has_external_debug_serial;
 extern int is_giant_panda;
-extern int revision;
+extern enum rev revision;
 
 void spi_flasher();
 void detect();
 void early();
+
+#endif
diff --git a/board/gpio.c b/board/gpio.c
index 5aa00b84..a3b9c653 100644
--- a/board/gpio.c
+++ b/board/gpio.c
@@ -1,25 +1,9 @@
-#ifdef STM32F4
-  #include "stm32f4xx_hal_gpio_ex.h"
-#else
-  #include "stm32f2xx_hal_gpio_ex.h"
-#endif
-
 #include "config.h"
 #include "gpio.h"
 #include "llgpio.h"
 #include "early.h"
 #include "can.h"
-
-void set_can_enable(uint8_t canid, int enabled) {
-  // enable CAN busses
-  gpio_pin *pin = &can_ports[canid].pin;
-
-  //enable,   high_val = 1;  1 xnor 1 = 1
-  //enable,  ~high_val = 0;  1 xnor 0 = 0
-  //~enable,  high_val = 0;  0 xnor 1 = 0
-  //~enable, ~high_val = 1;  0 xnor 0 = 1
-  set_gpio_output(pin->port, pin->num, !(enabled ^ pin->high_val));
-}
+#include "rev.h"
 
 void set_led(int led_num, int on) {
   if (led_num == -1) return;
@@ -31,6 +15,20 @@ void set_led(int led_num, int on) {
   #endif
 }
 
+/* In REVC, this should always be enabled under normal
+ * operation. Otherwise, the CAN transceiver chip will try to pull the
+ * pus lines to ground and anger other CAN devices. Eddie is looking
+ * into if this is useful or not. */
+void set_can_enable(uint8_t canid, int enabled) {
+  // enable CAN busses
+  gpio_pin *pin = &can_ports[canid].enable_pin;
+
+  //enable,   high_val = 1;  1 xnor 1 = 1
+  //enable,  ~high_val = 0;  1 xnor 0 = 0
+  //~enable,  high_val = 0;  0 xnor 1 = 0
+  //~enable, ~high_val = 1;  0 xnor 0 = 1
+  set_gpio_output(pin->port, pin->num, !(enabled ^ pin->high_val));
+}
 
 // TODO: does this belong here?
 void periph_init() {
@@ -64,77 +62,6 @@ void periph_init() {
   RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
 }
 
-void set_can_mode(int canid, int use_gmlan) {
-  if(canid >= CAN_MAX) return;
-
-  if(!can_ports[canid].gmlan_support) return;
-  can_ports[canid].gmlan = use_gmlan;
-
-  /* GMLAN mode pins:
-     M0(B15)  M1(B14)  mode
-     =======================
-     0        0        sleep
-     1        0        100kbit
-     0        1        high voltage wakeup
-     1        1        33kbit (normal)
-  */
-
-  // connects to CAN2 xcvr or GMLAN xcvr
-  if (use_gmlan) {
-    if (canid == 1) {
-      // B5,B6: disable normal mode
-      set_gpio_mode(GPIOB, 5, MODE_INPUT);
-      set_gpio_mode(GPIOB, 6, MODE_INPUT);
-
-      // B12,B13: gmlan mode
-      set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2);
-      set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2);
-
-    } else if (revision == PANDA_REV_C && canid == 2) {
-      #ifdef CAN3
-      // A8,A15: disable normal mode
-      set_gpio_mode(GPIOA, 8, MODE_INPUT);
-      set_gpio_mode(GPIOA, 15, MODE_INPUT);
-
-      // B3,B4: enable gmlan mode
-      set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3);
-      set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3);
-      #endif
-    }
-
-    can_ports[canid].bitrate = GMLAN_DEFAULT_BITRATE;
-  } else {
-    if (canid == 1) {
-      // B12,B13: disable gmlan mode
-      set_gpio_mode(GPIOB, 12, MODE_INPUT);
-      set_gpio_mode(GPIOB, 13, MODE_INPUT);
-
-      // B5,B6: normal mode
-      set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2);
-      set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2);
-    } else if (canid == 2) {
-      #ifdef CAN3
-      if(revision == PANDA_REV_C){
-        // B3,B4: disable gmlan mode
-        set_gpio_mode(GPIOB, 3, MODE_INPUT);
-        set_gpio_mode(GPIOB, 4, MODE_INPUT);
-      }
-
-      // A8,A15: normal mode
-      set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3);
-      set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3);
-      #endif
-    }
-
-    can_ports[canid].bitrate = CAN_DEFAULT_BITRATE;
-  }
-
-  set_gpio_output(GPIOB, 14, use_gmlan);
-  set_gpio_output(GPIOB, 15, use_gmlan);
-
-  can_init(canid);
-}
-
 // board specific
 void gpio_init() {
   // pull low to hold ESP in reset??
@@ -185,29 +112,9 @@ void gpio_init() {
   set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
 #endif
 
-  // B8,B9: CAN 1
-  set_can_enable(0, 0);
-#ifdef STM32F4
-  set_gpio_alternate(GPIOB, 8, GPIO_AF8_CAN1);
-  set_gpio_alternate(GPIOB, 9, GPIO_AF8_CAN1);
-#else
-  set_gpio_alternate(GPIOB, 8, GPIO_AF9_CAN1);
-  set_gpio_alternate(GPIOB, 9, GPIO_AF9_CAN1);
-#endif
-
-  // B5,B6: CAN 2
-  set_can_enable(1, 0);
-  set_can_mode(1, 0);
-
-  // A8,A15: CAN3
-  #ifdef CAN3
-    set_can_enable(2, 0);
-    set_can_mode(2, 0);
-  #endif
-
   #ifdef PANDA
     // K-line enable moved from B4->B7 to make room for GMLAN on CAN3
-    if(revision == PANDA_REV_C)
+    if(revision == REV_C)
       set_gpio_output(GPIOB, 7, 1); // REV C
     else
       set_gpio_output(GPIOB, 4, 1); // REV AB
@@ -226,7 +133,7 @@ void gpio_init() {
     set_gpio_pullup(GPIOC, 11, PULL_UP);
   #endif
 
-  if(revision == PANDA_REV_C) {
+  if(revision == REV_C) {
     // B2,A13: set DCP mode on the charger (breaks USB!)
     //set_gpio_output(GPIOB, 2, 0);
     //set_gpio_output(GPIOA, 13, 0);
diff --git a/board/gpio.h b/board/gpio.h
index 2c30a0d1..6398fe61 100644
--- a/board/gpio.h
+++ b/board/gpio.h
@@ -5,7 +5,5 @@ void set_led(int led_num, int on);
 // TODO: does this belong here?
 void periph_init();
 
-void set_can_mode(int can, int use_gmlan);
-
 // board specific
 void gpio_init();
diff --git a/board/main.c b/board/main.c
index 4358a5f2..f8764a5e 100644
--- a/board/main.c
+++ b/board/main.c
@@ -361,7 +361,10 @@ void usb_cb_ep0_out(uint8_t *usbdata, int len, int hardwired) {
     uint32_t bitrate = *(int*)usbdata;
     uint16_t canb_id = setup.b.wValue.w;
 
-    can_ports[canb_id].bitrate = bitrate;
+    if (can_ports[canb_id].gmlan)
+      can_ports[canb_id].gmlan_bitrate = bitrate;
+    else
+      can_ports[canb_id].bitrate = bitrate;
     can_init(canb_id);
   }
 }
@@ -513,6 +516,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
       break;
     case 0xdb: // toggle GMLAN
       puts("Toggle GMLAN canid: ");
+      puth(setup->b.wValue.w);
 
       uint16_t canid = setup->b.wValue.w;
       bool gmlan_enable = setup->b.wIndex.w;
@@ -523,42 +527,39 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
       puts("\n");
 
       if (canid >= CAN_MAX) {
-        puts("  Out of range!\n");
+        puts(" !!Out of range!!\n");
         return -1;
       }
 
       can_port_desc *port = &can_ports[canid];
 
+      //puts(" gmlan_support ");
+      //puth(port->gmlan_support);
+      //puts(" mode ");
+      //puth(gmlan_enable);
+      //puts("\n");
+
       //Fail if canid doesn't support gmlan
-      if (!port->gmlan_support)
+      if (!port->gmlan_support) {
+        puts(" !!Unsupported!!\n");
         return -1;
+      }
 
       //ACK the USB pipe but don't do anything; nothing to do.
       if (port->gmlan == gmlan_enable) {
-        puts("The CAN bus is already in the requested gmlan config.\n");
+        puts(" ~~Nochange~~.\n");
         break;
       }
 
       // Check to see if anyther canid is acting as gmlan, disable it.
-      if (gmlan_enable) {
-        for (i = 0; i < CAN_MAX; i++) {
-          if (can_ports[i].gmlan) {
-            puts("Disable old gmlan mode\n");
-            set_can_mode(i, 0);
-          }
-        }
-      }
-
       set_can_mode(canid, gmlan_enable);
+      puts(" Done\n");
       break;
     case 0xdc: // set controls allowed
       controls_allowed = setup->b.wValue.w == 0x1337;
       // take CAN out of SILM, careful with speed!
-      can_init(0);
-      can_init(1);
-      #ifdef CAN3
-      can_init(2);
-      #endif
+      for(i=0; i < CAN_MAX; i++)
+        can_init(i);
       break;
     case 0xdd: // enable can forwarding
       if (setup->b.wValue.w < CAN_MAX) { //Set forwarding
@@ -574,7 +575,11 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
       break;
     case 0xdf: // Get Can bitrate
       if (setup->b.wValue.w < CAN_MAX) {
-        memcpy(resp, (void *)&can_ports[setup->b.wValue.w].bitrate, 4);
+        if(can_ports[setup->b.wValue.w].gmlan){
+          memcpy(resp, (void *)&can_ports[setup->b.wValue.w].gmlan_bitrate, 4);
+        }else{
+          memcpy(resp, (void *)&can_ports[setup->b.wValue.w].bitrate, 4);
+        }
         resp_len = 4;
         break;
       }
@@ -762,6 +767,8 @@ void __initialize_hardware_early() {
 }
 
 int main() {
+  int i;
+
   // init devices
   clock_init();
   periph_init();
@@ -784,18 +791,15 @@ int main() {
   // enable USB
   usb_init();
 
-  // default to silent mode to prevent issues with Ford
+  // enable CAN
 #ifdef PANDA_SAFETY
   controls_allowed = 0;
 #else
   controls_allowed = 1;
 #endif
 
-  can_init(0);
-  can_init(1);
-  #ifdef CAN3
-    can_init(2);
-  #endif
+  for(i=0; i < CAN_MAX; i++)
+    can_init(i);
 
   adc_init();
 
@@ -818,7 +822,6 @@ int main() {
   // set PWM
   set_fan_speed(65535);
 
-
   puts("**** INTERRUPTS ON ****\n");
   __disable_irq();
 
diff --git a/board/rev.h b/board/rev.h
new file mode 100644
index 00000000..16a90a0d
--- /dev/null
+++ b/board/rev.h
@@ -0,0 +1,39 @@
+#ifndef PANDA_REV_H
+#define PANDA_REV_H
+
+enum rev {
+  REV_A,
+  REV_B,
+  REV_C
+};
+
+#ifdef STM32F4
+  #define PANDA
+#endif
+
+#ifdef PANDA
+  #include "stm32f4xx.h"
+  #include "stm32f4xx_hal_gpio_ex.h"
+#else
+  #include "stm32f2xx.h"
+  #include "stm32f2xx_hal_gpio_ex.h"
+#endif
+
+#ifdef PANDA
+  #define ENABLE_CURRENT_SENSOR
+  #define ENABLE_SPI
+#endif
+
+#ifdef PANDA
+  #define LED_RED 9
+  #define LED_GREEN 7
+  #define LED_BLUE 6
+#else
+  #define LED_RED 10
+  #define LED_GREEN 11
+  #define LED_BLUE -1
+#endif
+
+#define FREQ 24000000 // 24 mhz
+
+#endif
diff --git a/board/usb.h b/board/usb.h
index c7f668b8..a422883e 100644
--- a/board/usb.h
+++ b/board/usb.h
@@ -1,5 +1,7 @@
 // **** supporting defines ****
 
+#include "config.h"
+
 typedef struct
 {
   __IO uint32_t HPRT;
@@ -241,6 +243,14 @@ void USB_WritePacket(const uint8_t *src, uint16_t len, uint32_t ep) {
   }
 }
 
+void USB_Stall_EP0(){
+  //TODO: Handle stalling both in and out requests.
+  //Check why INEP works for both in and out requests.
+  USBx_INEP(0)->DIEPCTL |= (USB_OTG_DIEPCTL_STALL | USB_OTG_DIEPCTL_EPENA);
+
+  //USBx_OUTEP(0)->DOEPCTL |= (USB_OTG_DOEPCTL_STALL | USB_OTG_DOEPCTL_EPENA);
+}
+
 void usb_reset() {
   // unmask endpoint interrupts, so many sets
   USBx_DEVICE->DAINT = 0xFFFFFFFF;
@@ -407,17 +417,19 @@ void usb_setup() {
       USB_WritePacket(0, 0, 0);
       USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
       //USBx_INEP(0)->DIEPINT = USBx_INEP(0)->DIEPINT
+      #ifdef DEBUG_USB
       puts("Setting Interface Alt: ");
       puth(current_int0_alt_setting);
       puts("\n");
+      #endif
       break;
     default:
       resp_len = usb_cb_control_msg(&setup, resp, 1);
       if(resp_len == -1){
-	USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_STALL;
+        USB_Stall_EP0();
       }else{
-	USB_WritePacket(resp, min(resp_len, setup.b.wLength.w), 0);
-	USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
+        USB_WritePacket(resp, min(resp_len, setup.b.wLength.w), 0);
+        USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
       }
   }
 }
@@ -493,7 +505,6 @@ void usb_init() {
 }
 
 // ***************************** USB port *****************************
-int dumb = 0;
 void usb_irqhandler(void) {
   //USBx->GINTMSK = 0;
 
@@ -503,14 +514,15 @@ void usb_irqhandler(void) {
 
   // gintsts SUSPEND? 04008428
   #ifdef DEBUG_USB
-    puth(gintsts);
-    puts(" ");
-    /*puth(USBx->GCCFG);
-    puts(" ");*/
-    puth(gotgint);
-    puts(" ep ");
+    puts("USB interrupt (DAINT[EP]:");
     puth(daint);
-    puts(" USB interrupt!\n");
+    puts(" GINTSTS:");
+    puth(gintsts);
+    //puts(" GCCFG:");
+    //puth(USBx->GCCFG);
+    //puts(" GOTGINT:");
+    //puth(gotgint);
+    puts(")\n");
   #endif
 
   if (gintsts & USB_OTG_GINTSTS_CIDSCHG) {
@@ -542,7 +554,7 @@ void usb_irqhandler(void) {
     //USBx->GOTGINT = USBx->GOTGINT;
   }
 
-  // RX FIFO first
+  // RX FIFO Has Data (OUT TRANSFER)
   if (gintsts & USB_OTG_GINTSTS_RXFLVL) {
     // 1. Read the Receive status pop register
     volatile unsigned int rxst = USBx->GRXSTSP;
@@ -557,7 +569,7 @@ void usb_irqhandler(void) {
       puts("\n");
     #endif
 
-    if (((rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17) == STS_DATA_UPDT) {
+    if (((rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17) == STS_DATA_UPDT) { //OUT DATA PKT RECEIVED
       int endpoint = (rxst & USB_OTG_GRXSTSP_EPNUM);
       int len = (rxst & USB_OTG_GRXSTSP_BCNT) >> 4;
       USB_ReadPacket(&usbdata, len);
@@ -569,7 +581,7 @@ void usb_irqhandler(void) {
       #endif
 
       if(endpoint == 0){
-	usb_cb_ep0_out(usbdata, len, 1);
+        usb_cb_ep0_out(usbdata, len, 1);
       }
 
       if (endpoint == 2) {
@@ -669,6 +681,9 @@ void usb_irqhandler(void) {
 
     // respond to setup packets
     if (USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) {
+      #ifdef DEBUG_USB
+      puts("Processing SETUP\n");
+      #endif
       usb_setup();
     }
 
@@ -704,11 +719,10 @@ void usb_irqhandler(void) {
     case 0: ////// Bulk config
       // *** IN token received when TxFIFO is empty
       if (USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) {
-	puth(dumb++);
-	if (can_rx_q.w_ptr != can_rx_q.r_ptr)
-	  puts("Rx CAN bulk: There is CAN data to send.\n");
-	else
-	  puts("Rx CAN bulk: No can data\n");
+        if (can_rx_q.w_ptr != can_rx_q.r_ptr)
+          puts("Rx CAN bulk: There is CAN data to send.\n");
+        else
+          puts("Rx CAN bulk: No can data\n");
 
         #ifdef DEBUG_USB
         puts("  IN PACKET QUEUE\n");