diff --git a/board/drivers/can.h b/board/drivers/can.h
index a91b656ede5910..cd809704b9a407 100644
--- a/board/drivers/can.h
+++ b/board/drivers/can.h
@@ -461,84 +461,18 @@ void CAN3_SCE_IRQHandler() { can_sce(CAN3); }
 
 #endif
 
-void set_bitbanged_gmlan(int val) {
-  if (val) {
-    GPIOB->ODR |= (1 << 13);
-  } else {
-    GPIOB->ODR &= ~(1 << 13);
-  }
-}
-
-void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
-  puts("called bitbang_gmlan\n");
-  puth(can_num_lookup[3]);
-  puts("\n");
-  char test_pkt[] = \
-    {0, // SOF
-     0,0,0,0, // ID10-ID7
-     1, // bitstuff
-     0,0,1,0,1,0,0,   // ID6-ID0
-     0, // RTR
-     0, // IDE
-     0, // reserved
-     1, // bitstuff
-     0,0,0,1, // len
-     0,0,0,0,0, // 1st byte 7-3
-     1, // bitstuff
-     0,0,1,     // 1st byte 2-0
-
-     // start CRC
-     /*0,1,0,0,0,0,1,1,0,0,0,0,0,
-     1, // bitstuff
-     0,0, // CRC field*/
-
-     // CRC from real message
-     1,1,1,0,1,1,1,0,1,0,1,0,0,1,1,1,
-
-     1,  // CRC delimiter
-
-     1,  // ACK
-     1,  // ACK delimiter
-     1,1,1,1,1,1,1, // EOF
-     1,1,1, // IFS
-     };
-  /*char test_pkt[] = {
-  0,1,0,1, 0,1,0,1, 0,1,0,1,
-  1,1,1,1,
-  0,1,0,1, 0,1,0,1, 0,1,0,1,
-  0,0,0,0,0,0,0,0,
-  0,1,0,1, 0,1,0,1, 0,1,0,1,
-  0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
-  0,1,0,1, 0,1,0,1, 0,1,0,1,};*/
-
-  #define SPEEED 30
-
-  // bitbang loop
-  set_bitbanged_gmlan(1); // recessive
-  set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
-  enter_critical_section();
-  int init = TIM2->CNT;
-  for (int i = 0; i < sizeof(test_pkt); i++) {
-    while ((TIM2->CNT - init) < (SPEEED*i));
-    set_bitbanged_gmlan(test_pkt[i]);
-  }
-  exit_critical_section();
-  set_gpio_mode(GPIOB, 13, MODE_INPUT);
-
-  puts("bitbang done\n");
-}
+#include "canbitbang.h"
 
 void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number) {
   if (safety_tx_hook(to_push) && !can_autobaud_enabled[bus_number]) {
-    //if (bus_number == 3) { //&& can_num_lookup[3] == -1) {
-    // TODO: why uint8 bro? only int8?
-    if (bus_number == 3 && can_num_lookup[3] == 0xFF) {
-      bitbang_gmlan(to_push);
-    } else {
-      if (bus_number < BUS_MAX) {
-        // add CAN packet to send queue
-        // bus number isn't passed through
-        to_push->RDTR &= 0xF;
+    if (bus_number < BUS_MAX) {
+      // add CAN packet to send queue
+      // bus number isn't passed through
+      to_push->RDTR &= 0xF;
+      if (bus_number == 3 && can_num_lookup[3] == 0xFF) {
+        // TODO: why uint8 bro? only int8?
+        bitbang_gmlan(to_push);
+      } else {
         can_push(can_queues[bus_number], to_push);
         process_can(CAN_NUM_FROM_BUS_NUM(bus_number));
       }
diff --git a/board/drivers/canbitbang.h b/board/drivers/canbitbang.h
new file mode 100644
index 00000000000000..9732adf597b195
--- /dev/null
+++ b/board/drivers/canbitbang.h
@@ -0,0 +1,109 @@
+
+// returns out_len
+int do_bitstuff(char *out, char *in, int in_len) {
+  int last_bit = -1;
+  int bit_cnt = 0;
+  int j = 0;
+  for (int i = 0; i < in_len; i++) {
+    char bit = in[i];
+    out[j++] = bit;
+
+    // do the stuffing
+    if (bit == last_bit) {
+      bit_cnt++;
+      if (bit_cnt == 5) {
+        // 5 in a row the same, do stuff
+        last_bit = !bit;
+        out[j++] = last_bit;
+        bit_cnt = 1;
+      }
+    } else {
+      // this is a new bit
+      last_bit = bit;
+      bit_cnt = 1;
+    }
+  }
+  return j;
+}
+
+int get_bit_message(char *out) {
+  char test_pkt[] = {
+    0, // SOF
+    0,0,0,0, // ID10-ID7
+    //1, // bitstuff
+    0,0,1,0,1,0,0,   // ID6-ID0
+    0, // RTR
+    0, // IDE
+    0, // reserved
+    //1, // bitstuff
+    0,0,0,1, // len
+    0,0,0,0,0, // 1st byte 7-3
+    //1, // bitstuff
+    0,0,1,     // 1st byte 2-0
+
+    // CRC from real message
+    1,1,1,0,1,1,1,0,1,0,1,0,0,1,1,
+  };
+  char footer[] = {
+    1,  // CRC delimiter
+
+    1,  // ACK
+    1,  // ACK delimiter
+    1,1,1,1,1,1,1, // EOF
+    1,1,1, // IFS
+  };
+  #define SPEEED 30
+
+  // testing
+  //char *test_pkt_stuffed = test_pkt;
+  //int len = sizeof(test_pkt);
+
+  // do bitstuffing
+  int len = do_bitstuff(out, test_pkt, sizeof(test_pkt));
+
+  // append footer
+  for (int i = 0; i < sizeof(footer); i++) {
+    out[len++] = footer[i];
+  }
+  return len;
+}
+
+// hardware stuff below this line
+
+#ifdef PANDA
+
+void set_bitbanged_gmlan(int val) {
+  if (val) {
+    GPIOB->ODR |= (1 << 13);
+  } else {
+    GPIOB->ODR &= ~(1 << 13);
+  }
+}
+
+void bitbang_gmlan(CAN_FIFOMailBox_TypeDef *to_bang) {
+  puts("called bitbang_gmlan\n");
+  puth(can_num_lookup[3]);
+  puts("\n");
+
+  char pkt_stuffed[64+44+25];
+  int len = get_bit_message(pkt_stuffed);
+
+  // actual bitbang loop
+  set_bitbanged_gmlan(1); // recessive
+  set_gpio_mode(GPIOB, 13, MODE_OUTPUT);
+  enter_critical_section();
+  int init = TIM2->CNT;
+
+  for (int i = 0; i < len; i++) {
+    while ((TIM2->CNT - init) < (SPEEED*i));
+    set_bitbanged_gmlan(pkt_stuffed[i]);
+  }
+
+  exit_critical_section();
+  set_gpio_mode(GPIOB, 13, MODE_INPUT);
+
+  puts("bitbang done\n");
+}
+
+#endif
+
diff --git a/tests/gmbitbang/rigol.py b/tests/gmbitbang/rigol.py
index 8ae6e7ea34296c..3d690fdd84c725 100755
--- a/tests/gmbitbang/rigol.py
+++ b/tests/gmbitbang/rigol.py
@@ -19,8 +19,8 @@
 data = np.frombuffer(rawdata, 'B')
 print data.shape
 
-s1 = data[0:600]
-s2 = data[600:]
+s1 = data[0:650]
+s2 = data[650:]
 s1i = np.argmax(s1 > 100)
 s2i = np.argmax(s2 > 100)
 s1 = s1[s1i:]
diff --git a/tests/gmbitbang/test_packer.c b/tests/gmbitbang/test_packer.c
new file mode 100644
index 00000000000000..0f4386fab278ca
--- /dev/null
+++ b/tests/gmbitbang/test_packer.c
@@ -0,0 +1,15 @@
+#include <stdio.h>
+#include "../../board/drivers/canbitbang.h"
+
+int main() {
+  char out[100];
+  int len = get_bit_message(out);
+  for (int i = 0; i < len; i++) {
+    printf("%d", out[i]);
+  }
+  printf("\n");
+  return 0;
+}
+
+
+