Code import

7 years ago · 56a935cdd0
commit 56a935cdd0
103 changed files with 14510 additions and 0 deletions
--- a/TinyFrame/TF_Config.h
+++ b/TinyFrame/TF_Config.h
@ -0,0 +1,72 @@
 //
 // Rename to TF_Config.h
 //
 #ifndef TF_CONFIG_H
 #define TF_CONFIG_H
 #include <stdint.h>
 //#include <esp8266.h> // when using with esphttpd
 //----------------------------- FRAME FORMAT ---------------------------------
 // The format can be adjusted to fit your particular application needs
 // If the connection is reliable, you can disable the SOF byte and checksums.
 // That can save up to 9 bytes of overhead.
 // ,-----+----+-----+------+------------+- - - -+------------,
 // | SOF | ID | LEN | TYPE | HEAD_CKSUM | DATA  | PLD_CKSUM  |
 // | 1   | ?  | ?   | ?    | ?          | ...   | ?          | <- size (bytes)
 // '-----+----+-----+------+------------+- - - -+------------'
 // !!! BOTH SIDES MUST USE THE SAME SETTINGS !!!
 // Adjust sizes as desired (1,2,4)
 #define TF_ID_BYTES     2
 #define TF_LEN_BYTES    2
 #define TF_TYPE_BYTES   1
 // Checksum type
 //#define TF_CKSUM_TYPE TF_CKSUM_NONE
 #define TF_CKSUM_TYPE TF_CKSUM_XOR
 //#define TF_CKSUM_TYPE TF_CKSUM_CRC16
 //#define TF_CKSUM_TYPE TF_CKSUM_CRC32
 // Use a SOF byte to mark the start of a frame
 #define TF_USE_SOF_BYTE 1
 // Value of the SOF byte (if TF_USE_SOF_BYTE == 1)
 #define TF_SOF_BYTE     0x01
 //----------------------- PLATFORM COMPATIBILITY ----------------------------
 // used for timeout tick counters - should be large enough for all used timeouts
 typedef uint16_t TF_TICKS;
 // used in loops iterating over listeners
 typedef uint8_t TF_COUNT;
 //----------------------------- PARAMETERS ----------------------------------
 // Maximum received payload size (static buffer)
 // Larger payloads will be rejected.
 #define TF_MAX_PAYLOAD_RX 640
 // Size of the sending buffer. Larger payloads will be split to pieces and sent
 // in multiple calls to the write function. This can be lowered to reduce RAM usage.
 #define TF_SENDBUF_LEN 64
 // --- Listener counts - determine sizes of the static slot tables ---
 // Frame ID listeners (wait for response / multi-part message)
 #define TF_MAX_ID_LST   4
 // Frame Type listeners (wait for frame with a specific first payload byte)
 #define TF_MAX_TYPE_LST 4
 // Generic listeners (fallback if no other listener catches it)
 #define TF_MAX_GEN_LST  1
 // Timeout for receiving & parsing a frame
 // ticks = number of calls to TF_Tick()
 #define TF_PARSER_TIMEOUT_TICKS 10
 //------------------------- End of user config ------------------------------
 #endif //TF_CONFIG_H
--- a/TinyFrame/TF_Integration.c
+++ b/TinyFrame/TF_Integration.c
@ -0,0 +1,44 @@
 //
 // Created by MightyPork on 2017/11/21.
 //
 // TinyFrame integration
 //
 #include "platform.h"
 #include "task_main.h"
 #include "USB/usbd_cdc_if.h"
 #include "TinyFrame.h"
 extern osSemaphoreId semVcomTxReadyHandle;
 extern osMutexId mutTinyFrameTxHandle;
 void TF_WriteImpl(TinyFrame *tf, const uint8_t *buff, size_t len)
 {
    (void) tf;
 #define CHUNK 64 // same as TF_SENDBUF_LEN, so we should always have only one run of the loop
    int32_t total = (int32_t) len;
    while (total > 0) {
        assert_param(osOK == osSemaphoreWait(semVcomTxReadyHandle, 5000));
        assert_param(USBD_OK == CDC_Transmit_FS((uint8_t *) buff, (uint16_t) MIN(total, CHUNK)));
        buff += CHUNK;
        total -= CHUNK;
    }
 }
 /** Claim the TX interface before composing and sending a frame */
 void TF_ClaimTx(TinyFrame *tf)
 {
    (void) tf;
    assert_param(osThreadGetId() != tskMainHandle);
    assert_param(!inIRQ());
    assert_param(osOK == osMutexWait(mutTinyFrameTxHandle, 5000));
 }
 /** Free the TX interface after composing and sending a frame */
 void TF_ReleaseTx(TinyFrame *tf)
 {
    (void) tf;
    assert_param(osOK == osMutexRelease(mutTinyFrameTxHandle));
 }
--- a/TinyFrame/TinyFrame.c
+++ b/TinyFrame/TinyFrame.c
@ -0,0 +1,865 @@
 //---------------------------------------------------------------------------
 #include "TinyFrame.h"
 #include <malloc.h>
 //---------------------------------------------------------------------------
 // Compatibility with ESP8266 SDK
 #ifdef ICACHE_FLASH_ATTR
 #define _TF_FN ICACHE_FLASH_ATTR
 #else
 #define _TF_FN
 #endif
 // Helper macros
 #define TF_MAX(a, b) ((a)>(b)?(a):(b))
 #define TF_MIN(a, b) ((a)<(b)?(a):(b))
 // TODO It would be nice to have per-instance configurable checksum types, but that would
 // mandate configurable field sizes unless we use u32 everywhere (and possibly shorten
 // it when encoding to the buffer). I don't really like this idea so much. -MP
 //region Checksums
 #if TF_CKSUM_TYPE == TF_CKSUM_NONE
 // NONE
 #define CKSUM_RESET(cksum)
 #define CKSUM_ADD(cksum, byte)
 #define CKSUM_FINALIZE(cksum)
 #elif TF_CKSUM_TYPE == TF_CKSUM_XOR
 // ~XOR
 #define CKSUM_RESET(cksum) do { (cksum) = 0; } while (0)
 #define CKSUM_ADD(cksum, byte) do { (cksum) ^= (byte); } while(0)
 #define CKSUM_FINALIZE(cksum)  do { (cksum) = (TF_CKSUM)~cksum; } while(0)
 #elif TF_CKSUM_TYPE == TF_CKSUM_CRC16
 // TODO try to replace with an algorithm
 /** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */
 static const uint16_t crc16_table[256] = {
    0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
    0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
    0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
    0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
    0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
    0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
    0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
    0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
    0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
    0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
    0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
    0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
    0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
    0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
    0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
    0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
    0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
    0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
    0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
    0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
    0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
    0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
    0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
    0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
    0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
    0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
    0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
    0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
    0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
    0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
    0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
    0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
 };
 static inline uint16_t crc16_byte(uint16_t cksum, const uint8_t byte)
 {
    return (cksum >> 8) ^ crc16_table[(cksum ^ byte) & 0xff];
 }
 #define CKSUM_RESET(cksum) do { (cksum) = 0; } while (0)
 #define CKSUM_ADD(cksum, byte) do { (cksum) = crc16_byte((cksum), (byte)); } while(0)
 #define CKSUM_FINALIZE(cksum)
 #elif TF_CKSUM_TYPE == TF_CKSUM_CRC32
 // TODO try to replace with an algorithm
 static const uint32_t crc32_table[] = { /* CRC polynomial 0xedb88320 */
    0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
    0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
    0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
    0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
    0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
    0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
    0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c,
    0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
    0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
    0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
    0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106,
    0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
    0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
    0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
    0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
    0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
    0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
    0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
    0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
    0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
    0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
    0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
    0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
    0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
    0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
    0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
    0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
    0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
    0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
    0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
    0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
    0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
    0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
    0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
    0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
    0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
    0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
    0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
    0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
    0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
    0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
    0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
    0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
 };
 static inline uint32_t crc32_byte(uint32_t cksum, const uint8_t byte)
 {
    return (crc32_table[((cksum) ^ ((uint8_t)byte)) & 0xff] ^ ((cksum) >> 8));
 }
 #define CKSUM_RESET(cksum) do { (cksum) = (TF_CKSUM)0xFFFFFFFF; } while (0)
 #define CKSUM_ADD(cksum, byte) do { (cksum) = crc32_byte(cksum, byte); } while(0)
 #define CKSUM_FINALIZE(cksum)  do { (cksum) = (TF_CKSUM)~(cksum); } while(0)
 #endif
 //endregion
 /** Init with a user-allocated buffer */
 void _TF_FN TF_InitStatic(TinyFrame *tf, TF_Peer peer_bit)
 {
    if (tf == NULL) return;
    // Zero it out, keeping user config
    uint32_t usertag = tf->usertag;
    void * userdata = tf->userdata;
    memset(tf, 0, sizeof(struct TinyFrame_));
    tf->usertag = usertag;
    tf->userdata = userdata;
    tf->peer_bit = peer_bit;
 }
 /** Init with malloc */
 TinyFrame * _TF_FN TF_Init(TF_Peer peer_bit)
 {
    TinyFrame *tf = malloc(sizeof(TinyFrame));
    TF_InitStatic(tf, peer_bit);
    return tf;
 }
 /** Release the struct */
 void TF_DeInit(TinyFrame *tf)
 {
    if (tf == NULL) return;
    free(tf);
 }
 //region Listeners
 /** Reset ID listener's timeout to the original value */
 static inline void _TF_FN renew_id_listener(struct TF_IdListener_ *lst)
 {
    lst->timeout = lst->timeout_max;
 }
 /** Notify callback about ID listener's demise & let it free any resources in userdata */
 static void _TF_FN cleanup_id_listener(TinyFrame *tf, TF_COUNT i, struct TF_IdListener_ *lst)
 {
    TF_Msg msg;
    if (lst->fn == NULL) return;
    // Make user clean up their data - only if not NULL
    if (lst->userdata != NULL) {
        msg.userdata = lst->userdata;
        msg.userdata2 = lst->userdata2;
        msg.data = NULL; // this is a signal that the listener should clean up
        lst->fn(tf, &msg); // return value is ignored here - use TF_STAY or TF_CLOSE
    }
    lst->fn = NULL; // Discard listener
    if (i == tf->count_id_lst - 1) {
        tf->count_id_lst--;
    }
 }
 /** Clean up Type listener */
 static inline void _TF_FN cleanup_type_listener(TinyFrame *tf, TF_COUNT i, struct TF_TypeListener_ *lst)
 {
    lst->fn = NULL; // Discard listener
    if (i == tf->count_type_lst - 1) {
        tf->count_type_lst--;
    }
 }
 /** Clean up Generic listener */
 static inline void _TF_FN cleanup_generic_listener(TinyFrame *tf, TF_COUNT i, struct TF_GenericListener_ *lst)
 {
    lst->fn = NULL; // Discard listener
    if (i == tf->count_generic_lst - 1) {
        tf->count_generic_lst--;
    }
 }
 /** Add a new ID listener. Returns 1 on success. */
 bool _TF_FN TF_AddIdListener(TinyFrame *tf, TF_Msg *msg, TF_Listener cb, TF_TICKS timeout)
 {
    TF_COUNT i;
    struct TF_IdListener_ *lst;
    for (i = 0; i < TF_MAX_ID_LST; i++) {
        lst = &tf->id_listeners[i];
        // test for empty slot
        if (lst->fn == NULL) {
            lst->fn = cb;
            lst->id = msg->frame_id;
            lst->userdata = msg->userdata;
            lst->userdata2 = msg->userdata2;
            lst->timeout_max = lst->timeout = timeout;
            if (i >= tf->count_id_lst) {
                tf->count_id_lst = (TF_COUNT) (i + 1);
            }
            return true;
        }
    }
    return false;
 }
 /** Add a new Type listener. Returns 1 on success. */
 bool _TF_FN TF_AddTypeListener(TinyFrame *tf, TF_TYPE frame_type, TF_Listener cb)
 {
    TF_COUNT i;
    struct TF_TypeListener_ *lst;
    for (i = 0; i < TF_MAX_TYPE_LST; i++) {
        lst = &tf->type_listeners[i];
        // test for empty slot
        if (lst->fn == NULL) {
            lst->fn = cb;
            lst->type = frame_type;
            if (i >= tf->count_type_lst) {
                tf->count_type_lst = (TF_COUNT) (i + 1);
            }
            return true;
        }
    }
    return false;
 }
 /** Add a new Generic listener. Returns 1 on success. */
 bool _TF_FN TF_AddGenericListener(TinyFrame *tf, TF_Listener cb)
 {
    TF_COUNT i;
    struct TF_GenericListener_ *lst;
    for (i = 0; i < TF_MAX_GEN_LST; i++) {
        lst = &tf->generic_listeners[i];
        // test for empty slot
        if (lst->fn == NULL) {
            lst->fn = cb;
            if (i >= tf->count_generic_lst) {
                tf->count_generic_lst = (TF_COUNT) (i + 1);
            }
            return true;
        }
    }
    return false;
 }
 /** Remove a ID listener by its frame ID. Returns 1 on success. */
 bool _TF_FN TF_RemoveIdListener(TinyFrame *tf, TF_ID frame_id)
 {
    TF_COUNT i;
    struct TF_IdListener_ *lst;
    for (i = 0; i < tf->count_id_lst; i++) {
        lst = &tf->id_listeners[i];
        // test if live & matching
        if (lst->fn != NULL && lst->id == frame_id) {
            cleanup_id_listener(tf, i, lst);
            return true;
        }
    }
    return false;
 }
 /** Remove a type listener by its type. Returns 1 on success. */
 bool _TF_FN TF_RemoveTypeListener(TinyFrame *tf, TF_TYPE type)
 {
    TF_COUNT i;
    struct TF_TypeListener_ *lst;
    for (i = 0; i < tf->count_type_lst; i++) {
        lst = &tf->type_listeners[i];
        // test if live & matching
        if (lst->fn != NULL    && lst->type == type) {
            cleanup_type_listener(tf, i, lst);
            return true;
        }
    }
    return false;
 }
 /** Remove a generic listener by its function pointer. Returns 1 on success. */
 bool _TF_FN TF_RemoveGenericListener(TinyFrame *tf, TF_Listener cb)
 {
    TF_COUNT i;
    struct TF_GenericListener_ *lst;
    for (i = 0; i < tf->count_generic_lst; i++) {
        lst = &tf->generic_listeners[i];
        // test if live & matching
        if (lst->fn == cb) {
            cleanup_generic_listener(tf, i, lst);
            return true;
        }
    }
    return false;
 }
 /** Handle a message that was just collected & verified by the parser */
 static void _TF_FN TF_HandleReceivedMessage(TinyFrame *tf)
 {
    TF_COUNT i;
    struct TF_IdListener_ *ilst;
    struct TF_TypeListener_ *tlst;
    struct TF_GenericListener_ *glst;
    TF_Result res;
    // Prepare message object
    TF_Msg msg;
    TF_ClearMsg(&msg);
    msg.frame_id = tf->id;
    msg.is_response = false;
    msg.type = tf->type;
    msg.data = tf->data;
    msg.len = tf->len;
    // Any listener can consume the message, or let someone else handle it.
    // The loop upper bounds are the highest currently used slot index
    // (or close to it, depending on the order of listener removals).
    // ID listeners first
    for (i = 0; i < tf->count_id_lst; i++) {
        ilst = &tf->id_listeners[i];
        if (ilst->fn && ilst->id == msg.frame_id) {
            msg.userdata = ilst->userdata; // pass userdata pointer to the callback
            msg.userdata2 = ilst->userdata2;
            res = ilst->fn(tf, &msg);
            ilst->userdata = msg.userdata; // put it back (may have changed the pointer or set to NULL)
            ilst->userdata2 = msg.userdata2; // put it back (may have changed the pointer or set to NULL)
            if (res != TF_NEXT) {
                // if it's TF_CLOSE, we assume user already cleaned up userdata
                if (res == TF_RENEW) {
                    renew_id_listener(ilst);
                }
                return;
            }
        }
    }
    // clean up for the following listeners that don't use userdata (this avoids data from
    // an ID listener that returned TF_NEXT from leaking into Type and Generic listeners)
    msg.userdata = NULL;
    msg.userdata2 = NULL;
    // Type listeners
    for (i = 0; i < tf->count_type_lst; i++) {
        tlst = &tf->type_listeners[i];
        if (tlst->fn && tlst->type == msg.type) {
            res = tlst->fn(tf, &msg);
            if (res != TF_NEXT) {
                // if it's TF_CLOSE, we assume user already cleaned up userdata
                // TF_RENEW doesn't make sense here because type listeners don't expire
                return;
            }
        }
    }
    // Generic listeners
    for (i = 0; i < tf->count_generic_lst; i++) {
        glst = &tf->generic_listeners[i];
        if (glst->fn) {
            res = glst->fn(tf, &msg);
            if (res != TF_NEXT) {
                // if it's TF_CLOSE, we assume user already cleaned up userdata
                // TF_RENEW doesn't make sense here because generic listeners don't expire
                return;
            }
        }
    }
 }
 //endregion Listeners
 /** Handle a received byte buffer */
 void _TF_FN TF_Accept(TinyFrame *tf, const uint8_t *buffer, size_t count)
 {
    size_t i;
    for (i = 0; i < count; i++) {
        TF_AcceptChar(tf, buffer[i]);
    }
 }
 /** Reset the parser's internal state. */
 void _TF_FN TF_ResetParser(TinyFrame *tf)
 {
    tf->state = TFState_SOF;
    // more init will be done by the parser when the first byte is received
 }
 /** SOF was received - prepare for the frame */
 static void _TF_FN pars_begin_frame(TinyFrame *tf) {
    // Reset state vars
    CKSUM_RESET(tf->cksum);
 #if TF_USE_SOF_BYTE
    CKSUM_ADD(tf->cksum, TF_SOF_BYTE);
 #endif
    tf->discard_data = false;
    // Enter ID state
    tf->state = TFState_ID;
    tf->rxi = 0;
 }
 /** Handle a received char - here's the main state machine */
 void _TF_FN TF_AcceptChar(TinyFrame *tf, unsigned char c)
 {
    // Parser timeout - clear
    if (tf->parser_timeout_ticks >= TF_PARSER_TIMEOUT_TICKS) {
        TF_ResetParser(tf);
    }
    tf->parser_timeout_ticks = 0;
 // DRY snippet - collect multi-byte number from the input stream, byte by byte
 // This is a little dirty, but makes the code easier to read. It's used like e.g. if(),
 // the body is run only after the entire number (of data type 'type') was received
 // and stored to 'dest'
 #define COLLECT_NUMBER(dest, type) dest = (type)(((dest) << 8) | c); \
                                   if (++tf->rxi == sizeof(type))
 #if !TF_USE_SOF_BYTE
    if (tf->state == TFState_SOF) {
            TF_ParsBeginFrame();
        }
 #endif
    switch (tf->state) {
        case TFState_SOF:
            if (c == TF_SOF_BYTE) {
                pars_begin_frame(tf);
            }
            break;
        case TFState_ID:
            CKSUM_ADD(tf->cksum, c);
            COLLECT_NUMBER(tf->id, TF_ID) {
                // Enter LEN state
                tf->state = TFState_LEN;
                tf->rxi = 0;
            }
            break;
        case TFState_LEN:
            CKSUM_ADD(tf->cksum, c);
            COLLECT_NUMBER(tf->len, TF_LEN) {
                // Enter TYPE state
                tf->state = TFState_TYPE;
                tf->rxi = 0;
            }
            break;
        case TFState_TYPE:
            CKSUM_ADD(tf->cksum, c);
            COLLECT_NUMBER(tf->type, TF_TYPE) {
 #if TF_CKSUM_TYPE == TF_CKSUM_NONE
                tf->state = TFState_DATA;
                tf->rxi = 0;
 #else
                // enter HEAD_CKSUM state
                tf->state = TFState_HEAD_CKSUM;
                tf->rxi = 0;
                tf->ref_cksum = 0;
 #endif
            }
            break;
        case TFState_HEAD_CKSUM:
            COLLECT_NUMBER(tf->ref_cksum, TF_CKSUM) {
                // Check the header checksum against the computed value
                CKSUM_FINALIZE(tf->cksum);
                if (tf->cksum != tf->ref_cksum) {
                    TF_ResetParser(tf);
                    break;
                }
                if (tf->len == 0) {
                    TF_HandleReceivedMessage(tf);
                    TF_ResetParser(tf);
                    break;
                }
                // Enter DATA state
                tf->state = TFState_DATA;
                tf->rxi = 0;
                CKSUM_RESET(tf->cksum); // Start collecting the payload
                if (tf->len >= TF_MAX_PAYLOAD_RX) {
                    // ERROR - frame too long. Consume, but do not store.
                    tf->discard_data = true;
                }
            }
            break;
        case TFState_DATA:
            if (tf->discard_data) {
                tf->rxi++;
            } else {
                CKSUM_ADD(tf->cksum, c);
                tf->data[tf->rxi++] = c;
            }
            if (tf->rxi == tf->len) {
 #if TF_CKSUM_TYPE == TF_CKSUM_NONE
                // All done
                TF_HandleReceivedMessage();
                TF_ResetParser();
 #else
                // Enter DATA_CKSUM state
                tf->state = TFState_DATA_CKSUM;
                tf->rxi = 0;
                tf->ref_cksum = 0;
 #endif
            }
            break;
        case TFState_DATA_CKSUM:
            COLLECT_NUMBER(tf->ref_cksum, TF_CKSUM) {
                // Check the header checksum against the computed value
                CKSUM_FINALIZE(tf->cksum);
                if (!tf->discard_data && tf->cksum == tf->ref_cksum) {
                    TF_HandleReceivedMessage(tf);
                }
                TF_ResetParser(tf);
            }
            break;
    }
    // we get here after finishing HEAD, if no data are to be received - handle and clear
    if (tf->len == 0 && tf->state == TFState_DATA) {
        TF_HandleReceivedMessage(tf);
        TF_ResetParser(tf);
    }
 }
 // Helper macros for the Compose functions
 // use variables: si - signed int, b - byte, outbuff - target buffer, pos - count of bytes in buffer
 /**
 * Write a number to the output buffer.
 *
 * @param type - data type
 * @param num - number to write
 * @param xtra - extra callback run after each byte, 'b' now contains the byte.
 */
 #define WRITENUM_BASE(type, num, xtra) \
    for (si = sizeof(type)-1; si>=0; si--) { \
        b = (uint8_t)((num) >> (si*8) & 0xFF); \
        outbuff[pos++] = b; \
        xtra; \
    }
 /**
 * Do nothing
 */
 #define _NOOP()
 /**
 * Write a number without adding its bytes to the checksum
 *
 * @param type - data type
 * @param num - number to write
 */
 #define WRITENUM(type, num)       WRITENUM_BASE(type, num, _NOOP())
 /**
 * Write a number AND add its bytes to the checksum
 *
 * @param type - data type
 * @param num - number to write
 */
 #define WRITENUM_CKSUM(type, num) WRITENUM_BASE(type, num, CKSUM_ADD(cksum, b))
 /**
 * Compose a frame (used internally by TF_Send and TF_Respond).
 * The frame can be sent using TF_WriteImpl(), or received by TF_Accept()
 *
 * @param outbuff - buffer to store the result in
 * @param msg - message written to the buffer
 * @return nr of bytes in outbuff used by the frame, 0 on failure
 */
 static inline size_t _TF_FN TF_ComposeHead(TinyFrame *tf, uint8_t *outbuff, TF_Msg *msg)
 {
    int8_t si = 0; // signed small int
    uint8_t b = 0;
    TF_ID id = 0;
    TF_CKSUM cksum = 0;
    size_t pos = 0; // can be needed to grow larger than TF_LEN
    (void)cksum;
    CKSUM_RESET(cksum);
    // Gen ID
    if (msg->is_response) {
        id = msg->frame_id;
    }
    else {
        id = (TF_ID) (tf->next_id++ & TF_ID_MASK);
        if (tf->peer_bit) {
            id |= TF_ID_PEERBIT;
        }
    }
    msg->frame_id = id; // put the resolved ID into the message object for later use
    // --- Start ---
    CKSUM_RESET(cksum);
 #if TF_USE_SOF_BYTE
    outbuff[pos++] = TF_SOF_BYTE;
    CKSUM_ADD(cksum, TF_SOF_BYTE);
 #endif
    WRITENUM_CKSUM(TF_ID, id);
    WRITENUM_CKSUM(TF_LEN, msg->len);
    WRITENUM_CKSUM(TF_TYPE, msg->type);
 #if TF_CKSUM_TYPE != TF_CKSUM_NONE
    CKSUM_FINALIZE(cksum);
    WRITENUM(TF_CKSUM, cksum);
 #endif
    return pos;
 }
 /**
 * Compose a frame (used internally by TF_Send and TF_Respond).
 * The frame can be sent using TF_WriteImpl(), or received by TF_Accept()
 *
 * @param outbuff - buffer to store the result in
 * @param data - data buffer
 * @param data_len - data buffer len
 * @param cksum - checksum variable, used for all calls to TF_ComposeBody. Must be reset before first use! (CKSUM_RESET(cksum);)
 * @return nr of bytes in outbuff used
 */
 static size_t _TF_FN TF_ComposeBody(uint8_t *outbuff,
                                    const uint8_t *data, TF_LEN data_len,
                                    TF_CKSUM *cksum)
 {
    TF_LEN i = 0;
    uint8_t b = 0;
    size_t pos = 0;
    for (i = 0; i < data_len; i++) {
        b = data[i];
        outbuff[pos++] = b;
        CKSUM_ADD(*cksum, b);
    }
    return pos;
 }
 /**
 * Finalize a frame
 *
 * @param outbuff - buffer to store the result in
 * @param cksum - checksum variable used for the body
 * @return nr of bytes in outbuff used
 */
 static size_t _TF_FN TF_ComposeTail(uint8_t *outbuff, TF_CKSUM *cksum)
 {
    int8_t si = 0; // signed small int
    uint8_t b = 0;
    size_t pos = 0;
 #if TF_CKSUM_TYPE != TF_CKSUM_NONE
    CKSUM_FINALIZE(*cksum);
    WRITENUM(TF_CKSUM, *cksum);
 #endif
    return pos;
 }
 /**
 * Send a message
 *
 * @param tf - instance
 * @param msg - message object
 * @param listener - ID listener, or NULL
 * @param timeout - listener timeout, 0 is none
 * @return true if sent
 */
 static bool _TF_FN TF_SendFrame(TinyFrame *tf, TF_Msg *msg, TF_Listener listener, TF_TICKS timeout)
 {
    size_t len = 0;
    size_t remain = 0;
    size_t sent = 0;
    TF_CKSUM cksum = 0;
    TF_ClaimTx(tf);
    len = TF_ComposeHead(tf, tf->sendbuf, msg);
    if (listener) TF_AddIdListener(tf, msg, listener, timeout);
    CKSUM_RESET(cksum);
    remain = msg->len;
    while (remain > 0) {
        size_t chunk = TF_MIN(TF_SENDBUF_LEN - len, remain);
        len += TF_ComposeBody(tf->sendbuf+len, msg->data+sent, (TF_LEN) chunk, &cksum);
        remain -= chunk;
        sent += chunk;
        // Flush if the buffer is full and we have more to send
        if (remain > 0 && len == TF_SENDBUF_LEN) {
            TF_WriteImpl(tf, (const uint8_t *) tf->sendbuf, len);
            len = 0;
        }
    }
    // Flush if checksum wouldn't fit in the buffer
    if (TF_SENDBUF_LEN - len < sizeof(TF_CKSUM)) {
        TF_WriteImpl(tf, (const uint8_t *) tf->sendbuf, len);
        len = 0;
    }
    // Add checksum, flush what remains to be sent
    len += TF_ComposeTail(tf->sendbuf+len, &cksum);
    TF_WriteImpl(tf, (const uint8_t *) tf->sendbuf, len);
    TF_ReleaseTx(tf);
    return true;
 }
 /** send without listener */
 bool _TF_FN TF_Send(TinyFrame *tf, TF_Msg *msg)
 {
    return TF_SendFrame(tf, msg, NULL, 0);
 }
 /** send without listener and struct */
 bool _TF_FN TF_SendSimple(TinyFrame *tf, TF_TYPE type, const uint8_t *data, TF_LEN len)
 {
    TF_Msg msg;
    TF_ClearMsg(&msg);
    msg.type = type;
    msg.data = data;
    msg.len = len;
    return TF_Send(tf, &msg);
 }
 /** send with a listener waiting for a reply, without the struct */
 bool _TF_FN TF_QuerySimple(TinyFrame *tf, TF_TYPE type, const uint8_t *data, TF_LEN len, TF_Listener listener, TF_TICKS timeout)
 {
    TF_Msg msg;
    TF_ClearMsg(&msg);
    msg.type = type;
    msg.data = data;
    msg.len = len;
    return TF_SendFrame(tf, &msg, listener, timeout);
 }
 /** send with a listener waiting for a reply */
 bool _TF_FN TF_Query(TinyFrame *tf, TF_Msg *msg, TF_Listener listener, TF_TICKS timeout)
 {
    return TF_SendFrame(tf, msg, listener, timeout);
 }
 /** Like TF_Send, but with explicit frame ID (set inside the msg object), use for responses */
 bool _TF_FN TF_Respond(TinyFrame *tf, TF_Msg *msg)
 {
    msg->is_response = true;
    return TF_Send(tf, msg);
 }
 /** Externally renew an ID listener */
 bool _TF_FN TF_RenewIdListener(TinyFrame *tf, TF_ID id)
 {
    TF_COUNT i;
    struct TF_IdListener_ *lst;
    for (i = 0; i < tf->count_id_lst; i++) {
        lst = &tf->id_listeners[i];
        // test if live & matching
        if (lst->fn != NULL && lst->id == id) {
            renew_id_listener(lst);
            return true;
        }
    }
    return false;
 }
 /** Timebase hook - for timeouts */
 void _TF_FN TF_Tick(TinyFrame *tf)
 {
    TF_COUNT i = 0;
    struct TF_IdListener_ *lst;
    // increment parser timeout (timeout is handled when receiving next byte)
    if (tf->parser_timeout_ticks < TF_PARSER_TIMEOUT_TICKS) {
        tf->parser_timeout_ticks++;
    }
    // decrement and expire ID listeners
    for (i = 0; i < tf->count_id_lst; i++) {
        lst = &tf->id_listeners[i];
        if (!lst->fn || lst->timeout == 0) continue;
        // count down...
        if (--lst->timeout == 0) {
            // Listener has expired
            cleanup_id_listener(tf, i, lst);
        }
    }
 }
 /** Default impl for claiming write mutex; can be specific to the instance */
 void __attribute__((weak)) TF_ClaimTx(TinyFrame *tf)
 {
    (void) tf;
    // do nothing
 }
 /** Default impl for releasing write mutex; can be specific to the instance */
 void __attribute__((weak)) TF_ReleaseTx(TinyFrame *tf)
 {
    (void) tf;
    // do nothing
 }
--- a/TinyFrame/TinyFrame.h
+++ b/TinyFrame/TinyFrame.h
@ -0,0 +1,384 @@
 #ifndef TinyFrameH
 #define TinyFrameH
 /**
 * TinyFrame protocol library
 * 
 * (c) Ondřej Hruška 2017, MIT License 
 * no liability/warranty, free for any use, must retain this notice & license
 * 
 * Upstream URL: https://github.com/MightyPork/TinyFrame
 */
 #define TF_VERSION "2.0.1"
 //---------------------------------------------------------------------------
 #include <stdint.h>  // for uint8_t etc
 #include <stdbool.h> // for bool
 #include <stddef.h>  // for NULL
 #include <string.h>  // for memset()
 //---------------------------------------------------------------------------
 // Select checksum type (0 = none, 8 = ~XOR, 16 = CRC16 0x8005, 32 = CRC32)
 #define TF_CKSUM_NONE  0
 #define TF_CKSUM_XOR   8
 #define TF_CKSUM_CRC16 16
 #define TF_CKSUM_CRC32 32
 #include "TF_Config.h"
 //region Resolve data types
 #if TF_LEN_BYTES == 1
 typedef uint8_t TF_LEN;
 #elif TF_LEN_BYTES == 2
 typedef uint16_t TF_LEN;
 #elif TF_LEN_BYTES == 4
 typedef uint32_t TF_LEN;
 #else
 #error Bad value of TF_LEN_BYTES, must be 1, 2 or 4
 #endif
 #if TF_TYPE_BYTES == 1
 typedef uint8_t TF_TYPE;
 #elif TF_TYPE_BYTES == 2
 typedef uint16_t TF_TYPE;
 #elif TF_TYPE_BYTES == 4
 typedef uint32_t TF_TYPE;
 #else
 #error Bad value of TF_TYPE_BYTES, must be 1, 2 or 4
 #endif
 #if TF_ID_BYTES == 1
 typedef uint8_t TF_ID;
 #elif TF_ID_BYTES == 2
 typedef uint16_t TF_ID;
 #elif TF_ID_BYTES == 4
 typedef uint32_t TF_ID;
 #else
 #error Bad value of TF_ID_BYTES, must be 1, 2 or 4
 #endif
 #if TF_CKSUM_TYPE == TF_CKSUM_XOR || TF_CKSUM_TYPE == TF_CKSUM_NONE
 // ~XOR (if 0, still use 1 byte - it won't be used)
 typedef uint8_t TF_CKSUM;
 #elif TF_CKSUM_TYPE == TF_CKSUM_CRC16
 // CRC16
 typedef uint16_t TF_CKSUM;
 #elif TF_CKSUM_TYPE == TF_CKSUM_CRC32
 // CRC32
 typedef uint32_t TF_CKSUM;
 #else
 #error Bad value for TF_CKSUM_TYPE, must be 8, 16 or 32
 #endif
 //endregion
 //---------------------------------------------------------------------------
 // Type-dependent masks for bit manipulation in the ID field
 #define TF_ID_MASK (TF_ID)(((TF_ID)1 << (sizeof(TF_ID)*8 - 1)) - 1)
 #define TF_ID_PEERBIT (TF_ID)((TF_ID)1 << ((sizeof(TF_ID)*8) - 1))
 //---------------------------------------------------------------------------
 /** Peer bit enum (used for init) */
 typedef enum {
    TF_SLAVE = 0,
    TF_MASTER = 1,
 } TF_Peer;
 /** Response from listeners */
 typedef enum {
    TF_NEXT = 0,   //!< Not handled, let other listeners handle it
    TF_STAY = 1,   //!< Handled, stay
    TF_RENEW = 2,  //!< Handled, stay, renew - useful only with listener timeout
    TF_CLOSE = 3,  //!< Handled, remove self
 } TF_Result;
 /** Data structure for sending / receiving messages */
 typedef struct _TF_MSG_STRUCT_ {
    TF_ID frame_id;       //!< message ID
    bool is_response;     //!< internal flag, set when using the Respond function. frame_id is then kept unchanged.
    TF_TYPE type;         //!< received or sent message type
    const uint8_t *data;  //!< buffer of received data or data to send. NULL = listener timed out, free userdata!
    TF_LEN len;           //!< length of the buffer
    void *userdata;       //!< here's a place for custom data; this data will be stored with the listener
    void *userdata2;
 } TF_Msg;
 /**
 * Clear message struct
 */
 static inline void TF_ClearMsg(TF_Msg *msg)
 {
    memset(msg, 0, sizeof(TF_Msg));
 }
 typedef struct TinyFrame_ TinyFrame;
 /**
 * TinyFrame Type Listener callback
 *
 * @param frame_id - ID of the received frame
 * @param type - type field from the message
 * @param data - byte buffer with the application data
 * @param len - number of bytes in the buffer
 * @return listener result
 */
 typedef TF_Result (*TF_Listener)(TinyFrame *tf, TF_Msg *msg);
 // -------------------------------------------------------------------
 // region Internal
 enum TFState_ {
    TFState_SOF = 0,      //!< Wait for SOF
    TFState_LEN,          //!< Wait for Number Of Bytes
    TFState_HEAD_CKSUM,   //!< Wait for header Checksum
    TFState_ID,           //!< Wait for ID
    TFState_TYPE,         //!< Wait for message type
    TFState_DATA,         //!< Receive payload
    TFState_DATA_CKSUM    //!< Wait for Checksum
 };
 struct TF_IdListener_ {
    TF_ID id;
    TF_Listener fn;
    TF_TICKS timeout;     // nr of ticks remaining to disable this listener
    TF_TICKS timeout_max; // the original timeout is stored here
    void *userdata;
    void *userdata2;
 };
 struct TF_TypeListener_ {
    TF_TYPE type;
    TF_Listener fn;
 };
 struct TF_GenericListener_ {
    TF_Listener fn;
 };
 /**
 * Frame parser internal state.
 */
 struct TinyFrame_ {
    /* Public user data */
    void *userdata;
    uint32_t usertag;
    // --- the rest of the struct is internal, do not access directly ---
    /* Own state */
    TF_Peer peer_bit;       //!< Own peer bit (unqiue to avoid msg ID clash)
    TF_ID next_id;          //!< Next frame / frame chain ID
    /* Parser state */
    enum TFState_ state;
    TF_TICKS parser_timeout_ticks;
    TF_ID id;               //!< Incoming packet ID
    TF_LEN len;             //!< Payload length
    uint8_t data[TF_MAX_PAYLOAD_RX]; //!< Data byte buffer
    TF_LEN rxi;             //!< Field size byte counter
    TF_CKSUM cksum;         //!< Checksum calculated of the data stream
    TF_CKSUM ref_cksum;     //!< Reference checksum read from the message
    TF_TYPE type;           //!< Collected message type number
    bool discard_data;      //!< Set if (len > TF_MAX_PAYLOAD) to read the frame, but ignore the data.
    /* --- Callbacks --- */
    /* Transaction callbacks */
    struct TF_IdListener_ id_listeners[TF_MAX_ID_LST];
    struct TF_TypeListener_ type_listeners[TF_MAX_TYPE_LST];
    struct TF_GenericListener_ generic_listeners[TF_MAX_GEN_LST];
    // Those counters are used to optimize look-up times.
    // They point to the highest used slot number,
    // or close to it, depending on the removal order.
    TF_COUNT count_id_lst;
    TF_COUNT count_type_lst;
    TF_COUNT count_generic_lst;
    // Buffer for building frames
    uint8_t sendbuf[TF_SENDBUF_LEN];
 };
 // endregion
 // -------------------------------------------------------------------
 /**
 * Initialize the TinyFrame engine.
 * This can also be used to completely reset it (removing all listeners etc).
 *
 * The field .userdata (or .usertag) can be used to identify different instances
 * in the TF_WriteImpl() function etc. Set this field after the init.
 *
 * This function is a wrapper around TF_InitStatic that calls malloc() to obtain
 * the instance.
 *
 * @param peer_bit - peer bit to use for self
 */
 TinyFrame *TF_Init(TF_Peer peer_bit);
 /**
 * Initialize the TinyFrame engine using a statically allocated instance struct.
 *
 * The .userdata / .usertag field is preserved when TF_InitStatic is called.
 *
 * @param peer_bit - peer bit to use for self
 */
 void TF_InitStatic(TinyFrame *tf, TF_Peer peer_bit);
 /**
 * De-init the dynamically allocated TF instance
 *
 * @param tf
 */
 void TF_DeInit(TinyFrame *tf);
 /**
 * Reset the frame parser state machine.
 * This does not affect registered listeners.
 */
 void TF_ResetParser(TinyFrame *tf);
 /**
 * Register a frame type listener.
 *
 * @param msg - message (contains frame_id and userdata)
 * @param cb - callback
 * @param timeout - timeout in ticks to auto-remove the listener (0 = keep forever)
 * @return slot index (for removing), or TF_ERROR (-1)
 */
 bool TF_AddIdListener(TinyFrame *tf, TF_Msg *msg, TF_Listener cb, TF_TICKS timeout);
 /**
 * Remove a listener by the message ID it's registered for
 *
 * @param frame_id - the frame we're listening for
 */
 bool TF_RemoveIdListener(TinyFrame *tf, TF_ID frame_id);
 /**
 * Register a frame type listener.
 *
 * @param frame_type - frame type to listen for
 * @param cb - callback
 * @return slot index (for removing), or TF_ERROR (-1)
 */
 bool TF_AddTypeListener(TinyFrame *tf, TF_TYPE frame_type, TF_Listener cb);
 /**
 * Remove a listener by type.
 *
 * @param type - the type it's registered for
 */
 bool TF_RemoveTypeListener(TinyFrame *tf, TF_TYPE type);
 /**
 * Register a generic listener.
 *
 * @param cb - callback
 * @return slot index (for removing), or TF_ERROR (-1)
 */
 bool TF_AddGenericListener(TinyFrame *tf, TF_Listener cb);
 /**
 * Remove a generic listener by function pointer
 *
 * @param cb - callback function to remove
 */
 bool TF_RemoveGenericListener(TinyFrame *tf, TF_Listener cb);
 /**
 * Send a frame, no listener
 *
 * @param msg - message struct. ID is stored in the frame_id field
 * @return success
 */
 bool TF_Send(TinyFrame *tf, TF_Msg *msg);
 /**
 * Like TF_Send, but without the struct
 */
 bool TF_SendSimple(TinyFrame *tf, TF_TYPE type, const uint8_t *data, TF_LEN len);
 /**
 * Send a frame, and optionally attach an ID listener.
 *
 * @param msg - message struct. ID is stored in the frame_id field
 * @param listener - listener waiting for the response (can be NULL)
 * @param timeout - listener expiry time in ticks
 * @return success
 */
 bool TF_Query(TinyFrame *tf, TF_Msg *msg, TF_Listener listener, TF_TICKS timeout);
 /**
 * Like TF_Query, but without the struct
 */
 bool TF_QuerySimple(TinyFrame *tf, TF_TYPE type, const uint8_t *data, TF_LEN len,
                    TF_Listener listener, TF_TICKS timeout);
 /**
 * Send a response to a received message.
 *
 * @param msg - message struct. ID is read from frame_id. set ->renew to reset listener timeout
 * @return success
 */
 bool TF_Respond(TinyFrame *tf, TF_Msg *msg);
 /**
 * Renew an ID listener timeout externally (as opposed to by returning TF_RENEW from the ID listener)
 *
 * @param id - listener ID to renew
 * @return true if listener was found and renewed
 */
 bool TF_RenewIdListener(TinyFrame *tf, TF_ID id);
 /**
 * Accept incoming bytes & parse frames
 *
 * @param buffer - byte buffer to process
 * @param count - nr of bytes in the buffer
 */
 void TF_Accept(TinyFrame *tf, const uint8_t *buffer, size_t count);
 /**
 * Accept a single incoming byte
 *
 * @param c - a received char
 */
 void TF_AcceptChar(TinyFrame *tf, uint8_t c);
 /**
 * This function should be called periodically.
 *
 * The time base is used to time-out partial frames in the parser and
 * automatically reset it.
 *
 * (suggestion - call this in a SysTick handler)
 */
 void TF_Tick(TinyFrame *tf);
 // --- TO BE IMPLEMENTED BY USER ---
 /**
 * 'Write bytes' function that sends data to UART
 *
 * ! Implement this in your application code !
 */
 extern void TF_WriteImpl(TinyFrame *tf, const uint8_t *buff, size_t len);
 /** Claim the TX interface before composing and sending a frame */
 extern void TF_ClaimTx(TinyFrame *tf);
 /** Free the TX interface after composing and sending a frame */
 extern void TF_ReleaseTx(TinyFrame *tf);
 #endif
--- a/USB/MSC_CDC/usbd_msc_cdc.c
+++ b/USB/MSC_CDC/usbd_msc_cdc.c
@ -0,0 +1,351 @@
 //
 // Created by MightyPork on 2017/11/07.
 //
 #include "platform.h"
 #include "framework/system_settings.h"
 #include "usbd_desc.h"
 #include "task_main.h"
 #include "usbd_msc.h"
 #include "usbd_cdc.h"
 #include "usbd_msc_cdc.h"
 #define USBD_MSC_CDC_CONFIG_DESC_SIZ 98
 /* USB Mass storage device Configuration Descriptor */
 /*   All Descriptors (Configuration, Interface, Endpoint, Class, Vendor */
 // NOTE: This must NOT be in flash, otherwise the driver / peripheral crashes
 __ALIGN_BEGIN uint8_t USBD_MSC_CDC_CfgFSDesc[USBD_MSC_CDC_CONFIG_DESC_SIZ]  __ALIGN_END =
    {
 /*0*/   0x09,   /* bLength: Configuation Descriptor size */
        USB_DESC_TYPE_CONFIGURATION,   /* bDescriptorType: Configuration */
        LOBYTE(USBD_MSC_CDC_CONFIG_DESC_SIZ), // TODO
        HIBYTE(USBD_MSC_CDC_CONFIG_DESC_SIZ),
        3,  /* bNumInterfaces - 1 MSC + 2 composite CDC ACM */
        0x01,   /* bConfigurationValue: */
        0,   /* iConfiguration: - string descriptor */
        0xC0,   /* bmAttributes: - self powered */
        150,   /* MaxPower 300 mA */
            /********************  Mass Storage interface ********************/
 /*9*/       0x09,   /* bLength: Interface Descriptor size */
            USB_DESC_TYPE_INTERFACE,   /* bDescriptorType: */
            0x00,   /* bInterfaceNumber: Number of Interface */
            0x00,   /* bAlternateSetting: Alternate setting */
            0x02,   /* bNumEndpoints*/
            0x08,   /* bInterfaceClass: MSC Class */ // #14
            0x06,   /* bInterfaceSubClass : SCSI transparent*/
            0x50,   /* nInterfaceProtocol */
            0x04,   /* iInterface: String descriptor */
                /********************  Mass Storage Endpoints ********************/
 /*18*/          0x07,   /*Endpoint descriptor length = 7*/
                0x05,   /*Endpoint descriptor type */
                MSC_EPIN_ADDR,   /*Endpoint address (IN, address 1) */
                0x02,   /*Bulk endpoint type */
                LOBYTE(MSC_MAX_FS_PACKET),
                HIBYTE(MSC_MAX_FS_PACKET),
                0x00,   /*Polling interval in milliseconds */
 /*25*/          0x07,   /*Endpoint descriptor length = 7 */
                0x05,   /*Endpoint descriptor type */
                MSC_EPOUT_ADDR,   /*Endpoint address (OUT, address 1) */
                0x02,   /*Bulk endpoint type */
                LOBYTE(MSC_MAX_FS_PACKET),
                HIBYTE(MSC_MAX_FS_PACKET),
                0x00,     /*Polling interval in milliseconds*/
            /********************** IFACE ASSOC *************************/
 /*32*/      0x08, /* bLength */
            USB_DESC_TYPE_IFACE_ASSOCIATION, /* bDescriptorType */
            0x01, /* bFirstInterface */
            0x02, /* bInterfaceCount */
            0x02, /* bFunctionClass */  // #36
            0x02, /* bFunctionSubClass (ACM) */
            0x01, /* bFunctionProtocol (AT-COMMANDS) */
            0x05, /* iFunction: string descriptor */
            /********************** ACM interface **********************/
 /*40*/      0x09,   /* bLength: Interface Descriptor size */
            USB_DESC_TYPE_INTERFACE,  /* bDescriptorType: Interface */
            0x01,   /* bInterfaceNumber: Number of Interface */
            0x00,   /* bAlternateSetting: Alternate setting */
            0x01,   /* bNumEndpoints: One endpoints used */
            0x02,   /* bInterfaceClass: Communication Interface Class */  // #45
            0x02,   /* bInterfaceSubClass: Abstract Control Model */
            0x01,   /* bInterfaceProtocol: Common AT commands */
            0x05,   /* iInterface: string descriptor */
                /**************** Header Functional Descriptor ***************/
 /*49*/          0x05,   /* bLength: Endpoint Descriptor size */
                0x24,   /* bDescriptorType: CS_INTERFACE */
                0x00,   /* bDescriptorSubtype: Header Func Desc */
                0x10,   /* bcdCDC: spec release number */
                0x01,
                /*********** Call Management Functional Descriptor **********/
 /*54*/          0x05,   /* bFunctionLength */
                0x24,   /* bDescriptorType: CS_INTERFACE */
                0x01,   /* bDescriptorSubtype: Call Management Func Desc */
                0x00,   /* bmCapabilities: D0+D1 */
                0x02,   /* bDataInterface: 2 */
                /*************** ACM Functional Descriptor ***************/
 /*59*/          0x04,   /* bFunctionLength */
                0x24,   /* bDescriptorType: CS_INTERFACE */
                0x02,   /* bDescriptorSubtype: Abstract Control Management desc */
                0x06,   /* bmCapabilities XXX was:0x02? */
                /************* Union Functional Descriptor **************/
 /*63*/          0x05,   /* bFunctionLength */
                0x24,   /* bDescriptorType: CS_INTERFACE */
                0x06,   /* bDescriptorSubtype: Union func desc */
                0x01,   /* bMasterInterface: Communication class interface */
                0x02,   /* bSlaveInterface0: Data Class Interface */
                /********************  CDC Endpoints ********************/
                /// Command endpoint
 /*68*/          0x07,                           /* bLength: Endpoint Descriptor size */
                USB_DESC_TYPE_ENDPOINT,   /* bDescriptorType: Endpoint */
                CDC_CMD_EP,                     /* bEndpointAddress */
                0x03,                           /* bmAttributes: Interrupt */
                LOBYTE(CDC_CMD_PACKET_SIZE),     /* wMaxPacketSize: TODO: 2?*/
                HIBYTE(CDC_CMD_PACKET_SIZE),
                0xFF,                           /* bInterval: TODO was 0x10?*/
            /********** CDC Data Class Interface Descriptor ***********/
 /*75*/      0x09,   /* bLength: Endpoint Descriptor size */
            USB_DESC_TYPE_INTERFACE,  /* bDescriptorType: */
            0x02,   /* bInterfaceNumber: Number of Interface */
            0x00,   /* bAlternateSetting: Alternate setting */
            0x02,   /* bNumEndpoints: Two endpoints used */
            0x0A,   /* bInterfaceClass: CDC */    // #80
            0x00,   /* bInterfaceSubClass: */
            0x00,   /* bInterfaceProtocol: */
            0x06,   /* iInterface: TODO: string descriptor */
                /// Endpoint OUT Descriptor
 /*84*/          0x07,   /* bLength: Endpoint Descriptor size */
                USB_DESC_TYPE_ENDPOINT,      /* bDescriptorType: Endpoint */
                CDC_OUT_EP,                        /* bEndpointAddress */
                0x02,                              /* bmAttributes: Bulk */
                LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE),  /* wMaxPacketSize: TODO 8? */
                HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE),
                0x00,                              /* bInterval: ignore for Bulk transfer */
                /// Endpoint IN Descriptor
 /*91*/          0x07,   /* bLength: Endpoint Descriptor size */
                USB_DESC_TYPE_ENDPOINT,      /* bDescriptorType: Endpoint */
                CDC_IN_EP,                         /* bEndpointAddress */
                0x02,                              /* bmAttributes: Bulk */
                LOBYTE(CDC_DATA_FS_MAX_PACKET_SIZE),  /* wMaxPacketSize: TODO 16? */
                HIBYTE(CDC_DATA_FS_MAX_PACKET_SIZE),
                0x00                               /* bInterval: ignore for Bulk transfer */
    };
 /* USB Standard Device Descriptor */
 __ALIGN_BEGIN  uint8_t USBD_MSC_CDC_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]  __ALIGN_END =
    {
        USB_LEN_DEV_QUALIFIER_DESC,
        USB_DESC_TYPE_DEVICE_QUALIFIER,
        0x00,
        0x02,
        0x00,
        0x00,
        0x00,
        MSC_MAX_FS_PACKET,
        0x01,
        0x00,
    };
 static uint8_t USBD_MSC_CDC_Init(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx)
 {
    // this is not correct, but will work if neither returns BUSY (which they don't)
    uint8_t ret = 0;
    ret |= USBD_MSC_Init(pdev, cfgidx);
    ret |= USBD_CDC_Init(pdev, cfgidx);
    return ret;
 }
 static uint8_t USBD_MSC_CDC_DeInit(struct _USBD_HandleTypeDef *pdev, uint8_t cfgidx)
 {
    uint8_t ret = 0;
    ret |= USBD_MSC_DeInit(pdev, cfgidx);
    ret |= USBD_CDC_DeInit(pdev, cfgidx);
    return ret;
 }
 /* Control Endpoints*/
 static uint8_t USBD_MSC_CDC_Setup(struct _USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req)
 {
    static uint8_t interface_bit;
    // handle common
    switch (req->bmRequest & USB_REQ_TYPE_MASK) {
        case USB_REQ_TYPE_STANDARD:
            switch (req->bRequest) {
                // those don't really do anything useful, but each class implements
                // them and it would send the data twice.
                case USB_REQ_GET_INTERFACE :
                    USBD_CtlSendData(pdev, &interface_bit, 1);
                    return USBD_OK;
                case USB_REQ_SET_INTERFACE :
                    interface_bit = (uint8_t) (req->wValue);
                    return USBD_OK;
            }
    }
    // class specific, or Interface -> Clear Feature
    USBD_MSC_Setup(pdev, req);
    USBD_CDC_Setup(pdev, req);
    return USBD_OK;
 }
 #if 0
 static uint8_t USBD_MSC_CDC_EP0_TxSent(struct _USBD_HandleTypeDef *pdev)
 {
    // not handled by either
    xTaskNotify(tskUsbEventHandle, USBEVT_FLAG_EP0_TX_SENT, eSetBits);
    return USBD_OK;
 }
 #endif
 static uint8_t USBD_MSC_CDC_EP0_RxReady(struct _USBD_HandleTypeDef *pdev)
 {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xTaskNotifyFromISR(tskMainHandle, USBEVT_FLAG_EP0_RX_RDY, eSetBits, &xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    //USBD_CDC_EP0_RxReady(pdev);
    return USBD_OK;
 }
 /* Class Specific Endpoints*/
 static uint8_t USBD_MSC_CDC_DataIn(struct _USBD_HandleTypeDef *pdev, uint8_t epnum)
 {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xTaskNotifyFromISR(tskMainHandle, USBEVT_FLAG_EPx_IN(epnum), eSetBits, &xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 //   if (epnum == MSC_EPIN_ADDR||epnum==MSC_EPOUT_ADDR) USBD_MSC_DataIn(pdev, epnum);
 //   else if (epnum == CDC_IN_EP||epnum == CDC_CMD_EP) USBD_CDC_DataIn(pdev, epnum);
    return USBD_OK;
 }
 static uint8_t USBD_MSC_CDC_DataOut(struct _USBD_HandleTypeDef *pdev, uint8_t epnum)
 {
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xTaskNotifyFromISR(tskMainHandle, USBEVT_FLAG_EPx_OUT(epnum), eSetBits, &xHigherPriorityTaskWoken);
    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 //    if (epnum == MSC_EPIN_ADDR||epnum == MSC_EPOUT_ADDR) USBD_MSC_DataOut(pdev, epnum);
 //    else if (epnum == CDC_OUT_EP||epnum == CDC_CMD_EP) USBD_CDC_DataOut(pdev, epnum);
    return USBD_OK;
 }
 #if 0
 static uint8_t USBD_MSC_CDC_SOF(struct _USBD_HandleTypeDef *pdev)
 {
    // not handled by either
    return USBD_OK;
 }
 static uint8_t USBD_MSC_CDC_IsoINIncomplete(struct _USBD_HandleTypeDef *pdev, uint8_t epnum)
 {
    // not handled by either
    return USBD_OK;
 }
 static uint8_t USBD_MSC_CDC_IsoOUTIncomplete(struct _USBD_HandleTypeDef *pdev, uint8_t epnum)
 {
    // not handled by either
    return USBD_OK;
 }
 static uint8_t *USBD_MSC_CDC_GetUsrStrDescriptor(struct _USBD_HandleTypeDef *pdev, uint8_t index, uint16_t *length)
 {
    // not handled by either
    return USBD_OK;
 }
 #endif
 #if 0
 uint8_t  *USBD_MSC_CDC_GetHSCfgDesc (uint16_t *length)
 {
 //    *length = sizeof (USBD_MSC_CDC_CfgHSDesc);
 //    return USBD_MSC_CDC_CfgHSDesc;
    *length = sizeof (USBD_MSC_CDC_CfgFSDesc);
    return USBD_MSC_CDC_CfgFSDesc;
 }
 #endif
 uint8_t  *USBD_MSC_CDC_GetFSCfgDesc (uint16_t *length)
 {
    *length = sizeof (USBD_MSC_CDC_CfgFSDesc);
    // Optionally hide CDC-ACM by setting its class to Vendor Specific
    bool cdc_visible = SystemSettings.visible_vcom;
    USBD_MSC_CDC_CfgFSDesc[36] = (uint8_t) (cdc_visible ? 0x02 : 0xFF);
    USBD_MSC_CDC_CfgFSDesc[45] = (uint8_t) (cdc_visible ? 0x02 : 0xFF);
    USBD_MSC_CDC_CfgFSDesc[80] = (uint8_t) (cdc_visible ? 0x0A : 0xFF);
    // Optionally hide settings (if lock jumper is installed)
    bool msc_visible = SystemSettings.editable;
    USBD_MSC_CDC_CfgFSDesc[14] = (uint8_t) (msc_visible ? 0x08 : 0xFF);
    return USBD_MSC_CDC_CfgFSDesc;
 }
 #if 0
 uint8_t  *USBD_MSC_CDC_GetOtherSpeedCfgDesc (uint16_t *length)
 {
 //    *length = sizeof (USBD_MSC_CDC_OtherSpeedCfgDesc);
 //    return USBD_MSC_CDC_OtherSpeedCfgDesc;
    *length = sizeof (USBD_MSC_CDC_CfgFSDesc);
    return USBD_MSC_CDC_CfgFSDesc;
 }
 #endif
 uint8_t  *USBD_MSC_CDC_GetDeviceQualifierDescriptor (uint16_t *length)
 {
    *length = sizeof (USBD_MSC_CDC_DeviceQualifierDesc);
    return USBD_MSC_CDC_DeviceQualifierDesc;
 }
 uint8_t  *USBD_MSC_CDC_GetUsrStrDescriptor(struct _USBD_HandleTypeDef *pdev, uint8_t index, uint16_t *length)
 {
    const char *str;
    switch (index) {
        case 0x04: str = "Settings Virtual Mass Storage"; break;
        case 0x05: str = "Virtual Comport ACM"; break;
        case 0x06: str = "Virtual Comport CDC"; break;
        default: str = "No Descriptor";
    }
    USBD_GetString ((uint8_t *) str, USBD_StrDesc, length);
    return USBD_StrDesc;
 }
 USBD_ClassTypeDef USBD_MSC_CDC =
    {
        USBD_MSC_CDC_Init,
        USBD_MSC_CDC_DeInit,
        USBD_MSC_CDC_Setup,
        NULL, // USBD_MSC_CDC_EP0_TxSent,
        USBD_MSC_CDC_EP0_RxReady,
        USBD_MSC_CDC_DataIn,
        USBD_MSC_CDC_DataOut,
        NULL, // USBD_MSC_CDC_SOF,
        NULL, // USBD_MSC_CDC_IsoINIncomplete,
        NULL, // USBD_MSC_CDC_IsoOUTIncomplete,
        // we return only the FS one, others are useless
        USBD_MSC_CDC_GetFSCfgDesc, //USBD_MSC_CDC_GetHSCfgDesc,
        USBD_MSC_CDC_GetFSCfgDesc,
        USBD_MSC_CDC_GetFSCfgDesc, //USBD_MSC_CDC_GetOtherSpeedCfgDesc,
        USBD_MSC_CDC_GetDeviceQualifierDescriptor,
        USBD_MSC_CDC_GetUsrStrDescriptor
    };
--- a/USB/MSC_CDC/usbd_msc_cdc.h
+++ b/USB/MSC_CDC/usbd_msc_cdc.h
@ -0,0 +1,11 @@
 //
 // Created by MightyPork on 2017/11/07.
 //
 #ifndef GEX_USBD_CDC_MSC_H
 #define GEX_USBD_CDC_MSC_H
 #include "usbd_def.h"
 extern USBD_ClassTypeDef  USBD_MSC_CDC;
 #endif //GEX_USBD_CDC_MSC_H
--- a/USB/usb_device.c
+++ b/USB/usb_device.c
@ -0,0 +1,115 @@
 /**
  ******************************************************************************
  * @file           : USB_DEVICE  
  * @version        : v2.0_Cube
  * @brief          : This file implements the USB Device 
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usb_device.h"
 #include "usbd_core.h"
 #include "usbd_desc.h"
 #include "usbd_msc.h"
 #include "usbd_storage_if.h"
 #include "usbd_cdc.h"
 #include "usbd_cdc_if.h"
 #include "usbd_msc_cdc.h"
 /* USB Device Core handle declaration */
 USBD_HandleTypeDef hUsbDeviceFS;
 /* USER CODE BEGIN Includes */
 extern PCD_HandleTypeDef hpcd_USB_FS;
 /* USER CODE END Includes */
 /* init function */                                        
 void MX_USB_DEVICE_Init(void)
 {
  /* USER CODE BEGIN USB_DEVICE_Init_PreTreatment */
  /* USER CODE END USB_DEVICE_Init_PreTreatment */
  /* Init Device Library,Add Supported Class and Start the library*/
  USBD_Init(&hUsbDeviceFS, &FS_Desc, DEVICE_FS);
  USBD_RegisterClass(&hUsbDeviceFS, &USBD_MSC_CDC);
  USBD_MSC_RegisterStorage(&hUsbDeviceFS, &USBD_Storage_Interface_fops_FS);
  USBD_CDC_RegisterInterface(&hUsbDeviceFS, &USBD_Interface_fops_FS);
  USBD_Start(&hUsbDeviceFS);
  /* USER CODE BEGIN USB_DEVICE_Init_PostTreatment */
  /* USER CODE END USB_DEVICE_Init_PostTreatment */
 }
 /* USER CODE BEGIN USB_IRQ */
 /**
 * @brief This function handles USB low priority or CAN RX0 interrupts.
 */
 void USB_LP_CAN1_RX0_IRQHandler(void)
 {
    /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 0 */
    /* USER CODE END USB_LP_CAN1_RX0_IRQn 0 */
    HAL_PCD_IRQHandler(&hpcd_USB_FS);
    /* USER CODE BEGIN USB_LP_CAN1_RX0_IRQn 1 */
    /* USER CODE END USB_LP_CAN1_RX0_IRQn 1 */
 }
 /* USER CODE END USB_IRQ */
 /**
  * @}
  */
 /**
  * @}
  */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usb_device.h
+++ b/USB/usb_device.h
@ -0,0 +1,78 @@
 /**
  ******************************************************************************
  * @file           : USB_DEVICE
  * @version        : v2.0_Cube
  * @brief          : Header for usb_device file.
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Define to prevent recursive inclusion -------------------------------------*/
 #ifndef __usb_device_H
 #define __usb_device_H
 #ifdef __cplusplus
 extern "C" {
 #endif
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_def.h"
 extern USBD_HandleTypeDef hUsbDeviceFS;
 /* USB_Device init function */	
 void MX_USB_DEVICE_Init(void);
 #ifdef __cplusplus
 }
 #endif
 #endif /*__usb_device_H */
 /**
  * @}
  */
 /**
  * @}
  */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_cdc_if.c
+++ b/USB/usbd_cdc_if.c
@ -0,0 +1,326 @@
 /**
  ******************************************************************************
  * @file           : usbd_cdc_if.c
  * @brief          :
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_cdc_if.h"
 /* USER CODE BEGIN INCLUDE */
 #include "task_main.h"
 #include "TinyFrame.h"
 #include "comm/messages.h"
 extern osSemaphoreId semVcomTxReadyHandle;
 /* USER CODE END INCLUDE */
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
  * @{
  */
 /** @defgroup USBD_CDC 
  * @brief usbd core module
  * @{
  */
 /** @defgroup USBD_CDC_Private_TypesDefinitions
  * @{
  */
 /* USER CODE BEGIN PRIVATE_TYPES */
 /* USER CODE END PRIVATE_TYPES */
 /**
  * @}
  */
 /** @defgroup USBD_CDC_Private_Defines
  * @{
  */
 /* USER CODE BEGIN PRIVATE_DEFINES */
 /* Define size for the receive and transmit buffer over CDC */
 /* It's up to user to redefine and/or remove those define */
 #define APP_RX_DATA_SIZE  64
 #define APP_TX_DATA_SIZE  64
 /* USER CODE END PRIVATE_DEFINES */
 /**
  * @}
  */
 /** @defgroup USBD_CDC_Private_Macros
  * @{
  */
 /* USER CODE BEGIN PRIVATE_MACRO */
 /* USER CODE END PRIVATE_MACRO */
 /**
  * @}
  */
 /** @defgroup USBD_CDC_Private_Variables
  * @{
  */
 /* Create buffer for reception and transmission           */
 /* It's up to user to redefine and/or remove those define */
 /* Received Data over USB are stored in this buffer       */
 uint8_t UserRxBufferFS[APP_RX_DATA_SIZE];
 /* Send Data over USB CDC are stored in this buffer       */
 //uint8_t UserTxBufferFS[APP_TX_DATA_SIZE];
 /* USER CODE BEGIN PRIVATE_VARIABLES */
 /* USER CODE END PRIVATE_VARIABLES */
 /**
  * @}
  */
 /** @defgroup USBD_CDC_IF_Exported_Variables
  * @{
  */
  extern USBD_HandleTypeDef hUsbDeviceFS;
 /* USER CODE BEGIN EXPORTED_VARIABLES */
 /* USER CODE END EXPORTED_VARIABLES */
 /**
  * @}
  */
 /** @defgroup USBD_CDC_Private_FunctionPrototypes
  * @{
  */
 static int8_t CDC_Init_FS     (void);
 static int8_t CDC_DeInit_FS   (void);
 static int8_t CDC_Control_FS  (uint8_t cmd, uint8_t* pbuf, uint16_t length);
 static int8_t CDC_Receive_FS  (uint8_t *Buf, uint32_t *Len);
 /* USER CODE BEGIN PRIVATE_FUNCTIONS_DECLARATION */
 /* USER CODE END PRIVATE_FUNCTIONS_DECLARATION */
 /**
  * @}
  */
 USBD_CDC_ItfTypeDef USBD_Interface_fops_FS =
 {
  CDC_Init_FS,
  CDC_DeInit_FS,
  CDC_Control_FS,
  CDC_Receive_FS
 };
 /* Private functions ---------------------------------------------------------*/
 /**
  * @brief  CDC_Init_FS
  *         Initializes the CDC media low layer over the FS USB IP
  * @param  None
  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
  */
 static int8_t CDC_Init_FS(void)
 {
  /* USER CODE BEGIN 3 */
  /* Set Application Buffers */
  USBD_CDC_SetTxBuffer(&hUsbDeviceFS, NULL, 0);
  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, UserRxBufferFS);
  return (USBD_OK);
  /* USER CODE END 3 */
 }
 /**
  * @brief  CDC_DeInit_FS
  *         DeInitializes the CDC media low layer
  * @param  None
  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
  */
 static int8_t CDC_DeInit_FS(void)
 {
  /* USER CODE BEGIN 4 */
  return (USBD_OK);
  /* USER CODE END 4 */
 }
 /**
  * @brief  CDC_Control_FS
  *         Manage the CDC class requests
  * @param  cmd: Command code            
  * @param  pbuf: Buffer containing command data (request parameters)
  * @param  length: Number of data to be sent (in bytes)
  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
  */
 static int8_t CDC_Control_FS  (uint8_t cmd, uint8_t* pbuf, uint16_t length)
 {
  /* USER CODE BEGIN 5 */
  switch (cmd)
  {
  case CDC_SEND_ENCAPSULATED_COMMAND:
    break;
  case CDC_GET_ENCAPSULATED_RESPONSE:
    break;
  case CDC_SET_COMM_FEATURE:
    break;
  case CDC_GET_COMM_FEATURE:
    break;
  case CDC_CLEAR_COMM_FEATURE:
    break;
  /*******************************************************************************/
  /* Line Coding Structure                                                       */
  /*-----------------------------------------------------------------------------*/
  /* Offset | Field       | Size | Value  | Description                          */
  /* 0      | dwDTERate   |   4  | Number |Data terminal rate, in bits per second*/
  /* 4      | bCharFormat |   1  | Number | Stop bits                            */
  /*                                        0 - 1 Stop bit                       */
  /*                                        1 - 1.5 Stop bits                    */
  /*                                        2 - 2 Stop bits                      */
  /* 5      | bParityType |  1   | Number | Parity                               */
  /*                                        0 - None                             */
  /*                                        1 - Odd                              */
  /*                                        2 - Even                             */
  /*                                        3 - Mark                             */
  /*                                        4 - Space                            */
  /* 6      | bDataBits  |   1   | Number Data bits (5, 6, 7, 8 or 16).          */
  /*******************************************************************************/
  case CDC_SET_LINE_CODING:
    break;
  case CDC_GET_LINE_CODING:
    break;
  case CDC_SET_CONTROL_LINE_STATE:
    break;
  case CDC_SEND_BREAK:
    break;
  default:
    break;
  }
  return (USBD_OK);
  /* USER CODE END 5 */
 }
 /**
  * @brief  CDC_Receive_FS
  *         Data received over USB OUT endpoint are sent over CDC interface 
  *         through this function.
  *           
  *         @note
  *         This function will block any OUT packet reception on USB endpoint 
  *         untill exiting this function. If you exit this function before transfer
  *         is complete on CDC interface (ie. using DMA controller) it will result 
  *         in receiving more data while previous ones are still not sent.
  *                 
  * @param  Buf: Buffer of data to be received
  * @param  Len: Number of data received (in bytes)
  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL
  */
 static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
 {
  /* USER CODE BEGIN 6 */
 // this does nothing?!
 // the buffer was already assigned in the init function and we got it as argument here?!
 //  USBD_CDC_SetRxBuffer(&hUsbDeviceFS, &Buf[0]);
  assert_param(*Len <= APP_RX_DATA_SIZE);
  USBD_CDC_ReceivePacket(&hUsbDeviceFS);
  TF_Accept(comm, Buf, *Len);
  return (USBD_OK);
  /* USER CODE END 6 */
 }
 /**
  * @brief  CDC_Transmit_FS
  *         Data send over USB IN endpoint are sent over CDC interface 
  *         through this function.           
  *         @note
  *         
  *                 
  * @param  Buf: Buffer of data to be send - does not need to stay alive after this call
  * @param  Len: Number of data to be send (in bytes)
  * @retval Result of the operation: USBD_OK if all operations are OK else USBD_FAIL or USBD_BUSY
  */
 uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len)
 {
  uint8_t result = USBD_OK;
  /* USER CODE BEGIN 7 */
  USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*)hUsbDeviceFS.pClassData2;
  if (hcdc->TxState != 0){
    return USBD_BUSY;
  }
  USBD_CDC_SetTxBuffer(&hUsbDeviceFS, Buf, Len);
  result = USBD_CDC_TransmitPacket(&hUsbDeviceFS);
  /* USER CODE END 7 */
  return result;
 }
 /* USER CODE BEGIN PRIVATE_FUNCTIONS_IMPLEMENTATION */
 void USBD_CDC_TransmitDone(USBD_HandleTypeDef *pdev)
 {
  assert_param(xTaskGetCurrentTaskHandle() == tskMainHandle);
  // Notify the semaphore that we're ready to transmit more
  assert_param(semVcomTxReadyHandle != NULL);
  xSemaphoreGive(semVcomTxReadyHandle);
 }
 /* USER CODE END PRIVATE_FUNCTIONS_IMPLEMENTATION */
 /**
  * @}
  */
 /**
  * @}
  */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_cdc_if.h
+++ b/USB/usbd_cdc_if.h
@ -0,0 +1,144 @@
 /**
  ******************************************************************************
  * @file           : usbd_cdc_if.h
  * @brief          : Header for usbd_cdc_if file.
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Define to prevent recursive inclusion -------------------------------------*/
 #ifndef __USBD_CDC_IF_H
 #define __USBD_CDC_IF_H
 #ifdef __cplusplus
 extern "C" {
 #endif
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_cdc.h"
 /* USER CODE BEGIN INCLUDE */
 /* USER CODE END INCLUDE */
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
  * @{
  */
 /** @defgroup USBD_CDC_IF
  * @brief header 
  * @{
  */ 
 /** @defgroup USBD_CDC_IF_Exported_Defines
  * @{
  */ 
 /* USER CODE BEGIN EXPORTED_DEFINES */
 /* USER CODE END EXPORTED_DEFINES */
 /**
  * @}
  */ 
 /** @defgroup USBD_CDC_IF_Exported_Types
  * @{
  */  
 /* USER CODE BEGIN EXPORTED_TYPES */
 /* USER CODE END EXPORTED_TYPES */
 /**
  * @}
  */ 
 /** @defgroup USBD_CDC_IF_Exported_Macros
  * @{
  */ 
 /* USER CODE BEGIN EXPORTED_MACRO */
 /* USER CODE END EXPORTED_MACRO */
 /**
  * @}
  */ 
 /** @defgroup USBD_AUDIO_IF_Exported_Variables
  * @{
  */ 
 extern USBD_CDC_ItfTypeDef  USBD_Interface_fops_FS;
 /* USER CODE BEGIN EXPORTED_VARIABLES */
 /* USER CODE END EXPORTED_VARIABLES */
 /**
  * @}
  */ 
 /** @defgroup USBD_CDC_IF_Exported_FunctionsPrototype
  * @{
  */ 
 uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len);
 /* USER CODE BEGIN EXPORTED_FUNCTIONS */
 /**
 * Semaphore that's "given" whenever the Tx queue is ready
 */
 extern SemaphoreHandle_t semCDCTxReady;
 /* USER CODE END EXPORTED_FUNCTIONS */
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 #ifdef __cplusplus
 }
 #endif
 #endif /* __USBD_CDC_IF_H */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_conf.c
+++ b/USB/usbd_conf.c
@ -0,0 +1,542 @@
 /**
  ******************************************************************************
  * @file           : usbd_conf.c
  * @version        : v2.0_Cube
  * @brief          : This file implements the board support package for the USB device library
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_def.h"
 #include "usbd_core.h"
 #include "usbd_msc.h"
 #include "usbd_cdc.h"
 /* Private typedef -----------------------------------------------------------*/
 /* Private define ------------------------------------------------------------*/
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
 extern PCD_HandleTypeDef hpcd_USB_FS;
 /* USER CODE BEGIN 0 */
 /* USER CODE END 0 */
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
 /* USER CODE BEGIN 1 */
 /* USER CODE END 1 */
 void HAL_PCDEx_SetConnectionState(PCD_HandleTypeDef *hpcd, uint8_t state);
 /*******************************************************************************
                       LL Driver Callbacks (PCD -> USB Device Library)
 *******************************************************************************/
 /* MSP Init */
 void HAL_PCD_MspInit(PCD_HandleTypeDef* pcdHandle)
 {
  if(pcdHandle->Instance==USB)
  {
  /* USER CODE BEGIN USB_MspInit 0 */
  /* USER CODE END USB_MspInit 0 */
    /* Peripheral clock enable */
    __HAL_RCC_USB_CLK_ENABLE();
    /* Peripheral interrupt init */
    HAL_NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, 5, 0);
    HAL_NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
  /* USER CODE BEGIN USB_MspInit 1 */
  /* USER CODE END USB_MspInit 1 */
  }
 }
 void HAL_PCD_MspDeInit(PCD_HandleTypeDef* pcdHandle)
 {
  if(pcdHandle->Instance==USB)
  {
  /* USER CODE BEGIN USB_MspDeInit 0 */
  /* USER CODE END USB_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_USB_CLK_DISABLE();
    /* Peripheral interrupt Deinit*/
    HAL_NVIC_DisableIRQ(USB_LP_CAN1_RX0_IRQn);
  /* USER CODE BEGIN USB_MspDeInit 1 */
  /* USER CODE END USB_MspDeInit 1 */
  }
 }
 /**
  * @brief  Setup Stage callback
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd)
 {
  USBD_LL_SetupStage((USBD_HandleTypeDef*)hpcd->pData, (uint8_t *)hpcd->Setup);
 }
 /**
  * @brief  Data Out Stage callback.
  * @param  hpcd: PCD handle
  * @param  epnum: Endpoint Number
  * @retval None
  */
 void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
 {
  USBD_LL_DataOutStage((USBD_HandleTypeDef*)hpcd->pData, epnum, hpcd->OUT_ep[epnum].xfer_buff);
 }
 /**
  * @brief  Data In Stage callback..
  * @param  hpcd: PCD handle
  * @param  epnum: Endpoint Number
  * @retval None
  */
 void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
 {
  USBD_LL_DataInStage((USBD_HandleTypeDef*)hpcd->pData, epnum, hpcd->IN_ep[epnum].xfer_buff);
 }
 /**
  * @brief  SOF callback.
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_SOFCallback(PCD_HandleTypeDef *hpcd)
 {
  USBD_LL_SOF((USBD_HandleTypeDef*)hpcd->pData);
 }
 /**
  * @brief  Reset callback.
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
 { 
 //  USBD_SpeedTypeDef speed = USBD_SPEED_FULL;
  /*Set USB Current Speed*/
 //  switch (hpcd->Init.speed)
 //  {
 //  case PCD_SPEED_FULL:
 //    speed = USBD_SPEED_FULL;
 //    break;
 //
 //  default:
 //    speed = USBD_SPEED_FULL;
 //    break;
 //  }
  USBD_LL_SetSpeed((USBD_HandleTypeDef*)hpcd->pData, USBD_SPEED_FULL);
  /*Reset Device*/
  USBD_LL_Reset((USBD_HandleTypeDef*)hpcd->pData);
 }
 /**
  * @brief  Suspend callback.
  * When Low power mode is enabled the debug cannot be used (IAR, Keil doesn't support it)
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd)
 {
  /* Inform USB library that core enters in suspend Mode */
  USBD_LL_Suspend((USBD_HandleTypeDef*)hpcd->pData);
  /*Enter in STOP mode */
  /* USER CODE BEGIN 2 */  
  if (hpcd->Init.low_power_enable)
  {
    /* Set SLEEPDEEP bit and SleepOnExit of Cortex System Control Register */
    SCB->SCR |= (uint32_t)((uint32_t)(SCB_SCR_SLEEPDEEP_Msk | SCB_SCR_SLEEPONEXIT_Msk));
  }
  /* USER CODE END 2 */
 }
 /**
  * @brief  Resume callback.
  * When Low power mode is enabled the debug cannot be used (IAR, Keil doesn't support it)
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd)
 {
  /* USER CODE BEGIN 3 */
  /* USER CODE END 3 */
  USBD_LL_Resume((USBD_HandleTypeDef*)hpcd->pData);
 }
 /**
  * @brief  ISOOUTIncomplete callback.
  * @param  hpcd: PCD handle
  * @param  epnum: Endpoint Number
  * @retval None
  */
 void HAL_PCD_ISOOUTIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
 {
  USBD_LL_IsoOUTIncomplete((USBD_HandleTypeDef*)hpcd->pData, epnum);
 }
 /**
  * @brief  ISOINIncomplete callback.
  * @param  hpcd: PCD handle
  * @param  epnum: Endpoint Number
  * @retval None
  */
 void HAL_PCD_ISOINIncompleteCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
 {
  USBD_LL_IsoINIncomplete((USBD_HandleTypeDef*)hpcd->pData, epnum);
 }
 /**
  * @brief  ConnectCallback callback.
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd)
 {
  USBD_LL_DevConnected((USBD_HandleTypeDef*)hpcd->pData);
 }
 /**
  * @brief  Disconnect callback.
  * @param  hpcd: PCD handle
  * @retval None
  */
 void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd)
 {
  USBD_LL_DevDisconnected((USBD_HandleTypeDef*)hpcd->pData);
 }
 /*******************************************************************************
                       LL Driver Interface (USB Device Library --> PCD)
 *******************************************************************************/
 /**
  * @brief  Initializes the Low Level portion of the Device driver.
  * @param  pdev: Device handle
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_Init (USBD_HandleTypeDef *pdev)
 { 
  /* Init USB_IP */
  /* Link The driver to the stack */
  hpcd_USB_FS.pData = pdev;
  pdev->pData = &hpcd_USB_FS;
  hpcd_USB_FS.Instance = USB;
  hpcd_USB_FS.Init.dev_endpoints = 8;
  hpcd_USB_FS.Init.speed = PCD_SPEED_FULL;
  hpcd_USB_FS.Init.ep0_mps = DEP0CTL_MPS_8;
  hpcd_USB_FS.Init.low_power_enable = DISABLE;
  hpcd_USB_FS.Init.lpm_enable = DISABLE;
  hpcd_USB_FS.Init.battery_charging_enable = DISABLE;
  if (HAL_PCD_Init(&hpcd_USB_FS) != HAL_OK)
  {
      _Error_Handler(__FILE__, __LINE__);
  }
  uint32_t ptr = 0;
  const int TOTAL_EPS = (USBD_NUM_ENDPOINTS*2+1);
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x00 , PCD_SNG_BUF, ptr = TOTAL_EPS*8); // 64
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , 0x80 , PCD_SNG_BUF, ptr += 64); // 64
  // MSC endpoints - EP1, two-way
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , MSC_EPIN_ADDR , PCD_SNG_BUF, ptr += 64); // 64
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , MSC_EPOUT_ADDR , PCD_SNG_BUF, ptr += 64); // 64
  // CDC endpoints, EP2 two-way and EP3 in-only
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , CDC_IN_EP , PCD_SNG_BUF, ptr += 64); // 64
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , CDC_OUT_EP , PCD_SNG_BUF, ptr += 64); // 64
  HAL_PCDEx_PMAConfig((PCD_HandleTypeDef*)pdev->pData , CDC_CMD_EP , PCD_SNG_BUF, ptr += 16); // 16
  (void)ptr;
  return USBD_OK;
 }
 #define HAL_TO_USB_STATUS(hal) ((hal)==HAL_OK?USBD_OK:(hal)==HAL_BUSY?USBD_BUSY:USBD_FAIL)
 /**
  * @brief  De-Initializes the Low Level portion of the Device driver.
  * @param  pdev: Device handle
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_DeInit (USBD_HandleTypeDef *pdev)
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_DeInit(pdev->pData);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Starts the Low Level portion of the Device driver. 
  * @param  pdev: Device handle
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_Start(USBD_HandleTypeDef *pdev)
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_Start(pdev->pData);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Stops the Low Level portion of the Device driver.
  * @param  pdev: Device handle
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_Stop (USBD_HandleTypeDef *pdev)
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_Stop(pdev->pData);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Opens an endpoint of the Low Level Driver.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @param  ep_type: Endpoint Type
  * @param  ep_mps: Endpoint Max Packet Size
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_OpenEP  (USBD_HandleTypeDef *pdev, 
                                      uint8_t  ep_addr,                                      
                                      uint8_t  ep_type,
                                      uint16_t ep_mps)
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_Open(pdev->pData,
                               ep_addr, 
                               ep_mps, 
                               ep_type);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Closes an endpoint of the Low Level Driver.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_CloseEP (USBD_HandleTypeDef *pdev, uint8_t ep_addr)   
 {
  //trap("CloseEP noimpl"); // XXX This uses ~ 500 bytes, maybe could be disabled
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_Close(pdev->pData, ep_addr);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Flushes an endpoint of the Low Level Driver.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_FlushEP (USBD_HandleTypeDef *pdev, uint8_t ep_addr)   
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_Flush(pdev->pData, ep_addr);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Sets a Stall condition on an endpoint of the Low Level Driver.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_StallEP (USBD_HandleTypeDef *pdev, uint8_t ep_addr)   
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_SetStall(pdev->pData, ep_addr);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Clears a Stall condition on an endpoint of the Low Level Driver.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_ClearStallEP (USBD_HandleTypeDef *pdev, uint8_t ep_addr)   
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_ClrStall(pdev->pData, ep_addr);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Returns Stall condition.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval Stall (1: Yes, 0: No)
  */
 uint8_t USBD_LL_IsStallEP (USBD_HandleTypeDef *pdev, uint8_t ep_addr)   
 {
  PCD_HandleTypeDef *hpcd = (PCD_HandleTypeDef*) pdev->pData;
  if((ep_addr & 0x80) == 0x80)
  {
    return hpcd->IN_ep[ep_addr & 0x7F].is_stall; 
  }
  else
  {
    return hpcd->OUT_ep[ep_addr & 0x7F].is_stall; 
  }
 }
 /**
  * @brief  Assigns a USB address to the device.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_SetUSBAddress (USBD_HandleTypeDef *pdev, uint8_t dev_addr)   
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_SetAddress(pdev->pData, dev_addr);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Transmits data over an endpoint.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @param  pbuf: Pointer to data to be sent
  * @param  size: Data size    
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_Transmit (USBD_HandleTypeDef *pdev, 
                                      uint8_t  ep_addr,                                      
                                      uint8_t  *pbuf,
                                      uint16_t  size)
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_Transmit(pdev->pData, ep_addr, pbuf, size);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Prepares an endpoint for reception.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @param  pbuf: Pointer to data to be received
  * @param  size: Data size
  * @retval USBD Status
  */
 USBD_StatusTypeDef  USBD_LL_PrepareReceive(USBD_HandleTypeDef *pdev, 
                                           uint8_t  ep_addr,                                      
                                           uint8_t  *pbuf,
                                           uint16_t  size)
 {
  HAL_StatusTypeDef hal_status = HAL_PCD_EP_Receive(pdev->pData, ep_addr, pbuf, size);
  return HAL_TO_USB_STATUS(hal_status);
 }
 /**
  * @brief  Returns the last transfered packet size.
  * @param  pdev: Device handle
  * @param  ep_addr: Endpoint Number
  * @retval Recived Data Size
  */
 uint32_t USBD_LL_GetRxDataSize  (USBD_HandleTypeDef *pdev, uint8_t  ep_addr)  
 {
  return HAL_PCD_EP_GetRxCount((PCD_HandleTypeDef*) pdev->pData, ep_addr);
 }
 /**
  * @brief  Delays routine for the USB Device Library.
  * @param  Delay: Delay in ms
  * @retval None
  */
 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
  * @retval None
  */
 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];
 }
 /**
  * @brief  Dummy memory free
  * @param  *p pointer to allocated  memory address
  * @retval None
  */
 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;
 }
 /**
 * @brief Software Device Connection
 * @param hpcd: PCD handle
 * @param state: connection state (0 : disconnected / 1: connected) 
 * @retval None
 */
 void HAL_PCDEx_SetConnectionState(PCD_HandleTypeDef *hpcd, uint8_t state)
 {
 ///* USER CODE BEGIN 5 */
 //  if (state == 1)
 //  {
 //    /* Configure Low Connection State */
 //
 //  }
 //  else
 //  {
 //    /* Configure High Connection State */
 //
 //  }
 ///* USER CODE END 5 */
 }
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_conf.h
+++ b/USB/usbd_conf.h
@ -0,0 +1,201 @@
 /**
  ******************************************************************************
  * @file           : usbd_conf.h
  * @version        : v2.0_Cube
  * @brief          : Header for usbd_conf file.
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Define to prevent recursive inclusion -------------------------------------*/
 #ifndef __USBD_CONF__H__
 #define __USBD_CONF__H__
 #ifdef __cplusplus
 extern "C" {
 #endif
 /* Includes ------------------------------------------------------------------*/
 //#include <stdio.h>
 //#include <stdlib.h>
 #include "platform.h"
 #include "usbd_def.h"
 /** @addtogroup USBD_OTG_DRIVER
  * @{
  */
 /** @defgroup USBD_CONF
  * @brief usb otg low level driver configuration file
  * @{
  */ 
 /** @defgroup USBD_CONF_Exported_Defines
  * @{
  */ 
 /*---------- -----------*/
 #define USBD_MAX_NUM_INTERFACES     2
 /*---------- -----------*/
 #define USBD_MAX_NUM_CONFIGURATION     1
 /*---------- -----------*/
 #define USBD_MAX_STR_DESC_SIZ     128
 /*---------- -----------*/
 #define USBD_SUPPORT_USER_STRING     1
 /*---------- -----------*/
 #define USBD_DEBUG_LEVEL     3
 /*---------- -----------*/
 #define USBD_SELF_POWERED     1
 /*---------- -----------*/
 #define MSC_MEDIA_PACKET     512
 /****************************************/
 /* #define for FS and HS identification */
 #define DEVICE_FS 		0
 // Custom config
 #define MSC_COMPOSITE
 #define CDC_COMPOSITE
 #define MSC_CUSTOM_EPS
 #define MSC_EPIN_ADDR                0x81
 #define MSC_EPOUT_ADDR               0x01
 #define CDC_CUSTOM_EPS
 #define CDC_IN_EP                    0x82  /* EP1 for data IN */
 #define CDC_OUT_EP                   0x02  /* EP1 for data OUT */
 #define CDC_CMD_EP                   0x83  /* EP2 for CDC commands */
 /** @defgroup USBD_Exported_Macros
  * @{
  */ 
 /* Memory management macros */  
 #define USBD_malloc               (uint32_t *)USBD_static_malloc
 #define USBD_free                 USBD_static_free
 #define USBD_memset               /* Not used */
 #define USBD_memcpy               /* Not used */
 #define USBD_Delay   HAL_Delay
 /* For footprint reasons and since only one allocation is handled in the HID class
   driver, the malloc/free is changed into a static allocation method */
 void *USBD_static_malloc(uint32_t size);
 void USBD_static_free(void *p);
 #if (USBD_DEBUG_LEVEL > 0)
 #define  USBD_UsrLog(...)   PRINTF("USB USER : ") ;\
                            PRINTF(__VA_ARGS__);\
                            PRINTF("\r\n");
 #else
 #define USBD_UsrLog(...)
 #endif
 #if (USBD_DEBUG_LEVEL > 1)
 #define  USBD_ErrLog(...)   PRINTF("USB ERROR: ") ;\
                            PRINTF(__VA_ARGS__);\
                            PRINTF("\r\n");
 #else
 #define USBD_ErrLog(...)
 #endif
 #if (USBD_DEBUG_LEVEL > 2)
 #define  USBD_DbgLog(...)   PRINTF("USB DEBUG: ") ;\
                            PRINTF(__VA_ARGS__);\
                            PRINTF("\r\n");
 #else
 #define USBD_DbgLog(...)
 #endif
 /**
  * @}
  */
 /**
  * @}
  */ 
 /** @defgroup USBD_CONF_Exported_Types
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_CONF_Exported_Macros
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_CONF_Exported_Variables
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_CONF_Exported_FunctionsPrototype
  * @{
  */ 
 /**
  * @}
  */ 
 #ifdef __cplusplus
 }
 #endif
 #endif /*__USBD_CONF__H__*/
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_desc.c
+++ b/USB/usbd_desc.c
@ -0,0 +1,297 @@
 /**
  ******************************************************************************
  * @file           : usbd_desc.c
  * @version        : v2.0_Cube
  * @brief          : This file implements the USB Device descriptors
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_core.h"
 #include "usbd_desc.h"
 #include "usbd_conf.h"
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
  * @{
  */
 /** @defgroup USBD_DESC 
  * @brief USBD descriptors module
  * @{
  */ 
 /** @defgroup USBD_DESC_Private_TypesDefinitions
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Private_Defines
  * @{
  */ 
 #define USBD_VID              1155
 #define USBD_PID_FS          22314
 #define USBD_LANGID_NUM       1033
 #define USBD_MANUFACTURER_STRING     "MightyPork"
 #define USBD_PRODUCT_STRING_FS       "GEX"
 #define USBD_VERSION_NUM             0x0001 // 0xMMmp
 //#define USBD_SERIALNUMBER_STRING_FS  "00000000001A"
 //#define USBD_CONFIGURATION_STRING_FS "MSC Config"
 //#define USBD_INTERFACE_STRING_FS     "MSC Interface"
 /* USER CODE BEGIN 0 */
 /* USER CODE END 0*/
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Private_Macros
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Private_Variables
  * @{
  */ 
 uint8_t *     USBD_FS_DeviceDescriptor( USBD_SpeedTypeDef speed , uint16_t *length);
 uint8_t *     USBD_FS_LangIDStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length);
 uint8_t *     USBD_FS_ManufacturerStrDescriptor ( USBD_SpeedTypeDef speed , uint16_t *length);
 uint8_t *     USBD_FS_ProductStrDescriptor ( USBD_SpeedTypeDef speed , uint16_t *length);
 uint8_t *     USBD_FS_SerialStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length);
 //uint8_t *     USBD_FS_ConfigStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length);
 //uint8_t *     USBD_FS_InterfaceStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length);
 #ifdef USB_SUPPORT_USER_STRING_DESC
 uint8_t *     USBD_FS_USRStringDesc (USBD_SpeedTypeDef speed, uint8_t idx , uint16_t *length);  
 #endif /* USB_SUPPORT_USER_STRING_DESC */  
 USBD_DescriptorsTypeDef FS_Desc =
 {
  USBD_FS_DeviceDescriptor,
  USBD_FS_LangIDStrDescriptor, 
  USBD_FS_ManufacturerStrDescriptor,
  USBD_FS_ProductStrDescriptor,
  USBD_FS_SerialStrDescriptor,
    NULL, NULL,
 //  USBD_FS_ConfigStrDescriptor,
 //  USBD_FS_InterfaceStrDescriptor,
 };
 #if defined ( __ICCARM__ ) /*!< IAR Compiler */
  #pragma data_alignment=4   
 #endif
 /* USB Standard Device Descriptor */
 __ALIGN_BEGIN uint8_t USBD_FS_DeviceDesc[USB_LEN_DEV_DESC] __ALIGN_END =
  {
    USB_LEN_DEV_DESC,           /*bLength */
    USB_DESC_TYPE_DEVICE,       /*bDescriptorType*/
    0x00,                       /* bcdUSB 2.0 */
    0x02,
    // magic triplet to allow use of the association descriptor
    0xEF,                       /*bDeviceClass*/
    0x02,                       /*bDeviceSubClass*/
    0x01,                       /*bDeviceProtocol*/
    USB_MAX_EP0_SIZE,           /*bMaxPacketSize*/
    LOBYTE(USBD_VID),           /*idVendor*/
    HIBYTE(USBD_VID),           /*idVendor*/
    LOBYTE(USBD_PID_FS),        /*idVendor*/
    HIBYTE(USBD_PID_FS),        /*idVendor*/
    LOBYTE(USBD_VERSION_NUM),   /*bcdDevice rel. 2.00*/
    HIBYTE(USBD_VERSION_NUM),
    USBD_IDX_MFC_STR,           /*Index of manufacturer  string*/
    USBD_IDX_PRODUCT_STR,       /*Index of product string*/
    USBD_IDX_SERIAL_STR,        /*Index of serial number string*/
    USBD_MAX_NUM_CONFIGURATION  /*bNumConfigurations*/
  } ; 
 /* USB_DeviceDescriptor */
 #if defined ( __ICCARM__ ) /*!< IAR Compiler */
  #pragma data_alignment=4   
 #endif
 /* USB Standard Device Descriptor */
 __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_LEN_LANGID_STR_DESC] __ALIGN_END =
 {
     USB_LEN_LANGID_STR_DESC,         
     USB_DESC_TYPE_STRING,       
     LOBYTE(USBD_LANGID_NUM),
     HIBYTE(USBD_LANGID_NUM),
 };
 #if defined ( __ICCARM__ ) /*!< IAR Compiler */
  #pragma data_alignment=4   
 #endif
 __ALIGN_BEGIN uint8_t USBD_StrDesc[USBD_MAX_STR_DESC_SIZ] __ALIGN_END;
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Private_FunctionPrototypes
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Private_Functions
  * @{
  */ 
 /**
 * @brief  USBD_FS_DeviceDescriptor 
 *         return the device descriptor
 * @param  speed : current device speed
 * @param  length : pointer to data length variable
 * @retval pointer to descriptor buffer
 */
 uint8_t *  USBD_FS_DeviceDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 {
  *length = sizeof(USBD_FS_DeviceDesc);
  return USBD_FS_DeviceDesc;
 }
 /**
 * @brief  USBD_FS_LangIDStrDescriptor 
 *         return the LangID string descriptor
 * @param  speed : current device speed
 * @param  length : pointer to data length variable
 * @retval pointer to descriptor buffer
 */
 uint8_t *  USBD_FS_LangIDStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 {
  *length =  sizeof(USBD_LangIDDesc);  
  return USBD_LangIDDesc;
 }
 /**
 * @brief  USBD_FS_ProductStrDescriptor 
 *         return the product string descriptor
 * @param  speed : current device speed
 * @param  length : pointer to data length variable
 * @retval pointer to descriptor buffer
 */
 uint8_t *  USBD_FS_ProductStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 {
  USBD_GetString (USBD_PRODUCT_STRING_FS, USBD_StrDesc, length);
  return USBD_StrDesc;
 }
 /**
 * @brief  USBD_FS_ManufacturerStrDescriptor 
 *         return the manufacturer string descriptor
 * @param  speed : current device speed
 * @param  length : pointer to data length variable
 * @retval pointer to descriptor buffer
 */
 uint8_t *  USBD_FS_ManufacturerStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 {
  USBD_GetString (USBD_MANUFACTURER_STRING, USBD_StrDesc, length);
  return USBD_StrDesc;
 }
 /**
 * @brief  USBD_FS_SerialStrDescriptor 
 *         return the serial number string descriptor
 * @param  speed : current device speed
 * @param  length : pointer to data length variable
 * @retval pointer to descriptor buffer
 */
 uint8_t *  USBD_FS_SerialStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 {
  char buff[25];
  fixup_sprintf(buff, "%08"PRIX32"%08"PRIX32"%08"PRIX32,
          LL_GetUID_Word0(),
          LL_GetUID_Word1(),
          LL_GetUID_Word2()
  );
  USBD_GetString ((uint8_t *) &buff[0], USBD_StrDesc, length);
  return USBD_StrDesc;
 }
 //
 ///**
 //* @brief  USBD_FS_ConfigStrDescriptor
 //*         return the configuration string descriptor
 //* @param  speed : current device speed
 //* @param  length : pointer to data length variable
 //* @retval pointer to descriptor buffer
 //*/
 //uint8_t *  USBD_FS_ConfigStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 //{
 //  USBD_GetString (USBD_CONFIGURATION_STRING_FS, USBD_StrDesc, length);
 //  return USBD_StrDesc;
 //}
 //
 ///**
 //* @brief  USBD_HS_InterfaceStrDescriptor
 //*         return the interface string descriptor
 //* @param  speed : current device speed
 //* @param  length : pointer to data length variable
 //* @retval pointer to descriptor buffer
 //*/
 //uint8_t *  USBD_FS_InterfaceStrDescriptor( USBD_SpeedTypeDef speed , uint16_t *length)
 //{
 //  USBD_GetString (USBD_INTERFACE_STRING_FS, USBD_StrDesc, length);
 //  return USBD_StrDesc;
 //}
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_desc.h
+++ b/USB/usbd_desc.h
@ -0,0 +1,123 @@
 /**
  ******************************************************************************
  * @file           : usbd_desc.h
  * @version        : v2.0_Cube
  * @brief          : Header for usbd_desc file.
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Define to prevent recursive inclusion -------------------------------------*/
 #ifndef __USBD_DESC__H__
 #define __USBD_DESC__H__
 #ifdef __cplusplus
 extern "C" {
 #endif
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_def.h"
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
  * @{
  */
 /** @defgroup USB_DESC
  * @brief general defines for the usb device library file
  * @{
  */ 
 /** @defgroup USB_DESC_Exported_Defines
  * @{
  */
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Exported_TypesDefinitions
  * @{
  */
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Exported_Macros
  * @{
  */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Exported_Variables
  * @{
  */ 
 extern USBD_DescriptorsTypeDef FS_Desc;
 // Buffer for string descriptors
 extern __ALIGN_BEGIN uint8_t USBD_StrDesc[USBD_MAX_STR_DESC_SIZ] __ALIGN_END;
 /**
  * @}
  */ 
 /** @defgroup USBD_DESC_Exported_FunctionsPrototype
  * @{
  */ 
 /**
  * @}
  */ 
 #ifdef __cplusplus
 }
 #endif
 #endif /* __USBD_DESC_H */
 /**
  * @}
  */ 
 /**
 * @}
 */ 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_storage_if.c
+++ b/USB/usbd_storage_if.c
@ -0,0 +1,319 @@
 /**
  ******************************************************************************
  * @file           : usbd_storage_if.c
  * @brief          : Memory management layer
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Includes ------------------------------------------------------------------*/
 #include <platform/status_led.h>
 #include "platform.h"
 #include "usbd_storage_if.h"
 /* USER CODE BEGIN INCLUDE */
 #include "vfs/vfs_manager.h"
 /* USER CODE END INCLUDE */
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
  * @{
  */
 /** @defgroup USBD_STORAGE 
  * @brief usbd core module
  * @{
  */ 
 /** @defgroup USBD_STORAGE_Private_TypesDefinitions
  * @{
  */ 
 /* USER CODE BEGIN PRIVATE_TYPES */
 /* USER CODE END PRIVATE_TYPES */ 
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Private_Defines
  * @{
  */ 
 #define STORAGE_LUN_NBR                  1  
 #define STORAGE_BLK_NBR                  0x10000  
 #define STORAGE_BLK_SIZ                  0x200
 /* USER CODE BEGIN PRIVATE_DEFINES */
 /* USER CODE END PRIVATE_DEFINES */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Private_Macros
  * @{
  */ 
 /* USER CODE BEGIN PRIVATE_MACRO */
 /* USER CODE END PRIVATE_MACRO */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_IF_Private_Variables
  * @{
  */
 /* USER CODE BEGIN INQUIRY_DATA_FS */ 
 /* USB Mass storage Standard Inquiry Data */
 int8_t  STORAGE_Inquirydata_FS[] = {/* 36 */
  /* LUN 0 */
  0x00,		
  0x80,		
  0x02,		
  0x02,
  (STANDARD_INQUIRY_DATA_LEN - 5),
  0x00,
  0x00,	
  0x00,
    // Those strings are visible in dmesg, probably nowhere else.
  'M', 'E', 'G', 'A', 'P', 'I', 'G', ' ', /* Manufacturer : 8 bytes */
  'G', 'E', 'X', ' ', 'G', 'P', 'I', 'O', /* Product      : 16 Bytes */
  '-', 'E', 'x', 'p', 'a', 'n', 'd', 'r',
  '0', '.', '0' ,'1',                     /* Version      : 4 Bytes */
 }; 
 /* USER CODE END INQUIRY_DATA_FS */ 
 /* USER CODE BEGIN PRIVATE_VARIABLES */
 /* USER CODE END PRIVATE_VARIABLES */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_IF_Exported_Variables
  * @{
  */ 
  extern USBD_HandleTypeDef hUsbDeviceFS;
 /* USER CODE BEGIN EXPORTED_VARIABLES */
 /* USER CODE END EXPORTED_VARIABLES */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Private_FunctionPrototypes
  * @{
  */
 static int8_t STORAGE_Init_FS (uint8_t lun);
 static int8_t STORAGE_GetCapacity_FS (uint8_t lun, 
                           uint32_t *block_num, 
                           uint16_t *block_size);
 static int8_t  STORAGE_IsReady_FS (uint8_t lun);
 static int8_t  STORAGE_IsWriteProtected_FS (uint8_t lun);
 static int8_t STORAGE_Read_FS (uint8_t lun, 
                        uint8_t *buf, 
                        uint32_t blk_addr,
                        uint16_t blk_len);
 static int8_t STORAGE_Write_FS (uint8_t lun, 
                        uint8_t *buf, 
                        uint32_t blk_addr,
                        uint16_t blk_len);
 static int8_t STORAGE_GetMaxLun_FS (void);
 /* USER CODE BEGIN PRIVATE_FUNCTIONS_DECLARATION */
 /* USER CODE END PRIVATE_FUNCTIONS_DECLARATION */
 /**
  * @}
  */ 
 USBD_StorageTypeDef USBD_Storage_Interface_fops_FS =
 {
  STORAGE_Init_FS,
  STORAGE_GetCapacity_FS,
  STORAGE_IsReady_FS,
  STORAGE_IsWriteProtected_FS,
  STORAGE_Read_FS,
  STORAGE_Write_FS,
  STORAGE_GetMaxLun_FS,
  (int8_t *)STORAGE_Inquirydata_FS,
 };
 /* Private functions ---------------------------------------------------------*/
 /*******************************************************************************
 * Function Name  : STORAGE_Init_FS
 * Description    : 
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t STORAGE_Init_FS (uint8_t lun)
 {
  /* USER CODE BEGIN 2 */
    dbg("Plug In");
  vfs_mngr_fs_enable(1);
  return (USBD_OK);
  /* USER CODE END 2 */ 
 }
 /*******************************************************************************
 * Function Name  : STORAGE_GetCapacity_FS
 * Description    : 
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t STORAGE_GetCapacity_FS (uint8_t lun, uint32_t *block_num, uint16_t *block_size)
 {
  /* USER CODE BEGIN 3 */   
 //  *block_num  = STORAGE_BLK_NBR;
 //  *block_size = STORAGE_BLK_SIZ;
  // we have only one LUN
  *block_num = vfs_info.BlockCount;
  *block_size = vfs_info.BlockSize;
  vfs_printf("Get Capacity: %"PRIu32" sectors x %"PRIu16" bytes", *block_num, *block_size);
  return (USBD_OK);
  /* USER CODE END 3 */ 
 }
 /*******************************************************************************
 * Function Name  : STORAGE_IsReady_FS
 * Description    : 
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t  STORAGE_IsReady_FS (uint8_t lun)
 {
  /* USER CODE BEGIN 4 */
 //    dbg("STORAGE_IsReady_FS? %d", vfs_info.MediaReady);
    StatusLed_Set(STATUS_DISK_ATTACHED, vfs_info.MediaReady);
    // Media change - no re-plug
    if (vfs_info.MediaChanged) {
        vfs_info.MediaChanged = false; // unset the flag
        return -1; // Notify about media change
    }
    // Plug out/in
    return vfs_info.MediaReady ? USBD_OK : USBD_BUSY;
 //  return (USBD_OK);
  /* USER CODE END 4 */ 
 }
 /*******************************************************************************
 * Function Name  : STORAGE_IsWriteProtected_FS
 * Description    : 
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t  STORAGE_IsWriteProtected_FS (uint8_t lun)
 {
  /* USER CODE BEGIN 5 */
 //    dbg("STORAGE_IsWriteProtected_FS? no");
  return false;
 //  return (USBD_OK);
  /* USER CODE END 5 */ 
 }
 /*******************************************************************************
 * Function Name  : STORAGE_Read_FS
 * Description    : 
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t STORAGE_Read_FS (uint8_t lun, 
                        uint8_t *buf, 
                        uint32_t blk_addr,                       
                        uint16_t blk_len)
 {
  /* USER CODE BEGIN 6 */
 //  dbg("rd lun %d adr %d len %d", lun, blk_addr, blk_len);
  vfs_if_usbd_msc_read_sect(blk_addr, buf, blk_len); // ????
  return (USBD_OK);
  /* USER CODE END 6 */ 
 }
 /*******************************************************************************
 * Function Name  : STORAGE_Write_FS
 * Description    :
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t STORAGE_Write_FS (uint8_t lun, 
                         uint8_t *buf, 
                         uint32_t blk_addr,
                         uint16_t blk_len)
 {
  /* USER CODE BEGIN 7 */
 //  dbg("STORAGE_Write_FS");
  vfs_if_usbd_msc_write_sect(blk_addr, buf, blk_len); // ???
  return (USBD_OK);
  /* USER CODE END 7 */ 
 }
 /*******************************************************************************
 * Function Name  : STORAGE_GetMaxLun_FS
 * Description    : 
 * Input          : None.
 * Output         : None.
 * Return         : None.
 *******************************************************************************/
 int8_t STORAGE_GetMaxLun_FS (void)
 {
  /* USER CODE BEGIN 8 */ 
  return 0; // we have 1 LUN
 //  return (STORAGE_LUN_NBR - 1);
  /* USER CODE END 8 */ 
 }
 /* USER CODE BEGIN PRIVATE_FUNCTIONS_IMPLEMENTATION */
 /* USER CODE END PRIVATE_FUNCTIONS_IMPLEMENTATION */
 /**
  * @}
  */ 
 /**
  * @}
  */  
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/USB/usbd_storage_if.h
+++ b/USB/usbd_storage_if.h
@ -0,0 +1,138 @@
 /**
  ******************************************************************************
  * @file           : usbd_storage_if.h
  * @brief          : header file for the usbd_storage_if.c file
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2017 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
 */
 /* Define to prevent recursive inclusion -------------------------------------*/
 #ifndef __USBD_STORAGE_IF_H_
 #define __USBD_STORAGE_IF_H_
 #ifdef __cplusplus
 extern "C" {
 #endif
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include "usbd_msc.h"
 /* USER CODE BEGIN INCLUDE */
 /* USER CODE END INCLUDE */
 /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
  * @{
  */
 /** @defgroup USBD_STORAGE
  * @brief header file for the USBD_STORAGE.c file
  * @{
  */ 
 /** @defgroup USBD_STORAGE_Exported_Defines
  * @{
  */ 
 /* USER CODE BEGIN EXPORTED_DEFINES */
 /* USER CODE END  EXPORTED_DEFINES */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Exported_Types
  * @{
  */  
 /* USER CODE BEGIN EXPORTED_TYPES */
 /* USER CODE END  EXPORTED_TYPES */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Exported_Macros
  * @{
  */ 
 /* USER CODE BEGIN EXPORTED_MACRO */
 /* USER CODE END  EXPORTED_MACRO */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Exported_Variables
  * @{
  */ 
  extern USBD_StorageTypeDef  USBD_Storage_Interface_fops_FS;
 /* USER CODE BEGIN EXPORTED_VARIABLES */
 /* USER CODE END  EXPORTED_VARIABLES */
 /**
  * @}
  */ 
 /** @defgroup USBD_STORAGE_Exported_FunctionsPrototype
  * @{
  */ 
 /* USER CODE BEGIN EXPORTED_FUNCTIONS */
 /* USER CODE END  EXPORTED_FUNCTIONS */
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 /**
  * @}
  */ 
 #ifdef __cplusplus
 }
 #endif
 #endif /* __USBD_STORAGE_IF_H */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/comm/messages.c
+++ b/comm/messages.c
@ -0,0 +1,218 @@
 //
 // Created by MightyPork on 2017/11/21.
 //
 #include "platform.h"
 #include "TinyFrame.h"
 #include "framework/unit_registry.h"
 #include "comm/messages.h"
 #include "task_sched.h"
 TinyFrame tf_;
 TinyFrame *comm = &tf_;
 // ---------------------------------------------------------------------------
 // Pre-declaring local functions
 static TF_Result lst_ping(TinyFrame *tf, TF_Msg *msg);
 static TF_Result lst_unit(TinyFrame *tf, TF_Msg *msg);
 static TF_Result lst_list_units(TinyFrame *tf, TF_Msg *msg);
 static TF_Result lst_default(TinyFrame *tf, TF_Msg *msg);
 void comm_init(void)
 {
    TF_InitStatic(comm, TF_SLAVE);
    TF_AddTypeListener(comm, MSG_PING, lst_ping);
    TF_AddTypeListener(comm, MSG_UNIT_REQUEST, lst_unit);
    TF_AddTypeListener(comm, MSG_LIST_UNITS, lst_list_units);
    // fall-through
    TF_AddGenericListener(comm, lst_default);
 }
 // ---------------------------------------------------------------------------
 void tf_respond_snprintf(TF_TYPE type, TF_ID id, const char *format, ...)
 {
 #define ERR_STR_LEN 64
    char buf[ERR_STR_LEN];
    va_list args;
    va_start(args, format);
    uint32_t len = (uint32_t) fixup_vsnprintf(&buf[0], ERR_STR_LEN, format, args);
    va_end(args);
    tf_respond_buf(type, id, (const uint8_t *) buf, len);
 }
 void tf_respond_buf(TF_TYPE type, TF_ID id, const uint8_t *buf, uint32_t len)
 {
    TF_Msg msg;
    TF_ClearMsg(&msg);
    {
        msg.type = type;
        msg.frame_id = id;
        msg.data = buf;
        msg.len = (TF_LEN) len;
    }
    TF_Respond(comm, &msg);
 }
 void tf_send_buf(TF_TYPE type, const uint8_t *buf, uint32_t len)
 {
    TF_Msg msg;
    TF_ClearMsg(&msg);
    {
        msg.type = MSG_UNIT_REPORT;
        msg.data = buf;
        msg.len = (TF_LEN) len;
        msg.type = type;
    }
    TF_Send(comm, &msg); // no listener
 }
 // ---------------------------------------------------------------------------
 static void job_respond_err(Job *job)
 {
    tf_respond_str(MSG_ERROR, job->frame_id, job->str);
 }
 void sched_respond_err(TF_ID frame_id, const char *message)
 {
    dbg("ERR: %s", message);
    Job job = {
        .cb = job_respond_err,
        .frame_id = frame_id,
        .str = message
    };
    scheduleJob(&job, TSK_SCHED_LOW);
 }
 void sched_respond_bad_cmd(TF_ID frame_id)
 {
    sched_respond_err(frame_id, "BAD COMMAND");
 }
 void sched_respond_malformed_cmd(TF_ID frame_id)
 {
    sched_respond_err(frame_id, "MALFORMED PAYLOAD");
 }
 // ---------------------------------------------------------------------------
 static void job_respond_suc(Job *job)
 {
    tf_respond_ok(job->frame_id);
 }
 void sched_respond_suc(TF_ID frame_id)
 {
    Job job = {
        .cb = job_respond_suc,
        .frame_id = frame_id
    };
    scheduleJob(&job, TSK_SCHED_LOW);
 }
 // ---------------------------------------------------------------------------
 static void job_respond_uX(Job *job)
 {
    tf_respond_buf(MSG_SUCCESS, job->frame_id, (const uint8_t *) &job->d32, job->len);
 }
 void sched_respond_u8(TF_ID frame_id, uint8_t d)
 {
    Job job = {
        .cb = job_respond_uX,
        .frame_id = frame_id,
        .d32 = d,
        .len = 1
    };
    scheduleJob(&job, TSK_SCHED_HIGH);
 }
 void sched_respond_u16(TF_ID frame_id, uint16_t d)
 {
    Job job = {
        .cb = job_respond_uX,
        .frame_id = frame_id,
        .d32 = d,
        .len = 2
    };
    scheduleJob(&job, TSK_SCHED_HIGH);
 }
 void sched_respond_u32(TF_ID frame_id, uint32_t d)
 {
    Job job = {
        .cb = job_respond_uX,
        .frame_id = frame_id,
        .d32 = d,
        .len = 4
    };
    scheduleJob(&job, TSK_SCHED_HIGH);
 }
 // ---------------------------------------------------------------------------
 static void job_ping_reply(Job *job)
 {
    tf_respond_snprintf(MSG_SUCCESS, job->frame_id, "%s/%s", GEX_VERSION, GEX_PLATFORM);
 }
 static TF_Result lst_ping(TinyFrame *tf, TF_Msg *msg)
 {
    Job job = {
        .cb = job_ping_reply,
        .frame_id = msg->frame_id
    };
    scheduleJob(&job, TSK_SCHED_LOW);
    return TF_STAY;
 }
 // ----------------------------------------------------------------------------
 static void job_unhandled_resp(Job *job)
 {
    tf_respond_snprintf(MSG_ERROR, job->frame_id, "UNKNOWN MSG %"PRIu32, job->d32);
 }
 static TF_Result lst_default(TinyFrame *tf, TF_Msg *msg)
 {
    Job job = {
        .cb = job_unhandled_resp,
        .frame_id = msg->frame_id,
        .d32 = msg->type
    };
    scheduleJob(&job, TSK_SCHED_LOW);
    return TF_STAY;
 }
 // ----------------------------------------------------------------------------
 static TF_Result lst_unit(TinyFrame *tf, TF_Msg *msg)
 {
    ureg_deliver_unit_request(msg);
    return TF_STAY;
 }
 // ----------------------------------------------------------------------------
 static void job_list_units(Job *job)
 {
    ureg_report_active_units(job->frame_id);
 }
 static TF_Result lst_list_units(TinyFrame *tf, TF_Msg *msg)
 {
    Job job = {
        .cb = job_list_units,
        .frame_id = msg->frame_id,
    };
    scheduleJob(&job, TSK_SCHED_LOW);
    return TF_STAY;
 }
--- a/comm/messages.h
+++ b/comm/messages.h
@ -0,0 +1,150 @@
 //
 // Created by MightyPork on 2017/11/21.
 //
 #ifndef GEX_MESSAGES_H
 #define GEX_MESSAGES_H
 #include "sched_queue.h"
 #include "task_sched.h"
 #include "TinyFrame.h"
 extern TinyFrame *comm;
 /**
 * Initialize TinyFrame and set up listeners
 */
 void comm_init(void);
 /**
 * Supported message types (TF_TYPE)
 */
 enum TF_Types_ {
    // General, low level
    MSG_SUCCESS  = 0x00, //!< Generic success response; used by default in all responses; payload is transaction-specific
    MSG_PING     = 0x01, //!< Ping request (or response), used to test connection
    MSG_ERROR    = 0x02, //!< Generic failure response (when a request fails to execute)
    // Unit messages
    MSG_UNIT_REQUEST  = 0x10, //!< Command addressed to a particular unit
    MSG_UNIT_REPORT   = 0x11, //!< Spontaneous report from a unit
    // System messages
    MSG_LIST_UNITS    = 0x20, //!< Get all unit call-signs and names
 };
 /**
 * Respond to a TF message using printf-like formatting.
 * Works synchronously, must be called on a job queue.
 *
 * @param type - response type byte
 * @param frame_id - ID of the original msg
 * @param format - printf format
 * @param ... - replacements
 */
 void  __attribute__((format(printf,3,4)))
 tf_respond_snprintf(TF_TYPE type, TF_ID frame_id, const char *format, ...);
 /**
 * Respond to a TF message with a buffer of fixed length and custom type.
 * Works synchronously, must be called on a job queue.
 *
 * @param type - response type byte
 * @param frame_id - ID of the original msg
 * @param buf - byte buffer
 * @param len - buffer size
 */
 void tf_respond_buf(TF_TYPE type, TF_ID frame_id, const uint8_t *buf, uint32_t len);
 /**
 * Respond to a TF message with empty body and MSG_SUCCESS type.
 * Works synchronously, must be called on a job queue.
 *
 * @param frame_id  - ID of the original msg
 */
 static inline void tf_respond_ok(TF_ID frame_id)
 {
    tf_respond_buf(MSG_SUCCESS, frame_id, NULL, 0);
 }
 /**
 * Same like tf_respond_buf(), but used for sending spontaneous reports.
 * Works synchronously, must be called on a job queue / timer task etc.
 *
 * @param type - response type byte
 * @param buf - byte buffer
 * @param len - buffer size
 */
 void tf_send_buf(TF_TYPE type, const uint8_t *buf, uint32_t len);
 /**
 * Same like tf_respond_buf(), but the buffer length is measured with strlen.
 * Used to sending ASCII string responses.
 * Works synchronously, must be called on a job queue.
 *
 * @param type - response type byte
 * @param frame_id - ID of the original msg
 * @param str - character buffer, zero terminated
 */
 static inline void tf_respond_str(TF_TYPE type, TF_ID frame_id, const char *str)
 {
    tf_respond_buf(type, frame_id, (const uint8_t *) str, (uint32_t) strlen(str));
 }
 /**
 * Schedule sending an ASCII string error response.
 * Schedules a low priority job.
 *
 * @param frame_id - ID of the original msg
 * @param str - character buffer, zero terminated
 */
 void sched_respond_err(TF_ID frame_id, const char *str);
 /**
 * Variant of sched_respond_err() for reporting bad received command code
 *
 * @param msg_id - ID of the original msg
 */
 void sched_respond_bad_cmd(TF_ID frame_id);
 /**
 * Variant of sched_respond_err() for reporting malformed commands (e.g. too short payload)
 *
 * @param msg_id - ID of the original msg
 */
 void sched_respond_malformed_cmd(TF_ID frame_id);
 /**
 * Schedule sending an empty response with MSG_SUCCESS type.
 * Schedules a low priority job.
 *
 * @param frame_id - ID of the original msg
 */
 void sched_respond_suc(TF_ID frame_id);
 /**
 * Schedule sending a one-byte response with MSG_SUCCESS type.
 * Schedules a high priority job.
 *
 * @param frame_id - ID of the original msg
 * @param d - data
 */
 void sched_respond_u8(TF_ID frame_id, uint8_t d);
 /**
 * Schedule sending a two-byte response with MSG_SUCCESS type.
 * Schedules a high priority job.
 *
 * @param frame_id - ID of the original msg
 * @param d - data
 */
 void sched_respond_u16(TF_ID frame_id, uint16_t d);
 /**
 * Schedule sending a 4-byte response with MSG_SUCCESS type.
 * Schedules a high priority job.
 *
 * @param frame_id - ID of the original msg
 * @param d - data
 */
 void sched_respond_u32(TF_ID frame_id, uint32_t d);
 #endif //GEX_MESSAGES_H
--- a/cortex_handlers.c
+++ b/cortex_handlers.c
@ -0,0 +1,184 @@
 /* Includes ------------------------------------------------------------------*/
 #include "platform.h"
 #include <TinyFrame.h>
 #include "platform/debug_uart.h"
 #include "platform/status_led.h"
 /* External variables --------------------------------------------------------*/
 /******************************************************************************/
 /*            Cortex-M3 Processor Interruption and Exception Handlers         */ 
 /******************************************************************************/
 /******************************************************************************/
 /* STM32F1xx Peripheral Interrupt Handlers                                    */
 /* Add here the Interrupt Handlers for the used peripherals.                  */
 /* For the available peripheral interrupt handler names,                      */
 /* please refer to the startup file (startup_stm32f1xx.s).                    */
 /******************************************************************************/
 /* USER CODE BEGIN 1 */
 #define tFAULT "\r\n\033[31mSYSTEM FAULT:\033[m"
 void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName)
 {
    /* Run time stack overflow checking is performed if
    configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
    called if a stack overflow is detected. */
    PRINTF(tFAULT" RTOS stack overflow! tsk: %s\r\n", (char *) pcTaskName);
    StatusLed_On(STATUS_FAULT);
    while (1);
 }
 #if VERBOSE_HARDFAULT
 void prvGetRegistersFromStack( uint32_t *origStack, uint32_t lr_value)
 {
 /* These are volatile to try and prevent the compiler/linker optimising them
 away as the variables never actually get used.  If the debugger won't show the
 values of the variables, make them global my moving their declaration outside
 of this function. */
    volatile uint32_t stacked_r0;
    volatile uint32_t stacked_r1;
    volatile uint32_t stacked_r2;
    volatile uint32_t stacked_r3;
    volatile uint32_t stacked_r12;
    volatile uint32_t stacked_lr; /* Link register. */
    volatile uint32_t stacked_pc; /* Program counter. */
    volatile uint32_t stacked_psr;/* Program status register. */
    uint32_t cfsr, hfsr, dfsr;
    uint32_t bus_fault_address;
    uint32_t memmanage_fault_address;
    bus_fault_address       = SCB->BFAR;
    memmanage_fault_address = SCB->MMFAR;
    cfsr = SCB->CFSR;
    hfsr = SCB->HFSR;
    dfsr = SCB->DFSR;
    stacked_r0 = origStack[0];
    stacked_r1 = origStack[1];
    stacked_r2 = origStack[2];
    stacked_r3 = origStack[3];
    stacked_r12 = origStack[4];
    stacked_lr = origStack[5];
    stacked_pc = origStack[6];
    stacked_psr = origStack[7];
 #define BS(reg, pos, str) (((reg)&(1<<(pos)))?(str" "):"")
 #define REDPTR(val)  (((val)&0xFF000000) != 0x08000000?"\033[31m":"\033[32m")
    /* USER CODE BEGIN HardFault_IRQn 0 */
    PRINTF(tFAULT" HARD FAULT\r\n\r\n");
    PRINTF("- Stack frame:\r\n");
    PRINTF(" R0  = \033[35m%"PRIX32"h\033[m\r\n", stacked_r0);
    PRINTF(" R1  = \033[35m%"PRIX32"h\033[m\r\n", stacked_r1);
    PRINTF(" R2  = \033[35m%"PRIX32"h\033[m\r\n", stacked_r2);
    PRINTF(" R3  = \033[35m%"PRIX32"h\033[m\r\n", stacked_r3);
    PRINTF(" R12 = \033[35m%"PRIX32"h\033[m\r\n", stacked_r12);
    PRINTF(" LR  = %s0x%08"PRIX32"\033[m\r\n", REDPTR(stacked_lr), stacked_lr);
    PRINTF(" PC  = %s0x%08"PRIX32"\033[m\r\n", REDPTR(stacked_pc), stacked_pc);
    PRINTF(" PSR = \033[36m0x%08"PRIX32"\033[m", stacked_psr);
    uint32_t exc = stacked_psr & 0x3F;
    PRINTF(" [ %s%s%s%s%s ]\r\n",
            BS(stacked_psr, 31, "N"),
            BS(stacked_psr, 30, "Z"),
            BS(stacked_psr, 29, "C"),
            BS(stacked_psr, 28, "V"),
        //BS(stacked_psr, 24, "T"), - thumb, always ON
            (exc==0)?"Thread":
            (exc==2)?"NMI":
            (exc==3)?"HardFault":
            (exc==11)?"SVCall":
            (exc==14)?"PendSV":
            (exc==15)?"SysTick":
            (exc>=16)?"IRQ":"Unknown"
    );
    PRINTF("\r\n- FSR/FAR:\r\n");
    PRINTF(" CFSR = \033[36m0x%08"PRIX32"\033[m\r\n", cfsr);
    PRINTF("      UsageFault: \033[31;1m%s%s%s%s%s%s%s\033[m\r\n"
                "      BusFault:   \033[31;1m%s%s%s%s%s%s%s%s\033[m\r\n"
                "      MemFault:   \033[31;1m%s%s%s%s%s%s%s\033[m\r\n",
            BS(cfsr, 0, "IAccViol"),
            BS(cfsr, 1, "DAccViol"),
            BS(cfsr, 3, "MUnstkErr"),
            BS(cfsr, 4, "MStkErr"),
            BS(cfsr, 5, "MLSPErr(FPU)"),
            BS(cfsr, 7, "MMArValid"),
            ((cfsr&0xFF)?"":"\033[m- "),
            BS(cfsr, 8, "IBusErr"),
            BS(cfsr, 9, "PreciseErr"),
            BS(cfsr, 10, "ImpreciseErr"),
            BS(cfsr, 11, "UnstkErr"),
            BS(cfsr, 12, "StkErr"),
            BS(cfsr, 13, "LSPErr"),
            BS(cfsr, 15, "BFArValid"),
            ((cfsr&0xFF00)?"":"\033[m- "),
            BS(cfsr, 16, "UndefInstr"),
            BS(cfsr, 17, "InvState"),
            BS(cfsr, 18, "InvPC"),
            BS(cfsr, 19, "NoCP"),
            BS(cfsr, 24, "Unaligned"),
            BS(cfsr, 25, "Div0"),
            ((cfsr&0xFFFF0000)?"":"\033[m- ")
    );
    PRINTF(" HFSR = \033[36m0x%08"PRIX32"\033[m", hfsr);
    PRINTF(" [ %s%s%s]\r\n",
            BS(hfsr, 31, "DebugEvt"),
            BS(hfsr, 30, "Forced"),
            BS(hfsr, 1, "VectTbl")
    );
    PRINTF(" DFSR = \033[36m0x%08"PRIX32"\033[m", dfsr);
    PRINTF(" [ %s%s%s%s%s]\r\n",
            BS(dfsr, 0, "Halted"),
            BS(dfsr, 1, "Bkpt"),
            BS(dfsr, 2, "DWtTrap"),
            BS(dfsr, 3, "VCatch"),
            BS(dfsr, 4, "External")
    );
    if (cfsr & 0x0080) PRINTF(" MMFAR = \033[33m0x%08"PRIX32"\033[m\r\n", memmanage_fault_address);
    if (cfsr & 0x8000) PRINTF(" BFAR = \033[33m0x%08"PRIX32"\033[m\r\n", bus_fault_address);
    PRINTF("\r\n- Misc\r\n");
    PRINTF(" LR/EXC_RETURN= %s0x%08"PRIX32"\033[m\n", REDPTR(lr_value), lr_value);
    StatusLed_On(STATUS_FAULT);
    while (1);
 }
 #endif
 /**
 * @brief This function handles Hard fault interrupt.
 */
 void  __attribute__((naked)) HardFault_Handler(void)
 {
 #if VERBOSE_HARDFAULT
    __asm volatile
    (
    " tst lr, #4                                                \n"
        " ite eq                                                    \n"
        " mrseq r0, msp                                             \n"
        " mrsne r0, psp                                             \n"
        " ldr r1, [r0, #24]                                         \n"
        " mov r2, lr                                                \n"
        " ldr r3, handler2_address_const                            \n"
        " bx r3                                                     \n"
        " handler2_address_const: .word prvGetRegistersFromStack    \n"
    );
 #endif
    PRINTF(tFAULT" HARD FAULT\r\n\r\n");
    StatusLed_On(STATUS_FAULT);
    while (1);
 }
 /* USER CODE END 1 */
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
--- a/debug.c
+++ b/debug.c
@ -0,0 +1,83 @@
 //
 // Created by MightyPork on 2017/11/04.
 //
 #include "platform.h"
 #include <reent.h>
 #if USE_DEBUG_UART
 #define DBG_BUF_LEN 80
 // debug printf
 int PRINTF(const char *format, ...)
 {
    va_list args;
    int len;
    char dbg_buf[DBG_BUF_LEN];
    va_start(args, format);
    /*convert into string at buff[0] of length iw*/
    len = (int)fixup_vsnprintf(&dbg_buf[0], DBG_BUF_LEN, format, args);
    _write_r(NULL, 2, dbg_buf, (size_t) len);
    va_end(args);
    return len;
 }
 /**
 * Puts with fixed size (print to debug uart)
 * @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);
 }
 /**
 * Print a string to debug uart
 * @param string - string to print, zero-terminated
 * @return number of characters printed
 */
 int PUTS(const char *string)
 {
    size_t len = strlen(string);
    _write_r(NULL, 2, string, len);
    return (int) 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
 }
 /**
 * Print string to debug uart, add newline if missing (printf-like)
 * @param format
 * @param ...
 */
 void dbg(const char *format, ...)
 {
    va_list args;
    int len;
    char dbg_buf[DBG_BUF_LEN];
    va_start(args, format);
    len = (int)VSNPRINTF(&dbg_buf[0], DBG_BUF_LEN, format, args);
    _write_r(NULL, 2, dbg_buf, (size_t) len);
    // add newline if not present
    if (dbg_buf[len-1] != '\n')
        _write_r(NULL, 2, "\r\n",  2);
    va_end(args);
 }
 #endif
--- a/debug.h
+++ b/debug.h
@ -0,0 +1,29 @@
 //
 // Created by MightyPork on 2017/11/04.
 //
 #ifndef GEX_DEBUG_H
 #define GEX_DEBUG_H
 #include <inttypes.h>
 #include <stddef.h>
 #include <stdarg.h>
 #if USE_DEBUG_UART
 void dbg(const char *format, ...) __attribute__((format(printf,1,2))) ;
 int PRINTF(const char *format, ...) __attribute__((format(printf,1,2))) ;
 void PUTSN(const char *string, size_t len);
 int PUTS(const char *string);
 int PUTCHAR(int ch);
 #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 PUTCHAR(ch) do {} while (0)
 #endif
 #endif //GEX_DEBUG_H
--- a/framework/resources.c
+++ b/framework/resources.c
@ -0,0 +1,136 @@
 //
 // Created by MightyPork on 2017/11/24.
 //
 #include "platform.h"
 #include "unit.h"
 #include "resources.h"
 static bool rsc_initialized = false;
 // This takes quite a lot of space, we could use u8 and IDs instead if needed
 struct resouce_slot {
    const char *name;
    Unit *owner;
 } __attribute__((packed));
 static struct resouce_slot resources[R_RESOURCE_COUNT];
 // here are the resource names for better debugging (could also be removed if absolutely necessary)
 const char *const rsc_names[] = {
 #define X(res_name) #res_name,
    XX_RESOURCES
 #undef X
 };
 /**
 * Initialize the resources registry
 */
 void rsc_init_registry(void)
 {
    for (int i = 0; i < R_RESOURCE_COUNT; i++) {
        resources[i].owner = &UNIT_PLATFORM;
    }
    rsc_initialized = true;
 }
 /**
 * Claim a resource for a unit
 *
 * @param unit - claiming unit
 * @param rsc - resource to claim
 * @return true on successful claim
 */
 bool rsc_claim(Unit *unit, Resource rsc)
 {
    assert_param(rsc_initialized);
    assert_param(rsc > R_NONE && rsc < R_RESOURCE_COUNT);
    assert_param(unit != NULL);
    if (resources[rsc].owner) {
        //TODO properly report to user
        dbg("ERROR!! Unit %s failed to claim resource %s, already held by %s!",
            unit->name, rsc_names[rsc], resources[rsc].owner->name);
        unit->status = E_RESOURCE_NOT_AVAILABLE;
        return false;
    }
    resources[rsc].owner = unit;
    return true;
 }
 /**
 * Claim a range of resources for a unit (useful for GPIO)
 *
 * @param unit - claiming unit
 * @param rsc0 - first resource to claim
 * @param rsc1 - last resource to claim
 * @return true on complete claim, false if any failed (none are claimed in that case)
 */
 bool rsc_claim_range(Unit *unit, Resource rsc0, Resource rsc1)
 {
    assert_param(rsc_initialized);
    assert_param(rsc0 > R_NONE && rsc0 < R_RESOURCE_COUNT);
    assert_param(rsc1 > R_NONE && rsc1 < R_RESOURCE_COUNT);
    assert_param(unit != NULL);
    for (int i = rsc0; i <= rsc1; i++) {
        if (!rsc_claim(unit, (Resource) i)) return false;
    }
    return true;
 }
 /**
 * Free a resource for other use
 *
 * @param unit - owning unit; if not null, free only resources claimed by this unit
 * @param rsc - resource to free
 */
 void rsc_free(Unit *unit, Resource rsc)
 {
    assert_param(rsc_initialized);
    assert_param(rsc > R_NONE && rsc < R_RESOURCE_COUNT);
    if (unit == NULL || resources[rsc].owner == unit) {
        resources[rsc].owner = NULL;
    }
 }
 /**
 * Free a range of resources (useful for GPIO)
 *
 * @param unit - owning unit; if not null, free only resources claimed by this unit
 * @param rsc0 - first resource to free
 * @param rsc1 - last resource to free
 */
 void rsc_free_range(Unit *unit, Resource rsc0, Resource rsc1)
 {
    assert_param(rsc_initialized);
    assert_param(rsc0 > R_NONE && rsc0 < R_RESOURCE_COUNT);
    assert_param(rsc1 > R_NONE && rsc1 < R_RESOURCE_COUNT);
    for (int i = rsc0; i <= rsc1; i++) {
        if (unit == NULL || resources[i].owner == unit) {
            resources[i].owner = NULL;
        }
    }
 }
 /**
 * Tear down a unit - release all resources owned by the unit
 *
 * @param unit - unit to tear down; free only resources claimed by this unit
 */
 void rsc_teardown(Unit *unit)
 {
    assert_param(rsc_initialized);
    assert_param(unit != NULL);
    for (int i = R_NONE+1; i < R_RESOURCE_COUNT; i++) {
        if (resources[i].owner == unit) {
            resources[i].owner = NULL;
        }
    }
 }
--- a/framework/resources.h
+++ b/framework/resources.h
@ -0,0 +1,80 @@
 //
 // Created by MightyPork on 2017/11/24.
 //
 #ifndef GEX_RESOURCES_H
 #define GEX_RESOURCES_H
 #include "platform.h"
 #include "unit.h"
 #define CHECK_SUC() do { if (!suc) return false; } while (0)
 // X macro: Resource name,
 #define XX_RESOURCES \
    X(NONE) \
    X(PA0) X(PA1) X(PA2) X(PA3) X(PA4) X(PA5) X(PA6) X(PA7) \
    X(PA8) X(PA9) X(PA10) X(PA11) X(PA12) X(PA13) X(PA14) X(PA15) \
    X(PB0) X(PB1) X(PB2) X(PB3) X(PB4) X(PB5) X(PB6) X(PB7) \
    X(PB8) X(PB9) X(PB10) X(PB11) X(PB12) X(PB13) X(PB14) X(PB15) \
    X(PC0) X(PC1) X(PC2) X(PC3) X(PC4) X(PC5) X(PC6) X(PC7) \
    X(PC8) X(PC9) X(PC10) X(PC11) X(PC12) X(PC13) X(PC14) X(PC15) \
    X(PD0) X(PD1) X(PD2) X(PD3) X(PD4) X(PD5) X(PD6) X(PD7) \
    X(PD8) X(PD9) X(PD10) X(PD11) X(PD12) X(PD13) X(PD14) X(PD15) \
    X(PE0) X(PE1) X(PE2) X(PE3) X(PE4) X(PE5) X(PE6) X(PE7) \
    X(PE8) X(PE9) X(PE10) X(PE11) X(PE12) X(PE13) X(PE14) X(PE15) \
    X(SPI1) X(SPI2) X(SPI3) \
    X(I2C1) X(I2C2) X(I2C3) \
    X(I2S1) X(I2S2) X(I2S3) \
    X(ADC1) X(ADC2) X(ADC3) X(ADC4) \
    X(DAC1) X(DAC2) \
    X(USART1) X(USART2) X(USART3) X(USART4) X(USART5) X(USART6) \
    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(RNG) X(LCD)
 // GPIOs are allocated whenever the pin is needed
 //  (e.g. when used for SPI, the R_SPI resource as well as the corresponding R_GPIO resources must be claimed)
 // Peripheral blocks (IPs) - not all chips have all blocks, usually the 1 and 2 are present as a minimum, if any.
 //  It doesn't really make sense to expose multiple instances of buses that support addressing
 // ADCs - some more advanced chips support differential input mode on some (not all!) inputs
 //  Usually only one or two instances are present
 // DAC - often only one is present, or none.
 // UARTs
 //  - 1 and 2 are present universally, 2 is connected to VCOM on Nucleo/Discovery boards, good for debug messages
 //  4 and 5 don't support synchronous mode.
 // Timers
 //  - 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
 // 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)
 typedef enum hw_resource Resource;
 enum hw_resource {
 #define X(res_name) R_##res_name,
    XX_RESOURCES
 #undef X
    R_RESOURCE_COUNT
 };
 void rsc_init_registry(void);
 bool rsc_claim(Unit *unit, Resource rsc);
 bool rsc_claim_range(Unit *unit, Resource rsc0, Resource rsc1);
 void rsc_teardown(Unit *unit);
 void rsc_free(Unit *unit, Resource rsc);
 void rsc_free_range(Unit *unit, Resource rsc0, Resource rsc1);
 #endif //GEX_RESOURCES_H
--- a/framework/settings.c
+++ b/framework/settings.c
@ -0,0 +1,236 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #include "platform.h"
 #include "settings.h"
 #include "unit_registry.h"
 #include "system_settings.h"
 #include "utils/str_utils.h"
 // This is the first entry in a valid config.
 // Change with each breaking change to force config reset.
 #define CONFIG_MARKER 0xA55C
 void settings_load(void)
 {
    dbg("Loading settings");
    uint8_t *buffer = (uint8_t *) SETTINGS_FLASH_ADDR;
    PayloadParser pp = pp_start(buffer, SETTINGS_BLOCK_SIZE, NULL);
    // Check the integrity marker
    if (pp_u16(&pp) != CONFIG_MARKER) {
        dbg("Config not valid!");
        // Save for next run
        settings_save();
        return;
    }
    // System section
    if (!systemsettings_load(&pp)) {
        dbg("!! System settings failed to load");
        return;
    }
    if (!ureg_load_units(&pp)) {
        dbg("!! Unit settings failed to load");
        return;
    }
 }
 #define SAVE_BUF_SIZE 256
 static uint8_t save_buffer[SAVE_BUF_SIZE];
 static uint32_t save_addr;
 #if DEBUG_FLASH_WRITE
 #define fls_printf(fmt, ...) dbg(fmt, ##__VA_ARGS__)
 #else
 #define fls_printf(fmt, ...) do {} while (0)
 #endif
 /**
 * Flush the save buffer to flash, moving leftovers from uneven half-words
 * to the beginning and adjusting the CWPack curent pointer accordingly.
 *
 * @param ctx - pack context
 * @param final - if true, flush uneven leftovers; else move them to the beginning and keep for next call.
 */
 static void savebuf_flush(PayloadBuilder *pb, bool final)
 {
    // TODO this might be buggy, was not tested cross-boundary yet
    // TODO remove those printf's after verifying correctness
    uint32_t bytes = (uint32_t) pb_length(pb);
    // Dump what we're flushing
    fls_printf("Flush: ");
    for (uint32_t i = 0; i < bytes; i++) {
        fls_printf("%02X ", save_buffer[i]);
    }
    fls_printf("\r\n");
    uint32_t halfwords = bytes >> 1;
    uint32_t remain = bytes & 1; // how many bytes won't be programmed
    fls_printf("Halfwords: %d, Remain: %d, last? %d\r\n", (int)halfwords, (int)remain, final);
    uint16_t *hwbuf = (void*) &save_buffer[0];
    for (; halfwords > 0; halfwords--) {
        uint16_t hword = *hwbuf++;
        fls_printf("%04X ", hword);
        HAL_StatusTypeDef res = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, save_addr, hword);
        assert_param(HAL_OK == res);
        save_addr += 2; // advance
    }
    // rewind the context buffer
    pb->current = pb->start;
    if (remain) {
        // We have an odd byte left to write
        if (final) {
            // We're done writing, this is the last call. Append the last byte to flash.
            uint16_t hword = save_buffer[bytes-1];
            fls_printf("& %02X ", hword);
            HAL_StatusTypeDef res = HAL_FLASH_Program(FLASH_TYPEPROGRAM_HALFWORD, save_addr, hword);
            assert_param(HAL_OK == res);
        } else {
            // Move the leftover to the beginning of the buffer for next call.
            save_buffer[0] = save_buffer[bytes-1];
            pb->current++;
        }
    }
    fls_printf("\r\n");
 }
 /**
 * Save buffer overflow handler.
 * This should flush whatever is in the buffer and let CWPack continue
 *
 * @param pb - buffer
 * @param more - how many more bytes are needed (this is meant for realloc / buffer expanding)
 * @return - success code
 */
 static bool savebuf_ovhandler(PayloadBuilder *pb, uint32_t more)
 {
    if (more > SAVE_BUF_SIZE) return false;
    savebuf_flush(pb, false);
    return true;
 }
 // Save settings to flash
 void settings_save(void)
 {
    HAL_StatusTypeDef hst;
    PayloadBuilder pb = pb_start(save_buffer, SAVE_BUF_SIZE, savebuf_ovhandler);
    save_addr = SETTINGS_FLASH_ADDR;
    fls_printf("--- Starting flash write... ---\r\n");
    hst = HAL_FLASH_Unlock();
    assert_param(hst == HAL_OK);
    {
        fls_printf("ERASE flash pages for settings storage...\r\n");
        // We have to first erase the pages
        FLASH_EraseInitTypeDef erase;
        erase.Banks = FLASH_BANK_1; // TODO ?????
        erase.NbPages = SETTINGS_BLOCK_SIZE/FLASH_PAGE_SIZE;
        erase.PageAddress = SETTINGS_FLASH_ADDR;
        erase.TypeErase = FLASH_TYPEERASE_PAGES;
        uint32_t pgerror = 0;
        hst = HAL_FLASHEx_Erase(&erase, &pgerror);
        assert_param(pgerror == 0xFFFFFFFFU);
        assert_param(hst == HAL_OK);
        // and now we can start writing...
        fls_printf("Beginning settings collect\r\n");
        // Marker that this is a valid save
        pb_u16(&pb, CONFIG_MARKER);
        fls_printf("Saving system settings\r\n");
        systemsettings_save(&pb);
        fls_printf("Saving units\r\n");
        ureg_save_units(&pb);
        fls_printf("Final flush\r\n");
        savebuf_flush(&pb, true);
    }
    fls_printf("Locking flash...\r\n");
    hst = HAL_FLASH_Lock();
    assert_param(hst == HAL_OK);
    fls_printf("--- Flash done ---\r\n");
 }
 /**
 * Write system settings to INI (without section)
 */
 void settings_write_ini(IniWriter *iw)
 {
    // File header
    iw_comment(iw, "CONFIG.INI");
    iw_comment(iw, "Changes are applied on file save and can be immediately tested and verified.");
    iw_comment(iw, "To persist to flash, replace the LOCK jumper before disconnecting from USB."); // TODO the jumper...
    systemsettings_write_ini(iw);
    iw_newline(iw);
    ureg_export_combined(iw);
 }
 void settings_read_ini_begin(void)
 {
    SystemSettings.modified = true;
    // load defaults
    systemsettings_init();
    ureg_remove_all_units();
 }
 void settings_read_ini(const char *restrict section, const char *restrict key, const char *restrict value)
 {
 //    dbg("[%s] %s = %s", section, key, value);
    if (streq(section, "SYSTEM")) {
        // system is always at the top
        systemsettings_read_ini(key, value);
    }
    else if (streq(section, "UNITS")) {
        // this will always come before individual units config
        // install or tear down units as described by the config
        ureg_instantiate_by_ini(key, value);
    } else {
        // not a standard section, may be some unit config
        // all unit sections contain the colon character [TYPE:NAME]
        const char *nameptr = strchr(section, ':');
        if (nameptr) {
            ureg_read_unit_ini(nameptr+1, key, value);
        } else {
            dbg("! Bad config key: [%s] %s = %s", section, key, value);
        }
    }
 }
 void settings_read_ini_end(void)
 {
    if (!ureg_finalize_all_init()) {
        dbg("Some units failed to init!!");
    }
 }
 uint32_t settings_get_ini_len(void)
 {
    // this writer is configured to skip everything, so each written byte will decrement the skip count
    IniWriter iw = iw_init(NULL, 0xFFFFFFFF, 1);
    settings_write_ini(&iw);
    // now we just check how many bytes were skipped
    return 0xFFFFFFFF - iw.skip;
 }
--- a/framework/settings.h
+++ b/framework/settings.h
@ -0,0 +1,66 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #ifndef GEX_SETTINGS_H
 #define GEX_SETTINGS_H
 #include "platform.h"
 #include "utils/ini_writer.h"
 /**
 * Load settings from flash (system settings + units).
 * Unit registry must be already initialized.
 *
 * This should happen only once, during the boot sequence.
 */
 void settings_load(void);
 /**
 * Save all settings to flash.
 * This may be called multiple times after user changes the config file.
 */
 void settings_save(void);
 /**
 * Call this before any of the ini read stuff
 *
 * Resets everything to defaults so we have a clean start.
 *
 * NOTE: Should the file be received only partially, this may corrupt the settings.
 * For this reason we don't commit it to flash immediately but require user to replace
 * the LOCK jumper before unplugging the device. (TODO implement the LOCK jumper and this feature!!)
 */
 void settings_read_ini_begin(void);
 /**
 * Load settings from INI kv pair.
 */
 void settings_read_ini(const char *restrict section, const char *restrict key, const char *restrict value);
 /**
 * Call this before any of the ini read stuff
 *
 * Resets everything to defaults so we have a clean start.
 *
 * NOTE: Should the file be received only partially, this may corrupt the settings.
 * For this reason we don't commit it to flash immediately but require user to replace
 * the LOCK jumper before unplugging the device. (TODO implement the LOCK jumper and this feature!!)
 */
 void settings_read_ini_end(void);
 /**
 * Write all settings to a iniwriter
 * @param iw - writer handle
 */
 void settings_write_ini(IniWriter *iw);
 /**
 * Get total settings len (caution: this is expensive, works by dummy-printing everything)
 *
 * @return bytes
 */
 uint32_t settings_get_ini_len(void);
 #endif //GEX_SETTINGS_H
--- a/framework/system_settings.c
+++ b/framework/system_settings.c
@ -0,0 +1,57 @@
 //
 // Created by MightyPork on 2017/12/02.
 //
 #include "platform.h"
 #include "utils/str_utils.h"
 #include "system_settings.h"
 struct system_settings SystemSettings;
 void systemsettings_init(void)
 {
    SystemSettings.visible_vcom = true;
    SystemSettings.editable = false; // This will be loaded in platform init based on the LOCK pin
    SystemSettings.modified = false;
 }
 // to binary
 void systemsettings_save(PayloadBuilder *pb)
 {
    pb_char(pb, 'S');
    pb_bool(pb, SystemSettings.visible_vcom);
 }
 // from binary
 bool systemsettings_load(PayloadParser *pp)
 {
    if (pp_char(pp) != 'S') return false;
    SystemSettings.visible_vcom = pp_bool(pp);
    return pp->ok;
 }
 /**
 * Write system settings to INI (without section)
 */
 void systemsettings_write_ini(IniWriter *iw)
 {
    iw_section(iw, "SYSTEM");
    iw_comment(iw, "Expose the comm. channel as a virtual comport (Y, N)");
    iw_entry(iw, "expose_vcom", str_yn(SystemSettings.visible_vcom));
 }
 /**
 * Load system settings from INI kv pair
 */
 bool systemsettings_read_ini(const char *restrict key, const char *restrict value)
 {
    bool suc = true;
    if (streq(key, "expose_vcom")) {
        bool yn = str_parse_yn(value, &suc);
        if (suc) SystemSettings.visible_vcom = yn;
    }
    return suc;
 }
--- a/framework/system_settings.h
+++ b/framework/system_settings.h
@ -0,0 +1,50 @@
 //
 // Created by MightyPork on 2017/12/02.
 //
 #ifndef GEX_SYSTEM_SETTINGS_H
 #define GEX_SYSTEM_SETTINGS_H
 #include "platform.h"
 #include "utils/ini_writer.h"
 #include "utils/payload_parser.h"
 #include "utils/payload_builder.h"
 struct system_settings {
    bool visible_vcom;
    // 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)
 };
 extern struct system_settings SystemSettings;
 /**
 * Load defaults
 */
 void systemsettings_init(void);
 /**
 * Write system settings to the pack context
 */
 void systemsettings_save(PayloadBuilder *pb);
 /**
 * Load system settings from the unpack context
 */
 bool systemsettings_load(PayloadParser *pp);
 /**
 * Write system settings to INI
 */
 void systemsettings_write_ini(IniWriter *iw);
 /**
 * Load system settings from INI kv pair
 *
 * @return true on success
 */
 bool systemsettings_read_ini(const char *restrict key, const char *restrict value);
 #endif //GEX_SYSTEM_SETTINGS_H
--- a/framework/unit.c
+++ b/framework/unit.c
@ -0,0 +1,47 @@
 //
 // Created by MightyPork on 2017/11/24.
 //
 #include "platform.h"
 #include "unit.h"
 #include "resources.h"
 // Abort partly inited unit
 void clean_failed_unit(Unit *unit)
 {
    if (unit == NULL) return;
    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;
    }
    dbg("Releasing any held resources");
    // Release any already claimed resources
    rsc_teardown(unit);
 }
 // ----------------------------------------------------
 // system unit is used to claim peripherals on behalf of the system (e.g. HAL tick source)
 Unit UNIT_SYSTEM = {
    .name = "SYSTEM"
 };
 // ----------------------------------------------------
 // platform unit is used to claim peripherals not present on the current platform
 Unit UNIT_PLATFORM = {
    .name = "PLATFORM"
 };
 // ----------------------------------------------------
--- a/framework/unit.h
+++ b/framework/unit.h
@ -0,0 +1,118 @@
 //
 // Created by MightyPork on 2017/11/24.
 //
 #ifndef GEX_UNIT_H
 #define GEX_UNIT_H
 #include "platform.h"
 #include <TinyFrame.h>
 #include "utils/ini_writer.h"
 #include "utils/payload_builder.h"
 #include "utils/payload_parser.h"
 typedef struct unit Unit;
 typedef struct unit_driver UnitDriver;
 struct unit {
    const UnitDriver *driver;
    /** Unit name (used in error messages) */
    const char *name;
    /**
     * Storage for arbitrary unit data (allocated in 'preInit' and freed in 'deInit' or when init/load fails)
     */
    void *data;
    /** Unit init status */
    error_t status;
    /** Unit call sign for messages */
    uint8_t callsign;
 };
 /**
 * Unit instance - statically or dynamically allocated (depends whether it's system or user unit)
 */
 struct unit_driver {
    /** Driver ID */
    const char *name;
    /** Unit type description (for use in comments) */
    const char *description;
    /**
     * Pre-init: allocate data object, init defaults
     */
    bool (*preInit)(Unit *unit);
    /**
     * Load settings from binary storage, parse and store them in the data object.
     * Don't do any validation, that's left for the init() function
     *
     * @param pp - parser
     */
    void (*cfgLoadBinary)(Unit *unit, PayloadParser *pp);
    /**
     * Write settings to binary storage.
     *
     * @param pb - builder
     */
    void (*cfgWriteBinary)(Unit *unit, PayloadBuilder *pb);
    /**
     * Load settings from a INI file.
     * Name has already been parsed and assigned.
     * This function is called repeatedly as kv-pairs are encountered in the stream.
     *
     * @param key - key from the INI file
     * @param value - value from the ini file; strings have already removed quotes and replaced escape sequences with ASCII as needed
     */
    bool (*cfgLoadIni)(Unit *unit, const char *key, const char *value);
    /**
     * Export settings to a INI file.
     * Capacity will likely be 512 bytes, do not waste space!
     *
     * @param buffer - destination buffer
     * @param capacity - buffer size
     * @return nubmer of bytes used
     */
    void (*cfgWriteIni)(Unit *unit, IniWriter *iw);
    /**
     * Finalize the init sequence, validate settings, enable peripherals and prepare for operation
     */
    bool (*init)(Unit *unit);
    /**
     * De-initialize the unit: de-init peripheral, free resources, free data object...
     * This is called when disabling all units in order to reload new config.
     */
    void (*deInit)(Unit *unit);
    /**
     * Handle an incoming request. Return true if command was OK.
     */
    bool (*handleRequest)(Unit *unit, TF_ID frame_id, uint8_t command, PayloadParser *pp);
 };
 /**
 * De-init a partially initialized unit (before 'init' succeeds)
 * This releases all held resources and frees the *data,
 * if it looks like it has been dynamically allocated.
 *
 * Does NOT free the unit struct itself
 *
 * @param unit - unit to discard
 */
 void clean_failed_unit(Unit *unit);
 /** Marks peripherals claimed by the system */
 extern Unit UNIT_SYSTEM;
 /** Marks peripherals not available on the platform */
 extern Unit UNIT_PLATFORM;
 #endif //GEX_UNIT_H
--- a/framework/unit_base.h
+++ b/framework/unit_base.h
@ -0,0 +1,12 @@
 //
 // Created by MightyPork on 2017/12/09.
 //
 #include "platform.h"
 #include "unit.h"
 #include "pin_utils.h"
 #include "resources.h"
 #include "utils/str_utils.h"
 #include "utils/malloc_safe.h"
 #include "payload_builder.h"
 #include "payload_parser.h"
--- a/framework/unit_registry.c
+++ b/framework/unit_registry.c
@ -0,0 +1,579 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #include "platform.h"
 #include "utils/avrlibc.h"
 #include "comm/messages.h"
 #include "utils/ini_writer.h"
 #include "utils/str_utils.h"
 #include "utils/malloc_safe.h"
 #include "unit_registry.h"
 #include "resources.h"
 // ** Unit repository **
 typedef struct ureg_entry UregEntry;
 typedef struct ulist_entry UlistEntry;
 struct ureg_entry {
    const UnitDriver *driver;
    UregEntry *next;
 };
 UregEntry *ureg_head = NULL;
 UregEntry *ureg_tail = NULL;
 // ---
 struct ulist_entry {
    Unit unit;
    UlistEntry *next;
 };
 UlistEntry *ulist_head = NULL;
 UlistEntry *ulist_tail = NULL;
 // ---
 void ureg_add_type(const UnitDriver *driver)
 {
    assert_param(driver != NULL);
    assert_param(driver->description != NULL);
    assert_param(driver->name != NULL);
    assert_param(driver->preInit != NULL);
    assert_param(driver->cfgLoadBinary != NULL);
    assert_param(driver->cfgLoadIni != NULL);
    assert_param(driver->cfgWriteBinary != NULL);
    assert_param(driver->cfgWriteIni != NULL);
    assert_param(driver->init != NULL);
    assert_param(driver->deInit != NULL);
    assert_param(driver->handleRequest != NULL);
    UregEntry *re = malloc_s(sizeof(UregEntry));
    re->driver = driver;
    re->next = NULL;
    if (ureg_head == NULL) {
        ureg_head = re;
    } else {
        ureg_tail->next = re;
    }
    ureg_tail = re;
 }
 /** Free unit in a list entry (do not free the list entry itself!) */
 static void free_le_unit(UlistEntry *le)
 {
    Unit *const pUnit = &le->unit;
    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;
    }
 }
 /** Remove a unit and update links appropriately */
 static void remove_unit_from_list(UlistEntry *restrict le, UlistEntry *restrict parent)
 {
    if (parent == NULL) {
        ulist_head = le->next;
    } else {
        parent->next = le->next;
    }
    // Fix tail potentially pointing to the removed entry
    if (ulist_tail == le) {
        ulist_tail = parent;
    }
 }
 /** Add unit to the list, updating references as needed */
 static void add_unit_to_list(UlistEntry *le)
 {
    // Attach to the list
    if (ulist_head == NULL) {
        ulist_head = le;
    } else {
        ulist_tail->next = le;
    }
    ulist_tail = le;
 }
 /** Find a unit in the list */
 static UlistEntry *find_unit(const Unit *unit, UlistEntry **pParent)
 {
    UlistEntry *le = ulist_head;
    UlistEntry *parent = NULL;
    while (le != NULL) {
        if (&le->unit == unit) {
            if (pParent != NULL) {
                *pParent = parent;
            }
            return le;
        }
        parent = le;
        le = le->next;
    }
    dbg("!! Unit was not found in registry");
    *pParent = NULL;
    return NULL;
 }
 // create a unit instance (not yet loading or initing - just pre-init)
 Unit *ureg_instantiate(const char *driver_name)
 {
    bool suc = true;
    dbg("Creating unit of type %s", driver_name);
    // Find type in the repository
    UregEntry *re = ureg_head;
    while (re != NULL) {
        if (streq(re->driver->name, driver_name)) {
            // Create new list entry
            UlistEntry *le = malloc_ck(sizeof(UlistEntry), &suc);
            CHECK_SUC();
            le->next = NULL;
            Unit *pUnit = &le->unit;
            pUnit->driver = re->driver;
            pUnit->status = E_LOADING;
            pUnit->data = NULL;
            pUnit->callsign = 0;
            suc = pUnit->driver->preInit(pUnit);
            if (!suc) {
                // tear down what we already allocated and abort
                // If it failed this early, the only plausible explanation is failed malloc,
                // in which case the data structure is not populated and keeping the
                // broken unit doesn't serve any purpose. Just ditch it...
                dbg("!! Unit failed to pre-init!");
                clean_failed_unit(pUnit);
                free(le);
                return NULL;
            }
            add_unit_to_list(le);
            return pUnit;
        }
        re = re->next;
    }
    dbg("!! Did not find unit type %s", driver_name);
    return NULL;
 }
 // remove before init()
 void ureg_clean_failed(Unit *unit)
 {
    dbg("Cleaning failed unit from registry");
    UlistEntry *le;
    UlistEntry *parent;
    le = find_unit(unit, &parent);
    if (!le) return;
    clean_failed_unit(&le->unit);
    remove_unit_from_list(le, parent);
    free(le);
 }
 // remove after successful init()
 void ureg_remove_unit(Unit *unit)
 {
    dbg("Cleaning & removing unit from registry");
    UlistEntry *le;
    UlistEntry *parent;
    le = find_unit(unit, &parent);
    if (!le) return;
    free_le_unit(le);
    remove_unit_from_list(le, parent);
    free(le);
 }
 void ureg_save_units(PayloadBuilder *pb)
 {
    assert_param(pb->ok);
    uint32_t count = ureg_get_num_units();
    pb_char(pb, 'U');
    pb_u16(pb, (uint16_t) count);
    UlistEntry *le = ulist_head;
    while (le != NULL) {
        Unit *const pUnit = &le->unit;
        pb_char(pb, 'u');
        pb_string(pb, pUnit->driver->name);
        pb_string(pb, pUnit->name);
        pb_u8(pb, pUnit->callsign);
        // Now all the rest, unit-specific
        pUnit->driver->cfgWriteBinary(pUnit, pb);
        assert_param(pb->ok);
        le = le->next;
    }
 }
 bool ureg_load_units(PayloadParser *pp)
 {
    bool suc;
    char typebuf[16];
    assert_param(pp->ok);
    if (pp_char(pp) != 'U') return false;
    uint16_t unit_count = pp_u16(pp);
    for (uint32_t j = 0; j < unit_count; j++) {
        // We're now unpacking a single unit
        // Marker that this is a unit - it could get out of alignment if structure changed
        if (pp_char(pp) != 'u') return false;
        // TYPE
        pp_string(pp, typebuf, 16);
        Unit *const pUnit = ureg_instantiate(typebuf);
        assert_param(pUnit);
        // NAME
        pp_string(pp, typebuf, 16);
        pUnit->name = strdup(typebuf);
        assert_param(pUnit->name);
        // CALLSIGN
        pUnit->callsign = pp_u8(pp);
        assert_param(pUnit->callsign != 0);
        // Load the rest of the unit
        pUnit->driver->cfgLoadBinary(pUnit, pp);
        assert_param(pp->ok);
        suc = pUnit->driver->init(pUnit); // finalize the load and init the unit
        if (pUnit->status == E_LOADING) {
            pUnit->status = suc ? E_SUCCESS : E_BAD_CONFIG;
        }
        // XXX we want to keep the failed unit to preserve settings and for error reporting
 //        if (!suc) {
 //            // Discard, remove from registry
 //            ureg_clean_failed(unit);
 //        }
    }
    return pp->ok;
 }
 void ureg_remove_all_units(void)
 {
    UlistEntry *le = ulist_head;
    UlistEntry *next;
    while (le != NULL) {
        next = le->next;
        free_le_unit(le);
        free(le);
        le = next;
    }
    ulist_head = ulist_tail = NULL;
 }
 bool ureg_instantiate_by_ini(const char *restrict driver_name, const char *restrict names)
 {
    UregEntry *re = ureg_head;
    while (re != NULL) {
        if (streq(re->driver->name, driver_name)) {
            const char *p = names;
            while (p != NULL) { // we use this to indicate we're done
                // skip leading whitespace (assume there's never whitespace before a comma)
                while (*p == ' ' || *p == '\t') p++;
                if (*p == 0) break; // out of characters
                const char *delim = strchr(p, ',');
                char *name = NULL;
                if (delim != NULL) {
                    // not last
                    name = strndup(p, delim - p);
                    p = delim + 1;
                } else {
                    // last name
                    name = strdup(p);
                    p = NULL; // quit after this loop ends
                }
                assert_param(name);
                Unit *pUnit = ureg_instantiate(driver_name);
                if (!pUnit) {
                    free(name);
                    return false;
                }
                pUnit->name = name;
                // don't init yet - leave that for when we're done with the INI
            }
            return true;
        }
        re = re->next;
    }
    dbg("! ureg instantiate - bad type");
    return false;
 }
 bool ureg_read_unit_ini(const char *restrict name,
                        const char *restrict key,
                        const char *restrict value)
 {
    UlistEntry *li = ulist_head;
    while (li != NULL) {
        if (streq(li->unit.name, name)) {
            Unit *const pUnit = &li->unit;
            if (streq(key, "CALLSIGN")) {
                // handled separately from unit data
                pUnit->callsign = (uint8_t) avr_atoi(value);
                return true;
            } else {
                return pUnit->driver->cfgLoadIni(pUnit, key, value);
            }
        }
        li = li->next;
    }
    return false;
 }
 bool ureg_finalize_all_init(void)
 {
    dbg("Finalizing units init...");
    bool suc = true;
    UlistEntry *li = ulist_head;
    uint8_t callsign = 1;
    while (li != NULL) {
        Unit *const pUnit = &li->unit;
        bool s = pUnit->driver->init(pUnit);
        if (!s) {
            dbg("!!!! error initing unit %s", pUnit->name);
            if (pUnit->status == E_LOADING) {
                // assume it's a config error if not otherwise specified
                pUnit->status = E_BAD_CONFIG;
            }
        } else {
            pUnit->status = E_SUCCESS;
        }
        // try to assign unique callsigns
        if (pUnit->callsign == 0) {
            pUnit->callsign = callsign++;
        } else {
            if (pUnit->callsign >= callsign) {
                callsign = (uint8_t) (pUnit->callsign + 1);
            }
        }
        suc &= s;
        li = li->next;
    }
    return suc;
 }
 static void export_unit_do(UlistEntry *li, IniWriter *iw)
 {
    Unit *const pUnit = &li->unit;
    iw_section(iw, "%s:%s", pUnit->driver->name, pUnit->name);
    iw_comment(iw, ">> Status: %s", error_get_string(pUnit->status));
    iw_newline(iw);
    iw_comment(iw, "Address for control messages (1-255)");
    iw_entry(iw, "CALLSIGN", "%d", pUnit->callsign);
    pUnit->driver->cfgWriteIni(pUnit, iw);
    iw_newline(iw);
 }
 // unit to INI
 void ureg_export_unit(uint32_t index, IniWriter *iw)
 {
    UlistEntry *li = ulist_head;
    uint32_t count = 0;
    while (li != NULL) {
        if (count == index) {
            export_unit_do(li, iw);
            return;
        }
        count++;
        li = li->next;
    }
 }
 // unit to INI
 void ureg_export_combined(IniWriter *iw)
 {
    UlistEntry *li;
    UregEntry *re;
    // Unit list
    iw_section(iw, "UNITS");
    iw_comment(iw, "Here is a list of all unit types supported by the current firmware.");
    iw_comment(iw, "To manage units, simply add/remove their comma-separated names next to");
    iw_comment(iw, "the desired unit type. Reload the file and the corresponding unit");
    iw_comment(iw, "sections should appear below, ready to configure.");
    // This could certainly be done in some more efficient way ...
    re = ureg_head;
    while (re != NULL) {
        // Should produce something like:
        // # Description string here
        // TYPE_ID=NAME1,NAME2
        //
        const UnitDriver *const pDriver = re->driver;
        iw_newline(iw);
        iw_comment(iw, pDriver->description);
        iw_string(iw, pDriver->name);
        iw_string(iw, "=");
        li = ulist_head;
        uint32_t count = 0;
        while (li != NULL) {
            Unit *const pUnit = &li->unit;
            if (streq(pUnit->driver->name, pDriver->name)) {
                if (count > 0) iw_string(iw, ",");
                iw_string(iw, pUnit->name);
                count++;
            }
            li = li->next;
        }
        re = re->next;
        iw_newline(iw);
    }
    iw_newline(iw); // space before the unit sections
    // Now we dump all the units
    li = ulist_head;
    while (li != NULL) {
        export_unit_do(li, iw);
        li = li->next;
    }
 }
 // count units
 uint32_t ureg_get_num_units(void)
 {
    // TODO keep this in a variable
    UlistEntry *li = ulist_head;
    uint32_t count = 0;
    while (li != NULL) {
        count++;
        li = li->next;
    }
    return count;
 }
 static void job_nosuch_unit(Job *job)
 {
    tf_respond_snprintf(MSG_ERROR, job->frame_id, "NO UNIT @ %"PRIu32, job->d32);
 }
 /** Deliver message to it's destination unit */
 void ureg_deliver_unit_request(TF_Msg *msg)
 {
    PayloadParser pp = pp_start(msg->data, msg->len, NULL);
    uint8_t callsign = pp_u8(&pp);
    uint8_t command = pp_u8(&pp);
    // highest bit indicates user wants an extra confirmation on success
    bool confirmed = (bool) (command & 0x80);
    command &= 0x7F;
    if (!pp.ok) { dbg("!! pp not OK!"); }
    if (callsign == 0 || !pp.ok) {
        sched_respond_malformed_cmd(msg->frame_id);
        return;
    }
    UlistEntry *li = ulist_head;
    while (li != NULL) {
        Unit *const pUnit = &li->unit;
        if (pUnit->callsign == callsign) {
            bool ok = pUnit->driver->handleRequest(pUnit, msg->frame_id, command, &pp);
            if (ok && confirmed) {
                sched_respond_suc(msg->frame_id);
            }
            return;
        }
        li = li->next;
    }
    // Not found
    Job job = {
        .cb = job_nosuch_unit,
        .frame_id = msg->frame_id,
        .d32 = callsign
    };
    scheduleJob(&job, TSK_SCHED_LOW);
 }
 void ureg_report_active_units(TF_ID frame_id)
 {
    // count bytes needed
    uint32_t needed = 1; //
    UlistEntry *li = ulist_head;
    uint32_t count = 0;
    while (li != NULL) {
        count++;
        needed += strlen(li->unit.name)+1;
        li = li->next;
    }
    needed += count;
    bool suc = true;
    uint8_t *buff = malloc_ck(needed, &suc);
    if (!suc) { tf_respond_str(MSG_ERROR, frame_id, "OUT OF MEMORY"); return; }
    {
        PayloadBuilder pb = pb_start(buff, needed, NULL);
        pb_u8(&pb, (uint8_t) count); // assume we don't have more than 255
        li = ulist_head;
        while (li != NULL) {
            pb_u8(&pb, li->unit.callsign);
            pb_string(&pb, li->unit.name);
            li = li->next;
        }
        assert_param(pb.ok);
        tf_respond_buf(MSG_SUCCESS, frame_id, buff, needed);
    }
    free(buff);
 }
--- a/framework/unit_registry.h
+++ b/framework/unit_registry.h
@ -0,0 +1,128 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #ifndef GEX_UNIT_REGISTRY_H
 #define GEX_UNIT_REGISTRY_H
 #include <TinyFrame/TinyFrame.h>
 #include "platform.h"
 #include "unit.h"
 /**
 * Add instantiable unit type to the registry
 *
 * @param driver - unit template, will be shallowly cloned for new instances
 */
 void ureg_add_type(const UnitDriver *driver);
 /**
 * Create an instance of a unit type. The unit is added to the unit list.
 *
 * @param driver_name - unit type, same as given when registering the type. CAN BE ON STACK! Not stored.
 * @return the unit, or NULL on failure
 */
 Unit *ureg_instantiate(const char *driver_name);
 /**
 * Clean a unit previously obtained by 'ureg_instantiate' that
 * failed to properly load or init. This tears it down and removes it from the unit list.
 *
 * @param unit - unit to remove
 */
 void ureg_clean_failed(Unit *unit);
 /**
 * Safely delete a unit instance (releasing memory and reosurces, removing it from the list)
 *
 * @param unit - unit to remove
 */
 void ureg_remove_unit(Unit *unit);
 /**
 * De-init and remove all units
 */
 void ureg_remove_all_units(void);
 /**
 * Save all units to a binary buffer
 * @param ctx
 */
 void ureg_save_units(PayloadBuilder *pb);
 /**
 * Load units from the binary format
 * @param ctx
 */
 bool ureg_load_units(PayloadParser *pp);
 /**
 * Export unit as INI to a buffer.
 *
 * @param index - unit index
 * @param iw - iniwriter instance to use
 * @return real number of bytes used (should end with a newline)
 */
 void ureg_export_unit(uint32_t index, IniWriter *iw);
 /**
 * Export everything to INI
 *
 * @param iw
 */
 void ureg_export_combined(IniWriter *iw);
 /**
 * Get number of instantiated units
 *
 * @return nr of units
 */
 uint32_t ureg_get_num_units(void);
 /**
 * Instantiate a unit by INI
 *
 * This is called for lines inside the [UNITS] section, e.g. PIN=LED1, BUTTON
 *
 * @param driver_name - unit type ID
 * @param names - names string, comma separated (may have whitespace after commas)
 * @return all OK
 */
 bool ureg_instantiate_by_ini(const char *restrict driver_name, const char *restrict names);
 /**
 * Load a single INI line to a unit.
 *
 * @param name - unit name (for look-up)
 * @param key - property key
 * @param value - value to set as string
 * @return success
 */
 bool ureg_read_unit_ini(const char *restrict name,
                        const char *restrict key,
                        const char *restrict value);
 /**
 * Run init() for all unit instances.
 *
 * @return all OK
 */
 bool ureg_finalize_all_init(void);
 /**
 * Deliver a TinyFrame message to it's designed destination.
 * Unit is identified by the data first byte which is the "call sign"
 *
 * @param msg - message to deliver
 * @return true if delivered
 */
 void ureg_deliver_unit_request(TF_Msg *msg);
 /**
 * Report all unit callsigns and names to TF master
 *
 * @param frame_id - original message ID
 */
 void ureg_report_active_units(TF_ID frame_id);
 #endif //GEX_UNIT_REGISTRY_H
--- a/gex.mk
+++ b/gex.mk
@ -0,0 +1,52 @@
 GEX_SRC_DIR = \
 User \
 User/utils \
 User/USB \
 User/comm \
 User/framework \
 User/platform \
 User/units \
 User/units/system \
 User/units/neopixel \
 User/units/pin \
 User/TinyFrame \
 User/CWPack \
 User/USB/MSC_CDC \
 User/vfs
 GEX_INCLUDES = \
 -IUser \
 -IUser/USB \
 -IUser/USB/MSC_CDC \
 -IUser/TinyFrame \
 -IUser/vfs \
 -IUser/utils \
 -IUser/units \
 -IUser/units/system \
 -IUser/units/neopixel \
 -IUser/units/pin \
 -IUser/framework \
 -IUser/platform
 GEX_CFLAGS      = -D__weak="__attribute__((weak))" -D__packed="__attribute__((__packed__))"
 GEX_CFLAGS      += -std=gnu99 -Wfatal-errors
 GEX_CFLAGS      += -Wall -Wextra -Wshadow
 GEX_CFLAGS      += -Wwrite-strings -Wold-style-definition -Winline -Wno-missing-noreturn -Wstrict-prototypes -Wreturn-type
 GEX_CFLAGS      += -Wredundant-decls -Wfloat-equal -Wsign-compare
 GEX_CFLAGS      += -fno-common -ffunction-sections -fdata-sections -Wno-unused-function
 GEX_CFLAGS      += -MD -Wno-format-zero-length -Wno-redundant-decls -Wno-unused-parameter
 GEX_CFLAGS      += -Wno-discarded-qualifiers -Wno-unused-variable -Wno-inline
 GEX_CFLAGS      += -Wno-float-equal -Wno-implicit-fallthrough -Wno-strict-aliasing
 GEX_CFLAGS      += -fmerge-constants -fmerge-all-constants
 GEX_CFLAGS      += -fno-exceptions -Wno-pointer-to-int-cast -Wno-int-to-pointer-cast -finline-small-functions -findirect-inlining
 GEX_CDEFS = \
 -D__weak="__attribute__((weak))" \
 -D__packed="__attribute__((__packed__))" \
 -DUSE_FULL_LL_DRIVER \
 -DUSE_FULL_ASSERT=1 \
 -DVERBOSE_ASSERT=1 \
 -DDEBUG_VFS=0 \
 -DVERBOSE_HARDFAULT=1 \
 -DUSE_STACK_MONITOR=1 \
 -DUSE_DEBUG_UART=1
--- a/gex_hooks.c
+++ b/gex_hooks.c
@ -0,0 +1,36 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #include "platform.h"
 #include "USB/usb_device.h"
 #include "TinyFrame.h"
 #include "comm/messages.h"
 #include "platform/status_led.h"
 #include "platform/debug_uart.h"
 #include "gex_hooks.h"
 /**
 * This is a systick callback for GEX application logic
 */
 void GEX_MsTick(void)
 {
    TF_Tick(comm);
    StatusLed_Tick();
 }
 /**
 * Early init, even before RTOS starts
 */
 void GEX_PreInit(void)
 {
    DebugUart_PreInit();
    dbg("\r\n\033[37;1m*** GEX "GEX_VERSION" on "GEX_PLATFORM" ***\033[m");
    dbg("Build "__DATE__" "__TIME__"\r\n");
    plat_init();
    MX_USB_DEVICE_Init();
    dbg("Starting FreeRTOS...");
 }
--- a/gex_hooks.h
+++ b/gex_hooks.h
@ -0,0 +1,11 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #ifndef GEX_GEX_HOOKS_H
 #define GEX_GEX_HOOKS_H
 void GEX_MsTick(void);
 void GEX_PreInit(void);
 #endif //GEX_GEX_HOOKS_H
--- a/platform/debug_uart.c
+++ b/platform/debug_uart.c
@ -0,0 +1,64 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #include "platform.h"
 #include "framework/resources.h"
 #include "debug_uart.h"
 #include "plat_compat.h"
 #if USE_DEBUG_UART
 /** Init the submodule. */
 void DebugUart_Init(void)
 {
    // Debug UART
    bool ok = true;
    ok &= rsc_claim(&UNIT_SYSTEM, R_USART2);
    ok &= rsc_claim(&UNIT_SYSTEM, R_PA2);
    assert_param(ok);
 }
 /** 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();
    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);
 //    LL_USART_SetDataWidth(USART2, LL_USART_DATAWIDTH_8B);
 //    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);
    LL_USART_SetBaudRate(USART2, SystemCoreClock/2, 115200); // This is not great, let's hope it's like this on all platforms...
    LL_USART_Enable(USART2);
 }
 /** 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;
 }
 #else
 // No-uart variant
 void DebugUart_Init(void) {}
 ssize_t _write_r(struct _reent *rptr, int fd, const void *buf, size_t len) {}
 #endif //USE_DEBUG_UART
--- a/platform/debug_uart.h
+++ b/platform/debug_uart.h
@ -0,0 +1,11 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #ifndef GEX_DEBUG_UART_H
 #define GEX_DEBUG_UART_H
 void DebugUart_PreInit(void);
 void DebugUart_Init(void);
 #endif //GEX_DEBUG_UART_H
--- a/platform/lock_jumper.c
+++ b/platform/lock_jumper.c
@ -0,0 +1,86 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #include <sched_queue.h>
 #include <task_sched.h>
 #include "usbd_core.h"
 #include "USB/usb_device.h"
 #include "framework/settings.h"
 #include "framework/resources.h"
 #include "framework/system_settings.h"
 #include "pin_utils.h"
 #include "lock_jumper.h"
 static GPIO_TypeDef *lock_periph;
 static uint32_t lock_llpin;
 // We use macros LOCK_JUMPER_PORT, LOCK_JUMPER_PIN from plat_compat.h
 /** Init the jumper subsystem */
 void LockJumper_Init(void)
 {
    bool suc = true;
    // Resolve and claim resource
    Resource rsc = plat_pin2resource(LOCK_JUMPER_PORT, LOCK_JUMPER_PIN, &suc);
    assert_param(suc);
    suc &= rsc_claim(&UNIT_SYSTEM, rsc);
    assert_param(suc);
    // Resolve pin
    lock_periph = plat_port2periph(LOCK_JUMPER_PORT, &suc);
    lock_llpin = plat_pin2ll(LOCK_JUMPER_PIN, &suc);
    assert_param(suc);
    // Configure for input
    LL_GPIO_SetPinMode(lock_periph, lock_llpin, LL_GPIO_MODE_INPUT);
    LL_GPIO_SetPinPull(lock_periph, lock_llpin, LL_GPIO_PULL_UP);
    SystemSettings.editable = (bool) LL_GPIO_IsInputPinSet(lock_periph, lock_llpin);
    dbg("Settings editable? %d", SystemSettings.editable);
 }
 /** Handle jumper state change */
 static void jumper_changed(void)
 {
    if (SystemSettings.editable) {
        // Unlock
        dbg("LOCK jumper removed, enabling MSC!");
    } else {
        // Lock
        dbg("LOCK jumper replaced, saving to Flash & disabling MSC!");
        if (SystemSettings.modified) {
            settings_save();
        }
    }
    plat_usb_reconnect();
 }
 /** Periodic jumper check */
 void LockJumper_Check(void)
 {
    // Debounce cooldown
    static uint32_t cooldown = 0;
    if (cooldown > 0) {
        cooldown--;
        return;
    }
    // Read the pin state
    bool old = SystemSettings.editable;
    SystemSettings.editable = (bool) LL_GPIO_IsInputPinSet(lock_periph, lock_llpin);
    if (old != SystemSettings.editable) {
        // --- State changed ---
        cooldown = 5; // 0.5s if called every 100 ms
        jumper_changed();
    }
 }
--- a/platform/lock_jumper.h
+++ b/platform/lock_jumper.h
@ -0,0 +1,20 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #ifndef GEX_LOCK_JUMPER_H
 #define GEX_LOCK_JUMPER_H
 #include "plat_compat.h"
 /**
 * Init the lock jumper subsystem
 */
 void LockJumper_Init(void);
 /**
 * Check state of the lock jumper
 */
 void LockJumper_Check(void);
 #endif //GEX_LOCK_JUMPER_H
--- a/platform/pin_utils.c
+++ b/platform/pin_utils.c
@ -0,0 +1,90 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #include "pin_utils.h"
 #define PINS_COUNT 16
 /** Pin number to LL bitfield mapping */
 static const uint32_t ll_pins[PINS_COUNT] = {
    LL_GPIO_PIN_0,
    LL_GPIO_PIN_1,
    LL_GPIO_PIN_2,
    LL_GPIO_PIN_3,
    LL_GPIO_PIN_4,
    LL_GPIO_PIN_5,
    LL_GPIO_PIN_6,
    LL_GPIO_PIN_7,
    LL_GPIO_PIN_8,
    LL_GPIO_PIN_9,
    LL_GPIO_PIN_10,
    LL_GPIO_PIN_11,
    LL_GPIO_PIN_12,
    LL_GPIO_PIN_13,
    LL_GPIO_PIN_14,
    LL_GPIO_PIN_15,
 };
 /** Port number (A=0) to config struct pointer mapping */
 static GPIO_TypeDef * const port_periphs[PORTS_COUNT] = {
    GPIOA,
    GPIOB,
    GPIOC,
    GPIOD,
    GPIOE,
 };
 /** Convert pin number to LL bitfield */
 uint32_t plat_pin2ll(uint8_t pin_number, bool *suc)
 {
    assert_param(suc != NULL);
    if(pin_number >= PINS_COUNT) {
        dbg("Bad pin: %d", pin_number);
        // TODO proper report
        *suc = false;
        return 0;
    }
    return ll_pins[pin_number];
 }
 /** Convert port name (A,B,C...) to peripheral struct pointer */
 GPIO_TypeDef *plat_port2periph(char port_name, bool *suc)
 {
    assert_param(suc != NULL);
    if(port_name < 'A' || port_name >= ('A'+PORTS_COUNT)) {
        dbg("Bad port: %c", port_name);
        // TODO proper report
        *suc = false;
        return NULL;
    }
    uint8_t num = (uint8_t) (port_name - 'A');
    return port_periphs[num];
 }
 /** Convert a pin to resource handle */
 Resource plat_pin2resource(char port_name, uint8_t pin_number, bool *suc)
 {
    assert_param(suc != NULL);
    if(port_name < 'A' || port_name >= ('A'+PORTS_COUNT)) {
        dbg("Bad port: %c", port_name);
        // TODO proper report
        *suc = false;
        return R_NONE;
    }
    if(pin_number >= PINS_COUNT) {
        // TODO proper report
        dbg("Bad pin: %d", pin_number);
        *suc = false;
        return R_NONE;
    }
    uint8_t num = (uint8_t) (port_name - 'A');
    return R_PA0 + num*16 + pin_number;
 }
--- a/platform/pin_utils.h
+++ b/platform/pin_utils.h
@ -0,0 +1,41 @@
 //
 // Utilities for parsing pins from settings to LL and resources
 //
 // Created by MightyPork on 2017/12/08.
 //
 #ifndef GEX_PIN_UTILS_H
 #define GEX_PIN_UTILS_H
 #include "platform.h"
 #include "resources.h"
 /**
 * Convert pin number to LL driver bitfield for working with the pin.
 *
 * @param pin_number - number 0..15
 * @param suc - set to false on failure, left unchanged on success.
 * @return LL_GPIO_PIN_x
 */
 uint32_t plat_pin2ll(uint8_t pin_number, bool *suc);
 /**
 * Convert pin name and number to a resource enum
 *
 * @param port_name - char 'A'..'Z'
 * @param pin_number - number 0..15
 * @param suc - set to false on failure, left unchanged on success
 * @return the resource, or R_NONE
 */
 Resource plat_pin2resource(char port_name, uint8_t pin_number, bool *suc);
 /**
 * Convert port name to peripheral instance
 *
 * @param port_name - char 'A'..'Z'
 * @param suc - set to false on failure, left unchanged on success.
 * @return instance
 */
 GPIO_TypeDef *plat_port2periph(char port_name, bool *suc);
 #endif //GEX_PIN_UTILS_H
--- a/platform/plat_compat.h
+++ b/platform/plat_compat.h
@ -0,0 +1,52 @@
 //
 // Created by MightyPork on 2017/12/08.
 //
 #ifndef GEX_PLAT_COMPAT_H
 #define GEX_PLAT_COMPAT_H
 // platform name for the version string
 #define GEX_PLATFORM "STM32F103"
 #include <stm32f1xx.h>
 #include <stm32f1xx_hal.h>
 #include <stm32f1xx_ll_adc.h>
 #include <stm32f1xx_ll_bus.h>
 #include <stm32f1xx_ll_cortex.h>
 #include <stm32f1xx_ll_crc.h>
 #include <stm32f1xx_ll_dac.h>
 #include <stm32f1xx_ll_dma.h>
 #include <stm32f1xx_ll_exti.h>
 #include <stm32f1xx_ll_fsmc.h>
 #include <stm32f1xx_ll_gpio.h>
 #include <stm32f1xx_ll_i2c.h>
 #include <stm32f1xx_ll_iwdg.h>
 #include <stm32f1xx_ll_pwr.h>
 #include <stm32f1xx_ll_rcc.h>
 #include <stm32f1xx_ll_rtc.h>
 #include <stm32f1xx_ll_sdmmc.h>
 #include <stm32f1xx_ll_spi.h>
 #include <stm32f1xx_ll_system.h>
 #include <stm32f1xx_ll_tim.h>
 #include <stm32f1xx_ll_usart.h>
 #include <stm32f1xx_ll_usb.h>
 #include <stm32f1xx_ll_utils.h>
 #include <stm32f1xx_ll_wwdg.h>
 // size, determines position of the flash storage
 #define FLASH_SIZE (64*1024)
 #define SETTINGS_BLOCK_SIZE (1024*2) // this must be a multiple of FLASH pages
 #define SETTINGS_FLASH_ADDR (0x08000000 + FLASH_SIZE - SETTINGS_BLOCK_SIZE)
 // Number of GPIO ports A,B,C...
 #define PORTS_COUNT 5
 // Lock jumper config
 #define LOCK_JUMPER_PORT 'C'
 #define LOCK_JUMPER_PIN  14
 // Status LED config
 #define STATUS_LED_PORT 'C'
 #define STATUS_LED_PIN  13
 #endif //GEX_PLAT_COMPAT_H
--- a/platform/plat_init.c
+++ b/platform/plat_init.c
@ -0,0 +1,37 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #include "platform.h"
 #include "comm/messages.h"
 #include "framework/resources.h"
 #include "framework/settings.h"
 #include "framework/system_settings.h"
 #include "lock_jumper.h"
 #include "status_led.h"
 #include "debug_uart.h"
 void plat_init(void)
 {
    // Load system defaults
    systemsettings_init();
    dbg("Setting up resources ...");
    rsc_init_registry();
    plat_init_resources();
    LockJumper_Init();
    StatusLed_Init();
    DebugUart_Init(); // <- only the resource claim
    dbg("Registering platform units ...");
    // All user-configurable units are now added to the repository
    plat_register_units();
    dbg("Loading settings ...");
    // Load settings from Flash and apply (includes System settings and all Unit settings)
    settings_load();
    comm_init();
 }
--- a/platform/platform.c
+++ b/platform/platform.c
@ -0,0 +1,87 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #include "platform.h"
 #include "usbd_core.h"
 #include "USB/usb_device.h"
 #include "framework/resources.h"
 // ----- SUPPORTED UNITS -----
 #include "framework/unit_registry.h"
 #include "units/pin/unit_pin.h"
 #include "units/neopixel/unit_neopixel.h"
 void plat_register_units(void)
 {
    ureg_add_type(&UNIT_PIN);
    ureg_add_type(&UNIT_NEOPIXEL);
 }
 // ----- RELEASE AVAILABLE RESOURCES -----
 void plat_init_resources(void)
 {
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOD_CLK_ENABLE();
    __HAL_RCC_GPIOE_CLK_ENABLE();
    // Platform F103C8T6 - free all present resources
    {
        rsc_free(NULL, R_ADC1);
        rsc_free(NULL, R_ADC2);
        rsc_free(NULL, R_I2C1);
        rsc_free(NULL, R_I2C2);
        rsc_free(NULL, R_SPI1);
        rsc_free(NULL, R_SPI2);
        rsc_free(NULL, R_TIM1);
        rsc_free(NULL, R_TIM2);
        rsc_free(NULL, R_TIM3);
        rsc_free(NULL, R_TIM4);
        rsc_free(NULL, R_USART1);
        rsc_free(NULL, R_USART2);
        rsc_free(NULL, R_USART3);
        rsc_free_range(NULL, R_PA0, R_PA15);
        rsc_free_range(NULL, R_PB0, R_PB15);
        rsc_free_range(NULL, R_PC13, R_PC15);
        rsc_free_range(NULL, R_PD0, R_PD1);
    }
    // Claim resources not available due to board layout or internal usage
    {
        bool ok = true;
        // HAL timebase
        ok &= rsc_claim(&UNIT_SYSTEM, R_TIM1);
        // HSE crystal
        ok &= rsc_claim(&UNIT_SYSTEM, R_PD0);
        ok &= rsc_claim(&UNIT_SYSTEM, R_PD1);
        // SWD
        ok &= rsc_claim(&UNIT_SYSTEM, R_PA13);
        ok &= rsc_claim(&UNIT_SYSTEM, R_PA14);
        // USB
        ok &= rsc_claim(&UNIT_SYSTEM, R_PA11);
        ok &= rsc_claim(&UNIT_SYSTEM, R_PA12);
        // BOOT pin(s)
        ok &= rsc_claim(&UNIT_SYSTEM, R_PB2); // BOOT1
        assert_param(ok);
    }
 }
 // ---- USB reconnect ----
 /** USB re-connect */
 void plat_usb_reconnect(void)
 {
    // F103 doesn't have pull-up control, this is probably the best we can do
    USBD_LL_Reset(&hUsbDeviceFS);
 }
--- a/platform/platform.h
+++ b/platform/platform.h
@ -0,0 +1,59 @@
 //
 // Created by MightyPork on 2017/12/08.
 //
 #ifndef GEX_PLATFORM_H
 #define GEX_PLATFORM_H
 #include <inttypes.h>
 #include <stdint.h>
 #include <stdarg.h>
 #include <stdbool.h>
 #include <inttypes.h>
 #include <stddef.h>
 #include <string.h>
 #include <malloc.h>
 // FreeRTOS includes
 #include <cmsis_os.h>
 // platform-specific stuff (includes stm32 driver headers)
 #include "plat_compat.h"
 // assert_param, trap...
 #include "stm32_assert.h"
 // inIRQ etc
 #include "cortex_utils.h"
 // MIN, MAX, static assert etc
 #include "macro.h"
 // smaller replacement for regular snprintf - SNPRINTF
 #include "snprintf.h"
 // debug logging
 #include "debug.h"
 // error codes and strings
 #include "utils/error.h"
 // GEX version string
 #include "version.h"
 // ---
 /**
 * Init the platform
 */
 void plat_init(void);
 /**
 * Init resources available for this platform
 */
 void plat_init_resources(void);
 /**
 * Register units available for this platform / build
 */
 void plat_register_units(void);
 /**
 * Re-connect the USB, triggering descriptors reload.
 * Use the DPPU bit on USB_BCDR, if available.
 */
 void plat_usb_reconnect(void);
 #endif //GEX_PLATFORM_H
--- a/platform/status_led.c
+++ b/platform/status_led.c
@ -0,0 +1,90 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #include "platform.h"
 #include "framework/resources.h"
 #include "status_led.h"
 #include "pin_utils.h"
 static uint32_t indicators[_INDICATOR_COUNT];
 static GPIO_TypeDef *led_periph;
 static uint32_t led_llpin;
 /** Set up the LED */
 void StatusLed_Init(void)
 {
    bool suc = true;
    // Resolve and claim resource
    Resource rsc = plat_pin2resource(STATUS_LED_PORT, STATUS_LED_PIN, &suc);
    assert_param(suc);
    suc &= rsc_claim(&UNIT_SYSTEM, rsc);
    assert_param(suc);
    // Resolve pin
    led_periph = plat_port2periph(STATUS_LED_PORT, &suc);
    led_llpin = plat_pin2ll(STATUS_LED_PIN, &suc);
    assert_param(suc);
    // Configure for output
    LL_GPIO_SetPinMode(led_periph, led_llpin, LL_GPIO_MODE_OUTPUT);
    LL_GPIO_SetPinOutputType(led_periph, led_llpin, LL_GPIO_OUTPUT_PUSHPULL);
    LL_GPIO_SetPinSpeed(led_periph, led_llpin, LL_GPIO_SPEED_FREQ_LOW);
 }
 /** Set indicator ON */
 void StatusLed_On(enum GEX_StatusIndicator indicator)
 {
    indicators[indicator] = osWaitForever;
    if (indicator == STATUS_FAULT) {
        // Persistent light
        GPIOC->ODR |= 1<<13;
    }
 }
 /** Set indicator OFF */
 void StatusLed_Off(enum GEX_StatusIndicator indicator)
 {
    indicators[indicator] = 0;
    // TODO some effect
 }
 /** Set or reset a indicator */
 void StatusLed_Set(enum GEX_StatusIndicator indicator, bool set)
 {
    if (set) {
        StatusLed_On(indicator);
    } else {
        StatusLed_Off(indicator);
    }
 }
 /** Turn indicator ON for a given interval */
 void StatusLed_Flash(enum GEX_StatusIndicator indicator, uint32_t ms)
 {
    indicators[indicator] = ms;
    // TODO
 }
 /** Millisecond tick */
 void StatusLed_Tick(void)
 {
    for (uint32_t i = 0; i < _INDICATOR_COUNT; i++) {
        if (indicators[i] != osWaitForever && indicators[i] != 0) {
            if (--indicators[i]) {
                StatusLed_Off((enum GEX_StatusIndicator) i);
            }
        }
    }
 }
 /** Heartbeat callback from the main thread */
 void StatusLed_Heartbeat(void)
 {
    // TODO fixme
    GPIOC->ODR ^= 1<<13;
 }
--- a/platform/status_led.h
+++ b/platform/status_led.h
@ -0,0 +1,67 @@
 //
 // Created by MightyPork on 2017/12/15.
 //
 #ifndef GEX_INDICATORS_H
 #define GEX_INDICATORS_H
 #include "platform.h"
 /**
 * Indicator (LED or blinking pattern)
 */
 enum GEX_StatusIndicator {
    STATUS_FAULT = 0,
    STATUS_USB_CONN,
    STATUS_USB_ACTIVITY,
    STATUS_DISK_BUSY,
    STATUS_DISK_ATTACHED,
    _INDICATOR_COUNT
 };
 /**
 * Initialize the statis LED(s)
 */
 void StatusLed_Init(void);
 /**
 * Set indicator ON
 *
 * @param indicator
 */
 void StatusLed_On(enum GEX_StatusIndicator indicator);
 /**
 * Set indicator OFF
 *
 * @param indicator
 */
 void StatusLed_Off(enum GEX_StatusIndicator indicator);
 /**
 * Indicator set or reset
 *
 * @param indicator
 * @param set
 */
 void StatusLed_Set(enum GEX_StatusIndicator indicator, bool set);
 /**
 * Turn indicator ON for a given interval
 *
 * @param indicator
 * @param ms - time ON in ms
 */
 void StatusLed_Flash(enum GEX_StatusIndicator indicator, uint32_t ms);
 /**
 * Ms tick for indicators
 */
 void StatusLed_Tick(void);
 /**
 * Heartbeat callback from the main thread to indicate activity
 */
 void StatusLed_Heartbeat(void);
 #endif //GEX_INDICATORS_H
--- a/sched_queue.h
+++ b/sched_queue.h
@ -0,0 +1,32 @@
 //
 // Created by MightyPork on 2017/11/21.
 //
 #ifndef GEX_SCHED_QUEUE_H
 #define GEX_SCHED_QUEUE_H
 #include <TinyFrame/TinyFrame.h>
 typedef struct sched_que_item Job;
 typedef void (*ScheduledJobCb) (Job *job);
 struct sched_que_item {
    ScheduledJobCb cb;
    union {
        TF_ID frame_id;
        void *data1;
    };
    union {
        uint32_t d32;
        uint8_t *buf;
        const uint8_t *cbuf;
        const char *str;
        void *data2;
    };
    union {
        uint32_t len;
        void *data3;
    };
 };
 #endif //GEX_SCHED_QUEUE_H
--- a/stm32_assert.c
+++ b/stm32_assert.c
@ -0,0 +1,38 @@
 #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)
 {
    dbg("\r\n\033[31m%s:\033[m %s:%"PRIu32"\r\n", msg, filename, line);
    vPortEnterCritical();
    StatusLed_On(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)
 {
    dbg("\r\n\033[33m%s:\033[m %s:%"PRIu32"\r\n", msg, filename, line);
 }
 /**
   * @brief Reports the name of the source file and the source line number
   * where the assert_param error has occurred.
   * @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)
 {
    abort_msg("ASSERT FAILED", file, line);
 }
--- a/stm32_assert.h
+++ b/stm32_assert.h
@ -0,0 +1,36 @@
 //
 // Created by MightyPork on 2017/11/20.
 //
 #ifndef STM32_ASSERT_H
 #define STM32_ASSERT_H
 #include <stdint.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);
 #if USE_FULL_ASSERT
    #if VERBOSE_ASSERT
        // 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__)
    #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__)
    #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)
    #define assert_param(expression) do { (void)(expression); } while(0)
    #define assert_warn(expression, msg) do { (void)(expression); } while(0)
    #define _Error_Handler(file, line) do {} while(1)
 #endif
 #endif //STM32_ASSERT_H
--- a/task_main.c
+++ b/task_main.c
@ -0,0 +1,84 @@
 //
 // Created by MightyPork on 2017/11/09.
 //
 #include "platform.h"
 #include "platform/lock_jumper.h"
 #include "status_led.h"
 #include "utils/stacksmon.h"
 #include "vfs/vfs_manager.h"
 #include "usbd_cdc.h"
 #include "usb_device.h"
 #include "usbd_msc.h"
 #include "task_main.h"
 extern void plat_init(void);
 /* TaskUsbEvent function */
 void TaskMain(void const * argument)
 {
    dbg("> Main task started!");
    vfs_if_usbd_msc_init();
    vfs_mngr_init(1);
    uint32_t startTime = xTaskGetTickCount();
    uint32_t cnt = 1;
    while(1) {
        uint32_t msg;
        xTaskNotifyWait(0, UINT32_MAX, &msg, 100); // time out if nothing happened
        // periodic updates to the VFS driver
        uint32_t now = xTaskGetTickCount();
        uint32_t elapsed = now - startTime;
        if (elapsed >= 100) {
            // interval 100ms or more - slow periodic
            vfs_mngr_periodic(elapsed);
            LockJumper_Check();
            startTime = now;
            cnt++;
            if (cnt%5==0) StatusLed_Heartbeat();
        }
        // if no message and it just timed out, go wait some more...
        if (msg == 0) {
            // Low priority periodic tasks... (TODO move to a timer?)
            // Periodically check stacks for overrun
            stackmon_check_canaries();
            // Periodically dump all stacks - for checking levels before critical (to reduce size if not needed)
            if ((cnt%50)==0) stackmon_dump();
            continue;
        }
        // Endpoint 0 - control messages for the different classes
        if (msg & USBEVT_FLAG_EP0_RX_RDY) {
            USBD_CDC_EP0_RxReady(&hUsbDeviceFS);
        }
        if (msg & USBEVT_FLAG_EP0_TX_SENT) {
            //
        }
        // MSC - read/write etc
        if (msg & (USBEVT_FLAG_EPx_IN(MSC_EPIN_ADDR))) {
            USBD_MSC_DataIn(&hUsbDeviceFS, MSC_EPIN_ADDR);
        }
        if (msg & (USBEVT_FLAG_EPx_OUT(MSC_EPOUT_ADDR))) {
            USBD_MSC_DataOut(&hUsbDeviceFS, MSC_EPOUT_ADDR);
        }
        // CDC - config packets and data in/out
        if (msg & (USBEVT_FLAG_EPx_IN(CDC_IN_EP))) {
            USBD_CDC_DataIn(&hUsbDeviceFS, CDC_IN_EP);
        }
        if (msg & (USBEVT_FLAG_EPx_IN(CDC_CMD_EP))) {
            USBD_CDC_DataIn(&hUsbDeviceFS, CDC_CMD_EP);
        }
        if (msg & (USBEVT_FLAG_EPx_OUT(CDC_OUT_EP))) {
            USBD_CDC_DataOut(&hUsbDeviceFS, CDC_OUT_EP);
        }
    }
 }
--- a/task_main.h
+++ b/task_main.h
@ -0,0 +1,19 @@
 //
 // Created by MightyPork on 2017/11/09.
 //
 #ifndef GEX_TASK_MAIN_H
 #define GEX_TASK_MAIN_H
 #include "platform.h"
 extern osThreadId tskMainHandle;
 void TaskMain(const void *argument);
 // Notify flags:
 #define USBEVT_FLAG_EPx_IN(ep) (uint32_t)(1<<((ep)&0x7))
 #define USBEVT_FLAG_EPx_OUT(ep) (uint32_t)(1<<(8 + ((ep)&0x7)))
 #define USBEVT_FLAG_EP0_RX_RDY (1<<16)
 #define USBEVT_FLAG_EP0_TX_SENT (1<<17)
 #endif //GEX_TASK_MAIN_H
--- a/task_sched.c
+++ b/task_sched.c
@ -0,0 +1,89 @@
 //
 // Created by MightyPork on 2017/11/21.
 //
 #include "platform.h"
 #include "task_sched.h"
 extern osMessageQId queSchedLPHandle;
 extern osMessageQId queSchedHPHandle;
 volatile uint32_t jobQueHighWaterMarkHP = 0;
 volatile uint32_t jobQueHighWaterMarkLP = 0;
 /**
 * Schedule a function for later execution in the jobs thread
 *
 * @param callback - the callback function
 * @param prio - priority, selects which job queue to use
 * @param data1 - first data object, NULL if not needed; usually context/handle
 * @param data2 - second data object, NULL if not needed; usually data/argument
 */
 void scheduleJob(Job *job, enum task_sched_prio prio)
 {
    QueueHandle_t que = (prio == TSK_SCHED_LOW ? queSchedLPHandle : queSchedHPHandle);
    assert_param(que != NULL);
    assert_param(job->cb != NULL);
    uint32_t count;
    if (inIRQ()) {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        assert_param(pdPASS == xQueueSendFromISR(que, job, &xHigherPriorityTaskWoken));
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 #if USE_STACK_MONITOR
        count = (uint32_t) uxQueueMessagesWaitingFromISR(que);
 #endif
    } else {
        assert_param(pdPASS == xQueueSend(que, job, 100));
 #if USE_STACK_MONITOR
        count = (uint32_t) uxQueueMessagesWaiting(que);
 #endif
    }
 #if USE_STACK_MONITOR
    if (prio == TSK_SCHED_LOW) {
        jobQueHighWaterMarkLP = MAX(jobQueHighWaterMarkLP, count);
    } else {
        jobQueHighWaterMarkHP = MAX(jobQueHighWaterMarkHP, count);
    }
 #endif
 }
 /**
 * Low priority task queue handler
 *
 * @param argument
 */
 void TaskSchedLP (const void * argument)
 {
    dbg("> Low priority queue task started!");
    struct sched_que_item job;
    while (1) {
        xQueueReceive(queSchedLPHandle, &job, osWaitForever);
        assert_param(job.cb != NULL);
        job.cb(&job);
    }
 }
 /**
 * High priority task queue handler
 *
 * @param argument
 */
 void TaskSchedHP (const void * argument)
 {
    dbg("> High priority queue task started!");
    struct sched_que_item job;
    while (1) {
        xQueueReceive(queSchedHPHandle, &job, osWaitForever);
        assert_param(job.cb != NULL);
        job.cb(&job);
    }
 }
--- a/task_sched.h
+++ b/task_sched.h
@ -0,0 +1,29 @@
 //
 // Created by MightyPork on 2017/11/21.
 //
 #ifndef GEX_TASK_SCHED_H
 #define GEX_TASK_SCHED_H
 #include "platform.h"
 #include "sched_queue.h"
 enum task_sched_prio {
    TSK_SCHED_LOW = 0,
    TSK_SCHED_HIGH = 1,
 };
 #if USE_STACK_MONITOR
 extern volatile uint32_t jobQueHighWaterMarkHP;
 extern volatile uint32_t jobQueHighWaterMarkLP;
 #endif
 extern osThreadId tskSchedLPHandle;
 void TaskSchedLP (const void * argument);
 extern osThreadId tskSchedHPHandle;
 void TaskSchedHP (const void * argument);
 void scheduleJob(Job *job, enum task_sched_prio prio);
 #endif //GEX_TASK_SCHED_H
--- a/units/neopixel/unit_neopixel.c
+++ b/units/neopixel/unit_neopixel.c
@ -0,0 +1,204 @@
 //
 // Created by MightyPork on 2017/11/25.
 //
 #include "utils/avrlibc.h"
 #include "comm/messages.h"
 #include "unit_base.h"
 #include "unit_neopixel.h"
 #include "ws2812.h"
 /** Private data structure */
 struct priv {
    char port_name;
    uint8_t pin_number;
    uint16_t pixels;
    uint32_t ll_pin;
    GPIO_TypeDef *port;
 };
 // ------------------------------------------------------------------------
 /** Load from a binary buffer stored in Flash */
 static void Npx_loadBinary(Unit *unit, PayloadParser *pp)
 {
    struct priv *priv = unit->data;
    priv->port_name = pp_char(pp);
    priv->pin_number = pp_u8(pp);
    priv->pixels = pp_u16(pp);
 }
 /** Write to a binary buffer for storing in Flash */
 static void Npx_writeBinary(Unit *unit, PayloadBuilder *pb)
 {
    struct priv *priv = unit->data;
    pb_char(pb, priv->port_name);
    pb_u8(pb, priv->pin_number);
    pb_u16(pb, priv->pixels);
 }
 // ------------------------------------------------------------------------
 /** Parse a key-value pair from the INI file */
 static bool Npx_loadIni(Unit *unit, const char *key, const char *value)
 {
    bool suc = true;
    struct priv *priv = unit->data;
    if (streq(key, "pin")) {
        suc = str_parse_pin(value, &priv->port_name, &priv->pin_number);
    }
    else if (streq(key, "pixels")) {
        priv->pixels = (uint16_t) avr_atoi(value);
    }
    else {
        return false;
    }
    return suc;
 }
 /** Generate INI file section for the unit */
 static void Npx_writeIni(Unit *unit, IniWriter *iw)
 {
    struct priv *priv = unit->data;
    iw_comment(iw, "Data pin");
    iw_entry(iw, "pin", "%c%d", priv->port_name,  priv->pin_number);
    iw_comment(iw, "Number of pixels");
    iw_entry(iw, "pixels", "%d", priv->pixels);
 }
 // ------------------------------------------------------------------------
 /** Allocate data structure and set defaults */
 static bool Npx_preInit(Unit *unit)
 {
    bool suc = true;
    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
    CHECK_SUC();
    // some defaults
    priv->pin_number = 0;
    priv->port_name = 'A';
    priv->pixels = 1;
    return true;
 }
 /** Finalize unit set-up */
 static bool Npx_init(Unit *unit)
 {
    bool suc = true;
    struct priv *priv = unit->data;
    // --- Parse config ---
    priv->ll_pin = plat_pin2ll(priv->pin_number, &suc);
    priv->port = plat_port2periph(priv->port_name, &suc);
    Resource rsc = plat_pin2resource(priv->port_name, priv->pin_number, &suc);
    if (!suc) {
        unit->status = E_BAD_CONFIG;
        return false;
    }
    // --- Claim resources ---
    if (!rsc_claim(unit, rsc)) return false;
    // --- Init hardware ---
    LL_GPIO_SetPinMode(priv->port, priv->ll_pin, LL_GPIO_MODE_OUTPUT);
    LL_GPIO_SetPinOutputType(priv->port, priv->ll_pin, LL_GPIO_OUTPUT_PUSHPULL);
    LL_GPIO_SetPinSpeed(priv->port, priv->ll_pin, LL_GPIO_SPEED_FREQ_HIGH);
    // clear strip
    ws2812_clear(priv->port, priv->ll_pin, priv->pixels);
    return true;
 }
 /** Tear down the unit */
 static void Npx_deInit(Unit *unit)
 {
    struct priv *priv = unit->data;
    // configure the pin as analog
    LL_GPIO_SetPinMode(priv->port, priv->ll_pin, LL_GPIO_MODE_ANALOG);
    // Release all resources
    rsc_teardown(unit);
    // Free memory
    free(unit->data);
    unit->data = NULL;
 }
 // ------------------------------------------------------------------------
 enum PinCmd_ {
    CMD_CLEAR = 0,
    CMD_LOAD = 1,
    CMD_LOAD_U32_LE = 2,
    CMD_LOAD_U32_BE = 3,
 };
 /** Handle a request message */
 static bool Npx_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, PayloadParser *pp)
 {
    (void)pp;
    struct priv *priv = unit->data;
    switch (command) {
        case CMD_CLEAR:
            ws2812_clear(priv->port, priv->ll_pin, priv->pixels);
            break;
        case CMD_LOAD:
            if (pp_length(pp) != priv->pixels*3) goto bad_count;
            ws2812_load_raw(priv->port, priv->ll_pin, pp->current, priv->pixels);
            break;
        case CMD_LOAD_U32_LE:
            if (pp_length(pp) != priv->pixels*4) goto bad_count;
            ws2812_load_sparse(priv->port, priv->ll_pin, pp->current, priv->pixels, 0);
            break;
        case CMD_LOAD_U32_BE:
            if (pp_length(pp) != priv->pixels*4) goto bad_count;
            ws2812_load_sparse(priv->port, priv->ll_pin, pp->current, priv->pixels, 1);
            break;
        default:
            sched_respond_bad_cmd(frame_id);
            return false;
    }
    return true;
 bad_count:
    sched_respond_err(frame_id, "BAD PIXEL COUNT");
    return false;
 }
 // ------------------------------------------------------------------------
 /** Unit template */
 const UnitDriver UNIT_NEOPIXEL = {
    .name = "NEOPIXEL",
    .description = "Neopixel RGB LED strip",
    // Settings
    .preInit = Npx_preInit,
    .cfgLoadBinary = Npx_loadBinary,
    .cfgWriteBinary = Npx_writeBinary,
    .cfgLoadIni = Npx_loadIni,
    .cfgWriteIni = Npx_writeIni,
    // Init
    .init = Npx_init,
    .deInit = Npx_deInit,
    // Function
    .handleRequest = Npx_handleRequest,
 };
--- a/units/neopixel/unit_neopixel.h
+++ b/units/neopixel/unit_neopixel.h
@ -0,0 +1,12 @@
 //
 // Created by MightyPork on 2017/11/25.
 //
 #ifndef U_NEOPIXEL_H
 #define U_NEOPIXEL_H
 #include "unit.h"
 extern const UnitDriver UNIT_NEOPIXEL;
 #endif //U_NEOPIXEL_H
--- a/units/neopixel/ws2812.c
+++ b/units/neopixel/ws2812.c
@ -0,0 +1,95 @@
 #include "platform.h"
 #include "ws2812.h"
 static //inline __attribute__((always_inline))
 void ws2812_byte(GPIO_TypeDef *port, uint32_t ll_pin, uint8_t b)
 {
    __disable_irq();
 	for (register volatile uint8_t i = 0; i < 8; i++) {
        LL_GPIO_SetOutputPin(port, ll_pin);
 		// duty cycle determines bit value
 		if (b & 0x80) {
            for(uint32_t _i = 0; _i < 10; _i++) asm volatile("nop");
            LL_GPIO_ResetOutputPin(port, ll_pin);
            for(uint32_t _i = 0; _i < 10; _i++) asm volatile("nop");
 		} else {
            for(uint32_t _i = 0; _i < 5; _i++) asm volatile("nop");
            LL_GPIO_ResetOutputPin(port, ll_pin);
            for(uint32_t _i = 0; _i < 10; _i++) asm volatile("nop");
 		}
 		b <<= 1; // shift to next bit
 	}
    __enable_irq();
 }
 /** Set many RGBs */
 void ws2812_load(GPIO_TypeDef *port, uint32_t ll_pin, uint32_t *rgbs, uint32_t count)
 {
    vPortEnterCritical();
 	for (uint32_t i = 0; i < count; i++) {
 		uint32_t rgb = *rgbs++;
        ws2812_byte(port, ll_pin, rgb_g(rgb));
        ws2812_byte(port, ll_pin, rgb_r(rgb));
        ws2812_byte(port, ll_pin, rgb_b(rgb));
 	}
    vPortExitCritical();
 	// TODO: Delay 50 us
 }
 /** Set many RGBs from packed stream */
 void ws2812_load_raw(GPIO_TypeDef *port, uint32_t ll_pin, uint8_t *rgbs, uint32_t count)
 {
    vPortEnterCritical();
    uint8_t b, g, r;
 	for (uint32_t i = 0; i < count; i++) {
 		r = *rgbs++;
 		g = *rgbs++;
 		b = *rgbs++;
 		ws2812_byte(port, ll_pin, g);
 		ws2812_byte(port, ll_pin, r);
 		ws2812_byte(port, ll_pin, b);
 	}
    vPortExitCritical();
 	// TODO: Delay 50 us
 }
 /** Set many RGBs from uint32 stream */
 void ws2812_load_sparse(GPIO_TypeDef *port, uint32_t ll_pin, uint8_t *rgbs, uint32_t count, bool bigendian)
 {
    vPortEnterCritical();
    uint8_t b, g, r;
 	for (uint32_t i = 0; i < count; i++) {
        if (bigendian) {
            rgbs++; // skip
            b = *rgbs++;
            g = *rgbs++;
            r = *rgbs++;
        } else {
            r = *rgbs++;
            g = *rgbs++;
            b = *rgbs++;
            rgbs++; // skip
        }
 		ws2812_byte(port, ll_pin, g);
 		ws2812_byte(port, ll_pin, r);
 		ws2812_byte(port, ll_pin, b);
 	}
    vPortExitCritical();
 	// TODO: Delay 50 us
 }
 /** Set many RGBs */
 void ws2812_clear(GPIO_TypeDef *port, uint32_t ll_pin, uint32_t count)
 {
    vPortEnterCritical();
 	for (uint32_t i = 0; i < count*3; i++) {
 		ws2812_byte(port, ll_pin, 0);
 	}
    vPortExitCritical();
 	// TODO: Delay 50 us
 }
--- a/units/neopixel/ws2812.h
+++ b/units/neopixel/ws2812.h
@ -0,0 +1,73 @@
 #pragma once
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
 /* Exported types ------------------------------------------------------------*/
 /* Exported constants --------------------------------------------------------*/
 /* Exported macros -----------------------------------------------------------*/
 /**
 * @brief Compose an RGB color.
 * @param r, g, b - components 0xFF
 * @returns integer 0xRRGGBB
 */
 #define rgb(r, g, b) (((0xFF & (r)) << 16) | ((0xFF & (g)) << 8) | (0xFF & (b)))
 /* Get components */
 #define rgb_r(rgb) (uint8_t)(((rgb) >> 16) & 0xFF)
 #define rgb_g(rgb) (uint8_t)(((rgb) >> 8) & 0xFF)
 #define rgb_b(rgb) (uint8_t)((rgb) & 0xFF)
 typedef union {
    struct {
        uint8_t r;
        uint8_t g;
        uint8_t b;
    };
    uint32_t num;
 } xrgb_t;
 /* Exported functions --------------------------------------------------------*/
 /**
 * @brief Set color of multiple chained RGB leds
 *
 * @param port
 * @param ll_pin
 * @param rgbs - array of colors (0xRRGGBB)
 * @param count - number of pixels
 */
 void ws2812_load(GPIO_TypeDef *port, uint32_t ll_pin, uint32_t *rgbs, uint32_t count);
 /**
 * Load RGBs from a packed byte stream
 *
 * @param port
 * @param ll_pin
 * @param rgbs - packed R,G,B, R,G,B, ... array
 * @param count - number of pixels (triplets)
 */
 void ws2812_load_raw(GPIO_TypeDef *port, uint32_t ll_pin, uint8_t *rgbs, uint32_t count);
 /**
 * Load all pixels with BLACK (0,0,0)
 *
 * @param port
 * @param ll_pin
 * @param count - number of pixels
 */
 void ws2812_clear(GPIO_TypeDef *port, uint32_t ll_pin, uint32_t count);
 /**
 * Load from a stream of 32-bit numbers (4th or 1st byte skipped)
 * @param port
 * @param ll_pin
 * @param rgbs - payload
 * @param count - number of pixels
 * @param bigendian - big endian ordering
 */
 void ws2812_load_sparse(GPIO_TypeDef *port, uint32_t ll_pin, uint8_t *rgbs, uint32_t count, bool bigendian);
--- a/units/pin/unit_pin.c
+++ b/units/pin/unit_pin.c
@ -0,0 +1,234 @@
 //
 // Created by MightyPork on 2017/11/25.
 //
 #include "comm/messages.h"
 #include "unit_base.h"
 #include "unit_pin.h"
 /** Private data structure */
 struct priv {
    char port_name;
    uint8_t pin_number;
    bool output;
    bool pull_up;
    bool open_drain;
    uint32_t ll_pin;
    GPIO_TypeDef *port;
 };
 // ------------------------------------------------------------------------
 /** Load from a binary buffer stored in Flash */
 static void Pin_loadBinary(Unit *unit, PayloadParser *pp)
 {
    struct priv *priv = unit->data;
    priv->port_name = pp_char(pp);
    priv->pin_number = pp_u8(pp);
    priv->output = pp_bool(pp);
    priv->pull_up = pp_bool(pp);
    priv->open_drain = pp_bool(pp);
 }
 /** Write to a binary buffer for storing in Flash */
 static void Pin_writeBinary(Unit *unit, PayloadBuilder *pb)
 {
    struct priv *priv = unit->data;
    pb_char(pb, priv->port_name);
    pb_u8(pb, priv->pin_number);
    pb_bool(pb, priv->output);
    pb_bool(pb, priv->pull_up);
    pb_bool(pb, priv->open_drain);
 }
 // ------------------------------------------------------------------------
 /** Parse a key-value pair from the INI file */
 static bool Pin_loadIni(Unit *unit, const char *key, const char *value)
 {
    bool suc = true;
    struct priv *priv = unit->data;
    if (streq(key, "pin")) {
        suc = str_parse_pin(value, &priv->port_name, &priv->pin_number);
    }
    else if (streq(key, "dir")) {
        priv->output = str_parse_01(value, "IN", "OUT", &suc);
    }
    else if (streq(key, "pull")) {
        priv->pull_up = str_parse_01(value, "DOWN", "UP", &suc);
    }
    else if (streq(key, "opendrain")) {
        priv->open_drain = str_parse_yn(value, &suc);
    } else {
        return false;
    }
    return suc;
 }
 /** Generate INI file section for the unit */
 static void Pin_writeIni(Unit *unit, IniWriter *iw)
 {
    struct priv *priv = unit->data;
    iw_comment(iw, "Physical pin");
    iw_entry(iw, "pin", "%c%d", priv->port_name,  priv->pin_number);
    iw_comment(iw, "Direction (IN, OUT)");
    iw_entry(iw, "dir", "%s", str_01(priv->output,  "IN",   "OUT"));
    iw_comment(iw, "Pull resistor, only for input (UP, DOWN)");
    iw_entry(iw, "pull", "%s", str_01(priv->pull_up, "DOWN", "UP"));
    iw_comment(iw, "Open drain, only for output (Y, N)");
    iw_entry(iw, "opendrain", "%s", str_yn(priv->open_drain));
 }
 // ------------------------------------------------------------------------
 /** Allocate data structure and set defaults */
 static bool Pin_preInit(Unit *unit)
 {
    bool suc = true;
    struct priv *priv = unit->data = calloc_ck(1, sizeof(struct priv), &suc);
    CHECK_SUC();
    // some defaults
    priv->pin_number = 0;
    priv->port_name = 'A';
    priv->output = true;
    priv->open_drain = false;
    priv->pull_up = false;
    return true;
 }
 /** Finalize unit set-up */
 static bool Pin_init(Unit *unit)
 {
    bool suc = true;
    struct priv *priv = unit->data;
    // --- Parse config ---
    priv->ll_pin = plat_pin2ll(priv->pin_number, &suc);
    priv->port = plat_port2periph(priv->port_name, &suc);
    Resource rsc = plat_pin2resource(priv->port_name, priv->pin_number, &suc);
    if (!suc) {
        unit->status = E_BAD_CONFIG;
        return false;
    }
    // --- Claim resources ---
    if (!rsc_claim(unit, rsc)) return false;
    // --- Init hardware ---
    LL_GPIO_SetPinMode(priv->port, priv->ll_pin,
        priv->output ? LL_GPIO_MODE_OUTPUT : LL_GPIO_MODE_INPUT);
    LL_GPIO_SetPinOutputType(priv->port, priv->ll_pin,
        priv->open_drain ? LL_GPIO_OUTPUT_OPENDRAIN : LL_GPIO_OUTPUT_PUSHPULL);
    LL_GPIO_SetPinPull(priv->port, priv->ll_pin,
        priv->pull_up ? LL_GPIO_PULL_UP : LL_GPIO_PULL_DOWN);
    LL_GPIO_SetPinSpeed(priv->port, priv->ll_pin,
        LL_GPIO_SPEED_FREQ_HIGH);
    return true;
 }
 /** Tear down the unit */
 static void Pin_deInit(Unit *unit)
 {
    struct priv *priv = unit->data;
    // configure the pin as analog
    LL_GPIO_SetPinMode(priv->port, priv->ll_pin, LL_GPIO_MODE_ANALOG);
    // Release all resources
    rsc_teardown(unit);
    // Free memory
    free(unit->data);
    unit->data = NULL;
 }
 // ------------------------------------------------------------------------
 enum PinCmd_ {
    CMD_CLEAR = 0,
    CMD_SET = 1,
    CMD_TOGGLE = 2,
    CMD_READ = 3,
 };
 /** Handle a request message */
 static bool Pin_handleRequest(Unit *unit, TF_ID frame_id, uint8_t command, PayloadParser *pp)
 {
    (void)pp;
    struct priv *priv = unit->data;
    switch (command) {
        case CMD_CLEAR:
            if (priv->output) {
                LL_GPIO_ResetOutputPin(priv->port, priv->ll_pin);
            } else goto must_be_output;
            break;
        case CMD_SET:
            if (priv->output) {
                LL_GPIO_SetOutputPin(priv->port, priv->ll_pin);
            } else goto must_be_output;
            break;
        case CMD_TOGGLE:
            if (priv->output) {
                LL_GPIO_TogglePin(priv->port, priv->ll_pin);
            } else goto must_be_output;
            break;
        case CMD_READ:
            if (!priv->output) {
                sched_respond_u8(frame_id, (bool) LL_GPIO_IsInputPinSet(priv->port, priv->ll_pin));
            } else goto must_be_input;
            break;
        default:
            sched_respond_bad_cmd(frame_id);
            return false;
    }
    return true;
 must_be_output:
    sched_respond_err(frame_id, "NOT OUTPUT PIN");
    return false;
 must_be_input:
    sched_respond_err(frame_id, "NOT INPUT PIN");
    return false;
 }
 // ------------------------------------------------------------------------
 /** Unit template */
 const UnitDriver UNIT_PIN = {
    .name = "PIN",
    .description = "Single digital I/O pin",
    // Settings
    .preInit = Pin_preInit,
    .cfgLoadBinary = Pin_loadBinary,
    .cfgWriteBinary = Pin_writeBinary,
    .cfgLoadIni = Pin_loadIni,
    .cfgWriteIni = Pin_writeIni,
    // Init
    .init = Pin_init,
    .deInit = Pin_deInit,
    // Function
    .handleRequest = Pin_handleRequest,
 };
--- a/units/pin/unit_pin.h
+++ b/units/pin/unit_pin.h
@ -0,0 +1,12 @@
 //
 // Created by MightyPork on 2017/11/25.
 //
 #ifndef U_PIN_H
 #define U_PIN_H
 #include "unit.h"
 extern const UnitDriver UNIT_PIN;
 #endif //U_PIN_H
--- a/utils/avr_atoi.c
+++ b/utils/avr_atoi.c
@ -0,0 +1,35 @@
 /* Copyright (c) 2002, Marek Michalkiewicz
   All rights reserved.
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:
   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.
   * Neither the name of the copyright holders nor the names of
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE. */
 #include "avrlibc.h"
 int
 avr_atoi(const char *p)
 {
 	return (int) avr_atol(p);
 }
--- a/utils/avr_atol.c
+++ b/utils/avr_atol.c
@ -0,0 +1,36 @@
 /* Copyright (c) 2002, Marek Michalkiewicz
   All rights reserved.
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:
   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.
   * Neither the name of the copyright holders nor the names of
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE. */
 #include <stdlib.h>
 #include "avrlibc.h"
 long
 avr_atol(const char *p)
 {
 	return avr_strtol(p, (char **) NULL, 10);
 }
--- a/utils/avr_strtod.c
+++ b/utils/avr_strtod.c
@ -0,0 +1,220 @@
 /* Copyright (c) 2002-2005  Michael Stumpf  <mistumpf@de.pepperl-fuchs.com>
   Copyright (c) 2006,2008  Dmitry Xmelkov
   All rights reserved.
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:
   * Redistributions of source code must retain the above copyright
     notice, this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright
     notice, this list of conditions and the following disclaimer in
     the documentation and/or other materials provided with the
     distribution.
   * Neither the name of the copyright holders nor the names of
     contributors may be used to endorse or promote products derived
     from this software without specific prior written permission.
   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   POSSIBILITY OF SUCH DAMAGE. */
 /* $Id: strtod.c 2191 2010-11-05 13:45:57Z arcanum $ */
 #include <errno.h>
 #include <limits.h>
 #include <math.h>		/* INFINITY, NAN		*/
 #include <strings.h>
 #include <stdint.h>
 #include "avrlibc.h"
 /* Only GCC 4.2 calls the library function to convert an unsigned long
   to float.  Other GCC-es (including 4.3) use a signed long to float
   conversion along with a large inline code to correct the result.	*/
 extern double __floatunsisf (unsigned long);
 static const float pwr_p10 [6] = {
    1e+1, 1e+2, 1e+4, 1e+8, 1e+16, 1e+32
 };
 static const float pwr_m10 [6] = {
    1e-1, 1e-2, 1e-4, 1e-8, 1e-16, 1e-32
 };
 /* PSTR() is not used to save 1 byte per string: '\0' at the tail.	*/
 static const char pstr_inf[] = {'I','N','F'};
 static const char pstr_inity[] = {'I','N','I','T','Y'};
 static const char pstr_nan[] = {'N','A','N'};
 /**  The strtod() function converts the initial portion of the string pointed
     to by \a nptr to double representation.
     The expected form of the string is an optional plus ( \c '+' ) or minus
     sign ( \c '-' ) followed by a sequence of digits optionally containing
     a decimal-point character, optionally followed by an exponent.  An
     exponent consists of an \c 'E' or \c 'e', followed by an optional plus
     or minus sign, followed by a sequence of digits.
     Leading white-space characters in the string are skipped.
     The strtod() function returns the converted value, if any.
     If \a endptr is not \c NULL, a pointer to the character after the last
     character used in the conversion is stored in the location referenced by
     \a endptr.
     If no conversion is performed, zero is returned and the value of
     \a nptr is stored in the location referenced by \a endptr.
     If the correct value would cause overflow, plus or minus \c INFINITY is
     returned (according to the sign of the value), and \c ERANGE is stored
     in \c errno.  If the correct value would cause underflow, zero is
     returned and \c ERANGE is stored in \c errno.
 */
 double
 avr_strtod (const char * nptr, char ** endptr)
 {
    union {
 	unsigned long u32;
 	float flt;
    } x;
    unsigned char c;
    int exp;
    unsigned char flag;
 #define FL_MINUS    0x01	/* number is negative	*/
 #define FL_ANY	    0x02	/* any digit was readed	*/
 #define FL_OVFL	    0x04	/* overflow was		*/
 #define FL_DOT	    0x08	/* decimal '.' was	*/
 #define FL_MEXP	    0x10	/* exponent 'e' is neg.	*/
    if (endptr)
 	*endptr = (char *)nptr;
    do {
 	c = *nptr++;
    } while (c!=0&&c<=' ');
    flag = 0;
    if (c == '-') {
 	flag = FL_MINUS;
 	c = *nptr++;
    } else if (c == '+') {
 	c = *nptr++;
    }
    if (!strncasecmp(nptr - 1, pstr_inf, 3)) {
 	nptr += 2;
 	if (!strncasecmp(nptr, pstr_inity, 5))
 	    nptr += 5;
 	if (endptr)
 	    *endptr = (char *)nptr;
 	return flag & FL_MINUS ? -INFINITY : +INFINITY;
    }
    /* NAN() construction is not realised.
       Length would be 3 characters only.	*/
    if (!strncasecmp(nptr - 1, pstr_nan, 3)) {
 	if (endptr)
 	    *endptr = (char *)nptr + 2;
 	return NAN;
    }
    x.u32 = 0;
    exp = 0;
    while (1) {
 	c -= '0';
 	if (c <= 9) {
 	    flag |= FL_ANY;
 	    if (flag & FL_OVFL) {
 		if (!(flag & FL_DOT))
 		    exp += 1;
 	    } else {
 		if (flag & FL_DOT)
 		    exp -= 1;
 		/* x.u32 = x.u32 * 10 + c	*/
 		x.u32 = (((x.u32 << 2) + x.u32) << 1) + c;
 		if (x.u32 >= (ULONG_MAX - 9) / 10)
 		    flag |= FL_OVFL;
 	    }
 	} else if (c == (('.'-'0') & 0xff)  &&  !(flag & FL_DOT)) {
 	    flag |= FL_DOT;
 	} else {
 	    break;
 	}
 	c = *nptr++;
    }
    if (c == (('e'-'0') & 0xff) || c == (('E'-'0') & 0xff))
    {
 	int i;
 	c = *nptr++;
 	i = 2;
 	if (c == '-') {
 	    flag |= FL_MEXP;
 	    c = *nptr++;
 	} else if (c == '+') {
 	    c = *nptr++;
 	} else {
 	    i = 1;
 	}
 	c -= '0';
 	if (c > 9) {
 	    nptr -= i;
 	} else {
 	    i = 0;
 	    do {
 		if (i < 3200)
 		    i = (((i << 2) + i) << 1) + c;	/* i = 10*i + c	*/
 		c = *nptr++ - '0';
 	    } while (c <= 9);
 	    if (flag & FL_MEXP)
 		i = -i;
 	    exp += i;
 	}
    }
    if ((flag & FL_ANY) && endptr)
 	*endptr = (char *)nptr - 1;
    x.flt = __floatunsisf (x.u32);		/* manually	*/
    if ((flag & FL_MINUS) && (flag & FL_ANY))
 	x.flt = -x.flt;
    if (x.flt != 0) {
 	int pwr;
 	if (exp < 0) {
 	    nptr = (void *)(pwr_m10 + 5);
 	    exp = -exp;
 	} else {
 	    nptr = (void *)(pwr_p10 + 5);
 	}
 	for (pwr = 32; pwr; pwr >>= 1) {
 	    for (; exp >= pwr; exp -= pwr) {
 		union {
 		    unsigned long u32;
 		    float flt;
 		} y;
 		y.u32 = (uint32_t) *((float *)nptr);
 		x.flt *= y.flt;
 	    }
 	    nptr -= sizeof(float);
 	}
 	if (!isfinite(x.flt) || x.flt == 0)
 	    errno = ERANGE;
    }
    return x.flt;
 }
--- a/utils/avr_strtol.c
+++ b/utils/avr_strtol.c
@ -0,0 +1,167 @@
 /*-
 * Copyright (c) 1990, 1993
 *	The Regents of the University of California.  All rights reserved.
 * Copyright (c) 2005, Dmitry Xmelkov
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $Id: strtol.c 1944 2009-04-01 23:12:20Z arcanum $
 */
 #include <limits.h>
 #include <errno.h>
 /*
 * Convert a string to a long integer.
 *
 * Ignores `locale' stuff.  Assumes that the upper and lower case
 * alphabets and digits are each contiguous.
 */
 long
 avr_strtol(const char *nptr, char **endptr, register int base)
 {
 	register unsigned long acc;
 	register unsigned char c;
 	register unsigned long cutoff;
 	register signed char any;
 	unsigned char flag = 0;
 #define FL_NEG	0x01		/* number is negative */
 #define FL_0X	0x02		/* number has a 0x prefix */
 	if (endptr)
 		*endptr = (char *)nptr;
 	if (base != 0 && (base < 2 || base > 36))
 		return 0;
 	/*
 	 * Skip white space and pick up leading +/- sign if any.
 	 * If base is 0, allow 0x for hex and 0 for octal, else
 	 * assume decimal; if base is already 16, allow 0x.
 	 */
 	do {
 		c = *nptr++;
 	} while (c!=0&&c<=' ');
 	if (c == '-') {
 		flag = FL_NEG;
 		c = *nptr++;
 	} else if (c == '+')
 		c = *nptr++;
 	if ((base == 0 || base == 16) &&
 	    c == '0' && (*nptr == 'x' || *nptr == 'X')) {
 		c = nptr[1];
 		nptr += 2;
 		base = 16;
 		flag |= FL_0X;
 	}
 	if (base == 0)
 		base = c == '0' ? 8 : 10;
 	/*
 	 * Compute the cutoff value between legal numbers and illegal
 	 * numbers.  That is the largest legal value, divided by the
 	 * base.  An input number that is greater than this value, if
 	 * followed by a legal input character, is too big.  One that
 	 * is equal to this value may be valid or not; the decision
 	 * about this is done as outlined below.
 	 *
 	 * Overflow detections works as follows:
 	 *
 	 * As:
 	 *    acc_old <= cutoff
 	 * then:
 	 *    acc_old * base <= 0x80000000        (unsigned)
 	 * then:
 	 *    acc_old * base + c <= 0x80000000 + c
 	 * or:
 	 *    acc_new <= 0x80000000 + 35
 	 *
 	 * i.e. carry from MSB (by calculating acc_new) is impossible
 	 * and we can check result directly:
 	 *
 	 *    if (acc_new > 0x80000000) then overflow
 	 *
 	 * Set any if any `digits' consumed; make it negative to indicate
 	 * overflow.
 	 */
 #if  LONG_MIN != -LONG_MAX - 1
 #  error "This implementation of strtol() does not work on this platform."
 #endif
 	switch (base) {
 	    case 10:
 		cutoff = ((unsigned long)LONG_MAX + 1) / 10;
 		break;
 	    case 16:
 		cutoff = ((unsigned long)LONG_MAX + 1) / 16;
 		break;
 	    case 8:
 		cutoff = ((unsigned long)LONG_MAX + 1) / 8;
 		break;
 	    case 2:
 		cutoff = ((unsigned long)LONG_MAX + 1) / 2;
 		break;
 	    default:
 		cutoff = ((unsigned long)LONG_MAX + 1) / base;
 	}
 	for (acc = 0, any = 0;; c = *nptr++) {
 		if (c >= '0' && c <= '9')
 			c -= '0';
 		else if (c >= 'A' && c <= 'Z')
 			c -= 'A' - 10;
 		else if (c >= 'a' && c <= 'z')
 			c -= 'a' - 10;
 		else
 			break;
 		if (c >= base)
 			break;
 		if (any < 0)
 			continue;
 		if (acc > cutoff) {
 			any = -1;
 			continue;
 		}
 		acc = acc * base + c;
 		if (acc > (unsigned long)LONG_MAX + 1)
 			any = -1;
 		else
 			any = 1;
 	}
 	if (endptr) {
 		if (any)
 		    *endptr = (char *)nptr - 1;
 		else if (flag & FL_0X)
 		    *endptr = (char *)nptr - 2;
 	}
 	if (any < 0) {
 		acc = (flag & FL_NEG) ? LONG_MIN : LONG_MAX;
 		errno = ERANGE;
 	} else if (flag & FL_NEG) {
 		acc = -acc;
 	} else if ((signed long)acc < 0) {
 		acc = LONG_MAX;
 		errno = ERANGE;
 	}
 	return (acc);
 }
--- a/utils/avr_strtoul.c
+++ b/utils/avr_strtoul.c
@ -0,0 +1,145 @@
 /*
 * Copyright (c) 1990, 1993
 *	The Regents of the University of California.  All rights reserved.
 * Copyright (c) 2005, Dmitry Xmelkov
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the University nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $Id: strtoul.c 1944 2009-04-01 23:12:20Z arcanum $
 */
 #include <limits.h>
 #include <errno.h>
 #include "avrlibc.h"
 /*
 * Convert a string to an unsigned long integer.
 *
 * Ignores `locale' stuff.  Assumes that the upper and lower case
 * alphabets and digits are each contiguous.
 */
 unsigned long
 avr_strtoul(const char *nptr, char **endptr, register int base)
 {
 	register unsigned long acc;
 	register unsigned char c;
 	register unsigned long cutoff;
 	register signed char any;
 	unsigned char flag = 0;
 #define FL_NEG	0x01		/* number is negative */
 #define FL_0X	0x02		/* number has a 0x prefix */
 	if (endptr)
 		*endptr = (char *)nptr;
 	if (base != 0 && (base < 2 || base > 36))
 		return 0;
 	/*
 	 * See strtol for comments as to the logic used.
 	 */
 	do {
 		c = *nptr++;
 	} while (c!=0&&c<=' ');
 	if (c == '-') {
 		flag = FL_NEG;
 		c = *nptr++;
 	} else if (c == '+')
 		c = *nptr++;
 	if ((base == 0 || base == 16) &&
 	    c == '0' && (*nptr == 'x' || *nptr == 'X')) {
 		c = nptr[1];
 		nptr += 2;
 		base = 16;
 		flag |= FL_0X;
 	}
 	if (base == 0)
 		base = c == '0' ? 8 : 10;
 	/*
 	 * cutoff computation is similar to strtol().
 	 *
 	 * Description of the overflow detection logic used.
 	 *
 	 * First, let us assume an overflow.
 	 *
 	 * Result of `acc_old * base + c' is cut to 32 bits:
 	 *  acc_new <-- acc_old * base + c - 0x100000000
 	 *
 	 *  `acc_old * base' is <= 0xffffffff   (cutoff control)
 	 *
 	 * then:   acc_new <= 0xffffffff + c - 0x100000000
 	 *
 	 * or:     acc_new <= c - 1
 	 *
 	 * or:     acc_new < c
 	 *
 	 * Second:
 	 * if (no overflow) then acc * base + c >= c
 	 *                        (or: acc_new >= c)
 	 * is clear (alls are unsigned).
 	 *
 	 */
 	switch (base) {
 		case 16:    cutoff = ULONG_MAX / 16;  break;
 		case 10:    cutoff = ULONG_MAX / 10;  break;
 		case 8:     cutoff = ULONG_MAX / 8;   break;
 		default:    cutoff = ULONG_MAX / base;
 	}
 	for (acc = 0, any = 0;; c = *nptr++) {
 		if (c >= '0' && c <= '9')
 			c -= '0';
 		else if (c >= 'A' && c <= 'Z')
 			c -= 'A' - 10;
 		else if (c >= 'a' && c <= 'z')
 			c -= 'a' - 10;
 		else
 			break;
 		if (c >= base)
 			break;
 		if (any < 0)
 			continue;
 		if (acc > cutoff) {
 			any = -1;
 			continue;
 		}
 		acc = acc * base + c;
 		any = (c > acc) ? -1 : 1;
 	}
 	if (endptr) {
 		if (any)
 			*endptr = (char *)nptr - 1;
 		else if (flag & FL_0X)
 			*endptr = (char *)nptr - 2;
 	}
 	if (flag & FL_NEG)
 		acc = -acc;
 	if (any < 0) {
 		acc = ULONG_MAX;
 		errno = ERANGE;
 	}
 	return (acc);
 }
--- a/utils/avrlibc.h
+++ b/utils/avrlibc.h
@ -0,0 +1,14 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #ifndef GEX_AVRLIBC_H_H
 #define GEX_AVRLIBC_H_H
 int avr_atoi(const char *p);
 long avr_strtol(const char *nptr, char **endptr, register int base);
 long avr_atol(const char *p);
 double avr_strtod (const char * nptr, char ** endptr);
 unsigned long avr_strtoul(const char *nptr, char **endptr, register int base);
 #endif //GEX_AVRLIBC_H_H
--- a/utils/build_parser.sh
+++ b/utils/build_parser.sh
@ -0,0 +1,5 @@
 #!/bin/bash
 echo "-- Building parser from Ragel source --"
 ragel -L -G0 ini_parser.rl -o ini_parser.c
--- a/utils/circ_buf.c
+++ b/utils/circ_buf.c
@ -0,0 +1,177 @@
 /**
 * @file    circular_buffer.c
 * @brief   Implementation of a circular buffer
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2016-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #include "platform.h"
 #include "circ_buf.h"
 void circ_buf_init(circ_buf_t *circ_buf, uint8_t *buffer, uint32_t size)
 {
 	vPortEnterCritical();
 	{
 		circ_buf->buf = buffer;
 		circ_buf->size = size;
 		circ_buf->head = 0;
 		circ_buf->tail = 0;
 	}
 	vPortExitCritical();
 }
 static void push_do(circ_buf_t *circ_buf, uint8_t data)
 {
 	circ_buf->buf[circ_buf->tail] = data;
 	circ_buf->tail += 1;
 	if (circ_buf->tail >= circ_buf->size) {
        assert_param(circ_buf->tail == circ_buf->size);
 		circ_buf->tail = 0;
 	}
 }
 bool circ_buf_push_try(circ_buf_t *circ_buf, uint8_t data)
 {
 	bool success;
 	vPortEnterCritical();
 	{
 		if (circ_buf_count_free(circ_buf) == 0) {
 			success = false;
 			goto quit;
 		}
 		push_do(circ_buf, data);
 		success = true;
 	}
 quit:
 	vPortExitCritical();
 	return success;
 }
 void circ_buf_push(circ_buf_t *circ_buf, uint8_t data)
 {
 	vPortEnterCritical();
 	{
 		push_do(circ_buf, data);
 		// Assert no overflow
        assert_param(circ_buf->head != circ_buf->tail);
 	}
 	vPortExitCritical();
 }
 static uint8_t pop_do(circ_buf_t *circ_buf)
 {
 	uint8_t data;
 	data = circ_buf->buf[circ_buf->head];
 	circ_buf->head += 1;
 	if (circ_buf->head >= circ_buf->size) {
        assert_param(circ_buf->head == circ_buf->size);
 		circ_buf->head = 0;
 	}
 	return data;
 }
 uint8_t circ_buf_pop(circ_buf_t *circ_buf)
 {
 	uint8_t data;
 	vPortEnterCritical();
 	{
 		// Assert buffer isn't empty
        assert_param(circ_buf->head != circ_buf->tail);
 		data = pop_do(circ_buf);
 	}
 	vPortExitCritical();
    return data;
 }
 bool circ_buf_pop_try(circ_buf_t *circ_buf, uint8_t *data)
 {
 	bool success;
 	vPortEnterCritical();
 	{
 		if (circ_buf->head == circ_buf->tail) {
 			success = false;
 			goto quit;
 		}
 		*data = pop_do(circ_buf);
 		success = true;
 	}
 quit:
 	vPortExitCritical();
    return success;
 }
 uint32_t circ_buf_count_used(circ_buf_t *circ_buf)
 {
    uint32_t cnt;
 	vPortEnterCritical();
 	{
 		if (circ_buf->tail >= circ_buf->head) {
 			cnt = circ_buf->tail - circ_buf->head;
 		} else {
 			cnt = circ_buf->tail + circ_buf->size - circ_buf->head;
 		}
 	}
 	vPortExitCritical();
    return cnt;
 }
 uint32_t circ_buf_count_free(circ_buf_t *circ_buf)
 {
    uint32_t cnt;
 	vPortEnterCritical();
 	{
 		cnt = circ_buf->size - circ_buf_count_used(circ_buf) - 1;
 	}
 	vPortExitCritical();
    return cnt;
 }
 uint32_t circ_buf_read(circ_buf_t *circ_buf, uint8_t *data, uint32_t size)
 {
    uint32_t cnt;
    uint32_t i;
    cnt = circ_buf_count_used(circ_buf);
    cnt = MIN(size, cnt);
    for (i = 0; i < cnt; i++) {
        data[i] = circ_buf_pop(circ_buf);
    }
    return cnt;
 }
 uint32_t circ_buf_write(circ_buf_t *circ_buf, const uint8_t *data, uint32_t size)
 {
    uint32_t cnt;
    uint32_t i;
    cnt = circ_buf_count_free(circ_buf);
    cnt = MIN(size, cnt);
    for (i = 0; i < cnt; i++) {
        circ_buf_push(circ_buf, data[i]);
    }
    return cnt;
 }
--- a/utils/circ_buf.h
+++ b/utils/circ_buf.h
@ -0,0 +1,70 @@
 /**
 * @file    circ_buf.h
 * @brief   Implementation of a circular buffer
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2016-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #ifndef CIRC_BUF_H
 #define CIRC_BUF_H
 #include <stdbool.h>
 #include <stdint.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
 typedef struct {
    uint32_t head;
    uint32_t tail;
    uint32_t size;
    uint8_t *buf;
 } circ_buf_t;
 // Initialize or reinitialize a circular buffer
 void circ_buf_init(circ_buf_t *circ_buf, uint8_t *buffer, uint32_t size);
 // Push a byte into the circular buffer
 void circ_buf_push(circ_buf_t *circ_buf, uint8_t data);
 // Return a byte from the circular buffer
 uint8_t circ_buf_pop(circ_buf_t *circ_buf);
 // try to read the buffer, return false if empty
 bool circ_buf_pop_try(circ_buf_t *circ_buf, uint8_t *data);
 // try to write the buffer, return false if full
 bool circ_buf_push_try(circ_buf_t *circ_buf, uint8_t data);
 // Get the number of bytes in the circular buffer
 uint32_t circ_buf_count_used(circ_buf_t *circ_buf);
 // Get the number of free spots left in the circular buffer
 uint32_t circ_buf_count_free(circ_buf_t *circ_buf);
 // Attempt to read size bytes from the buffer. Return the number of bytes read
 uint32_t circ_buf_read(circ_buf_t *circ_buf, uint8_t *data, uint32_t size);
 // Attempt to write size bytes to the buffer. Return the number of bytes written
 uint32_t circ_buf_write(circ_buf_t *circ_buf, const uint8_t *data, uint32_t size);
 #ifdef __cplusplus
 }
 #endif
 #endif
--- a/utils/cortex_utils.h
+++ b/utils/cortex_utils.h
@ -0,0 +1,26 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #ifndef GEX_CORTEX_UTILS_H
 #define GEX_CORTEX_UTILS_H
 #include "platform.h"
 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);
 }
 #endif //GEX_CORTEX_UTILS_H
--- a/utils/error.c
+++ b/utils/error.c
@ -0,0 +1,45 @@
 /**
 * @file    error.c
 * @brief   Implementation of error.h
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #include "platform.h"
 static const char *const error_message[] = {
 #define X(name, text) text,
    X_ERROR_CODES
 #undef X
 };
 COMPILER_ASSERT(ERROR_COUNT == ELEMENTS_IN_ARRAY(error_message));
 const char *error_get_string(error_t error)
 {
    const char *msg = 0;
    if (error < ERROR_COUNT) {
        msg = error_message[error];
    }
    if (0 == msg) {
        assert_param(0);
        msg = "";
    }
    return msg;
 }
--- a/utils/error.h
+++ b/utils/error.h
@ -0,0 +1,65 @@
 /**
 * @file    error.h
 * @brief   collection of known errors and accessor for the friendly string
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #ifndef ERROR_H
 #define ERROR_H
 #ifdef __cplusplus
 }
 #endif
 #define X_ERROR_CODES \
    /* Shared errors */  \
    X(SUCCESS,"OK") \
    X(FAILURE,"Failure") \
    X(INTERNAL,"Internal error") \
    X(LOADING,"Init in progress") \
    \
    /* VFS user errors */ \
    X(ERROR_DURING_TRANSFER,"Error during transfer") \
    X(TRANSFER_TIMEOUT,"Transfer timed out") \
    /*X(FILE_BOUNDS,"Possible mismatch between file size and size programmed")*/ \
    X(OOO_SECTOR,"File sent out of order by PC") \
    \
    /* File stream errors */ \
    X(SUCCESS_DONE,"End of stream") \
    X(SUCCESS_DONE_OR_CONTINUE,"End of stream, or more to come") \
    \
    /* Unit loading */ \
    X(BAD_CONFIG, "Configuration error") \
    X(OUT_OF_MEM, "Not enough RAM") \
    X(RESOURCE_NOT_AVAILABLE, "Required pin / peripheral not available")
 // Keep in sync with the list error_message
 typedef enum {
 #define X(name, text) E_##name,
    X_ERROR_CODES
 #undef X
    ERROR_COUNT
 } error_t;
 const char *error_get_string(error_t error) __attribute__((pure));
 #ifdef __cplusplus
 }
 #endif
 #endif
--- a/utils/hexdump.c
+++ b/utils/hexdump.c
@ -0,0 +1,57 @@
 //
 // Created by MightyPork on 2017/12/04.
 //
 #include "platform.h"
 #include "hexdump.h"
 void hexDump(const char *restrict desc, const void *restrict addr, uint32_t len)
 {
    uint32_t i;
    uint8_t buff[17];
    uint8_t *pc = (unsigned char*)addr;
    // Output description if given.
    if (desc != NULL)
        PRINTF ("%s:\r\n", desc);
    if (len == 0) {
        PRINTF("  ZERO LENGTH\r\n");
        return;
    }
    // Process every byte in the data.
    for (i = 0; i < len; i++) {
        // Multiple of 16 means new line (with line offset).
        if ((i % 16) == 0) {
            // Just don't print ASCII for the zeroth line.
            if (i != 0)
                PRINTF ("  %s\r\n", buff);
            // Output the offset.
            PRINTF ("  %04"PRIx32" ", i);
        }
        // Now the hex code for the specific character.
        PRINTF (" %02x", pc[i]);
        // And store a printable ASCII character for later.
        if ((pc[i] < 0x20) || (pc[i] > 0x7e))
            buff[i % 16] = '.';
        else
            buff[i % 16] = pc[i];
        buff[(i % 16) + 1] = '\0';
    }
    // Pad out last line if not exactly 16 characters.
    while ((i % 16) != 0) {
        PRINTF ("   ");
        i++;
    }
    // And print the final ASCII bit.
    PRINTF ("  %s\r\n", buff);
    (void)buff;
 }
--- a/utils/hexdump.h
+++ b/utils/hexdump.h
@ -0,0 +1,19 @@
 //
 // Created by MightyPork on 2017/12/04.
 //
 #ifndef GEX_HEXDUMP_H
 #define GEX_HEXDUMP_H
 #include <stdint.h>
 /**
 * HEXDUMP https://stackoverflow.com/a/7776146/2180189
 *
 * @param desc - description printed above the dump
 * @param addr - address to dump
 * @param len - number of bytes
 */
 void hexDump(const char *restrict desc, const void *restrict addr, uint32_t len);
 #endif //GEX_HEXDUMP_H
--- a/utils/ini_parser.c
+++ b/utils/ini_parser.c
@ -0,0 +1,526 @@
 /* #line 1 "ini_parser.rl" */
 /* Ragel constants block */
 #include "ini_parser.h"
 // Ragel setup
 /* #line 10 "ini_parser.c" */
 static const char _ini_actions[] = {
 	0, 1, 1, 1, 2, 1, 3, 1, 
 	4, 1, 5, 1, 6, 1, 7, 1, 
 	8, 1, 9, 1, 10, 1, 11, 1, 
 	13, 2, 0, 4, 2, 12, 4
 };
 static const char _ini_eof_actions[] = {
 	0, 23, 5, 5, 15, 15, 15, 15, 
 	19, 19, 0, 0, 0, 0, 0, 0, 
 	0
 };
 static const int ini_start = 1;
 static const int ini_first_final = 12;
 static const int ini_error = 0;
 static const int ini_en_section = 2;
 static const int ini_en_keyvalue = 4;
 static const int ini_en_comment = 8;
 static const int ini_en_discard2eol = 10;
 static const int ini_en_main = 1;
 /* #line 10 "ini_parser.rl" */
 // Persistent state
 static int8_t cs = -1; //!< Ragel's Current State variable
 static uint32_t buff_i = 0; //!< Write pointer for the buffers
 static char value_quote = 0; //!< Quote character of the currently collected value
 static bool value_nextesc = false; //!< Next character is escaped, trated specially, and if quote, as literal quote character
 static IniParserCallback keyCallback = NULL; //!< Currently assigned callback
 static void *userdata = NULL; //!< Currently assigned user data for the callback
 // Buffers
 static char keybuf[INI_KEY_MAX];
 static char secbuf[INI_KEY_MAX];
 static char valbuf[INI_VALUE_MAX];
 // See header for doxygen!
 void
 ini_parse_reset_partial(void)
 {
 	buff_i = 0;
 	value_quote = 0;
 	value_nextesc = false;
 }
 void
 ini_parse_reset(void)
 {
 	ini_parse_reset_partial();
 	keybuf[0] = secbuf[0] = valbuf[0] = 0;
 /* #line 67 "ini_parser.c" */
 	{
 	cs = ini_start;
 	}
 /* #line 41 "ini_parser.rl" */
 }
 void
 ini_parser_error(const char* msg)
 {
 	ini_error("Parser error: %s", msg);
 	ini_parse_reset_partial();
 }
 void
 ini_parse_begin(IniParserCallback callback, void *userData)
 {
 	keyCallback = callback;
 	userdata = userData;
 	ini_parse_reset();
 }
 void
 *ini_parse_end(void)
 {
 	ini_parse("\n", 1);
 	if (keyCallback) {
 		keyCallback = NULL;
 	}
 	void *ud = userdata;
 	userdata = NULL;
 	return ud;
 }
 void
 ini_parse_file(const char *text, size_t len, IniParserCallback callback, void *userData)
 {
 	ini_parse_begin(callback, userData);
 	ini_parse(text, len);
 	ini_parse_end();
 }
 static void
 rtrim_buf(char *buf, int32_t end)
 {
 	if (end > 0) {
 		while ((uint8_t)buf[--end] < 33);
 		end++; // go past the last character
 	}
 	buf[end] = 0;
 }
 void
 ini_parse(const char *newstr, size_t len)
 {
 	int32_t i;
 	char c;
 	bool isnl;
 	bool isquot;
 	// Load new data to Ragel vars
 	const uint8_t *p;
 	const uint8_t *eof;
 	const uint8_t *pe;
 	if (len == 0) while(newstr[++len] != 0); // alternative to strlen
 	p = (const uint8_t *) newstr;
 	eof = NULL;
 	pe = (const uint8_t *) (newstr + len);
 	// Init Ragel on the first run
 	if (cs == -1) {
 		ini_parse_reset();
 	}
 	// The parser
 /* #line 152 "ini_parser.c" */
 	{
 	const char *_acts;
 	unsigned int _nacts;
 	if ( p == pe )
 		goto _test_eof;
 	if ( cs == 0 )
 		goto _out;
 _resume:
 	switch ( cs ) {
 case 1:
 	switch( (*p) ) {
 		case 32u: goto tr1;
 		case 35u: goto tr3;
 		case 58u: goto tr0;
 		case 59u: goto tr3;
 		case 61u: goto tr0;
 		case 91u: goto tr4;
 	}
 	if ( (*p) < 9u ) {
 		if ( (*p) <= 8u )
 			goto tr0;
 	} else if ( (*p) > 13u ) {
 		if ( 14u <= (*p) && (*p) <= 31u )
 			goto tr0;
 	} else
 		goto tr1;
 	goto tr2;
 case 0:
 	goto _out;
 case 12:
 	goto tr0;
 case 2:
 	switch( (*p) ) {
 		case 9u: goto tr6;
 		case 32u: goto tr6;
 		case 93u: goto tr5;
 	}
 	if ( (*p) <= 31u )
 		goto tr5;
 	goto tr7;
 case 3:
 	if ( (*p) == 93u )
 		goto tr8;
 	if ( (*p) > 8u ) {
 		if ( 10u <= (*p) && (*p) <= 31u )
 			goto tr5;
 	} else
 		goto tr5;
 	goto tr7;
 case 13:
 	goto tr5;
 case 4:
 	switch( (*p) ) {
 		case 10u: goto tr10;
 		case 58u: goto tr11;
 		case 61u: goto tr11;
 	}
 	goto tr9;
 case 5:
 	switch( (*p) ) {
 		case 9u: goto tr13;
 		case 10u: goto tr14;
 		case 13u: goto tr15;
 		case 32u: goto tr13;
 	}
 	goto tr12;
 case 6:
 	switch( (*p) ) {
 		case 10u: goto tr14;
 		case 13u: goto tr15;
 	}
 	goto tr12;
 case 14:
 	goto tr10;
 case 7:
 	if ( (*p) == 10u )
 		goto tr14;
 	goto tr10;
 case 8:
 	switch( (*p) ) {
 		case 10u: goto tr17;
 		case 13u: goto tr18;
 	}
 	goto tr16;
 case 15:
 	goto tr19;
 case 9:
 	if ( (*p) == 10u )
 		goto tr17;
 	goto tr19;
 case 10:
 	switch( (*p) ) {
 		case 10u: goto tr21;
 		case 13u: goto tr22;
 	}
 	goto tr20;
 case 16:
 	goto tr23;
 case 11:
 	if ( (*p) == 10u )
 		goto tr21;
 	goto tr23;
 	}
 	tr23: cs = 0; goto _again;
 	tr0: cs = 0; goto f0;
 	tr5: cs = 0; goto f4;
 	tr10: cs = 0; goto f7;
 	tr19: cs = 0; goto f11;
 	tr1: cs = 1; goto _again;
 	tr6: cs = 2; goto _again;
 	tr7: cs = 3; goto f5;
 	tr9: cs = 4; goto f8;
 	tr13: cs = 5; goto _again;
 	tr11: cs = 5; goto f9;
 	tr12: cs = 6; goto f10;
 	tr15: cs = 7; goto _again;
 	tr16: cs = 8; goto _again;
 	tr18: cs = 9; goto _again;
 	tr20: cs = 10; goto _again;
 	tr22: cs = 11; goto _again;
 	tr2: cs = 12; goto f1;
 	tr3: cs = 12; goto f2;
 	tr4: cs = 12; goto f3;
 	tr8: cs = 13; goto f6;
 	tr14: cs = 14; goto f10;
 	tr17: cs = 15; goto f12;
 	tr21: cs = 16; goto f13;
 	f5: _acts = _ini_actions + 1; goto execFuncs;
 	f6: _acts = _ini_actions + 3; goto execFuncs;
 	f4: _acts = _ini_actions + 5; goto execFuncs;
 	f1: _acts = _ini_actions + 7; goto execFuncs;
 	f8: _acts = _ini_actions + 9; goto execFuncs;
 	f9: _acts = _ini_actions + 11; goto execFuncs;
 	f10: _acts = _ini_actions + 13; goto execFuncs;
 	f7: _acts = _ini_actions + 15; goto execFuncs;
 	f12: _acts = _ini_actions + 17; goto execFuncs;
 	f11: _acts = _ini_actions + 19; goto execFuncs;
 	f13: _acts = _ini_actions + 21; goto execFuncs;
 	f0: _acts = _ini_actions + 23; goto execFuncs;
 	f3: _acts = _ini_actions + 25; goto execFuncs;
 	f2: _acts = _ini_actions + 28; goto execFuncs;
 execFuncs:
 	_nacts = *_acts++;
 	while ( _nacts-- > 0 ) {
 		switch ( *_acts++ ) {
 	case 0:
 /* #line 130 "ini_parser.rl" */
 	{
 			buff_i = 0;
 			{cs = 2;goto _again;}
 		}
 	break;
 	case 1:
 /* #line 135 "ini_parser.rl" */
 	{
 			if (buff_i >= INI_KEY_MAX) {
 				ini_parser_error("Section name too long");
 				{cs = 10;goto _again;}
 			}
 			keybuf[buff_i++] = (*p);
 		}
 	break;
 	case 2:
 /* #line 143 "ini_parser.rl" */
 	{
 			// we need a separate buffer for the result, otherwise a failed
 			// partial parse would corrupt the section string
 			rtrim_buf(keybuf, buff_i);
 			for (i = 0; (c = keybuf[i]) != 0; i++) secbuf[i] = c;
 			secbuf[i] = 0;
 			{cs = 1;goto _again;}
 		}
 	break;
 	case 3:
 /* #line 155 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in [section]");
 				if((*p) == '\n') {cs = 1;goto _again;} else {cs = 10;goto _again;}
 			}
 	break;
 	case 4:
 /* #line 162 "ini_parser.rl" */
 	{
 			buff_i = 0;
 			keybuf[buff_i++] = (*p); // add the first char
 			{cs = 4;goto _again;}
 		}
 	break;
 	case 5:
 /* #line 168 "ini_parser.rl" */
 	{
 			if (buff_i >= INI_KEY_MAX) {
 				ini_parser_error("Key too long");
 				{cs = 10;goto _again;}
 			}
 			keybuf[buff_i++] = (*p);
 		}
 	break;
 	case 6:
 /* #line 176 "ini_parser.rl" */
 	{
 			rtrim_buf(keybuf, buff_i);
 			// --- Value begin ---
 			buff_i = 0;
 			value_quote = 0;
 			value_nextesc = false;
 		}
 	break;
 	case 7:
 /* #line 185 "ini_parser.rl" */
 	{
 			isnl = ((*p) == '\r' || (*p) == '\n');
 			isquot = ((*p) == '\'' || (*p) == '"');
 			// detect our starting quote
 			if (isquot && !value_nextesc && buff_i == 0 && value_quote == 0) {
 				value_quote = (*p);
 				goto valueCharDone;
 			}
 			if (buff_i >= INI_VALUE_MAX) {
 				ini_parser_error("Value too long");
 				{cs = 10;goto _again;}
 			}
 			// end of string - clean up and report
 			if ((!value_nextesc && (*p) == value_quote) || isnl) {
 				if (isnl && value_quote) {
 					ini_parser_error("Unterminated string");
 					{cs = 1;goto _again;}
 				}
 				// unquoted: trim from the end
 				if (!value_quote) {
 					rtrim_buf(valbuf, buff_i);
 				} else {
 					valbuf[buff_i] = 0;
 				}
 				if (keyCallback) {
 					keyCallback(secbuf, keybuf, valbuf, userdata);
 				}
 				// we don't want to discard to eol if the string was terminated by eol
 				// - would delete the next line
 				if (isnl) {cs = 1;goto _again;} else {cs = 10;goto _again;}
 			}
 			c = (*p);
 			// escape...
 			if (value_nextesc) {
 				if ((*p) == 'n') c = '\n';
 				else if ((*p) == 'r') c = '\r';
 				else if ((*p) == 't') c = '\t';
 				else if ((*p) == 'e') c = '\033';
 			}
 			// collecting characters...
 			if (value_nextesc || (*p) != '\\') { // is quoted, or is not a quoting backslash - literal character
 				valbuf[buff_i++] = c;
 			}
 			value_nextesc = (!value_nextesc && (*p) == '\\');
 valueCharDone:;
 		}
 	break;
 	case 8:
 /* #line 247 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in key=value");
 				if((*p) == '\n') {cs = 1;goto _again;} else {cs = 10;goto _again;}
 			}
 	break;
 	case 9:
 /* #line 257 "ini_parser.rl" */
 	{ {cs = 1;goto _again;} }
 	break;
 	case 10:
 /* #line 258 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in comment");
 				if((*p) == '\n') {cs = 1;goto _again;} else {cs = 10;goto _again;}
 			}
 	break;
 	case 11:
 /* #line 265 "ini_parser.rl" */
 	{ {cs = 1;goto _again;} }
 	break;
 	case 12:
 /* #line 273 "ini_parser.rl" */
 	{ {cs = 8;goto _again;} }
 	break;
 	case 13:
 /* #line 276 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in root");
 				{cs = 10;goto _again;}
 			}
 	break;
 /* #line 458 "ini_parser.c" */
 		}
 	}
 	goto _again;
 _again:
 	if ( cs == 0 )
 		goto _out;
 	if ( ++p != pe )
 		goto _resume;
 	_test_eof: {}
 	if ( p == eof )
 	{
 	const char *__acts = _ini_actions + _ini_eof_actions[cs];
 	unsigned int __nacts = (unsigned int) *__acts++;
 	while ( __nacts-- > 0 ) {
 		switch ( *__acts++ ) {
 	case 3:
 /* #line 155 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in [section]");
 				if((*p) == '\n') {cs = 1;	if ( p == pe )
 		goto _test_eof;
 goto _again;} else {cs = 10;	if ( p == pe )
 		goto _test_eof;
 goto _again;}
 			}
 	break;
 	case 8:
 /* #line 247 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in key=value");
 				if((*p) == '\n') {cs = 1;	if ( p == pe )
 		goto _test_eof;
 goto _again;} else {cs = 10;	if ( p == pe )
 		goto _test_eof;
 goto _again;}
 			}
 	break;
 	case 10:
 /* #line 258 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in comment");
 				if((*p) == '\n') {cs = 1;	if ( p == pe )
 		goto _test_eof;
 goto _again;} else {cs = 10;	if ( p == pe )
 		goto _test_eof;
 goto _again;}
 			}
 	break;
 	case 13:
 /* #line 276 "ini_parser.rl" */
 	{
 				ini_parser_error("Syntax error in root");
 				{cs = 10;	if ( p == pe )
 		goto _test_eof;
 goto _again;}
 			}
 	break;
 /* #line 517 "ini_parser.c" */
 		}
 	}
 	}
 	_out: {}
 	}
 /* #line 283 "ini_parser.rl" */
 }
--- a/utils/ini_parser.h
+++ b/utils/ini_parser.h
@ -0,0 +1,65 @@
 #ifndef INIPARSE_STREAM_H
 #define INIPARSE_STREAM_H
 #include "platform.h"
 #ifdef DEBUG_INI
 #define ini_error(fmt, ...) dbg("! INI err: "#fmt, ##__VA_ARGS__)
 #else
 #define ini_error(fmt, ...)
 #endif
 // buffer sizes
 #define INI_KEY_MAX 20
 #define INI_VALUE_MAX 30
 /**
 * INI parser callback, called for each found key-value pair.
 *
 * @param section - current section, empty string for global keys
 * @param key - found key (trimmed of whitespace)
 * @param value - value, trimmed of quotes or whitespace
 * @param userData - opaque user data pointer, general purpose
 */
 typedef void (*IniParserCallback)(const char *section, const char *key, const char *value, void *userData);
 /**
 * Begin parsing a stream
 *
 * @param callback - key callback to assign
 * @param userData - optional user data that will be passed to the callback
 */
 void ini_parse_begin(IniParserCallback callback, void *userData);
 /**
 * End parse stream.
 * Flushes what remains in the buffer and removes callback.
 *
 * @returns userData or NULL if none
 */
 void* ini_parse_end(void);
 /**
 * Parse a string (needn't be complete line or file)
 *
 * @param data - string to parse
 * @param len - string length (0 = use strlen)
 */
 void ini_parse(const char *data, size_t len);
 /**
 * Parse a complete file loaded to string
 *
 * @param text - entire file as string
 * @param len - file length (0 = use strlen)
 * @param callback - key callback
 * @param userData - optional user data for key callback
 */
 void ini_parse_file(const char *text, size_t len, IniParserCallback callback, void *userData);
 /**
 * Explicitly reset the parser
 */
 void ini_parse_reset(void);
 #endif // INIPARSE_STREAM_H
--- a/utils/ini_parser.rl
+++ b/utils/ini_parser.rl
@ -0,0 +1,284 @@
 /* Ragel constants block */
 #include "ini_parser.h"
 // Ragel setup
 %%{
 	machine ini;
 	write data;
 	alphtype unsigned char;
 }%%
 // Persistent state
 static int8_t cs = -1; //!< Ragel's Current State variable
 static uint32_t buff_i = 0; //!< Write pointer for the buffers
 static char value_quote = 0; //!< Quote character of the currently collected value
 static bool value_nextesc = false; //!< Next character is escaped, trated specially, and if quote, as literal quote character
 static IniParserCallback keyCallback = NULL; //!< Currently assigned callback
 static void *userdata = NULL; //!< Currently assigned user data for the callback
 // Buffers
 static char keybuf[INI_KEY_MAX];
 static char secbuf[INI_KEY_MAX];
 static char valbuf[INI_VALUE_MAX];
 // See header for doxygen!
 void
 ini_parse_reset_partial(void)
 {
 	buff_i = 0;
 	value_quote = 0;
 	value_nextesc = false;
 }
 void
 ini_parse_reset(void)
 {
 	ini_parse_reset_partial();
 	keybuf[0] = secbuf[0] = valbuf[0] = 0;
 	%% write init;
 }
 void
 ini_parser_error(const char* msg)
 {
 	ini_error("Parser error: %s", msg);
 	ini_parse_reset_partial();
 }
 void
 ini_parse_begin(IniParserCallback callback, void *userData)
 {
 	keyCallback = callback;
 	userdata = userData;
 	ini_parse_reset();
 }
 void
 *ini_parse_end(void)
 {
 	ini_parse("\n", 1);
 	if (keyCallback) {
 		keyCallback = NULL;
 	}
 	void *ud = userdata;
 	userdata = NULL;
 	return ud;
 }
 void
 ini_parse_file(const char *text, size_t len, IniParserCallback callback, void *userData)
 {
 	ini_parse_begin(callback, userData);
 	ini_parse(text, len);
 	ini_parse_end();
 }
 static void
 rtrim_buf(char *buf, int32_t end)
 {
 	if (end > 0) {
 		while ((uint8_t)buf[--end] < 33);
 		end++; // go past the last character
 	}
 	buf[end] = 0;
 }
 void
 ini_parse(const char *newstr, size_t len)
 {
 	int32_t i;
 	char c;
 	bool isnl;
 	bool isquot;
 	// Load new data to Ragel vars
 	const uint8_t *p;
 	const uint8_t *eof;
 	const uint8_t *pe;
 	if (len == 0) while(newstr[++len] != 0); // alternative to strlen
 	p = (const uint8_t *) newstr;
 	eof = NULL;
 	pe = (const uint8_t *) (newstr + len);
 	// Init Ragel on the first run
 	if (cs == -1) {
 		ini_parse_reset();
 	}
 	// The parser
 	%%{
 #/ *
 		ispace = [ \t]; # inline space
 		wchar  = any - 0..8 - 10..31;
 		#apos  = '\'';
 		#quot  = '\"';
 		nonl   = [^\r\n];
 		nl     = '\r'? '\n';
 		# ---- [SECTION] ----
 		action sectionStart {
 			buff_i = 0;
 			fgoto section;
 		}
 		action sectionChar {
 			if (buff_i >= INI_KEY_MAX) {
 				ini_parser_error("Section name too long");
 				fgoto discard2eol;
 			}
 			keybuf[buff_i++] = fc;
 		}
 		action sectionEnd {
 			// we need a separate buffer for the result, otherwise a failed
 			// partial parse would corrupt the section string
 			rtrim_buf(keybuf, buff_i);
 			for (i = 0; (c = keybuf[i]) != 0; i++) secbuf[i] = c;
 			secbuf[i] = 0;
 			fgoto main;
 		}
 		section :=
 			(
 				ispace* <: ((wchar - ']')+ @sectionChar) ']' @sectionEnd
 			) $!{
 				ini_parser_error("Syntax error in [section]");
 				if(fc == '\n') fgoto main; else fgoto discard2eol;
 			};
 		# ---- KEY=VALUE ----
 		action keyStart {
 			buff_i = 0;
 			keybuf[buff_i++] = fc; // add the first char
 			fgoto keyvalue;
 		}
 		action keyChar {
 			if (buff_i >= INI_KEY_MAX) {
 				ini_parser_error("Key too long");
 				fgoto discard2eol;
 			}
 			keybuf[buff_i++] = fc;
 		}
 		action keyEnd {
 			rtrim_buf(keybuf, buff_i);
 			// --- Value begin ---
 			buff_i = 0;
 			value_quote = 0;
 			value_nextesc = false;
 		}
 		action valueChar {
 			isnl = (fc == '\r' || fc == '\n');
 			isquot = (fc == '\'' || fc == '"');
 			// detect our starting quote
 			if (isquot && !value_nextesc && buff_i == 0 && value_quote == 0) {
 				value_quote = fc;
 				goto valueCharDone;
 			}
 			if (buff_i >= INI_VALUE_MAX) {
 				ini_parser_error("Value too long");
 				fgoto discard2eol;
 			}
 			// end of string - clean up and report
 			if ((!value_nextesc && fc == value_quote) || isnl) {
 				if (isnl && value_quote) {
 					ini_parser_error("Unterminated string");
 					fgoto main;
 				}
 				// unquoted: trim from the end
 				if (!value_quote) {
 					rtrim_buf(valbuf, buff_i);
 				} else {
 					valbuf[buff_i] = 0;
 				}
 				if (keyCallback) {
 					keyCallback(secbuf, keybuf, valbuf, userdata);
 				}
 				// we don't want to discard to eol if the string was terminated by eol
 				// - would delete the next line
 				if (isnl) fgoto main; else fgoto discard2eol;
 			}
 			c = fc;
 			// escape...
 			if (value_nextesc) {
 				if (fc == 'n') c = '\n';
 				else if (fc == 'r') c = '\r';
 				else if (fc == 't') c = '\t';
 				else if (fc == 'e') c = '\033';
 			}
 			// collecting characters...
 			if (value_nextesc || fc != '\\') { // is quoted, or is not a quoting backslash - literal character
 				valbuf[buff_i++] = c;
 			}
 			value_nextesc = (!value_nextesc && fc == '\\');
 valueCharDone:;
 		}
 		# use * for key, first char is already consumed.
 		keyvalue :=
 			(
 				([^\n=:]* @keyChar %keyEnd)
 				[=:] ispace* <: nonl* @valueChar nl @valueChar
 			) $!{
 				ini_parser_error("Syntax error in key=value");
 				if(fc == '\n') fgoto main; else fgoto discard2eol;
 			};
 		# ---- COMMENT ----
 		comment :=
 			(
 				nonl* nl
 				@{ fgoto main; }
 			) $!{
 				ini_parser_error("Syntax error in comment");
 				if(fc == '\n') fgoto main; else fgoto discard2eol;
 			};
 		# ---- CLEANUP ----
 		discard2eol := nonl* nl @{ fgoto main; };
 		# ---- ROOT ----
 		main :=
 			(space*
 				(
 					'[' @sectionStart |
 					[#;] @{ fgoto comment; } |
 					(wchar - [\t =:]) @keyStart
 				)
 			) $!{
 				ini_parser_error("Syntax error in root");
 				fgoto discard2eol;
 			};
 		write exec;
 #*/
 	}%%
 }
--- a/utils/ini_writer.c
+++ b/utils/ini_writer.c
@ -0,0 +1,82 @@
 //
 // Created by MightyPork on 2017/12/01.
 //
 #include "platform.h"
 #include "ini_writer.h"
 #ifndef MIN
 #define MIN(a,b) ((a)>(b)?(b):(a))
 #endif
 #ifndef MAX
 #define MAX(a,b) ((a)>(b)?(a):(b))
 #endif
 #define IWBUFFER_LEN 128
 // sprintf from varargs, allocating buffer on stack. Uses 'format' argument
 #define IW_VPRINTF() do { \
    char iwbuffer[IWBUFFER_LEN]; \
    va_list args; \
    va_start(args, format); \
    uint32_t len = (int)fixup_vsnprintf(&iwbuffer[0], IWBUFFER_LEN, format, args); \
    iw_buff(iw, (uint8_t *) iwbuffer, len); \
    va_end(args); \
    } while(0)
 void iw_buff(IniWriter *iw, const uint8_t *buf, uint32_t len)
 {
    if (iw->count == 0) return;
    uint32_t skip = 0;
    if (iw->skip >= len) {
        // Skip entire string
        iw->skip -= len;
        return;
    } else {
        // skip part
        skip = iw->skip;
        iw->skip = 0;
        uint32_t count = MIN(iw->count, len-skip);
        memcpy(iw->ptr, buf+skip, count);
        iw->ptr += count;
        iw->count -= count;
    }
 }
 void iw_sprintf(IniWriter *iw, const char *format, ...)
 {
    if (iw->count == 0) return;
    IW_VPRINTF();
 }
 // High level stuff
 void iw_section(IniWriter *iw, const char *format, ...)
 {
    if (iw->count == 0) return;
    iw_string(iw, "\r\n["); // newline before section as a separator
    IW_VPRINTF();
    iw_string(iw, "]\r\n");
 }
 void iw_comment(IniWriter *iw, const char *format, ...)
 {
    if (iw->count == 0) return;
    iw_string(iw, "# ");
    IW_VPRINTF();
    iw_newline(iw);
 }
 void iw_entry(IniWriter *iw, const char *key, const char *format, ...)
 {
    if (iw->count == 0) return;
    iw_string(iw, key);
    iw_string(iw, "=");
    IW_VPRINTF();
    iw_newline(iw);	// one newline after entry
 }
--- a/utils/ini_writer.h
+++ b/utils/ini_writer.h
@ -0,0 +1,91 @@
 //
 // Created by MightyPork on 2017/12/01.
 //
 #ifndef INIWRITER_H
 #define INIWRITER_H
 #include "platform.h"
 typedef struct iniwriter_ {
    char *ptr;
    uint32_t skip;
    uint32_t count;
 } IniWriter;
 /**
 * Initialize a IniWriter struct (macro)
 *
 * @param buffer - buffer backing the writer, result will be written here
 * @param skip - number of written bytes to skip
 * @param count - number of bytes to write, truncate rest
 * @return structure initializer
 */
 #define iw_init(buffer, skip, count) (IniWriter){buffer, skip, count}
 /**
 * Try to write a buffer to the file
 *
 * @param iw - iniwriter handle
 * @param buf - buffer to write
 * @param len - buffer len
 */
 void iw_buff(IniWriter *iw, const uint8_t *buf, uint32_t len);
 /**
 * Try to write a string to the file
 *
 * @param iw - iniwriter handle
 * @param str - string to write
 */
 static inline void iw_string(IniWriter *iw, const char *str)
 {
    if (iw->count != 0) {
        iw_buff(iw, (uint8_t *) str, (uint32_t) strlen(str));
    }
 }
 #define iw_newline(iw) iw_string(iw, "\r\n")
 /**
 * Try to snprintf to the file
 *
 * @param iw - iniwriter handle
 * @param format - format, like printf
 * @param ... - replacements
 */
 void iw_sprintf(IniWriter *iw, const char *format, ...)
    __attribute__((format(printf,2,3)));
 // High level stuff
 /**
 * Try to write a INI section header [foobar]\r\n
 * @param iw - iniwriter handle
 * @param format - format, like printf
 * @param ... - replacements
 */
 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 format - format, like printf
 * @param ... - replacements
 */
 void iw_comment(IniWriter *iw, const char *format, ...)
    __attribute__((format(printf,2,3)));
 /**
 * Try to write a key-value entry
 *
 * @param iw - iniwriter handle
 * @param key - key
 * @param format - value format (like printf)
 * @param ... - replacements
 */
 void iw_entry(IniWriter *iw, const char *key, const char *format, ...)
    __attribute__((format(printf,3,4)));
 #endif //INIWRITER_H
--- a/utils/macro.h
+++ b/utils/macro.h
@ -0,0 +1,74 @@
 /**
 * @file    macro.h
 * @brief   useful things + Special asserts and macros
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #ifndef VFS_MACRO_H
 #define VFS_MACRO_H
 #ifdef __cplusplus
 extern "C" {
 #endif
 // --------------- VFS macros and general purpose --------------------
 #define ELEMENTS_IN_ARRAY(array)        (sizeof(array)/sizeof(array[0]))
 #define MB(size)                        ((size) * 1024 * 1024)
 #define KB(size)                        ((size) * 1024)
 #ifndef MIN
 #define MIN(a,b)                        ((a) < (b) ? (a) : (b))
 #endif
 #ifndef MAX
 #define MAX(a,b)                        ((a) > (b) ? (a) : (b))
 #endif
 #define ROUND_UP(value, boundary)       ((value) + ((boundary) - (value)) % (boundary))
 #define ROUND_DOWN(value, boundary)     ((value) - ((value) % (boundary)))
 // ---------- HELPERS FOR XMACROS -----------------
 #define XJOIN(a, b) a##b
 #define STR_(x) #x
 #define STR(x) STR_(x)
 // ---------- COMPILER SPECIAL MACROS -------------
 #define COMPILER_CONCAT_(a, b) a##b
 #define COMPILER_CONCAT(a, b) COMPILER_CONCAT_(a, b)
 // Divide by zero if the the expression is false.  This
 // causes an error at compile time.
 //
 // The special value '__COUNTER__' is used to create a unique value to
 // append to 'compiler_assert_' to create a unique token.  This prevents
 // conflicts resulting from the same enum being declared multiple times.
 #define COMPILER_ASSERT(e) enum { COMPILER_CONCAT(compiler_assert_, __COUNTER__) = 1/((e) ? 1 : 0) }
 #define __at(_addr) __attribute__ ((at(_addr)))
 #ifdef __cplusplus
 }
 #endif
 #endif
--- a/utils/malloc_safe.c
+++ b/utils/malloc_safe.c
@ -0,0 +1,48 @@
 #include <stdlib.h>
 #include <stdint.h>
 #include <stdbool.h>
 #include <inttypes.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 *mem = malloc(size);
    if (mem == NULL) abort_msg("MALLOC FAILED", file, line);
    return mem;
 }
 void *calloc_safe_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);
    return mem;
 }
 void *malloc_ck_do(size_t size, bool *suc, 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;
 }
 void *calloc_ck_do(size_t nmemb, size_t size, bool *suc, 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;
 }
 #endif
--- a/utils/malloc_safe.h
+++ b/utils/malloc_safe.h
@ -0,0 +1,22 @@
 #ifndef MALLOC_SAFE_H
 #define MALLOC_SAFE_H
 /**
 * Malloc that prints error and restarts the system on failure.
 */
 #include <stdlib.h>
 #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));
 #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__)
 #endif // MALLOC_SAFE_H
--- a/utils/payload_builder.c
+++ b/utils/payload_builder.c
@ -0,0 +1,100 @@
 #include <string.h>
 #include "payload_builder.h"
 #define pb_check_capacity(pb, needed) \
    if ((pb)->current + (needed) > (pb)->end) { \
        if ((pb)->full_handler == NULL || !(pb)->full_handler(pb, needed)) (pb)->ok = 0; \
    }
 /** Write from a buffer */
 bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len)
 {
    pb_check_capacity(pb, len);
    if (!pb->ok) return false;
    memcpy(pb->current, buf, len);
    pb->current += len;
    return true;
 }
 /** Write s zero terminated string */
 bool pb_string(PayloadBuilder *pb, const char *str)
 {
    uint32_t len = (uint32_t) strlen(str);
    pb_check_capacity(pb, len+1);
    if (!pb->ok) return false;
    memcpy(pb->current, str, len+1);
    pb->current += len+1;
    return true;
 }
 /** Write uint8_t to the buffer */
 bool pb_u8(PayloadBuilder *pb, uint8_t byte)
 {
    pb_check_capacity(pb, 1);
    if (!pb->ok) return false;
    *pb->current++ = byte;
    return true;
 }
 /** Write uint16_t to the buffer. */
 bool pb_u16(PayloadBuilder *pb, uint16_t word)
 {
    pb_check_capacity(pb, 2);
    if (!pb->ok) return false;
    if (pb->bigendian) {
        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
        *pb->current++ = (uint8_t) (word & 0xFF);
    } else {
        *pb->current++ = (uint8_t) (word & 0xFF);
        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
    }
    return true;
 }
 /** Write uint32_t to the buffer. */
 bool pb_u32(PayloadBuilder *pb, uint32_t word)
 {
    pb_check_capacity(pb, 4);
    if (!pb->ok) return false;
    if (pb->bigendian) {
        *pb->current++ = (uint8_t) ((word >> 24) & 0xFF);
        *pb->current++ = (uint8_t) ((word >> 16) & 0xFF);
        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
        *pb->current++ = (uint8_t) (word & 0xFF);
    } else {
        *pb->current++ = (uint8_t) (word & 0xFF);
        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
        *pb->current++ = (uint8_t) ((word >> 16) & 0xFF);
        *pb->current++ = (uint8_t) ((word >> 24) & 0xFF);
    }
    return true;
 }
 /** Write int8_t to the buffer. */
 bool pb_i8(PayloadBuilder *pb, int8_t byte)
 {
    return pb_u8(pb, ((union conv8){.i8 = byte}).u8);
 }
 /** Write int16_t to the buffer. */
 bool pb_i16(PayloadBuilder *pb, int16_t word)
 {
    return pb_u16(pb, ((union conv16){.i16 = word}).u16);
 }
 /** Write int32_t to the buffer. */
 bool pb_i32(PayloadBuilder *pb, int32_t word)
 {
    return pb_u32(pb, ((union conv32){.i32 = word}).u32);
 }
 /** Write 4-byte float to the buffer. */
 bool pb_float(PayloadBuilder *pb, float f)
 {
    return pb_u32(pb, ((union conv32){.f32 = f}).u32);
 }
--- a/utils/payload_builder.h
+++ b/utils/payload_builder.h
@ -0,0 +1,110 @@
 #ifndef PAYLOAD_BUILDER_H
 #define PAYLOAD_BUILDER_H
 /**
 * PayloadBuilder, part of the TinyFrame utilities collection
 * 
 * (c) Ondřej Hruška, 2014-2017. MIT license.
 * 
 * The builder supports big and little endian which is selected when 
 * initializing it or by accessing the bigendian struct field.
 * 
 * This module helps you with building payloads (not only for TinyFrame)
 *
 * The builder performs bounds checking and calls the provided handler when 
 * the requested write wouldn't fit. Use the handler to realloc / flush the buffer
 * or report an error.
 */
 #include <stdint.h>
 #include <stdbool.h>
 #include <stddef.h>
 #include "type_coerce.h"
 typedef struct PayloadBuilder_ PayloadBuilder;
 /**
 * Full buffer handler. 
 * 
 * 'needed' more bytes should be written but the end of the buffer was reached.
 * 
 * Return true if the problem was solved (e.g. buffer was flushed and the 
 * 'current' pointer moved to the beginning).
 * 
 * If false is returned, the 'ok' flag on the struct is set to false
 * and all following writes are discarded.
 */
 typedef bool (*pb_full_handler)(PayloadBuilder *pb, uint32_t needed);
 struct PayloadBuilder_ {
    uint8_t *start;   //!< Pointer to the beginning of the buffer
    uint8_t *current; //!< Pointer to the next byte to be read
    uint8_t *end;     //!< Pointer to the end of the buffer (start + length)
    pb_full_handler full_handler; //!< Callback for buffer overrun
    bool bigendian;   //!< Flag to use big-endian parsing
    bool ok;          //!< Indicates that all reads were successful
 };
 // --- initializer helper macros ---
 /** Start the builder. */
 #define pb_start_e(buf, capacity, bigendian, full_handler) ((PayloadBuilder){buf, buf, (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)
 /** Start the builder in little-endian mode */
 #define pb_start_le(buf, capacity, full_handler) pb_start_e(buf, capacity, 0, full_handler)
 /** Start the parser in little-endian mode (default) */
 #define pb_start(buf, capacity, full_handler) pb_start_le(buf, capacity, full_handler)
 // --- utilities ---
 /** Get already used bytes count */
 #define pb_length(pb) ((pb)->current - (pb)->start)
 /** Reset the current pointer to start */
 #define pb_rewind(pb) do { pb->current = pb->start; } while (0)
 /** Write from a buffer */
 bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len);
 /** Write a zero terminated string */
 bool pb_string(PayloadBuilder *pb, const char *str);
 /** Write uint8_t to the buffer */
 bool pb_u8(PayloadBuilder *pb, uint8_t byte);
 /** Write boolean to the buffer. */
 static inline bool pb_bool(PayloadBuilder *pb, bool b)
 {
    return pb_u8(pb, (uint8_t) b);
 }
 /** Write uint16_t to the buffer. */
 bool pb_u16(PayloadBuilder *pb, uint16_t word);
 /** Write uint32_t to the buffer. */
 bool pb_u32(PayloadBuilder *pb, uint32_t word);
 /** Write int8_t to the buffer. */
 bool pb_i8(PayloadBuilder *pb, int8_t byte);
 /** Write char (int8_t) to the buffer. */
 static inline bool pb_char(PayloadBuilder *pb, char c)
 {
    return pb_i8(pb, c);
 }
 /** Write int16_t to the buffer. */
 bool pb_i16(PayloadBuilder *pb, int16_t word);
 /** Write int32_t to the buffer. */
 bool pb_i32(PayloadBuilder *pb, int32_t word);
 /** Write 4-byte float to the buffer. */
 bool pb_float(PayloadBuilder *pb, float f);
 #endif // PAYLOAD_BUILDER_H
--- a/utils/payload_parser.c
+++ b/utils/payload_parser.c
@ -0,0 +1,121 @@
 #include "payload_parser.h"
 #define pp_check_capacity(pp, needed) \
    if ((pp)->current + (needed) > (pp)->end) { \
        if ((pp)->empty_handler == NULL || !(pp)->empty_handler(pp, needed)) {(pp)->ok = 0;} ; \
    }
 void pp_skip(PayloadParser *pp, uint32_t num)
 {
    pp->current += num;
 }
 uint8_t pp_u8(PayloadParser *pp)
 {
    pp_check_capacity(pp, 1);
    if (!pp->ok) return 0;
    return *pp->current++;
 }
 uint16_t pp_u16(PayloadParser *pp)
 {
    pp_check_capacity(pp, 2);
    if (!pp->ok) return 0;
    uint16_t x = 0;
    if (pp->bigendian) {
        x |= *pp->current++ << 8;
        x |= *pp->current++;
    } else {
        x |= *pp->current++;
        x |= *pp->current++ << 8;
    }
    return x;
 }
 uint32_t pp_u32(PayloadParser *pp)
 {
    pp_check_capacity(pp, 4);
    if (!pp->ok) return 0;
    uint32_t x = 0;
    if (pp->bigendian) {
        x |= (uint32_t) (*pp->current++ << 24);
        x |= (uint32_t) (*pp->current++ << 16);
        x |= (uint32_t) (*pp->current++ << 8);
        x |= *pp->current++;
    } else {
        x |= *pp->current++;
        x |= (uint32_t) (*pp->current++ << 8);
        x |= (uint32_t) (*pp->current++ << 16);
        x |= (uint32_t) (*pp->current++ << 24);
    }
    return x;
 }
 const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length)
 {
    int32_t len = (int) (pp->end - pp->current);
    if (!pp->ok || len <= 0) {
        if (length != NULL) *length = 0;
        return NULL;
    }
    if (length != NULL) {
        *length = (uint32_t) len;
    }
    return pp->current;
 }
 /** Read int8_t from the payload. */
 int8_t pp_i8(PayloadParser *pp)
 {
    return ((union conv8) {.u8 = pp_u8(pp)}).i8;
 }
 /** Read int16_t from the payload. */
 int16_t pp_i16(PayloadParser *pp)
 {
    return ((union conv16) {.u16 = pp_u16(pp)}).i16;
 }
 /** Read int32_t from the payload. */
 int32_t pp_i32(PayloadParser *pp)
 {
    return ((union conv32) {.u32 = pp_u32(pp)}).i32;
 }
 /** Read 4-byte float from the payload. */
 float pp_float(PayloadParser *pp)
 {
    return ((union conv32) {.u32 = pp_u32(pp)}).f32;
 }
 /** Read a zstring */
 uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen)
 {
    pp_check_capacity(pp, 1);
    uint32_t len = 0;
    while (len < maxlen-1 && pp->current != pp->end) {
        char c = *buffer++ = *pp->current++;
        if (c == 0) break;
        len++;
    }
    *buffer = 0;
    return len;
 }
 /** Read a buffer */
 uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen)
 {
    uint32_t len = 0;
    while (len < maxlen && pp->current != pp->end) {
        *buffer++ = *pp->current++;
        len++;
    }
    return len;
 }
--- a/utils/payload_parser.h
+++ b/utils/payload_parser.h
@ -0,0 +1,143 @@
 #ifndef PAYLOAD_PARSER_H
 #define PAYLOAD_PARSER_H
 /**
 * PayloadParser, part of the TinyFrame utilities collection
 * 
 * (c) Ondřej Hruška, 2016-2017. MIT license.
 * 
 * This module helps you with parsing payloads (not only from TinyFrame).
 * 
 * The parser supports big and little-endian which is selected when 
 * initializing it or by accessing the bigendian struct field.
 *
 * The parser performs bounds checking and calls the provided handler when 
 * the requested read doesn't have enough data. Use the callback to take
 * appropriate action, e.g. report an error.
 * 
 * If the handler function is not defined, the pb->ok flag is set to false
 * (use this to check for success), and further reads won't have any effect 
 * and always result in 0 or empty array.
 */
 #include <stdint.h>
 #include <stddef.h>
 #include <stdbool.h>
 #include "type_coerce.h"
 typedef struct PayloadParser_ PayloadParser;
 /**
 * Empty buffer handler. 
 * 
 * 'needed' more bytes should be read but the end was reached.
 * 
 * Return true if the problem was solved (e.g. new data loaded into 
 * the buffer and the 'current' pointer moved to the beginning).
 *  
 * If false is returned, the 'ok' flag on the struct is set to false
 * and all following reads will fail / return 0.
 */
 typedef bool (*pp_empty_handler)(PayloadParser *pp, uint32_t needed);
 struct PayloadParser_ {
    uint8_t *start;   //!< Pointer to the beginning of the buffer
    uint8_t *current; //!< Pointer to the next byte to be read
    uint8_t *end;     //!< Pointer to the end of the buffer (start + length)
    pp_empty_handler empty_handler; //!< Callback for buffer underrun
    bool bigendian;   //!< Flag to use big-endian parsing
    bool ok;          //!< Indicates that all reads were successful
 };
 // --- initializer helper macros ---
 /** Start the parser. */
 #define pp_start_e(buf, length, bigendian, empty_handler) ((PayloadParser){buf, buf, (buf)+(length), empty_handler, bigendian, 1})
 /** Start the parser in big-endian mode */
 #define pp_start_be(buf, length, empty_handler) pp_start_e(buf, length, 1, empty_handler)
 /** Start the parser in little-endian mode */
 #define pp_start_le(buf, length, empty_handler) pp_start_e(buf, length, 0, empty_handler)
 /** Start the parser in little-endian mode (default) */
 #define pp_start(buf, length, empty_handler) pp_start_le(buf, length, empty_handler)
 // --- utilities ---
 /** Get remaining length */
 #define pp_length(pp) ((pp)->end - (pp)->current)
 /** Reset the current pointer to start */
 #define pp_rewind(pp) do { pp->current = pp->start; } while (0)
 /**
 * @brief Get the remainder of the buffer.
 *
 * Returns NULL and sets 'length' to 0 if there are no bytes left.
 *
 * @param pp
 * @param length : here the buffer length will be stored. NULL to do not store.
 * @return the remaining portion of the input buffer
 */
 const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length);
 /** Read uint8_t from the payload. */
 uint8_t pp_u8(PayloadParser *pp);
 /** Read bool from the payload. */
 static inline int8_t pp_bool(PayloadParser *pp)
 {
    return pp_u8(pp) != 0;
 }
 /** Skip bytes */
 void pp_skip(PayloadParser *pp, uint32_t num);
 /** Read uint16_t from the payload. */
 uint16_t pp_u16(PayloadParser *pp);
 /** Read uint32_t from the payload. */
 uint32_t pp_u32(PayloadParser *pp);
 /** Read int8_t from the payload. */
 int8_t pp_i8(PayloadParser *pp);
 /** Read char (int8_t) from the payload. */
 static inline int8_t pp_char(PayloadParser *pp)
 {
    return pp_i8(pp);
 }
 /** Read int16_t from the payload. */
 int16_t pp_i16(PayloadParser *pp);
 /** Read int32_t from the payload. */
 int32_t pp_i32(PayloadParser *pp);
 /** Read 4-byte float from the payload. */
 float pp_float(PayloadParser *pp);
 /**
 * Parse a zero-terminated string
 *
 * @param pp - parser
 * @param buffer - target buffer
 * @param maxlen - buffer size
 * @return actual number of bytes, excluding terminator
 */
 uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen);
 /**
 * Parse a buffer
 *
 * @param pp - parser
 * @param buffer - target buffer
 * @param maxlen - buffer size
 * @return actual number of bytes, excluding terminator
 */
 uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen);
 #endif // PAYLOAD_PARSER_H
--- a/utils/snprintf.c
+++ b/utils/snprintf.c
--- a/utils/snprintf.h
+++ b/utils/snprintf.h
@ -0,0 +1,31 @@
 //
 // Created by MightyPork on 2017/11/09.
 //
 #ifndef GEX_SNPRINTF_H
 #define GEX_SNPRINTF_H
 #include <stdarg.h>
 #include <limits.h>
 #include "macro.h"
 size_t fixup_vsnprintf(char *str, size_t count, const char *fmt, va_list args);
 size_t fixup_snprintf(char *str, size_t count,const char *fmt,...);
 size_t fixup_vasprintf(char **ptr, const char *format, va_list ap);
 size_t fixup_asprintf(char **ptr, const char *format, ...);
 size_t fixup_sprintf(char *ptr, const char *format, ...);
 // Trap for using newlib functions
 //#define vsnprintf fuck1
 //#define snprintf fuck2
 //#define vasprintf fuck3
 //#define asprintf fuck4
 //#define sprintf fuck5
 #define VSNPRINTF(...) fixup_vsnprintf(__VA_ARGS__)
 #define SNPRINTF(...) fixup_snprintf(__VA_ARGS__)
 #define VASPRINTF(...) fixup_vasprintf(__VA_ARGS__)
 #define ASPRINTF(...) fixup_asprintf(__VA_ARGS__)
 #define SPRINTF(...) fixup_sprintf(__VA_ARGS__)
 #endif //GEX_SNPRINTF_H
--- a/utils/stacksmon.c
+++ b/utils/stacksmon.c
@ -0,0 +1,87 @@
 //
 // Created by MightyPork on 2017/12/04.
 //
 #include <task_sched.h>
 #include "platform.h"
 #include "stacksmon.h"
 #if USE_STACK_MONITOR
 struct stackhandle {
    const char *description;
    uint8_t *buffer;
    uint32_t len;
 };
 #define STACK_NUM 16
 static uint32_t nextidx = 0;
 static struct stackhandle stacks[STACK_NUM];
 void stackmon_register(const char *description, void *buffer, uint32_t len)
 {
    assert_param(nextidx < STACK_NUM);
    uint8_t *bv = buffer;
    // This is probably redundant, FreeRTOS does it too.
    for (uint32_t i = 0; i < len; i++) bv[i] = 0xA5;
    stacks[nextidx].description = description;
    stacks[nextidx].buffer = bv;
    stacks[nextidx].len = len;
    nextidx++;
 }
 void stackmon_dump(void)
 {
    PUTS("\r\n\033[1m---- STACK USAGE REPORT ---\033[m\r\n");
    for (uint32_t i = 0; i < nextidx; i++) {
        PRINTF("\033[36m>> %s\033[m @ %p\r\n", stacks[i].description, (void *) stacks[i].buffer);
        struct stackhandle *stack = &stacks[i];
        uint32_t free = 0;
        if (stack->buffer[0] != 0xA5) {
            PUTS("   \033[31m!!! OVERRUN !!!\033[m\r\n");
        } else {
            for (uint32_t j = 0; j < stack->len; j++) {
                if (stack->buffer[j] == 0xA5) {
                    free++;
                } else {
                    break;
                }
            }
        }
        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",
               (stack->len-free),
               stack->len,
               words,
               stack->len>>2,
               (stack->len-free)*100/stack->len
        );
    }
    PUTS("\033[36m>> QUEUES\033[m\r\n");
    PRINTF("   Used slots: \033[33mHP %"PRIu32", LP %"PRIu32"\033[m\r\n",
           jobQueHighWaterMarkHP, jobQueHighWaterMarkLP);
    PRINTF("\033[1m---------------------------\033[m\r\n\r\n");
 }
 void stackmon_check_canaries(void)
 {
    for (uint32_t i = 0; i < nextidx; i++) {
        struct stackhandle *stack = &stacks[i];
        if (stack->buffer[0] != 0xA5) {
            dbg("\r\n\033[31;1m!!!! STACK \"%s\" OVERRUN - CANARY IS DEAD !!!!\033[m\r\n", stack->description);
            stackmon_dump();
            trap("ABORT");
        }
    }
 }
 #endif
--- a/utils/stacksmon.h
+++ b/utils/stacksmon.h
@ -0,0 +1,20 @@
 //
 // Created by MightyPork on 2017/12/04.
 //
 #ifndef GEX_STACKSMON_H
 #define GEX_STACKSMON_H
 #include "platform.h"
 #if USE_STACK_MONITOR
 void stackmon_check_canaries(void);
 void stackmon_dump(void);
 void stackmon_register(const char *description, void *buffer, uint32_t len);
 #else
 #define stackmon_check_canaries() do {} while(0)
 #define stackmon_dump() do {} while(0)
 #define stackmon_register(description, buffer, len) do {} while(0)
 #endif
 #endif //GEX_STACKSMON_H
--- a/utils/str_utils.c
+++ b/utils/str_utils.c
@ -0,0 +1,57 @@
 //
 // Created by MightyPork on 2017/11/26.
 //
 #include "str_utils.h"
 #include "avrlibc.h"
 bool str_parse_yn(const char *str, bool *suc)
 {
    if (streq(str, "Y")) return true;
    if (streq(str, "1")) return true;
    if (streq(str, "YES")) return true;
    if (streq(str, "N")) return false;
    if (streq(str, "0")) return false;
    if (streq(str, "NO")) return false;
    *suc = false;
    return false;
 }
 uint8_t str_parse_01(const char *str, const char *a, const char *b, bool *suc)
 {
    if (streq(str, a)) return 0;
    if (streq(str, b)) return 1;
    *suc = false;
    return 0;
 }
 uint8_t str_parse_012(const char *str, const char *a, const char *b, const char *c, bool *suc)
 {
    if (streq(str, a)) return 0;
    if (streq(str, b)) return 1;
    if (streq(str, c)) return 2;
    *suc = false;
    return 0;
 }
 bool str_parse_pin(const char *value, char *targetName, uint8_t *targetNumber)
 {
    // discard leading 'P'
    if (value[0] == 'P') {
        value++;
    }
    size_t len = strlen(value);
    if (len<2||len>3) return false;
    *targetName = (uint8_t) value[0];
    if (!(*targetName >= 'A' && *targetName <= 'H')) return false;
    // lets just hope it's OK
    *targetNumber = (uint8_t) avr_atoi(value + 1);
    return true;
 }
--- a/utils/str_utils.h
+++ b/utils/str_utils.h
@ -0,0 +1,59 @@
 #ifndef PLATFORSTR_UTILS_H
 #define PLATFORSTR_UTILS_H
 #include <stdbool.h>
 #include <string.h>
 #include "stringbuilder.h"
 static inline bool __attribute__((const))
 streq(const char *restrict str1, const char *restrict str2)
 {
    return strcmp(str1, str2) == 0;
 }
 static inline bool __attribute__((const))
 strcaseq(const char *restrict str1, const char *restrict str2)
 {
    return strcasecmp(str1, str2) == 0;
 }
 static inline bool __attribute__((const))
 strneq(const char *restrict str1, const char *restrict str2, uint32_t limit)
 {
    return strncmp(str1, str2, limit) == 0;
 }
 static inline bool __attribute__((const))
 strncaseq(const char *restrict str1, const char *restrict str2, uint32_t limit)
 {
    return strncasecmp(str1, str2, limit) == 0;
 }
 static inline bool __attribute__((const))
 strstarts(const char *restrict str, const char *restrict prefix)
 {
    return strncmp(str, prefix, strlen(prefix)) == 0;
 }
 static inline char __attribute__((const))
 last_char_n(const char *str, uint32_t nth)
 {
    return str[strlen(str) - nth];
 }
 static inline char __attribute__((const))
 last_char(const char *str)
 {
    return str[strlen(str) - 1];
 }
 bool str_parse_yn(const char *str, bool *suc);
 uint8_t str_parse_01(const char *str, const char *a, const char *b, bool *suc);
 uint8_t str_parse_012(const char *str, const char *a, const char *b, const char *c, bool *suc);
 #define str_01(cond, a, b) ((cond) ? (b) : (a))
 #define str_yn(cond) ((cond) ? ("Y") : ("N"))
 bool str_parse_pin(const char *str, char *targetName, uint8_t *targetNumber);
 #endif
--- a/utils/stringbuilder.c
+++ b/utils/stringbuilder.c
@ -0,0 +1,114 @@
 /**
 * @file    stringbuilder.c
 *
 * ADAPTED FROM:
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #include "stringbuilder.h"
 #include <string.h>
 #ifdef __cplusplus
 extern "C" {
 #endif
 uint32_t strb_write_hex8(char *str, uint8_t value)
 {
    static const char nybble_chars[] = "0123456789abcdef";
    *(str + 0) = nybble_chars[(value >> 4) & 0x0F ];
    *(str + 1) = nybble_chars[(value >> 0) & 0x0F ];
    return 2;
 }
 uint32_t strb_write_hex16(char *str, uint16_t value)
 {
    uint32_t pos = 0;
    pos += strb_write_hex8(str + pos, (uint8_t) ((value >> 8) & 0xFF));
    pos += strb_write_hex8(str + pos, (uint8_t) ((value >> 0) & 0xFF));
    return pos;
 }
 uint32_t strb_write_hex32(char *str, uint32_t value)
 {
    uint32_t pos = 0;
    pos += strb_write_hex8(str + pos, (uint8_t) ((value >> 0x18) & 0xFF));
    pos += strb_write_hex8(str + pos, (uint8_t) ((value >> 0x10) & 0xFF));
    pos += strb_write_hex8(str + pos, (uint8_t) ((value >> 0x08) & 0xFF));
    pos += strb_write_hex8(str + pos, (uint8_t) ((value >> 0x00) & 0xFF));
    return pos;
 }
 uint32_t strb_write_uint32(char *str, uint32_t value)
 {
    uint32_t temp_val;
    uint32_t digits;
    uint32_t i;
    // Count the number of digits
    digits = 0;
    temp_val = value;
    while (temp_val > 0) {
        temp_val /= 10;
        digits += 1;
    }
    if (digits <= 0) {
        digits = 1;
    }
    // Write the number
    for (i = 0; i < digits; i++) {
        str[digits - i - 1] = (char) ('0' + (value % 10));
        value /= 10;
    }
    return digits;
 }
 uint32_t strb_write_uint32_zp(char *str, uint32_t value, uint16_t total_size)
 {
    uint32_t size;
    // Get the size of value
    size = strb_write_uint32(str, value);
    if (size >= total_size) {
        return size;
    }
    // Zero fill
    memset(str, '0', total_size);
    // Write value
    strb_write_uint32(str + (total_size - size), value);
    return total_size;
 }
 uint32_t strb_write_string(char *str, const char *data)
 {
    uint32_t pos = 0;
    while (0 != data[pos]) {
        str[pos] = data[pos];
        pos++;
    }
    return pos;
 }
 #ifdef __cplusplus
 }
 #endif
--- a/utils/stringbuilder.h
+++ b/utils/stringbuilder.h
@ -0,0 +1,36 @@
 /**
 * @file    stringbuilder.h
 *
 * ADAPTED FROM:
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #ifndef STRINGBUILDER_H
 #define STRINGBUILDER_H
 #include <stdint.h>
 // Write the value to the address specified and return the size
 uint32_t strb_write_hex8(char *str, uint8_t value);
 uint32_t strb_write_hex16(char *str, uint16_t value);
 uint32_t strb_write_hex32(char *str, uint32_t value);
 uint32_t strb_write_uint32(char *str, uint32_t value);
 uint32_t strb_write_uint32_zp(char *str, uint32_t value, uint16_t total_size);
 uint32_t strb_write_string(char *str, const char *data);
 #endif //STRINGBUILDER_H
--- a/utils/type_coerce.h
+++ b/utils/type_coerce.h
@ -0,0 +1,32 @@
 #ifndef TYPE_COERCE_H
 #define TYPE_COERCE_H
 /**
 * Structs for conversion between types,
 * part of the TinyFrame utilities collection
 * 
 * (c) Ondřej Hruška, 2016-2017. MIT license.
 * 
 * This is a support header file for PayloadParser and PayloadBuilder.
 */
 #include <stdint.h>
 #include <stddef.h>
 union conv8 {
    uint8_t u8;
    int8_t i8;
 };
 union conv16 {
    uint16_t u16;
    int16_t i16;
 };
 union conv32 {
    uint32_t u32;
    int32_t i32;
    float f32;
 };
 #endif // TYPE_COERCE_H
--- a/version.h
+++ b/version.h
@ -0,0 +1,10 @@
 //
 // Created by MightyPork on 2017/12/08.
 //
 #ifndef GEX_VERSION_H
 #define GEX_VERSION_H
 #define GEX_VERSION "0.0.1"
 #endif //GEX_VERSION_H
--- a/vfs/file_stream.c
+++ b/vfs/file_stream.c
@ -0,0 +1,245 @@
 /**
 * @file    file_stream.c
 * @brief   Implementation of file_stream.h
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #include <platform/status_led.h>
 #include "platform.h"
 #include "utils/ini_parser.h"
 #include "framework/settings.h"
 #include "file_stream.h"
 #include "vfs_manager.h"
 typedef enum {
    STREAM_STATE_CLOSED,
    STREAM_STATE_OPEN,
    STREAM_STATE_END,
    STREAM_STATE_ERROR
 } stream_state_t;
 typedef bool (*stream_detect_cb_t)(const uint8_t *data, uint32_t size);
 typedef error_t (*stream_open_cb_t)(void *state);
 typedef error_t (*stream_write_cb_t)(void *state, const uint8_t *data, uint32_t size);
 typedef error_t (*stream_close_cb_t)(void *state);
 typedef struct {
    stream_detect_cb_t detect;
    stream_open_cb_t open;
    stream_write_cb_t write;
    stream_close_cb_t close;
 } stream_t;
 typedef struct {
    size_t file_pos;
 } conf_state_t;
 typedef union {
    conf_state_t conf;
 } shared_state_t;
 static bool detect_conf(const uint8_t *data, uint32_t size);
 static error_t open_conf(void *state);
 static error_t write_conf(void *state, const uint8_t *data, uint32_t size);
 static error_t close_conf(void *state);
 stream_t stream[] = {
    {detect_conf, open_conf, write_conf, close_conf},   // STREAM_TYPE_CONF
 };
 COMPILER_ASSERT(ELEMENTS_IN_ARRAY(stream) == STREAM_TYPE_COUNT);
 // STREAM_TYPE_NONE must not be included in count
 COMPILER_ASSERT(STREAM_TYPE_NONE > STREAM_TYPE_COUNT);
 static shared_state_t shared_state;
 static stream_state_t stream_state = STREAM_STATE_CLOSED;
 static stream_t *current_stream = 0;
 stream_type_t stream_start_identify(const uint8_t *data, uint32_t size)
 {
    stream_type_t i;
    for (i = STREAM_TYPE_START; i < STREAM_TYPE_COUNT; i++) {
        if (stream[i].detect(data, size)) {
            return i;
        }
    }
    return STREAM_TYPE_NONE;
 }
 // Identify the file type from its extension
 stream_type_t stream_type_from_name(const vfs_filename_t filename)
 {
    // 8.3 file names must be in upper case
    if (0 == strncmp("INI", &filename[8], 3)) {
        // This is used only to verify we identified the file correctly (?)
        return STREAM_TYPE_CONF;
    } else {
        return STREAM_TYPE_NONE;
    }
 }
 error_t stream_open(stream_type_t stream_type)
 {
    error_t status;
    // Stream must not be open already
    if (stream_state != STREAM_STATE_CLOSED) {
        vfs_printf("!! Stream is not closed, cant open");
        assert_param(0);
        return E_INTERNAL;
    }
    // Stream must be of a supported type
    if (stream_type >= STREAM_TYPE_COUNT) {
        vfs_printf("!! Stream bad type");
        assert_param(0);
        return E_INTERNAL;
    }
    StatusLed_On(STATUS_DISK_BUSY);
    // TODO create a thread...?
    // Initialize all variables
    memset(&shared_state, 0, sizeof(shared_state));
    stream_state = STREAM_STATE_OPEN;
    current_stream = &stream[stream_type];
    // Initialize the specified stream
    status = current_stream->open(&shared_state);
    if (E_SUCCESS != status) {
        stream_state = STREAM_STATE_ERROR;
        vfs_printf("!! not success");
    }
    return status;
 }
 error_t stream_write(const uint8_t *data, uint32_t size)
 {
    error_t status;
    // Stream must be open already
    if (stream_state != STREAM_STATE_OPEN) {
        vfs_printf("!! Stream is not open, cant write");
        assert_param(0);
        return E_INTERNAL;
    }
    // Check thread after checking state since the stream thread is
    // set only if stream_open has been called
    //stream_thread_assert(); // ???
    // Write to stream
    status = current_stream->write(&shared_state, data, size);
    if (E_SUCCESS_DONE == status) {
        vfs_printf("Stream DONE");
        stream_state = STREAM_STATE_END;
    } else if ((E_SUCCESS_DONE_OR_CONTINUE == status) || (E_SUCCESS == status)) {
        // Stream should remain in the open state
        assert_param(STREAM_STATE_OPEN == stream_state);
        vfs_printf("Stream may close or get more data.,,");
    } else {
        stream_state = STREAM_STATE_ERROR;
        vfs_printf("!! FAIL in stream");
    }
    return status;
 }
 error_t stream_close(void)
 {
    error_t status;
    // Stream must not be closed already
    if (STREAM_STATE_CLOSED == stream_state) {
        vfs_printf("!! Stream already closed");
        assert_param(0);
        return E_INTERNAL;
    }
    // Check thread after checking state since the stream thread is
    // set only if stream_open has been called
 //    stream_thread_assert(); // ???
    // Close stream
    StatusLed_Off(STATUS_DISK_BUSY);
    status = current_stream->close(&shared_state);
    stream_state = STREAM_STATE_CLOSED;
    return status;
 }
 static bool detect_conf(const uint8_t *data, uint32_t size)
 {
    // Here we have received the first sector of a potential INI file (assuming it's a whole sector, since
    // this is called from the MSC driver).
    //
    // We can start parsing and look for the first section or some other marker. The file name is yet unknown
    // and may not be known for a while - we cannot use that to detect anything, unless we buffer the entire file
    // (a bad idea)
    // TODO detect config file
    return data[0] == '#'; // here we just assume everything is INI
 }
 static void iniparser_cb(const char *section, const char *key, const char *value, void *userData)
 {
    settings_read_ini(section, key, value);
 }
 static error_t open_conf(void *state)
 {
    conf_state_t *conf = state;
    conf->file_pos = 0;
    vfs_printf("\r\n---- INI OPEN! ----");
    settings_read_ini_begin();
    ini_parse_begin(iniparser_cb, NULL);
    return E_SUCCESS;
 }
 static error_t write_conf(void *state, const uint8_t *data, uint32_t size)
 {
    conf_state_t *conf = state;
    conf->file_pos += size;
    vfs_printf("Writing INI - RX %d bytes", size);
    vfs_printf_nonl("\033[92m", 5);
    vfs_printf_nonl((const char *) data, size);
    vfs_printf_nonl("\033[0m\r\n", 6);
    ini_parse((const char *) data, size);
    return E_SUCCESS_DONE_OR_CONTINUE; // indicate we don't really know if it's over or not
    // TODO use some marker for EOF in the actual config files
 }
 static error_t close_conf(void *state)
 {
    conf_state_t *conf = state;
    vfs_printf("Close INI, total bytes = %d", conf->file_pos);
    ini_parse_end();
    settings_read_ini_end();
    // force a full remount to have the changes be visible
    vfs_mngr_fs_remount(true);
    return E_SUCCESS;
 }
--- a/vfs/file_stream.h
+++ b/vfs/file_stream.h
@ -0,0 +1,61 @@
 /**
 * @file    file_stream.h
 * @brief   Different file stream parsers that are supported
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #ifndef VFS_FILE_STREAM_H
 #define VFS_FILE_STREAM_H
 #include "platform.h"
 #include "virtual_fs.h"
 #ifdef __cplusplus
 extern "C" {
 #endif
 typedef enum {
    STREAM_TYPE_START = 0,
    STREAM_TYPE_CONF = STREAM_TYPE_START,
 //    STREAM_TYPE_HEX,
    // Add new stream types here
    STREAM_TYPE_COUNT,
    STREAM_TYPE_NONE
 } stream_type_t;
 // Stateless function to identify a filestream by its contents
 stream_type_t stream_start_identify(const uint8_t *data, uint32_t size);
 // Stateless function to identify a filestream by its name
 stream_type_t stream_type_from_name(const vfs_filename_t filename);
 error_t stream_open(stream_type_t stream_type);
 error_t stream_write(const uint8_t *data, uint32_t size);
 error_t stream_close(void);
 #ifdef __cplusplus
 }
 #endif
 #endif//VFS_FILE_STREAM_H
--- a/vfs/vfs_manager.c
+++ b/vfs/vfs_manager.c
@ -0,0 +1,949 @@
 /**
 * @file    vfs_manager.c
 * @brief   Implementation of vfs_manager.h
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #include <task_main.h>
 #include "platform.h"
 #include "task_main.h"
 #include "virtual_fs.h"
 #include "vfs_manager.h"
 #include "file_stream.h"
 #define INVALID_TIMEOUT_MS  0xFFFFFFFF
 #define MAX_EVENT_TIME_MS   60000
 #define CONNECT_DELAY_MS 0
 #define RECONNECT_DELAY_MS 2500    // Must be above 1s for windows (more for linux)
 // TRANSFER_IN_PROGRESS
 #define DISCONNECT_DELAY_TRANSFER_TIMEOUT_MS 500 // was 20000 - this is triggered after a partial file upload
 // TRANSFER_CAN_BE_FINISHED
 #define DISCONNECT_DELAY_TRANSFER_IDLE_MS 500
 // TRANSFER_NOT_STARTED || TRASNFER_FINISHED
 #define DISCONNECT_DELAY_MS 500
 // Make sure none of the delays exceed the max time
 COMPILER_ASSERT(CONNECT_DELAY_MS < MAX_EVENT_TIME_MS);
 COMPILER_ASSERT(RECONNECT_DELAY_MS < MAX_EVENT_TIME_MS);
 COMPILER_ASSERT(DISCONNECT_DELAY_TRANSFER_TIMEOUT_MS < MAX_EVENT_TIME_MS);
 COMPILER_ASSERT(DISCONNECT_DELAY_TRANSFER_IDLE_MS < MAX_EVENT_TIME_MS);
 COMPILER_ASSERT(DISCONNECT_DELAY_MS < MAX_EVENT_TIME_MS);
 volatile vfs_info_t vfs_info;
 typedef enum {
    TRANSFER_NOT_STARTED,
    TRANSFER_IN_PROGRESS,
    TRANSFER_CAN_BE_FINISHED,
    TRASNFER_FINISHED,
 } transfer_state_t;
 typedef struct {
    vfs_file_t file_to_program;     // A pointer to the directory entry of the file being programmed
    vfs_sector_t start_sector;      // Start sector of the file being programmed
    vfs_sector_t file_next_sector;  // Expected next sector of the file
    vfs_sector_t last_ooo_sector;   // Last out of order sector within the file
    uint32_t size_processed;        // The number of bytes processed by the stream
    uint32_t file_size;             // Size of the file indicated by root dir.  Only allowed to increase
    uint32_t size_transferred;      // The number of bytes transferred
    transfer_state_t transfer_state;// Transfer state
    bool stream_open;               // State of the stream
    bool stream_started;            // Stream processing started. This only gets reset remount
    bool stream_finished;           // Stream processing is done. This only gets reset remount
    bool stream_optional_finish;    // True if the stream processing can be considered done
    bool file_info_optional_finish; // True if the file transfer can be considered done
    bool transfer_timeout;          // Set if the transfer was finished because of a timeout. This only gets reset remount
    stream_type_t stream;           // Current stream or STREAM_TYPE_NONE is stream is closed.  This only gets reset remount
 } file_transfer_state_t;
 typedef enum {
    VFS_MNGR_STATE_DISCONNECTED,
    VFS_MNGR_STATE_RECONNECTING,
    VFS_MNGR_STATE_CONNECTED
 } vfs_mngr_state_t;
 static const file_transfer_state_t default_transfer_state = {
    VFS_FILE_INVALID,
    VFS_INVALID_SECTOR,
    VFS_INVALID_SECTOR,
    VFS_INVALID_SECTOR,
    0,
    0,
    0,
    TRANSFER_NOT_STARTED,
    false,
    false,
    false,
    false,
    false,
    false,
    STREAM_TYPE_NONE,
 };
 //static uint32_t usb_buffer[VFS_SECTOR_SIZE / sizeof(uint32_t)];
 static error_t fail_reason = E_SUCCESS;
 static file_transfer_state_t file_transfer_state;
 // These variables can be access from multiple threads
 // so access to them must be synchronized
 static vfs_mngr_state_t vfs_state;
 static vfs_mngr_state_t vfs_state_next;
 static bool vfs_next_remount_full = false;
 static uint32_t time_usb_idle;
 static osStaticMutexDef_t vfsMutexControlBlock;
 static osSemaphoreId vfsMutexHandle = NULL;
 // Synchronization functions
 static void sync_init(void);
 static void sync_assert_usb_thread(void);
 static void sync_lock(void);
 static void sync_unlock(void);
 static bool changing_state(void);
 static void build_filesystem(void);
 static void file_change_handler(const vfs_filename_t filename, vfs_file_change_t change, vfs_file_t file, vfs_file_t new_file_data);
 static void file_data_handler(uint32_t sector, const uint8_t *buf, uint32_t num_of_sectors);
 static bool ready_for_state_change(void);
 static void abort_remount(void);
 static void transfer_update_file_info(vfs_file_t file, uint32_t start_sector, uint32_t size, stream_type_t stream);
 static void transfer_reset_file_info(void);
 static void transfer_stream_open(stream_type_t stream, uint32_t start_sector);
 static void transfer_stream_data(uint32_t sector, const uint8_t *data, uint32_t size);
 static void transfer_update_state(error_t status);
 void vfs_mngr_fs_enable(bool enable)
 {
    sync_lock();
    vfs_printf("Enable = %d", enable);
    if (enable) {
        if (VFS_MNGR_STATE_DISCONNECTED == vfs_state_next) {
            vfs_printf(" Switch to Connected");
            vfs_state_next = VFS_MNGR_STATE_CONNECTED;
        } else {
            vfs_printf(" no switch");
        };
    } else {
        vfs_printf(" Switch to DISconnected");
        vfs_state_next = VFS_MNGR_STATE_DISCONNECTED;
    }
    sync_unlock();
 }
 void vfs_mngr_fs_remount(bool force_full)
 {
    sync_lock();
    // Only start a remount if in the connected state and not in a transition
    if (!changing_state() && (VFS_MNGR_STATE_CONNECTED == vfs_state)) {
        vfs_state_next = VFS_MNGR_STATE_RECONNECTING;
    }
    vfs_next_remount_full |= force_full;
    sync_unlock();
 }
 void vfs_mngr_init(bool enable)
 {
    vfs_printf("vfs_mngr_init");
    sync_assert_usb_thread();
    build_filesystem();
    if (enable) {
        vfs_state = VFS_MNGR_STATE_CONNECTED;
        vfs_state_next = VFS_MNGR_STATE_CONNECTED;
        vfs_info.MediaReady = 1;
    } else {
        vfs_state = VFS_MNGR_STATE_DISCONNECTED;
        vfs_state_next = VFS_MNGR_STATE_DISCONNECTED;
        vfs_info.MediaReady = 0;
    }
    vfs_info.MediaChanged = 0;
 }
 void vfs_mngr_periodic(uint32_t elapsed_ms)
 {
    bool change_state;
    vfs_mngr_state_t vfs_state_local;
    vfs_mngr_state_t vfs_state_local_prev;
    sync_assert_usb_thread();
    sync_lock();
    // Return immediately if the desired state has been reached
    if (!changing_state()) {
        sync_unlock();
        return;
    }
    change_state = ready_for_state_change();
    if (time_usb_idle < MAX_EVENT_TIME_MS) {
        time_usb_idle += elapsed_ms;
    }
    if (!change_state) {
        sync_unlock();
        return;
    }
    vfs_printf("vfs_mngr_periodic()\r\n");
    vfs_printf("   time_usb_idle=%i\r\n", time_usb_idle);
    vfs_printf("   transfer_state=%i\r\n", file_transfer_state.transfer_state);
    // Transition to new state
    vfs_state_local_prev = vfs_state;
    vfs_state = vfs_state_next;
    switch (vfs_state) {
        case VFS_MNGR_STATE_RECONNECTING:
            // Transition back to the connected state
            vfs_state_next = VFS_MNGR_STATE_CONNECTED;
            break;
        default:
            // No state change logic required in other states
            break;
    }
    vfs_state_local = vfs_state;
    time_usb_idle = 0;
    sync_unlock();
    // Processing when leaving a state
    vfs_printf("    state %i->%i\r\n", vfs_state_local_prev, vfs_state_local);
    bool want_notify_only = false; // Use this if the transfer timed out and we dont need full reconnect
    switch (vfs_state_local_prev) {
        case VFS_MNGR_STATE_DISCONNECTED:
            // No action needed
            break;
        case VFS_MNGR_STATE_RECONNECTING:
            // No action needed
            break;
        case VFS_MNGR_STATE_CONNECTED:
            // Close ongoing transfer if there is one
            if (file_transfer_state.transfer_state != TRASNFER_FINISHED) {
                vfs_printf("    transfer timeout\r\n");
                file_transfer_state.transfer_timeout = true;
                transfer_update_state(E_SUCCESS);
                want_notify_only = true;
            }
            assert_param(TRASNFER_FINISHED == file_transfer_state.transfer_state);
            vfs_user_disconnecting();
            break;
    }
    // Maybe make this configurable?
    //want_notify_only = false;
    // Processing when entering a state
    switch (vfs_state_local) {
        case VFS_MNGR_STATE_DISCONNECTED:
            vfs_printf("+DISCON");
            if (want_notify_only && !vfs_next_remount_full) {
                vfs_info.MediaChanged = 1;
                vfs_printf("Notify media change");
            } else {
                vfs_info.MediaReady = 0;
                vfs_printf("Reconnect mass storage");
            }
            vfs_next_remount_full = false;
            break;
        case VFS_MNGR_STATE_RECONNECTING:
            vfs_printf("+RECON");
            if (want_notify_only && !vfs_next_remount_full) {
                vfs_info.MediaChanged = 1;
                vfs_printf("Notify media change");
            } else {
                vfs_info.MediaReady = 0;
                vfs_printf("Reconnect mass storage");
            }
            vfs_next_remount_full = false;
            break;
        case VFS_MNGR_STATE_CONNECTED:
            vfs_printf("+CONNECTED");
            build_filesystem();
            vfs_info.MediaReady = 1;
            break;
    }
    return;
 }
 error_t vfs_mngr_get_transfer_status(void)
 {
    sync_assert_usb_thread();
    return fail_reason;
 }
 void vfs_if_usbd_msc_init(void)
 {
    sync_init();
    build_filesystem();
    vfs_state = VFS_MNGR_STATE_DISCONNECTED;
    vfs_state_next = VFS_MNGR_STATE_DISCONNECTED;
    time_usb_idle = 0;
    vfs_info.MediaReady = 0;
    vfs_info.MediaChanged = 0;
    vfs_printf("vfs_if_usbd_msc_init");
 }
 void vfs_if_usbd_msc_read_sect(uint32_t sector, uint8_t *buf, uint32_t num_of_sectors)
 {
    sync_assert_usb_thread();
    // /* this is very spammy */ vfs_printf("\033[35mREAD @ %d, len %d\033[0m", sector, num_of_sectors);
    // dont proceed if we're not ready
    if (!vfs_info.MediaReady) {
        vfs_printf("Not Rdy");
        return;
    }
    // indicate msc activity
 //    main_blink_msc_led(MAIN_LED_OFF);
    vfs_read(sector, buf, num_of_sectors);
 }
 void vfs_if_usbd_msc_write_sect(uint32_t sector, uint8_t *buf, uint32_t num_of_sectors)
 {
    sync_assert_usb_thread();
    vfs_printf("\033[32mWRITE @ %d, len %d\033[0m", sector, num_of_sectors);
    if (buf[0] == 0xF8 && buf[1] == 0xFF && buf[2] == 0xFF && buf[3] == 0xFF) {
        vfs_printf("Discard write of F8,FF,FF,FF");
        return;
    }
    if (!vfs_info.MediaReady) {
        vfs_printf("Not Rdy");
        return;
    }
    // Restart the disconnect counter on every packet
    // so the device does not detach in the middle of a
    // transfer.
    time_usb_idle = 0;
    if (TRASNFER_FINISHED == file_transfer_state.transfer_state) {
        vfs_printf("Xfer done.");
        return;
    }
    // indicate msc activity
 //    main_blink_msc_led(MAIN_LED_OFF);
    vfs_printf("call vfs_write");
    vfs_write(sector, buf, num_of_sectors);
    if (TRASNFER_FINISHED == file_transfer_state.transfer_state) {
        vfs_printf("Xfer done now.");
        return;
    }
    file_data_handler(sector, buf, num_of_sectors);
 }
 static void sync_init(void)
 {
    osMutexStaticDef(vfsMutex, &vfsMutexControlBlock);
    vfsMutexHandle = osMutexCreate(osMutex(vfsMutex));
 }
 static inline void sync_assert_usb_thread(void)
 {
    assert_param(osThreadGetId() == tskMainHandle);
 }
 static void sync_lock(void)
 {
    assert_param(osOK == osMutexWait(vfsMutexHandle, 100));
 }
 static void sync_unlock(void)
 {
    assert_param(osOK == osMutexRelease(vfsMutexHandle));
 }
 static bool changing_state(void)
 {
    return vfs_state != vfs_state_next;
 }
 static void build_filesystem(void)
 {
    // Update anything that could have changed file system state
    file_transfer_state = default_transfer_state;
    vfs_user_build_filesystem();
    vfs_set_file_change_callback(file_change_handler);
    // Set mass storage parameters
    vfs_info.MemorySize = vfs_get_total_size();
    vfs_info.BlockSize = VFS_SECTOR_SIZE;
    vfs_info.BlockGroup = 1;
    vfs_info.BlockCount = vfs_info.MemorySize / vfs_info.BlockSize;
 //    vfs_info.BlockBuf = (uint8_t *) usb_buffer;
 }
 static void switch_to_new_file(stream_type_t stream, uint32_t start_sector, bool andReopen)
 { // This should close the stream
    vfs_printf("****** NEW FILE STREAM! *******");
    if (!file_transfer_state.transfer_state) {
        file_transfer_state.transfer_timeout = true;
        transfer_update_state(E_SUCCESS);
    } else {
        if (file_transfer_state.stream_open) {
            stream_close();
            file_transfer_state.stream_open = false;
        }
    }
    if (andReopen) {
        // and we start anew
        file_transfer_state.start_sector = VFS_INVALID_SECTOR; // pretend we have no srtart sector yet!
        transfer_stream_open(stream, start_sector);
    }
 }
 // Callback to handle changes to the root directory.  Should be used with vfs_set_file_change_callback
 static void file_change_handler(const vfs_filename_t filename, vfs_file_change_t change, vfs_file_t file, vfs_file_t new_file_data)
 {
    vfs_printf("\033[33m@file_change_handler\033[0m (name=%*s, file=%p, ftp=%p, change=%i)\r\n", 11, filename, file, change);
    vfs_user_file_change_handler(filename, change, file, new_file_data);
    if (TRASNFER_FINISHED == file_transfer_state.transfer_state) {
        // If the transfer is finished stop further processing
        vfs_printf("> Transfer is finished.");
        return;
    }
    if (VFS_FILE_CHANGED == change) {
        vfs_printf("> Change");
        if (file == file_transfer_state.file_to_program) {
            vfs_printf("  Stream is open, continue");
            stream_type_t stream;
            uint32_t size = vfs_file_get_size(new_file_data);
            vfs_sector_t sector = vfs_file_get_start_sector(new_file_data);
            stream = stream_type_from_name(filename);
            transfer_update_file_info(file, sector, size, stream);
        } else {
            vfs_printf("  No stream.");
        }
    }
    if (VFS_FILE_CREATED == change) {
        stream_type_t stream;
        vfs_printf("> Created");
        if (STREAM_TYPE_NONE != stream_type_from_name(filename)) {
            vfs_printf("  Stream is open, continue");
            // Check for a know file extension to detect the current file being
            // transferred.  Ignore hidden files since MAC uses hidden files with
            // the same extension to keep track of transfer info in some cases.
            if (!(VFS_FILE_ATTR_HIDDEN & vfs_file_get_attr(new_file_data))) {
                stream = stream_type_from_name(filename);
                uint32_t size = vfs_file_get_size(new_file_data);
                vfs_sector_t sector = vfs_file_get_start_sector(new_file_data);
                transfer_update_file_info(file, sector, size, stream);
            }
        } else {
            vfs_printf("  No matching stream found!");
        }
    }
    if (VFS_FILE_DELETED == change) {
        vfs_printf("> Deleted");
        if (file == file_transfer_state.file_to_program) {
            vfs_printf("  Deleted transferred file!");
            // The file that was being transferred has been deleted
            transfer_reset_file_info();
        } else {
            vfs_printf("Delete other file");
        }
    }
 }
 // Handler for file data arriving over USB.  This function is responsible
 // for detecting the start of a BIN/HEX file and performing programming
 static void file_data_handler(uint32_t sector, const uint8_t *buf, uint32_t num_of_sectors)
 {
    stream_type_t stream;
    uint32_t size;
    vfs_printf("\033[33m@file_data_handler\033[0m (sec=%d, num=%d)", sector, num_of_sectors);
    if (sector <= 1) {
        vfs_printf("Discard write to sector %d", sector);
        return;
    }
    // this is the key for starting a file write - we dont care what file types are sent
    //  just look for something unique (NVIC table, hex, srec, etc) until root dir is updated
    if (!file_transfer_state.stream_started) {
        vfs_printf("Stream not started yet");
        // look for file types we can program
        stream = stream_start_identify((uint8_t *) buf, VFS_SECTOR_SIZE * num_of_sectors);
        if (STREAM_TYPE_NONE != stream) {
            vfs_printf("Opening a stream...");
            transfer_stream_open(stream, sector);
        }
    }
    if (file_transfer_state.stream_started) {
        vfs_printf("Stream is open, check if we can write ....");
 //        // Ignore sectors coming before this file
 //        if (sector < file_transfer_state.start_sector) {
 //            vfs_printf("Sector ABOVE current file!?");
 //            return;
 //        }
        // sectors must be in order
        if (sector != file_transfer_state.file_next_sector) {
            vfs_printf("file_data_handler BAD sector=%i\r\n", sector);
            // Try to find what file this belongs to, if any
            // OS sometimes first writes the FAT and then the individual files,
            // so it looks to us as a discontinuous file (in the better case)
            vfs_filename_t fname;
            vfs_file_t *file;
            if (vfs_find_file(sector, &fname, &file)) {
                vfs_printf("FOUND A FILE!! matches to %s", fname);
                file_transfer_state.file_to_program = file;
                stream = stream_start_identify((uint8_t *) buf, VFS_SECTOR_SIZE * num_of_sectors);
                if (stream != STREAM_TYPE_NONE) {
                    switch_to_new_file(stream, sector, true);
                    goto proceed;
                }
                vfs_printf("No stream can handle this, give up. Could be kateswap junk (1)\r\n");
                return;
            }
            if (sector >= file_transfer_state.start_sector && sector < file_transfer_state.file_next_sector) {
                vfs_printf("    sector out of order! lowest ooo = %i\r\n",
                           file_transfer_state.last_ooo_sector);
                if (VFS_INVALID_SECTOR == file_transfer_state.last_ooo_sector) {
                    file_transfer_state.last_ooo_sector = sector;
                }
                file_transfer_state.last_ooo_sector =
                    MIN(file_transfer_state.last_ooo_sector, sector);
            } else {
                vfs_printf("    sector not part of file transfer\r\n");
                // BUT!! this can be a whole different file written elsewhere
                // Let's try it.
                if (sector > 70) {
                    // this is a guess as to where the actual data can start - usually 34 and 67 are some garbage in the FAT(s)
                    stream = stream_start_identify((uint8_t *) buf, VFS_SECTOR_SIZE * num_of_sectors);
                    if (stream != STREAM_TYPE_NONE) {
                        switch_to_new_file(stream, sector, true);
                        goto proceed;
                    }
                    vfs_printf("No stream can handle this, give up. Could be kateswap junk (2)\r\n");
                    return;
                }
            }
            vfs_printf("    discarding data - size transferred=0x%x\r\n",
                       file_transfer_state.size_transferred);
            vfs_printf_nonl("\033[31m", 5);
            vfs_printf_nonl((const char *) buf, VFS_SECTOR_SIZE * num_of_sectors);
            vfs_printf_nonl("\033[0m\r\n", 6);
            return;
        } else {
            vfs_printf("sector is good");
        }
        proceed:
        // This sector could be part of the file so record it
        size = VFS_SECTOR_SIZE * num_of_sectors;
        file_transfer_state.size_transferred += size;
        file_transfer_state.file_next_sector = sector + num_of_sectors;
        // If stream processing is done then discard the data
        if (file_transfer_state.stream_finished) {
            vfs_printf("vfs_manager file_data_handler\r\n    sector=%i, size=%i\r\n", sector, size);
            vfs_printf("    discarding data - size transferred=0x%x\r\n",
                       file_transfer_state.size_transferred);
            vfs_printf_nonl("\033[31m", 5);
            vfs_printf_nonl((const char *) buf, VFS_SECTOR_SIZE * num_of_sectors);
            vfs_printf_nonl("\033[0m\r\n", 6);
            transfer_update_state(E_SUCCESS);
            return;
        } else {
            vfs_printf("stream is not finished, can handle...");
        }
        transfer_stream_data(sector, buf, size);
    } else {
        vfs_printf("Stream not started!!!!!!");
    }
 }
 static bool ready_for_state_change(void)
 {
    uint32_t timeout_ms = INVALID_TIMEOUT_MS;
    assert_param(vfs_state != vfs_state_next);
    if (VFS_MNGR_STATE_CONNECTED == vfs_state) {
        switch (file_transfer_state.transfer_state) {
            case TRANSFER_NOT_STARTED:
            case TRASNFER_FINISHED:
                timeout_ms = DISCONNECT_DELAY_MS;
                break;
            case TRANSFER_IN_PROGRESS:
                timeout_ms = DISCONNECT_DELAY_TRANSFER_TIMEOUT_MS;
                break;
            case TRANSFER_CAN_BE_FINISHED:
                timeout_ms = DISCONNECT_DELAY_TRANSFER_IDLE_MS;
                break;
            default:
                assert_param(0);
                timeout_ms = DISCONNECT_DELAY_MS;
                break;
        }
    } else if ((VFS_MNGR_STATE_DISCONNECTED == vfs_state) &&
               (VFS_MNGR_STATE_CONNECTED == vfs_state_next)) {
        timeout_ms = CONNECT_DELAY_MS;
    } else if ((VFS_MNGR_STATE_RECONNECTING == vfs_state) &&
               (VFS_MNGR_STATE_CONNECTED == vfs_state_next)) {
        timeout_ms = RECONNECT_DELAY_MS;
    } else if ((VFS_MNGR_STATE_RECONNECTING == vfs_state) &&
               (VFS_MNGR_STATE_DISCONNECTED == vfs_state_next)) {
        timeout_ms = 0;
    }
    if (INVALID_TIMEOUT_MS == timeout_ms) {
        assert_param(0);
        timeout_ms = 0;
    }
    return time_usb_idle > timeout_ms ? true : false;
 }
 // Abort a remount if one is pending
 void abort_remount(void)
 {
    sync_lock();
    // Only abort a remount if in the connected state and reconnecting is the next state
    if ((VFS_MNGR_STATE_RECONNECTING == vfs_state_next) && (VFS_MNGR_STATE_CONNECTED == vfs_state)) {
        vfs_state_next = VFS_MNGR_STATE_CONNECTED;
    }
    sync_unlock();
 }
 // Update the tranfer state with file information
 static void transfer_update_file_info(vfs_file_t file, uint32_t start_sector, uint32_t size, stream_type_t stream)
 {
    vfs_printf("\033[33m@transfer_update_file_info\033[0m (file=%p, start_sector=%i, size=%i)\r\n", file, start_sector, size);
    if (TRASNFER_FINISHED == file_transfer_state.transfer_state) {
        assert_param(0);
        return;
    }
    // Initialize the directory entry if it has not been set
    if (VFS_FILE_INVALID == file_transfer_state.file_to_program) {
        file_transfer_state.file_to_program = file;
        if (file != VFS_FILE_INVALID) {
            vfs_printf("    file_to_program=%p\r\n", file);
        }
    }
    // Initialize the starting sector if it has not been set
    if (VFS_INVALID_SECTOR == file_transfer_state.start_sector) {
        file_transfer_state.start_sector = start_sector;
        if (start_sector != VFS_INVALID_SECTOR) {
            vfs_printf("    start_sector=%i\r\n", start_sector);
        }
    }
    // Initialize the stream if it has not been set
    if (STREAM_TYPE_NONE == file_transfer_state.stream) {
        file_transfer_state.stream = stream;
        if (stream != STREAM_TYPE_NONE) {
            vfs_printf("    stream=%i\r\n", stream);
        }
    }
    // Check - File size must either grow or be smaller than the size already transferred
    if ((size < file_transfer_state.file_size) && (size < file_transfer_state.size_transferred)) {
        vfs_printf("    error: file size changed from %i to %i\r\n", file_transfer_state.file_size, size);
        // this is probably a new file
        trap("File shrinks");//XXX
        switch_to_new_file(stream, start_sector, true);
    }
    // Check - Starting sector must be the same  - this is optional for file info since it may not be present initially
    if ((VFS_INVALID_SECTOR != start_sector) && (start_sector != file_transfer_state.start_sector)) {
        vfs_printf("    error: starting sector changed from %i to %i\r\n", file_transfer_state.start_sector, start_sector);
        // this is probably a new file
        trap("Changed start offset");//XXX
        switch_to_new_file(stream, start_sector, true);
    }
    // Check - stream must be the same
    if (stream != file_transfer_state.stream) {
        vfs_printf("    error: changed types during transfer from %i to %i\r\n", stream, file_transfer_state.stream);
        transfer_update_state(E_ERROR_DURING_TRANSFER);
        return;
    }
    // Update values - Size is the only value that can change
    file_transfer_state.file_size = size;
    vfs_printf("    updated size=%i\r\n", size);
    transfer_update_state(E_SUCCESS);
 }
 // Reset the transfer information or error if transfer is already in progress
 static void transfer_reset_file_info(void)
 {
    vfs_printf("vfs_manager transfer_reset_file_info()\r\n");
    if (file_transfer_state.stream_open) {
        transfer_update_state(E_ERROR_DURING_TRANSFER);
    } else {
        file_transfer_state = default_transfer_state;
        abort_remount();
    }
 }
 // Update the tranfer state with new information
 static void transfer_stream_open(stream_type_t stream, uint32_t start_sector)
 {
    error_t status;
    assert_param(!file_transfer_state.stream_open);
    assert_param(start_sector != VFS_INVALID_SECTOR);
    vfs_printf("\033[33m@transfer_stream_open\033[0m (stream=%i, start_sector=%i)\r\n",
               stream, start_sector);
    // Check - Starting sector must be the same
    if (start_sector != file_transfer_state.start_sector && file_transfer_state.start_sector != VFS_INVALID_SECTOR) {
        vfs_printf("    error: starting sector changed from %i to %i\r\n", file_transfer_state.start_sector, start_sector);
        // this is probably a new file
        switch_to_new_file(stream, start_sector, false);
        file_transfer_state.start_sector = VFS_INVALID_SECTOR;
    }
    // Check - stream must be the same
    if (stream != file_transfer_state.stream && file_transfer_state.stream != STREAM_TYPE_NONE) {
        vfs_printf("    error: changed types during tranfer from %i to %i\r\n", stream, file_transfer_state.stream);
        // this is probably a new file
        switch_to_new_file(stream, start_sector, false);
        file_transfer_state.start_sector = VFS_INVALID_SECTOR;
    }
    // Initialize the starting sector if it has not been set
    if (VFS_INVALID_SECTOR == file_transfer_state.start_sector) {
        file_transfer_state.start_sector = start_sector;
        if (start_sector != VFS_INVALID_SECTOR) {
            vfs_printf("    start_sector=%i\r\n", start_sector);
        }
    }
    // Initialize the stream if it has not been set
    if (STREAM_TYPE_NONE == file_transfer_state.stream) {
        file_transfer_state.stream = stream;
        if (stream != STREAM_TYPE_NONE) {
            vfs_printf("    stream=%i\r\n", stream);
        }
    }
    // Open stream
    status = stream_open(stream);
    vfs_printf("    stream_open stream=%i ret %i\r\n", stream, status);
    if (E_SUCCESS == status) {
        file_transfer_state.file_next_sector = start_sector;
        file_transfer_state.stream_open = true;
        file_transfer_state.stream_started = true;
    }
    transfer_update_state(status);
 }
 // Update the tranfer state with new information
 static void transfer_stream_data(uint32_t sector, const uint8_t *data, uint32_t size)
 {
    error_t status;
    vfs_printf("\033[33m@transfer_stream_data\033[0m (sector=%i, size=%i)\r\n", sector, size);
    vfs_printf("    size processed=0x%x, data=%x,%x,%x,%x,...\r\n",
               file_transfer_state.size_processed, data[0], data[1], data[2], data[3]);
    if (file_transfer_state.stream_finished) {
        assert_param(0);
        return;
    }
    assert_param(size % VFS_SECTOR_SIZE == 0);
    assert_param(file_transfer_state.stream_open);
    status = stream_write((uint8_t *) data, size);
    vfs_printf("    stream_write ret=%i\r\n", status);
    if (E_SUCCESS_DONE == status) {
        // Override status so E_SUCCESS_DONE
        // does not get passed into transfer_update_state
        status = stream_close();
        vfs_printf("    stream_close ret=%i\r\n", status);
        file_transfer_state.stream_open = false;
        file_transfer_state.stream_finished = true;
        file_transfer_state.stream_optional_finish = true;
    } else if (E_SUCCESS_DONE_OR_CONTINUE == status) {
        status = E_SUCCESS;
        file_transfer_state.stream_optional_finish = true;
    } else {
        file_transfer_state.stream_optional_finish = false;
    }
    file_transfer_state.size_processed += size;
    transfer_update_state(status);
 }
 // Check if the current transfer is still in progress, done, or if an error has occurred
 static void transfer_update_state(error_t status)
 {
    bool transfer_timeout;
    bool transfer_started;
    bool transfer_can_be_finished;
    bool transfer_must_be_finished;
    bool out_of_order_sector;
    error_t local_status = status;
    assert_param((status != E_SUCCESS_DONE) &&
                 (status != E_SUCCESS_DONE_OR_CONTINUE));
    if (TRASNFER_FINISHED == file_transfer_state.transfer_state) {
        assert_param(0);
        return;
    }
    // Update file info status.  The end of a file is never known for sure since
    // what looks like a complete file could be part of a file getting flushed to disk.
    // The criteria for an successful optional finish is
    // 1. A file has been detected
    // 2. The size of the file indicated in the root dir has been transferred
    // 3. The file size is greater than zero
    file_transfer_state.file_info_optional_finish =
        (file_transfer_state.file_to_program != VFS_FILE_INVALID) &&
        (file_transfer_state.size_transferred >= file_transfer_state.file_size) &&
        (file_transfer_state.file_size > 0);
    transfer_timeout = file_transfer_state.transfer_timeout;
    transfer_started = (VFS_FILE_INVALID != file_transfer_state.file_to_program) ||
                       (STREAM_TYPE_NONE != file_transfer_state.stream);
    // The transfer can be finished if both file and stream processing
    // can be considered complete
    transfer_can_be_finished = file_transfer_state.file_info_optional_finish &&
                               file_transfer_state.stream_optional_finish;
    // The transfer must be fnished if stream processing is for sure complete
    // and file processing can be considered complete
    transfer_must_be_finished = file_transfer_state.stream_finished &&
                                file_transfer_state.file_info_optional_finish;
    out_of_order_sector = false;
    if (file_transfer_state.last_ooo_sector != VFS_INVALID_SECTOR) {
        assert_param(file_transfer_state.start_sector != VFS_INVALID_SECTOR);
        uint32_t sector_offset = (file_transfer_state.last_ooo_sector -
                                  file_transfer_state.start_sector) * VFS_SECTOR_SIZE;
        if (sector_offset < file_transfer_state.size_processed) {
            // The out of order sector was within the range of data already
            // processed.
            out_of_order_sector = true;
        }
    }
    // Set the transfer state and set the status if necessary
    if (local_status != E_SUCCESS) {
        file_transfer_state.transfer_state = TRASNFER_FINISHED;
    } else if (transfer_timeout) {
        if (out_of_order_sector) {
            local_status = E_OOO_SECTOR;
        } else if (!transfer_started) {
            local_status = E_SUCCESS;
        } else if (transfer_can_be_finished) {
            local_status = E_SUCCESS;
        } else {
            local_status = E_TRANSFER_TIMEOUT;
        }
        file_transfer_state.transfer_state = TRASNFER_FINISHED;
    } else if (transfer_must_be_finished) {
        file_transfer_state.transfer_state = TRASNFER_FINISHED;
    } else if (transfer_can_be_finished) {
        file_transfer_state.transfer_state = TRANSFER_CAN_BE_FINISHED;
    } else if (transfer_started) {
        file_transfer_state.transfer_state = TRANSFER_IN_PROGRESS;
    }
    if (TRASNFER_FINISHED == file_transfer_state.transfer_state) {
        vfs_printf("vfs_manager transfer_update_state(status=%i)\r\n", status);
        vfs_printf("    file=%p, start_sect= %i, size=%i\r\n",
                   file_transfer_state.file_to_program, file_transfer_state.start_sector,
                   file_transfer_state.file_size);
        vfs_printf("    stream=%i, size_processed=%i, opt_finish=%i, timeout=%i\r\n",
                   file_transfer_state.stream, file_transfer_state.size_processed,
                   file_transfer_state.file_info_optional_finish, transfer_timeout);
        // Close the file stream if it is open
        if (file_transfer_state.stream_open) {
            error_t close_status;
            close_status = stream_close();
            vfs_printf("    stream closed ret=%i\r\n", close_status);
            file_transfer_state.stream_open = false;
            if (E_SUCCESS == local_status) {
                local_status = close_status;
            }
        }
        // Set the fail reason
        fail_reason = local_status;
        vfs_printf("    Transfer finished, status: %i=%s\r\n", fail_reason, error_get_string(fail_reason));
    }
    // If this state change is not from aborting a transfer
    // due to a remount then trigger a remount
    if (!transfer_timeout) {
        vfs_printf("~~ request Remount from transfer_update_state()");
        vfs_mngr_fs_remount(false);
    }
 }
--- a/vfs/vfs_manager.h
+++ b/vfs/vfs_manager.h
@ -0,0 +1,91 @@
 /**
 * @file    vfs_manager.h
 * @brief   Methods that build and manipulate a virtual file system
 *
 * DAPLink Interface Firmware
 * Copyright (c) 2009-2016, ARM Limited, All Rights Reserved
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may
 * not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 #ifndef VFS_MANAGER_USER_H
 #define VFS_MANAGER_USER_H
 #include "platform.h"
 #include "virtual_fs.h"
 #ifdef __cplusplus
 extern "C" {
 #endif
 /* Callable from anywhere */
 // Enable or disable the virtual filesystem
 void vfs_mngr_fs_enable(bool enabled);
 // Remount the virtual filesystem
 void vfs_mngr_fs_remount(bool force_full);
 /* Callable only from the thread running the virtual fs */
 // Initialize the VFS manager
 // Must be called after USB has been initialized (usbd_init())
 // Notes: Must only be called from the thread runnning USB
 void vfs_mngr_init(bool enabled);
 // Run the vfs manager state machine
 // Notes: Must only be called from the thread runnning USB
 void vfs_mngr_periodic(uint32_t elapsed_ms);
 // Return the status of the last transfer or E_SUCCESS
 // if none have been performed yet
 error_t vfs_mngr_get_transfer_status(void);
 /* Use functions */
 // Build the filesystem by calling vfs_init and then adding files with vfs_create_file
 void vfs_user_build_filesystem(void);
 // Called when a file on the filesystem changes
 void vfs_user_file_change_handler(const vfs_filename_t filename, vfs_file_change_t change, vfs_file_t file, vfs_file_t new_file_data);
 // Called when VFS is disconnecting
 void vfs_user_disconnecting(void);
 // --- interface ---
 void vfs_if_usbd_msc_init(void);
 void vfs_if_usbd_msc_read_sect(uint32_t sector, uint8_t *buf, uint32_t num_of_sectors);
 void vfs_if_usbd_msc_write_sect(uint32_t sector, uint8_t *buf, uint32_t num_of_sectors);
 typedef struct {
    uint32_t MemorySize;
    uint16_t BlockSize;
    uint32_t BlockGroup; // LUN?
    uint32_t BlockCount;
 //    uint8_t *BlockBuf; // apparently unused :thaenkin:
    bool MediaReady;
    bool MediaChanged;
 } vfs_info_t;
 extern volatile vfs_info_t vfs_info;
 #ifdef __cplusplus
 }
 #endif
 #endif// VFS_MANAGER_USER_H
--- a/Show More
+++ b/Show More