diff --git a/board/build.mk b/board/build.mk
index 43b54eea9..6b2be50a1 100644
--- a/board/build.mk
+++ b/board/build.mk
@@ -76,13 +76,13 @@ obj/%.$(PROJ_NAME).o: ../crypto/%.c
 obj/$(STARTUP_FILE).o: $(STARTUP_FILE).s
 	$(CC) $(CFLAGS) -o $@ -c $<
 
-obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o obj/early.$(PROJ_NAME).o obj/llgpio.$(PROJ_NAME).o
+obj/$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/main.$(PROJ_NAME).o obj/early.$(PROJ_NAME).o obj/llgpio.$(PROJ_NAME).o obj/can.$(PROJ_NAME).o obj/uart.$(PROJ_NAME).o obj/gpio.$(PROJ_NAME).o obj/libc.$(PROJ_NAME).o
   # hack
 	$(CC) -Wl,--section-start,.isr_vector=0x8004000 $(CFLAGS) -o obj/$(PROJ_NAME).elf $^
 	$(OBJCOPY) -v -O binary obj/$(PROJ_NAME).elf obj/code.bin
 	SETLEN=1 ../crypto/sign.py obj/code.bin $@ $(CERT)
 
-obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o obj/early.$(PROJ_NAME).o obj/llgpio.$(PROJ_NAME).o
+obj/bootstub.$(PROJ_NAME).bin: obj/$(STARTUP_FILE).o obj/bootstub.$(PROJ_NAME).o obj/sha.$(PROJ_NAME).o obj/rsa.$(PROJ_NAME).o obj/early.$(PROJ_NAME).o obj/llgpio.$(PROJ_NAME).o obj/libc.$(PROJ_NAME).o
 	$(CC) $(CFLAGS) -o obj/bootstub.$(PROJ_NAME).elf $^
 	$(OBJCOPY) -v -O binary obj/bootstub.$(PROJ_NAME).elf $@
 
diff --git a/board/can.c b/board/can.c
new file mode 100644
index 000000000..02d1d87ec
--- /dev/null
+++ b/board/can.c
@@ -0,0 +1,186 @@
+#include <stdint.h>
+#include "config.h"
+#include "can.h"
+#include "uart.h"
+#include "gpio.h"
+#include "libc.h"
+
+// assign CAN numbering
+// old:         CAN1 = 1   CAN2 = 0
+// panda:       CAN1 = 0   CAN2 = 1   CAN3 = 2
+#ifdef PANDA
+  CAN_TypeDef *can_numbering[] = {CAN1, CAN2, CAN3};
+  int8_t can_forwarding[] = {-1, -1, -1};
+  uint32_t can_bitrate[] = {CAN_DEFAULT_BITRATE,
+			  CAN_DEFAULT_BITRATE,
+			  CAN_DEFAULT_BITRATE};
+#else
+  CAN_TypeDef *can_numbering[] = {CAN2, CAN1};
+  int8_t can_forwarding[] = {-1,-1};
+  uint32_t can_bitrate[] = {CAN_DEFAULT_BITRATE,
+			    CAN_DEFAULT_BITRATE};
+#endif
+
+int controls_allowed = 0;
+
+int pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
+  if (q->w_ptr != q->r_ptr) {
+    *elem = q->elems[q->r_ptr];
+    if ((q->r_ptr + 1) == q->fifo_size) q->r_ptr = 0;
+    else q->r_ptr += 1;
+    return 1;
+  }
+  return 0;
+}
+
+int push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
+  uint32_t next_w_ptr;
+  if ((q->w_ptr + 1) == q->fifo_size) next_w_ptr = 0;
+  else next_w_ptr = q->w_ptr + 1;
+  if (next_w_ptr != q->r_ptr) {
+    q->elems[q->w_ptr] = *elem;
+    q->w_ptr = next_w_ptr;
+    return 1;
+  }
+  puts("push failed!\n");
+  return 0;
+}
+
+// ********************* CAN Functions *********************
+
+void can_init(uint8_t canid) {
+  uint32_t bitrate = can_bitrate[canid];
+  CAN_TypeDef *CAN = can_numbering[canid];
+  uint8_t quanta;
+  uint16_t prescaler;
+  uint8_t seq1, seq2;
+
+  puts("Configuring Can Interface index ");
+  puth(canid);
+  puts("\n");
+
+  //MAX 1 Megabaud
+  if(bitrate > 1000000)
+    bitrate = 1000000;
+
+  puts("Can Speed request to ");
+  puth(bitrate);
+  puts("\n");
+
+  //TODO: Try doing both and find the more accurate values.
+  if(min((FREQ / 2) / bitrate, 16) == 16){
+    quanta = 16;
+    seq1 = 13;//roundf(quanta * 0.875f) - 1;
+    seq2 = 2;//roundf(quanta * 0.125f);
+  }else{
+    quanta = 8;
+    seq1 = 6;//roundf(quanta * 0.875f) - 1;
+    seq2 = 1;//roundf(quanta * 0.125f);
+  }
+
+  // TODO: Look into better accuracy with rounding.
+  prescaler = FREQ / quanta / bitrate;
+
+  //Check the prescaler is not larger than max
+  if(prescaler > 0x3FF)
+    prescaler = 0x3FF;
+
+  can_bitrate[canid] = FREQ/quanta/prescaler;
+
+  puts("Can Speed set to ");
+  puth(can_bitrate[canid]);
+  puts("\n");
+
+  set_can_enable(CAN, 1);
+
+  // Move CAN to initialization mode and sync.
+  CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
+  while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
+
+  // seg 1: 13 time quanta, seg 2: 2 time quanta
+  CAN->BTR = (CAN_BTR_TS1_0 * (seq1 - 1)) |
+             (CAN_BTR_TS2_0 * (seq2 - 1)) |
+             (prescaler - 1);
+
+  // silent loopback mode for debugging
+  #ifdef CAN_LOOPBACK_MODE
+    CAN->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM;
+  #endif
+
+  if (!controls_allowed) {
+    CAN->BTR |= CAN_BTR_SILM;
+  }
+
+  // reset
+  CAN->MCR = CAN_MCR_TTCM;
+
+  int tmp = 0;
+  while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++;
+
+  if (tmp == CAN_TIMEOUT) {
+    set_led(LED_BLUE, 1);
+    puts("CAN init FAILED!!!!!\n");
+  } else {
+    puts("CAN init done\n");
+  }
+
+  // accept all filter
+  CAN->FMR |= CAN_FMR_FINIT;
+
+  // no mask
+  CAN->sFilterRegister[0].FR1 = 0;
+  CAN->sFilterRegister[0].FR2 = 0;
+  CAN->sFilterRegister[14].FR1 = 0;
+  CAN->sFilterRegister[14].FR2 = 0;
+  CAN->FA1R |= 1 | (1 << 14);
+
+  CAN->FMR &= ~(CAN_FMR_FINIT);
+
+  // enable all CAN interrupts
+  CAN->IER = 0xFFFFFFFF;
+  //CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_FMPIE1;
+}
+
+// CAN error
+void can_sce(CAN_TypeDef *CAN) {
+  #ifdef DEBUG
+    if (CAN==CAN1) puts("CAN1:  ");
+    if (CAN==CAN2) puts("CAN2:  ");
+    #ifdef CAN3
+      if (CAN==CAN3) puts("CAN3:  ");
+    #endif
+    puts("MSR:");
+    puth(CAN->MSR);
+    puts(" TSR:");
+    puth(CAN->TSR);
+    puts(" RF0R:");
+    puth(CAN->RF0R);
+    puts(" RF1R:");
+    puth(CAN->RF1R);
+    puts(" ESR:");
+    puth(CAN->ESR);
+    puts("\n");
+  #endif
+
+  // clear
+  //CAN->sTxMailBox[0].TIR &= ~(CAN_TI0R_TXRQ);
+  CAN->TSR |= CAN_TSR_ABRQ0;
+  //CAN->ESR |= CAN_ESR_LEC;
+  //CAN->MSR &= ~(CAN_MSR_ERRI);
+  CAN->MSR = CAN->MSR;
+}
+
+int can_cksum(uint8_t *dat, int len, int addr, int idx) {
+  int i;
+  int s = 0;
+  for (i = 0; i < len; i++) {
+    s += (dat[i] >> 4);
+    s += dat[i] & 0xF;
+  }
+  s += (addr>>0)&0xF;
+  s += (addr>>4)&0xF;
+  s += (addr>>8)&0xF;
+  s += idx;
+  s = 8-s;
+  return s&0xF;
+}
diff --git a/board/can.h b/board/can.h
index f8387db12..16923dbaf 100644
--- a/board/can.h
+++ b/board/can.h
@@ -1,101 +1,46 @@
-void can_init(CAN_TypeDef *CAN, int silent) {
-  set_can_enable(CAN, 1);
+#ifndef PANDA_CAN_H
+#define PANDA_CAN_H
 
-  CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
-  while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
+#define CAN_TIMEOUT 1000000
 
-  // http://www.bittiming.can-wiki.info/
-  // PCLK = 24 MHz
-  uint32_t pclk = 24000;
-  uint32_t num_time_quanta = 16;
+extern CAN_TypeDef *can_numbering[];
+extern int8_t can_forwarding[];
+extern uint32_t can_bitrate[];
 
-  // 500 kbps
-  uint32_t prescaler = pclk / num_time_quanta / 500;
+#ifdef PANDA
+  #define CAN_MAX 3
+#else
+  #define CAN_MAX 2
+#endif
 
-  // seg 1: 13 time quanta, seg 2: 2 time quanta
-  CAN->BTR = (CAN_BTR_TS1_0 * 12) |
-    CAN_BTR_TS2_0 | (prescaler - 1);
+// ********************* queues types *********************
 
-  // silent loopback mode for debugging
-  #ifdef CAN_LOOPBACK_MODE
-    CAN->BTR |= CAN_BTR_SILM | CAN_BTR_LBKM;
-  #endif
+typedef struct {
+  uint32_t w_ptr;
+  uint32_t r_ptr;
+  uint32_t fifo_size;
+  CAN_FIFOMailBox_TypeDef *elems;
+} can_ring;
 
-  if (silent) {
-    CAN->BTR |= CAN_BTR_SILM;
-  }
+#define can_buffer(x, size) \
+  CAN_FIFOMailBox_TypeDef elems_##x[size]; \
+  can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = size, .elems = (CAN_FIFOMailBox_TypeDef *)&elems_##x };
 
-  // reset
-  CAN->MCR = CAN_MCR_TTCM;
+// ********************* interrupt safe queue *********************
 
-  #define CAN_TIMEOUT 1000000
-  int tmp = 0;
-  while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK && tmp < CAN_TIMEOUT) tmp++;
+int pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
 
-  if (tmp == CAN_TIMEOUT) {
-    set_led(LED_BLUE, 1);
-    puts("CAN init FAILED!!!!!\n");
-  } else {
-    puts("CAN init done\n");
-  }
+int push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem);
 
-  // accept all filter
-  CAN->FMR |= CAN_FMR_FINIT;
+// ********************* CAN Functions *********************
 
-  // no mask
-  CAN->sFilterRegister[0].FR1 = 0;
-  CAN->sFilterRegister[0].FR2 = 0;
-  CAN->sFilterRegister[14].FR1 = 0;
-  CAN->sFilterRegister[14].FR2 = 0;
-  CAN->FA1R |= 1 | (1 << 14);
-
-  CAN->FMR &= ~(CAN_FMR_FINIT);
-
-  // enable all CAN interrupts
-  CAN->IER = 0xFFFFFFFF;
-  //CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_FMPIE1;
-}
+void can_init(uint8_t canid);
 
 // CAN error
-void can_sce(CAN_TypeDef *CAN) {
-  #ifdef DEBUG
-    if (CAN==CAN1) puts("CAN1:  ");
-    if (CAN==CAN2) puts("CAN2:  ");
-    #ifdef CAN3
-      if (CAN==CAN3) puts("CAN3:  ");
-    #endif
-    puts("MSR:");
-    puth(CAN->MSR);
-    puts(" TSR:");
-    puth(CAN->TSR);
-    puts(" RF0R:");
-    puth(CAN->RF0R);
-    puts(" RF1R:");
-    puth(CAN->RF1R);
-    puts(" ESR:");
-    puth(CAN->ESR);
-    puts("\n");
-  #endif
+void can_sce(CAN_TypeDef *CAN);
 
-  // clear
-  //CAN->sTxMailBox[0].TIR &= ~(CAN_TI0R_TXRQ);
-  CAN->TSR |= CAN_TSR_ABRQ0;
-  //CAN->ESR |= CAN_ESR_LEC;
-  //CAN->MSR &= ~(CAN_MSR_ERRI);
-  CAN->MSR = CAN->MSR;
-}
+int can_cksum(uint8_t *dat, int len, int addr, int idx);
 
-int can_cksum(uint8_t *dat, int len, int addr, int idx) {
-  int i;
-  int s = 0;
-  for (i = 0; i < len; i++) {
-    s += (dat[i] >> 4);
-    s += dat[i] & 0xF;
-  }
-  s += (addr>>0)&0xF;
-  s += (addr>>4)&0xF;
-  s += (addr>>8)&0xF;
-  s += idx;
-  s = 8-s;
-  return s&0xF;
-}
+extern int controls_allowed;
+
+#endif
diff --git a/board/config.h b/board/config.h
index d9304bd82..bfecb34d8 100644
--- a/board/config.h
+++ b/board/config.h
@@ -17,7 +17,29 @@
   #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 FIFO_SIZE 0x100
+
+#define NULL ((void*)0)
+
+#define PANDA_REV_AB 0
+#define PANDA_REV_C 1
+
 #endif
diff --git a/board/early.c b/board/early.c
index bac6c6be9..30bd7b80e 100644
--- a/board/early.c
+++ b/board/early.c
@@ -1,6 +1,7 @@
 #include "config.h"
 #include "early.h"
 #include "llgpio.h"
+#include "uart.h"
 
 int has_external_debug_serial = 0;
 int is_giant_panda = 0;
diff --git a/board/early.h b/board/early.h
index 26608cf5c..5c445251a 100644
--- a/board/early.h
+++ b/board/early.h
@@ -2,9 +2,6 @@
 #define POST_BOOTLOADER_MAGIC 0xdeadb111
 #define PULL_EFFECTIVE_DELAY 10
 
-#define PANDA_REV_AB 0
-#define PANDA_REV_C 1
-
 extern uint32_t enter_bootloader_mode;
 extern void *_app_start[];
 extern void *g_pfnVectors;
diff --git a/board/gpio.c b/board/gpio.c
new file mode 100644
index 000000000..0e618519e
--- /dev/null
+++ b/board/gpio.c
@@ -0,0 +1,270 @@
+#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(CAN_TypeDef *CAN, int enabled) {
+  // enable CAN busses
+  if (CAN == CAN1) {
+    #ifdef PANDA
+      // CAN1_EN
+      set_gpio_output(GPIOC, 1, !enabled);
+    #else
+      // CAN1_EN
+      set_gpio_output(GPIOB, 3, enabled);
+    #endif
+  } else if (CAN == CAN2) {
+    #ifdef PANDA
+      // CAN2_EN
+      set_gpio_output(GPIOC, 13, !enabled);
+    #else
+      // CAN2_EN
+      set_gpio_output(GPIOB, 4, enabled);
+    #endif
+  #ifdef CAN3
+  } else if (CAN == CAN3) {
+    // CAN3_EN
+    set_gpio_output(GPIOA, 0, !enabled);
+  #endif
+  }
+}
+
+void set_led(int led_num, int on) {
+  if (led_num == -1) return;
+
+  #ifdef PANDA
+    set_gpio_output(GPIOC, led_num, !on);
+  #else
+    set_gpio_output(GPIOB, led_num, !on);
+  #endif
+}
+
+
+// TODO: does this belong here?
+void periph_init() {
+  // enable GPIOB, UART2, CAN, USB clock
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
+  RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
+
+  RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
+  RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
+  RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
+  #ifdef PANDA
+    RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
+  #endif
+  RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+  RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
+  #ifdef CAN3
+    RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
+  #endif
+  RCC->APB1ENR |= RCC_APB1ENR_DACEN;
+  RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
+  //RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
+  RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
+  RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
+  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+  RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
+  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+
+  // needed?
+  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
+}
+
+void set_can_mode(int can, int use_gmlan) {
+  // http://www.bittiming.can-wiki.info/#bxCAN
+  // 24 MHz, sample point at 87.5%
+  uint32_t pclk = 24000;
+  uint32_t num_time_quanta = 16;
+  uint32_t prescaler;
+  CAN_TypeDef *CAN;
+
+  if(can >= CAN_MAX) return;
+  CAN = can_numbering[can];
+
+  // connects to CAN2 xcvr or GMLAN xcvr
+  if (use_gmlan) {
+    if (can == 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);
+
+      /* GMLAN mode pins:
+      M0(B15)  M1(B14)  mode
+      =======================
+      0        0        sleep
+      1        0        100kbit
+      0        1        high voltage wakeup
+      1        1        33kbit (normal)
+      */
+
+    } else if (revision == PANDA_REV_C && can == 2) {
+      // 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);
+    }
+
+    // put gmlan transceiver in normal mode
+    set_gpio_output(GPIOB, 14, 1);
+    set_gpio_output(GPIOB, 15, 1);
+
+    // 83.3 kbps
+    // prescaler = pclk / num_time_quanta * 10 / 833;
+
+    // 33.3 kbps
+    prescaler = pclk / num_time_quanta * 10 / 333;
+  } else {
+    if (can == 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 (can == 2) {
+      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);
+    }
+
+    // 500 kbps
+    prescaler = pclk / num_time_quanta / 500;
+  }
+
+  // init
+  CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
+  while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
+
+  // set speed
+  // seg 1: 13 time quanta, seg 2: 2 time quanta
+  CAN->BTR = (CAN_BTR_TS1_0 * 12) |
+    CAN_BTR_TS2_0 | (prescaler - 1);
+
+  // running
+  CAN->MCR = CAN_MCR_TTCM;
+  while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK);
+}
+
+// board specific
+void gpio_init() {
+  // pull low to hold ESP in reset??
+  // enable OTG out tied to ground
+  GPIOA->ODR = 0;
+  GPIOB->ODR = 0;
+  GPIOA->PUPDR = 0;
+  //GPIOC->ODR = 0;
+  GPIOB->AFR[0] = 0;
+  GPIOB->AFR[1] = 0;
+
+  // C2,C3: analog mode, voltage and current sense
+  set_gpio_mode(GPIOC, 2, MODE_ANALOG);
+  set_gpio_mode(GPIOC, 3, MODE_ANALOG);
+
+  // C8: FAN aka TIM3_CH4
+  set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
+
+  // turn off LEDs and set mode
+  set_led(LED_RED, 0);
+  set_led(LED_GREEN, 0);
+  set_led(LED_BLUE, 0);
+
+  // A11,A12: USB
+  set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS);
+  set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
+  GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
+
+#ifdef PANDA
+  // enable started_alt on the panda
+  set_gpio_pullup(GPIOA, 1, PULL_UP);
+
+  // A2,A3: USART 2 for debugging
+  set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2);
+  set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2);
+
+  // A9,A10: USART 1 for talking to the ESP
+  set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
+  set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
+
+  // B12: GMLAN, ignition sense, pull up
+  set_gpio_pullup(GPIOB, 12, PULL_UP);
+
+  // A4,A5,A6,A7: setup SPI
+  set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
+  set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
+  set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1);
+  set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
+#endif
+
+  // B8,B9: CAN 1
+  set_can_enable(CAN1, 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(CAN2, 0);
+  set_can_mode(2, 0);
+
+  // A8,A15: CAN3
+  #ifdef CAN3
+    set_can_enable(CAN3, 0);
+    set_can_mode(3, 0);
+  #endif
+
+  #ifdef PANDA
+    // K-line enable moved from B4->B7 to make room for GMLAN on CAN3
+    if(revision == PANDA_REV_C)
+      set_gpio_output(GPIOB, 7, 1); // REV C
+    else
+      set_gpio_output(GPIOB, 4, 1); // REV AB
+
+    // C12,D2: K-Line setup on UART 5
+    set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5);
+    set_gpio_alternate(GPIOD, 2, GPIO_AF8_UART5);
+    set_gpio_pullup(GPIOD, 2, PULL_UP);
+
+    // L-line enable
+    set_gpio_output(GPIOA, 14, 1);
+
+    // C10,C11: L-Line setup on USART 3
+    set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3);
+    set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
+    set_gpio_pullup(GPIOC, 11, PULL_UP);
+  #endif
+
+  if(revision == PANDA_REV_C) {
+    // B2,A13: set DCP mode on the charger (breaks USB!)
+    //set_gpio_output(GPIOB, 2, 0);
+    //set_gpio_output(GPIOA, 13, 0);
+
+    //set_gpio_output(GPIOA, 13, 1); //CTRL 1
+    //set_gpio_output(GPIOB, 2, 0);  //CTRL 2
+  }
+}
diff --git a/board/gpio.h b/board/gpio.h
index c0910fb6a..c7e39dd8b 100644
--- a/board/gpio.h
+++ b/board/gpio.h
@@ -1,278 +1,11 @@
-#ifdef STM32F4
-  #include "stm32f4xx_hal_gpio_ex.h"
-#else
-  #include "stm32f2xx_hal_gpio_ex.h"
-#endif
-
-#include "llgpio.h"
-
-void set_can_enable(CAN_TypeDef *CAN, int enabled) {
-  // enable CAN busses
-  if (CAN == CAN1) {
-    #ifdef PANDA
-      // CAN1_EN
-      set_gpio_output(GPIOC, 1, !enabled);
-    #else
-      // CAN1_EN
-      set_gpio_output(GPIOB, 3, enabled);
-    #endif
-  } else if (CAN == CAN2) {
-    #ifdef PANDA
-      // CAN2_EN
-      set_gpio_output(GPIOC, 13, !enabled);
-    #else
-      // CAN2_EN
-      set_gpio_output(GPIOB, 4, enabled);
-    #endif
-  #ifdef CAN3
-  } else if (CAN == CAN3) {
-    // CAN3_EN
-    set_gpio_output(GPIOA, 0, !enabled);
-  #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
-
-void set_led(int led_num, int on) {
-  if (led_num == -1) return;
-
-  #ifdef PANDA
-    set_gpio_output(GPIOC, led_num, !on);
-  #else
-    set_gpio_output(GPIOB, led_num, !on);
-  #endif
-}
+void set_can_enable(CAN_TypeDef *CAN, int enabled);
 
+void set_led(int led_num, int on);
 
 // TODO: does this belong here?
-void periph_init() {
-  // enable GPIOB, UART2, CAN, USB clock
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN;
-  RCC->AHB1ENR |= RCC_AHB1ENR_GPIODEN;
+void periph_init();
 
-  RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
-  RCC->APB1ENR |= RCC_APB1ENR_USART2EN;
-  RCC->APB1ENR |= RCC_APB1ENR_USART3EN;
-  #ifdef PANDA
-    RCC->APB1ENR |= RCC_APB1ENR_UART5EN;
-  #endif
-  RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
-  RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
-  #ifdef CAN3
-    RCC->APB1ENR |= RCC_APB1ENR_CAN3EN;
-  #endif
-  RCC->APB1ENR |= RCC_APB1ENR_DACEN;
-  RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
-  //RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
-  RCC->APB2ENR |= RCC_APB2ENR_USART1EN;
-  RCC->AHB2ENR |= RCC_AHB2ENR_OTGFSEN;
-  RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_ADC1EN;
-  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
-
-  // needed?
-  RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN;
-}
-
-void set_can_mode(int can, int use_gmlan) {
-  // http://www.bittiming.can-wiki.info/#bxCAN
-  // 24 MHz, sample point at 87.5%
-  uint32_t pclk = 24000;
-  uint32_t num_time_quanta = 16;
-  uint32_t prescaler;
-  CAN_TypeDef *CAN = NULL;
-  if (can == 2) CAN = CAN2;
-#ifdef CAN3
-  else if (can == 3) CAN = CAN3;
-#endif
-  if (CAN == NULL) return;
-
-  // connects to CAN2 xcvr or GMLAN xcvr
-  if (use_gmlan) {
-    if (can == 2) {
-      // 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);
-
-      /* GMLAN mode pins:
-      M0(B15)  M1(B14)  mode
-      =======================
-      0        0        sleep
-      1        0        100kbit
-      0        1        high voltage wakeup
-      1        1        33kbit (normal)
-      */
-
-    } else if (revision == PANDA_REV_C && can == 3) {
-      // 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);
-    }
-
-    // put gmlan transceiver in normal mode
-    set_gpio_output(GPIOB, 14, 1);
-    set_gpio_output(GPIOB, 15, 1);
-
-    // 83.3 kbps
-    // prescaler = pclk / num_time_quanta * 10 / 833;
-
-    // 33.3 kbps
-    prescaler = pclk / num_time_quanta * 10 / 333;
-  } else {
-    if (can == 2) {
-      // 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 (can == 3) {
-      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);
-    }
-
-    // 500 kbps
-    prescaler = pclk / num_time_quanta / 500;
-  }
-
-  // init
-  CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ;
-  while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK);
-
-  // set speed
-  // seg 1: 13 time quanta, seg 2: 2 time quanta
-  CAN->BTR = (CAN_BTR_TS1_0 * 12) |
-    CAN_BTR_TS2_0 | (prescaler - 1);
-
-  // running
-  CAN->MCR = CAN_MCR_TTCM;
-  while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK);
-}
+void set_can_mode(int can, int use_gmlan);
 
 // board specific
-void gpio_init() {
-  // pull low to hold ESP in reset??
-  // enable OTG out tied to ground
-  GPIOA->ODR = 0;
-  GPIOB->ODR = 0;
-  GPIOA->PUPDR = 0;
-  //GPIOC->ODR = 0;
-  GPIOB->AFR[0] = 0;
-  GPIOB->AFR[1] = 0;
-
-  // C2,C3: analog mode, voltage and current sense
-  set_gpio_mode(GPIOC, 2, MODE_ANALOG);
-  set_gpio_mode(GPIOC, 3, MODE_ANALOG);
-
-  // C8: FAN aka TIM3_CH4
-  set_gpio_alternate(GPIOC, 8, GPIO_AF2_TIM3);
-
-  // turn off LEDs and set mode
-  set_led(LED_RED, 0);
-  set_led(LED_GREEN, 0);
-  set_led(LED_BLUE, 0);
-
-  // A11,A12: USB
-  set_gpio_alternate(GPIOA, 11, GPIO_AF10_OTG_FS);
-  set_gpio_alternate(GPIOA, 12, GPIO_AF10_OTG_FS);
-  GPIOA->OSPEEDR = GPIO_OSPEEDER_OSPEEDR11 | GPIO_OSPEEDER_OSPEEDR12;
-
-#ifdef PANDA
-  // enable started_alt on the panda
-  set_gpio_pullup(GPIOA, 1, PULL_UP);
-
-  // A2,A3: USART 2 for debugging
-  set_gpio_alternate(GPIOA, 2, GPIO_AF7_USART2);
-  set_gpio_alternate(GPIOA, 3, GPIO_AF7_USART2);
-
-  // A9,A10: USART 1 for talking to the ESP
-  set_gpio_alternate(GPIOA, 9, GPIO_AF7_USART1);
-  set_gpio_alternate(GPIOA, 10, GPIO_AF7_USART1);
-
-  // B12: GMLAN, ignition sense, pull up
-  set_gpio_pullup(GPIOB, 12, PULL_UP);
-
-  // A4,A5,A6,A7: setup SPI
-  set_gpio_alternate(GPIOA, 4, GPIO_AF5_SPI1);
-  set_gpio_alternate(GPIOA, 5, GPIO_AF5_SPI1);
-  set_gpio_alternate(GPIOA, 6, GPIO_AF5_SPI1);
-  set_gpio_alternate(GPIOA, 7, GPIO_AF5_SPI1);
-#endif
-
-  // B8,B9: CAN 1
-  set_can_enable(CAN1, 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(CAN2, 0);
-  set_can_mode(2, 0);
-
-  // A8,A15: CAN3
-  #ifdef CAN3
-    set_can_enable(CAN3, 0);
-    set_can_mode(3, 0);
-  #endif
-
-  #ifdef PANDA
-    // K-line enable moved from B4->B7 to make room for GMLAN on CAN3
-    if(revision == PANDA_REV_C)
-      set_gpio_output(GPIOB, 7, 1); // REV C
-    else
-      set_gpio_output(GPIOB, 4, 1); // REV AB
-
-    // C12,D2: K-Line setup on UART 5
-    set_gpio_alternate(GPIOC, 12, GPIO_AF8_UART5);
-    set_gpio_alternate(GPIOD, 2, GPIO_AF8_UART5);
-    set_gpio_pullup(GPIOD, 2, PULL_UP);
-
-    // L-line enable
-    set_gpio_output(GPIOA, 14, 1);
-
-    // C10,C11: L-Line setup on USART 3
-    set_gpio_alternate(GPIOC, 10, GPIO_AF7_USART3);
-    set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3);
-    set_gpio_pullup(GPIOC, 11, PULL_UP);
-  #endif
-
-  if(revision == PANDA_REV_C) {
-    // B2,A13: set DCP mode on the charger (breaks USB!)
-    //set_gpio_output(GPIOB, 2, 0);
-    //set_gpio_output(GPIOA, 13, 0);
-
-    //set_gpio_output(GPIOA, 13, 1); //CTRL 1
-    //set_gpio_output(GPIOB, 2, 0);  //CTRL 2
-  }
-}
+void gpio_init();
diff --git a/board/libc.c b/board/libc.c
new file mode 100644
index 000000000..b6b3d8e48
--- /dev/null
+++ b/board/libc.c
@@ -0,0 +1,66 @@
+#include <stdint.h>
+#include "config.h"
+#include "libc.h"
+
+void clock_init() {
+  // enable external oscillator
+  RCC->CR |= RCC_CR_HSEON;
+  while ((RCC->CR & RCC_CR_HSERDY) == 0);
+
+  // divide shit
+  RCC->CFGR = RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4;
+  #ifdef PANDA
+    RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
+                   RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE;
+  #else
+    RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
+                   RCC_PLLCFGR_PLLN_7 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLSRC_HSE;
+  #endif
+
+  // start PLL
+  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;
+
+  // switch to PLL
+  RCC->CFGR |= RCC_CFGR_SW_PLL;
+  while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
+
+  // *** running on PLL ***
+}
+
+
+void delay(int a) {
+  volatile int i;
+  for (i=0;i<a;i++);
+}
+
+void *memset(void *str, int c, unsigned int n) {
+  int i;
+  for (i = 0; i < n; i++) {
+    *((uint8_t*)str) = c;
+    ++str;
+  }
+  return str;
+}
+
+void *memcpy(void *dest, const void *src, unsigned int n) {
+  int i;
+  // TODO: make not slow
+  for (i = 0; i < n; i++) {
+    ((uint8_t*)dest)[i] = *(uint8_t*)src;
+    ++src;
+  }
+  return dest;
+}
+
+int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {
+  int i;
+  for (i = 0; i < num; i++) {
+    if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) return -1;
+  }
+  return 0;
+}
diff --git a/board/libc.h b/board/libc.h
index 6b3f5d274..d035c28e4 100644
--- a/board/libc.h
+++ b/board/libc.h
@@ -1,3 +1,5 @@
+#ifndef PANDA_LIBC_H
+#define PANDA_LIBC_H
 
 #define min(a,b) \
  ({ __typeof__ (a) _a = (a); \
@@ -11,65 +13,14 @@
 
 // **** shitty libc ****
 
-void clock_init() {
-  // enable external oscillator
-  RCC->CR |= RCC_CR_HSEON;
-  while ((RCC->CR & RCC_CR_HSERDY) == 0);
+void clock_init();
 
-  // divide shit
-  RCC->CFGR = RCC_CFGR_HPRE_DIV1 | RCC_CFGR_PPRE2_DIV2 | RCC_CFGR_PPRE1_DIV4;
-  #ifdef PANDA
-    RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
-                   RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLN_5 | RCC_PLLCFGR_PLLSRC_HSE;
-  #else
-    RCC->PLLCFGR = RCC_PLLCFGR_PLLQ_2 | RCC_PLLCFGR_PLLM_3 |
-                   RCC_PLLCFGR_PLLN_7 | RCC_PLLCFGR_PLLN_6 | RCC_PLLCFGR_PLLSRC_HSE;
-  #endif
+void delay(int a);
 
-  // start PLL
-  RCC->CR |= RCC_CR_PLLON;
-  while ((RCC->CR & RCC_CR_PLLRDY) == 0);
+void *memset(void *str, int c, unsigned int n);
 
-  // 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;
+void *memcpy(void *dest, const void *src, unsigned int n);
 
-  // switch to PLL
-  RCC->CFGR |= RCC_CFGR_SW_PLL;
-  while ((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
+int memcmp(const void * ptr1, const void * ptr2, unsigned int num);
 
-  // *** running on PLL ***
-}
-
-
-void delay(int a) {
-  volatile int i;
-  for (i=0;i<a;i++);
-}
-
-void *memset(void *str, int c, unsigned int n) {
-  int i;
-  for (i = 0; i < n; i++) {
-    *((uint8_t*)str) = c;
-    ++str;
-  }
-  return str;
-}
-
-void *memcpy(void *dest, const void *src, unsigned int n) {
-  int i;
-  // TODO: make not slow
-  for (i = 0; i < n; i++) {
-    ((uint8_t*)dest)[i] = *(uint8_t*)src;
-    ++src;
-  }
-  return dest;
-}
-
-int memcmp(const void * ptr1, const void * ptr2, unsigned int num) {
-  int i;
-  for (i = 0; i < num; i++) {
-    if ( ((uint8_t*)ptr1)[i] != ((uint8_t*)ptr2)[i] ) return -1;
-  }
-  return 0;
-}
+#endif
diff --git a/board/main.c b/board/main.c
index e36ebb61f..3c082dc98 100644
--- a/board/main.c
+++ b/board/main.c
@@ -1,28 +1,8 @@
 #include "config.h"
 #include "early.h"
-
-#define NULL ((void*)0)
-
-
-// assign CAN numbering
-// old:         CAN1 = 1   CAN2 = 0
-// panda:       CAN1 = 0   CAN2 = 1   CAN3 = 4
-#ifdef PANDA
-  int can_numbering[] = {0,1,4};
-#else
-  int can_numbering[] = {1,0,-1};
-#endif
-
-// can forwards FROM -> TO
-#define CAN_MAX 3
-int can_forwarding[] = {-1,-1,-1};
-
-// *** end config ***
-
 #include "obj/gitversion.h"
 
 // debug safety check: is controls allowed?
-int controls_allowed = 0;
 int started = 0;
 int can_live = 0, pending_can_live = 0;
 
@@ -34,70 +14,19 @@ int started_signal_detected = 0;
 // TODO: check for UART high
 int did_usb_enumerate = 0;
 
-// Declare puts to supress warning
-int puts ( const char * str );
 
-// ********************* instantiate queues *********************
+#include "uart.h"
+#include "can.h"
 
-typedef struct {
-  uint32_t w_ptr;
-  uint32_t r_ptr;
-  uint32_t fifo_size;
-  CAN_FIFOMailBox_TypeDef *elems;
-} can_ring;
-
-#define can_buffer(x, size) \
-  CAN_FIFOMailBox_TypeDef elems_##x[size]; \
-  can_ring can_##x = { .w_ptr = 0, .r_ptr = 0, .fifo_size = size, .elems = (CAN_FIFOMailBox_TypeDef *)&elems_##x };
+// ********************* instantiate can queues *********************
 
 can_buffer(rx_q, 0x1000)
 can_buffer(tx1_q, 0x100)
 can_buffer(tx2_q, 0x100)
 can_buffer(tx3_q, 0x100)
 
-// ********************* interrupt safe queue *********************
-
-int pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
-  if (q->w_ptr != q->r_ptr) {
-    *elem = q->elems[q->r_ptr];
-    if ((q->r_ptr + 1) == q->fifo_size) q->r_ptr = 0;
-    else q->r_ptr += 1;
-    return 1;
-  }
-  return 0;
-}
-
-int push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
-  uint32_t next_w_ptr;
-  if ((q->w_ptr + 1) == q->fifo_size) next_w_ptr = 0;
-  else next_w_ptr = q->w_ptr + 1;
-  if (next_w_ptr != q->r_ptr) {
-    q->elems[q->w_ptr] = *elem;
-    q->w_ptr = next_w_ptr;
-    return 1;
-  }
-  puts("push failed!\n");
-  return 0;
-}
-
 // ***************************** serial port queues *****************************
 
-#define FIFO_SIZE 0x100
-
-typedef struct uart_ring {
-  uint8_t w_ptr_tx;
-  uint8_t r_ptr_tx;
-  uint8_t elems_tx[FIFO_SIZE];
-  uint8_t w_ptr_rx;
-  uint8_t r_ptr_rx;
-  uint8_t elems_rx[FIFO_SIZE];
-  USART_TypeDef *uart;
-  void (*callback)(struct uart_ring*);
-} uart_ring;
-
-int getc(uart_ring *q, char *elem);
-int putc(uart_ring *q, char elem);
-
 // esp = USART1
 uart_ring esp_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
                        .w_ptr_rx = 0, .r_ptr_rx = 0,
@@ -113,12 +42,6 @@ uart_ring lin2_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
                         .uart = USART3 };
 
 // debug = USART2
-void debug_ring_callback(uart_ring *ring);
-uart_ring debug_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
-                         .w_ptr_rx = 0, .r_ptr_rx = 0,
-                         .uart = USART2,
-                         .callback = debug_ring_callback};
-
 
 uart_ring *get_ring_by_number(int a) {
   switch(a) {
@@ -213,36 +136,6 @@ void debug_ring_callback(uart_ring *ring) {
   }
 }
 
-// ***************************** serial port *****************************
-
-void uart_ring_process(uart_ring *q) {
-  // TODO: check if external serial is connected
-  int sr = q->uart->SR;
-
-  if (q->w_ptr_tx != q->r_ptr_tx) {
-    if (sr & USART_SR_TXE) {
-      q->uart->DR = q->elems_tx[q->r_ptr_tx];
-      q->r_ptr_tx += 1;
-    } else {
-      // push on interrupt later
-      q->uart->CR1 |= USART_CR1_TXEIE;
-    }
-  } else {
-    // nothing to send
-    q->uart->CR1 &= ~USART_CR1_TXEIE;
-  }
-
-  if (sr & USART_SR_RXNE) {
-    uint8_t c = q->uart->DR;  // TODO: can drop packets
-    uint8_t next_w_ptr = q->w_ptr_rx + 1;
-    if (next_w_ptr != q->r_ptr_rx) {
-      q->elems_rx[q->w_ptr_rx] = c;
-      q->w_ptr_rx = next_w_ptr;
-      if (q->callback) q->callback(q);
-    }
-  }
-}
-
 // interrupt boilerplate
 
 void USART1_IRQHandler(void) {
@@ -269,47 +162,12 @@ void UART5_IRQHandler(void) {
   NVIC_EnableIRQ(UART5_IRQn);
 }
 
-int getc(uart_ring *q, char *elem) {
-  if (q->w_ptr_rx != q->r_ptr_rx) {
-    *elem = q->elems_rx[q->r_ptr_rx];
-    q->r_ptr_rx += 1;
-    return 1;
-  }
-  return 0;
-}
-
-int injectc(uart_ring *q, char elem) {
-  uint8_t next_w_ptr = q->w_ptr_rx + 1;
-  int ret = 0;
-  if (next_w_ptr != q->r_ptr_rx) {
-    q->elems_rx[q->w_ptr_rx] = elem;
-    q->w_ptr_rx = next_w_ptr;
-    ret = 1;
-  }
-  return ret;
-}
-
-int putc(uart_ring *q, char elem) {
-  uint8_t next_w_ptr = q->w_ptr_tx + 1;
-  int ret = 0;
-  if (next_w_ptr != q->r_ptr_tx) {
-    q->elems_tx[q->w_ptr_tx] = elem;
-    q->w_ptr_tx = next_w_ptr;
-    ret = 1;
-  }
-  uart_ring_process(q);
-  return ret;
-}
-
 // ********************* includes *********************
-
 #include "libc.h"
 #include "gpio.h"
-#include "uart.h"
 #include "adc.h"
 #include "timer.h"
 #include "usb.h"
-#include "can.h"
 #include "spi.h"
 
 void safety_rx_hook(CAN_FIFOMailBox_TypeDef *to_push);
@@ -322,6 +180,8 @@ int safety_tx_lin_hook(int lin_num, uint8_t *data, int len, int hardwired);
 #include "honda_safety.h"
 #endif
 
+#define PANDA_CANB_RETURN_FLAG 0x80
+
 // ***************************** CAN *****************************
 
 void process_can(CAN_TypeDef *CAN, can_ring *can_q, int can_number) {
@@ -333,7 +193,11 @@ void process_can(CAN_TypeDef *CAN, can_ring *can_q, int can_number) {
   if ((CAN->TSR & CAN_TSR_TXOK0) == CAN_TSR_TXOK0) {
     CAN_FIFOMailBox_TypeDef to_push;
     to_push.RIR = CAN->sTxMailBox[0].TIR;
-    to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) | ((can_number+2) << 4);
+    to_push.RDTR = (CAN->sTxMailBox[0].TDTR & 0xFFFF000F) |
+      ((PANDA_CANB_RETURN_FLAG | (can_number & 0x7F)) << 4);
+    puts("RDTR: ");
+    puth(to_push.RDTR);
+    puts("\n");
     to_push.RDLR = CAN->sTxMailBox[0].TDLR;
     to_push.RDHR = CAN->sTxMailBox[0].TDHR;
     push(&can_rx_q, &to_push);
@@ -359,16 +223,16 @@ void process_can(CAN_TypeDef *CAN, can_ring *can_q, int can_number) {
 
 
 void CAN1_TX_IRQHandler() {
-  process_can(CAN1, &can_tx1_q, can_numbering[0]);
+  process_can(can_numbering[0], &can_tx1_q, 0);
 }
 
 void CAN2_TX_IRQHandler() {
-  process_can(CAN2, &can_tx2_q, can_numbering[1]);
+  process_can(can_numbering[1], &can_tx2_q, 1);
 }
 
-#ifdef CAN3
+#ifdef PANDA
 void CAN3_TX_IRQHandler() {
-  process_can(CAN3, &can_tx3_q, can_numbering[2]);
+  process_can(can_numbering[2], &can_tx3_q, 2);
 }
 #endif
 
@@ -377,7 +241,7 @@ void send_can(CAN_FIFOMailBox_TypeDef *to_push, int flags);
 // CAN receive handlers
 // blink blue when we are receiving CAN messages
 void can_rx(CAN_TypeDef *CAN, int can_index) {
-  int can_number = can_numbering[can_index];
+  //int can_number = can_numbering[can_index];
   while (CAN->RF0R & CAN_RF0R_FMP0) {
     // can is live
     pending_can_live = 1;
@@ -391,18 +255,18 @@ void can_rx(CAN_TypeDef *CAN, int can_index) {
 
     // forwarding (panda only)
     #ifdef PANDA
-      if (can_forwarding[can_index] != -1 && can_numbering[can_forwarding[can_index]] != -1) {
+      if (can_forwarding[can_index] != -1 && can_forwarding[can_index] != -1) {
         CAN_FIFOMailBox_TypeDef to_send;
         to_send.RIR = to_push.RIR | 1; // TXRQ
         to_send.RDTR = to_push.RDTR;
         to_send.RDLR = to_push.RDLR;
         to_send.RDHR = to_push.RDHR;
-        send_can(&to_send, can_numbering[can_forwarding[can_index]]);
+        send_can(&to_send, can_forwarding[can_index]);
       }
     #endif
 
     // modify RDTR for our API
-    to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (can_number << 4);
+    to_push.RDTR = (to_push.RDTR & 0xFFFF000F) | (can_index << 4);
 
     safety_rx_hook(&to_push);
 
@@ -491,6 +355,17 @@ void set_fan_speed(int fan_speed) {
   TIM3->CCR3 = fan_speed;
 }
 
+void usb_cb_ep0_out(uint8_t *usbdata, int len, int hardwired) {
+  if(setup.b.bRequest == 0xde){
+    puts("Setting baud rate from usb\n");
+    uint32_t bitrate = *(int*)usbdata;
+    uint16_t canb_id = setup.b.wValue.w;
+
+    can_bitrate[canb_id] = bitrate;
+    can_init(canb_id);
+  }
+}
+
 int usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired) {
   CAN_FIFOMailBox_TypeDef *reply = (CAN_FIFOMailBox_TypeDef *)usbdata;;
 
@@ -513,27 +388,30 @@ void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired) {
 
 void send_can(CAN_FIFOMailBox_TypeDef *to_push, int flags) {
   int i;
-  CAN_TypeDef *CAN;
   can_ring *can_q;
-  if (flags == can_numbering[0])  {
-    CAN = CAN1;
+  uart_ring *lin_ring;
+  CAN_TypeDef *CAN = can_numbering[flags];
+  switch(flags){
+  case 0:
     can_q = &can_tx1_q;
-  } else if (flags == can_numbering[1]) {
-    CAN = CAN2;
+    break;
+  case 1:
     can_q = &can_tx2_q;
+    break;
   #ifdef CAN3
-  } else if (flags == can_numbering[2]) {
-    CAN = CAN3;
+  case 2:
     can_q = &can_tx3_q;
+    break;
   #endif
-  } else if (flags == 8 || flags == 9) {
+  case 8:
+  case 9:
     // fake LIN as CAN
-    uart_ring *lin_ring = (flags == 8) ? &lin1_ring : &lin2_ring;
+    lin_ring = (flags == 8) ? &lin1_ring : &lin2_ring;
     for (i = 0; i < min(8, to_push->RDTR & 0xF); i++) {
       putc(lin_ring, ((uint8_t*)&to_push->RDLR)[i]);
     }
     return;
-  } else {
+  default:
     // no crash
     return;
   }
@@ -549,6 +427,7 @@ void send_can(CAN_FIFOMailBox_TypeDef *to_push, int flags) {
 
 // send on CAN
 void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired) {
+  puts("usb_cb_ep3_out called\n");
   int dpkt = 0;
   for (dpkt = 0; dpkt < len; dpkt += 0x10) {
     uint32_t *tf = (uint32_t*)(&usbdata[dpkt]);
@@ -652,10 +531,10 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
     case 0xdc: // set controls allowed
       controls_allowed = setup->b.wValue.w == 0x1337;
       // take CAN out of SILM, careful with speed!
-      can_init(CAN1, 0);
-      can_init(CAN2, 0);
+      can_init(0);
+      can_init(1);
       #ifdef CAN3
-        can_init(CAN3, 0);
+      can_init(2);
       #endif
       break;
     case 0xdd: // enable can forwarding
@@ -666,6 +545,27 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
         }
       }
       break;
+    case 0xde: // Set Can bitrate
+      puts("Set can bitrate\n");
+      if (!(setup->b.wValue.w < CAN_MAX && setup->b.wLength.w == 4)) {
+	return -1;
+      }
+      break;
+    case 0xdf: // Set Can bitrate
+      puts("Get can bitrate\n");
+      if (setup->b.wValue.w < CAN_MAX) {
+	//TODO: Make fail if asking for can3 and no can3
+	puts("Canid: ");
+	puth(setup->b.wValue.w);
+	puts(" bitrate: ");
+	puth(can_bitrate[setup->b.wValue.w]);
+	puts("\n");
+	memcpy(resp, (void *)&can_bitrate[setup->b.wValue.w], 4);
+	resp_len = 4;
+      }else{
+	return -1;
+      }
+      break;
     case 0xe0: // uart read
       ur = get_ring_by_number(setup->b.wValue.w);
       if (!ur) break;
@@ -751,7 +651,7 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) {
       puts("NO HANDLER ");
       puth(setup->b.bRequest);
       puts("\n");
-      break;
+      return -1;
   }
   return resp_len;
 }
@@ -867,27 +767,24 @@ int main() {
   uart_init(USART3, 10400);
   USART3->CR2 |= USART_CR2_LINEN;
 
-  /*puts("EXTERNAL");
-  puth(has_external_debug_serial);
-  puts("\n");*/
-
   // enable USB
   usb_init();
 
   // default to silent mode to prevent issues with Ford
 #ifdef PANDA_SAFETY
-  can_init(CAN1, 1);
-  can_init(CAN2, 1);
-  #ifdef CAN3
-    can_init(CAN3, 1);
-  #endif
+  controls_allowed = 0;
 #else
-  can_init(CAN1, 0);
-  can_init(CAN2, 0);
-  #ifdef CAN3
-    can_init(CAN3, 0);
-  #endif
+  controls_allowed = 1;
 #endif
+  puts("Can0 default bitrate ");
+  puth(can_bitrate[0]);
+  puts("\n");
+
+  can_init(0);
+  can_init(1);
+  #ifdef CAN3
+    can_init(2);
+  #endif
 
   adc_init();
 
diff --git a/board/spi_flasher.h b/board/spi_flasher.h
index a26a13c1b..ce92014f9 100644
--- a/board/spi_flasher.h
+++ b/board/spi_flasher.h
@@ -1,3 +1,5 @@
+#include "libc.h"
+
 /*void lock_bootloader() {
   if (FLASH->OPTCR & FLASH_OPTCR_nWRP_0) {
     FLASH->OPTKEYR = 0x08192A3B;
diff --git a/board/uart.c b/board/uart.c
new file mode 100644
index 000000000..9f4879933
--- /dev/null
+++ b/board/uart.c
@@ -0,0 +1,135 @@
+#include <stdint.h>
+#include "config.h"
+#include "uart.h"
+#include "early.h"
+
+uart_ring debug_ring = { .w_ptr_tx = 0, .r_ptr_tx = 0,
+                         .w_ptr_rx = 0, .r_ptr_rx = 0,
+                         .uart = USART2,
+                         .callback = debug_ring_callback};
+
+
+void uart_set_baud(USART_TypeDef *u, int baud) {
+  if (u == USART1) {
+    // USART1 is on APB2
+    u->BRR = __USART_BRR(48000000, baud);
+  } else {
+    u->BRR = __USART_BRR(24000000, baud);
+  }
+}
+
+void uart_init(USART_TypeDef *u, int baud) {
+  // enable uart and tx+rx mode
+  u->CR1 = USART_CR1_UE;
+  uart_set_baud(u, baud);
+
+  u->CR1 |= USART_CR1_TE | USART_CR1_RE;
+  //u->CR2 = USART_CR2_STOP_0 | USART_CR2_STOP_1;
+  //u->CR2 = USART_CR2_STOP_0;
+  // ** UART is ready to work **
+
+  // enable interrupts
+  u->CR1 |= USART_CR1_RXNEIE;
+}
+
+void putch(const char a) {
+  if (has_external_debug_serial) {
+    putc(&debug_ring, a);
+  } else {
+    injectc(&debug_ring, a);
+  }
+}
+
+int puts(const char *a) {
+  for (;*a;a++) {
+    if (*a == '\n') putch('\r');
+    putch(*a);
+  }
+  return 0;
+}
+
+void puth(unsigned int i) {
+  int pos;
+  char c[] = "0123456789abcdef";
+  for (pos = 28; pos != -4; pos -= 4) {
+    putch(c[(i >> pos) & 0xF]);
+  }
+}
+
+void puth2(unsigned int i) {
+  int pos;
+  char c[] = "0123456789abcdef";
+  for (pos = 4; pos != -4; pos -= 4) {
+    putch(c[(i >> pos) & 0xF]);
+  }
+}
+
+void hexdump(void *a, int l) {
+  int i;
+  for (i=0;i<l;i++) {
+    if (i != 0 && (i&0xf) == 0) puts("\n");
+    puth2(((unsigned char*)a)[i]);
+    puts(" ");
+  }
+  puts("\n");
+}
+
+int getc(uart_ring *q, char *elem) {
+  if (q->w_ptr_rx != q->r_ptr_rx) {
+    *elem = q->elems_rx[q->r_ptr_rx];
+    q->r_ptr_rx += 1;
+    return 1;
+  }
+  return 0;
+}
+
+int putc(uart_ring *q, char elem) {
+  uint8_t next_w_ptr = q->w_ptr_tx + 1;
+  int ret = 0;
+  if (next_w_ptr != q->r_ptr_tx) {
+    q->elems_tx[q->w_ptr_tx] = elem;
+    q->w_ptr_tx = next_w_ptr;
+    ret = 1;
+  }
+  uart_ring_process(q);
+  return ret;
+}
+
+int injectc(uart_ring *q, char elem) {
+  uint8_t next_w_ptr = q->w_ptr_rx + 1;
+  int ret = 0;
+  if (next_w_ptr != q->r_ptr_rx) {
+    q->elems_rx[q->w_ptr_rx] = elem;
+    q->w_ptr_rx = next_w_ptr;
+    ret = 1;
+  }
+  return ret;
+}
+
+void uart_ring_process(uart_ring *q) {
+  // TODO: check if external serial is connected
+  int sr = q->uart->SR;
+
+  if (q->w_ptr_tx != q->r_ptr_tx) {
+    if (sr & USART_SR_TXE) {
+      q->uart->DR = q->elems_tx[q->r_ptr_tx];
+      q->r_ptr_tx += 1;
+    } else {
+      // push on interrupt later
+      q->uart->CR1 |= USART_CR1_TXEIE;
+    }
+  } else {
+    // nothing to send
+    q->uart->CR1 &= ~USART_CR1_TXEIE;
+  }
+
+  if (sr & USART_SR_RXNE) {
+    uint8_t c = q->uart->DR;  // TODO: can drop packets
+    uint8_t next_w_ptr = q->w_ptr_rx + 1;
+    if (next_w_ptr != q->r_ptr_rx) {
+      q->elems_rx[q->w_ptr_rx] = c;
+      q->w_ptr_rx = next_w_ptr;
+      if (q->callback) q->callback(q);
+    }
+  }
+}
diff --git a/board/uart.h b/board/uart.h
index 0c062aa3c..b6bdb2808 100644
--- a/board/uart.h
+++ b/board/uart.h
@@ -1,70 +1,48 @@
-#define __DIV(_PCLK_, _BAUD_)                        (((_PCLK_)*25)/(4*(_BAUD_)))
-#define __DIVMANT(_PCLK_, _BAUD_)                    (__DIV((_PCLK_), (_BAUD_))/100)
-#define __DIVFRAQ(_PCLK_, _BAUD_)                    (((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16 + 50) / 100)
-#define __USART_BRR(_PCLK_, _BAUD_)              ((__DIVMANT((_PCLK_), (_BAUD_)) << 4)|(__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F))
+#ifndef PANDA_UART_H
+#define PANDA_UART_H
 
-void uart_set_baud(USART_TypeDef *u, int baud) {
-  if (u == USART1) {
-    // USART1 is on APB2
-    u->BRR = __USART_BRR(48000000, baud);
-  } else {
-    u->BRR = __USART_BRR(24000000, baud);
-  }
-}
+#define __DIV(_PCLK_, _BAUD_)       (((_PCLK_)*25)/(4*(_BAUD_)))
+#define __DIVMANT(_PCLK_, _BAUD_)   (__DIV((_PCLK_), (_BAUD_))/100)
+#define __DIVFRAQ(_PCLK_, _BAUD_)   (((__DIV((_PCLK_), (_BAUD_)) - (__DIVMANT((_PCLK_), (_BAUD_)) * 100)) * 16 + 50) / 100)
+#define __USART_BRR(_PCLK_, _BAUD_) ((__DIVMANT((_PCLK_), (_BAUD_)) << 4)|(__DIVFRAQ((_PCLK_), (_BAUD_)) & 0x0F))
 
-void uart_init(USART_TypeDef *u, int baud) {
-  // enable uart and tx+rx mode
-  u->CR1 = USART_CR1_UE;
-  uart_set_baud(u, baud);
-  
-  u->CR1 |= USART_CR1_TE | USART_CR1_RE;
-  //u->CR2 = USART_CR2_STOP_0 | USART_CR2_STOP_1;
-  //u->CR2 = USART_CR2_STOP_0;
-  // ** UART is ready to work **
+typedef struct uart_ring {
+  uint8_t w_ptr_tx;
+  uint8_t r_ptr_tx;
+  uint8_t elems_tx[FIFO_SIZE];
+  uint8_t w_ptr_rx;
+  uint8_t r_ptr_rx;
+  uint8_t elems_rx[FIFO_SIZE];
+  USART_TypeDef *uart;
+  void (*callback)(struct uart_ring*);
+} uart_ring;
 
-  // enable interrupts
-  u->CR1 |= USART_CR1_RXNEIE;
-}
+extern int has_external_debug_serial;
 
-void putch(const char a) {
-  if (has_external_debug_serial) {
-    putc(&debug_ring, a);
-  } else {
-    injectc(&debug_ring, a);
-  }
-}
+extern uart_ring debug_ring;
 
-int puts(const char *a) {
-  for (;*a;a++) {
-    if (*a == '\n') putch('\r');
-    putch(*a);
-  }
-  return 0;
-}
+void debug_ring_callback(uart_ring *ring);
 
-void puth(unsigned int i) {
-  int pos;
-  char c[] = "0123456789abcdef";
-  for (pos = 28; pos != -4; pos -= 4) {
-    putch(c[(i >> pos) & 0xF]);
-  }
-}
+void uart_set_baud(USART_TypeDef *u, int baud);
 
-void puth2(unsigned int i) {
-  int pos;
-  char c[] = "0123456789abcdef";
-  for (pos = 4; pos != -4; pos -= 4) {
-    putch(c[(i >> pos) & 0xF]);
-  }
-}
+void uart_init(USART_TypeDef *u, int baud);
 
-void hexdump(void *a, int l) {
-  int i;
-  for (i=0;i<l;i++) {
-    if (i != 0 && (i&0xf) == 0) puts("\n");
-    puth2(((unsigned char*)a)[i]);
-    puts(" ");
-  }
-  puts("\n");
-}
+void putch(const char a);
 
+int puts(const char *a);
+
+void puth(unsigned int i);
+
+void puth2(unsigned int i);
+
+void hexdump(void *a, int l);
+
+int getc(uart_ring *q, char *elem);
+
+int putc(uart_ring *q, char elem);
+
+int injectc(uart_ring *q, char elem);
+
+void uart_ring_process(uart_ring *q);
+
+#endif
diff --git a/board/usb.h b/board/usb.h
index c8f7689a6..c7f668b84 100644
--- a/board/usb.h
+++ b/board/usb.h
@@ -80,6 +80,7 @@ USB_Setup_TypeDef;
 // interfaces
 void usb_cb_enumeration_complete();
 int  usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *usbdata, int hardwired);
+void usb_cb_ep0_out(uint8_t *usbdata, int len, int hardwired);
 int  usb_cb_ep1_in(uint8_t *usbdata, int len, int hardwired);
 void usb_cb_ep2_out(uint8_t *usbdata, int len, int hardwired);
 void usb_cb_ep3_out(uint8_t *usbdata, int len, int hardwired);
@@ -221,8 +222,8 @@ void *USB_ReadPacket(void *dest, uint16_t len) {
 
 void USB_WritePacket(const uint8_t *src, uint16_t len, uint32_t ep) {
   #ifdef DEBUG_USB
-  puts("writing ");
-  hexdump(src, len);
+  //puts("writing ");
+  //hexdump(src, len);
   #endif
 
   uint8_t numpacket = (len+(MAX_RESP_LEN-1))/MAX_RESP_LEN;
@@ -405,11 +406,19 @@ void usb_setup() {
       current_int0_alt_setting = setup.b.wValue.w;
       USB_WritePacket(0, 0, 0);
       USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
+      //USBx_INEP(0)->DIEPINT = USBx_INEP(0)->DIEPINT
+      puts("Setting Interface Alt: ");
+      puth(current_int0_alt_setting);
+      puts("\n");
       break;
     default:
       resp_len = usb_cb_control_msg(&setup, resp, 1);
-      USB_WritePacket(resp, min(resp_len, setup.b.wLength.w), 0);
-      USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
+      if(resp_len == -1){
+	USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_STALL;
+      }else{
+	USB_WritePacket(resp, min(resp_len, setup.b.wLength.w), 0);
+	USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
+      }
   }
 }
 
@@ -470,10 +479,11 @@ void usb_init() {
   // 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;
+  //SRQ is when a 'session' starts.
+  USBx->GINTMSK = USB_OTG_GINTMSK_SRQIM    | USB_OTG_GINTMSK_CIDSCHGM  | USB_OTG_GINTMSK_OEPINT    |
+                  USB_OTG_GINTMSK_IEPINT   | USB_OTG_GINTMSK_ENUMDNEM  | USB_OTG_GINTMSK_USBRST    |
+                  USB_OTG_GINTMSK_USBSUSPM | USB_OTG_GINTMSK_GONAKEFFM | USB_OTG_GINTMSK_GINAKEFFM |
+                  USB_OTG_GINTMSK_RXFLVLM  | USB_OTG_GINTMSK_OTGINT    | USB_OTG_GINTMSK_MMISM;
 
   USBx->GAHBCFG = USB_OTG_GAHBCFG_GINT;
 
@@ -483,7 +493,7 @@ void usb_init() {
 }
 
 // ***************************** USB port *****************************
-
+int dumb = 0;
 void usb_irqhandler(void) {
   //USBx->GINTMSK = 0;
 
@@ -558,6 +568,10 @@ void usb_irqhandler(void) {
         hexdump(&usbdata, len);
       #endif
 
+      if(endpoint == 0){
+	usb_cb_ep0_out(usbdata, len, 1);
+      }
+
       if (endpoint == 2) {
         usb_cb_ep2_out(usbdata, len, 1);
       }
@@ -690,6 +704,12 @@ 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");
+
         #ifdef DEBUG_USB
         puts("  IN PACKET QUEUE\n");
         #endif