diff --git a/FreeRTOSConfig.h b/FreeRTOSConfig.h
index 2c2a0b7..c56a883 100644
--- a/FreeRTOSConfig.h
+++ b/FreeRTOSConfig.h
@@ -95,7 +95,7 @@
 
 #define configUSE_PREEMPTION                     1
 #define configSUPPORT_STATIC_ALLOCATION          1
-#define configSUPPORT_DYNAMIC_ALLOCATION         0
+#define configSUPPORT_DYNAMIC_ALLOCATION         1
 #define configUSE_IDLE_HOOK                      0
 #define configUSE_TICK_HOOK                      0
 #define configCPU_CLOCK_HZ                       ( SystemCoreClock )
@@ -109,6 +109,8 @@
 #define configCHECK_FOR_STACK_OVERFLOW           2
 #define configENABLE_BACKWARD_COMPATIBILITY      0
 
+#define configTOTAL_HEAP_SIZE 4096
+
 /* Co-routine definitions. */
 #define configUSE_CO_ROUTINES                    0
 #define configMAX_CO_ROUTINE_PRIORITIES          ( 2 )
diff --git a/USB/usbd_conf.c b/USB/usbd_conf.c
index 3f9f3ab..4e29378 100644
--- a/USB/usbd_conf.c
+++ b/USB/usbd_conf.c
@@ -47,6 +47,7 @@
   ******************************************************************************
 */
 /* Includes ------------------------------------------------------------------*/
+#include <utils/malloc_safe.h>
 #include "platform.h"
 #include "usbd_def.h"
 #include "usbd_core.h"
@@ -633,8 +634,6 @@ void  USBD_LL_Delay (uint32_t Delay)
   HAL_Delay(Delay);  
 }
 
-static uint32_t _static_malloc_pos = 0;
-
 /**
   * @brief  static single allocation.
   * @param  size: size of allocated memory
@@ -642,11 +641,7 @@ static uint32_t _static_malloc_pos = 0;
   */
 void *USBD_static_malloc(uint32_t size)
 {
-    // XXX this was modified to support multiple classes
-    static uint32_t mem[(sizeof(USBD_MSC_BOT_HandleTypeDef)/4)+1+(sizeof(USBD_CDC_HandleTypeDef)/4)+1];/* On 32-bit boundary */
-    uint32_t oldpos = _static_malloc_pos;
-    _static_malloc_pos += size/4+1;
-    return &mem[oldpos];
+    return malloc_ck(size);
 }
 
 /**
@@ -656,9 +651,7 @@ void *USBD_static_malloc(uint32_t size)
   */
 void USBD_static_free(void *p)
 {
-    // This is wrong, but will work if both frees and malloc's
-    // are always called together and not interleaved
-    _static_malloc_pos = 0;
+    free_ck(p);
 }
 
 /**
diff --git a/USB/usbd_desc.c b/USB/usbd_desc.c
index 1df44b6..1199390 100644
--- a/USB/usbd_desc.c
+++ b/USB/usbd_desc.c
@@ -246,7 +246,7 @@ uint8_t *  USBD_FS_ManufacturerStrDescriptor( USBD_SpeedTypeDef speed , uint16_t
 */
 uint8_t *  USBD_FS_SerialStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 {
-  char buff[25];
+  char buff[26];
   fixup_sprintf(buff, "%08"PRIX32"-%08"PRIX32"-%08"PRIX32,
           LL_GetUID_Word0(),
           LL_GetUID_Word1(),
diff --git a/comm/messages.c b/comm/messages.c
index 5add7b2..2348ca8 100644
--- a/comm/messages.c
+++ b/comm/messages.c
@@ -2,13 +2,14 @@
 // Created by MightyPork on 2017/11/21.
 //
 
-#include <framework/system_settings.h>
 #include "platform.h"
 #include "framework/settings.h"
 #include "utils/ini_parser.h"
 #include "TinyFrame.h"
 #include "framework/unit_registry.h"
 #include "comm/messages.h"
+#include "framework/system_settings.h"
+#include "utils/malloc_safe.h"
 
 static TinyFrame tf_;
 TinyFrame *comm = &tf_;
@@ -61,11 +62,14 @@ static void settings_bulkread_cb(BulkRead *bulk, uint32_t chunk, uint8_t *buffer
 {
     // clean-up request
     if (buffer == NULL) {
-        free(bulk);
+        free_ck(bulk);
+        iw_end();
         dbg("INI read complete.");
         return;
     }
 
+    if (bulk->offset == 0) iw_begin();
+
     IniWriter iw = iw_init((char *)buffer, bulk->offset, chunk);
     settings_build_units_ini(&iw);
 }
@@ -77,8 +81,8 @@ static TF_Result lst_ini_export(TinyFrame *tf, TF_Msg *msg)
 {
     dbg("Bulk read INI file");
 
-    BulkRead *bulk = malloc(sizeof(BulkRead));
-    assert_param(bulk);
+    BulkRead *bulk = malloc_ck(sizeof(BulkRead));
+    assert_param(bulk != NULL);
 
     bulk->frame_id = msg->frame_id;
     bulk->len = iw_measure_total(settings_build_units_ini);
@@ -112,7 +116,7 @@ static void settings_bulkwrite_cb(BulkWrite *bulk, const uint8_t *chunk, uint32_
             dbg("INI write failed");
         }
 
-        free(bulk);
+        free_ck(bulk);
         return;
     }
 
@@ -126,7 +130,7 @@ static TF_Result lst_ini_import(TinyFrame *tf, TF_Msg *msg)
 {
     dbg("Bulk write INI file");
 
-    BulkWrite *bulk = malloc(sizeof(BulkWrite));
+    BulkWrite *bulk = malloc_ck(sizeof(BulkWrite));
     assert_param(bulk);
 
     bulk->frame_id = msg->frame_id;
diff --git a/comm/msg_bulkread.c b/comm/msg_bulkread.c
index 0bc31f8..15dfd0f 100644
--- a/comm/msg_bulkread.c
+++ b/comm/msg_bulkread.c
@@ -5,14 +5,12 @@
 #include "platform.h"
 
 #include <TinyFrame.h>
+#include "utils/malloc_safe.h"
 
 #include "messages.h"
 #include "utils/payload_parser.h"
 #include "utils/payload_builder.h"
 
-/** Buffer for preparing bulk chunks */
-static uint8_t bulkread_buffer[BULK_READ_BUF_LEN];
-
 /**
  * TF listener for the bulk read transaction
  */
@@ -26,6 +24,7 @@ static TF_Result bulkread_lst(TinyFrame *tf, TF_Msg *msg)
     }
 
     assert_param(NULL != bulk);
+    assert_param(NULL != bulk->_buffer);
 
     if (msg->type == MSG_BULK_ABORT) {
         goto close;
@@ -39,7 +38,7 @@ static TF_Result bulkread_lst(TinyFrame *tf, TF_Msg *msg)
         chunk = MIN(chunk, BULK_READ_BUF_LEN);
 
         // load data into the buffer
-        bulk->read(bulk, chunk, bulkread_buffer);
+        bulk->read(bulk, chunk, bulk->_buffer);
 
         bool last = (bulk->offset + chunk >= bulk->len);
 
@@ -47,7 +46,7 @@ static TF_Result bulkread_lst(TinyFrame *tf, TF_Msg *msg)
         TF_ClearMsg(&resp);
         resp.frame_id = bulk->frame_id;
         resp.type = (last ? MSG_BULK_END : MSG_BULK_DATA); // the last chunk has the END type
-        resp.data = bulkread_buffer;
+        resp.data = bulk->_buffer;
         resp.len = (TF_LEN) chunk;
         TF_Respond(tf, &resp);
 
@@ -64,6 +63,7 @@ close:
         // Ask user to free the bulk and userdata
         bulk->read(bulk, 0, NULL);
         msg->userdata = NULL;
+        free_ck(bulk->_buffer);
     }
     return TF_CLOSE;
 }
@@ -76,6 +76,7 @@ void bulkread_start(TinyFrame *tf, BulkRead *bulk)
     assert_param(bulk->read);
 
     bulk->offset = 0;
+    bulk->_buffer = malloc_ck(BULK_READ_BUF_LEN);
 
     {
         uint8_t buf[8];
diff --git a/comm/msg_bulkread.h b/comm/msg_bulkread.h
index 91ad454..eb9fefe 100644
--- a/comm/msg_bulkread.h
+++ b/comm/msg_bulkread.h
@@ -30,6 +30,7 @@ struct bulk_read {
     bulkread_data_cb read; //!< Read callback
     uint32_t len;          //!< Total data length
     void *userdata;        //!< A place for arbitrary userdata
+    uint8_t *_buffer;
 
     uint32_t offset;       //!< Internal offset counter, will be set to 0 on start.
 };
diff --git a/comm/msg_responses.c b/comm/msg_responses.c
index 3e6128b..a59b0d8 100644
--- a/comm/msg_responses.c
+++ b/comm/msg_responses.c
@@ -5,6 +5,7 @@
 #include "platform.h"
 #include "messages.h"
 #include "msg_responses.h"
+#include "payload_builder.h"
 
 void com_respond_snprintf(TF_ID frame_id, TF_TYPE type, const char *format, ...)
 {
@@ -37,6 +38,12 @@ void com_respond_ok(TF_ID frame_id)
     com_respond_buf(frame_id, MSG_SUCCESS, NULL, 0);
 }
 
+void com_send_pb(TF_TYPE type, PayloadBuilder *pb)
+{
+    uint32_t len;
+    uint8_t *buf = pb_close(pb, &len);
+    com_send_buf(type, buf, len);
+}
 
 void com_send_buf(TF_TYPE type, const uint8_t *buf, uint32_t len)
 {
diff --git a/comm/msg_responses.h b/comm/msg_responses.h
index e773504..f91f2c9 100644
--- a/comm/msg_responses.h
+++ b/comm/msg_responses.h
@@ -9,6 +9,8 @@
 #error "Include messages.h instead!"
 #endif
 
+#include "payload_builder.h"
+
 /**
  * Respond to a TF message using printf-like formatting.
  *
@@ -54,6 +56,14 @@ void com_respond_error(TF_ID frame_id, error_t error);
  */
 void com_send_buf(TF_TYPE type, const uint8_t *buf, uint32_t len);
 
+/**
+ * Send a payload builder's content
+ *
+ * @param type - response type byte
+ * @param pb - builder
+ */
+void com_send_pb(TF_TYPE type, PayloadBuilder *pb);
+
 /**
  * Same like tf_respond_buf(), but the buffer length is measured with strlen.
  * Used to sending ASCII string responses.
diff --git a/cortex_handlers.c b/cortex_handlers.c
index 8599f03..6e5584d 100644
--- a/cortex_handlers.c
+++ b/cortex_handlers.c
@@ -159,6 +159,11 @@ of this function. */
 #endif
 
     Indicator_Effect(STATUS_FAULT);
+
+    // throw in the canary dump, just in case
+#if USE_STACK_MONITOR
+    stackmon_dump();
+#endif
     while (1);
 }
 #endif
@@ -201,5 +206,31 @@ void  __attribute__((naked)) HardFault_Handler(void)
     while (1);
 }
 
+#if 0
+char *heap_end = 0;
+caddr_t _sbrk(int incr) {
+    extern char _end; // this is the end of bbs, defined in LD
+    extern char _estack; // this is the end of the allocable memory - defined in LD. Top level stack lives here.
+    char *prev_heap_end;
+
+    if (heap_end == 0) {
+        heap_end = &_end;
+    }
+    prev_heap_end = heap_end;
+
+    if (heap_end + incr > &_estack) {
+        /* Heap and stack collision */
+        dbg("\r\nOUT OF MEMORY");
+        return (char*)-1;
+    }
+#if DEBUG_MALLOC
+    PRINTF(" !sbrk(%d),total=%d! ", incr, (int)(heap_end - &_end));
+#endif
+
+    heap_end += incr;
+    return (caddr_t) prev_heap_end;
+}
+#endif
+
 /* USER CODE END 1 */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/debug.c b/debug.c
index db31482..21a4bf6 100644
--- a/debug.c
+++ b/debug.c
@@ -8,7 +8,7 @@
 #if USE_DEBUG_UART
 
 // debug printf
-int PRINTF(const char *format, ...)
+void _DO_PRINTF(const char *format, ...)
 {
     va_list args;
     int len;
@@ -23,10 +23,8 @@ int PRINTF(const char *format, ...)
         len = DBG_BUF_LEN-1;
     }
 
-    _write_r(NULL, 2, dbg_buf, (size_t) len);
+    debug_write(dbg_buf, (uint16_t) len);
     va_end(args);
-
-    return len;
 }
 
 /**
@@ -34,19 +32,10 @@ int PRINTF(const char *format, ...)
  * @param string - buffer to print
  * @param len - number of bytes to print
  */
-void PUTSN(const char *string, size_t len)
-{
-    if (len == 0) len = strlen(string);
-    _write_r(NULL, 2, string, (size_t) len);
-}
-
-/**
- * Puts a newline
- *
- */
-void PUTNL(void)
+void PUTSN(const char *string, uint16_t len)
 {
-    _write_r(NULL, 2, "\r\n", 2);
+    if (len == 0) len = (uint16_t) strlen(string);
+    debug_write(string, len);
 }
 
 /**
@@ -54,22 +43,11 @@ void PUTNL(void)
  * @param string - string to print, zero-terminated
  * @return number of characters printed
  */
-int PUTS(const char *string)
+void PUTS(const char *string)
 {
     size_t len = strlen(string);
-    _write_r(NULL, 2, string, len);
-    return (int) len;
+    debug_write(string, (uint16_t) len);
 }
 
-/**
- * Print one character to debug uart
- * @param ch - character ASCII code
- * @return the character code
- */
-int PUTCHAR(int ch)
-{
-    _write_r(NULL, 2, &ch, 1);
-    return ch; // or EOF
-}
 
 #endif
diff --git a/debug.h b/debug.h
index 20bdff2..65ec0fd 100644
--- a/debug.h
+++ b/debug.h
@@ -8,22 +8,55 @@
 #include <inttypes.h>
 #include <stddef.h>
 #include <stdarg.h>
+#include "macro.h"
 
 #if USE_DEBUG_UART
 
-int PRINTF(const char *format, ...) __attribute__((format(printf,1,2))) ;
-void PUTSN(const char *string, size_t len);
-int PUTS(const char *string);
-void PUTNL(void);
-int PUTCHAR(int ch);
+extern void debug_write(const char *buf, uint16_t len);
+
+void _DO_PRINTF(const char *format, ...) __attribute__((format(printf,1,2))) ;
+void PUTSN(const char *string, uint16_t len);
+void PUTS(const char *string);
+
+static inline void PUTNL(void)
+{
+    debug_write("\r\n", 2);
+}
+
+/**
+ * Print one character to debug uart
+ * @param ch - character ASCII code
+ * @return the character code
+ */
+static inline void PUTCHAR(char ch)
+{
+    debug_write(&ch, 1);
+}
+
+#define PRINTF(format, ...) do { \
+    if (VA_ARG_COUNT(__VA_ARGS__) == 0) { \
+      PUTS(format); \
+    } else { \
+      _DO_PRINTF(format, ##__VA_ARGS__); \
+    } \
+  } while (0)
+
+#define dbg(format, ...) do { \
+    if (VA_ARG_COUNT(__VA_ARGS__) == 0) { \
+      PUTS(format); \
+    } else { \
+      PRINTF(format, ##__VA_ARGS__); \
+    } \
+    PUTNL(); \
+  } while (0)
 
-#define dbg(format, ...) do { PRINTF(format, ##__VA_ARGS__); PUTNL(); } while (0)
 
 #else
 #define dbg(format, ...) do {} while (0)
 #define PRINTF(format, ...) do {} while (0)
 #define PUTSN(string, len) do {} while (0)
 #define PUTS(string) do {} while (0)
+#define PUTNL() do {} while (0)
 #define PUTCHAR(ch) do {} while (0)
 #endif
 
diff --git a/framework/resources.c b/framework/resources.c
index cc29001..2fe7748 100644
--- a/framework/resources.c
+++ b/framework/resources.c
@@ -5,7 +5,7 @@
 #include "platform.h"
 #include "unit.h"
 #include "resources.h"
-#include "pin_utils.h"
+#include "hw_utils.h"
 #include "unit_registry.h"
 
 static bool rsc_initialized = false;
@@ -73,7 +73,7 @@ error_t rsc_claim(Unit *unit, Resource rsc)
     assert_param(rsc < RESOURCE_COUNT);
     assert_param(unit != NULL);
 
-//    dbg("%s claims %s", unit->name, rsc_get_name(rsc));
+    rsc_dbg("%s claims %s", unit->name, rsc_get_name(rsc));
 
     if (RSC_IS_HELD(global_rscmap, rsc)) {
         // this whole branch is just reporting the error
@@ -81,11 +81,13 @@ error_t rsc_claim(Unit *unit, Resource rsc)
         Unit *holder = ureg_get_rsc_owner(rsc);
         assert_param(holder != NULL);
 
-        dbg("ERROR!! Unit %s failed to claim resource %s, already held by %s!",
+        dbg("ERROR!! Unit %s failed to claim rsc %s, already held by %s!",
             unit->name,
             rsc_get_name(rsc),
             holder->name);
 
+        if (holder == unit) dbg("DOUBLE CLAIM, This is probably a bug!");
+
         unit->failed_rsc = rsc;
 
         return E_RESOURCE_NOT_AVAILABLE;
@@ -127,7 +129,7 @@ error_t rsc_claim_gpios(Unit *unit, char port_name, uint16_t pins)
 
     for (int i = 0; i < 16; i++) {
         if (pins & (1 << i)) {
-            Resource rsc = pin2resource(port_name, (uint8_t) i, &suc);
+            Resource rsc = hw_pin2resource(port_name, (uint8_t) i, &suc);
             if (!suc) return E_BAD_CONFIG;
 
             TRY(rsc_claim(unit, rsc));
@@ -139,7 +141,7 @@ error_t rsc_claim_gpios(Unit *unit, char port_name, uint16_t pins)
 error_t rsc_claim_pin(Unit *unit, char port_name, uint8_t pin)
 {
     bool suc = true;
-    Resource rsc = pin2resource(port_name,  pin, &suc);
+    Resource rsc = hw_pin2resource(port_name, pin, &suc);
     if (!suc) return E_BAD_CONFIG;
     TRY(rsc_claim(unit, rsc));
     return E_SUCCESS;
@@ -156,7 +158,7 @@ void rsc_free(Unit *unit, Resource rsc)
     assert_param(rsc_initialized);
     assert_param(rsc < RESOURCE_COUNT);
 
-//    dbg("Free resource %s", rsc_get_name(rsc));
+    rsc_dbg("Free resource %s", rsc_get_name(rsc));
 
     if (RSC_IS_FREE(global_rscmap, rsc)) return;
 
@@ -211,8 +213,8 @@ void rsc_teardown(Unit *unit)
     assert_param(rsc_initialized);
     assert_param(unit != NULL);
 
-//    dbg("Tearing down unit %s", unit->name);
-    deinit_unit_pins(unit);
+    rsc_dbg("Tearing down unit %s", unit->name);
+    hw_deinit_unit_pins(unit);
 
     for (uint32_t i = 0; i < RSCMAP_LEN; i++) {
         global_rscmap[i] &= ~unit->resources[i];
@@ -224,8 +226,6 @@ void rsc_print_all_available(IniWriter *iw)
 {
     if (iw->count == 0) return;
 
-    static char buf[80];
-
     iw_string(iw, "Resources available on this platform\r\n"
                   "------------------------------------\r\n");
 
@@ -256,7 +256,7 @@ void rsc_print_all_available(IniWriter *iw)
         if (i%16 == 0) {
             // here we print the previous port
             if (bitmap != 0) {
-                iw_string(iw, str_pinmask(bitmap, buf));
+                iw_string(iw, pinmask2str(bitmap, iwbuffer));
                 bitmap = 0;
             }
 
@@ -271,7 +271,7 @@ void rsc_print_all_available(IniWriter *iw)
     }
     // the last one
     if (bitmap != 0) {
-        iw_string(iw, str_pinmask(bitmap, buf));
+        iw_string(iw, pinmask2str(bitmap, iwbuffer));
     }
     iw_newline(iw);
     iw_newline(iw);
diff --git a/framework/resources.h b/framework/resources.h
index 7b51e26..3e3e5b5 100644
--- a/framework/resources.h
+++ b/framework/resources.h
@@ -9,6 +9,12 @@
 #include "unit.h"
 #include "rsc_enum.h"
 
+#if DEBUG_RSC
+#define rsc_dbg(fmt, ...) dbg("[RSC] "fmt, ##__VA_ARGS__)
+#else
+#define rsc_dbg(fmt, ...)
+#endif
+
 #define CHECK_SUC() do { if (!suc) return false; } while (0)
 
 void rsc_init_registry(void);
diff --git a/framework/rsc_enum.h b/framework/rsc_enum.h
index 8344de4..6854825 100644
--- a/framework/rsc_enum.h
+++ b/framework/rsc_enum.h
@@ -15,7 +15,8 @@
     X(TIM1) X(TIM2) X(TIM3) X(TIM4) X(TIM5) \
     X(TIM6) X(TIM7) X(TIM8) X(TIM9) X(TIM10) X(TIM11) X(TIM12) X(TIM13) X(TIM14) \
     X(TIM15) X(TIM16) X(TIM17) \
-    X(DMA1) X(DMA2)
+    X(DMA1_1) X(DMA1_2) X(DMA1_3) X(DMA1_4) X(DMA1_5) X(DMA1_6) X(DMA1_7) X(DMA1_8) \
+    X(DMA2_1) X(DMA2_2) X(DMA2_3) X(DMA2_4) X(DMA2_5) X(DMA2_6) X(DMA2_7) X(DMA2_8)
 
 // Resources not used anywhere:
 // X(I2S1) X(I2S2) X(I2S3)
@@ -63,7 +64,7 @@
 //  - some support quadrature input, probably all support external clock / gating / clock-out/PWM generation
 //  Not all chips have all timers and not all timers are equal.
 
-// DMA - Direct memory access lines - TODO split those to channels, they can be used separately
+// DMA - Direct memory access lines
 
 // The resource registry will be pre-loaded with platform-specific config of which blocks are available - the rest will be "pre-claimed"
 // (i.e. unavailable to functional modules)
diff --git a/framework/settings.c b/framework/settings.c
index 41d1bbf..744bbcf 100644
--- a/framework/settings.c
+++ b/framework/settings.c
@@ -59,7 +59,7 @@ void settings_load(void)
 }
 
 
-static uint8_t save_buffer[FLASH_SAVE_BUF_LEN];
+static uint8_t *save_buffer = NULL;
 static uint32_t save_addr;
 
 #if DEBUG_FLASH_WRITE
@@ -87,6 +87,11 @@ static bool savebuf_ovhandler(PayloadBuilder *pb, uint32_t more)
 void settings_save(void)
 {
     HAL_StatusTypeDef hst;
+
+    assert_param(save_buffer == NULL); // It must be NULL here - otherwise we have a leak
+    save_buffer = malloc_ck(FLASH_SAVE_BUF_LEN);
+    assert_param(save_buffer != NULL);
+
     PayloadBuilder pb = pb_start(save_buffer, FLASH_SAVE_BUF_LEN, savebuf_ovhandler);
 
     save_addr = SETTINGS_FLASH_ADDR;
@@ -145,6 +150,9 @@ void settings_save(void)
     assert_param(hst == HAL_OK);
     fls_printf("--- Flash done ---\r\n");
 
+    free_ck(save_buffer);
+    save_buffer = NULL;
+
 #if DEBUG_FLASH_WRITE
     dbg("written @ %p", (void*)SETTINGS_FLASH_ADDR);
     hexDump("Flash", (void*)SETTINGS_FLASH_ADDR, 64);
@@ -160,6 +168,8 @@ void settings_save(void)
  */
 static void savebuf_flush(PayloadBuilder *pb, bool final)
 {
+    assert_param(save_buffer != NULL);
+
     // TODO this might be buggy, was not tested cross-boundary yet
     // TODO remove those printf's after verifying correctness
 
@@ -283,21 +293,20 @@ void settings_build_pinout_txt(IniWriter *iw)
 void settings_load_ini_begin(void)
 {
     SystemSettings.modified = true;
-    SystemSettings.pristine = true;
+    SystemSettings.loading_inifile = 0;
 }
 
 void settings_load_ini_key(const char *restrict section, const char *restrict key, const char *restrict value)
 {
 //    dbg("[%s] %s = %s", section, key, value);
-    static char namebuf[INI_KEY_MAX];
+    char namebuf[INI_KEY_MAX];
 
     // SYSTEM and UNITS files must be separate.
     // Init functions are run for first key in the section.
 
     if (streq(section, "SYSTEM")) {
-
-        if (SystemSettings.pristine) {
-            SystemSettings.pristine = false;
+        if (SystemSettings.loading_inifile == 0) {
+            SystemSettings.loading_inifile = 'S';
             systemsettings_loadDefaults();
         }
 
@@ -306,8 +315,8 @@ void settings_load_ini_key(const char *restrict section, const char *restrict ke
     }
     else if (streq(section, "UNITS")) {
 
-        if (SystemSettings.pristine) {
-            SystemSettings.pristine = false;
+        if (SystemSettings.loading_inifile == 0) {
+            SystemSettings.loading_inifile = 'U';
             ureg_remove_all_units();
         }
 
@@ -337,7 +346,8 @@ void settings_load_ini_key(const char *restrict section, const char *restrict ke
 
 void settings_load_ini_end(void)
 {
-    if (!ureg_finalize_all_init()) {
-        dbg("Some units failed to init!!");
+    if (SystemSettings.loading_inifile == 'U') {
+        bool suc = ureg_finalize_all_init();
+        if (!suc) dbg("Some units failed to init!!");
     }
 }
diff --git a/framework/system_settings.h b/framework/system_settings.h
index b7f1e1b..9953adc 100644
--- a/framework/system_settings.h
+++ b/framework/system_settings.h
@@ -17,7 +17,7 @@ struct system_settings {
     // Support flags put here for scoping, but not atcually part of the persistent settings
     volatile bool editable; //!< True if we booted with the LOCK jumper removed
     volatile bool modified; //!< True if user did any change to the settings (checked when the LOCK jumper is replaced)
-    volatile bool pristine; //!< Marks unknown state before we reach first section marker that determines what file it is
+    volatile char loading_inifile; // S-system, U-units
 };
 
 extern struct system_settings SystemSettings;
diff --git a/framework/unit.c b/framework/unit.c
index a1839f4..36da8c3 100644
--- a/framework/unit.c
+++ b/framework/unit.c
@@ -5,6 +5,7 @@
 #include "platform.h"
 #include "unit.h"
 #include "resources.h"
+#include "unit_base.h"
 
 char unit_tmp512[UNIT_TMP_LEN];
 
@@ -16,16 +17,8 @@ void clean_failed_unit(Unit *unit)
     dbg("!! Init of [%s] failed!", unit->name);
 
     // Free if it looks like it might've been allocated
-    if (isDynAlloc(unit->data)) {
-        dbg("Freeing allocated unit data");
-        free(unit->data);
-        unit->data = NULL;
-    }
-    if (isDynAlloc(unit->name)) {
-        dbg("Freeing allocated name");
-        free((void *) unit->name);
-        unit->name = NULL;
-    }
+    free_ck(unit->data);
+    free_ck(unit->name);
 
     dbg("Releasing any held resources");
     // Release any already claimed resources
diff --git a/framework/unit.h b/framework/unit.h
index 3b530b1..961eb2e 100644
--- a/framework/unit.h
+++ b/framework/unit.h
@@ -45,6 +45,9 @@ struct unit {
 
     /** Bit-map of held resources */
     ResourceMap resources;
+
+    uint16_t tick_interval;
+    uint16_t _tick_cnt;
 };
 
 /**
@@ -111,6 +114,13 @@ struct unit_driver {
      * Handle an incoming request. Return true if command was OK.
      */
     error_t (*handleRequest)(Unit *unit, TF_ID frame_id, uint8_t command, PayloadParser *pp);
+
+    /**
+     * Periodic update call.
+     * This is run from the SysTick interrupt handler,
+     * any communication should be deferred via the job queue.
+     */
+    void (*updateTick)(Unit *unit);
 };
 
 /**
diff --git a/framework/unit_base.h b/framework/unit_base.h
index 7ef9f6b..e145236 100644
--- a/framework/unit_base.h
+++ b/framework/unit_base.h
@@ -4,7 +4,7 @@
 
 #include "platform.h"
 #include "unit.h"
-#include "pin_utils.h"
+#include "hw_utils.h"
 #include "resources.h"
 #include "utils/str_utils.h"
 #include "utils/malloc_safe.h"
diff --git a/framework/unit_registry.c b/framework/unit_registry.c
index 27ef88e..d6524ea 100644
--- a/framework/unit_registry.c
+++ b/framework/unit_registry.c
@@ -58,8 +58,8 @@ void ureg_add_type(const UnitDriver *driver)
     assert_param(driver->deInit != NULL);
     assert_param(driver->handleRequest != NULL);
 
-    UregEntry *re = calloc_ck(1, sizeof(UregEntry), &suc);
-    assert_param(suc);
+    UregEntry *re = calloc_ck(1, sizeof(UregEntry));
+    assert_param(re != NULL);
 
     re->driver = driver;
     re->next = NULL;
@@ -81,11 +81,7 @@ static void free_le_unit(UlistEntry *le)
     pUnit->driver->deInit(pUnit);
     // Name is not expected to be freed by the deInit() function
     // - was alloc'd in the settings load loop
-    if (isDynAlloc(pUnit->name)) {
-        dbg("Freeing allocated name");
-        free((void *) pUnit->name);
-        pUnit->name = NULL;
-    }
+    free_ck(pUnit->name);
 }
 
 /** Add unit to the list, updating references as needed */
@@ -103,7 +99,6 @@ static void add_unit_to_list(UlistEntry *le)
 // create a unit instance (not yet loading or initing - just pre-init)
 Unit *ureg_instantiate(const char *driver_name)
 {
-    bool suc = true;
     error_t rv;
 
     // Find type in the repository
@@ -111,8 +106,8 @@ Unit *ureg_instantiate(const char *driver_name)
     while (re != NULL) {
         if (streq(re->driver->name, driver_name)) {
             // Create new list entry
-            UlistEntry *le = calloc_ck(1, sizeof(UlistEntry), &suc);
-            CHECK_SUC();
+            UlistEntry *le = calloc_ck(1, sizeof(UlistEntry));
+            if (le == NULL) return NULL;
 
             le->next = NULL;
 
@@ -132,7 +127,7 @@ Unit *ureg_instantiate(const char *driver_name)
 
                 dbg("!! Unit type %s failed to pre-init! %s", driver_name, error_get_message(rv));
                 clean_failed_unit(pUnit);
-                free(le);
+                free_ck(le);
                 return NULL;
             }
 
@@ -211,7 +206,7 @@ bool ureg_load_units(PayloadParser *pp)
 
                 // NAME
                 pp_string(pp, typebuf, 16);
-                pUnit->name = strdup(typebuf);
+                pUnit->name = strdup_ck(typebuf);
                 assert_param(pUnit->name);
 
                 // callsign
@@ -247,7 +242,7 @@ void ureg_remove_all_units(void)
         next = le->next;
 
         free_le_unit(le);
-        free(le);
+        free_ck(le);
 
         le = next;
     }
@@ -274,17 +269,17 @@ bool ureg_instantiate_by_ini(const char *restrict driver_name, const char *restr
                 char *name = NULL;
                 if (delim != NULL) {
                     // not last
-                    name = strndup(p, delim - p);
+                    name = strndup_ck(p, delim - p);
                     p = delim + 1;
                 } else {
                     // last name
-                    name = strdup(p);
+                    name = strdup_ck(p);
                     p = NULL; // quit after this loop ends
                 }
                 assert_param(name);
                 Unit *pUnit = ureg_instantiate(driver_name);
                 if (!pUnit) {
-                    free(name);
+                    free_ck(name);
                     return false;
                 }
 
@@ -331,18 +326,24 @@ bool ureg_finalize_all_init(void)
     while (li != NULL) {
         Unit *const pUnit = &li->unit;
 
-        pUnit->status = pUnit->driver->init(pUnit);
-        if (pUnit->status != E_SUCCESS) {
-            dbg("!!! error initing unit %s: %s", pUnit->name, error_get_message(pUnit->status));
-        }
-
-        // try to assign unique callsigns
-        // FIXME this is wrong, sometimes leads to duplicate CS
-        if (pUnit->callsign == 0) {
-            pUnit->callsign = callsign++;
+        if (pUnit->status == E_SUCCESS) {
+            dbg("! Unit seems already loaded, skipping");
         } else {
-            if (pUnit->callsign >= callsign) {
-                callsign = (uint8_t) (pUnit->callsign + 1);
+            pUnit->status = pUnit->driver->init(pUnit);
+            if (pUnit->status != E_SUCCESS) {
+                dbg("!!! error initing unit %s: %s", pUnit->name,
+                    error_get_message(pUnit->status));
+            }
+
+            // try to assign unique callsigns
+            // FIXME this is wrong, sometimes leads to duplicate CS
+            if (pUnit->callsign == 0) {
+                pUnit->callsign = callsign++;
+            }
+            else {
+                if (pUnit->callsign >= callsign) {
+                    callsign = (uint8_t) (pUnit->callsign + 1);
+                }
             }
         }
 
@@ -364,12 +365,12 @@ static void export_unit_do(UlistEntry *li, IniWriter *iw)
         {
             // special message for failed unit die to resource
             if (pUnit->status == E_RESOURCE_NOT_AVAILABLE) {
-                iw_comment(iw, "!!! %s not available, already held by %s",
+                iw_commentf(iw, "!!! %s not available, already held by %s",
                            rsc_get_name(pUnit->failed_rsc),
                            rsc_get_owner_name(pUnit->failed_rsc));
             }
             else {
-                iw_comment(iw, "!!! %s", error_get_message(pUnit->status));
+                iw_commentf(iw, "!!! %s", error_get_message(pUnit->status));
             }
             iw_cmt_newline(iw);
         }
@@ -514,9 +515,8 @@ void ureg_report_active_units(TF_ID frame_id)
     }
     msglen += count; // one byte per message for the callsign
 
-    bool suc = true;
-    uint8_t *buff = calloc_ck(1, msglen, &suc);
-    if (!suc) {
+    uint8_t *buff = calloc_ck(1, msglen);
+    if (buff == NULL) {
         com_respond_error(frame_id, E_OUT_OF_MEM);
         return;
     }
@@ -539,7 +539,7 @@ void ureg_report_active_units(TF_ID frame_id)
 
         com_respond_buf(frame_id, MSG_SUCCESS, buff, msglen);
     }
-    free(buff);
+    free_ck(buff);
 }
 
 Unit *ureg_get_rsc_owner(Resource resource)
@@ -583,3 +583,21 @@ void ureg_print_unit_resources(IniWriter *iw)
     }
     iw_newline(iw);
 }
+
+void ureg_tick_units(void)
+{
+    UlistEntry *li = ulist_head;
+    while (li != NULL) {
+        Unit *const pUnit = &li->unit;
+        if (pUnit->status == E_SUCCESS && pUnit->tick_interval > 0) {
+            if (pUnit->_tick_cnt == 0) {
+                if (pUnit->driver->updateTick) {
+                    pUnit->driver->updateTick(pUnit);
+                }
+                pUnit->_tick_cnt = pUnit->tick_interval;
+            }
+            pUnit->_tick_cnt--;
+        }
+        li = li->next;
+    }
+}
diff --git a/framework/unit_registry.h b/framework/unit_registry.h
index d42f76a..23ab198 100644
--- a/framework/unit_registry.h
+++ b/framework/unit_registry.h
@@ -142,4 +142,9 @@ Unit *ureg_get_rsc_owner(Resource resource);
  */
 void ureg_print_unit_resources(IniWriter *iw);
 
+/**
+ * 1ms tick for all units that want it
+ */
+void ureg_tick_units(void);
+
 #endif //GEX_UNIT_REGISTRY_H
diff --git a/gex.mk b/gex.mk
index f630515..0b7e2fe 100644
--- a/gex.mk
+++ b/gex.mk
@@ -5,11 +5,11 @@ GEX_SRC_DIR = \
     User/framework \
     User/platform \
     User/units \
-    User/units/system \
     User/units/neopixel \
     User/units/test \
     User/units/digital_out \
     User/units/digital_in \
+    User/units/usart \
     User/units/i2c \
     User/units/spi \
     User/TinyFrame \
@@ -34,6 +34,7 @@ GEX_INCLUDES = \
     -IUser/TinyFrame \
     -IUser/vfs \
     -IUser/utils \
+    -IUser/units \
     -IUser/framework \
     -IUser/platform \
     -IUser/tasks \
@@ -56,7 +57,8 @@ GEX_CFLAGS = \
     -MD -Wno-redundant-decls -Wno-unused-parameter \
     -Wno-unused-variable -Wno-inline \
     -fmerge-constants -fmerge-all-constants -Wno-implicit-fallthrough \
-    -fno-exceptions -finline-small-functions -findirect-inlining -Wno-strict-aliasing -Wno-float-equal -Wno-discarded-qualifiers
+    -fno-exceptions -finline-small-functions -findirect-inlining -Wno-strict-aliasing -Wno-float-equal \
+    -Wno-discarded-qualifiers -fstack-usage
 
 GEX_CDEFS_BASE =  \
     -D__weak="__attribute__((weak))" \
@@ -69,23 +71,27 @@ ifeq '$(DISABLE_DEBUG)' '1'
 
 GEX_CDEFS = $(GEX_CDEFS_BASE) \
     -DUSE_FULL_ASSERT=0 \
-    -DVERBOSE_ASSERT=0 \
+    -DASSERT_FILENAMES=0 \
     -DDEBUG_VFS=0 \
     -DDEBUG_FLASH_WRITE=0 \
     -DVERBOSE_HARDFAULT=0 \
     -DUSE_STACK_MONITOR=0 \
-    -DUSE_DEBUG_UART=0
+    -DUSE_DEBUG_UART=0 \
+    -DDEBUG_MALLOC=0 \
+    -DDEBUG_RSC=0
 
 else
 
 GEX_CDEFS = $(GEX_CDEFS_BASE) \
     -DUSE_FULL_ASSERT=1 \
-    -DVERBOSE_ASSERT=1 \
+    -DASSERT_FILENAMES=1 \
     -DDEBUG_VFS=0 \
     -DDEBUG_FLASH_WRITE=0 \
     -DVERBOSE_HARDFAULT=1 \
-    -DUSE_STACK_MONITOR=1 \
-    -DUSE_DEBUG_UART=1
+    -DUSE_STACK_MONITOR=0 \
+    -DUSE_DEBUG_UART=1 \
+    -DDEBUG_MALLOC=0 \
+    -DDEBUG_RSC=0
 
 endif
 
diff --git a/gex_hooks.c b/gex_hooks.c
index 8712475..e60f4bb 100644
--- a/gex_hooks.c
+++ b/gex_hooks.c
@@ -9,6 +9,7 @@
 #include "platform/status_led.h"
 #include "platform/debug_uart.h"
 #include "gex_hooks.h"
+#include "unit_registry.h"
 
 /**
  * This is a systick callback for GEX application logic
@@ -17,6 +18,7 @@ void GEX_MsTick(void)
 {
     TF_Tick(comm);
     Indicator_Tick();
+    ureg_tick_units();
 }
 
 /**
diff --git a/platform/debug_uart.c b/platform/debug_uart.c
index 6b57eab..ce28d75 100644
--- a/platform/debug_uart.c
+++ b/platform/debug_uart.c
@@ -6,26 +6,74 @@
 #include "framework/resources.h"
 #include "debug_uart.h"
 #include "plat_compat.h"
+#include "hw_utils.h"
 
 #if USE_DEBUG_UART
 
+#define DEBUG_USART_BAUD 115200
+
+#if GEX_PLAT_F072_DISCOVERY
+
+    #define DEBUG_USART USART1
+    #define DEBUG_USART_RSC R_USART1
+    #define DEBUG_USART_PORT 'A'
+    #define DEBUG_USART_PIN  9
+    #define DEBUG_USART_AF  1
+    #define DEBUG_USART_PCLK PLAT_APB1_HZ
+
+#elif GEX_PLAT_F103_BLUEPILL
+
+    #define DEBUG_USART USART2
+    #define DEBUG_USART_RSC R_USART2
+    #define DEBUG_USART_PORT 'A'
+    #define DEBUG_USART_PIN  2
+    #define DEBUG_USART_PCLK PLAT_APB1_HZ
+
+#elif GEX_PLAT_F303_DISCOVERY
+
+    #define DEBUG_USART USART3
+    #define DEBUG_USART_RSC R_USART3
+    #define DEBUG_USART_PORT 'D'
+    #define DEBUG_USART_PIN  8
+    #define DEBUG_USART_AF  7
+    #define DEBUG_USART_PCLK PLAT_APB1_HZ
+
+#elif GEX_PLAT_F407_DISCOVERY
+
+    #define DEBUG_USART USART2
+    #define DEBUG_USART_RSC R_USART2
+    #define DEBUG_USART_PORT 'A'
+    #define DEBUG_USART_PIN  2
+    #define DEBUG_USART_AF  7
+    #define DEBUG_USART_PCLK PLAT_APB1_HZ
+
+#else
+    #error "BAD PLATFORM!"
+#endif
+
+
+
 /** Init the submodule. */
 void DebugUart_Init(void)
 {
     // Debug UART
-    assert_param(E_SUCCESS == rsc_claim(&UNIT_SYSTEM, R_USART2));
-    assert_param(E_SUCCESS == rsc_claim(&UNIT_SYSTEM, R_PA2));
+    assert_param(E_SUCCESS == rsc_claim(&UNIT_SYSTEM, DEBUG_USART_RSC));
+    assert_param(E_SUCCESS == rsc_claim_pin(&UNIT_SYSTEM, DEBUG_USART_PORT, DEBUG_USART_PIN));
 }
 
 /** Init the hardware peripheral - this is called early in the boot process */
 void DebugUart_PreInit(void)
 {
-    __HAL_RCC_USART2_CLK_ENABLE();
-    __HAL_RCC_GPIOA_CLK_ENABLE();
+    // configure AF only if platform uses AF numbers
+#if !PLAT_NO_AFNUM
+    hw_configure_gpio_af(DEBUG_USART_PORT, DEBUG_USART_PIN, DEBUG_USART_AF);
+#endif
+
+    hw_periph_clock_enable(DEBUG_USART);
 
-    LL_GPIO_SetPinMode(GPIOA, LL_GPIO_PIN_2, LL_GPIO_MODE_ALTERNATE);
-    LL_GPIO_SetPinOutputType(GPIOA, LL_GPIO_PIN_2, LL_GPIO_OUTPUT_PUSHPULL);
-    LL_GPIO_SetPinSpeed(GPIOA, LL_GPIO_PIN_2, LL_GPIO_SPEED_FREQ_HIGH);
+//    LL_GPIO_SetPinMode(GPIOA, LL_GPIO_PIN_2, LL_GPIO_MODE_ALTERNATE);
+//    LL_GPIO_SetPinOutputType(GPIOA, LL_GPIO_PIN_2, LL_GPIO_OUTPUT_PUSHPULL);
+//    LL_GPIO_SetPinSpeed(GPIOA, LL_GPIO_PIN_2, LL_GPIO_SPEED_FREQ_HIGH);
 
     // commented out default values
 //    LL_USART_ConfigAsyncMode(USART2);
@@ -33,45 +81,23 @@ void DebugUart_PreInit(void)
 //    LL_USART_SetParity(USART2, LL_USART_PARITY_NONE);
 //    LL_USART_SetStopBitsLength(USART2, LL_USART_STOPBITS_1);
 //    LL_USART_SetHWFlowCtrl(USART2, LL_USART_HWCONTROL_NONE);
-    LL_USART_EnableDirectionTx(USART2);
-
-#if GEX_PLAT_F072_DISCOVERY
-    LL_USART_SetBaudRate(USART2, SystemCoreClock, LL_USART_OVERSAMPLING_16, 115200); // This is not great, let's hope it's like this on all platforms...
-    LL_GPIO_SetAFPin_0_7(GPIOA, LL_GPIO_PIN_2, LL_GPIO_AF_1);
-#elif GEX_PLAT_F103_BLUEPILL
-    LL_USART_SetBaudRate(USART2, SystemCoreClock/2, 115200); // This is not great, let's hope it's like this on all platforms...
-#elif GEX_PLAT_F303_DISCOVERY
-    LL_USART_SetBaudRate(USART2,
-                         LL_RCC_GetUSARTClockFreq(LL_RCC_USART2_CLKSOURCE),
-                         LL_USART_OVERSAMPLING_16,
-                         115200);
-    LL_GPIO_SetAFPin_0_7(GPIOA, LL_GPIO_PIN_2, LL_GPIO_AF_7); // uart2 is AF7 here
-#elif GEX_PLAT_F407_DISCOVERY
-    LL_USART_SetBaudRate(USART2,
-                         SystemCoreClock/4, // if core is at 168 MHz, this is 48 MHz
-                         LL_USART_OVERSAMPLING_16,
-                         115200);
-    LL_GPIO_SetAFPin_0_7(GPIOA, LL_GPIO_PIN_2, LL_GPIO_AF_7); // uart2 is AF7 here (same like 303)
-#else
-    #error "BAD PLATFORM!"
-#endif
+    LL_USART_EnableDirectionTx(DEBUG_USART);
+    LL_USART_SetBaudRate(DEBUG_USART, DEBUG_USART_PCLK, LL_USART_OVERSAMPLING_16, DEBUG_USART_BAUD);
+    LL_USART_Enable(DEBUG_USART);
+}
 
-    LL_USART_Enable(USART2);
+void debug_write(const char *buf, uint16_t len)
+{
+    for (uint16_t i = 0; i < len; i++) {
+        while (!LL_USART_IsActiveFlag_TC(DEBUG_USART));
+        LL_USART_TransmitData8(DEBUG_USART, (uint8_t) *buf++);
+    }
 }
 
 /** Debug print, used by debug / newlib */
 ssize_t _write_r(struct _reent *rptr, int fd, const void *buf, size_t len)
 {
-    (void)rptr;
-
-    uint8_t *buff = buf;
-
-    for (uint32_t i = 0; i < len; i++) {
-        while (!LL_USART_IsActiveFlag_TC(USART2));
-        LL_USART_TransmitData8(USART2, *buff++);
-    }
-
-    return len;
+    trap("Use of newlib printf");
 }
 
 #else
diff --git a/platform/debug_uart.h b/platform/debug_uart.h
index bd171f9..7971d82 100644
--- a/platform/debug_uart.h
+++ b/platform/debug_uart.h
@@ -8,4 +8,6 @@
 void DebugUart_PreInit(void);
 void DebugUart_Init(void);
 
+void debug_write(const char *buf, uint16_t len);
+
 #endif //GEX_DEBUG_UART_H
diff --git a/platform/pin_utils.c b/platform/hw_utils.c
similarity index 51%
rename from platform/pin_utils.c
rename to platform/hw_utils.c
index 3df83c7..5c0c9fb 100644
--- a/platform/pin_utils.c
+++ b/platform/hw_utils.c
@@ -3,7 +3,7 @@
 //
 
 #include <utils/avrlibc.h>
-#include "pin_utils.h"
+#include "hw_utils.h"
 #include "macro.h"
 
 #define PINS_COUNT 16
@@ -46,7 +46,7 @@ static GPIO_TypeDef * const port_periphs[] = {
 COMPILER_ASSERT(PORTS_COUNT == ELEMENTS_IN_ARRAY(port_periphs));
 
 /** Convert pin number to LL bitfield */
-uint32_t pin2ll(uint8_t pin_number, bool *suc)
+uint32_t hw_pin2ll(uint8_t pin_number, bool *suc)
 {
     assert_param(suc != NULL);
 
@@ -60,7 +60,7 @@ uint32_t pin2ll(uint8_t pin_number, bool *suc)
 }
 
 /** Convert port name (A,B,C...) to peripheral struct pointer */
-GPIO_TypeDef *port2periph(char port_name, bool *suc)
+GPIO_TypeDef *hw_port2periph(char port_name, bool *suc)
 {
     assert_param(suc != NULL);
 
@@ -75,7 +75,7 @@ GPIO_TypeDef *port2periph(char port_name, bool *suc)
 }
 
 /** Convert a pin to resource handle */
-Resource pin2resource(char port_name, uint8_t pin_number, bool *suc)
+Resource hw_pin2resource(char port_name, uint8_t pin_number, bool *suc)
 {
     assert_param(suc != NULL);
 
@@ -116,7 +116,7 @@ bool parse_pin(const char *value, char *targetName, uint8_t *targetNumber)
 }
 
 /** Parse port name */
-bool parse_port(const char *value, char *targetName)
+bool parse_port_name(const char *value, char *targetName)
 {
     *targetName = (uint8_t) value[0];
     if (!(*targetName >= 'A' && *targetName < 'A' + PORTS_COUNT)) return false;
@@ -177,7 +177,7 @@ uint16_t parse_pinmask(const char *value, bool *suc)
 }
 
 /** Convert a pin bitmask to the ASCII format understood by str_parse_pinmask() */
-char * str_pinmask(uint16_t pins, char *buffer)
+char * pinmask2str(uint16_t pins, char *buffer)
 {
     char *b = buffer;
     uint32_t start = 0;
@@ -230,7 +230,7 @@ char * str_pinmask(uint16_t pins, char *buffer)
 }
 
 /** Spread packed port pins using a mask */
-uint16_t port_spread(uint16_t packed, uint16_t mask)
+uint16_t pinmask_spread(uint16_t packed, uint16_t mask)
 {
     uint16_t result = 0;
     uint16_t poke = 1;
@@ -246,7 +246,7 @@ uint16_t port_spread(uint16_t packed, uint16_t mask)
 }
 
 /** Pack spread port pins using a mask */
-uint16_t port_pack(uint16_t spread, uint16_t mask)
+uint16_t pinmask_pack(uint16_t spread, uint16_t mask)
 {
     uint16_t result = 0;
     uint16_t poke = 1;
@@ -262,11 +262,11 @@ uint16_t port_pack(uint16_t spread, uint16_t mask)
 }
 
 /** Configure unit pins as analog (part of unit teardown) */
-void deinit_unit_pins(Unit *unit)
+void hw_deinit_unit_pins(Unit *unit)
 {
     for (uint32_t rsc = R_PA0; rsc <= R_PF15; rsc++) {
         if (RSC_IS_HELD(unit->resources, rsc)) {
-//            dbg("Freeing pin %s", rsc_get_name((Resource)rsc));
+            rsc_dbg("Freeing pin %s", rsc_get_name((Resource)rsc));
             GPIO_TypeDef *port = port_periphs[(rsc-R_PA0) / 16];
             uint32_t ll_pin = ll_pins[(rsc-R_PA0)%16];
             LL_GPIO_SetPinMode(port, ll_pin, LL_GPIO_MODE_ANALOG);
@@ -275,11 +275,15 @@ void deinit_unit_pins(Unit *unit)
 }
 
 /** Configure a pin to alternate function */
-error_t configure_gpio_alternate(char port_name, uint8_t pin_num, uint32_t ll_af)
+error_t hw_configure_gpio_af(char port_name, uint8_t pin_num, uint32_t ll_af)
 {
+#if PLAT_NO_AFNUM
+    trap("Illegal call to hw_configure_gpio_af() on this platform");
+#else
+
     bool suc = true;
-    GPIO_TypeDef *port = port2periph(port_name, &suc);
-    uint32_t ll_pin = pin2ll(pin_num, &suc);
+    GPIO_TypeDef *port = hw_port2periph(port_name, &suc);
+    uint32_t ll_pin = hw_pin2ll(pin_num, &suc);
     if (!suc) return E_BAD_CONFIG;
 
     if (pin_num < 8)
@@ -289,19 +293,21 @@ error_t configure_gpio_alternate(char port_name, uint8_t pin_num, uint32_t ll_af
 
     LL_GPIO_SetPinMode(port, ll_pin, LL_GPIO_MODE_ALTERNATE);
 
+#endif
     return E_SUCCESS;
 }
 
 /** Configure pins using sparse map */
-error_t configure_sparse_pins(char port_name, uint16_t mask, GPIO_TypeDef **port_dest, uint32_t ll_mode, uint32_t ll_otype)
+error_t hw_configure_sparse_pins(char port_name, uint16_t mask, GPIO_TypeDef **port_dest,
+                                 uint32_t ll_mode, uint32_t ll_otype)
 {
     bool suc = true;
-    GPIO_TypeDef *port = port2periph(port_name, &suc);
+    GPIO_TypeDef *port = hw_port2periph(port_name, &suc);
     if (!suc) return E_BAD_CONFIG;
 
     for (int i = 0; i < 16; i++) {
         if (mask & (1<<i)) {
-            uint32_t ll_pin = pin2ll((uint8_t) i, &suc);
+            uint32_t ll_pin = hw_pin2ll((uint8_t) i, &suc);
             LL_GPIO_SetPinMode(port, ll_pin, ll_mode);
             LL_GPIO_SetPinOutputType(port, ll_pin, ll_otype);
             LL_GPIO_SetPinSpeed(port, ll_pin, LL_GPIO_SPEED_FREQ_HIGH);
@@ -314,3 +320,212 @@ error_t configure_sparse_pins(char port_name, uint16_t mask, GPIO_TypeDef **port
 
     return E_SUCCESS;
 }
+
+void hw_periph_clock_enable(void *periph)
+{
+    // GPIOs are enabled by default on start-up
+
+    // --- USART ---
+    if (periph == USART1) __HAL_RCC_USART1_CLK_ENABLE();
+    else if (periph == USART2) __HAL_RCC_USART2_CLK_ENABLE();
+#ifdef USART3
+    else if (periph == USART3) __HAL_RCC_USART3_CLK_ENABLE();
+#endif
+#ifdef USART4
+    else if (periph == USART4) __HAL_RCC_USART4_CLK_ENABLE();
+#endif
+#ifdef USART5
+    else if (periph == USART5) __HAL_RCC_USART5_CLK_ENABLE();
+#endif
+
+    // --- SPI ---
+    else if (periph == SPI1) __HAL_RCC_SPI1_CLK_ENABLE();
+#ifdef SPI2
+    else if (periph == SPI2) __HAL_RCC_SPI2_CLK_ENABLE();
+#endif
+#ifdef SPI3
+    else if (periph == SPI3) __HAL_RCC_SPI3_CLK_ENABLE();
+#endif
+
+    // --- I2C ---
+    else if (periph == I2C1) __HAL_RCC_I2C1_CLK_ENABLE();
+    else if (periph == I2C2) __HAL_RCC_I2C2_CLK_ENABLE();
+#ifdef I2C3
+    else if (periph == I2C3) __HAL_RCC_I2C3_CLK_ENABLE();
+#endif
+
+    // --- DMA ---
+    else if (periph == DMA1) __HAL_RCC_DMA1_CLK_ENABLE();
+#ifdef DMA2
+    else if (periph == DMA2) __HAL_RCC_DMA2_CLK_ENABLE();
+#endif
+
+    // --- TIM ---
+    else if (periph == TIM1) __HAL_RCC_TIM1_CLK_ENABLE();
+    else if (periph == TIM2) __HAL_RCC_TIM2_CLK_ENABLE();
+    else if (periph == TIM3) __HAL_RCC_TIM3_CLK_ENABLE();
+#ifdef TIM4
+    else if (periph == TIM4) __HAL_RCC_TIM4_CLK_ENABLE();
+#endif
+#ifdef TIM5
+    else if (periph == TIM5) __HAL_RCC_TIM5_CLK_ENABLE();
+#endif
+#ifdef TIM6
+    else if (periph == TIM6) __HAL_RCC_TIM7_CLK_ENABLE();
+#endif
+#ifdef TIM7
+    else if (periph == TIM7) __HAL_RCC_TIM7_CLK_ENABLE();
+#endif
+#ifdef TIM8
+    else if (periph == TIM8) __HAL_RCC_TIM8_CLK_ENABLE();
+#endif
+#ifdef TIM9
+    else if (periph == TIM9) __HAL_RCC_TIM9_CLK_ENABLE();
+#endif
+#ifdef TIM11
+    else if (periph == TIM11) __HAL_RCC_TIM11_CLK_ENABLE();
+#endif
+#ifdef TIM12
+    else if (periph == TIM12) __HAL_RCC_TIM12_CLK_ENABLE();
+#endif
+#ifdef TIM13
+    else if (periph == TIM13) __HAL_RCC_TIM13_CLK_ENABLE();
+#endif
+#ifdef TIM14
+    else if (periph == TIM14) __HAL_RCC_TIM14_CLK_ENABLE();
+#endif
+#ifdef TIM15
+    else if (periph == TIM15) __HAL_RCC_TIM15_CLK_ENABLE();
+#endif
+#ifdef TIM16
+    else if (periph == TIM16) __HAL_RCC_TIM15_CLK_ENABLE();
+#endif
+#ifdef TIM17
+    else if (periph == TIM17) __HAL_RCC_TIM17_CLK_ENABLE();
+#endif
+
+    // --- ADC ---
+#ifdef ADC1
+    else if (periph == ADC1) __HAL_RCC_ADC1_CLK_ENABLE();
+#endif
+#ifdef ADC2
+    else if (periph == ADC2) __HAL_RCC_ADC2_CLK_ENABLE();
+#endif
+
+    // --- DAC ---
+#ifdef DAC1
+    else if (periph == DAC1) __HAL_RCC_DAC1_CLK_ENABLE();
+#endif
+#ifdef DAC2
+    else if (periph == DAC2) __HAL_RCC_DAC2_CLK_ENABLE();
+#endif
+    else {
+        dbg("Periph 0x%p missing in hw clock enable func", periph);
+        trap("BUG");
+    }
+}
+
+
+void hw_periph_clock_disable(void *periph)
+{
+    // GPIOs are enabled by default on start-up
+
+    // --- USART ---
+    if (periph == USART1) __HAL_RCC_USART1_CLK_DISABLE();
+    else if (periph == USART2) __HAL_RCC_USART2_CLK_DISABLE();
+#ifdef USART3
+    else if (periph == USART3) __HAL_RCC_USART3_CLK_DISABLE();
+#endif
+#ifdef USART4
+    else if (periph == USART4) __HAL_RCC_USART4_CLK_DISABLE();
+#endif
+#ifdef USART5
+    else if (periph == USART5) __HAL_RCC_USART5_CLK_DISABLE();
+#endif
+
+        // --- SPI ---
+    else if (periph == SPI1) __HAL_RCC_SPI1_CLK_DISABLE();
+#ifdef SPI2
+    else if (periph == SPI2) __HAL_RCC_SPI2_CLK_DISABLE();
+#endif
+#ifdef SPI3
+    else if (periph == SPI3) __HAL_RCC_SPI3_CLK_DISABLE();
+#endif
+
+        // --- I2C ---
+    else if (periph == I2C1) __HAL_RCC_I2C1_CLK_DISABLE();
+    else if (periph == I2C2) __HAL_RCC_I2C2_CLK_DISABLE();
+#ifdef I2C3
+    else if (periph == I2C3) __HAL_RCC_I2C3_CLK_DISABLE();
+#endif
+
+        // --- DMA ---
+    else if (periph == DMA1) __HAL_RCC_DMA1_CLK_DISABLE();
+#ifdef DMA2
+    else if (periph == DMA2) __HAL_RCC_DMA2_CLK_DISABLE();
+#endif
+
+        // --- TIM ---
+    else if (periph == TIM1) __HAL_RCC_TIM1_CLK_DISABLE();
+    else if (periph == TIM2) __HAL_RCC_TIM2_CLK_DISABLE();
+    else if (periph == TIM3) __HAL_RCC_TIM3_CLK_DISABLE();
+#ifdef TIM4
+    else if (periph == TIM4) __HAL_RCC_TIM4_CLK_DISABLE();
+#endif
+#ifdef TIM5
+    else if (periph == TIM5) __HAL_RCC_TIM5_CLK_DISABLE();
+#endif
+#ifdef TIM6
+    else if (periph == TIM6) __HAL_RCC_TIM7_CLK_DISABLE();
+#endif
+#ifdef TIM7
+    else if (periph == TIM7) __HAL_RCC_TIM7_CLK_DISABLE();
+#endif
+#ifdef TIM8
+    else if (periph == TIM8) __HAL_RCC_TIM8_CLK_DISABLE();
+#endif
+#ifdef TIM9
+    else if (periph == TIM9) __HAL_RCC_TIM9_CLK_DISABLE();
+#endif
+#ifdef TIM11
+    else if (periph == TIM11) __HAL_RCC_TIM11_CLK_DISABLE();
+#endif
+#ifdef TIM12
+    else if (periph == TIM12) __HAL_RCC_TIM12_CLK_DISABLE();
+#endif
+#ifdef TIM13
+    else if (periph == TIM13) __HAL_RCC_TIM13_CLK_DISABLE();
+#endif
+#ifdef TIM14
+    else if (periph == TIM14) __HAL_RCC_TIM14_CLK_DISABLE();
+#endif
+#ifdef TIM15
+    else if (periph == TIM15) __HAL_RCC_TIM15_CLK_DISABLE();
+#endif
+#ifdef TIM16
+    else if (periph == TIM16) __HAL_RCC_TIM15_CLK_DISABLE();
+#endif
+#ifdef TIM17
+    else if (periph == TIM17) __HAL_RCC_TIM17_CLK_DISABLE();
+#endif
+
+        // --- ADC ---
+#ifdef ADC1
+    else if (periph == ADC1) __HAL_RCC_ADC1_CLK_DISABLE();
+#endif
+#ifdef ADC2
+    else if (periph == ADC2) __HAL_RCC_ADC2_CLK_DISABLE();
+#endif
+
+        // --- DAC ---
+#ifdef DAC1
+    else if (periph == DAC1) __HAL_RCC_DAC1_CLK_DISABLE();
+#endif
+#ifdef DAC2
+        else if (periph == DAC2) __HAL_RCC_DAC2_CLK_DISABLE();
+#endif
+    else {
+        dbg("Periph 0x%p missing in hw clock disable func", periph);
+        trap("BUG");
+    }
+}
diff --git a/platform/pin_utils.h b/platform/hw_utils.h
similarity index 56%
rename from platform/pin_utils.h
rename to platform/hw_utils.h
index 0b2413a..aefeeb3 100644
--- a/platform/pin_utils.h
+++ b/platform/hw_utils.h
@@ -7,6 +7,7 @@
 #ifndef GEX_PIN_UTILS_H
 #define GEX_PIN_UTILS_H
 
+#include <stm32f072xb.h>
 #include "platform.h"
 #include "resources.h"
 
@@ -17,7 +18,7 @@
  * @param suc - set to false on failure, left unchanged on success.
  * @return LL_GPIO_PIN_x
  */
-uint32_t pin2ll(uint8_t pin_number, bool *suc);
+uint32_t hw_pin2ll(uint8_t pin_number, bool *suc);
 
 /**
  * Convert pin name and number to a resource enum
@@ -27,7 +28,7 @@ uint32_t pin2ll(uint8_t pin_number, bool *suc);
  * @param suc - set to false on failure, left unchanged on success
  * @return the resource, or R_NONE
  */
-Resource pin2resource(char port_name, uint8_t pin_number, bool *suc);
+Resource hw_pin2resource(char port_name, uint8_t pin_number, bool *suc);
 
 /**
  * Convert port name to peripheral instance
@@ -36,7 +37,7 @@ Resource pin2resource(char port_name, uint8_t pin_number, bool *suc);
  * @param suc - set to false on failure, left unchanged on success.
  * @return instance
  */
-GPIO_TypeDef *port2periph(char port_name, bool *suc);
+GPIO_TypeDef *hw_port2periph(char port_name, bool *suc);
 
 /**
  * Parse a pin name (e.g. PA0 or A0) to port name and pin number
@@ -55,7 +56,7 @@ bool parse_pin(const char *str, char *targetName, uint8_t *targetNumber);
  * @param targetName - output: port name (one character)
  * @return success
  */
-bool parse_port(const char *value, char *targetName);
+bool parse_port_name(const char *value, char *targetName);
 
 /**
  * Parse a list of pin numbers with ranges and commands/semicolons to a bitmask.
@@ -76,7 +77,7 @@ uint16_t parse_pinmask(const char *value, bool *suc);
  * @param buffer - output string buffer
  * @return the output buffer
  */
-char * str_pinmask(uint16_t pins, char *buffer);
+char * pinmask2str(uint16_t pins, char *buffer);
 
 /**
  * Spread packed port pins using a mask
@@ -85,7 +86,7 @@ char * str_pinmask(uint16_t pins, char *buffer);
  * @param mask - positions of the bits (eg. 0x8803)
  * @return - bits spread to their positions (always counting from right)
  */
-uint16_t port_spread(uint16_t packed, uint16_t mask);
+uint16_t pinmask_spread(uint16_t packed, uint16_t mask);
 
 /**
  * Pack spread port pins using a mask
@@ -94,7 +95,7 @@ uint16_t port_spread(uint16_t packed, uint16_t mask);
  * @param mask - mask of the bits we want to pack (eg. 0x8803)
  * @return - packed bits, right aligned (eg. 0b1110)
  */
-uint16_t port_pack(uint16_t spread, uint16_t mask);
+uint16_t pinmask_pack(uint16_t spread, uint16_t mask);
 
 /**
  * Set all GPIO resources held by unit to analog.
@@ -102,7 +103,7 @@ uint16_t port_pack(uint16_t spread, uint16_t mask);
  *
  * @param unit - holding unit
  */
-void deinit_unit_pins(Unit *unit);
+void hw_deinit_unit_pins(Unit *unit);
 
 /**
  * Configure a GPIO pin to alternate function.
@@ -112,7 +113,7 @@ void deinit_unit_pins(Unit *unit);
  * @param ll_af - LL alternate function constant
  * @return success
  */
-error_t configure_gpio_alternate(char port_name, uint8_t pin_num, uint32_t ll_af);
+error_t hw_configure_gpio_af(char port_name, uint8_t pin_num, uint32_t ll_af);
 
 /**
  * Configure multiple pins using the bitmap pattern
@@ -124,6 +125,69 @@ error_t configure_gpio_alternate(char port_name, uint8_t pin_num, uint32_t ll_af
  * @param ll_otype - LL output type (push/pull, opendrain)
  * @return success
  */
-error_t configure_sparse_pins(char port_name, uint16_t mask, GPIO_TypeDef **port_dest, uint32_t ll_mode, uint32_t ll_otype);
+error_t hw_configure_sparse_pins(char port_name,
+                                 uint16_t mask, GPIO_TypeDef **port_dest,
+                                 uint32_t ll_mode, uint32_t ll_otype);
+
+/** Helper struct for defining alternate mappings */
+struct PinAF {
+    char port;
+    uint8_t pin;
+    uint8_t af;
+};
+
+/**
+ * Enable a peripheral clock
+ * @param periph - any peripheral
+ */
+void hw_periph_clock_enable(void *periph);
+
+/**
+ * Disable a peripheral clock
+ * @param periph - any peripheral
+ */
+void hw_periph_clock_disable(void *periph);
+
+// ---------- LL extras ------------
+
+static inline bool LL_DMA_IsActiveFlag_G(uint32_t isr_snapshot, uint8_t channel)
+{
+    return 0 != (isr_snapshot & (DMA_ISR_GIF1 << (uint32_t)((channel-1) * 4)));
+}
+
+static inline bool LL_DMA_IsActiveFlag_TC(uint32_t isr_snapshot, uint8_t channel)
+{
+    return 0 != (isr_snapshot & (DMA_ISR_TCIF1 << (uint32_t)((channel-1) * 4)));
+}
+
+static inline bool LL_DMA_IsActiveFlag_HT(uint32_t isr_snapshot, uint8_t channel)
+{
+    return 0 != (isr_snapshot & (DMA_ISR_HTIF1 << (uint32_t)((channel-1) * 4)));
+}
+
+static inline bool LL_DMA_IsActiveFlag_TE(uint32_t isr_snapshot, uint8_t channel)
+{
+    return 0 != (isr_snapshot & (DMA_ISR_TEIF1 << (uint32_t)((channel-1) * 4)));
+}
+
+static inline void LL_DMA_ClearFlag_HT(DMA_TypeDef *DMAx, uint8_t channel)
+{
+    DMAx->IFCR = (DMA_IFCR_CHTIF1 << (uint32_t)((channel-1) * 4));
+}
+
+static inline void LL_DMA_ClearFlag_TC(DMA_TypeDef *DMAx, uint8_t channel)
+{
+    DMAx->IFCR = (DMA_IFCR_CTCIF1 << (uint32_t)((channel-1) * 4));
+}
+
+static inline void LL_DMA_ClearFlag_TE(DMA_TypeDef *DMAx, uint8_t channel)
+{
+    DMAx->IFCR = (DMA_IFCR_CTEIF1 << (uint32_t)((channel-1) * 4));
+}
+
+static inline void LL_DMA_ClearFlags(DMA_TypeDef *DMAx, uint8_t channel)
+{
+    DMAx->IFCR = (DMA_IFCR_CGIF1 << (uint32_t)((channel-1) * 4));
+}
 
 #endif //GEX_PIN_UTILS_H
diff --git a/platform/irq_dispatcher.c b/platform/irq_dispatcher.c
new file mode 100644
index 0000000..40715a1
--- /dev/null
+++ b/platform/irq_dispatcher.c
@@ -0,0 +1,378 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+
+#include <stm32f072xb.h>
+#include "platform.h"
+#include "irq_dispatcher.h"
+
+void irqd_init(void)
+{
+//    NVIC_EnableIRQ(WWDG_IRQn);                  /*!< Window WatchDog Interrupt                                       */
+//    NVIC_EnableIRQ(PVD_VDDIO2_IRQn);            /*!< PVD & VDDIO2 Interrupt through EXTI Lines 16 and 31             */
+//    NVIC_EnableIRQ(RTC_IRQn);                   /*!< RTC Interrupt through EXTI Lines 17, 19 and 20                  */
+//    NVIC_EnableIRQ(FLASH_IRQn);                 /*!< FLASH global Interrupt                                          */
+//    NVIC_EnableIRQ(RCC_CRS_IRQn);               /*!< RCC & CRS global Interrupt                                      */
+//    NVIC_EnableIRQ(EXTI0_1_IRQn);               /*!< EXTI Line 0 and 1 Interrupt                                     */
+//    NVIC_EnableIRQ(EXTI2_3_IRQn);               /*!< EXTI Line 2 and 3 Interrupt                                     */
+//    NVIC_EnableIRQ(EXTI4_15_IRQn);              /*!< EXTI Line 4 to 15 Interrupt                                     */
+//    NVIC_EnableIRQ(TSC_IRQn);                   /*!< Touch Sensing Controller Interrupts                             */
+    NVIC_EnableIRQ(DMA1_Channel1_IRQn);         /*!< DMA1 Channel 1 Interrupt                                        */
+    NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);       /*!< DMA1 Channel 2 and Channel 3 Interrupt                          */
+    NVIC_EnableIRQ(DMA1_Channel4_5_6_7_IRQn);   /*!< DMA1 Channel 4 to Channel 7 Interrupt                           */
+    HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 3, 0);
+    HAL_NVIC_SetPriority(DMA1_Channel2_3_IRQn, 3, 0);
+    HAL_NVIC_SetPriority(DMA1_Channel4_5_6_7_IRQn, 3, 0);
+//    NVIC_EnableIRQ(ADC1_COMP_IRQn);             /*!< ADC1 and COMP interrupts (ADC interrupt combined with EXTI Lines 21 and 22 */
+//    NVIC_EnableIRQ(TIM2_IRQn);                  /*!< TIM2 global Interrupt                                           */
+//    NVIC_EnableIRQ(TIM3_IRQn);                  /*!< TIM3 global Interrupt                                           */
+//    NVIC_EnableIRQ(TIM6_DAC_IRQn);              /*!< TIM6 global and DAC channel underrun error Interrupt            */
+//    NVIC_EnableIRQ(TIM7_IRQn);                  /*!< TIM7 global Interrupt                                           */
+//    NVIC_EnableIRQ(TIM14_IRQn);                 /*!< TIM14 global Interrupt                                          */
+//    NVIC_EnableIRQ(TIM15_IRQn);                 /*!< TIM15 global Interrupt                                          */
+//    NVIC_EnableIRQ(TIM16_IRQn);                 /*!< TIM16 global Interrupt                                          */
+//    NVIC_EnableIRQ(TIM17_IRQn);                 /*!< TIM17 global Interrupt                                          */
+//    NVIC_EnableIRQ(I2C1_IRQn);                  /*!< I2C1 Event Interrupt & EXTI Line23 Interrupt (I2C1 wakeup)      */
+//    NVIC_EnableIRQ(I2C2_IRQn);                  /*!< I2C2 Event Interrupt                                            */
+//    NVIC_EnableIRQ(SPI1_IRQn);                  /*!< SPI1 global Interrupt                                           */
+//    NVIC_EnableIRQ(SPI2_IRQn);                  /*!< SPI2 global Interrupt                                           */
+    NVIC_EnableIRQ(USART1_IRQn);                /*!< USART1 global Interrupt & EXTI Line25 Interrupt (USART1 wakeup) */
+    NVIC_EnableIRQ(USART2_IRQn);                /*!< USART2 global Interrupt & EXTI Line26 Interrupt (USART2 wakeup) */
+    NVIC_EnableIRQ(USART3_4_IRQn);              /*!< USART3 and USART4 global Interrupt                              */
+    HAL_NVIC_SetPriority(USART1_IRQn, 3, 0);
+    HAL_NVIC_SetPriority(USART2_IRQn, 3, 0);
+    HAL_NVIC_SetPriority(USART3_4_IRQn, 3, 0);
+//    NVIC_EnableIRQ(CEC_CAN_IRQn);               /*!< CEC and CAN global Interrupts & EXTI Line27 Interrupt           */
+
+//    NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn);   /*!< TIM1 Break, Update, Trigger and Commutation Interrupt           */ // - handled by hal msp init
+//    NVIC_EnableIRQ(TIM1_CC_IRQn);               /*!< TIM1 Capture Compare Interrupt                                  */
+//    NVIC_EnableIRQ(USB_IRQn);                   /*!< USB global Interrupt  & EXTI Line18 Interrupt                   */ // - USB IRQ is handled by the USB library
+}
+
+//void Default_Handler(void)
+//{
+//    warn("Missing IRQHandler, ISPR[0]=0x%p", (void*)NVIC->ISPR[0]);
+//}
+
+struct cbslot {
+    IrqCallback callback;
+    void *arg;
+};
+
+static struct callbacks_ {
+    struct cbslot usart1;
+    struct cbslot usart2;
+    struct cbslot usart3;
+#ifdef USART4
+    struct cbslot usart4;
+#endif
+#ifdef USART5
+    struct cbslot usart5;
+#endif
+    struct cbslot dma1_1;
+    struct cbslot dma1_2;
+    struct cbslot dma1_3;
+    struct cbslot dma1_4;
+    struct cbslot dma1_5;
+    struct cbslot dma1_6;
+    struct cbslot dma1_7;
+    struct cbslot dma1_8;
+
+    struct cbslot dma2_1;
+    struct cbslot dma2_2;
+    struct cbslot dma2_3;
+    struct cbslot dma2_4;
+    struct cbslot dma2_5;
+    struct cbslot dma2_6;
+    struct cbslot dma2_7;
+    struct cbslot dma2_8;
+
+    // XXX add more callbacks here when needed
+} callbacks = {0};
+
+static struct cbslot *get_slot_for_periph(void *periph)
+{
+    struct cbslot *slot = NULL;
+    // --- USART ---
+    if (periph == USART1) slot = &callbacks.usart1;
+    else if (periph == USART2) slot = &callbacks.usart2;
+    else if (periph == USART3) slot = &callbacks.usart3;
+#ifdef USART4
+    else if (periph == USART4) slot = &callbacks.usart4;
+#endif
+#ifdef USART5
+        else if (periph == USART5) slot = &callbacks.usart5;
+#endif
+
+        // --- DMA1 ---
+    else if (periph == DMA1_Channel1) slot = &callbacks.dma1_1;
+    else if (periph == DMA1_Channel2) slot = &callbacks.dma1_2;
+    else if (periph == DMA1_Channel3) slot = &callbacks.dma1_3;
+    else if (periph == DMA1_Channel4) slot = &callbacks.dma1_4;
+    else if (periph == DMA1_Channel5) slot = &callbacks.dma1_5;
+    else if (periph == DMA1_Channel6) slot = &callbacks.dma1_6;
+    else if (periph == DMA1_Channel7) slot = &callbacks.dma1_7;
+#ifdef DMA1_Channel8
+        else if (periph == DMA1_Channel7) slot = &callbacks.dma1_8;
+#endif
+
+        // --- DMA2 ---
+#ifdef DMA2
+        else if (periph == DMA2_Channel1) slot = &callbacks.dma2_1;
+    else if (periph == DMA2_Channel2) slot = &callbacks.dma2_2;
+    else if (periph == DMA2_Channel3) slot = &callbacks.dma2_3;
+    else if (periph == DMA2_Channel4) slot = &callbacks.dma2_4;
+    else if (periph == DMA2_Channel5) slot = &callbacks.dma2_5;
+    #ifdef DMA2_Channel6
+        else if (periph == DMA2_Channel6) slot = &callbacks.dma2_6;
+    #endif
+    #ifdef DMA2_Channel7
+        else if (periph == DMA2_Channel7) slot = &callbacks.dma2_7;
+    #endif
+    #ifdef DMA2_Channel8
+        else if (periph == DMA2_Channel7) slot = &callbacks.dma2_8;
+    #endif
+#endif
+    else {
+        trap("No IRQ cb slot for periph 0x%p", periph);
+    }
+
+    return slot;
+}
+
+void irqd_attach(void *periph, IrqCallback callback, void *arg)
+{
+    struct cbslot *slot = get_slot_for_periph(periph);
+    assert_param(slot->callback == NULL);
+    slot->callback = callback;
+    slot->arg = arg;
+}
+
+void irqd_detach(void *periph, IrqCallback callback)
+{
+    struct cbslot *slot = get_slot_for_periph(periph);
+    if (slot->callback == callback) {
+        slot->callback = NULL;
+        slot->arg = NULL;
+    }
+}
+
+#define CALL_IRQ_HANDLER(slot) do { if (slot.callback) slot.callback(slot.arg); } while (0)
+
+void USART1_IRQHandler(void)
+{
+    CALL_IRQ_HANDLER(callbacks.usart1);
+}
+
+void USART2_IRQHandler(void)
+{
+    CALL_IRQ_HANDLER(callbacks.usart2);
+}
+
+#if 0
+static bool usart_check_irq(USART_TypeDef *USARTx)
+{
+    uint32_t isrflags   = USARTx->ISR;
+    uint32_t cr1its     = USARTx->CR1;
+    uint32_t cr2its     = USARTx->CR2;
+    uint32_t cr3its     = USARTx->CR3;
+
+    if ((cr1its & USART_CR1_RTOIE) && (isrflags & USART_ISR_RTOF)) return true;
+    if ((cr1its & USART_CR1_RXNEIE) && (isrflags & USART_ISR_RXNE)) return true;
+    if ((cr1its & USART_CR1_TCIE) && (isrflags & USART_ISR_TC)) return true;
+    if ((cr1its & USART_CR1_TXEIE) && (isrflags & USART_ISR_TXE)) return true;
+    if ((cr3its & USART_CR3_EIE) && (isrflags & (USART_ISR_PE | USART_ISR_FE | USART_ISR_ORE | USART_ISR_NE))) return true;
+
+    if ((cr1its & USART_CR1_IDLEIE) && (isrflags & USART_ISR_IDLE)) return true;
+    if ((cr1its & USART_CR1_PEIE) && (isrflags & USART_ISR_PE)) return true;
+    if ((cr1its & USART_CR1_CMIE) && (isrflags & USART_ISR_CMF)) return true;
+    if ((cr1its & USART_CR1_EOBIE) && (isrflags & USART_ISR_EOBF)) return true;
+    if ((cr2its & USART_CR2_LBDIE) && (isrflags & USART_ISR_LBDF)) return true;
+    if ((cr3its & USART_CR3_CTSIE) && (isrflags & USART_ISR_CTS)) return true;
+    if ((cr3its & USART_CR3_WUFIE) && (isrflags & USART_ISR_WUF)) return true;
+
+    return false;
+}
+#endif
+
+void USART3_4_IRQHandler(void)
+{
+    // we won't check the flags here, both can be pending and it's better to let the handler deal with it
+    CALL_IRQ_HANDLER(callbacks.usart3);
+    CALL_IRQ_HANDLER(callbacks.usart4);
+}
+
+void DMA1_Channel1_IRQHandler(void)
+{
+    CALL_IRQ_HANDLER(callbacks.dma1_1);
+}
+
+void DMA1_Channel2_3_IRQHandler(void)
+{
+    if (LL_DMA_IsActiveFlag_GI2(DMA1)) CALL_IRQ_HANDLER(callbacks.dma1_2);
+    if (LL_DMA_IsActiveFlag_GI3(DMA1)) CALL_IRQ_HANDLER(callbacks.dma1_3);
+}
+
+void DMA1_Channel4_5_6_7_IRQHandler(void)
+{
+    if (LL_DMA_IsActiveFlag_GI4(DMA1)) CALL_IRQ_HANDLER(callbacks.dma1_4);
+    if (LL_DMA_IsActiveFlag_GI5(DMA1)) CALL_IRQ_HANDLER(callbacks.dma1_5);
+    if (LL_DMA_IsActiveFlag_GI6(DMA1)) CALL_IRQ_HANDLER(callbacks.dma1_6);
+    if (LL_DMA_IsActiveFlag_GI7(DMA1)) CALL_IRQ_HANDLER(callbacks.dma1_7);
+}
+
+#if 0
+void WWDG_IRQHandler(void)
+{
+    //
+}
+
+void PVD_VDDIO2_IRQHandler(void)
+{
+    //
+}
+
+void RTC_IRQHandler(void)
+{
+    //
+}
+
+void FLASH_IRQHandler(void)
+{
+    //
+}
+
+void RCC_CRS_IRQHandler(void)
+{
+    //
+}
+
+void EXTI0_1_IRQHandler(void)
+{
+    //
+}
+
+void EXTI2_3_IRQHandler(void)
+{
+    //
+}
+
+void EXTI4_15_IRQHandler(void)
+{
+    //
+}
+
+void TSC_IRQHandler(void)
+{
+    //
+}
+
+void ADC1_COMP_IRQHandler(void)
+{
+    //
+}
+
+void TIM1_BRK_UP_TRG_COM_IRQHandler(void)
+{
+    //
+}
+
+void TIM1_CC_IRQHandler(void)
+{
+    //
+}
+
+void TIM2_IRQHandler(void)
+{
+    //
+}
+
+void TIM3_IRQHandler(void)
+{
+    //
+}
+
+// not on F072
+void TIM4_IRQHandler(void)
+{
+    //
+}
+
+// not on F072
+void TIM5_IRQHandler(void)
+{
+    //
+}
+
+void TIM6_DAC_IRQHandler(void)
+{
+    //
+}
+
+void TIM7_IRQHandler(void)
+{
+    //
+}
+
+void TIM8_IRQHandler(void)
+{
+    //
+}
+
+void TIM9_IRQHandler(void)
+{
+    //
+}
+
+void TIM10_IRQHandler(void)
+{
+    //
+}
+
+void TIM11_IRQHandler(void)
+{
+    //
+}
+
+void TIM12_IRQHandler(void)
+{
+    //
+}
+
+void TIM13_IRQHandler(void)
+{
+    //
+}
+
+void TIM14_IRQHandler(void)
+{
+    //
+}
+
+void TIM15_IRQHandler(void)
+{
+    //
+}
+
+void TIM16_IRQHandler(void)
+{
+    //
+}
+
+void TIM17_IRQHandler(void)
+{
+    //
+}
+
+void CEC_CAN_IRQHandler(void)
+{
+    //
+}
+
+// USB is handled by the USB driver
+
+//void USB_IRQHandler(void)
+//{
+//    //
+//}
+#endif
diff --git a/platform/irq_dispatcher.h b/platform/irq_dispatcher.h
new file mode 100644
index 0000000..76cb731
--- /dev/null
+++ b/platform/irq_dispatcher.h
@@ -0,0 +1,39 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+
+#ifndef GEX_F072_IRQ_DISPATCHER_H
+#define GEX_F072_IRQ_DISPATCHER_H
+
+/**
+ * Initialize the interrupt dispatcher
+ */
+void irqd_init(void);
+
+/** Typedef for a IRQ callback run by the dispatcher */
+typedef void (*IrqCallback)(void*);
+
+/**
+ * Attach a callback to a IRQ handler.
+ * Pass NULL to remove the handler.
+ *
+ * NOTE: The handler is responsible for clearing any interrupt status flags.
+ * NOTE: It is not guaranteed that the particular peripheral caused the interrupt when
+ *       the function is called; some interrupt vectors are shared. Make sure to check the flags.
+ *
+ * @param periph - peripheral we're attaching to
+ * @param callback - callback to fire on IRQ
+ * @param data - data passed to the callback (user context)
+ */
+void irqd_attach(void *periph, IrqCallback callback, void *data);
+
+
+/**
+ * Remove a callback
+ *
+ * @param periph - peripheral we're attaching to
+ * @param callback - callback to remove, if it doesn't match, do nothing
+ */
+void irqd_detach(void *periph, IrqCallback callback);
+
+#endif //GEX_F072_IRQ_DISPATCHER_H
diff --git a/platform/lock_jumper.c b/platform/lock_jumper.c
index e70a419..81315e5 100644
--- a/platform/lock_jumper.c
+++ b/platform/lock_jumper.c
@@ -7,7 +7,7 @@
 #include "framework/settings.h"
 #include "framework/resources.h"
 #include "framework/system_settings.h"
-#include "pin_utils.h"
+#include "hw_utils.h"
 #include "lock_jumper.h"
 #include "status_led.h"
 
@@ -27,14 +27,14 @@ void LockJumper_Init(void)
     bool suc = true;
 
     // Resolve and claim resource
-    Resource rsc = pin2resource(LOCK_JUMPER_PORT, LOCK_JUMPER_PIN, &suc);
+    Resource rsc = hw_pin2resource(LOCK_JUMPER_PORT, LOCK_JUMPER_PIN, &suc);
     assert_param(suc);
 
     assert_param(E_SUCCESS == rsc_claim(&UNIT_SYSTEM, rsc));
 
     // Resolve pin
-    lock_periph = port2periph(LOCK_JUMPER_PORT, &suc);
-    lock_llpin = pin2ll(LOCK_JUMPER_PIN, &suc);
+    lock_periph = hw_port2periph(LOCK_JUMPER_PORT, &suc);
+    lock_llpin = hw_pin2ll(LOCK_JUMPER_PIN, &suc);
     assert_param(suc);
 
     // Configure for input
diff --git a/platform/plat_compat.h b/platform/plat_compat.h
index cd6345e..2ef70c4 100644
--- a/platform/plat_compat.h
+++ b/platform/plat_compat.h
@@ -15,12 +15,13 @@
  #define TSK_STACK_MAIN      160
 #endif
 
-#define TSK_STACK_MSG       220 // TF message handler task stack size (all unit commands run on this thread)
+// 180 is normally enough if not doing extensive debug logging
+#define TSK_STACK_MSG       200 // TF message handler task stack size (all unit commands run on this thread)
 
 #define BULK_READ_BUF_LEN 256   // Buffer for TF bulk reads
-#define UNIT_TMP_LEN      512   // Buffer for bulk read and varions internal unit operations
+#define UNIT_TMP_LEN      512   // Buffer for internal unit operations
 
-#define FLASH_SAVE_BUF_LEN  128 // Static buffer for saving to flash
+#define FLASH_SAVE_BUF_LEN  128 // Malloc'd buffer for saving to flash
 
 #define MSG_QUE_SLOT_SIZE 64 // FIXME this should be possible to lower, but there's some bug with bulk transfer / INI parser
 #define RX_QUE_CAPACITY    8 // TinyFrame rx queue size (64 bytes each)
@@ -57,16 +58,19 @@
 // PLAT_USB_OTGFS - uses the USB OTG IP, needs different config code
 // PLAT_LOCK_BTN - use a lock button instead of a lock jumper (push to toggle)
 // PLAT_LOCK_1CLOSED - lock jumper is active (closed / button pressed) in logical 1
+// PLAT_NO_AFNUM - legacy platform without numbered AF alternatives
 
 #if defined(GEX_PLAT_F103_BLUEPILL)
 
     // platform name for the version string
     #define GEX_PLATFORM "STM32F103-Bluepill"
     #define PLAT_AHB_MHZ 72
+    #define PLAT_APB1_MHZ 36
 
     // feature flags
     #define PLAT_FLASHBANKS 1
     #define PLAT_NO_FLOATING_INPUTS 1
+    #define PLAT_NO_AFNUM 1
 
     #include <stm32f1xx.h>
     #include <stm32f1xx_hal.h>
@@ -114,6 +118,7 @@
     // platform name for the version string
     #define GEX_PLATFORM "STM32F072-Discovery"
     #define PLAT_AHB_MHZ 48
+    #define PLAT_APB1_MHZ 48
 
     #include <stm32f0xx.h>
     #include <stm32f0xx_ll_adc.h>
@@ -161,6 +166,8 @@
     // platform name for the version string
     #define GEX_PLATFORM "STM32F303-Discovery"
     #define PLAT_AHB_MHZ 72
+    #define PLAT_APB1_MHZ 36
+    #define PLAT_APB2_MHZ 72
 
     #include <stm32f3xx.h>
     #include <stm32f3xx_hal.h>
@@ -211,6 +218,8 @@
     // platform name for the version string
     #define GEX_PLATFORM "STM32F407-Discovery"
     #define PLAT_AHB_MHZ 168
+    #define PLAT_APB1_MHZ 48
+    #define PLAT_APB2_MHZ 96
 
     #define PLAT_USB_PHYCLOCK 1
     #define PLAT_USB_OTGFS 1
@@ -263,4 +272,8 @@
     #error "BAD PLATFORM! Please select GEX platform using a -DGEX_PLAT_* compile flag"
 #endif
 
+#define PLAT_AHB_HZ (PLAT_AHB_MHZ*1000000)
+#define PLAT_APB1_HZ (PLAT_APB1_MHZ*1000000)
+#define PLAT_APB2_HZ (PLAT_APB2_MHZ*1000000)
+
 #endif //GEX_PLAT_COMPAT_H
diff --git a/platform/plat_init.c b/platform/plat_init.c
index d6b5034..743babd 100644
--- a/platform/plat_init.c
+++ b/platform/plat_init.c
@@ -11,6 +11,7 @@
 #include "lock_jumper.h"
 #include "status_led.h"
 #include "debug_uart.h"
+#include "irq_dispatcher.h"
 
 void plat_init(void)
 {
@@ -30,4 +31,5 @@ void plat_init(void)
     settings_load(); // XXX maybe this should be moved to the main task
 
     comm_init();
+    irqd_init();
 }
diff --git a/platform/platform.c b/platform/platform.c
index e2eff32..01793d6 100644
--- a/platform/platform.c
+++ b/platform/platform.c
@@ -2,18 +2,21 @@
 // Created by MightyPork on 2017/11/26.
 //
 
+#include <stm32f072xb.h>
 #include "platform.h"
 #include "usbd_core.h"
 #include "USB/usb_device.h"
 #include "framework/resources.h"
 #include "framework/unit_registry.h"
-
 #include "units/digital_out/unit_dout.h"
+
 #include "units/digital_in/unit_din.h"
 #include "units/neopixel/unit_neopixel.h"
 #include "units/i2c/unit_i2c.h"
 #include "units/test/unit_test.h"
+#include "units/usart/unit_usart.h"
 #include "units/spi/unit_spi.h"
+#include "hw_utils.h"
 
 void plat_init_resources(void)
 {
@@ -24,6 +27,12 @@ void plat_init_resources(void)
     __HAL_RCC_GPIOC_CLK_ENABLE();
     __HAL_RCC_GPIOD_CLK_ENABLE();
     __HAL_RCC_GPIOE_CLK_ENABLE();
+    __HAL_RCC_GPIOF_CLK_ENABLE();
+
+    hw_periph_clock_enable(DMA1);
+#ifdef DMA2
+    hw_periph_clock_enable(DMA2);
+#endif
 
     // --- Common unit drivers ---
 
@@ -79,6 +88,7 @@ void plat_init_resources(void)
     ureg_add_type(&UNIT_NEOPIXEL);
     ureg_add_type(&UNIT_I2C);
     ureg_add_type(&UNIT_SPI);
+    ureg_add_type(&UNIT_USART);
 
     // Free all present resources
     {
@@ -95,6 +105,7 @@ void plat_init_resources(void)
         rsc_free_range(NULL, R_TIM6, R_TIM7);
         rsc_free_range(NULL, R_TIM14, R_TIM17);
         rsc_free_range(NULL, R_USART1, R_USART4);
+        rsc_free_range(NULL, R_DMA1_1, R_DMA1_7);
 
         rsc_free_range(NULL, R_PA0, R_PA15);
         rsc_free_range(NULL, R_PB0, R_PB15);
diff --git a/platform/status_led.c b/platform/status_led.c
index 64dbd91..668a56b 100644
--- a/platform/status_led.c
+++ b/platform/status_led.c
@@ -5,7 +5,7 @@
 #include "platform.h"
 #include "framework/resources.h"
 #include "status_led.h"
-#include "pin_utils.h"
+#include "hw_utils.h"
 
 static GPIO_TypeDef *led_periph;
 static uint32_t led_llpin;
@@ -26,8 +26,8 @@ void Indicator_PreInit(void)
 {
     bool suc = true;
     // Resolve pin
-    led_periph = port2periph(STATUS_LED_PORT, &suc);
-    led_llpin = pin2ll(STATUS_LED_PIN, &suc);
+    led_periph = hw_port2periph(STATUS_LED_PORT, &suc);
+    led_llpin = hw_pin2ll(STATUS_LED_PIN, &suc);
 
     // Configure for output
     LL_GPIO_SetPinMode(led_periph, led_llpin, LL_GPIO_MODE_OUTPUT);
@@ -53,7 +53,7 @@ void Indicator_Init(void)
     bool suc = true;
 
     // Resolve and claim resource
-    Resource rsc = pin2resource(STATUS_LED_PORT, STATUS_LED_PIN, &suc);
+    Resource rsc = hw_pin2resource(STATUS_LED_PORT, STATUS_LED_PIN, &suc);
     assert_param(suc);
 
     assert_param(E_SUCCESS == rsc_claim(&UNIT_SYSTEM, rsc));
diff --git a/stm32_assert.c b/stm32_assert.c
index 6cfcdff..be139da 100644
--- a/stm32_assert.c
+++ b/stm32_assert.c
@@ -1,29 +1,15 @@
 #include "platform.h"
 #include "platform/status_led.h"
 
-/**
- * Abort at file, line with a custom tag (eg. ASSERT FAILED)
- * @param msg - tag message
- * @param filename - file
- * @param line - line
- */
-void __attribute__((noreturn)) abort_msg(const char *msg, const char *filename, uint32_t line)
+void _abort_errlight(void)
 {
-    dbg("\r\n\033[31m%s:\033[m %s:%"PRIu32, msg, filename, line);
-    vPortEnterCritical();
     Indicator_Effect(STATUS_FAULT);
-    while(1);
 }
 
-/**
- * Warn at file, line with a custom tag (eg. ASSERT FAILED)
- * @param msg - tag message
- * @param filename - file
- * @param line - line
- */
-void warn_msg(const char *msg, const char *filename, uint32_t line)
+void __attribute__((noreturn)) _abort_do(void)
 {
-    dbg("\r\n\033[33m%s:\033[m %s:%"PRIu32, msg, filename, line);
+    vPortEnterCritical();
+    while(1);
 }
 
 /**
@@ -32,7 +18,7 @@ void warn_msg(const char *msg, const char *filename, uint32_t line)
    * @param file: pointer to the source file name
    * @param line: assert_param error line source number
    */
-void __attribute__((noreturn)) assert_failed_(const char *file, uint32_t line)
+void __attribute__((noreturn)) _assert_failed(const char *file, uint32_t line)
 {
-    abort_msg("ASSERT FAILED", file, line);
+    _abort_msg(file, line, "ASSERT FAILED");
 }
diff --git a/stm32_assert.h b/stm32_assert.h
index 4de2d02..f3c9e09 100644
--- a/stm32_assert.h
+++ b/stm32_assert.h
@@ -6,30 +6,45 @@
 #define STM32_ASSERT_H
 
 #include <stdint.h>
+#include "debug.h"
 
-void __attribute__((noreturn)) abort_msg(const char *msg, const char *filename, uint32_t line);
-void warn_msg(const char *msg, const char *filename, uint32_t line);
-void __attribute__((noreturn)) assert_failed_(const char *file, uint32_t line);
+void _abort_errlight(void);
+void __attribute__((noreturn)) _assert_failed(const char *file, uint32_t line);
+void __attribute__((noreturn)) _abort_do(void);
+
+#define _abort_msg(file, line, format, ...) do { \
+    _abort_errlight(); \
+    dbg("\r\n\033[31m" format ":\033[m %s:%d", ##__VA_ARGS__, file, (int)line); \
+    _abort_do(); \
+} while (0)
+
+#define _warn_msg(file, line, format, ...) do { \
+    dbg("\r\n\033[33m" format ":\033[m %s:%d", ##__VA_ARGS__, file, (int)line); \
+} while (0)
 
 #if USE_FULL_ASSERT
-    #if VERBOSE_ASSERT
+    #if ASSERT_FILENAMES
         // With the filename enabled.
-        #define trap(msg) abort_msg(msg, __BASE_FILE__, __LINE__)
-        #define assert_param(expression) do { if (!(expression)) assert_failed_(__BASE_FILE__, __LINE__); } while(0)
-        #define assert_warn(expression, msg) do { if (!(expression)) warn_msg(msg, __BASE_FILE__, __LINE__); } while(0)
-        #define _Error_Handler(file, line) assert_failed_(__BASE_FILE__, __LINE__)
+        #define trap(msg, ...) _abort_msg(__BASE_FILE__, __LINE__, msg, ##__VA_ARGS__)
+        #define warn(msg, ...) _warn_msg(__BASE_FILE__, __LINE__, msg, ##__VA_ARGS__)
+        #define assert_param(expression) do { if (!(expression)) _assert_failed(__BASE_FILE__, __LINE__); } while(0)
+        #define assert_warn(expression, msg, ...) do { if (!(expression)) _warn_msg(__BASE_FILE__, __LINE__, msg, ##__VA_ARGS__); } while(0)
+        #define _Error_Handler(file, line) _assert_failed(__BASE_FILE__, __LINE__)
     #else
         // Filename disabled to save code size.
-        #define trap(msg) abort_msg(msg, "??", __LINE__)
-        #define assert_param(expression) do { if (!(expression)) assert_failed_("??", __LINE__); } while(0)
-        #define assert_warn(expression, msg) do { if (!(expression)) warn_msg(msg, "??", __LINE__); } while(0)
-        #define _Error_Handler(file, line) assert_failed_("??", __LINE__)
+        #define trap(msg, ...) _abort_msg("??", __LINE__, msg, ##__VA_ARGS__)
+        #define warn(msg, ...) _warn_msg("??", __LINE__, msg, ##__VA_ARGS__)
+        #define assert_param(expression) do { if (!(expression)) _assert_failed("??", __LINE__); } while(0)
+        #define assert_warn(expression, msg, ...) do { if (!(expression)) _warn_msg("??", __LINE__, msg, ##__VA_ARGS__); } while(0)
+        #define _Error_Handler(file, line) _assert_failed("??", __LINE__)
     #endif
 #else
     // This is after everything is well tested, to cut some flash and make code faster by removing checks
-    #define trap(msg) do {} while(1)
+    // must take care to evaluate the expressions regardless in case they have side effects.
+    #define trap(msg, ...) do {} while(1)
+    #define warn(msg, ...)
     #define assert_param(expression) do { (void)(expression); } while(0)
-    #define assert_warn(expression, msg) do { (void)(expression); } while(0)
+    #define assert_warn(expression, msg, ...) do { (void)(expression); } while(0)
     #define _Error_Handler(file, line) do {} while(1)
 #endif
 
diff --git a/tasks/task_msg.c b/tasks/task_msg.c
index 4a51a98..510ef9e 100644
--- a/tasks/task_msg.c
+++ b/tasks/task_msg.c
@@ -80,7 +80,7 @@ void TaskMsgJob(const void *argument)
 
 #if USE_STACK_MONITOR
         uint32_t count;
-        count = (uint32_t) uxQueueMessagesWaiting(queMsgJobHandle); // this seems to return N+1, hence we don't add the +1 for the one just removed.
+        count = (uint32_t) uxQueueMessagesWaiting(queMsgJobHandle)+1;
         msgQueHighWaterMark = MAX(msgQueHighWaterMark, count);
 #endif
     }
diff --git a/units/digital_in/unit_din.c b/units/digital_in/unit_din.c
index 049da57..d4df042 100644
--- a/units/digital_in/unit_din.c
+++ b/units/digital_in/unit_din.c
@@ -57,7 +57,7 @@ static error_t DI_loadIni(Unit *unit, const char *key, const char *value)
     struct priv *priv = unit->data;
 
     if (streq(key, "port")) {
-        suc = parse_port(value, &priv->port_name);
+        suc = parse_port_name(value, &priv->port_name);
     }
     else if (streq(key, "pins")) {
         priv->pins = parse_pinmask(value, &suc);
@@ -85,13 +85,13 @@ static void DI_writeIni(Unit *unit, IniWriter *iw)
     iw_entry(iw, "port", "%c", priv->port_name);
 
     iw_comment(iw, "Pins (comma separated, supports ranges)");
-    iw_entry(iw, "pins", "%s", str_pinmask(priv->pins, unit_tmp512));
+    iw_entry(iw, "pins", "%s", pinmask2str(priv->pins, unit_tmp512));
 
     iw_comment(iw, "Pins with pull-up");
-    iw_entry(iw, "pull-up", "%s", str_pinmask(priv->pullup, unit_tmp512));
+    iw_entry(iw, "pull-up", "%s", pinmask2str(priv->pullup, unit_tmp512));
 
     iw_comment(iw, "Pins with pull-down");
-    iw_entry(iw, "pull-down", "%s", str_pinmask(priv->pulldown, unit_tmp512));
+    iw_entry(iw, "pull-down", "%s", pinmask2str(priv->pulldown, unit_tmp512));
 
 #if PLAT_NO_FLOATING_INPUTS
     iw_comment(iw, "NOTE: Pins use pull-up by default.\r\n");
@@ -103,9 +103,8 @@ static void DI_writeIni(Unit *unit, IniWriter *iw)
 /** Allocate data structure and set defaults */
 static error_t DI_preInit(Unit *unit)
 {
-    bool suc = true;
-    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
-    if (!suc) return E_OUT_OF_MEM;
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL) return E_OUT_OF_MEM;
 
     // some defaults
     priv->port_name = 'A';
@@ -126,7 +125,7 @@ static error_t DI_init(Unit *unit)
     priv->pullup &= priv->pins;
 
     // --- Parse config ---
-    priv->port = port2periph(priv->port_name, &suc);
+    priv->port = hw_port2periph(priv->port_name, &suc);
     if (!suc) return E_BAD_CONFIG;
 
     // Claim all needed pins
@@ -135,7 +134,7 @@ static error_t DI_init(Unit *unit)
     uint16_t mask = 1;
     for (int i = 0; i < 16; i++, mask <<= 1) {
         if (priv->pins & mask) {
-            uint32_t ll_pin = pin2ll((uint8_t) i, &suc);
+            uint32_t ll_pin = hw_pin2ll((uint8_t) i, &suc);
 
             // --- Init hardware ---
             LL_GPIO_SetPinMode(priv->port, ll_pin, LL_GPIO_MODE_INPUT);
@@ -167,8 +166,7 @@ static void DI_deInit(Unit *unit)
     rsc_teardown(unit);
 
     // Free memory
-    free(unit->data);
-    unit->data = NULL;
+    free_ck(unit->data);
 }
 
 // ------------------------------------------------------------------------
@@ -178,7 +176,7 @@ error_t UU_DI_Read(Unit *unit, uint16_t *packed)
 {
     CHECK_TYPE(unit, &UNIT_DIN);
     struct priv *priv = unit->data;
-    *packed = port_pack((uint16_t) priv->port->IDR, priv->pins);
+    *packed = pinmask_pack((uint16_t) priv->port->IDR, priv->pins);
     return E_SUCCESS;
 }
 
diff --git a/units/digital_out/unit_dout.c b/units/digital_out/unit_dout.c
index 141a3db..7937fc6 100644
--- a/units/digital_out/unit_dout.c
+++ b/units/digital_out/unit_dout.c
@@ -54,7 +54,7 @@ static error_t DO_loadIni(Unit *unit, const char *key, const char *value)
     struct priv *priv = unit->data;
 
     if (streq(key, "port")) {
-        suc = parse_port(value, &priv->port_name);
+        suc = parse_port_name(value, &priv->port_name);
     }
     else if (streq(key, "pins")) {
         priv->pins = parse_pinmask(value, &suc);
@@ -82,13 +82,13 @@ static void DO_writeIni(Unit *unit, IniWriter *iw)
     iw_entry(iw, "port", "%c", priv->port_name);
 
     iw_comment(iw, "Pins (comma separated, supports ranges)");
-    iw_entry(iw, "pins", "%s", str_pinmask(priv->pins, unit_tmp512));
+    iw_entry(iw, "pins", "%s", pinmask2str(priv->pins, unit_tmp512));
 
     iw_comment(iw, "Initially high pins");
-    iw_entry(iw, "initial", "%s", str_pinmask(priv->initial, unit_tmp512));
+    iw_entry(iw, "initial", "%s", pinmask2str(priv->initial, unit_tmp512));
 
     iw_comment(iw, "Open-drain pins");
-    iw_entry(iw, "open-drain", "%s", str_pinmask(priv->open_drain, unit_tmp512));
+    iw_entry(iw, "open-drain", "%s", pinmask2str(priv->open_drain, unit_tmp512));
 }
 
 // ------------------------------------------------------------------------
@@ -96,9 +96,8 @@ static void DO_writeIni(Unit *unit, IniWriter *iw)
 /** Allocate data structure and set defaults */
 static error_t DO_preInit(Unit *unit)
 {
-    bool suc = true;
-    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
-    if (!suc) return E_OUT_OF_MEM;
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL) return E_OUT_OF_MEM;
 
     // some defaults
     priv->port_name = 'A';
@@ -119,7 +118,7 @@ static error_t DO_init(Unit *unit)
     priv->open_drain &= priv->pins;
 
     // --- Parse config ---
-    priv->port = port2periph(priv->port_name, &suc);
+    priv->port = hw_port2periph(priv->port_name, &suc);
     if (!suc) return E_BAD_CONFIG;
 
     // Claim all needed pins
@@ -127,7 +126,7 @@ static error_t DO_init(Unit *unit)
 
     for (int i = 0; i < 16; i++) {
         if (priv->pins & (1 << i)) {
-            uint32_t ll_pin = pin2ll((uint8_t) i, &suc);
+            uint32_t ll_pin = hw_pin2ll((uint8_t) i, &suc);
 
             // --- Init hardware ---
             LL_GPIO_SetPinMode(priv->port, ll_pin, LL_GPIO_MODE_OUTPUT);
@@ -153,8 +152,7 @@ static void DO_deInit(Unit *unit)
     rsc_teardown(unit);
 
     // Free memory
-    free(unit->data);
-    unit->data = NULL;
+    free_ck(unit->data);
 }
 
 // ------------------------------------------------------------------------
@@ -165,7 +163,7 @@ error_t UU_DO_Write(Unit *unit, uint16_t packed)
 
     struct priv *priv = unit->data;
     uint16_t mask = priv->pins;
-    uint16_t spread = port_spread(packed, mask);
+    uint16_t spread = pinmask_spread(packed, mask);
 
     uint16_t set = spread;
     uint16_t reset = ((~spread) & mask);
@@ -179,7 +177,7 @@ error_t UU_DO_Set(Unit *unit, uint16_t packed)
 
     struct priv *priv = unit->data;
     uint16_t mask = priv->pins;
-    uint16_t spread = port_spread(packed, mask);
+    uint16_t spread = pinmask_spread(packed, mask);
 
     priv->port->BSRR = spread;
     return E_SUCCESS;
@@ -191,7 +189,7 @@ error_t UU_DO_Clear(Unit *unit, uint16_t packed)
 
     struct priv *priv = unit->data;
     uint16_t mask = priv->pins;
-    uint16_t spread = port_spread(packed, mask);
+    uint16_t spread = pinmask_spread(packed, mask);
 
     priv->port->BSRR = (spread<<16);
     return E_SUCCESS;
@@ -203,7 +201,7 @@ error_t UU_DO_Toggle(Unit *unit, uint16_t packed)
 
     struct priv *priv = unit->data;
     uint16_t mask = priv->pins;
-    uint16_t spread = port_spread(packed, mask);
+    uint16_t spread = pinmask_spread(packed, mask);
 
     uint16_t flipped = (uint16_t) (~priv->port->ODR) & mask;
     uint16_t set = flipped & spread;
@@ -217,7 +215,7 @@ error_t UU_DO_GetPinCount(Unit *unit, uint8_t *count)
     CHECK_TYPE(unit, &UNIT_DOUT);
     struct priv *priv = unit->data;
 
-    uint32_t packed = port_pack(0xFFFF, priv->pins);
+    uint32_t packed = pinmask_pack(0xFFFF, priv->pins);
     *count = (uint8_t)(32 - __CLZ(packed));
     return E_SUCCESS;
 }
diff --git a/units/i2c/unit_i2c.c b/units/i2c/unit_i2c.c
index 71b932b..7315071 100644
--- a/units/i2c/unit_i2c.c
+++ b/units/i2c/unit_i2c.c
@@ -2,7 +2,6 @@
 // Created by MightyPork on 2018/01/02.
 //
 
-#include <framework/system_settings.h>
 #include "comm/messages.h"
 #include "unit_base.h"
 #include "utils/avrlibc.h"
@@ -130,9 +129,8 @@ static void UI2C_writeIni(Unit *unit, IniWriter *iw)
 /** Allocate data structure and set defaults */
 static error_t UI2C_preInit(Unit *unit)
 {
-    bool suc = true;
-    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
-    if (!suc) return E_OUT_OF_MEM;
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL) return E_OUT_OF_MEM;
 
     // some defaults
     priv->periph_num = 1;
@@ -237,14 +235,10 @@ static error_t UI2C_init(Unit *unit)
     TRY(rsc_claim_pin(unit, portname, pin_sda));
     TRY(rsc_claim_pin(unit, portname, pin_scl));
 
-    configure_gpio_alternate(portname, pin_sda, af_i2c);
-    configure_gpio_alternate(portname, pin_scl, af_i2c);
+    hw_configure_gpio_af(portname, pin_sda, af_i2c);
+    hw_configure_gpio_af(portname, pin_scl, af_i2c);
 
-    if (priv->periph_num == 1) {
-        __HAL_RCC_I2C1_CLK_ENABLE();
-    } else {
-        __HAL_RCC_I2C2_CLK_ENABLE();
-    }
+    hw_periph_clock_enable(priv->periph);
 
     /* Disable the selected I2Cx Peripheral */
     LL_I2C_Disable(priv->periph);
@@ -270,22 +264,16 @@ static void UI2C_deInit(Unit *unit)
     // de-init the pins & peripheral only if inited correctly
     if (unit->status == E_SUCCESS) {
         assert_param(priv->periph);
-
         LL_I2C_DeInit(priv->periph);
 
-        if (priv->periph_num == 1) {
-            __HAL_RCC_I2C1_CLK_DISABLE();
-        } else {
-            __HAL_RCC_I2C2_CLK_DISABLE();
-        }
+        hw_periph_clock_disable(priv->periph);
     }
 
     // Release all resources
     rsc_teardown(unit);
 
     // Free memory
-    free(unit->data);
-    unit->data = NULL;
+    free_ck(unit->data);
 }
 
 // ------------------------------------------------------------------------
diff --git a/units/neopixel/unit_neopixel.c b/units/neopixel/unit_neopixel.c
index 585fdf2..984d5f3 100644
--- a/units/neopixel/unit_neopixel.c
+++ b/units/neopixel/unit_neopixel.c
@@ -79,11 +79,10 @@ static void Npx_writeIni(Unit *unit, IniWriter *iw)
 /** Allocate data structure and set defaults */
 static error_t Npx_preInit(Unit *unit)
 {
-    bool suc = true;
-    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
-    if (!suc) return E_OUT_OF_MEM;
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL)
 
-    // some defaults
+        // some defaults
     priv->pin_number = 0;
     priv->port_name = 'A';
     priv->pixels = 1;
@@ -98,9 +97,9 @@ static error_t Npx_init(Unit *unit)
     struct priv *priv = unit->data;
 
     // --- Parse config ---
-    priv->ll_pin = pin2ll(priv->pin_number, &suc);
-    priv->port = port2periph(priv->port_name, &suc);
-    Resource rsc = pin2resource(priv->port_name, priv->pin_number, &suc);
+    priv->ll_pin = hw_pin2ll(priv->pin_number, &suc);
+    priv->port = hw_port2periph(priv->port_name, &suc);
+    Resource rsc = hw_pin2resource(priv->port_name, priv->pin_number, &suc);
     if (!suc) return E_BAD_CONFIG;
 
     // --- Claim resources ---
@@ -127,8 +126,7 @@ static void Npx_deInit(Unit *unit)
     rsc_teardown(unit);
 
     // Free memory
-    free(unit->data);
-    unit->data = NULL;
+    free_ck(unit->data);
 }
 
 // ------------------------------------------------------------------------
diff --git a/units/spi/unit_spi.c b/units/spi/unit_spi.c
index 9164f21..1560a8b 100644
--- a/units/spi/unit_spi.c
+++ b/units/spi/unit_spi.c
@@ -2,7 +2,6 @@
 // Created by MightyPork on 2018/01/02.
 //
 
-#include <framework/system_settings.h>
 #include <stm32f072xb.h>
 #include "comm/messages.h"
 #include "unit_base.h"
@@ -98,11 +97,11 @@ static error_t USPI_loadIni(Unit *unit, const char *key, const char *value)
     else if (streq(key, "tx-only")) {
         priv->tx_only = str_parse_yn(value, &suc);
     }
-    else if (streq(key, "lsb-first")) {
-        priv->lsb_first = str_parse_yn(value, &suc);
+    else if (streq(key, "first-bit")) {
+        priv->lsb_first = (bool)str_parse_2(value, "MSB", 0, "LSB", 1, &suc);
     }
     else if (streq(key, "port")) {
-        suc = parse_port(value, &priv->ssn_port_name);
+        suc = parse_port_name(value, &priv->ssn_port_name);
     }
     else if (streq(key, "pins")) {
         priv->ssn_pins = parse_pinmask(value, &suc);
@@ -152,15 +151,17 @@ static void USPI_writeIni(Unit *unit, IniWriter *iw)
     iw_comment(iw, "Transmit only, disable MISO");
     iw_entry(iw, "tx-only", str_yn(priv->tx_only));
 
-    iw_comment(iw, "Use LSB-first bit order");
-    iw_entry(iw, "lsb-first", str_yn(priv->lsb_first));
+    iw_comment(iw, "Bit order (LSB or MSB first)");
+    iw_entry(iw, "first-bit", str_2((uint32_t)priv->lsb_first,
+                                    0, "MSB",
+                                    1, "LSB"));
 
     iw_cmt_newline(iw);
     iw_comment(iw, "SS port name");
     iw_entry(iw, "port", "%c", priv->ssn_port_name);
 
     iw_comment(iw, "SS pins (comma separated, supports ranges)");
-    iw_entry(iw, "pins", "%s", str_pinmask(priv->ssn_pins, unit_tmp512));
+    iw_entry(iw, "pins", "%s", pinmask2str(priv->ssn_pins, unit_tmp512));
 }
 
 // ------------------------------------------------------------------------
@@ -168,9 +169,8 @@ static void USPI_writeIni(Unit *unit, IniWriter *iw)
 /** Allocate data structure and set defaults */
 static error_t USPI_preInit(Unit *unit)
 {
-    bool suc = true;
-    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
-    if (!suc) return E_OUT_OF_MEM;
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL) return E_OUT_OF_MEM;
 
     // some defaults
     priv->periph_num = 1;
@@ -291,26 +291,22 @@ static error_t USPI_init(Unit *unit)
     TRY(rsc_claim_pin(unit, spi_portname, pin_miso));
     TRY(rsc_claim_pin(unit, spi_portname, pin_sck));
 
-    configure_gpio_alternate(spi_portname, pin_mosi, af_spi);
-    configure_gpio_alternate(spi_portname, pin_miso, af_spi);
-    configure_gpio_alternate(spi_portname, pin_sck, af_spi);
-
-    if (priv->periph_num == 1) {
-        __HAL_RCC_SPI1_CLK_ENABLE();
-    } else {
-        __HAL_RCC_SPI2_CLK_ENABLE();
-    }
+    hw_configure_gpio_af(spi_portname, pin_mosi, af_spi);
+    hw_configure_gpio_af(spi_portname, pin_miso, af_spi);
+    hw_configure_gpio_af(spi_portname, pin_sck, af_spi);
 
     // configure SSN GPIOs
     {
         // Claim all needed pins
         TRY(rsc_claim_gpios(unit, priv->ssn_port_name, priv->ssn_pins));
-        TRY(configure_sparse_pins(priv->ssn_port_name, priv->ssn_pins, &priv->ssn_port,
-                                  LL_GPIO_MODE_OUTPUT, LL_GPIO_OUTPUT_PUSHPULL));
+        TRY(hw_configure_sparse_pins(priv->ssn_port_name, priv->ssn_pins, &priv->ssn_port,
+                                     LL_GPIO_MODE_OUTPUT, LL_GPIO_OUTPUT_PUSHPULL));
         // Set the initial state - all high
         priv->ssn_port->BSRR = priv->ssn_pins;
     }
 
+    hw_periph_clock_enable(priv->periph);
+
     // Configure SPI - must be configured under reset
     LL_SPI_Disable(priv->periph);
     {
@@ -345,22 +341,16 @@ static void USPI_deInit(Unit *unit)
     // de-init the pins & peripheral only if inited correctly
     if (unit->status == E_SUCCESS) {
         assert_param(priv->periph);
-
         LL_SPI_DeInit(priv->periph);
 
-        if (priv->periph_num == 1) {
-            __HAL_RCC_SPI1_CLK_DISABLE();
-        } else {
-            __HAL_RCC_SPI2_CLK_DISABLE();
-        }
+        hw_periph_clock_disable(priv->periph);
     }
 
     // Release all resources
     rsc_teardown(unit);
 
     // Free memory
-    free(unit->data);
-    unit->data = NULL;
+    free_ck(unit->data);
 }
 
 // ------------------------------------------------------------------------
@@ -432,7 +422,7 @@ error_t UU_SPI_Multicast(Unit *unit, uint16_t slaves,
                      const uint8_t *request, uint32_t req_len)
 {
     struct priv *priv= unit->data;
-    uint16_t mask = port_spread(slaves, priv->ssn_pins);
+    uint16_t mask = pinmask_spread(slaves, priv->ssn_pins);
     priv->ssn_port->BRR = mask;
     {
         TRY(xfer_do(priv, request, NULL, req_len, 0, 0));
@@ -448,7 +438,7 @@ error_t UU_SPI_Write(Unit *unit, uint8_t slave_num,
 {
     struct priv *priv= unit->data;
 
-    uint16_t mask = port_spread((uint16_t) (1 << slave_num), priv->ssn_pins);
+    uint16_t mask = pinmask_spread((uint16_t) (1 << slave_num), priv->ssn_pins);
     priv->ssn_port->BRR = mask;
     {
         TRY(xfer_do(priv, request, response, req_len, resp_len, resp_skip));
diff --git a/units/test/unit_test.c b/units/test/unit_test.c
index 51ce0b6..322daf6 100644
--- a/units/test/unit_test.c
+++ b/units/test/unit_test.c
@@ -50,9 +50,8 @@ static void Tst_writeIni(Unit *unit, IniWriter *iw)
 /** Allocate data structure and set defaults */
 static error_t Tst_preInit(Unit *unit)
 {
-    bool suc = true;
-    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
-    if (!suc) return E_OUT_OF_MEM;
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL) return E_OUT_OF_MEM;
 
     //
 
@@ -78,8 +77,7 @@ static void Tst_deInit(Unit *unit)
     //
 
     // Free memory
-    free(unit->data);
-    unit->data = NULL;
+    free_ck(unit->data);
 }
 
 // ------------------------------------------------------------------------
@@ -98,7 +96,7 @@ static void br_longtext(struct bulk_read *bulk, uint32_t chunk, uint8_t *buffer)
 {
     // clean-up request
     if (buffer == NULL) {
-        free(bulk);
+        free_ck(bulk);
         return;
     }
 
@@ -109,7 +107,7 @@ static void bw_dump(struct bulk_write *bulk, const uint8_t *chunk, uint32_t len)
 {
     // clean-up request
     if (chunk == NULL) {
-        free(bulk);
+        free_ck(bulk);
         return;
     }
 
@@ -133,7 +131,7 @@ static error_t Tst_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, Pa
             return E_SUCCESS;
 
         case CMD_BULKREAD:;
-            BulkRead *br = malloc(sizeof(struct bulk_read));
+            BulkRead *br = malloc_ck(sizeof(struct bulk_read));
             assert_param(br);
 
             br->len = (uint32_t) strlen(longtext);
@@ -144,7 +142,7 @@ static error_t Tst_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, Pa
             return E_SUCCESS;
 
         case CMD_BULKWRITE:;
-            BulkWrite *bw = malloc(sizeof(struct bulk_write));
+            BulkWrite *bw = malloc_ck(sizeof(struct bulk_write));
             assert_param(bw);
 
             bw->len = 10240;
diff --git a/units/usart/_dmas.c b/units/usart/_dmas.c
new file mode 100644
index 0000000..12adaab
--- /dev/null
+++ b/units/usart/_dmas.c
@@ -0,0 +1,444 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+
+#include "platform.h"
+#include "irq_dispatcher.h"
+#include "unit_base.h"
+
+#define UUSART_INTERNAL
+#include "_internal.h"
+
+static void UUSART_DMA_RxHandler(void *arg);
+static void UUSART_DMA_TxHandler(void *arg);
+
+#if UUSART_DEBUG
+#define dbg_uusart(fmt, ...) dbg(fmt, ##__VA_ARGS__)
+#else
+#define dbg_uusart(fmt, ...)
+#endif
+
+error_t UUSART_ClaimDMAs(Unit *unit)
+{
+    error_t rv;
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    priv->dma = DMA1;
+
+    switch (priv->periph_num) {
+        /* USART1 */
+        case 1:
+            // TX
+            rv = rsc_claim(unit, R_DMA1_2);
+            if (rv == E_SUCCESS) {
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART1TX_RMP_DMA1CH2);
+                priv->dma_tx = DMA1_Channel2;
+                priv->dma_tx_chnum = 2;
+            } else {
+                // try the remap
+                TRY(rsc_claim(unit, R_DMA1_4));
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART1TX_RMP_DMA1CH4);
+                priv->dma_tx = DMA1_Channel4;
+                priv->dma_tx_chnum = 4;
+            }
+
+            // RX
+            rv = rsc_claim(unit, R_DMA1_3);
+            if (rv == E_SUCCESS) {
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART1RX_RMP_DMA1CH3);
+                priv->dma_rx = DMA1_Channel3;
+                priv->dma_rx_chnum = 3;
+            } else {
+                // try the remap
+                TRY(rsc_claim(unit, R_DMA1_5));
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART1RX_RMP_DMA1CH5);
+                priv->dma_rx = DMA1_Channel5;
+                priv->dma_rx_chnum = 5;
+            }
+            break;
+
+        /* USART2 */
+        case 2:
+            // RX,TX
+            rv = rsc_claim_range(unit, R_DMA1_4, R_DMA1_5);
+            if (rv == E_SUCCESS) {
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART2_RMP_DMA1CH54);
+                priv->dma_tx = DMA1_Channel4;
+                priv->dma_rx = DMA1_Channel5;
+                priv->dma_tx_chnum = 4;
+                priv->dma_rx_chnum = 5;
+            } else {
+                // try the remap
+                TRY(rsc_claim_range(unit, R_DMA1_6, R_DMA1_7));
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART2_RMP_DMA1CH67);
+                priv->dma_tx = DMA1_Channel7;
+                priv->dma_rx = DMA1_Channel6;
+                priv->dma_tx_chnum = 7;
+                priv->dma_rx_chnum = 6;
+            }
+            break;
+
+        /* USART3 */
+        case 3:
+            // RX,TX
+            rv = rsc_claim_range(unit, R_DMA1_6, R_DMA1_7);
+            if (rv == E_SUCCESS) {
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART3_RMP_DMA1CH67);
+                priv->dma_tx = DMA1_Channel7;
+                priv->dma_rx = DMA1_Channel6;
+                priv->dma_tx_chnum = 7;
+                priv->dma_rx_chnum = 6;
+            } else {
+                // try the remap
+                TRY(rsc_claim_range(unit, R_DMA1_2, R_DMA1_3));
+                LL_SYSCFG_SetRemapDMA_USART(LL_SYSCFG_USART3_RMP_DMA1CH32);
+                priv->dma_tx = DMA1_Channel2;
+                priv->dma_rx = DMA1_Channel3;
+                priv->dma_tx_chnum = 2;
+                priv->dma_rx_chnum = 3;
+            }
+            break;
+
+        /* USART4 */
+        case 4:
+            // RX,TX
+            TRY(rsc_claim_range(unit, R_DMA1_6, R_DMA1_7));
+            priv->dma_tx = DMA1_Channel7;
+            priv->dma_rx = DMA1_Channel6;
+            priv->dma_tx_chnum = 7;
+            priv->dma_rx_chnum = 6;
+            break;
+
+        default:
+            trap("Missing DMA mapping for USART%d", (int)priv->periph_num);
+    }
+
+    dbg_uusart("USART %d - selected DMA ch Tx(%d), Rx(%d)", priv->periph_num, priv->dma_tx_chnum, priv->dma_rx_chnum);
+
+    return E_SUCCESS;
+}
+
+error_t UUSART_SetupDMAs(Unit *unit)
+{
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    priv->rx_buffer = malloc_ck(UUSART_RXBUF_LEN);
+    if (NULL == priv->rx_buffer) return E_OUT_OF_MEM;
+
+    priv->tx_buffer = malloc_ck(UUSART_TXBUF_LEN);
+    if (NULL == priv->tx_buffer) return E_OUT_OF_MEM;
+
+    // Those must be aligned to a word boundary for the DMAs to work.
+    // Any well-behaved malloc impl should do this correctly.
+    assert_param(((uint32_t)priv->rx_buffer & 3) == 0);
+    assert_param(((uint32_t)priv->tx_buffer & 3) == 0);
+
+    priv->rx_buf_readpos = 0;
+
+    LL_DMA_InitTypeDef init;
+
+    // Transmit buffer
+    {
+        LL_DMA_StructInit(&init);
+        init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
+        init.Mode = LL_DMA_MODE_NORMAL;
+
+        init.PeriphOrM2MSrcAddress = (uint32_t) &priv->periph->TDR;
+        init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
+        init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
+
+        init.MemoryOrM2MDstAddress = (uint32_t) priv->tx_buffer;
+        init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
+        init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
+
+        assert_param(SUCCESS == LL_DMA_Init(priv->dma, priv->dma_tx_chnum, &init));
+
+        irqd_attach(priv->dma_tx, UUSART_DMA_TxHandler, unit);
+        // Interrupt on transfer complete
+        LL_DMA_EnableIT_TC(priv->dma, priv->dma_tx_chnum);
+    }
+
+    // Receive buffer
+    {
+        LL_DMA_StructInit(&init);
+        init.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
+
+        init.Mode = LL_DMA_MODE_CIRCULAR;
+        init.NbData = UUSART_RXBUF_LEN;
+
+        init.PeriphOrM2MSrcAddress = (uint32_t) &priv->periph->RDR;
+        init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
+        init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
+
+        init.MemoryOrM2MDstAddress = (uint32_t) priv->rx_buffer;
+        init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
+        init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
+
+        assert_param(SUCCESS == LL_DMA_Init(priv->dma, priv->dma_rx_chnum, &init));
+
+        irqd_attach(priv->dma_rx, UUSART_DMA_RxHandler, unit);
+        // Interrupt on transfer 1/2 and complete
+        // We will capture the first and second half and send it while the other half is being filled.
+        LL_DMA_EnableIT_HT(priv->dma, priv->dma_rx_chnum);
+        LL_DMA_EnableIT_TC(priv->dma, priv->dma_rx_chnum);
+    }
+
+    LL_DMA_EnableChannel(priv->dma, priv->dma_rx_chnum);
+    LL_DMA_EnableChannel(priv->dma, priv->dma_tx_chnum);
+
+    // TODO also set up usart timeout interrupt that grabs whatever is in the DMA buffer and sends it
+    return E_SUCCESS;
+}
+
+/**
+ * Handler for the Rx DMA half or full interrupt
+ * @param arg - unit instance
+ */
+static void UUSART_DMA_RxHandler(void *arg)
+{
+    Unit *unit = arg;
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    uint32_t isrsnapshot = priv->dma->ISR;
+
+    if (LL_DMA_IsActiveFlag_G(isrsnapshot, priv->dma_rx_chnum)) {
+        bool tc = LL_DMA_IsActiveFlag_TC(isrsnapshot, priv->dma_rx_chnum);
+        bool ht = LL_DMA_IsActiveFlag_HT(isrsnapshot, priv->dma_rx_chnum);
+
+        // Here we have to either copy it somewhere else, or notify another thread (queue?)
+        // that the data is ready for reading
+
+        if (ht) {
+            uint16_t end = (uint16_t) UUSART_RXBUF_LEN / 2;
+            UUSART_DMA_HandleRxFromIRQ(unit, end);
+            LL_DMA_ClearFlag_HT(priv->dma, priv->dma_rx_chnum);
+        }
+
+        if (tc) {
+            uint16_t end = (uint16_t) UUSART_RXBUF_LEN;
+            UUSART_DMA_HandleRxFromIRQ(unit, end);
+            LL_DMA_ClearFlag_TC(priv->dma, priv->dma_rx_chnum);
+        }
+
+        if (LL_DMA_IsActiveFlag_TE(isrsnapshot, priv->dma_rx_chnum)) {
+            // this shouldn't happen
+            LL_DMA_ClearFlag_TE(priv->dma, priv->dma_rx_chnum);
+        }
+    }
+}
+
+/**
+ * Start sending a chunk of data.
+ * This must be called when the DMA is completed.
+ *
+ * @param priv
+ */
+static void UUSART_DMA_TxStart(struct priv *priv)
+{
+    priv->tx_dma_busy = true;
+    assert_param(priv->dma_tx->CNDTR == 0);
+
+    dbg_uusart("DMA_TxStart (nr %d, nw %d)", (int)priv->tx_buf_nr, (int)priv->tx_buf_nw);
+
+    uint16_t nr = priv->tx_buf_nr;
+    uint16_t nw = priv->tx_buf_nw;
+
+    if (nr == nw) {
+        dbg_uusart("remain=0,do nothing");
+        return;
+    } // do nothing if we're done
+
+    uint8_t chunk = priv->tx_buffer[nr++];
+    //nr += (uint16_t) (4 - (nr & 0b11));
+    if (chunk == 0) {
+        // wrap-around
+        chunk = priv->tx_buffer[0];
+        nr = 1;
+        assert_param(nr < nw);
+    }
+
+    // nr was advanced by the lpad preamble
+    priv->tx_buf_nr = nr;
+    priv->tx_buf_chunk = chunk; // will be further moved by 'chunk' bytes when dma completes
+
+    dbg_uusart("# TX: chunk start %d, len %d", (int)nr, (int)chunk);
+//#if UUSART_DEBUG
+//    PUTS(">"); PUTSN((char *) (priv->tx_buffer + nr), chunk); PUTS("<");
+//    PUTNL();
+//#endif
+
+    LL_DMA_DisableChannel(priv->dma, priv->dma_tx_chnum);
+    {
+        LL_DMA_ClearFlags(priv->dma, priv->dma_tx_chnum);
+        LL_DMA_SetMemoryAddress(priv->dma, priv->dma_tx_chnum, (uint32_t) (priv->tx_buffer + nr));
+        LL_DMA_SetDataLength(priv->dma, priv->dma_tx_chnum, chunk);
+        LL_USART_ClearFlag_TC(priv->periph);
+    }
+    LL_DMA_EnableChannel(priv->dma, priv->dma_tx_chnum);
+}
+
+COMPILER_ASSERT(UUSART_TXBUF_LEN <= 256); // more would break the "len tag" algorithm
+
+/**
+ * Put data on the queue. Only a part may be sent due to a buffer size limit.
+ *
+ * @param priv
+ * @param buffer - buffer to send
+ * @param len - buffer size
+ * @return number of bytes that were really written (from the beginning)
+ */
+uint16_t UUSART_DMA_TxQueue(struct priv *priv, const uint8_t *buffer, uint16_t len)
+{
+    const uint16_t nr = priv->tx_buf_nr;
+    uint16_t nw = priv->tx_buf_nw;
+
+    // shortcut for checking a completely full buffer
+    if (nw == nr-1 || (nr==0&&nw==UUSART_TXBUF_LEN-1)) {
+        dbg_uusart("Buffer full, cant queue");
+        return 0;
+    }
+
+    dbg_uusart("\r\nQueue..");
+
+    uint16_t used = 0;
+    if (nr == nw) {
+        used = 0;
+    } else if (nw > nr) { // simple linear
+        used = (uint16_t) (nw - nr);
+    } else if (nw < nr) { // wrapped
+        used = (uint16_t) ((UUSART_TXBUF_LEN - nr) + nw);
+    }
+
+    dbg_uusart("Trying to send buffer of len %d", (int)len);
+    uint16_t avail = (const uint16_t) (UUSART_TXBUF_LEN - 1 - used);
+    dbg_uusart("nr %d, nw %d, used %d, avail %d", (int)nr, (int)nw, (int)used, (int)avail);
+
+    // hack to avoid too large chunks (we use 1 byte to store chunk size)
+    if (avail > 255) avail = 255;
+
+    uint8_t written = 0;
+
+    // this avoids attempting to write if we don't have space
+    if (avail <= 5) {
+        dbg_uusart("No space (only %d)", (int) avail);
+        return written;
+    }
+
+    int cnt = 0;
+    while (avail > 0 && written < len) {
+        assert_param(cnt < 2); // if more than two, we have a bug and it's repeating infinitely
+
+        cnt++;
+        // Padding with chunk information (1 byte: length) - for each chunk
+        const uint8_t lpad = 1;
+
+        // Chunk can go max to the end of the buffer
+        uint8_t chunk = (uint8_t) MIN((len-written) + lpad, UUSART_TXBUF_LEN - nw);
+        if (chunk > avail) chunk = (uint8_t) avail;
+
+        dbg_uusart("nw %d, raw available chunk %d", (int) nw, (int)chunk);
+        if (chunk < lpad + 1) {
+            // write 0 to indicate a wrap-around
+            dbg_uusart("Wrap-around marker at offset %d", (int) nw);
+            priv->tx_buffer[nw] = 0;
+            nw = 0;
+        }
+        else {
+            // enough space for a preamble + some data
+            dbg_uusart("Preamble of %d bytes at offset %d", (int) lpad, (int) nw);
+            priv->tx_buffer[nw] = (uint8_t) (chunk - lpad);
+            nw += lpad;
+            uint8_t datachunk = (uint8_t) (chunk - lpad);
+            dbg_uusart("Datachunk len %d at offset %d", (int) datachunk, (int) nw);
+//#if UUSART_DEBUG
+//            PUTS("mcpy src >"); PUTSN((char *) (buffer), datachunk); PUTS("<\r\n");
+//#endif
+            memcpy((uint8_t *) (priv->tx_buffer + nw), buffer, datachunk);
+//#if UUSART_DEBUG
+//            PUTS("mcpy dst >"); PUTSN((char *) (priv->tx_buffer + nw), datachunk); PUTS("<\r\n");
+//#endif
+            buffer += datachunk;
+            nw += datachunk;
+            written += datachunk;
+            if (nw == UUSART_TXBUF_LEN) nw = 0;
+        }
+        avail -= chunk;
+        dbg_uusart(". end of loop, avail is %d", (int)avail);
+    }
+
+    {
+        dbg_uusart("Write done -> nr %d, nw %d", (int) nr, (int) nw);
+
+        // FIXME a potential race condition can happen here (but it's unlikely)
+
+        priv->tx_buf_nw = nw;
+
+        if (!priv->tx_dma_busy) {
+            dbg_uusart("Write done, requesting DMA.");
+            UUSART_DMA_TxStart(priv);
+        }
+        else {
+            dbg_uusart("DMA in progress, not requesting");
+        }
+    }
+
+    return written;
+}
+
+/**
+ * Handler for the Tx DMA - completion interrupt
+ * @param arg - unit instance
+ */
+static void UUSART_DMA_TxHandler(void *arg)
+{
+    Unit *unit = arg;
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    uint32_t isrsnapshot = priv->dma->ISR;
+    if (LL_DMA_IsActiveFlag_TC(isrsnapshot, priv->dma_tx_chnum)) {
+        // chunk Tx is finished
+        dbg_uusart("~ DMA tx done, nr %d, nw %d, chunk %d", (int)priv->tx_buf_nr, (int)priv->tx_buf_nw, (int)priv->tx_buf_chunk);
+
+        priv->tx_buf_nr += priv->tx_buf_chunk;
+        if (UUSART_TXBUF_LEN == priv->tx_buf_nr) priv->tx_buf_nr = 0;
+        priv->tx_buf_chunk = 0;
+
+        LL_DMA_ClearFlag_TC(priv->dma, priv->dma_tx_chnum);
+
+        // Wait for TC
+        while (!LL_USART_IsActiveFlag_TC(priv->periph)); // TODO timeout
+
+        // start the next chunk
+        if (priv->tx_buf_nr != priv->tx_buf_nw) {
+            dbg_uusart("  Asking for more, if any");
+            UUSART_DMA_TxStart(priv);
+        } else {
+            priv->tx_dma_busy = false;
+        }
+    }
+}
+
+
+void UUSART_DeInitDMAs(Unit *unit)
+{
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    irqd_detach(priv->dma_tx, UUSART_DMA_RxHandler);
+    irqd_detach(priv->dma_rx, UUSART_DMA_TxHandler);
+
+    LL_DMA_DeInit(priv->dma, priv->dma_rx_chnum);
+    LL_DMA_DeInit(priv->dma, priv->dma_tx_chnum);
+
+    free_ck(priv->rx_buffer);
+    free_ck(priv->tx_buffer);
+}
diff --git a/units/usart/_init.c b/units/usart/_init.c
new file mode 100644
index 0000000..a4068f5
--- /dev/null
+++ b/units/usart/_init.c
@@ -0,0 +1,345 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+#include <platform/irq_dispatcher.h>
+#include "platform.h"
+#include "unit_base.h"
+
+#define UUSART_INTERNAL
+#include "_internal.h"
+
+extern error_t UUSART_ClaimDMAs(Unit *unit);
+extern error_t UUSART_SetupDMAs(Unit *unit);
+extern void UUSART_DeInitDMAs(Unit *unit);
+
+/** Allocate data structure and set defaults */
+error_t UUSART_preInit(Unit *unit)
+{
+    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv));
+    if (priv == NULL) return E_OUT_OF_MEM;
+
+    // some defaults
+    priv->periph_num = 1;
+    priv->remap = 0;
+
+    priv->baudrate = 115200;
+    priv->parity = 0;         //!< 0-none, 1-odd, 2-even
+    priv->stopbits = 1;       //!< 0-half, 1-one, 2-1.5, 3-two
+    priv->direction = 3; // RXTX
+
+    priv->hw_flow_control = false;
+    priv->clock_output = false;
+    priv->cpol = 0;
+    priv->cpha = 0;
+    priv->lsb_first = true; // LSB first is default for UART
+    priv->width = 8;
+
+    priv->data_inv = false;
+    priv->rx_inv = false;
+    priv->tx_inv = false;
+
+    priv->de_output = false;
+    priv->de_polarity = 1; // active high
+    // this should equal to a half-byte length when oversampling by 16 is used (default)
+    priv->de_assert_time = 8;
+    priv->de_clear_time = 8;
+
+    return E_SUCCESS;
+}
+
+/** Claim the peripheral and assign priv->periph */
+static inline error_t UUSART_claimPeriph(Unit *unit)
+{
+    struct priv *priv = unit->data;
+
+    if (!(priv->periph_num >= 1 && priv->periph_num <= 5)) {
+        dbg("!! Bad USART periph");
+        return E_BAD_CONFIG;
+    }
+
+    // assign and claim the peripheral
+    if (priv->periph_num == 1) {
+        TRY(rsc_claim(unit, R_USART1));
+        priv->periph = USART1;
+    }
+    else if (priv->periph_num == 2) {
+        TRY(rsc_claim(unit, R_USART2));
+        priv->periph = USART2;
+    }
+    else if (priv->periph_num == 3) {
+        TRY(rsc_claim(unit, R_USART3));
+        priv->periph = USART3;
+    }
+#if defined(USART4)
+    else if (priv->periph_num == 4) {
+        TRY(rsc_claim(unit, R_USART4));
+        priv->periph = USART4;
+    }
+#endif
+#if defined(USART5)
+        else if (priv->periph_num == 5) {
+        TRY(rsc_claim(unit, R_USART5));
+        priv->periph = USART5;
+    }
+#endif
+    else return E_BAD_CONFIG;
+
+    TRY(UUSART_ClaimDMAs(unit));
+
+    return E_SUCCESS;
+}
+
+/** Claim and configure GPIOs used */
+static inline error_t UUSART_configPins(Unit *unit)
+{
+    struct priv *priv = unit->data;
+    // This is written for F072, other platforms will need adjustments
+
+    // Configure UART pins (AF)
+
+#define want_ck_pin(priv) ((priv)->clock_output)
+#define want_tx_pin(priv) (bool)((priv)->direction & 2)
+#define want_rx_pin(priv) (bool)((priv)->direction & 1)
+#define want_cts_pin(priv) ((priv)->hw_flow_control==2 || (priv)->hw_flow_control==3)
+#define want_rts_pin(priv) ((priv)->de_output || (priv)->hw_flow_control==1 || (priv)->hw_flow_control==3)
+
+    /* List of required pins based on the user config */
+    bool pins_wanted[5] = {
+        want_ck_pin(priv),
+        want_tx_pin(priv),
+        want_rx_pin(priv),
+        want_cts_pin(priv),
+        want_rts_pin(priv)
+    };
+
+#if GEX_PLAT_F072_DISCOVERY
+
+    const struct PinAF *mappings = NULL;
+
+    // TODO adjust this, possibly remove / split to individual pin config for ..
+    // the final board
+
+    const struct PinAF mapping_1_0[5] = {
+        {'A', 8, LL_GPIO_AF_1}, // CK
+        {'A', 9, LL_GPIO_AF_1}, // TX
+        {'A', 10, LL_GPIO_AF_1}, // RX
+        {'A', 11, LL_GPIO_AF_1}, // CTS - collides with USB
+        {'A', 12, LL_GPIO_AF_1}, // RTS - collides with USB
+    };
+
+    const struct PinAF mapping_1_1[5] = {
+        {'A', 8, LL_GPIO_AF_1}, // CK*
+        {'B', 6, LL_GPIO_AF_1}, // TX
+        {'B', 7, LL_GPIO_AF_1}, // RX
+        {'A', 11, LL_GPIO_AF_1}, // CTS* - collides with USB
+        {'A', 12, LL_GPIO_AF_1}, // RTS* - collides with USB
+    };
+
+    const struct PinAF mapping_2_0[5] = {
+        {'A', 4, LL_GPIO_AF_1}, // CK
+        {'A', 2, LL_GPIO_AF_1}, // TX
+        {'A', 3, LL_GPIO_AF_1}, // RX
+        {'A', 0, LL_GPIO_AF_1}, // CTS
+        {'A', 1, LL_GPIO_AF_1}, // RTS
+    };
+
+    const struct PinAF mapping_2_1[5] = {
+        {'A', 4, LL_GPIO_AF_1}, // CK*
+        {'A', 14, LL_GPIO_AF_1}, // TX
+        {'A', 15, LL_GPIO_AF_1}, // RX
+        {'A', 0, LL_GPIO_AF_1}, // CTS*
+        {'A', 1, LL_GPIO_AF_1}, // RTS*
+    };
+
+    const struct PinAF mapping_3_0[5] = {
+        {'B', 12, LL_GPIO_AF_4}, // CK
+        {'B', 10, LL_GPIO_AF_4}, // TX
+        {'B', 11, LL_GPIO_AF_4}, // RX
+        {'B', 13, LL_GPIO_AF_4}, // CTS
+        {'B', 14, LL_GPIO_AF_4}, // RTS
+    };
+
+    const struct PinAF mapping_4_0[5] = {
+        {'C', 12, LL_GPIO_AF_0}, // CK
+        {'A', 0, LL_GPIO_AF_4}, // TX
+        {'A', 1, LL_GPIO_AF_4}, // RX
+        {'B', 7, LL_GPIO_AF_4}, // CTS
+        {'A', 15, LL_GPIO_AF_4}, // RTS
+    };
+
+    const struct PinAF mapping_4_1[5] = {
+        {'C', 12, LL_GPIO_AF_0}, // CK*
+        {'C', 10, LL_GPIO_AF_0}, // TX
+        {'C', 11, LL_GPIO_AF_0}, // RX
+        {'B', 7,  LL_GPIO_AF_4}, // CTS*
+        {'A', 15, LL_GPIO_AF_4}, // RTS*
+    };
+
+    if (priv->periph_num == 1) {
+        // USART1
+        if (priv->remap == 0) mappings = &mapping_1_0[0];
+        else if (priv->remap == 1) mappings = &mapping_1_1[0];
+        else return E_BAD_CONFIG;
+    }
+    else if (priv->periph_num == 2) {
+        // USART2
+        if (priv->remap == 0) mappings = &mapping_2_0[0];
+        else if (priv->remap == 1) mappings = &mapping_2_1[0];
+        else return E_BAD_CONFIG;
+    }
+    else if (priv->periph_num == 3) {
+        // USART3
+        if (priv->remap == 0) mappings = &mapping_3_0[0];
+        else return E_BAD_CONFIG;
+    }
+    else if (priv->periph_num == 4) {
+        // USART3
+        if (priv->remap == 0) mappings = &mapping_4_0[0];
+        else if (priv->remap == 1) mappings = &mapping_4_1[0];
+        else return E_BAD_CONFIG;
+    }
+    else return E_BAD_CONFIG;
+
+    // Apply mappings based on the 'wanted' table
+    for (int i = 0; i < 5; i++) {
+        if (pins_wanted[i]) {
+            if (mappings[i].port == 0) return E_BAD_CONFIG;
+            TRY(rsc_claim_pin(unit, mappings[i].port, mappings[i].pin));
+            hw_configure_gpio_af(mappings[i].port, mappings[i].pin, mappings[i].af);
+        }
+    }
+
+#elif GEX_PLAT_F103_BLUEPILL
+    #error "NO IMPL"
+#elif GEX_PLAT_F303_DISCOVERY
+    #error "NO IMPL"
+#elif GEX_PLAT_F407_DISCOVERY
+    #error "NO IMPL"
+#else
+    #error "BAD PLATFORM!"
+#endif
+
+    return E_SUCCESS;
+}
+
+/** Finalize unit set-up */
+error_t UUSART_init(Unit *unit)
+{
+    struct priv *priv = unit->data;
+
+    TRY(UUSART_claimPeriph(unit));
+    TRY(UUSART_configPins(unit));
+
+    // --- Configure the peripheral ---
+
+    // Enable clock for the peripheral used
+    hw_periph_clock_enable(priv->periph);
+
+    LL_USART_Disable(priv->periph);
+    {
+        LL_USART_DeInit(priv->periph);
+        LL_USART_SetBaudRate(priv->periph, PLAT_APB1_HZ, LL_USART_OVERSAMPLING_16, priv->baudrate);
+
+        LL_USART_SetParity(priv->periph,
+                           priv->parity == 0 ? LL_USART_PARITY_NONE :
+                           priv->parity == 1 ? LL_USART_PARITY_ODD
+                                             : LL_USART_PARITY_EVEN);
+
+        LL_USART_SetStopBitsLength(priv->periph,
+                                   priv->stopbits == 0 ? LL_USART_STOPBITS_0_5 :
+                                   priv->stopbits == 1 ? LL_USART_STOPBITS_1 :
+                                   priv->stopbits == 2 ? LL_USART_STOPBITS_1_5
+                                                       : LL_USART_STOPBITS_2);
+
+        LL_USART_SetTransferDirection(priv->periph,
+                                      priv->direction == 1 ? LL_USART_DIRECTION_RX :
+                                      priv->direction == 2 ? LL_USART_DIRECTION_TX
+                                                           : LL_USART_DIRECTION_TX_RX);
+
+        LL_USART_SetHWFlowCtrl(priv->periph,
+                               priv->hw_flow_control == 0 ? LL_USART_HWCONTROL_NONE :
+                               priv->hw_flow_control == 1 ? LL_USART_HWCONTROL_RTS :
+                               priv->hw_flow_control == 2 ? LL_USART_HWCONTROL_CTS
+                                                          : LL_USART_HWCONTROL_RTS_CTS);
+
+        LL_USART_ConfigClock(priv->periph,
+                             priv->cpha ? LL_USART_PHASE_2EDGE : LL_USART_PHASE_1EDGE,
+                             priv->cpol ? LL_USART_POLARITY_HIGH : LL_USART_POLARITY_LOW,
+                             true); // clock on last bit - TODO configurable?
+
+        if (priv->clock_output)
+            LL_USART_EnableSCLKOutput(priv->periph);
+        else
+            LL_USART_DisableSCLKOutput(priv->periph);
+
+        LL_USART_SetTransferBitOrder(priv->periph,
+                                     priv->lsb_first ? LL_USART_BITORDER_LSBFIRST
+                                                     : LL_USART_BITORDER_MSBFIRST);
+
+        LL_USART_SetDataWidth(priv->periph,
+                              priv->width == 7 ? LL_USART_DATAWIDTH_7B :
+                              priv->width == 8 ? LL_USART_DATAWIDTH_8B
+                                               : LL_USART_DATAWIDTH_9B);
+
+        LL_USART_SetBinaryDataLogic(priv->periph,
+                                    priv->data_inv ? LL_USART_BINARY_LOGIC_NEGATIVE
+                                                   : LL_USART_BINARY_LOGIC_POSITIVE);
+
+        LL_USART_SetRXPinLevel(priv->periph, priv->rx_inv ? LL_USART_RXPIN_LEVEL_INVERTED
+                                                          : LL_USART_RXPIN_LEVEL_STANDARD);
+
+        LL_USART_SetTXPinLevel(priv->periph, priv->tx_inv ? LL_USART_TXPIN_LEVEL_INVERTED
+                                                          : LL_USART_TXPIN_LEVEL_STANDARD);
+
+        if (priv->de_output)
+            LL_USART_EnableDEMode(priv->periph);
+        else
+            LL_USART_DisableDEMode(priv->periph);
+
+        LL_USART_SetDESignalPolarity(priv->periph,
+                                     priv->de_polarity ? LL_USART_DE_POLARITY_HIGH
+                                                       : LL_USART_DE_POLARITY_LOW);
+
+        LL_USART_SetDEAssertionTime(priv->periph, priv->de_assert_time);
+        LL_USART_SetDEDeassertionTime(priv->periph, priv->de_clear_time);
+
+        // Prepare for DMA
+        LL_USART_ClearFlag_TC(priv->periph);
+        LL_USART_EnableDMAReq_RX(priv->periph);
+        LL_USART_EnableDMAReq_TX(priv->periph);
+    }
+    LL_USART_Enable(priv->periph);
+
+    // modifies some usart registers that can't be modified when enabled
+    TRY(UUSART_SetupDMAs(unit));
+
+    // timeout based on the baudrate
+    unit->tick_interval = (uint16_t) ((50 * 1000) / priv->baudrate); // receive timeout (ms)
+    if (unit->tick_interval < 5) unit->tick_interval = 5;
+
+    return E_SUCCESS;
+}
+
+/** Tear down the unit */
+void UUSART_deInit(Unit *unit)
+{
+    struct priv *priv = unit->data;
+
+    // de-init the pins & peripheral only if inited correctly
+    if (unit->status == E_SUCCESS) {
+        assert_param(priv->periph);
+        LL_USART_DeInit(priv->periph);
+
+        // Disable clock
+        hw_periph_clock_disable(priv->periph);
+
+        UUSART_DeInitDMAs(unit);
+    }
+
+    // Release all resources
+    rsc_teardown(unit);
+
+    // Free memory
+    free_ck(unit->data);
+    unit->data = NULL;
+}
diff --git a/units/usart/_internal.h b/units/usart/_internal.h
new file mode 100644
index 0000000..3aeb0d2
--- /dev/null
+++ b/units/usart/_internal.h
@@ -0,0 +1,110 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+
+#ifndef GEX_F072_UUSART_INTERNAL_H
+#define GEX_F072_UUSART_INTERNAL_H
+
+#ifndef UUSART_INTERNAL
+#error "Bad include"
+#endif
+
+#include "platform.h"
+
+#define UUSART_RXBUF_LEN 64
+#define UUSART_TXBUF_LEN 128
+
+/** Private data structure */
+struct priv {
+    uint8_t periph_num; //!< 1-6
+    uint8_t remap;      //!< UART remap option
+
+    uint32_t baudrate;  //!< baud rate
+    uint8_t parity;     //!< 0-none, 1-odd, 2-even
+    uint8_t stopbits;   //!< 0-half, 1-one, 2-one-and-half, 3-two (halves - 1)
+    uint8_t direction;  //!< 1-RX, 2-TX, 3-RXTX
+
+    uint8_t hw_flow_control; //!< HW flow control 0-none, 1-RTC, 2-CTS, 3-full
+    bool clock_output;  //!< Output serial clock
+    bool cpol;          //!< clock CPOL setting
+    bool cpha;          //!< clock CPHA setting
+    bool lsb_first;     //!< bit order
+    uint8_t width;      //!< word width - 7, 8, 9 (this includes parity)
+
+    bool data_inv;  //!< Invert data bytes
+    bool rx_inv;    //!< Invert the RX pin levels
+    bool tx_inv;    //!< Invert the TX pin levels
+
+    bool de_output; //!< Generate the Driver Enable signal for RS485
+    bool de_polarity; //!< DE active level
+    uint8_t de_assert_time;   //!< Time to assert the DE signal before transmit
+    uint8_t de_clear_time; //!< Time to clear the DE signal after transmit
+
+    USART_TypeDef *periph;
+
+    DMA_TypeDef *dma;
+    DMA_Channel_TypeDef *dma_rx;
+    DMA_Channel_TypeDef *dma_tx;
+    uint8_t dma_rx_chnum;
+    uint8_t dma_tx_chnum;
+
+    // DMA stuff
+    volatile uint8_t *rx_buffer;
+    volatile uint16_t rx_buf_readpos;
+
+    volatile uint8_t *tx_buffer;
+    volatile uint16_t tx_buf_nr;
+    volatile uint16_t tx_buf_nw;
+    volatile uint16_t tx_buf_chunk;
+    volatile bool tx_dma_busy;
+
+    volatile uint16_t rx_last_dmapos;
+};
+
+/** Allocate data structure and set defaults */
+error_t UUSART_preInit(Unit *unit);
+// ------------------------------------------------------------------------
+/** Load from a binary buffer stored in Flash */
+void UUSART_loadBinary(Unit *unit, PayloadParser *pp);
+/** Write to a binary buffer for storing in Flash */
+void UUSART_writeBinary(Unit *unit, PayloadBuilder *pb);
+// ------------------------------------------------------------------------
+/** Parse a key-value pair from the INI file */
+error_t UUSART_loadIni(Unit *unit, const char *key, const char *value);
+/** Generate INI file section for the unit */
+void UUSART_writeIni(Unit *unit, IniWriter *iw);
+// ------------------------------------------------------------------------
+/** Tear down the unit */
+void UUSART_deInit(Unit *unit);
+/** Finalize unit set-up */
+error_t UUSART_init(Unit *unit);
+
+/**
+ * Handle received data (we're inside the IRQ)
+ *
+ * @param unit - handled unit
+ * @param endpos - end position in the buffer
+ */
+void UUSART_DMA_HandleRxFromIRQ(Unit *unit, uint16_t endpos);
+
+/**
+ * Put data on the queue. Only a part may be sent due to a buffer size limit.
+ *
+ * @param priv
+ * @param buffer - buffer to send
+ * @param len - buffer size
+ * @return number of bytes that were really written (from the beginning)
+ */
+uint16_t UUSART_DMA_TxQueue(struct priv *priv, const uint8_t *buffer, uint16_t len);
+
+/**
+ * Handle rx timeout, grab what is received and send it immediately.
+ *
+ * @param unit
+ */
+void UUSART_DMA_HandleRxTimeout(Unit *unit);
+
+// ------------------------------------------------------------------------
+
+
+#endif //GEX_F072_UUSART_INTERNAL_H
diff --git a/units/usart/_settings_bin.c b/units/usart/_settings_bin.c
new file mode 100644
index 0000000..dcfe398
--- /dev/null
+++ b/units/usart/_settings_bin.c
@@ -0,0 +1,74 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+
+#include "platform.h"
+#include "unit_base.h"
+
+#define UUSART_INTERNAL
+#include "_internal.h"
+
+/** Load from a binary buffer stored in Flash */
+void UUSART_loadBinary(Unit *unit, PayloadParser *pp)
+{
+    struct priv *priv = unit->data;
+
+    uint8_t version = pp_u8(pp);
+    (void)version;
+
+    priv->periph_num = pp_u8(pp);
+    priv->remap      = pp_u8(pp);
+
+    priv->baudrate   = pp_u32(pp);
+    priv->parity     = pp_u8(pp);
+    priv->stopbits   = pp_u8(pp);
+    priv->direction  = pp_u8(pp);
+
+    priv->hw_flow_control = pp_u8(pp);
+    priv->clock_output    = pp_bool(pp);
+    priv->cpol            = pp_bool(pp);
+    priv->cpha            = pp_bool(pp);
+    priv->lsb_first       = pp_bool(pp);
+    priv->width           = pp_u8(pp);
+
+    priv->data_inv = pp_bool(pp);
+    priv->rx_inv = pp_bool(pp);
+    priv->tx_inv = pp_bool(pp);
+
+    priv->de_output = pp_bool(pp);
+    priv->de_polarity = pp_bool(pp);
+    priv->de_assert_time = pp_u8(pp);
+    priv->de_clear_time = pp_u8(pp);
+}
+
+/** Write to a binary buffer for storing in Flash */
+void UUSART_writeBinary(Unit *unit, PayloadBuilder *pb)
+{
+    struct priv *priv = unit->data;
+
+    pb_u8(pb, 0); // version
+
+    pb_u8(pb, priv->periph_num);
+    pb_u8(pb, priv->remap);
+
+    pb_u32(pb, priv->baudrate);
+    pb_u8(pb, priv->parity);
+    pb_u8(pb, priv->stopbits);
+    pb_u8(pb, priv->direction);
+
+    pb_u8(pb, priv->hw_flow_control);
+    pb_bool(pb, priv->clock_output);
+    pb_bool(pb, priv->cpol);
+    pb_bool(pb, priv->cpha);
+    pb_bool(pb, priv->lsb_first);
+    pb_u8(pb, priv->width);
+
+    pb_bool(pb, priv->data_inv);
+    pb_bool(pb, priv->rx_inv);
+    pb_bool(pb, priv->tx_inv);
+
+    pb_bool(pb, priv->de_output);
+    pb_bool(pb, priv->de_polarity);
+    pb_u8(pb, priv->de_assert_time);
+    pb_u8(pb, priv->de_clear_time);
+}
diff --git a/units/usart/_settings_ini.c b/units/usart/_settings_ini.c
new file mode 100644
index 0000000..487465c
--- /dev/null
+++ b/units/usart/_settings_ini.c
@@ -0,0 +1,168 @@
+//
+// Created by MightyPork on 2018/01/14.
+//
+
+#include "platform.h"
+#include "unit_base.h"
+#include "avrlibc.h"
+
+#define UUSART_INTERNAL
+#include "_internal.h"
+
+/** Parse a key-value pair from the INI file */
+error_t UUSART_loadIni(Unit *unit, const char *key, const char *value)
+{
+    bool suc = true;
+    struct priv *priv = unit->data;
+
+    if (streq(key, "device")) {
+        priv->periph_num = (uint8_t) avr_atoi(value);
+    }
+    else if (streq(key, "remap")) {
+        priv->remap = (uint8_t) avr_atoi(value);
+    }
+    else if (streq(key, "baud-rate")) {
+        priv->baudrate = (uint32_t ) avr_atoi(value);
+    }
+    else if (streq(key, "parity")) {
+        priv->parity = (uint8_t) str_parse_3(value,
+                                             "NONE", 0,
+                                             "ODD", 1,
+                                             "EVEN", 2, &suc);
+    }
+    else if (streq(key, "stop-bits")) {
+        priv->stopbits = (uint8_t) str_parse_4(value,
+                                               "0.5", 0,
+                                               "1", 1,
+                                               "1.5", 2,
+                                               "2", 3, &suc);
+    }
+    else if (streq(key, "direction")) {
+        priv->direction = (uint8_t) str_parse_3(value,
+                                                "RX", 1,
+                                                "TX", 2,
+                                                "RXTX", 3, &suc);
+    }
+    else if (streq(key, "hw-flow-control")) {
+        priv->hw_flow_control = (uint8_t) str_parse_4(value,
+                                                      "NONE", 0,
+                                                      "RTS", 1,
+                                                      "CTS", 2,
+                                                      "FULL", 3, &suc);
+    }
+    else if (streq(key, "word-width")) {
+        priv->width = (uint8_t ) avr_atoi(value);
+    }
+    else if (streq(key, "first-bit")) {
+        priv->lsb_first = (bool)str_parse_2(value, "MSB", 0, "LSB", 1, &suc);
+    }
+    else if (streq(key, "clock-output")) {
+        priv->clock_output = str_parse_yn(value, &suc);
+    }
+    else if (streq(key, "cpol")) {
+        priv->cpol = (bool) avr_atoi(value);
+    }
+    else if (streq(key, "cpha")) {
+        priv->cpha = (bool) avr_atoi(value);
+    }
+    else if (streq(key, "de-output")) {
+        priv->de_output = str_parse_yn(value, &suc);
+    }
+    else if (streq(key, "de-polarity")) {
+        priv->de_polarity = (bool) avr_atoi(value);
+    }
+    else if (streq(key, "de-assert-time")) {
+        priv->de_assert_time = (uint8_t) avr_atoi(value);
+    }
+    else if (streq(key, "de-clear-time")) {
+        priv->de_clear_time = (uint8_t) avr_atoi(value);
+    }
+    else {
+        return E_BAD_KEY;
+    }
+
+    if (!suc) return E_BAD_VALUE;
+    return E_SUCCESS;
+}
+
+/** Generate INI file section for the unit */
+void UUSART_writeIni(Unit *unit, IniWriter *iw)
+{
+    struct priv *priv = unit->data;
+
+    iw_comment(iw, "Peripheral number (UARTx 1-4)");
+    iw_entry(iw, "device", "%d", (int)priv->periph_num);
+
+    iw_comment(iw, "Pin mappings (TX,RX,CK,CTS,RTS/DE)");
+#if GEX_PLAT_F072_DISCOVERY
+    iw_comment(iw, " USART1: (0) A9,A10,A8,A11,A12   (1) B6,B7,A8,A11,A12");
+    iw_comment(iw, " USART2: (0) A2,A3,A4,A0,A1      (1) A14,A15,A4,A0,A1");
+    iw_comment(iw, " USART3: (0) B10,B11,B12,B13,B14");
+    iw_comment(iw, " USART4: (0) A0,A1,C12,B7,A15    (1) C10,C11,C12,B7,A15");
+#elif GEX_PLAT_F103_BLUEPILL
+    #error "NO IMPL"
+#elif GEX_PLAT_F303_DISCOVERY
+    #error "NO IMPL"
+#elif GEX_PLAT_F407_DISCOVERY
+    #error "NO IMPL"
+#else
+    #error "BAD PLATFORM!"
+#endif
+    iw_entry(iw, "remap", "%d", (int)priv->remap);
+
+    iw_cmt_newline(iw);
+    iw_comment(iw, "Baud rate in bps (eg. 9600, 115200)"); // TODO examples/range
+    iw_entry(iw, "baud-rate", "%d", (int)priv->baudrate);
+
+    iw_comment(iw, "Parity type (NONE, ODD, EVEN)");
+    iw_entry(iw, "parity", "%s", str_3(priv->parity,
+                                       0, "NONE",
+                                       1, "ODD",
+                                       2, "EVEN"));
+
+    iw_comment(iw, "Number of stop bits (0.5, 1, 1.5, 2)");
+    iw_entry(iw, "stop-bits", "%s", str_4(priv->stopbits,
+                                          0, "0.5",
+                                          1, "1",
+                                          2, "1.5",
+                                          3, "2"));
+
+    iw_comment(iw, "Bit order (LSB or MSB first)");
+    iw_entry(iw, "first-bit", str_2((uint32_t)priv->lsb_first,
+                                    0, "MSB",
+                                    1, "LSB"));
+
+    iw_comment(iw, "Word width (7,8,9) - including parity bit if used");
+    iw_entry(iw, "word-width", "%d", (int)priv->width);
+
+    iw_comment(iw, "Enabled lines (RX,TX,RXTX)");
+    iw_entry(iw, "direction", str_3(priv->direction,
+                                    1, "RX",
+                                    2, "TX",
+                                    3, "RXTX"));
+
+    iw_comment(iw, "Hardware flow control (NONE, RTS, CTS, FULL)");
+    iw_entry(iw, "hw-flow-control", "%s", str_4(priv->hw_flow_control,
+                                                0, "NONE",
+                                                1, "RTS",
+                                                2, "CTS",
+                                                3, "FULL"));
+
+    iw_cmt_newline(iw);
+    iw_comment(iw, "Generate serial clock (Y,N)");
+    iw_entry(iw, "clock-output", str_yn(priv->clock_output));
+    iw_comment(iw, "Output clock polarity: 0,1 (clock idle level)");
+    iw_entry(iw, "cpol", "%d", (int)priv->cpol);
+    iw_comment(iw, "Output clock phase: 0,1 (active edge, 0-first, 1-second)");
+    iw_entry(iw, "cpha", "%d", (int)priv->cpha);
+
+    iw_cmt_newline(iw);
+    iw_comment(iw, "Generate RS485 Driver Enable signal (Y,N) - uses RTS pin");
+    iw_entry(iw, "de-output", str_yn(priv->de_output));
+    iw_comment(iw, "DE active level: 0,1");
+    iw_entry(iw, "de-polarity", "%d", (int)(priv->de_polarity));
+    iw_comment(iw, "DE assert time (0-31)");
+    iw_entry(iw, "de-assert-time", "%d", (int)(priv->de_assert_time));
+    iw_comment(iw, "DE clear time (0-31)");
+    iw_entry(iw, "de-clear-time", "%d", (int)(priv->de_clear_time));
+}
diff --git a/units/usart/unit_usart.c b/units/usart/unit_usart.c
new file mode 100644
index 0000000..6ff862b
--- /dev/null
+++ b/units/usart/unit_usart.c
@@ -0,0 +1,178 @@
+//
+// Created by MightyPork on 2018/01/02.
+//
+
+#include <stm32f072xb.h>
+#include "platform.h"
+#include "comm/messages.h"
+#include "unit_base.h"
+#include "unit_usart.h"
+#include "tasks/task_msg.h"
+
+#define UUSART_INTERNAL
+#include "_internal.h"
+
+static void UUSART_SendReceivedDataToMaster(Job *job)
+{
+    Unit *unit = job->data1;
+    struct priv *priv = unit->data;
+
+    uint32_t readpos = job->d32;
+    uint32_t count = job->len;
+
+    // Debug: print to debug port
+//    PUTS("Job rx >");
+//    PUTSN((char *) priv->rx_buffer + readpos, (uint16_t) count);
+//    PUTS("<\r\n");
+
+    // Debug: Write out
+//    UU_USART_Write(unit, (const uint8_t *) (priv->rx_buffer + readpos), count);
+
+    // TODO modify TF to allow writing in multiple chunks to avoid this useless buffer copying
+    PayloadBuilder pb = pb_start(unit_tmp512, UNIT_TMP_LEN, NULL);
+    pb_u8(&pb, unit->callsign);
+    pb_u8(&pb, 0x00); // report type "Data received"
+    pb_buf(&pb, (uint8_t *) (priv->rx_buffer + readpos), count);
+    assert_param(pb.ok);
+    com_send_pb(MSG_UNIT_REPORT, &pb);
+}
+
+/**
+ * Handle received data (we're inside the IRQ)
+ *
+ * @param unit - handled unit
+ * @param endpos - end position in the buffer
+ */
+void UUSART_DMA_HandleRxFromIRQ(Unit *unit, uint16_t endpos)
+{
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    uint16_t readpos = priv->rx_buf_readpos;
+    assert_param(endpos > readpos);
+
+    uint16_t count = (endpos - readpos);
+
+    // We defer it to the job queue
+    // FIXME this can starve the shared queue if full duplex is used, we need a second higher priority queue for those report jobs
+    Job j = {
+        .data1 = unit,
+        .d32 = priv->rx_buf_readpos,
+        .len = count,
+        .cb = UUSART_SendReceivedDataToMaster
+    };
+    scheduleJob(&j);
+
+    // Move the read cursor, wrap around if needed
+    if (endpos == UUSART_RXBUF_LEN) endpos = 0;
+    priv->rx_buf_readpos = endpos;
+}
+
+void UUSART_Tick(Unit *unit)
+{
+    assert_param(unit);
+    struct priv *priv = unit->data;
+    assert_param(priv);
+
+    if (priv->rx_last_dmapos == priv->dma_rx->CNDTR) {
+        uint16_t endpos = (uint16_t) (UUSART_RXBUF_LEN - priv->rx_last_dmapos);
+        if (endpos != priv->rx_buf_readpos) {
+            dbg("DMA timeout");
+            UUSART_DMA_HandleRxFromIRQ(unit, endpos);
+        }
+    } else {
+        priv->rx_last_dmapos = (uint16_t) priv->dma_rx->CNDTR;
+    }
+}
+
+
+error_t UU_USART_Write(Unit *unit, const uint8_t *buffer, uint32_t len)
+{
+    CHECK_TYPE(unit, &UNIT_USART);
+    struct priv *priv = unit->data;
+
+    uint32_t t_start = HAL_GetTick();
+    while (len > 0) {
+        // this should be long enough even for the slowest bitrates and 512 bytes
+        if (HAL_GetTick() - t_start > 5000) {
+            return E_HW_TIMEOUT;
+        }
+
+        uint16_t chunk = UUSART_DMA_TxQueue(priv, buffer, (uint16_t) len);
+
+        buffer += chunk;
+        len -= chunk;
+
+        // We give up control if there's another thread waiting and this isn't the last cycle
+        if (len > 0) {
+            osThreadYield();
+        }
+    }
+
+    return E_SUCCESS;
+}
+
+error_t UU_USART_WriteSync(Unit *unit, const uint8_t *buffer, uint32_t len)
+{
+    CHECK_TYPE(unit, &UNIT_USART);
+    struct priv *priv = unit->data;
+
+    TRY(UU_USART_Write(unit, buffer, len));
+
+    // Now wait for the last DMA to complete
+    uint32_t t_start = HAL_GetTick();
+    while (priv->tx_dma_busy) {
+        if (HAL_GetTick() - t_start > 1000) {
+            return E_HW_TIMEOUT;
+        }
+    }
+
+    return E_SUCCESS;
+}
+
+enum PinCmd_ {
+    CMD_WRITE = 0,
+    CMD_WRITE_SYNC = 1,
+};
+
+/** Handle a request message */
+static error_t UUSART_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, PayloadParser *pp)
+{
+    uint32_t len;
+    const uint8_t *pld;
+    switch (command) {
+        case CMD_WRITE:
+            pld = pp_tail(pp, &len);
+            TRY(UU_USART_Write(unit, pld, len));
+            return E_SUCCESS;
+
+        case CMD_WRITE_SYNC:
+            pld = pp_tail(pp, &len);
+            TRY(UU_USART_WriteSync(unit, pld, len));
+            return E_SUCCESS;
+
+        default:
+            return E_UNKNOWN_COMMAND;
+    }
+}
+
+// ------------------------------------------------------------------------
+
+/** Unit template */
+const UnitDriver UNIT_USART = {
+    .name = "USART",
+    .description = "Serial port",
+    // Settings
+    .preInit = UUSART_preInit,
+    .cfgLoadBinary = UUSART_loadBinary,
+    .cfgWriteBinary = UUSART_writeBinary,
+    .cfgLoadIni = UUSART_loadIni,
+    .cfgWriteIni = UUSART_writeIni,
+    // Init
+    .init = UUSART_init,
+    .deInit = UUSART_deInit,
+    // Function
+    .updateTick = UUSART_Tick,
+    .handleRequest = UUSART_handleRequest,
+};
diff --git a/units/usart/unit_usart.h b/units/usart/unit_usart.h
new file mode 100644
index 0000000..32b363a
--- /dev/null
+++ b/units/usart/unit_usart.h
@@ -0,0 +1,34 @@
+//
+// Created by MightyPork on 2018/01/02.
+//
+
+#ifndef GEX_F072_UNIT_USART_H
+#define GEX_F072_UNIT_USART_H
+
+#include "unit.h"
+
+extern const UnitDriver UNIT_USART;
+
+/**
+ * Write bytes. This function is asynchronous and does not wait for completion.
+ * It blocks until there's space in the Tx buffer for the data.
+ *
+ * @param unit
+ * @param buffer - bytes to send
+ * @param len - number of bytes to send
+ * @return success
+ */
+error_t UU_USART_Write(Unit *unit, const uint8_t *buffer, uint32_t len);
+
+/**
+ * Write bytes. Same like UU_USART_Write(), except it waits for the transmission
+ * to complete after sending the last data.
+ *
+ * @param unit
+ * @param buffer - bytes to send
+ * @param len - number of bytes to send
+ * @return success
+ */
+error_t UU_USART_WriteSync(Unit *unit, const uint8_t *buffer, uint32_t len);
+
+#endif //GEX_F072_UNIT_USART_H
diff --git a/utils/cortex_utils.h b/utils/cortex_utils.h
index 1a4db8a..d52cc7a 100644
--- a/utils/cortex_utils.h
+++ b/utils/cortex_utils.h
@@ -12,17 +12,6 @@ static inline bool inIRQ(void)
     return __get_IPSR() != 0;
 }
 
-register char *__SP asm("sp");
-
-static inline bool isDynAlloc(const void *obj)
-{
-    // this is the 0x20000000-something address that should correspond to heap bottom
-    extern char heapstart __asm("end");
-
-    return ((uint32_t)obj >= (uint32_t)&heapstart)
-           && ((uint32_t)obj < (uint32_t)__SP);
-}
-
 /** Tight asm loop */
 #define __asm_loop(cycles) \
 do { \
diff --git a/utils/hexdump.c b/utils/hexdump.c
index 79f8c9b..cb57b64 100644
--- a/utils/hexdump.c
+++ b/utils/hexdump.c
@@ -16,7 +16,7 @@ void hexDump(const char *restrict desc, const void *restrict addr, uint32_t len)
         PRINTF ("%s:\r\n", desc);
 
     if (len == 0) {
-        PRINTF("  ZERO LENGTH\r\n");
+        PUTS("  ZERO LENGTH\r\n");
         return;
     }
 
@@ -26,8 +26,11 @@ void hexDump(const char *restrict desc, const void *restrict addr, uint32_t len)
 
         if ((i % 16) == 0) {
             // Just don't print ASCII for the zeroth line.
-            if (i != 0)
-                PRINTF ("  %s\r\n", buff);
+            if (i != 0) {
+                PUTS("  ");
+                PUTS((const char *) buff);
+                PUTS("\r\n");
+            }
 
             // Output the offset.
             PRINTF ("  %04"PRIx32" ", i);
@@ -38,7 +41,7 @@ void hexDump(const char *restrict desc, const void *restrict addr, uint32_t len)
 
         // And store a printable ASCII character for later.
         if ((pc[i] < 0x20) || (pc[i] > 0x7e))
-            buff[i % 16] = '.';
+            buff[i % 16] = (uint8_t) ((pc[i] == 0xA5) ? ' ' : '.'); // special treatment for 0xA5 which is used as a filler in stacks
         else
             buff[i % 16] = pc[i];
         buff[(i % 16) + 1] = '\0';
@@ -46,7 +49,7 @@ void hexDump(const char *restrict desc, const void *restrict addr, uint32_t len)
 
     // Pad out last line if not exactly 16 characters.
     while ((i % 16) != 0) {
-        PRINTF ("   ");
+        PUTS("   ");
         i++;
     }
 
diff --git a/utils/ini_writer.c b/utils/ini_writer.c
index 52440cb..ab4e83d 100644
--- a/utils/ini_writer.c
+++ b/utils/ini_writer.c
@@ -2,9 +2,10 @@
 // Created by MightyPork on 2017/12/01.
 //
 
-#include <framework/system_settings.h>
 #include "platform.h"
+#include "framework/system_settings.h"
 #include "ini_writer.h"
+#include "malloc_safe.h"
 
 #ifndef MIN
 #define MIN(a,b) ((a)>(b)?(b):(a))
@@ -16,9 +17,25 @@
 
 //#define IWBUFFER_LEN 128 // moved to plat_compat.h
 
-// sprintf from varargs, allocating buffer on stack. Uses 'format' argument
+char *iwbuffer = NULL;
+
+/** Allocate the helper buffer */
+void iw_begin(void)
+{
+    assert_param(iwbuffer == NULL);
+    iwbuffer = malloc_ck(IWBUFFER_LEN);
+    assert_param(iwbuffer != NULL);
+}
+
+/** Release the helper buffer */
+void iw_end(void)
+{
+    assert_param(iwbuffer != NULL);
+    free_ck(iwbuffer);
+}
+
 #define IW_VPRINTF() do { \
-    char iwbuffer[IWBUFFER_LEN]; \
+    assert_param(iwbuffer != NULL); \
     va_list args; \
     va_start(args, format); \
     uint32_t len = (int)fixup_vsnprintf(&iwbuffer[0], IWBUFFER_LEN, format, args); \
@@ -63,7 +80,7 @@ void iw_section(IniWriter *iw, const char *format, ...)
     iw_string(iw, "]\r\n");
 }
 
-void iw_comment(IniWriter *iw, const char *format, ...)
+void iw_commentf(IniWriter *iw, const char *format, ...)
 {
     if (iw->count == 0) return;
     if (!SystemSettings.ini_comments) return;
@@ -73,6 +90,16 @@ void iw_comment(IniWriter *iw, const char *format, ...)
     iw_newline(iw);
 }
 
+void iw_comment(IniWriter *iw, const char *text)
+{
+    if (iw->count == 0) return;
+    if (!SystemSettings.ini_comments) return;
+
+    iw_string(iw, "# ");
+    iw_string(iw, text);
+    iw_newline(iw);
+}
+
 void iw_hdr_comment(IniWriter *iw, const char *format, ...)
 {
     if (iw->count == 0) return;
@@ -95,6 +122,15 @@ void iw_entry(IniWriter *iw, const char *key, const char *format, ...)
 uint32_t iw_measure_total(void (*handler)(IniWriter *))
 {
     IniWriter iw = iw_init(NULL, 0xFFFFFFFF, 1);
+    iw_begin();
     handler(&iw);
+    iw_end();
     return 0xFFFFFFFF - iw.skip;
 }
+
+
+
+void iw_cmt_newline(IniWriter *iw)
+{
+    if (SystemSettings.ini_comments) iw_newline(iw);
+}
diff --git a/utils/ini_writer.h b/utils/ini_writer.h
index 7f77ae9..0ae4ca3 100644
--- a/utils/ini_writer.h
+++ b/utils/ini_writer.h
@@ -13,6 +13,21 @@ typedef struct iniwriter_ {
     uint32_t count;
 } IniWriter;
 
+/**
+ * IniWriter helper buffer, available within a IW-scope only.
+ *
+ * This buffer is used internally by printf-like iw functions.
+ * It can be used to prepare buffer for iw_buff or iw_string,
+ * but must not be used for %s substitutions in iw_* functions.
+ */
+extern char *iwbuffer;
+
+/** Allocate the helper buffer */
+void iw_begin(void);
+
+/** Release the helper buffer */
+void iw_end(void);
+
 /**
  * Initialize a IniWriter struct (macro)
  *
@@ -45,10 +60,9 @@ static inline void iw_string(IniWriter *iw, const char *str)
     }
 }
 
+void iw_cmt_newline(IniWriter *iw);
 #define iw_newline(iw) iw_string(iw, "\r\n")
-#define iw_cmt_newline(iw) do { \
-    if (SystemSettings.ini_comments) iw_string(iw, "\r\n"); \
-} while (0)
+
 
 /**
  * Try to snprintf to the file
@@ -71,13 +85,20 @@ void iw_sprintf(IniWriter *iw, const char *format, ...)
 void iw_section(IniWriter *iw, const char *format, ...)
     __attribute__((format(printf,2,3)));
 
+/**
+ * Try to write a INI comment # blah\r\n
+ * @param iw - iniwriter handle
+ * @param text - format, like printf
+ */
+void iw_comment(IniWriter *iw, const char *text);
+
 /**
  * Try to write a INI comment # blah\r\n
  * @param iw - iniwriter handle
  * @param format - format, like printf
  * @param ... - replacements
  */
-void iw_comment(IniWriter *iw, const char *format, ...)
+void iw_commentf(IniWriter *iw, const char *format, ...)
     __attribute__((format(printf,2,3)));
 
 /**
diff --git a/utils/macro.h b/utils/macro.h
index 924e529..1c65320 100644
--- a/utils/macro.h
+++ b/utils/macro.h
@@ -67,6 +67,9 @@ extern "C" {
 
 #define __at(_addr) __attribute__ ((at(_addr)))
 
+#define VA_ARG_COUNT(...) INTERNAL_GET_ARG_COUNT_PRIVATE(0, ## __VA_ARGS__, 70, 69, 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 32, 31, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0)
+#define INTERNAL_GET_ARG_COUNT_PRIVATE(_0, _1_, _2_, _3_, _4_, _5_, _6_, _7_, _8_, _9_, _10_, _11_, _12_, _13_, _14_, _15_, _16_, _17_, _18_, _19_, _20_, _21_, _22_, _23_, _24_, _25_, _26_, _27_, _28_, _29_, _30_, _31_, _32_, _33_, _34_, _35_, _36, _37, _38, _39, _40, _41, _42, _43, _44, _45, _46, _47, _48, _49, _50, _51, _52, _53, _54, _55, _56, _57, _58, _59, _60, _61, _62, _63, _64, _65, _66, _67, _68, _69, _70, count, ...) count
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/utils/malloc_safe.c b/utils/malloc_safe.c
index 1cc187a..fa57637 100644
--- a/utils/malloc_safe.c
+++ b/utils/malloc_safe.c
@@ -1,48 +1,40 @@
-#include <stdlib.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <inttypes.h>
+#include "platform.h"
 #include "debug.h"
 #include "stm32_assert.h"
 #include "malloc_safe.h"
 
-#if 1
-
-void *malloc_safe_do(size_t size, const char *file, uint32_t line)
+void *malloc_ck_do(size_t size,  const char *file, uint32_t line)
 {
-    void *mem = malloc(size);
-    if (mem == NULL) abort_msg("MALLOC FAILED", file, line);
+    void *mem = pvPortMalloc(size);
+    _malloc_trace(size, mem, file, line);
+    if (mem == NULL) {
+        _warn_msg(file, line, "MALLOC FAILED");
+    }
     return mem;
 }
 
-void *calloc_safe_do(size_t nmemb, size_t size, const char *file, uint32_t line)
+void *calloc_ck_do(size_t nmemb, size_t size, const char *file, uint32_t line)
 {
-    void *mem = calloc(size, nmemb);
-    if (mem == NULL) abort_msg("CALLOC FAILED", file, line);
+    void *mem = malloc_ck_do(nmemb*size, file, line);
+    memset(mem, 0, size*nmemb);
     return mem;
 }
 
-
-void *malloc_ck_do(size_t size, bool *suc, const char *file, uint32_t line)
+char *strdup_ck_do(const char *s, const char* file, uint32_t line)
 {
-    void *mem = malloc(size);
-    if (mem == NULL) {
-        warn_msg("MALLOC FAILED", file, line);
-        *suc = false;
-        mem = NULL;
-    }
-    return mem;
+    size_t len = strlen(s) + 1;
+    void *new = malloc_ck_do(len, file, line);
+    if (new == NULL) return NULL;
+    return (char *) memcpy (new, s, len);
 }
 
-void *calloc_ck_do(size_t nmemb, size_t size, bool *suc, const char *file, uint32_t line)
+char *strndup_ck_do(const char *s, uint32_t len, const char* file, uint32_t line)
 {
-    void *mem = calloc(size, nmemb);
-    if (mem == NULL) {
-        warn_msg("CALLOC FAILED", file, line);
-        *suc = false;
-        mem = NULL;
-    }
-    return mem;
+    // TODO verify - this was not tested
+    size_t alen = MIN(strlen(s) + 1, len);
+    uint8_t *new = malloc_ck_do(alen, file, line);
+    if (new == NULL) return NULL;
+    memcpy (new, s, alen-1);
+    new[alen-1] = '\0';
+    return (char *) new;
 }
-
-#endif
diff --git a/utils/malloc_safe.h b/utils/malloc_safe.h
index 501f80c..da3a6bd 100644
--- a/utils/malloc_safe.h
+++ b/utils/malloc_safe.h
@@ -9,14 +9,35 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-void *malloc_safe_do(size_t size, const char* file, uint32_t line) __attribute__((malloc));
-void *calloc_safe_do(size_t nmemb, size_t size, const char* file, uint32_t line) __attribute__((malloc));
-void *malloc_ck_do(size_t size, bool *suc, const char* file, uint32_t line) __attribute__((malloc));
-void *calloc_ck_do(size_t nmemb, size_t size, bool *suc, const char* file, uint32_t line) __attribute__((malloc));
+void *malloc_ck_do(size_t size, const char* file, uint32_t line) __attribute__((malloc));
+void *calloc_ck_do(size_t nmemb, size_t size, const char* file, uint32_t line) __attribute__((malloc));
+char *strdup_ck_do(const char *s, const char* file, uint32_t line) __attribute__((malloc));
+char *strndup_ck_do(const char *s, uint32_t len, const char* file, uint32_t line) __attribute__((malloc));
 
-#define malloc_s(size)        malloc_safe_do(size,        __BASE_FILE__, __LINE__)
-#define calloc_s(nmemb, size) calloc_safe_do(nmemb, size, __BASE_FILE__, __LINE__)
-#define malloc_ck(size, suc)        malloc_ck_do(size, suc,   __BASE_FILE__, __LINE__)
-#define calloc_ck(nmemb, size, suc) calloc_ck_do(nmemb, size, suc, __BASE_FILE__, __LINE__)
+#if DEBUG_MALLOC
+
+    #define _malloc_trace(len, obj, file, line) do { PRINTF("~ malloc(%d) -> 0x%p at ", len, obj); PUTS(file); PUTCHAR(':'); PRINTF("%d\r\n", (int)line); } while (0)
+    #define _free_trace(obj, file, line)   do { PRINTF("~ free(0x%p) at ", obj); PUTS(file); PUTCHAR(':'); PRINTF("%d\r\n", (int)line); } while (0)
+    #define malloc_ck(size) malloc_ck_do(size, __BASE_FILE__, __LINE__)
+    #define calloc_ck(nmemb, size) calloc_ck_do(nmemb, size, __BASE_FILE__, __LINE__)
+    #define strdup_ck(s) strdup_ck_do(s, __BASE_FILE__, __LINE__)
+    #define strndup_ck(s, len) strndup_ck_do(s, (uint32_t)(len), __BASE_FILE__, __LINE__)
+#else
+    #define _malloc_trace(len, obj, file, line)
+    #define _free_trace(obj, file, line)
+    #define malloc_ck(size) malloc_ck_do(size, "", 0)
+    #define calloc_ck(nmemb, size) calloc_ck_do(nmemb, size, "", 0)
+    #define strdup_ck(s) strdup_ck_do(s, "", 0)
+    #define strndup_ck(s, len) strndup_ck_do(s, (uint32_t)(len), "", 0)
+#endif
+
+/**
+ * Free an allocated object, and assign it to NULL.
+ */
+#define free_ck(obj) do { \
+    _free_trace(obj, __BASE_FILE__, __LINE__); \
+    if ((obj) != NULL) vPortFree((void *)(obj)); \
+    obj = NULL; \
+} while (0)
 
 #endif // MALLOC_SAFE_H
diff --git a/utils/payload_builder.h b/utils/payload_builder.h
index 4c24aa2..cd34b91 100644
--- a/utils/payload_builder.h
+++ b/utils/payload_builder.h
@@ -48,7 +48,7 @@ struct PayloadBuilder_ {
 // --- initializer helper macros ---
 
 /** Start the builder. */
-#define pb_start_e(buf, capacity, bigendian, full_handler) ((PayloadBuilder){buf, buf, (buf)+(capacity), full_handler, bigendian, 1})
+#define pb_start_e(buf, capacity, bigendian, full_handler) ((PayloadBuilder){(uint8_t*)buf, (uint8_t*)buf, (uint8_t*)((buf)+(capacity)), full_handler, bigendian, 1})
 
 /** Start the builder in big-endian mode */
 #define pb_start_be(buf, capacity, full_handler) pb_start_e(buf, capacity, 1, full_handler)
@@ -67,6 +67,12 @@ struct PayloadBuilder_ {
 /** Reset the current pointer to start */
 #define pb_rewind(pb) do { pb->current = pb->start; } while (0)
 
+/** Finalize the buffer composition and get the size */
+static inline uint8_t *pb_close(PayloadBuilder *pb, uint32_t *lendst)
+{
+    *lendst = pb_length(pb);
+    return pb->start;
+}
 
 /** Write from a buffer */
 bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len);
diff --git a/utils/snprintf.c b/utils/snprintf.c
index 1600738..d9f2ed3 100644
--- a/utils/snprintf.c
+++ b/utils/snprintf.c
@@ -23,6 +23,7 @@
 
 /**** pts: sam2p-specific defines ****/
 #include "snprintf.h"
+#include "malloc_safe.h"
 /* #include <stdarg.h> -- from snprintf.h */
 #ifdef NULL  /* as on Mac OS/X 10.5.7 <stdlib.h> */
 #  undef NULL
@@ -32,7 +33,7 @@
 #  define malloc ::operator new
 #else
 #  include <stdlib.h> /* malloc() */
-#include <debug.h>
+#include "debug.h"
 
 #endif
 #define size_t size_t /* normally: int, unsigned */
@@ -834,7 +835,7 @@ size_t vasprintf(char **ptr, const char *format, va_list ap)
     ret = vsnprintf((char*)NULL, 0, format, ap);
     if (ret+1 <= 1) return ret; /* pts: bit of old unsigned trick... */
 
-    if (NULL==(*ptr = (char *)malloc(ret+1))) return (size_t)-1;
+    if (NULL==(*ptr = (char *)malloc_ck(ret+1))) return (size_t)-1;
     ret = vsnprintf(*ptr, ret+1, format, ap);
 
     return ret;
diff --git a/utils/stacksmon.c b/utils/stacksmon.c
index 56e4fed..1fce31c 100644
--- a/utils/stacksmon.c
+++ b/utils/stacksmon.c
@@ -5,6 +5,7 @@
 #include "task_msg.h"
 #include "platform.h"
 #include "stacksmon.h"
+#include "hexdump.h"
 
 #if USE_STACK_MONITOR
 
@@ -14,7 +15,7 @@ struct stackhandle {
     uint32_t len;
 };
 
-#define STACK_NUM 8
+#define STACK_NUM 3
 static uint32_t nextidx = 0;
 static struct stackhandle stacks[STACK_NUM];
 
@@ -55,18 +56,19 @@ void stackmon_dump(void)
         uint32_t words = ((stack->len-free)>>2)+1;
         if (words>stack->len>>2) words=stack->len>>2;
 
-        PRINTF("   Used: \033[33m%"PRIu32" / %"PRIu32" bytes\033[m (\033[33m%"PRIu32" / %"PRIu32" words\033[m) ~ \033[33m%"PRIu32" %%\033[m\r\n",
+        PRINTF("   Used: \033[33m%"PRIu32" / %"PRIu32" bytes\033[m (\033[33m%"PRIu32" / %"PRIu32" words\033[m)",
                (stack->len-free),
                stack->len,
                words,
-               stack->len>>2,
-               (stack->len-free)*100/stack->len
+               stack->len>>2
         );
+        PRINTF(" ~ \033[33m%"PRIu32" %%\033[m\r\n",
+               (stack->len-free)*100/stack->len);
     }
 
     PUTS("\033[36m>> MSG+JOB QUEUE\033[m\r\n");
-    PRINTF("   Used slots: \033[33m%"PRIu32"\033[m\r\n",
-           msgQueHighWaterMark);
+    PRINTF("   Used slots: \033[33m%"PRIu32" / %d\033[m\r\n",
+           msgQueHighWaterMark, RX_QUE_CAPACITY);
 
     PRINTF("\033[1m---------------------------\033[m\r\n\r\n");
 }
diff --git a/utils/str_utils.c b/utils/str_utils.c
index b0fb104..1f67552 100644
--- a/utils/str_utils.c
+++ b/utils/str_utils.c
@@ -40,3 +40,75 @@ uint8_t str_parse_012(const char *str, const char *a, const char *b, const char
     *suc = false;
     return 0;
 }
+
+/** Convert number to one of 2 options */
+const char *str_2(uint32_t n,
+                  uint32_t na, const char *a,
+                  uint32_t nb, const char *b)
+{
+    if (n == nb) return b;
+    return a;
+}
+
+/** Convert number to one of 3 options */
+const char *str_3(uint32_t n,
+                  uint32_t na, const char *a,
+                  uint32_t nb, const char *b,
+                  uint32_t nc, const char *c)
+{
+    if (n == nb) return b;
+    if (n == nc) return c;
+    return a;
+}
+
+/** Convert number to one of 4 options */
+const char *str_4(uint32_t n,
+                  uint32_t na, const char *a,
+                  uint32_t nb, const char *b,
+                  uint32_t nc, const char *c,
+                  uint32_t nd, const char *d)
+{
+    if (n == nb) return b;
+    if (n == nc) return c;
+    if (n == nd) return d;
+    return a;
+}
+
+uint32_t str_parse_2(const char *tpl,
+                     const char *a, uint32_t na,
+                     const char *b, uint32_t nb,
+                     bool *suc)
+{
+    if (streq(tpl, a)) return na;
+    if (streq(tpl, b)) return nb;
+    *suc = false;
+    return na;
+}
+
+uint32_t str_parse_3(const char *tpl,
+                     const char *a, uint32_t na,
+                     const char *b, uint32_t nb,
+                     const char *c, uint32_t nc,
+                     bool *suc)
+{
+    if (streq(tpl, a)) return na;
+    if (streq(tpl, b)) return nb;
+    if (streq(tpl, c)) return nc;
+    *suc = false;
+    return na;
+}
+
+uint32_t str_parse_4(const char *tpl,
+                     const char *a, uint32_t na,
+                     const char *b, uint32_t nb,
+                     const char *c, uint32_t nc,
+                     const char *d, uint32_t nd,
+                     bool *suc)
+{
+    if (streq(tpl, a)) return na;
+    if (streq(tpl, b)) return nb;
+    if (streq(tpl, c)) return nc;
+    if (streq(tpl, d)) return nd;
+    *suc = false;
+    return na;
+}
diff --git a/utils/str_utils.h b/utils/str_utils.h
index bffd070..a4170e0 100644
--- a/utils/str_utils.h
+++ b/utils/str_utils.h
@@ -107,8 +107,41 @@ uint8_t str_parse_01(const char *str, const char *a, const char *b, bool *suc);
 /** Compare string with three options */
 uint8_t str_parse_012(const char *str, const char *a, const char *b, const char *c, bool *suc);
 
-/** Convert bool to one of two options */
-#define str_01(cond, a, b) ((cond) ? (b) : (a))
+/** Convert number to one of 4 options */
+const char *str_2(uint32_t n,
+                  uint32_t na, const char *a,
+                  uint32_t nb, const char *b);
+
+/** Convert number to one of 4 options */
+const char *str_3(uint32_t n,
+                  uint32_t na, const char *a,
+                  uint32_t nb, const char *b,
+                  uint32_t nc, const char *c);
+
+/** Convert number to one of 4 options */
+const char *str_4(uint32_t n,
+                     uint32_t na, const char *a,
+                     uint32_t nb, const char *b,
+                     uint32_t nc, const char *c,
+                     uint32_t nd, const char *d);
+
+uint32_t str_parse_2(const char *tpl,
+                     const char *a, uint32_t na,
+                     const char *b, uint32_t nb,
+                     bool *suc);
+
+uint32_t str_parse_3(const char *tpl,
+                     const char *a, uint32_t na,
+                     const char *b, uint32_t nb,
+                     const char *c, uint32_t nc,
+                     bool *suc);
+
+uint32_t str_parse_4(const char *tpl,
+                     const char *a, uint32_t na,
+                     const char *b, uint32_t nb,
+                     const char *c, uint32_t nc,
+                     const char *d, uint32_t nd,
+                     bool *suc);
 
 /** Convert bool to Y or N */
 #define str_yn(cond) ((cond) ? ("Y") : ("N"))
diff --git a/vfs/vfs_user.c b/vfs/vfs_user.c
index ea9410b..653225c 100644
--- a/vfs/vfs_user.c
+++ b/vfs/vfs_user.c
@@ -32,7 +32,9 @@ static uint32_t read_iw_sector(uint32_t sector_offset, uint8_t *data, uint32_t n
     const uint32_t avail = num_sectors*VFS_SECTOR_SIZE;
     const uint32_t skip = sector_offset*VFS_SECTOR_SIZE;
     IniWriter iw = iw_init((char *)data, skip, avail);
+    iw_begin();
     handler(&iw);
+    iw_end();
     return avail - iw.count;
 }