Support CAN FD for STM32 targets

drivers/include/drivers/CAN.h

@@ -94,8 +94,9 @@ class CAN
       * @param rd the read pin
       * @param td the transmit pin
       * @param hz the bus frequency in hertz
+      * @param data_hz the data frequency in hertz(CAN FD only)
       */
-    CAN(PinName rd, PinName td, int hz);
+    CAN(PinName rd, PinName td, int hz, int data_hz = 0);
 
     /** Initialize CAN interface
       *
@@ -108,21 +109,23 @@ class CAN
       *
       * @param pinmap reference to structure which holds static pinmap
       * @param hz the bus frequency in hertz
+      * @param data_hz the data frequency in hertz(CAN FD only)
       */
-    CAN(const can_pinmap_t &pinmap, int hz);
+    CAN(const can_pinmap_t &pinmap, int hz, int data_hz = 0);
     CAN(const can_pinmap_t &&, int) = delete; // prevent passing of temporary objects
 
     virtual ~CAN();
 
     /** Set the frequency of the CAN interface
      *
      *  @param hz The bus frequency in hertz
+      * @param data_hz the data frequency in hertz(CAN FD only)
      *
      *  @returns
      *    1 if successful,
      *    0 otherwise
      */
-    int frequency(int hz);
+    int frequency(int hz, int data_hz = 0);
 
     /** Write a CANMessage to the bus.
      *
@@ -145,6 +148,31 @@ class CAN
      */
     int read(CANMessage &msg, int handle = 0);
 
+#if DEVICE_CAN_FD
+
+    /** Write a CANFDMessage to the bus.
+     *
+     *  @param msg The CANFDMessage to write.
+     *
+     *  @returns
+     *    0 if write failed,
+     *    1 if write was successful
+     */
+    int write(CANFDMessage msg);
+
+    /** Read a CANFDMessage from the bus.
+     *
+     *  @param msg A CANFDMessage to read to.
+     *  @param handle message filter handle (0 for any message)
+     *
+     *  @returns
+     *    0 if no message arrived,
+     *    1 if message arrived
+     */
+    int read(CANFDMessage &msg, int handle = 0);
+
+#endif
+
     /** Reset CAN interface.
      *
      * To use after error overflow.

drivers/include/drivers/interfaces/InterfaceCAN.h

@@ -140,6 +140,107 @@ class CANMessage : public CAN_Message {
     }
 };
 
+#if DEVICE_CAN_FD
+
+/** CANFDMessage class
+ *
+ * @note Synchronization level: Thread safe
+ */
+class CANFDMessage : public CANFD_Message {
+
+public:
+    /** Creates empty CANFD message.
+     */
+    CANFDMessage() : CANFD_Message()
+    {
+        len    = 64U;
+        type   = CANData;
+        format = CANStandard;
+        id     = 0U;
+        memset(data, 0, 64);
+    }
+
+    /** Creates CANFD message with specific content.
+     *
+     *  @param _id      Message ID
+     *  @param _data    Mesaage Data
+     *  @param _len     Message Data length
+     *  @param _type    Type of Data: Use enum CANType for valid parameter values
+     *  @param _format  Data Format: Use enum CANFormat for valid parameter values
+     */
+    CANFDMessage(unsigned int _id, const unsigned char *_data, unsigned char _len = 64, CANType _type = CANData, CANFormat _format = CANStandard)
+    {
+        len    = (_len > 64) ? 64 : _len;
+        type   = _type;
+        format = _format;
+        id     = _id;
+        memcpy(data, _data, len);
+    }
+
+
+    /** Creates CANFD message with specific content.
+     *
+     *  @param _id      Message ID
+     *  @param _data    Mesaage Data
+     *  @param _len     Message Data length
+     *  @param _type    Type of Data: Use enum CANType for valid parameter values
+     *  @param _format  Data Format: Use enum CANFormat for valid parameter values
+     */
+    CANFDMessage(unsigned int _id, const char *_data, unsigned char _len = 64, CANType _type = CANData, CANFormat _format = CANStandard)
+    {
+        len    = (_len > 64) ? 64 : _len;
+        type   = _type;
+        format = _format;
+        id     = _id;
+        memcpy(data, _data, len);
+    }
+
+    /** Creates CANFD remote message.
+     *
+     *  @param _id      Message ID
+     *  @param _format  Data Format: Use enum CANType for valid parameter values
+     */
+    CANFDMessage(unsigned int _id, CANFormat _format = CANStandard)
+    {
+        len    = 0;
+        type   = CANRemote;
+        format = _format;
+        id     = _id;
+        memset(data, 0, 64);
+    }
+
+    /**
+     * "Deep" comparison operator (ie: compare value of each data member)
+     */
+    bool operator ==(const CANFDMessage &b) const
+    {
+        if (id != b.id) {
+            return false;
+        }
+        if (len != b.len) {
+            return false;
+        }
+        if (format != b.format) {
+            return false;
+        }
+        if (type != b.type) {
+            return false;
+        }
+        if (memcmp(data, b.data, len) != 0) {
+            return false;
+        }
+
+        return true;
+    }
+
+    bool operator !=(const CANFDMessage &b) const
+    {
+        return !(*this == b);
+    }
+};
+
+#endif
+
 /** @}*/
 
 namespace interface {

drivers/source/CAN.cpp

@@ -30,10 +30,14 @@ CAN::CAN(PinName rd, PinName td) : _can(), _irq()
     can_irq_init(&_can, (&CAN::_irq_handler), reinterpret_cast<uintptr_t>(this));
 }
 
-CAN::CAN(PinName rd, PinName td, int hz) : _can(), _irq()
+CAN::CAN(PinName rd, PinName td, int hz, int data_hz) : _can(), _irq()
 {
     // No lock needed in constructor
+#if DEVICE_CAN_FD
+    canfd_init_freq(&_can, rd, td, hz, data_hz);
+#else
     can_init_freq(&_can, rd, td, hz);
+#endif
     can_irq_init(&_can, (&CAN::_irq_handler), reinterpret_cast<uintptr_t>(this));
 }
 
@@ -44,10 +48,14 @@ CAN::CAN(const can_pinmap_t &pinmap) : _can(), _irq()
     can_irq_init(&_can, (&CAN::_irq_handler), reinterpret_cast<uintptr_t>(this));
 }
 
-CAN::CAN(const can_pinmap_t &pinmap, int hz) : _can(), _irq()
+CAN::CAN(const can_pinmap_t &pinmap, int hz, int data_hz) : _can(), _irq()
 {
     // No lock needed in constructor
+#if DEVICE_CAN_FD
+    canfd_init_freq_direct(&_can, &pinmap, hz, data_hz);
+#else
     can_init_freq_direct(&_can, &pinmap, hz);
+#endif
     can_irq_init(&_can, (&CAN::_irq_handler), reinterpret_cast<uintptr_t>(this));
 }
 
@@ -63,10 +71,14 @@ CAN::~CAN()
     can_free(&_can);
 }
 
-int CAN::frequency(int f)
+int CAN::frequency(int f, int data_f)
 {
     lock();
+#if DEVICE_CAN_FD
+    int ret = canfd_frequency(&_can, f, data_f);
+#else
     int ret = can_frequency(&_can, f);
+#endif
     unlock();
     return ret;
 }
@@ -90,6 +102,29 @@ int CAN::read(CANMessage &msg, int handle)
     return ret;
 }
 
+#if DEVICE_CAN_FD
+
+int CAN::write(CANFDMessage msg)
+{
+    lock();
+    int ret = canfd_write(&_can, msg, 0);
+    unlock();
+    return ret;
+}
+
+int CAN::read(CANFDMessage &msg, int handle)
+{
+    lock();
+    int ret = canfd_read(&_can, &msg, handle);
+    if (msg.len > 64) {
+        MBED_ERROR(MBED_MAKE_ERROR(MBED_MODULE_DRIVER_CAN, MBED_ERROR_CODE_READ_FAILED), "Read tried to write more than 64 bytes");
+    }
+    unlock();
+    return ret;
+}
+
+#endif
+
 void CAN::reset()
 {
     lock();

hal/include/hal/can_api.h

@@ -87,6 +87,14 @@ unsigned char can_rderror(can_t *obj);
 unsigned char can_tderror(can_t *obj);
 void          can_monitor(can_t *obj, int silent);
 
+#if DEVICE_CAN_FD
+void          canfd_init_freq(can_t *obj, PinName rd, PinName td, int hz, int data_hz);
+void          canfd_init_freq_direct(can_t *obj, const can_pinmap_t *pinmap, int hz, int data_hz);
+int           canfd_frequency(can_t *obj, int hz, int data_hz);
+int           canfd_write(can_t *obj, CANFD_Message, int cc);
+int           canfd_read(can_t *obj, CANFD_Message *msg, int handle);
+#endif
+
 /** Get the pins that support CAN RD
  *
  * Return a PinMap array of pins that support CAN RD. The

hal/include/hal/can_helper.h

@@ -67,6 +67,26 @@ struct CAN_Message {
 };
 typedef struct CAN_Message CAN_Message;
 
+#if DEVICE_CAN_FD
+
+/**
+ *
+ * \struct  CAN_Message
+ *
+ * \brief   Holder for single CAN message.
+ *
+**/
+struct CANFD_Message {
+    unsigned int   id;                 // 29 bit identifier
+    unsigned char  data[64];            // Data field
+    unsigned char  len;                // Length of data field in bytes
+    CANFormat      format;             // Format ::CANFormat
+    CANType        type;               // Type ::CANType
+};
+typedef struct CANFD_Message CANFD_Message;
+
+#endif
+
 #ifdef __cplusplus
 }
 #endif

targets/TARGET_STM/TARGET_STM32G0/objects.h

@@ -75,6 +75,15 @@ struct analogin_s {
     uint8_t channel;
 };
 
+#if DEVICE_CAN
+struct can_s {
+    FDCAN_HandleTypeDef CanHandle;
+    int index;
+    int hz;
+    int data_hz;
+};
+#endif
+
 #include "gpio_object.h"
 
 #if DEVICE_ANALOGOUT

targets/TARGET_STM/TARGET_STM32G4/objects.h

@@ -88,6 +88,7 @@ struct can_s {
     FDCAN_HandleTypeDef CanHandle;
     int index;
     int hz;
+    int data_hz;
 };
 #endif
 

targets/TARGET_STM/TARGET_STM32H5/clock_cfg/system_clock.c

@@ -125,8 +125,9 @@ else // Divisible by 5MHz
     RCC_OscInitStruct.PLL.PLLP = 2;
     // Most of the SPI busses are clocked off of PLL1Q, and the max usable frequency for SPI is about
     // ~50MHz.  Plus, SPI has only limited, power-of-2 prescaler options so a high input clock really hurts
-    // its clock resolution.  So, give it a much lower input clock.
-    RCC_OscInitStruct.PLL.PLLQ = 10; // output freq = 50MHz
+    // its clock resolution.  So, give it a lower input clock.
+    // Note PLL1Q is also used by the CAN FD, 100MHz is needed to support a data rate of 5Mbit/s.
+    RCC_OscInitStruct.PLL.PLLQ = 5; // output freq = 100MHz
     RCC_OscInitStruct.PLL.PLLR = 2;
     RCC_OscInitStruct.PLL.PLLFRACN = 0;
     RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1_VCORANGE_WIDE;
@@ -194,8 +195,8 @@ uint8_t SetSysClock_PLL_HSI(void)
     RCC_OscInitStruct.PLL.PLLP = 2;
     // Most of the SPI busses are clocked off of PLL1Q, and the max usable frequency for SPI is about
     // ~50MHz.  Plus, SPI has only limited, power-of-2 prescaler options so a high input clock really hurts
-    // its clock resolution.  So, give it a much lower input clock.
-    RCC_OscInitStruct.PLL.PLLQ = 10; // output freq = 50MHz
+    // its clock resolution.  So, give it a lower input clock.
+    RCC_OscInitStruct.PLL.PLLQ = 5; // output freq = 100MHz
     RCC_OscInitStruct.PLL.PLLR = 2;
     RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1_VCIRANGE_3;
     RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1_VCORANGE_WIDE;

targets/TARGET_STM/TARGET_STM32H5/objects.h

@@ -133,6 +133,7 @@ struct can_s {
     FDCAN_HandleTypeDef CanHandle;
     int index;
     int hz;
+    int data_hz;
 };
 #endif
 

targets/TARGET_STM/TARGET_STM32H7/clock_cfg/TARGET_STM32H7_280MHZ/system_clock.c

@@ -27,12 +27,12 @@
   * It is used for all STM32H7 family microcontrollers with a top speed of 280MHz.
   * The input clock from the external oscillator may be any frequency evenly divisible by
   * 5MHz or 2MHz, and must be between 4MHz and 50MHz.
-  * 
+  *
   * Note that 280MHz is the "overdrive" mode and is basically an overclock.  It is only supported
   * under certain conditions (LDO in use) and cannot be used over the full temperature range.
   * For industrial applications it is recommended to disable overdrive.  Overdrive can be enabled/
   * disabled via the "target.enable-overdrive-mode" option in mbed_app.json.
-  * 
+  *
 **/
 
 #include "stm32h7xx.h"
@@ -160,9 +160,9 @@ MBED_WEAK uint8_t SetSysClock_PLL_HSE(uint8_t bypass)
     RCC_OscInitStruct.PLL.PLLP = 2;   // PLLCLK = SYSCLK = 280/225 MHz
 
 #if MBED_CONF_TARGET_ENABLE_OVERDRIVE_MODE
-    RCC_OscInitStruct.PLL.PLLQ = 56;  // PLL1Q used for FDCAN = 10 MHz
+    RCC_OscInitStruct.PLL.PLLQ = 7;  // PLL1Q used for FDCAN = 80 MHz
 #else
-    RCC_OscInitStruct.PLL.PLLQ = 45;  // PLL1Q used for FDCAN = 10 MHz
+    RCC_OscInitStruct.PLL.PLLQ = 5;  // PLL1Q used for FDCAN = 90 MHz
 #endif
 
     RCC_OscInitStruct.PLL.PLLR = 2;
@@ -234,11 +234,11 @@ uint8_t SetSysClock_PLL_HSI(void)
     RCC_OscInitStruct.PLL.PLLP = 2;   // PLLCLK = SYSCLK = 280/225 MHz
 
 #if MBED_CONF_TARGET_ENABLE_OVERDRIVE_MODE
-    RCC_OscInitStruct.PLL.PLLQ = 56;  // PLL1Q used for FDCAN = 10 MHz
+    RCC_OscInitStruct.PLL.PLLQ = 7;  // PLL1Q used for FDCAN = 80 MHz
 #else
-    RCC_OscInitStruct.PLL.PLLQ = 45;  // PLL1Q used for FDCAN = 10 MHz
+    RCC_OscInitStruct.PLL.PLLQ = 5;  // PLL1Q used for FDCAN = 90 MHz
 #endif
-    
+
     RCC_OscInitStruct.PLL.PLLR = 2;
     RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; // PLL1 VCO clock is between 128 and 560 MHz
     RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_1; // PLL1 input clock is between 2 and 4 MHz