CAN FD non-ISO support

commaai · Oct 1, 2022 · 52423a7 · 52423a7
1 parent 1910db8
commit 52423a7
Show file tree

Hide file tree

Showing 6 changed files with 52 additions and 5 deletions.
diff --git a/board/drivers/can_common.h b/board/drivers/can_common.h
@@ -12,6 +12,7 @@ typedef struct {
   uint32_t can_data_speed;
   bool canfd_enabled;
   bool brs_enabled;
+  bool canfd_non_iso;
 } bus_config_t;
 
 uint32_t safety_tx_blocked = 0;
@@ -155,10 +156,10 @@ void can_clear(can_ring *q) {
 // Helpers
 // Panda:       Bus 0=CAN1   Bus 1=CAN2   Bus 2=CAN3
 bus_config_t bus_config[] = {
-  { .bus_lookup = 0U, .can_num_lookup = 0U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false },
-  { .bus_lookup = 1U, .can_num_lookup = 1U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false },
-  { .bus_lookup = 2U, .can_num_lookup = 2U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false },
-  { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false },
+  { .bus_lookup = 0U, .can_num_lookup = 0U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 1U, .can_num_lookup = 1U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 2U, .can_num_lookup = 2U, .can_speed = 5000U, .can_data_speed = 20000U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
+  { .bus_lookup = 0xFFU, .can_num_lookup = 0xFFU, .can_speed = 333U, .can_data_speed = 333U, .canfd_enabled = false, .brs_enabled = false, .canfd_non_iso = false },
 };
 
 #define CANIF_FROM_CAN_NUM(num) (cans[num])

diff --git a/board/drivers/fdcan.h b/board/drivers/fdcan.h
@@ -26,6 +26,18 @@ bool can_set_speed(uint8_t can_number) {
   return ret;
 }
 
+bool can_set_non_iso(uint8_t can_number) {
+  bool ret = true;
+  FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
+  uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number);
+
+  ret &= llcan_set_non_iso(
+    CANx,
+    bus_config[bus_number].canfd_non_iso
+  );
+  return ret;
+}
+
 void can_set_gmlan(uint8_t bus) {
   UNUSED(bus);
   puts("GMLAN not available on red panda\n");
@@ -226,6 +238,7 @@ bool can_init(uint8_t can_number) {
   if (can_number != 0xffU) {
     FDCAN_GlobalTypeDef *CANx = CANIF_FROM_CAN_NUM(can_number);
     ret &= can_set_speed(can_number);
+    ret &= can_set_non_iso(can_number);
     ret &= llcan_init(CANx);
     // in case there are queued up messages
     process_can(can_number);

diff --git a/board/health.h b/board/health.h
@@ -49,4 +49,5 @@ typedef struct __attribute__((packed)) {
   uint16_t can_data_speed;
   uint8_t canfd_enabled;
   uint8_t brs_enabled;
+  uint8_t canfd_non_iso;
 } can_health_t;
diff --git a/board/main_comms.h b/board/main_comms.h
@@ -253,6 +253,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
         can_health[req->param1].can_data_speed = (bus_config[req->param1].can_data_speed / 10U);
         can_health[req->param1].canfd_enabled = bus_config[req->param1].canfd_enabled;
         can_health[req->param1].brs_enabled = bus_config[req->param1].brs_enabled;
+        can_health[req->param1].canfd_non_iso = bus_config[req->param1].canfd_non_iso;
         resp_len = sizeof(can_health[req->param1]);
         (void)memcpy(resp, &can_health[req->param1], resp_len);
       }
@@ -533,7 +534,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
         heartbeat_disabled = true;
       }
       break;
-    // **** 0xde: set CAN FD data bitrate
+    // **** 0xf9: set CAN FD data bitrate
     case 0xf9:
       if ((req->param1 < CAN_CNT) &&
            current_board->has_canfd &&
@@ -557,6 +558,14 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
     case 0xfb:
       deepsleep_allowed = true;
       break;
+    // **** 0xfc: set CAN FD non-ISO mode
+    case 0xfc:
+      if ((req->param1 < CAN_CNT) && current_board->has_canfd) {
+        bus_config[req->param1].canfd_non_iso = (req->param2 != 0U);
+        bool ret = can_init(CAN_NUM_FROM_BUS_NUM(req->param1));
+        UNUSED(ret);
+      }
+      break;
     default:
       puts("NO HANDLER ");
       puth(req->request);

diff --git a/board/stm32h7/llfdcan.h b/board/stm32h7/llfdcan.h
@@ -150,6 +150,25 @@ bool llcan_set_speed(FDCAN_GlobalTypeDef *CANx, uint32_t speed, uint32_t data_sp
   return ret;
 }
 
+bool llcan_set_non_iso(FDCAN_GlobalTypeDef *CANx, bool non_iso) {
+  bool ret = fdcan_request_init(CANx);
+
+  if (ret) {
+    if (non_iso) {
+      CANx->CCCR |= FDCAN_CCCR_NISO;
+    } else {
+      CANx->CCCR &= ~(FDCAN_CCCR_NISO);
+    }
+    ret = fdcan_exit_init(CANx);
+    if (!ret) {
+      puts(CAN_NAME_FROM_CANIF(CANx)); puts(" set_non_iso timed out! (2)\n");
+    }
+  } else {
+    puts(CAN_NAME_FROM_CANIF(CANx)); puts(" set_non_iso timed out! (1)\n");
+  }
+  return ret;
+}
+
 bool llcan_init(FDCAN_GlobalTypeDef *CANx) {
   uint32_t can_number = CAN_NUM_FROM_CANIF(CANx);
   bool ret = fdcan_request_init(CANx);

diff --git a/python/__init__.py b/python/__init__.py
@@ -507,6 +507,7 @@ def can_health(self, can_number):
       "can_data_speed": a[17],
       "canfd_enabled": a[18],
       "brs_enabled": a[19],
+      "canfd_non_iso": a[20],
     }
 
   # ******************* control *******************
@@ -633,6 +634,9 @@ def get_canfd_status(self, bus):
     else:
       return (None, None)
 
+  def set_canfd_non_iso(self, bus, non_iso):
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xfc, bus, int(non_iso), b'')
+
   def set_uart_baud(self, uart, rate):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xe4, uart, int(rate / 300), b'')