From 83f2edfb7811259c1d11b123ab7ff6a02adf4ae1 Mon Sep 17 00:00:00 2001
From: George Hotz <george@comma.ai>
Date: Sat, 10 Mar 2018 11:06:40 -0800
Subject: [PATCH] isotp can support in softloader

---
 board/pedal/main.c  |  6 ---
 board/spi_flasher.h | 98 ++++++++++++++++++++++++++++++++++++++++-----
 2 files changed, 89 insertions(+), 15 deletions(-)

diff --git a/board/pedal/main.c b/board/pedal/main.c
index 6f6055271504bc..f57c5d498f6832 100644
--- a/board/pedal/main.c
+++ b/board/pedal/main.c
@@ -268,12 +268,6 @@ int main() {
 
   // 48mhz / 65536 ~= 732
   timer_init(TIM3, 15);
-
-  // needed?
-  NVIC_EnableIRQ(CAN1_TX_IRQn);
-  NVIC_EnableIRQ(CAN1_RX0_IRQn);
-  NVIC_EnableIRQ(CAN1_SCE_IRQn);
-
   NVIC_EnableIRQ(TIM3_IRQn);
 
   // setup watchdog
diff --git a/board/spi_flasher.h b/board/spi_flasher.h
index 4f2b07e2459b57..e2583370d6f81e 100644
--- a/board/spi_flasher.h
+++ b/board/spi_flasher.h
@@ -142,14 +142,99 @@ void CAN1_TX_IRQHandler() {
   CAN->TSR |= CAN_TSR_RQCP0;
 }
 
+#define ISOTP_BUF_SIZE 0x110
+
+uint8_t isotp_buf[ISOTP_BUF_SIZE];
+uint8_t *isotp_buf_ptr = NULL;
+int isotp_buf_remain = 0;
+
+uint8_t isotp_buf_out[ISOTP_BUF_SIZE];
+uint8_t *isotp_buf_out_ptr = NULL;
+int isotp_buf_out_remain = 0;
+int isotp_buf_out_idx = 0;
+
+void bl_can_send(uint8_t *odat) {
+  // wait for send
+  while (!(CAN->TSR & CAN_TSR_TME0));
+
+  // send continue
+  CAN->sTxMailBox[0].TDLR = ((uint32_t*)odat)[0];
+  CAN->sTxMailBox[0].TDHR = ((uint32_t*)odat)[1];
+  CAN->sTxMailBox[0].TDTR = 8;
+  CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
+}
+
 void CAN1_RX0_IRQHandler() {
   while (CAN->RF0R & CAN_RF0R_FMP0) {
     if ((CAN->sFIFOMailBox[0].RIR>>21) == CAN_BL_INPUT) {
-      CAN->sTxMailBox[0].TDLR = 0xAABBCCDD;
-      CAN->sTxMailBox[0].TDHR = 0xAABBCCDD;
-      CAN->sTxMailBox[0].TDTR = 8;
-      CAN->sTxMailBox[0].TIR = (CAN_BL_OUTPUT << 21) | 1;
+      uint8_t dat[8];
+      ((uint32_t*)dat)[0] = CAN->sFIFOMailBox[0].RDLR;
+      ((uint32_t*)dat)[1] = CAN->sFIFOMailBox[0].RDHR;
+      uint8_t odat[8];
+      uint8_t type = dat[0] & 0xF0;
+      if (type == 0x30) {
+        // continue
+        while (isotp_buf_out_remain > 0) {
+          // wait for send
+          while (!(CAN->TSR & CAN_TSR_TME0));
+
+          odat[0] = 0x20 | isotp_buf_out_idx;
+          memcpy(odat+1, isotp_buf_out_ptr, 7);
+          isotp_buf_out_remain -= 7;
+          isotp_buf_out_ptr += 7;
+          isotp_buf_out_idx++;
+
+          bl_can_send(odat);
+        }
+      } else if (type == 0x20) {
+        if (isotp_buf_remain > 0) {
+          memcpy(isotp_buf_ptr, dat, 7);
+          isotp_buf_ptr += 7;
+          isotp_buf_remain -= 7;
+        }
+        if (isotp_buf_remain <= 0) {
+          int len = isotp_buf_ptr - isotp_buf + isotp_buf_remain;
+
+          // call the function
+          memset(isotp_buf_out, 0, ISOTP_BUF_SIZE);
+          isotp_buf_out_remain = spi_cb_rx(isotp_buf, len, isotp_buf_out);
+          isotp_buf_out_ptr = isotp_buf_out;
+          isotp_buf_out_idx = 0;
+
+          // send initial
+          if (isotp_buf_out_remain <= 7) {
+            odat[0] = isotp_buf_out_remain;
+            //memcpy(odat+1, isotp_buf_out_ptr, isotp_buf_out_remain);
+          } else {
+            odat[0] = 0x10 | (isotp_buf_out_remain>>8);
+            odat[1] = isotp_buf_out_remain & 0xFF;
+            //memcpy(odat+2, isotp_buf_out_ptr, 6);
+            isotp_buf_out_remain -= 6;
+            isotp_buf_out_ptr += 6;
+            isotp_buf_out_idx++;
+          }
+
+          bl_can_send(odat);
+        }
+      } else if (type == 0x10) {
+        int len = ((dat[0]&0xF)<<8) | dat[1];
+
+        // setup buffer
+        isotp_buf_ptr = isotp_buf;
+        memcpy(isotp_buf_ptr, dat+2, 6);
+
+        if (len < (ISOTP_BUF_SIZE-0x10)) {
+          isotp_buf_ptr += 6;
+          isotp_buf_remain = len-6;
+        }
+
+        memset(odat, 0, 8);
+        odat[0] = 0x30;
+        bl_can_send(odat);
+      }
     }
+    // next
+    CAN->RF0R |= CAN_RF0R_RFOM0;
   }
 }
 
@@ -181,11 +266,6 @@ void soft_flasher_start() {
   // init can
   can_silent = ALL_CAN_LIVE;
   can_init(0);
-
-  // needed?
-  NVIC_EnableIRQ(CAN1_TX_IRQn);
-  NVIC_EnableIRQ(CAN1_RX0_IRQn);
-  NVIC_EnableIRQ(CAN1_SCE_IRQn);
 #endif
 
   // A4,A5,A6,A7: setup SPI