Add stm32cube framework

library.json

@@ -25,6 +25,6 @@
         "extraScript": "./extra_script.py"
     },
 
-    "frameworks": "arduino",
+    "frameworks": "arduino, stm32cube",
     "platforms": "teensy, https://github.com/platformio/platform-espressif32.git#feature/arduino-upstream, atmelsam, raspberrypi, ststm32"
 }

platform_code/stm32cube/clock_gettime.cpp

@@ -0,0 +1,34 @@
+#include <sys/time.h>
+
+#if defined(STM32F0xx)
+#include "stm32f0xx_hal.h"
+#elif defined(STM32F1xx)
+#include "stm32f1xx_hal.h"
+#elif defined(STM32F2xx)
+#include "stm32f2xx_hal.h"
+#elif defined(STM32F3xx)
+#include "stm32f3xx_hal.h"
+#elif defined(STM32F4xx)
+#include "stm32f4xx_hal.h"
+#elif defined(STM32F7xx)
+#include "stm32f7xx_hal.h"
+#endif
+
+constexpr uint64_t GETTICK_ROLLOVER_USECONDS = 4294967296000UL;
+
+extern "C" int clock_gettime(clockid_t unused, struct timespec *tp) {
+  (void)unused;
+  static uint32_t rollover = 0;
+  static uint32_t last_measure = 0;
+
+  uint32_t m = HAL_GetTick();
+  rollover += (m < last_measure) ? 1 : 0;
+
+  uint64_t real_us =
+      (uint64_t)(m * 1000 + rollover * GETTICK_ROLLOVER_USECONDS);
+  tp->tv_sec = real_us / 1000000;
+  tp->tv_nsec = (real_us % 1000000) * 1000;
+  last_measure = m;
+
+  return 0;
+}

platform_code/stm32cube/serial/micro_ros_transport.cpp

@@ -0,0 +1,107 @@
+#include <micro_ros_platformio.h>
+
+static uint16_t rhead, rtail;
+static volatile uint16_t thead, thead_next, ttail;
+static uint32_t open_count = 0;
+
+static void stream_flush(DMAStream *stream) {
+  static volatile bool mutex = false;
+
+  if ((stream->uart->gState == HAL_UART_STATE_READY) && !mutex) {
+    mutex = true;
+
+    if (thead != ttail) {
+      uint16_t len =
+          thead < ttail ? ttail - thead : stream->tbuffer_size - thead;
+      thead_next = (thead + len) & (stream->tbuffer_size - 1);
+      HAL_UART_Transmit_DMA(stream->uart, stream->tbuffer + thead, len);
+    }
+    mutex = false;
+  }
+}
+
+void uart_transfer_complete_callback(DMAStream *stream) {
+  thead = thead_next;
+  stream_flush(stream);
+}
+
+extern "C" {
+
+bool platformio_transport_open(struct uxrCustomTransport *transport) {
+  DMAStream *stream = (DMAStream *)transport->args;
+
+  if (open_count == 0) {
+    rhead = rtail = thead = thead_next = ttail = 0;
+    HAL_UART_Receive_DMA(stream->uart, stream->rbuffer, stream->rbuffer_size);
+  }
+  open_count++;
+
+  return true;
+}
+
+bool platformio_transport_close(struct uxrCustomTransport *transport) {
+  DMAStream *stream = (DMAStream *)transport->args;
+
+  open_count--;
+  if (open_count == 0) {
+    HAL_UART_DMAStop(stream->uart);
+  }
+
+  return true;
+}
+
+size_t platformio_transport_write(struct uxrCustomTransport *transport,
+                                  const uint8_t *buf, size_t len,
+                                  uint8_t *errcode) {
+  DMAStream *stream = (DMAStream *)transport->args;
+
+  uint16_t n = len;
+  uint16_t buffer_available =
+      thead <= ttail ? stream->tbuffer_size - ttail + thead : thead - ttail;
+  if (n > buffer_available) n = buffer_available;
+
+  uint16_t n_tail =
+      n <= stream->tbuffer_size - ttail ? n : stream->tbuffer_size - ttail;
+
+  memcpy(stream->tbuffer + ttail, buf, n_tail);
+
+  if (n != n_tail) {
+    memcpy(stream->tbuffer, buf + n_tail, n - n_tail);
+  }
+
+  ttail = (ttail + n) & (stream->tbuffer_size - 1);
+
+  stream_flush(stream);
+
+  return n;
+}
+
+size_t platformio_transport_read(struct uxrCustomTransport *transport,
+                                 uint8_t *buf, size_t len, int timeout,
+                                 uint8_t *errcode) {
+  DMAStream *stream = (DMAStream *)transport->args;
+
+  int ms_used = 0;
+  while (true) {
+    __disable_irq();
+    rtail = stream->rbuffer_size - __HAL_DMA_GET_COUNTER(stream->uart->hdmarx);
+    __enable_irq();
+
+    uint16_t data_available =
+        rhead <= rtail ? rtail - rhead : stream->rbuffer_size - rhead + rtail;
+
+    if (data_available >= len || ms_used >= timeout) break;
+
+    HAL_Delay(1);
+    ms_used++;
+  }
+
+  size_t wrote = 0;
+  while ((rhead != rtail) && (wrote < len)) {
+    buf[wrote++] = stream->rbuffer[rhead];
+    rhead = (rhead + 1) & (stream->rbuffer_size - 1);
+  }
+
+  return wrote;
+}
+}

platform_code/stm32cube/serial/micro_ros_transport.h

@@ -0,0 +1,31 @@
+#include <rmw_microros/custom_transport.h>
+
+#if defined(STM32F0xx)
+#include "stm32f0xx_hal.h"
+#elif defined(STM32F1xx)
+#include "stm32f1xx_hal.h"
+#elif defined(STM32F2xx)
+#include "stm32f2xx_hal.h"
+#elif defined(STM32F3xx)
+#include "stm32f3xx_hal.h"
+#elif defined(STM32F4xx)
+#include "stm32f4xx_hal.h"
+#elif defined(STM32F7xx)
+#include "stm32f7xx_hal.h"
+#endif
+
+struct DMAStream {
+  UART_HandleTypeDef* uart;
+  uint16_t rbuffer_size;
+  uint8_t* rbuffer;
+  uint16_t tbuffer_size;
+  uint8_t* tbuffer;
+};
+
+static inline void set_microros_serial_transports(DMAStream& stream) {
+  rmw_uros_set_custom_transport(
+      true, &stream, platformio_transport_open, platformio_transport_close,
+      platformio_transport_write, platformio_transport_read);
+}
+
+void uart_transfer_complete_callback(DMAStream* stream);