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

@@ -44,8 +44,6 @@ class CAN;
  */
 
 /** CANMessage class
- *
- * @note Synchronization level: Thread safe
  */
 class CANMessage : public CAN_Message {
 
@@ -140,6 +138,105 @@ class CANMessage : public CAN_Message {
     }
 };
 
+#if DEVICE_CAN_FD
+
+/** CANFDMessage class
+ */
+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,15 @@ 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
+    MBED_ASSERT(data_hz == 0);
     can_init_freq(&_can, rd, td, hz);
+#endif
     can_irq_init(&_can, (&CAN::_irq_handler), reinterpret_cast<uintptr_t>(this));
 }
 
@@ -44,10 +49,15 @@ 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
+    MBED_ASSERT(data_hz == 0);
     can_init_freq_direct(&_can, &pinmap, hz);
+#endif
     can_irq_init(&_can, (&CAN::_irq_handler), reinterpret_cast<uintptr_t>(this));
 }
 
@@ -63,18 +73,23 @@ 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
+    MBED_ASSERT(data_f == 0);
     int ret = can_frequency(&_can, f);
+#endif
     unlock();
     return ret;
 }
 
 int CAN::write(CANMessage msg)
 {
     lock();
-    int ret = can_write(&_can, msg, 0);
+    int ret = can_write(&_can, msg);
     unlock();
     return ret;
 }
@@ -90,6 +105,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);
+    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();