Add 'fw/' from commit '5b94dee9cfb1eaaf28510b843c60355663b660ea'

git-subtree-dir: fw
git-subtree-mainline: 6eddc61626d470363ba464c57a5fc5ec7e8ce329
git-subtree-split: 5b94dee9cfb1eaaf28510b843c60355663b660ea

29 files changed, 7823 insertions, 0 deletions

diff --git a/fw/src/CMakeLists.txt b/fw/src/CMakeLists.txt
new file mode 100644
index 0000000..f2ae83f
--- /dev/null
+++ b/fw/src/CMakeLists.txt
@@ -0,0 +1,69 @@
+set (inc ${CMAKE_SOURCE_DIR}/include)
+
+add_library (usbhost
+	${USART_HELPERS}
+	${inc}/usbh_core.h
+	${inc}/usbh_driver_ac_midi.h
+	${inc}/usbh_driver_gp_xbox.h
+	${inc}/usbh_driver_hid.h
+	${inc}/usbh_driver_hub.h
+	${inc}/usbh_lld_stm32f4.h
+	${inc}/driver/usbh_device_driver.h
+	${inc}/usbh_config.h
+
+	usbh_core.c
+	usbh_driver_ac_midi.c
+	usbh_driver_ac_midi_private.h
+	usbh_driver_gp_xbox.c
+	usbh_driver_hid.c
+	usbh_driver_hub.c
+	usbh_driver_hub_private.h
+	usbh_lld_stm32f4.c
+    usart_helpers.c
+    tinyprintf.c
+    cobs.c
+    noise.c
+    packet_interface.c
+    words.c
+    hid_keycodes.c
+)
+
+add_subdirectory (crypto)
+
+add_definitions (
+    -DBLAKE2S_USE_VECTOR_MATH=0
+)
+
+target_link_libraries (usbhost
+    noise
+	${LIBOPENCM3_LIB}
+)
+
+add_executable (demo
+    rand_stm32.c
+	demo.c
+)
+
+target_link_libraries (demo
+	usbhost
+    noise
+)
+
+add_custom_command (TARGET demo
+	POST_BUILD
+	COMMAND ${CMAKE_OBJCOPY} -Oihex $<TARGET_FILE:demo> ${CMAKE_BINARY_DIR}/demo.hex
+	COMMENT "Generating output files: ${CMAKE_BINARY_DIR}/demo.hex"
+)
+
+add_custom_command (TARGET demo
+	POST_BUILD
+	COMMAND ${CMAKE_SIZE} $<TARGET_FILE:demo>
+	COMMENT "Calculating size of the binary"
+)
+
+add_custom_command (TARGET usbhost
+	POST_BUILD
+	COMMENT "Calculating size of the library"
+	COMMAND ${CMAKE_SIZE} $<TARGET_FILE:usbhost>
+)
+
diff --git a/fw/src/cobs.c b/fw/src/cobs.c
new file mode 100644
index 0000000..19dcad2
--- /dev/null
+++ b/fw/src/cobs.c
@@ -0,0 +1,390 @@
+
+#include <cobs.h>
+
+/*@ requires \valid(dst + (0..dstlen-1));
+  @ requires \valid_read(src + (0..srclen-1));
+  @ requires \separated(dst + (0..dstlen-1), src + (0..srclen-1));
+  @
+  @ behavior valid:
+  @     assumes 0 <= srclen <= 254;
+  @     assumes 0 <= dstlen <= 65535;
+  @     assumes dstlen >= srclen+2;
+  @     assigns dst[0..srclen+1];
+  @     ensures \forall integer i; (0 <= i < srclen && \old(src[i]) != 0) ==> dst[i+1] == src[i];
+  @     ensures \result == srclen+2;
+  @     ensures \forall integer i; 0 <= i <= srclen ==> dst[i] != 0;
+  @     ensures dst[srclen+1] == 0;
+  @
+  @ behavior invalid:
+  @     assumes    srclen < 0 || srclen > 254
+  @             || dstlen < 0 || dstlen > 65535
+  @             || dstlen < srclen+2;
+  @     assigns \nothing;
+  @     ensures \result == -1;
+  @
+  @ complete behaviors;
+  @ disjoint behaviors;
+ @*/
+ssize_t cobs_encode(char *dst, size_t dstlen, char *src, size_t srclen) {
+    if (dstlen > 65535 || srclen > 254)
+        return -1;
+    //@ assert 0 <= dstlen <= 65535 && 0 <= srclen <= 254;
+
+    if (dstlen < srclen+2)
+        return -1;
+    //@ assert 0 <= srclen < srclen+2 <= dstlen;
+
+    size_t p = 0;
+    /*@ loop invariant 0 <= p <= srclen+1;
+      @ loop invariant \forall integer i; 0 <= i < p ==> dst[i] != 0;
+      @ loop invariant \forall integer i; 0 < i < p ==> (src[i-1] != 0 ==> dst[i] == src[i-1]);
+      @ loop assigns p, dst[0..srclen+1];
+      @ loop variant srclen-p+1;
+      @*/
+    while (p <= srclen) {
+
+        char val;
+        if (p != 0 && src[p-1] != 0) {
+            val = src[p-1];
+
+        } else {
+            size_t q = p;
+            /*@ loop invariant 0 <= p <= q <= srclen;
+              @ loop invariant \forall integer i; p <= i < q ==> src[i] != 0;
+              @ loop assigns q;
+              @ loop variant srclen-q;
+              @*/
+            while (q < srclen && src[q] != 0)
+                q++;
+            //@ assert q == srclen || src[q] == 0;
+            //@ assert q <= srclen <= 254;
+            val = (char)q-p+1;
+            //@ assert val != 0;
+        }
+
+        dst[p] = val;
+        p++;
+    }
+
+    dst[p] = 0;
+    //@ assert p == srclen+1;
+
+    return srclen+2;
+}
+
+/*@ requires \valid_read(src + (0..srclen-1));
+  @*/
+#ifndef VERIFICATION
+int cobs_encode_incremental(void *f, int (*output)(void *f, unsigned char c), unsigned char *src, size_t srclen) {
+#else
+int fc_verification_cobs_encode_incremental(unsigned char *src, size_t srclen) {
+#endif
+    //@ ghost unsigned char output_buf[1000000];
+    if (srclen > 254)
+        return -1;
+    //@ assert 0 <= srclen <= 254;
+
+    size_t p = 0;
+    /*@ loop invariant 0 <= p <= srclen+1;
+      @ loop invariant \forall integer i; 0 <= i < p ==> output_buf[i] != 0;
+      @ loop assigns p, output_buf[0..srclen+1];
+      @ loop variant srclen-p+1;
+      @*/
+    while (p <= srclen) {
+
+        unsigned char val;
+        if (p != 0 && src[p-1] != 0) {
+            val = src[p-1];
+
+        } else {
+            size_t q = p;
+            /*@ loop invariant 0 <= p <= q <= srclen;
+              @ loop invariant \forall integer i; p <= i < q ==> src[i] != 0;
+              @ loop assigns q;
+              @ loop variant srclen-q;
+              @*/
+            while (q < srclen && src[q] != 0)
+                q++;
+            val = (unsigned char)q-p+1;
+        }
+
+        //@ ghost output_buf[p] = val;
+#ifndef VERIFICATION
+        int rv = output(f, val);
+        if (rv)
+            return rv;
+#endif
+        p++;
+    }
+
+    //@ assert frame_size: p == srclen+1;
+    //@ assert synchronization_robustness: \forall integer i; 0 <= i < p ==> output_buf[i] != 0;
+#ifndef VERIFICATION
+    int rv = output(f, 0);
+    if (rv)
+        return rv;
+#endif
+
+    return 0;
+}
+
+/*@ requires \valid(dst + (0..dstlen-1));
+  @ requires \valid_read(src + (0..srclen-1));
+  @ requires \separated(dst + (0..dstlen-1), src + (0..srclen-1));
+  @ 
+  @ behavior maybe_valid_frame:
+  @     assumes 1 <= srclen <= dstlen <= 65535;
+  @     assumes \exists integer j; j > 0 && \forall integer i; 0 <= i < j ==> src[i] != 0;
+  @     assumes \exists integer i; 0 <= i < srclen && src[i] == 0;
+  @     assigns dst[0..dstlen-1];
+  @     ensures \result >= 0 || \result == -3;
+  @     ensures \result >= 0 ==> src[\result+1] == 0;
+  @     ensures \result >= 0 ==> (\forall integer i; 0 <= i < \result ==> src[i] != 0);
+  @
+  @ behavior invalid_frame:
+  @     assumes 1 <= srclen <= dstlen <= 65535;
+  @     assumes src[0] == 0 || \forall integer i; 0 <= i < srclen ==> src[i] != 0;
+  @     assigns dst[0..dstlen-1];
+  @     ensures \result == -2;
+  @
+  @ behavior invalid_buffers:
+  @     assumes    dstlen < 0 || dstlen > 65535
+  @             || srclen < 1 || srclen > 65535
+  @             || dstlen < srclen;
+  @     assigns \nothing;
+  @     ensures \result == -1;
+  @
+  @ complete behaviors;
+  @ disjoint behaviors;
+  @*/
+ssize_t cobs_decode(char *dst, size_t dstlen, char *src, size_t srclen) {
+    if (dstlen > 65535 || srclen > 65535)
+        return -1;
+
+    if (srclen < 1)
+        return -1;
+
+    if (dstlen < srclen)
+        return -1;
+
+    size_t p = 1;
+    size_t c = (unsigned char)src[0];
+    //@ assert 0 <= c < 256;
+    //@ assert 0 <= c;
+    //@ assert c < 256;
+    if (c == 0)
+        return -2; /* invalid framing. An empty frame would be [...] 00 01 00, not [...] 00 00 */
+    //@ assert c >= 0;
+    //@ assert c != 0;
+    //@ assert c <= 257;
+    //@ assert c > 0;
+    //@ assert c >= 0 && c != 0 ==> c > 0;
+
+    /*@ //loop invariant \forall integer i; 0 <= i <= p ==> (i == srclen || src[i] != 0);
+      @ loop invariant \forall integer i; 1 <= i < p ==> src[i] != 0;
+      @ loop invariant c > 0;
+      @ loop invariant 1 <= p <= srclen <= dstlen <= 65535;
+      @ loop invariant \separated(dst + (0..dstlen-1), src + (0..srclen-1));
+      @ loop invariant \valid_read(src + (0..srclen-1));
+      @ loop invariant \forall integer i; 1 <= i <= srclen ==> \valid(dst + i - 1);
+      @ loop assigns dst[0..dstlen-1], p, c;
+      @ loop variant srclen-p;
+      @*/
+    while (p < srclen && src[p]) {
+        char val;
+        c--;
+
+        //@ assert src[p] != 0;
+        if (c == 0) {
+            c = (unsigned char)src[p];
+            val = 0;
+        } else {
+            val = src[p];
+        }
+
+        //@ assert 0 <= p-1 <= dstlen-1;
+        dst[p-1] = val;
+        p++;
+    }
+
+    if (p == srclen)
+        return -2; /* Invalid framing. The terminating null byte should always be present in the input buffer. */
+
+    if (c != 1)
+        return -3; /* Invalid framing. The skip counter does not hit the end of the frame. */
+
+    //@ assert 0 < p <= srclen <= 65535;
+    //@ assert src[p] == 0;
+    //@ assert \forall integer i; 1 <= i < p ==> src[i] != 0;
+    return p-1;
+}
+
+void cobs_decode_incremental_initialize(struct cobs_decode_state *state) {
+    state->p = 0;
+    state->c = 0;
+}
+
+/*@ requires    separation: \separated(state, dst + (0..dstlen-1));
+    requires   state_valid: \valid(state);
+    requires     dst_valid: \valid(dst + (0..dstlen-1));
+    requires dstlen_bounds: 0 < dstlen <= INT_MAX;
+    requires       p_valid: 0 <= state->p <= dstlen+1;
+    requires       c_valid: state->p != 0 ==> 1 <= state->c <= 255;
+    
+    // Sanity properties
+    ensures         c_valid: state->p != 0 ==> 1 <= state->c <= 255;
+    ensures   buffer_bounds: 0 <= state->p <= dstlen+1;
+    ensures    return_value: 0 <= \result <= dstlen || \result \in {-1, -2, -3, -4};
+    ensures  reset_on_error: \result < -1 ==> state->p == 0 && state->c == 0;
+
+    // Synchronization properties
+    ensures       self_synchronization: src == 0 ==> \result >= 0 || \result \in {-2, -3};
+    ensures synchronization_robustness: src != 0 ==> \result \in {-1, -4};
+
+    // Basic functional sanity
+    ensures       legal_advance: \result != -1 <==> state->p \in {0, \old(state->p)};
+    ensures incremental_advance: \result == -1 <==> state->p == \old(state->p) + 1;
+    ensures  nonzero_unmodified: \result == -1 && state->p > 1 ==> dst[state->p-2] \in {0, src};
+
+    assigns *state, dst[0..dstlen-1];
+  
+    behavior eof_bad_empty_frame:
+        assumes state->p == 0 && src == 0;
+        ensures \result == -3;
+        assigns *state;
+
+    behavior eof_maybe_good_frame:
+        assumes state->p != 0 && src == 0;
+        ensures \result >= 0 || \result == -2;
+        assigns *state;
+
+    behavior decoding_no_overflow:
+        assumes src != 0 && state->p <= dstlen;
+        ensures \result == -1;
+        assigns *state, dst[0..dstlen-1];
+
+    behavior decoding_overflow:
+        assumes src != 0 && state->p > dstlen;
+        ensures \result == -4;
+        assigns *state;
+
+    complete behaviors;
+    disjoint behaviors;
+  @*/
+int cobs_decode_incremental(struct cobs_decode_state *state, unsigned char *dst, size_t dstlen, unsigned char src) {
+    if (state->p == 0) {
+        if (src == 0) {
+            /* invalid framing. An empty frame would be [...] 00 01 00, not [...] 00 00 */
+            cobs_decode_incremental_initialize(state);
+            return -3;
+        }
+        state->c = src;
+        state->p++;
+        return -1;
+    }
+
+    if (!src) {
+        if (state->c != 1) {
+            /* invalid framing. The skip counter does not hit the end of the frame. */
+            cobs_decode_incremental_initialize(state);
+            return -2;
+        }
+        size_t rv = state->p-1;
+        cobs_decode_incremental_initialize(state);
+        return rv;
+    }
+
+    unsigned char val;
+    state->c--;
+
+    if (state->c == 0) {
+        state->c = src;
+        val = 0;
+    } else {
+        val = src;
+    }
+
+    size_t pos = state->p-1;
+    if (pos >= dstlen) {
+        cobs_decode_incremental_initialize(state);
+        return -4; /* output buffer too small */
+    }
+    dst[pos] = val;
+    state->p++;
+    return -1;
+}
+
+/*@ requires \valid_read(buf + (0..len-1));
+    assigns \nothing;
+  @*/
+void handle_packet(unsigned char *buf, size_t len);
+
+/*@ requires \valid(dst + (0..dstlen-1));
+    requires \valid_read(src + (0..srclen-1));
+    requires 0 < dstlen <= 65535;
+    assigns dst[0..dstlen-1];
+  @*/
+void cobs_decode_test(unsigned char *src, size_t srclen, unsigned char *dst, size_t dstlen) {
+    struct cobs_decode_state state;
+    cobs_decode_incremental_initialize(&state);
+    //@ assert state.p == 0;
+    //@ assert state.c == 0;
+    //@ assert state.p != 0 ==> 1 <= state.c <= 255;
+    /*@ loop invariant \separated(&state, dst + (0..dstlen-1), &i);
+        loop invariant \valid(&state);
+        loop invariant \valid(dst + (0..dstlen-1));
+        loop invariant 0 < dstlen <= 65535;
+        loop invariant 0 <= state.p <= dstlen+1;
+        loop invariant state.p != 0 ==> 1 <= state.c <= 255;
+        loop assigns dst[0..dstlen-1], state, i;
+        loop variant srclen-i;
+      @*/
+    for (size_t i=0; i<srclen; i++) {
+        int rv = cobs_decode_incremental(&state, dst, dstlen, src[i]);
+        if (rv >= 0)
+            handle_packet(dst, rv);
+        if (rv == -1)
+            continue;
+        if (rv < -1)
+            continue;
+    }
+}
+
+#ifdef VALIDATION
+/*@ 
+  @ requires 0 <= d < 256;
+  @ assigns \nothing;
+  @*/
+size_t test(char foo, unsigned int d) {
+    unsigned int c = (unsigned char)foo;
+    if (c != 0) {
+        //@ assert c < 256;
+        //@ assert c >= 0;
+        //@ assert c != 0;
+        //@ assert c > 0;
+    }
+    if (d != 0) {
+        //@ assert d >= 0;
+        //@ assert d != 0;
+        //@ assert d > 0;
+    }
+    return c + d;
+}
+
+#include <__fc_builtin.h>
+
+void main(void) {
+    char inbuf[254];
+    char cobsbuf[256];
+    char outbuf[256];
+
+    size_t range = Frama_C_interval(0, sizeof(inbuf));
+    Frama_C_make_unknown((char *)inbuf, range);
+
+    cobs_encode(cobsbuf, sizeof(cobsbuf), inbuf, sizeof(inbuf));
+    cobs_decode(outbuf, sizeof(outbuf), cobsbuf, sizeof(cobsbuf));
+    
+    //@ assert \forall integer i; 0 <= i < sizeof(inbuf) ==> outbuf[i] == inbuf[i];
+}
+#endif//VALIDATION
+
diff --git a/fw/src/crypto/CMakeLists.txt b/fw/src/crypto/CMakeLists.txt
new file mode 100644
index 0000000..b21dc2b
--- /dev/null
+++ b/fw/src/crypto/CMakeLists.txt
@@ -0,0 +1,93 @@
+
+include_directories (
+    noise-c/include
+    noise-c/include/noise/keys
+    noise-c/src
+    noise-c/src/crypto/goldilocks/include
+    noise-c/src/crypto/goldilocks/src/include
+    noise-c/src/crypto/goldilocks/src/p448/arch_arm_32
+    noise-c/src/crypto/goldilocks/src/p448
+    noise-c/src/protocol
+)
+
+add_library (noise
+    noise-c/src/protocol/util.c
+    noise-c/src/protocol/patterns.c
+    noise-c/src/protocol/signstate.c
+    noise-c/src/protocol/randstate.c
+    noise-c/src/protocol/symmetricstate.c
+    noise-c/src/protocol/internal.c
+    noise-c/src/protocol/names.c
+    noise-c/src/protocol/hashstate.c
+    noise-c/src/protocol/errors.c
+    noise-c/src/protocol/cipherstate.c
+    noise-c/src/protocol/handshakestate.c
+    noise-c/src/protocol/dhstate.c
+    noise-c/src/keys/certificate.c
+    noise-c/src/keys/loader.c
+    noise-c/src/crypto/sha2/sha256.c
+    noise-c/src/crypto/sha2/sha512.c
+    noise-c/src/crypto/ghash/ghash.c
+    noise-c/src/crypto/ed25519/ed25519.c
+    noise-c/src/crypto/blake2/blake2s.c
+    noise-c/src/crypto/blake2/blake2b.c
+    noise-c/src/crypto/chacha/chacha.c
+    noise-c/src/crypto/goldilocks/src/ec_point.c
+    noise-c/src/crypto/goldilocks/src/sha512.c
+    noise-c/src/crypto/goldilocks/src/p448/arch_32/p448.c
+    noise-c/src/crypto/goldilocks/src/p448/f_arithmetic.c
+    noise-c/src/crypto/goldilocks/src/p448/arch_arm_32/p448.c
+    noise-c/src/crypto/goldilocks/src/p448/magic.c
+    noise-c/src/crypto/goldilocks/src/barrett_field.c
+    noise-c/src/crypto/goldilocks/src/goldilocks.c
+    noise-c/src/crypto/goldilocks/src/arithmetic.c
+    noise-c/src/crypto/goldilocks/src/crandom.c
+    noise-c/src/crypto/goldilocks/src/scalarmul.c
+    noise-c/src/crypto/newhope/poly.c
+    noise-c/src/crypto/newhope/randombytes.c
+    noise-c/src/crypto/newhope/reduce.c
+    noise-c/src/crypto/newhope/ntt.c
+    noise-c/src/crypto/newhope/crypto_stream_chacha20.c
+    noise-c/src/crypto/newhope/error_correction.c
+    noise-c/src/crypto/newhope/batcher.c
+    noise-c/src/crypto/newhope/fips202.c
+    noise-c/src/crypto/newhope/newhope.c
+    noise-c/src/crypto/newhope/precomp.c
+    noise-c/src/crypto/aes/rijndael-alg-fst.c
+    noise-c/src/crypto/curve448/curve448.c
+    noise-c/src/crypto/donna/poly1305-donna.c
+    noise-c/src/crypto/donna/curve25519-donna.c
+    noise-c/src/protobufs/protobufs.c
+    noise-c/src/backend/ref/sign-ed25519.c
+    noise-c/src/backend/ref/hash-blake2b.c
+    noise-c/src/backend/ref/hash-sha512.c
+    noise-c/src/backend/ref/hash-sha256.c
+    noise-c/src/backend/ref/cipher-aesgcm.c
+    noise-c/src/backend/ref/cipher-chachapoly.c
+    noise-c/src/backend/ref/dh-curve25519.c
+    noise-c/src/backend/ref/dh-newhope.c
+    noise-c/src/backend/ref/dh-curve448.c
+    noise-c/src/backend/ref/hash-blake2s.c
+)
+
+add_definitions (
+    -DUSE_LIBSODIUM=0
+    -DUSE_SODIUM=0
+    -DHAVE_PTHREAD=0
+    -DUSE_OPENSSL=0
+    -D__WORDSIZE=32
+    -D__BIG_ENDIAN=4321
+    -D__LITTLE_ENDIAN=1234
+    -D__BYTE_ORDER=__LITTLE_ENDIAN
+    -DED25519_CUSTOMRANDOM=1
+    -DED25519_CUSTOMHASH=1
+    -DED25519_REFHASH=1
+    -DBLAKE2S_USE_VECTOR_MATH=0
+    -DEXPERIMENT_CRANDOM_CUTOFF_BYTES=0
+    -D__clang__=0
+)
+
+set (CMAKE_C_FLAGS
+    "${CMAKE_C_FLAGS} -Wno-implicit-fallthrough -Wno-shadow -Wno-unused-parameter"
+)
+
diff --git a/fw/src/demo.c b/fw/src/demo.c
new file mode 100644
index 0000000..4d6a040
--- /dev/null
+++ b/fw/src/demo.c
@@ -0,0 +1,684 @@
+/*

+ * This file is part of the libusbhost library

+ * hosted at http://github.com/libusbhost/libusbhost

+ *

+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>

+ *

+ *

+ * libusbhost is free software: you can redistribute it and/or modify

+ * it under the terms of the GNU Lesser General Public License as published by

+ * the Free Software Foundation, either version 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This library is distributed in the hope that it will be useful,

+ * but WITHOUT ANY WARRANTY; without even the implied warranty of

+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+ * GNU Lesser General Public License for more details.

+ *

+ * You should have received a copy of the GNU Lesser General Public License

+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.

+ *

+ */

+

+#include <libopencm3/stm32/rcc.h>

+#include <libopencm3/stm32/gpio.h>

+#include <libopencm3/stm32/usart.h>

+#include <libopencm3/stm32/timer.h>

+#include <libopencm3/stm32/otg_hs.h>

+#include <libopencm3/stm32/otg_fs.h>

+#include <libopencm3/stm32/pwr.h>

+#include <libopencm3/stm32/dma.h>

+#include <libopencm3/cm3/nvic.h>

+#include <libopencmsis/core_cm3.h>

+

+#include <stdint.h>

+#include <stdlib.h>

+#include <string.h>

+

+#include "usart_helpers.h"

+#include "usbh_core.h"

+#include "usbh_lld_stm32f4.h"

+#include "usbh_driver_hid.h"

+#include "usbh_driver_hub.h"

+#include "rand_stm32.h"

+#include "packet_interface.h"

+#include "noise.h"

+#include "hid_keycodes.h"

+#include "words.h"

+#include "tracing.h"

+

+#include "crypto/noise-c/src/protocol/internal.h"

+

+#ifndef USE_STM32F4_USBH_DRIVER_FS

+#error The full-speed USB driver must be enabled with USE_STM32F4_USBH_DRIVER_FS in usbh_config.h!

+#endif

+

+#ifndef MAX_FAILED_HANDSHAKES

+#define MAX_FAILED_HANDSHAKES 5

+#endif

+

+static struct NoiseState noise_state;

+static struct {

+    union {

+        struct {

+            uint8_t local_key[CURVE25519_KEY_LEN];

+            uint8_t remote_key_reference[BLAKE2S_HASH_SIZE];

+        };

+        uint32_t all_keys[0];

+    } keys;

+    struct {

+        uint8_t identity_key_valid;

+        uint8_t scrub_backup;

+        uint8_t scrubber_armed;

+        uint32_t old_scrub_pattern;

+        uint32_t new_scrub_pattern;

+        int scrub_idx_read;

+        int scrub_idx_done;

+    } mgmt __attribute__((aligned(4)));

+} keystore __attribute__((section(".backup_sram"))) = {0};

+

+

+void _fini(void);

+

+static inline void delay(uint32_t n) {

+	for (volatile uint32_t i = 0; i < 1490*n; i++);

+}

+

+

+/* Set STM32 to 168 MHz. */

+static void clock_setup(void) {

+	rcc_clock_setup_hse_3v3(&hse_8mhz_3v3[CLOCK_3V3_168MHZ]);

+

+	rcc_periph_clock_enable(RCC_GPIOA);

+	rcc_periph_clock_enable(RCC_GPIOB);

+	rcc_periph_clock_enable(RCC_GPIOD);

+	rcc_periph_clock_enable(RCC_GPIOE);

+

+	rcc_periph_clock_enable(RCC_USART1);

+	rcc_periph_clock_enable(RCC_USART2);

+	rcc_periph_clock_enable(RCC_OTGFS);

+	rcc_periph_clock_enable(RCC_TIM6);

+	rcc_periph_clock_enable(RCC_DMA2);

+	rcc_periph_clock_enable(RCC_DMA1);

+

+	rcc_periph_clock_enable(RCC_PWR);

+	rcc_periph_clock_enable(RCC_BKPSRAM);

+

+	rcc_periph_clock_enable(RCC_RNG);

+}

+

+void arm_key_scrubber() {

+    keystore.mgmt.scrubber_armed = 1;

+}

+

+static void finish_scrub(int start_index, uint32_t pattern);

+static void finish_interrupted_scrub(void);

+

+void disarm_key_scrubber() {

+    keystore.mgmt.scrubber_armed = 0;

+    keystore.mgmt.old_scrub_pattern = keystore.mgmt.new_scrub_pattern;

+    keystore.mgmt.new_scrub_pattern = 0x00000000;

+    finish_scrub(0, keystore.mgmt.old_scrub_pattern);

+}

+

+static void finish_scrub(int start_index, uint32_t pattern) {

+    for (size_t i=start_index; i<sizeof(keystore.keys)/sizeof(keystore.keys.all_keys[0]); i++) {

+        keystore.mgmt.scrub_backup = keystore.keys.all_keys[i];

+        keystore.mgmt.scrub_idx_read = i;

+        keystore.keys.all_keys[i] ^= pattern;

+        keystore.mgmt.scrub_idx_done = i;

+    }

+}

+

+static void finish_interrupted_scrub(void) {

+    if (keystore.mgmt.scrub_idx_read != keystore.mgmt.scrub_idx_done)

+        keystore.keys.all_keys[keystore.mgmt.scrub_idx_read] = keystore.mgmt.scrub_backup;

+

+    finish_scrub(keystore.mgmt.scrub_idx_done, keystore.mgmt.old_scrub_pattern ^ keystore.mgmt.new_scrub_pattern);

+}

+

+/* setup 10kHz timer */

+static void tim6_setup(void) {

+	timer_reset(TIM6);

+	timer_set_prescaler(TIM6, 8400 - 1);	// 84Mhz/10kHz - 1

+	timer_set_period(TIM6, 65535);			// Overflow in ~6.5 seconds

+    timer_enable_irq(TIM6, TIM_DIER_UIE);

+    nvic_enable_irq(NVIC_TIM6_DAC_IRQ);

+    nvic_set_priority(NVIC_TIM6_DAC_IRQ, 15<<4); /* really low priority */

+	timer_enable_counter(TIM6);

+}

+

+void tim6_dac_isr(void) {

+    /* Runs every ~6.5s on timer overrun */

+    timer_clear_flag(TIM6, TIM_SR_UIF);

+

+    if (!keystore.mgmt.scrubber_armed)

+        return;

+

+    keystore.mgmt.old_scrub_pattern = keystore.mgmt.new_scrub_pattern;

+    noise_rand_bytes(&keystore.mgmt.new_scrub_pattern, sizeof(keystore.mgmt.new_scrub_pattern));

+    LOG_PRINTF("Scrubbing keys using pattern %08x\n", keystore.mgmt.new_scrub_pattern);

+    finish_scrub(0, keystore.mgmt.old_scrub_pattern ^ keystore.mgmt.new_scrub_pattern);

+}

+

+static uint32_t tim6_get_time_us(void)

+{

+	uint32_t cnt = timer_get_counter(TIM6);

+

+	// convert to 1MHz less precise timer value -> units: microseconds

+	uint32_t time_us = cnt * 100;

+

+	return time_us;

+}

+

+static void gpio_setup(void)

+{

+    /* Tracing */

+	gpio_mode_setup(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, 0xffff);

+

+    /* D2, D3 LEDs */

+	//gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6 | GPIO7);

+	//gpio_set(GPIOA, GPIO6 | GPIO7);

+

+    /* Status LEDs (PE4-15) */

+	gpio_mode_setup(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, 0xfff0);

+

+    /* Alarm LEDs (PA6,7) */

+	gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6 | GPIO7);

+	gpio_set(GPIOA, GPIO6 | GPIO7);

+    

+    /* Speaker */

+	gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO10);

+	gpio_set(GPIOB, GPIO10);

+

+    /* USB OTG FS phy outputs */

+	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO11 | GPIO12);

+	gpio_set_af(GPIOA, GPIO_AF10, GPIO11 | GPIO12);

+

+	/* USART1 (debug) */

+	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9 | GPIO10);

+	gpio_set_af(GPIOA, GPIO_AF7, GPIO9 | GPIO10);

+

+    /* USART2 (host link) */

+	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO2 | GPIO3);

+	gpio_set_af(GPIOA, GPIO_AF7, GPIO2 | GPIO3);

+

+    /* K0 (PE4)/K1 (PE3) buttons */

+	//gpio_mode_setup(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, GPIO3 | GPIO4);

+}

+

+struct hid_report {

+    uint8_t modifiers;

+    uint8_t _reserved;

+    uint8_t keycodes[6];

+} __attribute__((__packed__));

+

+static char pairing_buf[512];

+static size_t pairing_buf_pos = 0;

+

+int pairing_check(struct NoiseState *st, const char *buf);

+void pairing_input(uint8_t modbyte, uint8_t keycode);

+void pairing_parse_report(struct hid_report *buf, uint8_t len);

+

+/* Minimum number of bytes of handshake hash to confirm during pairing */

+#define MIN_PAIRING_SEQUENCE_LENGTH 8

+

+int pairing_check(struct NoiseState *st, const char *buf) {

+    //LOG_PRINTF("Checking pairing\n");

+    const char *p = buf;

+    int idx = 0;

+    do {

+        /* Skip over most special chars */

+        while (*p) {

+            char c = *p;

+            if ('0' <= c && c <= '9') break;

+            if ('a' <= c && c <= 'z') break;

+            if ('A' <= c && c <= 'Z') break;

+            if (c == '-') break;

+            p++;

+        }

+

+        const char *found = strchr(p, ' ');

+        size_t plen = found ? (size_t)(found - p) : strlen(p); /* p >= found */

+

+        while (plen > 0) {

+            char c = p[plen];

+            if ('0' <= c && c <= '9') break;

+            if ('a' <= c && c <= 'z') break;

+            if ('A' <= c && c <= 'Z') break;

+            if (c == '-') break;

+            plen--;

+        }

+        plen++;

+        //LOG_PRINTF("matching: \"%s\" - \"%s\" %d\n", p, p+plen, plen);

+

+        if (strncasecmp(p, "and", plen)) { /* ignore "and" */

+            int num = -1;

+            for (int i=0; i<256; i++) {

+                if ((!strncasecmp(p, even[i], plen) && plen == strlen(even[i]))

+                 || (!strncasecmp(p, odd[i],      plen) && plen == strlen(odd[i]     ))) {

+                    //LOG_PRINTF("    idx=%02d h=%02x i=%02x adj=%s n=%s plen=%d s=%s\n", idx, st->handshake_hash[idx], i, adjectives[i], nouns[i], plen, p);

+                    num = i;

+                    break;

+                }

+            }

+            if (num == -1) {

+                LOG_PRINTF("Pairing word \"%s\" not found in dictionary\n", p);

+                return -1;

+            }

+            if (st->handshake_hash[idx] != num) {

+                LOG_PRINTF("Pairing data does not match hash\n");

+                return -1;

+            }

+            idx++;

+        }

+

+        p = strchr(p, ' ');

+        if (!p)

+            break; /* end of string */

+        p++; /* skip space */

+    } while (idx < BLAKE2S_HASH_SIZE);

+

+    if (idx < MIN_PAIRING_SEQUENCE_LENGTH) {

+        LOG_PRINTF("Pairing sequence too short, only %d bytes of hash checked\n", idx);

+        return -1;

+    }

+

+    LOG_PRINTF("Pairing sequence match\n");

+    return 0;

+}

+

+void pairing_input(uint8_t modbyte, uint8_t keycode) {

+    char ch = 0;

+    uint8_t level = modbyte & MOD_XSHIFT ? LEVEL_SHIFT : LEVEL_NONE;

+    switch (keycode) {

+        case KEY_ENTER:

+            pairing_buf[pairing_buf_pos++] = '\0';

+            if (!pairing_check(&noise_state, pairing_buf)) {

+                persist_remote_key(&noise_state);

+                /* FIXME write key to backup memory */

+

+                uint8_t response = REPORT_PAIRING_SUCCESS;

+                if (send_encrypted_message(&noise_state, &response, sizeof(response)))

+                    LOG_PRINTF("Error sending pairing response packet\n");

+

+            } else {

+                /* FIXME sound alarm */

+

+                pairing_buf_pos = 0; /* Reset input buffer */

+                uint8_t response = REPORT_PAIRING_ERROR;

+                if (send_encrypted_message(&noise_state, &response, sizeof(response)))

+                    LOG_PRINTF("Error sending pairing response packet\n");

+            }

+            break;

+

+        case KEY_BACKSPACE:

+            if (pairing_buf_pos > 0)

+                pairing_buf_pos--;

+            pairing_buf[pairing_buf_pos] = '\0';

+            ch = '\b';

+            break;

+

+        default:

+            for (size_t i=0; keycode_mapping[i].kc != KEY_NONE; i++) {

+                if (keycode_mapping[i].kc == keycode) {

+                    ch = keycode_mapping[i].ch[level];

+                    if (!ch)

+                        break;

+

+                    if (pairing_buf_pos < sizeof(pairing_buf)-1) /* allow for terminating null byte */ {

+                        pairing_buf[pairing_buf_pos++] = ch;

+                        pairing_buf[pairing_buf_pos] = '\0';

+                    } else {

+                        LOG_PRINTF("Pairing confirmation user input buffer full\n");

+

+                        uint8_t response = REPORT_PAIRING_ERROR;

+                        if (send_encrypted_message(&noise_state, &response, sizeof(response)))

+                            LOG_PRINTF("Error sending pairing response packet\n");

+                    }

+                    break;

+                }

+            }

+            break;

+    }

+

+    if (ch) {

+        //LOG_PRINTF("Input: %s\n", pairing_buf);

+        struct hid_report_packet pkt = {

+            .type = REPORT_PAIRING_INPUT,

+            .pairing_input = { .c = ch }

+        };

+        if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) {

+            LOG_PRINTF("Error sending pairing input packet\n");

+            return;

+        }

+    }

+}

+

+void pairing_parse_report(struct hid_report *buf, uint8_t len) {

+    static uint8_t old_keycodes[6] = {0};

+

+    for (int i=0; i<len-2; i++) {

+        if (!buf->keycodes[i])

+            break; /* keycodes are always populated from low to high */

+

+        int found = 0;

+        for (int j=0; j<6; j++) {

+            if (old_keycodes[j] == buf->keycodes[i]) {

+                found = 1;

+                break;

+            }

+        }

+        if (!found) /* key pressed */

+            pairing_input(buf->modifiers, buf->keycodes[i]);

+    }

+

+    memcpy(old_keycodes, buf->keycodes, 6);

+}

+

+static void hid_in_message_handler(uint8_t device_id, const uint8_t *data, uint32_t length) {

+    TRACING_SET(TR_HID_MESSAGE_HANDLER);

+	if (length < 4 || length > 8) {

+		LOG_PRINTF("HID report length must be 4 < len < 8, is %d bytes\n", length);

+        TRACING_CLEAR(TR_HID_MESSAGE_HANDLER);

+		return;

+	}

+

+	//LOG_PRINTF("Sending event %02X %02X %02X %02X\n", data[0], data[1], data[2], data[3]);

+	int type = hid_get_type(device_id);

+    if (type != HID_TYPE_KEYBOARD && type != HID_TYPE_MOUSE) {

+        LOG_PRINTF("Unsupported HID report type %x\n", type);

+        TRACING_CLEAR(TR_HID_MESSAGE_HANDLER);

+        return;

+    }

+

+    if (noise_state.handshake_state == HANDSHAKE_DONE_UNKNOWN_HOST) {

+        if (type == HID_TYPE_KEYBOARD)

+            pairing_parse_report((struct hid_report *)data, length);

+        else

+            LOG_PRINTF("Not sending HID mouse report during pairing\n");

+        TRACING_CLEAR(TR_HID_MESSAGE_HANDLER);

+        return;

+    }

+

+    struct hid_report_packet pkt = {

+        .type = type == HID_TYPE_KEYBOARD ? REPORT_KEYBOARD : REPORT_MOUSE,

+        .report = {

+            .len = length,

+            .report = {0}

+        }

+    };

+    memcpy(pkt.report.report, data, length);

+

+    if (send_encrypted_message(&noise_state, (uint8_t *)&pkt, sizeof(pkt))) {

+        LOG_PRINTF("Error sending HID report packet\n");

+        TRACING_CLEAR(TR_HID_MESSAGE_HANDLER);

+        return;

+    }

+    TRACING_CLEAR(TR_HID_MESSAGE_HANDLER);

+}

+

+volatile struct {

+    struct dma_buf dma;

+    uint8_t data[256];

+} debug_buf = { .dma = { .len = sizeof(debug_buf.data) } };

+

+struct dma_usart_file debug_out_s = {

+    .usart = DEBUG_USART,

+    .baudrate = DEBUG_USART_BAUDRATE,

+    .dma = DMA(DEBUG_USART_DMA_NUM),

+    .stream = DEBUG_USART_DMA_STREAM_NUM,

+    .channel = DEBUG_USART_DMA_CHANNEL_NUM,

+    .irqn = NVIC_DMA_IRQ(DEBUG_USART_DMA_NUM, DEBUG_USART_DMA_STREAM_NUM),

+    .buf = &debug_buf.dma

+};

+struct dma_usart_file *debug_out = &debug_out_s;

+

+/* FIXME start unsafe debug code */

+void usart1_isr(void) {

+    if (USART1_SR & USART_SR_ORE) { /* Overrun handling */

+        LOG_PRINTF("USART1 data register overrun\n");

+        /* Clear interrupt flag */

+        int dummy = USART1_DR;

+        return;

+    }

+

+    uint8_t data = USART1_DR; /* This automatically acknowledges the IRQ */

+    for (size_t i=0; keycode_mapping[i].kc != KEY_NONE; i++) {

+        struct hid_report report = {0};

+        if (keycode_mapping[i].ch[0] == data)

+            report.modifiers = 0;

+        else if (keycode_mapping[i].ch[1] == data)

+            report.modifiers = MOD_LSHIFT;

+        else continue;

+

+        report.keycodes[0] = keycode_mapping[i].kc;

+        pairing_parse_report(&report, 8);

+        break;

+    }

+    LOG_PRINTF(" %02x ", data);

+    if (data == 0x7f) {

+        struct hid_report report = {.modifiers=0, .keycodes={KEY_BACKSPACE, 0}};

+        pairing_parse_report(&report, 8);

+    } else if (data == '\r') {

+        struct hid_report report = {.modifiers=0, .keycodes={KEY_ENTER, 0}};

+        pairing_parse_report(&report, 8);

+        LOG_PRINTF("\n");

+    }

+

+    struct hid_report report = {0};

+    pairing_parse_report(&report, 8);

+}

+/* end unsafe debug code */

+

+void DMA_ISR(DEBUG_USART_DMA_NUM, DEBUG_USART_DMA_STREAM_NUM)(void) {

+    TRACING_SET(TR_DEBUG_OUT_DMA_IRQ);

+    if (dma_get_interrupt_flag(debug_out->dma, debug_out->stream, DMA_FEIF)) {

+        /* Ignore FIFO errors as they're 100% non-critical for UART applications */

+        dma_clear_interrupt_flags(debug_out->dma, debug_out->stream, DMA_FEIF);

+        TRACING_CLEAR(TR_DEBUG_OUT_DMA_IRQ);

+        return;

+    }

+

+    /* Transfer complete */

+	dma_clear_interrupt_flags(debug_out->dma, debug_out->stream, DMA_TCIF);

+

+    if (debug_out->buf->wr_pos != debug_out->buf->xfr_end) /* buffer not empty */

+        schedule_dma(debug_out);

+    TRACING_CLEAR(TR_DEBUG_OUT_DMA_IRQ);

+}

+

+/*@ requires \valid_read(&pkt->type) && \valid_read(pkt->payload + (0..payload_length-1));

+    requires \valid(st);

+    requires \valid(st->handshake);

+    requires \separated(st, st->rx_cipher, st->tx_cipher, st->handshake, (uint8_t *)pkt->payload, &usart2_out, &st->handshake_hash);

+    requires \valid(usart2_out);

+

+    assigns pairing_buf_pos, *usart2_out, *st;

+

+    assigns st->handshake, st->handshake_state, st->rx_cipher, st->tx_cipher;

+  @*/

+void handle_host_packet(struct NoiseState *st, const struct control_packet *pkt, size_t payload_length) {

+    TRACING_SET(TR_HOST_PKT_HANDLER);

+    if (pkt->type == HOST_INITIATE_HANDSHAKE) {

+        /* It is important that we acknowledge this command right away. Starting the handshake involves key

+         * generation which takes a few milliseconds. If we'd acknowledge this later, we might run into an

+         * overrun here since we would be blocking the buffer during key generation. */

+

+        if (payload_length > 0) {

+            LOG_PRINTF("Extraneous data in INITIATE_HANDSHAKE message\n");

+        } else if (st->failed_handshakes < MAX_FAILED_HANDSHAKES) {

+            LOG_PRINTF("Starting noise protocol handshake...\n");

+            if (reset_protocol_handshake(st))

+                LOG_PRINTF("Error starting protocol handshake.\n");

+            pairing_buf_pos = 0; /* Reset channel binding keyboard input buffer */

+        } else {

+            LOG_PRINTF("Too many failed handshake attempts, not starting another one\n");

+            struct control_packet out = { .type=HOST_TOO_MANY_FAILS };

+            send_packet(usart2_out, (uint8_t *)&out, sizeof(out));

+        }

+

+    } else if (pkt->type == HOST_HANDSHAKE) {

+        LOG_PRINTF("Handling handshake packet of length %d\n", payload_length);

+        TRACING_SET(TR_NOISE_HANDSHAKE);

+        if (try_continue_noise_handshake(st, pkt->payload, payload_length)) {

+            TRACING_CLEAR(TR_NOISE_HANDSHAKE);

+            LOG_PRINTF("Reporting handshake error to host\n");

+            struct control_packet out = { .type=HOST_CRYPTO_ERROR };

+            send_packet(usart2_out, (uint8_t *)&out, sizeof(out));

+        } else TRACING_CLEAR(TR_NOISE_HANDSHAKE);

+

+    } else {

+        LOG_PRINTF("Unhandled packet of type %d\n", pkt->type);

+    }

+    TRACING_CLEAR(TR_HOST_PKT_HANDLER);

+}

+

+

+int main(void)

+{

+	clock_setup();

+	gpio_setup();

+    pwr_disable_backup_domain_write_protect();

+    PWR_CSR |= PWR_CSR_BRE; /* Enable backup SRAM battery power regulator */

+

+    finish_interrupted_scrub();

+    disarm_key_scrubber();

+	tim6_setup();

+

+#ifdef USART_DEBUG

+    usart_dma_init(debug_out);

+    /* FIXME start unsafe debug code */

+    usart_enable_rx_interrupt(debug_out->usart);

+    nvic_enable_irq(NVIC_USART1_IRQ);

+    nvic_set_priority(NVIC_USART1_IRQ, 3<<4);

+    /* end unsafe debug code */

+#endif

+

+    usart_dma_init(usart2_out);

+    usart_enable_rx_interrupt(USART2);

+    nvic_enable_irq(NVIC_USART2_IRQ);

+    nvic_set_priority(NVIC_USART2_IRQ, 3<<4);

+    nvic_set_priority(debug_out_s.irqn, 1<<4);

+

+	LOG_PRINTF("\n==================================\n");

+	LOG_PRINTF("SecureHID device side initializing\n");

+	LOG_PRINTF("==================================\n");

+

+    LOG_PRINTF("Initializing USB...\n");

+    const hid_config_t hid_config = { .hid_in_message_handler = &hid_in_message_handler };

+	hid_driver_init(&hid_config);

+	hub_driver_init();

+    const usbh_dev_driver_t *device_drivers[] = { &usbh_hub_driver, &usbh_hid_driver, NULL };

+    const usbh_low_level_driver_t * const lld_drivers[] = { &usbh_lld_stm32f4_driver_fs, NULL };

+	usbh_init(lld_drivers, device_drivers);

+

+	LOG_PRINTF("Initializing RNG...\n");

+    rand_init();

+

+    //@ assert \valid(&noise_state);

+    //@ assert \valid((uint8_t *)keystore.keys.remote_key_reference + (0..31)) && \valid((uint8_t *)keystore.keys.local_key + (0..31));

+    noise_state_init(&noise_state, keystore.keys.remote_key_reference, keystore.keys.local_key);

+

+    //@ assert \valid(noise_state.local_key + (0..31));

+    /* FIXME load remote key from backup memory */

+    /* FIXME only run this on first boot and persist key in backup sram. Allow reset via jumper-triggered factory reset function. */

+    if (!keystore.mgmt.identity_key_valid) {

+        LOG_PRINTF("Generating identity key...\n");

+        if (generate_identity_key(&noise_state)) {

+            LOG_PRINTF("Error generating identiy key\n");

+        } else {

+            keystore.mgmt.identity_key_valid = 1;

+        }

+    }

+

+    int poll_ctr = 0;

+    int led_ctr = 0;

+    int led_idx = 0;

+    int spk_ctr = 0;

+    int spk_ctr2 = 0;

+    int spk_adv = 0;

+    int spk_inc = 1;

+    gpio_clear(GPIOA, GPIO6);

+    gpio_clear(GPIOA, GPIO7);

+    gpio_clear(GPIOB, GPIO10);

+	while (23) {

+        delay(1);

+

+        led_ctr++;

+        if (led_ctr == 10) {

+            gpio_clear(GPIOA, GPIO6);

+            gpio_clear(GPIOA, GPIO7);

+        } else if (led_ctr == 300) {

+            gpio_mode_setup(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, 0xfff0);

+        } else if (led_ctr == 400) {

+            if (++led_idx == 12)

+                led_idx = 0;

+            gpio_mode_setup(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, 1<<(4+led_idx));

+            gpio_clear(GPIOE, 0xfff0);

+            if (led_idx & 1)

+                gpio_set(GPIOA, GPIO6);

+            else

+                gpio_set(GPIOA, GPIO7);

+            led_ctr = 0;

+        }

+

+        spk_ctr++;

+        spk_ctr2++;

+        if (spk_ctr2 == 100) {

+            spk_adv += spk_inc;

+            if (spk_adv > 31)

+                spk_inc = -3;

+            if (spk_adv < 1)

+                spk_inc = 1;

+            spk_ctr2 = 0;

+        }

+        if (spk_ctr%spk_adv == 0) {

+            gpio_set(GPIOB, GPIO10);

+        } else {

+            gpio_clear(GPIOB, GPIO10);

+        }

+        continue;

+

+        if (++poll_ctr == 10) {

+            poll_ctr = 0;

+            TRACING_SET(TR_USBH_POLL);

+            usbh_poll(tim6_get_time_us());

+            TRACING_CLEAR(TR_USBH_POLL);

+        }

+

+        if (host_packet_length > 0) {

+            handle_host_packet(&noise_state, (struct control_packet *)host_packet_buf, host_packet_length - 1);

+            host_packet_length = 0; /* Acknowledge to USART ISR the buffer has been handled */

+

+        } else if (host_packet_length < 0) { /* USART error */

+            host_packet_length = 0; /* Acknowledge to USART ISR the error has been handled */

+            if (noise_state.handshake_state < HANDSHAKE_DONE_UNKNOWN_HOST) {

+                LOG_PRINTF("USART error, aborting handshake\n");

+

+                struct control_packet pkt = { .type=HOST_COMM_ERROR };

+                send_packet(usart2_out, (uint8_t *)&pkt, sizeof(pkt));

+

+                if (reset_protocol_handshake(&noise_state))

+                    LOG_PRINTF("Error starting protocol handshake.\n");

+

+                pairing_buf_pos = 0; /* Reset channel binding keyboard input buffer */

+            }

+        }

+

+        if (noise_state.handshake_state == HANDSHAKE_IN_PROGRESS) {

+            TRACING_SET(TR_NOISE_HANDSHAKE);

+            if (try_continue_noise_handshake(&noise_state, NULL, 0)) { /* handle outgoing messages */

+                TRACING_CLEAR(TR_NOISE_HANDSHAKE);

+                LOG_PRINTF("Reporting handshake error to host\n");

+                struct control_packet pkt = { .type=HOST_CRYPTO_ERROR };

+                send_packet(usart2_out, (uint8_t *)&pkt, sizeof(pkt));

+            } else TRACING_CLEAR(TR_NOISE_HANDSHAKE);

+        }

+	}

+}

+

+void _fini() {

+    while (1);

+}

+

diff --git a/fw/src/frama_c_cmdline b/fw/src/frama_c_cmdline
new file mode 100644
index 0000000..d6c6adc
--- /dev/null
+++ b/fw/src/frama_c_cmdline
@@ -0,0 +1 @@
+frama-c-gui -c11 -cpp-extra-args="-DVERIFICATION -DSTM32F4 -DUSE_STM32F4_USBH_DRIVER_FS -DDEBUG_USART=USART1 -DDEBUG_USART_BAUDRATE=115200 -DDEBUG_USART_DMA_CHANNEL_NUM=4 -DDEBUG_USART_DMA_NUM=2 -DDEBUG_USART_DMA_STREAM_NUM=7 -I. -I"(frama-c -print-path)"/libc -I crypto/noise-c/include -I../include -I../libopencm3/include" -wp -wp-rte -wp-model +cint -wp-verbose 2 -slevel 8 -wp-out goals noise.c demo.c -wp-fct try_continue_noise_handshake
diff --git a/fw/src/hid_keycodes.c b/fw/src/hid_keycodes.c
new file mode 100644
index 0000000..ac3899a
--- /dev/null
+++ b/fw/src/hid_keycodes.c
@@ -0,0 +1,48 @@
+
+#include "hid_keycodes.h"
+
+struct keymap_entry keycode_mapping[] = {
+    { KEY_A, {'a', 'A'}},
+    { KEY_B, {'b', 'B'}},
+    { KEY_C, {'c', 'C'}},
+    { KEY_D, {'d', 'D'}},
+    { KEY_E, {'e', 'E'}},
+    { KEY_F, {'f', 'F'}},
+    { KEY_G, {'g', 'G'}},
+    { KEY_H, {'h', 'H'}},
+    { KEY_I, {'i', 'I'}},
+    { KEY_J, {'j', 'J'}},
+    { KEY_K, {'k', 'K'}},
+    { KEY_L, {'l', 'L'}},
+    { KEY_M, {'m', 'M'}},
+    { KEY_N, {'n', 'N'}},
+    { KEY_O, {'o', 'O'}},
+    { KEY_P, {'p', 'P'}},
+    { KEY_Q, {'q', 'Q'}},
+    { KEY_R, {'r', 'R'}},
+    { KEY_S, {'s', 'S'}},
+    { KEY_T, {'t', 'T'}},
+    { KEY_U, {'u', 'U'}},
+    { KEY_V, {'v', 'V'}},
+    { KEY_W, {'w', 'W'}},
+    { KEY_X, {'x', 'X'}},
+    { KEY_Y, {'y', 'Y'}},
+    { KEY_Z, {'z', 'Z'}},
+    { KEY_1, {'1', '!'}},
+    { KEY_2, {'2', '@'}},
+    { KEY_3, {'3', '#'}},
+    { KEY_4, {'4', '$'}},
+    { KEY_5, {'5', '%'}},
+    { KEY_6, {'6', '^'}},
+    { KEY_7, {'7', '&'}},
+    { KEY_8, {'8', '*'}},
+    { KEY_9, {'9', '('}},
+    { KEY_0, {'0', ')'}},
+    { KEY_MINUS,     {'-', '_'}},
+    { KEY_SPACE,     {' ', ' '}},
+    { KEY_COMMA,     {',', '<'}},
+    { KEY_DOT,       {'.', '>'}},
+    { KEY_SEMICOLON, {';', ':'}},
+    { KEY_NONE, {0, 0}}, /* end marker */
+};
+
diff --git a/fw/src/hid_keycodes.h b/fw/src/hid_keycodes.h
new file mode 100644
index 0000000..f269e5b
--- /dev/null
+++ b/fw/src/hid_keycodes.h
@@ -0,0 +1,218 @@
+#ifndef __HID_KEYCODES_H__
+#define __HID_KEYCODES_H__
+
+enum mod_levels {
+    LEVEL_NONE,
+    LEVEL_SHIFT,
+    LEVEL_NLEVELS
+};
+
+enum mod_bits {
+    MOD_LCTRL,
+    MOD_LSHIFT,
+    MOD_LALT,
+    MOD_LMETA,
+    MOD_RCTRL,
+    MOD_RSHIFT,
+    MOD_RALT,
+    MOD_RMETA,
+};
+
+enum mod_bitmaps {
+    MOD_XCTRL  = MOD_LCTRL  | MOD_RCTRL,
+    MOD_XSHIFT = MOD_LSHIFT | MOD_RSHIFT,
+    MOD_XALT   = MOD_LALT   | MOD_RALT,
+    MOD_XMETA  = MOD_LMETA  | MOD_RMETA,
+};
+
+struct keymap_entry {
+    unsigned char kc;
+    char ch[LEVEL_NLEVELS];
+};
+
+extern struct keymap_entry keycode_mapping[];
+
+enum hid_keycode {
+    KEY_NONE = 0x00, // No key pressed
+    KEY_ERR_OVF = 0x01, //  Keyboard Error Roll Over - used for all slots if too many keys are pressed ("Phantom key")
+    KEY_A = 0x04, // Keyboard a and A
+    KEY_B = 0x05, // Keyboard b and B
+    KEY_C = 0x06, // Keyboard c and C
+    KEY_D = 0x07, // Keyboard d and D
+    KEY_E = 0x08, // Keyboard e and E
+    KEY_F = 0x09, // Keyboard f and F
+    KEY_G = 0x0a, // Keyboard g and G
+    KEY_H = 0x0b, // Keyboard h and H
+    KEY_I = 0x0c, // Keyboard i and I
+    KEY_J = 0x0d, // Keyboard j and J
+    KEY_K = 0x0e, // Keyboard k and K
+    KEY_L = 0x0f, // Keyboard l and L
+    KEY_M = 0x10, // Keyboard m and M
+    KEY_N = 0x11, // Keyboard n and N
+    KEY_O = 0x12, // Keyboard o and O
+    KEY_P = 0x13, // Keyboard p and P
+    KEY_Q = 0x14, // Keyboard q and Q
+    KEY_R = 0x15, // Keyboard r and R
+    KEY_S = 0x16, // Keyboard s and S
+    KEY_T = 0x17, // Keyboard t and T
+    KEY_U = 0x18, // Keyboard u and U
+    KEY_V = 0x19, // Keyboard v and V
+    KEY_W = 0x1a, // Keyboard w and W
+    KEY_X = 0x1b, // Keyboard x and X
+    KEY_Y = 0x1c, // Keyboard y and Y
+    KEY_Z = 0x1d, // Keyboard z and Z
+
+    KEY_1 = 0x1e, // Keyboard 1 and !
+    KEY_2 = 0x1f, // Keyboard 2 and @
+    KEY_3 = 0x20, // Keyboard 3 and #
+    KEY_4 = 0x21, // Keyboard 4 and $
+    KEY_5 = 0x22, // Keyboard 5 and %
+    KEY_6 = 0x23, // Keyboard 6 and ^
+    KEY_7 = 0x24, // Keyboard 7 and &
+    KEY_8 = 0x25, // Keyboard 8 and *
+    KEY_9 = 0x26, // Keyboard 9 and (
+    KEY_0 = 0x27, // Keyboard 0 and )
+
+    KEY_ENTER = 0x28, // Keyboard Return (ENTER)
+    KEY_ESC = 0x29, // Keyboard ESCAPE
+    KEY_BACKSPACE = 0x2a, // Keyboard DELETE (Backspace)
+    KEY_TAB = 0x2b, // Keyboard Tab
+    KEY_SPACE = 0x2c, // Keyboard Spacebar
+    KEY_MINUS = 0x2d, // Keyboard - and _
+    KEY_EQUAL = 0x2e, // Keyboard = and +
+    KEY_LEFTBRACE = 0x2f, // Keyboard [ and {
+    KEY_RIGHTBRACE = 0x30, // Keyboard ] and }
+    KEY_BACKSLASH = 0x31, // Keyboard \ and |
+    KEY_HASHTILDE = 0x32, // Keyboard Non-US # and ~
+    KEY_SEMICOLON = 0x33, // Keyboard ; and :
+    KEY_APOSTROPHE = 0x34, // Keyboard ' and "
+    KEY_GRAVE = 0x35, // Keyboard ` and ~
+    KEY_COMMA = 0x36, // Keyboard , and <
+    KEY_DOT = 0x37, // Keyboard . and >
+    KEY_SLASH = 0x38, // Keyboard / and ?
+    KEY_CAPSLOCK = 0x39, // Keyboard Caps Lock
+
+    KEY_F1 = 0x3a, // Keyboard F1
+    KEY_F2 = 0x3b, // Keyboard F2
+    KEY_F3 = 0x3c, // Keyboard F3
+    KEY_F4 = 0x3d, // Keyboard F4
+    KEY_F5 = 0x3e, // Keyboard F5
+    KEY_F6 = 0x3f, // Keyboard F6
+    KEY_F7 = 0x40, // Keyboard F7
+    KEY_F8 = 0x41, // Keyboard F8
+    KEY_F9 = 0x42, // Keyboard F9
+    KEY_F10 = 0x43, // Keyboard F10
+    KEY_F11 = 0x44, // Keyboard F11
+    KEY_F12 = 0x45, // Keyboard F12
+
+    KEY_SYSRQ = 0x46, // Keyboard Print Screen
+    KEY_SCROLLLOCK = 0x47, // Keyboard Scroll Lock
+    KEY_PAUSE = 0x48, // Keyboard Pause
+    KEY_INSERT = 0x49, // Keyboard Insert
+    KEY_HOME = 0x4a, // Keyboard Home
+    KEY_PAGEUP = 0x4b, // Keyboard Page Up
+    KEY_DELETE = 0x4c, // Keyboard Delete Forward
+    KEY_END = 0x4d, // Keyboard End
+    KEY_PAGEDOWN = 0x4e, // Keyboard Page Down
+    KEY_RIGHT = 0x4f, // Keyboard Right Arrow
+    KEY_LEFT = 0x50, // Keyboard Left Arrow
+    KEY_DOWN = 0x51, // Keyboard Down Arrow
+    KEY_UP = 0x52, // Keyboard Up Arrow
+
+    KEY_NUMLOCK = 0x53, // Keyboard Num Lock and Clear
+    KEY_KPSLASH = 0x54, // Keypad /
+    KEY_KPASTERISK = 0x55, // Keypad *
+    KEY_KPMINUS = 0x56, // Keypad -
+    KEY_KPPLUS = 0x57, // Keypad +
+    KEY_KPENTER = 0x58, // Keypad ENTER
+    KEY_KP1 = 0x59, // Keypad 1 and End
+    KEY_KP2 = 0x5a, // Keypad 2 and Down Arrow
+    KEY_KP3 = 0x5b, // Keypad 3 and PageDn
+    KEY_KP4 = 0x5c, // Keypad 4 and Left Arrow
+    KEY_KP5 = 0x5d, // Keypad 5
+    KEY_KP6 = 0x5e, // Keypad 6 and Right Arrow
+    KEY_KP7 = 0x5f, // Keypad 7 and Home
+    KEY_KP8 = 0x60, // Keypad 8 and Up Arrow
+    KEY_KP9 = 0x61, // Keypad 9 and Page Up
+    KEY_KP0 = 0x62, // Keypad 0 and Insert
+    KEY_KPDOT = 0x63, // Keypad . and Delete
+
+    KEY_102ND = 0x64, // Keyboard Non-US \ and |
+    KEY_COMPOSE = 0x65, // Keyboard Application
+    KEY_POWER = 0x66, // Keyboard Power
+    KEY_KPEQUAL = 0x67, // Keypad =
+
+    KEY_F13 = 0x68, // Keyboard F13
+    KEY_F14 = 0x69, // Keyboard F14
+    KEY_F15 = 0x6a, // Keyboard F15
+    KEY_F16 = 0x6b, // Keyboard F16
+    KEY_F17 = 0x6c, // Keyboard F17
+    KEY_F18 = 0x6d, // Keyboard F18
+    KEY_F19 = 0x6e, // Keyboard F19
+    KEY_F20 = 0x6f, // Keyboard F20
+    KEY_F21 = 0x70, // Keyboard F21
+    KEY_F22 = 0x71, // Keyboard F22
+    KEY_F23 = 0x72, // Keyboard F23
+    KEY_F24 = 0x73, // Keyboard F24
+
+    KEY_OPEN = 0x74, // Keyboard Execute
+    KEY_HELP = 0x75, // Keyboard Help
+    KEY_PROPS = 0x76, // Keyboard Menu
+    KEY_FRONT = 0x77, // Keyboard Select
+    KEY_STOP = 0x78, // Keyboard Stop
+    KEY_AGAIN = 0x79, // Keyboard Again
+    KEY_UNDO = 0x7a, // Keyboard Undo
+    KEY_CUT = 0x7b, // Keyboard Cut
+    KEY_COPY = 0x7c, // Keyboard Copy
+    KEY_PASTE = 0x7d, // Keyboard Paste
+    KEY_FIND = 0x7e, // Keyboard Find
+    KEY_MUTE = 0x7f, // Keyboard Mute
+    KEY_VOLUMEUP = 0x80, // Keyboard Volume Up
+    KEY_VOLUMEDOWN = 0x81, // Keyboard Volume Down
+    KEY_KPCOMMA = 0x85, // Keypad Comma
+    KEY_RO = 0x87, // Keyboard International1
+    KEY_KATAKANAHIRAGANA = 0x88, // Keyboard International2
+    KEY_YEN = 0x89, // Keyboard International3
+    KEY_HENKAN = 0x8a, // Keyboard International4
+    KEY_MUHENKAN = 0x8b, // Keyboard International5
+    KEY_KPJPCOMMA = 0x8c, // Keyboard International6
+    KEY_HANGEUL = 0x90, // Keyboard LANG1
+    KEY_HANJA = 0x91, // Keyboard LANG2
+    KEY_KATAKANA = 0x92, // Keyboard LANG3
+    KEY_HIRAGANA = 0x93, // Keyboard LANG4
+    KEY_ZENKAKUHANKAKU = 0x94, // Keyboard LANG5
+    KEY_KPLEFTPAREN = 0xb6, // Keypad (
+    KEY_KPRIGHTPAREN = 0xb7, // Keypad )
+
+    KEY_LEFTCTRL = 0xe0, // Keyboard Left Control
+    KEY_LEFTSHIFT = 0xe1, // Keyboard Left Shift
+    KEY_LEFTALT = 0xe2, // Keyboard Left Alt
+    KEY_LEFTMETA = 0xe3, // Keyboard Left GUI
+    KEY_RIGHTCTRL = 0xe4, // Keyboard Right Control
+    KEY_RIGHTSHIFT = 0xe5, // Keyboard Right Shift
+    KEY_RIGHTALT = 0xe6, // Keyboard Right Alt
+    KEY_RIGHTMETA = 0xe7, // Keyboard Right GUI
+
+    KEY_MEDIA_PLAYPAUSE = 0xe8,
+    KEY_MEDIA_STOPCD = 0xe9,
+    KEY_MEDIA_PREVIOUSSONG = 0xea,
+    KEY_MEDIA_NEXTSONG = 0xeb,
+    KEY_MEDIA_EJECTCD = 0xec,
+    KEY_MEDIA_VOLUMEUP = 0xed,
+    KEY_MEDIA_VOLUMEDOWN = 0xee,
+    KEY_MEDIA_MUTE = 0xef,
+    KEY_MEDIA_WWW = 0xf0,
+    KEY_MEDIA_BACK = 0xf1,
+    KEY_MEDIA_FORWARD = 0xf2,
+    KEY_MEDIA_STOP = 0xf3,
+    KEY_MEDIA_FIND = 0xf4,
+    KEY_MEDIA_SCROLLUP = 0xf5,
+    KEY_MEDIA_SCROLLDOWN = 0xf6,
+    KEY_MEDIA_EDIT = 0xf7,
+    KEY_MEDIA_SLEEP = 0xf8,
+    KEY_MEDIA_COFFEE = 0xf9,
+    KEY_MEDIA_REFRESH = 0xfa,
+    KEY_MEDIA_CALC = 0xfb,
+};
+
+#endif
diff --git a/fw/src/noise.c b/fw/src/noise.c
new file mode 100644
index 0000000..bd0974c
--- /dev/null
+++ b/fw/src/noise.c
@@ -0,0 +1,445 @@
+
+#include <string.h>
+
+#include "noise.h"
+#include "packet_interface.h"
+#include "rand_stm32.h"
+
+#include "crypto/noise-c/src/crypto/blake2/blake2s.h"
+
+
+#ifdef VERIFICATION
+#define HANDLE_NOISE_ERROR(x, msg) if (x) { goto errout; }
+#else
+#define HANDLE_NOISE_ERROR(x, msg) do { \
+        err = x; \
+        if (err != NOISE_ERROR_NONE) { \
+            char errbuf[256]; \
+            noise_strerror(err, errbuf, sizeof(errbuf)); \
+            LOG_PRINTF("Error " msg ": %s\n", errbuf); \
+            goto errout; \
+        } \
+    } while(0);
+#endif
+
+#ifdef VERIFICATION
+
+/*@ requires \valid(s + (0..n-1));
+  @ assigns s[0..n-1] \from c;
+  @ assigns \result \from s;
+  @ ensures result_ptr: \result == s;
+  @*/
+uint8_t *fc_memset_uint8(uint8_t *s, int c, size_t n);
+
+/*@ requires \valid(dest + (0..n-1));
+  @ requires \valid_read(src + (0..n-1));
+  @ assigns dest[0..n-1] \from src[0..n-1];
+  @ assigns \result \from dest;
+  @ ensures result_ptr: \result == dest;
+  @ ensures equals: \forall integer i; 0 <= i <= n-1 ==> dest[i] == src[i];
+  @*/
+uint8_t *fc_memcpy_uint8(uint8_t *dest, const uint8_t *src, size_t n);
+
+/*@ requires valid_s1: \valid_read(s1 + (0..n-1));
+  @ requires valid_s2: \valid_read(s2 + (0..n-1));
+  @ assigns \result \from
+  @   indirect:s1[0.. n-1], indirect:s2[0.. n-1];
+  @ ensures logic_spec: \result == 0 <==> (
+		\forall integer i; 0 <= i <= n-1 ==> s1[i] == s2[i]);
+  @*/
+int fc_memcmp_uint8(const uint8_t *s1, const uint8_t *s2, size_t n);
+
+#else
+#define fc_memset_uint8 memset
+#define fc_memcpy_uint8 memcpy
+#define fc_memcmp_uint8 memcmp
+#endif
+
+
+volatile uint8_t host_packet_buf[MAX_HOST_PACKET_SIZE];
+volatile int host_packet_length = 0;
+
+
+/*@
+    requires validity: \valid(st);
+
+    ensures equal: st->remote_key_reference == remote_key_reference && st->local_key == local_key;
+    ensures equal: st->handshake_state == HANDSHAKE_UNINITIALIZED;
+    ensures equal: st->failed_handshakes == 0;
+    ensures equal: st->tx_cipher == NULL && st->rx_cipher == NULL && st->handshake == NULL;
+
+    assigns *st;
+ */
+void noise_state_init(struct NoiseState *st, uint8_t *remote_key_reference, uint8_t *local_key) {
+    st->handshake_state = HANDSHAKE_UNINITIALIZED;
+    st->handshake = NULL;
+    st->tx_cipher = NULL;
+    st->rx_cipher = NULL;
+    fc_memset_uint8(st->handshake_hash, 0, sizeof(st->handshake_hash));
+    st->remote_key_reference = remote_key_reference;
+    st->local_key = local_key;
+    st->failed_handshakes = 0;
+}
+
+/*@
+    requires validity: \valid(st) && \valid(st->handshake_hash + (0..31)) && \valid_read(st->local_key + (0..31));
+    requires separation: \separated(st, st->rx_cipher, st->tx_cipher, st->handshake);
+
+    ensures result: \result \in {0, -1};
+    ensures success: \result == 0 ==> (
+        \valid(st->handshake) &&
+        (st->handshake_state == HANDSHAKE_PHASE1));
+    ensures failure: \result != 0 ==> (
+        (st->handshake == NULL) &&
+        (st->handshake_state == HANDSHAKE_UNINITIALIZED));
+	ensures unmodified: \old(st->failed_handshakes) == st->failed_handshakes;
+
+    assigns *st, *st->rx_cipher, *st->tx_cipher;
+ */
+int reset_protocol_handshake(struct NoiseState *st) {
+    uninit_handshake(st, HANDSHAKE_UNINITIALIZED);
+    disarm_key_scrubber();
+    noise_cipherstate_free(st->tx_cipher);
+    noise_cipherstate_free(st->rx_cipher);
+    st->tx_cipher = NULL;
+    st->rx_cipher = NULL;
+    st->handshake = NULL;
+    fc_memset_uint8(st->handshake_hash, 0, sizeof(st->handshake_hash));
+    return start_protocol_handshake(st);
+}
+
+/*@ requires validity: \valid(st) && \valid_read(st->local_key + (0..31));
+  
+    ensures result: \result \in {0, -1};
+    ensures success: \result == 0 ==> (
+        \valid(st->handshake) &&
+        st->handshake_state == HANDSHAKE_PHASE1);
+    ensures failure: \result != 0 ==> (
+        st->handshake == \old(st->handshake) &&
+        st->handshake_state == \old(st->handshake_state));
+
+    assigns st->handshake, st->handshake_state;
+ */
+int start_protocol_handshake(struct NoiseState *st) {
+    /* TODO Noise-C is nice for prototyping, but we should really get rid of it for mostly three reasons:
+     *   * We don't need cipher/protocol agility, and by baking the final protocol into the firmware we can save a lot
+     *     of flash space by not including all the primitives we don't need as well as noise's dynamic protocol
+     *     abstraction layer.
+     *   * Noise-c is not very embedded-friendly, in particular it uses malloc and free. We should be able to run
+     *     everything with statically allocated buffers instead.
+     *   * Parts of it are not written that well
+     */
+    NoiseHandshakeState *handshake;
+    int err;
+    
+    HANDLE_NOISE_ERROR(noise_init(), "initializing noise");
+
+    HANDLE_NOISE_ERROR(noise_handshakestate_new_by_name(&handshake, "Noise_XX_25519_ChaChaPoly_BLAKE2s", NOISE_ROLE_RESPONDER), "instantiating handshake pattern");
+
+    NoiseDHState *dh = noise_handshakestate_get_local_keypair_dh(handshake);
+    HANDLE_NOISE_ERROR(noise_dhstate_set_keypair_private(dh, st->local_key, CURVE25519_KEY_LEN), "loading local private keys");
+
+    HANDLE_NOISE_ERROR(noise_handshakestate_start(handshake), "starting handshake");
+
+    st->handshake = handshake;
+    st->handshake_state = HANDSHAKE_PHASE1;
+    return 0;
+
+errout:
+    noise_handshakestate_free(handshake);
+    return -1;
+}
+
+/*@ requires validity: \valid(st) && \valid(st->local_key + (0..31));
+
+    assigns st->local_key[0..31];
+ */
+int generate_identity_key(struct NoiseState *st) {
+    NoiseDHState *dh;
+    int err;
+
+    HANDLE_NOISE_ERROR(noise_dhstate_new_by_name(&dh, "25519"), "creating dhstate for key generation"); 
+    HANDLE_NOISE_ERROR(noise_dhstate_generate_keypair(dh), "generating key pair");
+
+    uint8_t unused[CURVE25519_KEY_LEN]; /* the noise api is a bit bad here. */
+    fc_memset_uint8(st->local_key, 0, CURVE25519_KEY_LEN);
+
+    HANDLE_NOISE_ERROR(noise_dhstate_get_keypair(dh, st->local_key, CURVE25519_KEY_LEN, unused, sizeof(unused)), "saving key pair");
+
+    noise_dhstate_free(dh);
+    return 0;
+errout:
+    if (dh)
+        noise_dhstate_free(dh);
+    return -1;
+}
+
+/*@requires validity: \valid(st);
+   requires state_valid: new_state \in
+           {HANDSHAKE_UNINITIALIZED, HANDSHAKE_DONE_UNKNOWN_HOST, HANDSHAKE_DONE_KNOWN_HOST};
+
+   ensures state: st->handshake_state == new_state;
+   ensures handshake: st->handshake == NULL;
+
+   assigns st->handshake, st->handshake_state;
+ @*/
+void uninit_handshake(struct NoiseState *st, enum handshake_state new_state) {
+    if (st->handshake)
+        noise_handshakestate_free(st->handshake);
+    st->handshake_state = new_state;
+    st->handshake = NULL;
+    arm_key_scrubber();
+}
+
+/*@ 
+    requires validity: \valid(st) && \valid(usart2_out) && \valid(st->handshake);
+    requires validity: \valid(st->remote_key + (0..sizeof(st->remote_key)-1));
+    requires validity: \valid(st->handshake_hash + (0..sizeof(st->handshake_hash)-1));
+
+    requires separation: \separated(usart2_out, st, buf, st->handshake);
+
+    ensures \result \in {-1, 0};
+
+    assigns *usart2_out, *st->handshake;
+  @*/
+int handshake_phase1(struct NoiseState * const st, uint8_t *buf, size_t len) {
+    int err;
+    struct {
+        struct control_packet header;
+        uint8_t payload[MAX_HOST_PACKET_SIZE];
+    } pkt;
+    NoiseBuffer noise_msg;
+
+    /* Read the next handshake message and discard the payload */
+    noise_buffer_set_input(noise_msg, buf, len);
+    HANDLE_NOISE_ERROR(noise_handshakestate_read_message(st->handshake, &noise_msg, NULL), "reading handshake message");
+
+    /* Write the next handshake message with a zero-length noise payload */
+    pkt.header.type = HOST_HANDSHAKE;
+    noise_buffer_set_output(noise_msg, &pkt.payload, sizeof(pkt.payload));
+    HANDLE_NOISE_ERROR(noise_handshakestate_write_message(st->handshake, &noise_msg, NULL), "writing handshake message");
+    send_packet(usart2_out, (uint8_t *)&pkt, noise_msg.size + sizeof(pkt.header));
+    
+    return 0;
+errout: /* for HANDLE_NOISE_ERROR macro */
+    return -1;
+}
+
+//@ ghost int key_checked_trace;
+//@ ghost int key_match_trace;
+/*@ 
+    requires validity: \valid(st) && \valid(usart2_out) && \valid(st->handshake);
+    requires validity: \valid(st->remote_key + (0..sizeof(st->remote_key)-1));
+    requires validity: \valid_read(st->remote_key_reference + (0..sizeof(st->remote_key)-1));
+    requires validity: \valid(st->handshake_hash + (0..sizeof(st->handshake_hash)-1));
+	requires separation: \separated(usart2_out, st);
+    requires sanity: 0 <= st->failed_handshakes < 100;
+
+    requires separation: \separated(&usart2_out, st, buf, st->handshake);
+
+    ensures result: \result \in {-1, 0, 1};
+    ensures sanity: 0 <= st->failed_handshakes <= 102;
+    ensures sanity: \result != 1 ==> st->failed_handshakes >= \old(st->failed_handshakes);
+
+    ensures permission_valid: \result != -1 ==> key_checked_trace == 1;
+    ensures permission_valid: \result ==  1 ==> key_match_trace == 1;
+    //
+    assigns *usart2_out, *st, *st->handshake, key_checked_trace, key_match_trace;
+  @*/
+int handshake_phase2(struct NoiseState * const st, uint8_t *buf, size_t len) {
+    //@ ghost int old_failed_handshakes = st->failed_handshakes;
+    int err;
+    struct {
+        struct control_packet header;
+        uint8_t payload[MAX_HOST_PACKET_SIZE];
+    } pkt;
+    NoiseBuffer noise_msg;
+    //@ ghost key_checked_trace = 0;
+    // ghost key_match_trace = 0;
+
+    /* Read the next handshake message and discard the payload */
+    noise_buffer_set_input(noise_msg, buf, len);
+    HANDLE_NOISE_ERROR(noise_handshakestate_read_message(st->handshake, &noise_msg, NULL), "reading handshake message");
+
+    HANDLE_NOISE_ERROR(noise_handshakestate_split(st->handshake, &st->tx_cipher, &st->rx_cipher), "splitting handshake state");
+    LOG_PRINTF("Noise protocol handshake completed successfully, handshake hash:\n");
+
+    if (noise_handshakestate_get_handshake_hash(st->handshake, st->handshake_hash, sizeof(st->handshake_hash)) != NOISE_ERROR_NONE) {
+        LOG_PRINTF("Error fetching noise handshake state\n");
+        goto errout;
+    } else {
+        LOG_PRINTF("    ");
+        /*@ loop assigns i; @*/
+        for (size_t i=0; i<sizeof(st->handshake_hash); i++)
+            LOG_PRINTF("%02x ", st->handshake_hash[i]);
+        LOG_PRINTF("\n");
+    }
+
+    
+    NoiseDHState *remote_dh = noise_handshakestate_get_remote_public_key_dh(st->handshake);
+    if (!remote_dh) {
+        LOG_PRINTF("Error: Host has not identified itself\n");
+        goto errout;
+    }
+
+    HANDLE_NOISE_ERROR(noise_dhstate_get_public_key(remote_dh, st->remote_key, sizeof(st->remote_key)), "getting remote pubkey");
+
+    /* TODO support list of known remote hosts here instead of just one */
+    uint8_t remote_fp[BLAKE2S_HASH_SIZE];
+    BLAKE2s_context_t bc;
+    BLAKE2s_reset(&bc);
+    fc_BLAKE2s_update_uint8(&bc, st->remote_key, sizeof(st->remote_key));
+    BLAKE2s_finish(&bc, remote_fp);
+
+    //@ ghost key_checked_trace = 1;
+    if (!fc_memcmp_uint8(remote_fp, st->remote_key_reference, sizeof(remote_fp))) { /* keys match */
+        //@ ghost key_match_trace = 1;
+        uint8_t response = REPORT_PAIRING_SUCCESS;
+        if (send_encrypted_message(st, &response, sizeof(response)))
+            LOG_PRINTF("Error sending pairing response packet\n");
+
+        st->failed_handshakes = 0;
+    	//@ assert 0 <= st->failed_handshakes <= 102;
+        return 1;
+
+    } else { /* keys don't match */
+        uint8_t response = REPORT_PAIRING_START;
+        if (send_encrypted_message(st, &response, sizeof(response)))
+            LOG_PRINTF("Error sending pairing response packet\n");
+
+        st->failed_handshakes++;
+    	//@ assert 0 <= st->failed_handshakes <= 102;
+    	//@ assert st->failed_handshakes >= old_failed_handshakes;
+        return 0;
+    }
+
+errout:
+	//@ assert 0 <= st->failed_handshakes <= 102;
+	//@ assert st->failed_handshakes >= old_failed_handshakes;
+    return -1;
+}
+/*@ 
+    requires validity: \valid(st) && \valid(usart2_out) && \valid(st->handshake);
+    requires validity: \valid(st->remote_key + (0..sizeof(st->remote_key)-1));
+    requires validity: \valid(st->remote_key_reference + (0..sizeof(st->remote_key)-1));
+    requires validity: \valid(st->handshake_hash + (0..sizeof(st->handshake_hash)-1));
+    requires sanity: 0 <= st->failed_handshakes < 100;
+
+    requires separation: \separated(usart2_out, st, buf, st->handshake);
+
+    ensures result: \result \in {0, -1};
+
+    ensures state_legal: st->handshake_state \in
+			{HANDSHAKE_UNINITIALIZED, HANDSHAKE_PHASE1, HANDSHAKE_PHASE2,
+			HANDSHAKE_DONE_KNOWN_HOST, HANDSHAKE_DONE_UNKNOWN_HOST};
+    ensures transition_legal: 
+			\result != -1 ==> \old(st->handshake_state) \in {HANDSHAKE_PHASE1, HANDSHAKE_PHASE2};
+    ensures transition_legal: (\old(st->handshake_state) == HANDSHAKE_PHASE1)
+            ==> st->handshake_state \in {HANDSHAKE_PHASE2, HANDSHAKE_UNINITIALIZED};
+    ensures transition_legal: (\old(st->handshake_state) == HANDSHAKE_PHASE2)
+            ==> st->handshake_state \in {HANDSHAKE_DONE_KNOWN_HOST, HANDSHAKE_DONE_UNKNOWN_HOST, HANDSHAKE_UNINITIALIZED};
+    ensures failure_handling: \result == -1 <==> st->handshake_state == HANDSHAKE_UNINITIALIZED;
+    ensures failure_handling: \result == -1 ==> st->failed_handshakes > \old(st->failed_handshakes);
+
+    ensures state_advance_condition: (st->handshake_state == HANDSHAKE_DONE_KNOWN_HOST) ==> key_match_trace == 1;
+
+    //assigns *usart2_out, *st, *st->rx_cipher, *st->tx_cipher, *st->handshake;
+	//assigns key_checked_trace, key_match_trace;
+  @*/
+int try_continue_noise_handshake(struct NoiseState * const st, uint8_t *buf, size_t len) {
+    //@ ghost key_checked_trace = 0;
+    //@ ghost key_match_trace = 0;
+    int rc;
+
+    if (!st->handshake) {
+        LOG_PRINTF("Error: Invalid handshake state\n");
+        goto errout;
+    }
+
+    /* Run the protocol handshake */
+    switch (st->handshake_state) {
+    case HANDSHAKE_PHASE1:
+        if (handshake_phase1(st, buf, len))
+            goto errout;
+
+        st->handshake_state = HANDSHAKE_PHASE2;
+        return 0;
+
+    case HANDSHAKE_PHASE2:
+        rc = handshake_phase2(st, buf, len);
+        if (rc < 0)
+            goto errout;
+        
+        if (rc == 1)
+            uninit_handshake(st, HANDSHAKE_DONE_KNOWN_HOST);
+        else
+            uninit_handshake(st, HANDSHAKE_DONE_UNKNOWN_HOST);
+        break;
+
+    default:
+        LOG_PRINTF("Invalid handshake state\n");
+        goto errout;
+    }
+
+    return 0;
+errout:
+    uninit_handshake(st, HANDSHAKE_UNINITIALIZED);
+    st->failed_handshakes++;
+    LOG_PRINTF("Noise protocol handshake failed, %d failed attempts\n", st->failed_handshakes);
+    return -1;
+}
+
+/*@
+    requires validity: \valid(st);
+    requires validity: \valid_read(st->remote_key + (0..sizeof(st->remote_key)-1));
+    requires validity: \valid(st->remote_key_reference + (0..31));
+
+    ensures state: st->handshake_state == HANDSHAKE_DONE_KNOWN_HOST;
+    assigns st->remote_key_reference[0..31], st->handshake_state;
+ */
+void persist_remote_key(struct NoiseState *st) {
+    BLAKE2s_context_t bc;
+    BLAKE2s_reset(&bc);
+    fc_BLAKE2s_update_uint8(&bc, st->remote_key, sizeof(st->remote_key));
+    BLAKE2s_finish(&bc, st->remote_key_reference);
+    st->handshake_state = HANDSHAKE_DONE_KNOWN_HOST;
+}
+
+/*@
+	requires validity: \valid(st) && \valid(usart2_out) && \valid(st->tx_cipher) && \valid_read(msg + (0..len-1));
+	requires separation: \separated(usart2_out, st);
+
+	ensures length: !(0 <= len <= MAX_HOST_PACKET_SIZE) <==> \result == -3;
+	ensures \result \in {0, -1, -2, -3};
+	assigns *st->tx_cipher, *usart2_out;
+ */
+int send_encrypted_message(struct NoiseState *st, const uint8_t *msg, size_t len) {
+    int err;
+    NoiseBuffer noise_buf;
+    struct {
+        struct control_packet header;
+        uint8_t payload[MAX_HOST_PACKET_SIZE];
+    } pkt;
+
+    if (len > sizeof(pkt.payload)) {
+        LOG_PRINTF("Packet too long\n");
+        return -3;
+    }
+
+    if (!st->tx_cipher) {
+        LOG_PRINTF("Cannot send encrypted packet: Data ciphers not yet initialized\n");
+        return -1;
+    }
+
+    pkt.header.type = HOST_DATA;
+    fc_memcpy_uint8(pkt.payload, msg, len); /* This is necessary because noises API doesn't support separate in and out buffers. D'oh! */
+    noise_buffer_set_inout(noise_buf, pkt.payload, len, sizeof(pkt.payload));
+
+    HANDLE_NOISE_ERROR(noise_cipherstate_encrypt(st->tx_cipher, &noise_buf), "encrypting data");
+    send_packet(usart2_out, (uint8_t *)&pkt, noise_buf.size + sizeof(pkt.header));
+
+    return 0;
+errout:
+    return -2;
+}
+
diff --git a/fw/src/noise.h b/fw/src/noise.h
new file mode 100644
index 0000000..1969945
--- /dev/null
+++ b/fw/src/noise.h
@@ -0,0 +1,56 @@
+#ifndef __NOISE_H__
+#define __NOISE_H__
+
+#include <stdint.h>
+
+#include <noise/protocol.h>
+
+#include "usart_helpers.h"
+#include "rand_stm32.h"
+
+
+#define CURVE25519_KEY_LEN 32
+#define MAX_HOST_PACKET_SIZE 128
+
+
+extern volatile uint8_t host_packet_buf[MAX_HOST_PACKET_SIZE];
+extern volatile int host_packet_length;
+
+enum handshake_state {
+    HANDSHAKE_UNINITIALIZED,
+    HANDSHAKE_PHASE1,
+    HANDSHAKE_PHASE2,
+    HANDSHAKE_DONE_UNKNOWN_HOST,
+    HANDSHAKE_DONE_KNOWN_HOST,
+};
+
+extern volatile enum handshake_state handshake_state;
+
+struct NoiseState {
+    NoiseHandshakeState *handshake;
+    enum handshake_state handshake_state;
+    NoiseCipherState *tx_cipher, *rx_cipher;
+    uint8_t *local_key;
+    uint8_t remote_key[CURVE25519_KEY_LEN];
+    uint8_t *remote_key_reference;
+    uint8_t handshake_hash[BLAKE2S_HASH_SIZE];
+    int failed_handshakes;
+};
+
+
+void uninit_handshake(struct NoiseState *st, enum handshake_state new_state);
+void noise_state_init(struct NoiseState *st, uint8_t *remote_key_reference, uint8_t *local_key);
+void persist_remote_key(struct NoiseState *st);
+int start_protocol_handshake(struct NoiseState *st);
+int reset_protocol_handshake(struct NoiseState *st);
+int generate_identity_key(struct NoiseState *st);
+int try_continue_noise_handshake(struct NoiseState * const st, uint8_t *buf, size_t len);
+int send_encrypted_message(struct NoiseState *st, const uint8_t *msg, size_t len);
+
+/*@ assigns \nothing; */
+void arm_key_scrubber(void);
+
+/*@ assigns \nothing; */
+void disarm_key_scrubber(void);
+
+#endif
diff --git a/fw/src/packet_interface.c b/fw/src/packet_interface.c
new file mode 100644
index 0000000..98a1ef2
--- /dev/null
+++ b/fw/src/packet_interface.c
@@ -0,0 +1,98 @@
+
+#include "packet_interface.h"
+#include "noise.h"
+#include "cobs.h"
+#include "tracing.h"
+
+#include <libopencm3/stm32/usart.h>
+#include <libopencm3/stm32/dma.h>
+#include <libopencm3/cm3/nvic.h>
+#include <libopencmsis/core_cm3.h>
+
+volatile struct {
+    struct dma_buf dma;
+    uint8_t data[256];
+} usart2_buf = { .dma = { .len = sizeof(usart2_buf.data) } };
+
+struct dma_usart_file usart2_out_s = {
+    .usart = USART2,
+    .baudrate = 115200,
+    .dma = DMA1,
+    .stream = 6,
+    .channel = 4,
+    .irqn = NVIC_DMA_IRQ(1, 6),
+    .buf = &usart2_buf.dma
+};
+struct dma_usart_file *usart2_out = &usart2_out_s;
+
+void dma1_stream6_isr(void) {
+    TRACING_SET(TR_HOST_IF_DMA_IRQ);
+    static unsigned int fifo_errors = 0; /* debug */
+    if (dma_get_interrupt_flag(usart2_out->dma, usart2_out->stream, DMA_FEIF)) {
+        /* Ignore FIFO errors as they're 100% non-critical for UART applications */
+        dma_clear_interrupt_flags(usart2_out->dma, usart2_out->stream, DMA_FEIF);
+        fifo_errors++;
+        TRACING_CLEAR(TR_HOST_IF_DMA_IRQ);
+        return;
+    }
+
+    /* Transfer complete interrupt */
+    dma_clear_interrupt_flags(usart2_out->dma, usart2_out->stream, DMA_TCIF);
+
+    if (usart2_out->buf->wr_pos != usart2_out->buf->xfr_end) /* buffer not empty */
+        schedule_dma(usart2_out);
+    TRACING_CLEAR(TR_HOST_IF_DMA_IRQ);
+}
+
+void usart2_isr(void) {
+    TRACING_SET(TR_HOST_IF_USART_IRQ);
+    static struct cobs_decode_state host_cobs_state = {0};
+    if (USART2_SR & USART_SR_ORE) { /* Overrun handling */
+        LOG_PRINTF("USART2 data register overrun\n");
+        /* Clear interrupt flag */
+        (void)USART2_DR; /* FIXME make sure this read is not optimized out */
+        host_packet_length = -1;
+        TRACING_CLEAR(TR_HOST_IF_USART_IRQ);
+        return;
+    }
+
+    uint8_t data = USART2_DR; /* This automatically acknowledges the IRQ */
+
+    if (host_packet_length) {
+        LOG_PRINTF("USART2 COBS buffer overrun\n");
+        host_packet_length = -1;
+        TRACING_CLEAR(TR_HOST_IF_USART_IRQ);
+        return;
+    }
+
+    ssize_t rv = cobs_decode_incremental(&host_cobs_state, (char *)host_packet_buf, sizeof(host_packet_buf), data);
+    if (rv == 0) {
+        /* good, empty frame */
+        LOG_PRINTF("Got empty frame from host\n");
+        host_packet_length = -1;
+    } else if (rv == -1) {
+        /* Decoding frame, wait for next byte */
+    } else if (rv == -2) {
+        LOG_PRINTF("Host interface COBS framing error\n");
+        host_packet_length = -1;
+    } else if (rv == -3) {
+        /* invalid empty frame */
+        LOG_PRINTF("Got double null byte from host\n");
+        host_packet_length = -1;
+    } else if (rv == -4) {
+        /* frame too large */
+        LOG_PRINTF("Got too large frame from host\n");
+        host_packet_length = -1;
+    } else if (rv > 0) {
+        /* Good, non-empty frame */
+        host_packet_length = rv;
+    }
+    TRACING_CLEAR(TR_HOST_IF_USART_IRQ);
+}
+
+void send_packet(struct dma_usart_file *f, const uint8_t *data, size_t len) {
+    /* ignore return value as putf is blocking and always succeeds */
+    (void)cobs_encode_incremental(f, putf, (char *)data, len);
+    flush(f);
+}
+
diff --git a/fw/src/packet_interface.h b/fw/src/packet_interface.h
new file mode 100644
index 0000000..35e9758
--- /dev/null
+++ b/fw/src/packet_interface.h
@@ -0,0 +1,58 @@
+#ifndef __PACKET_INTERFACE_H__
+#define __PACKET_INTERFACE_H__
+
+#include "usart_helpers.h"
+
+
+extern struct dma_usart_file *usart2_out;
+
+enum control_packet_types {
+    _HOST_RESERVED = 0,
+    HOST_INITIATE_HANDSHAKE = 1,
+    HOST_HANDSHAKE = 2,
+    HOST_DATA = 3,
+    HOST_COMM_ERROR = 4,
+    HOST_CRYPTO_ERROR = 5,
+    HOST_TOO_MANY_FAILS = 6,
+};
+
+enum packet_types {
+    _REPORT_RESERVED = 0,
+    REPORT_KEYBOARD= 1,
+    REPORT_MOUSE= 2,
+    REPORT_PAIRING_INPUT = 3,
+    REPORT_PAIRING_SUCCESS = 4,
+    REPORT_PAIRING_ERROR = 5,
+    REPORT_PAIRING_START = 6,
+};
+
+struct hid_report_packet {
+    uint8_t type;
+    union {
+        struct {
+            uint8_t len;
+            uint8_t report[8];
+        } report;
+        struct {
+            char c;
+        } pairing_input;
+    };
+} __attribute__((__packed__));
+
+
+struct control_packet {
+    uint8_t type;
+    uint8_t payload[0];
+} __attribute__((__packed__));
+
+
+/*@
+    requires \valid(f);
+    requires \valid_read(data + (0..len-1));
+    requires len > 0;
+
+    assigns *f;
+ */
+void send_packet(struct dma_usart_file *f, const uint8_t *data, size_t len);
+
+#endif
diff --git a/fw/src/pgp_wordlist b/fw/src/pgp_wordlist
new file mode 100644
index 0000000..fcc3508
--- /dev/null
+++ b/fw/src/pgp_wordlist
@@ -0,0 +1,257 @@
+Hex 	Even Word 	Odd Word
+00 	aardvark 	adroitness
+01 	absurd 	adviser
+02 	accrue 	aftermath
+03 	acme 	aggregate
+04 	adrift 	alkali
+05 	adult 	almighty
+06 	afflict 	amulet
+07 	ahead 	amusement
+08 	aimless 	antenna
+09 	Algol 	applicant
+0A 	allow 	Apollo
+0B 	alone 	armistice
+0C 	ammo 	article
+0D 	ancient 	asteroid
+0E 	apple 	Atlantic
+0F 	artist 	atmosphere
+10 	assume 	autopsy
+11 	Athens 	Babylon
+12 	atlas 	backwater
+13 	Aztec 	barbecue
+14 	baboon 	belowground
+15 	backfield 	bifocals
+16 	backward 	bodyguard
+17 	banjo 	bookseller
+18 	beaming 	borderline
+19 	bedlamp 	bottomless
+1A 	beehive 	Bradbury
+1B 	beeswax 	bravado
+1C 	befriend 	Brazilian
+1D 	Belfast 	breakaway
+1E 	berserk 	Burlington
+1F 	billiard 	businessman
+20 	bison 	butterfat
+21 	blackjack 	Camelot
+22 	blockade 	candidate
+23 	blowtorch 	cannonball
+24 	bluebird 	Capricorn
+25 	bombast 	caravan
+26 	bookshelf 	caretaker
+27 	brackish 	celebrate
+28 	breadline 	cellulose
+29 	breakup 	certify
+2A 	brickyard 	chambermaid
+2B 	briefcase 	Cherokee
+2C 	Burbank 	Chicago
+2D 	button 	clergyman
+2E 	buzzard 	coherence
+2F 	cement 	combustion
+30 	chairlift 	commando
+31 	chatter 	company
+32 	checkup 	component
+33 	chisel 	concurrent
+34 	choking 	confidence
+35 	chopper 	conformist
+36 	Christmas 	congregate
+37 	clamshell 	consensus
+38 	classic 	consulting
+39 	classroom 	corporate
+3A 	cleanup 	corrosion
+3B 	clockwork 	councilman
+3C 	cobra 	crossover
+3D 	commence 	crucifix
+3E 	concert 	cumbersome
+3F 	cowbell 	customer
+40 	crackdown 	Dakota
+41 	cranky 	decadence
+42 	crowfoot 	December
+43 	crucial 	decimal
+44 	crumpled 	designing
+45 	crusade 	detector
+46 	cubic 	detergent
+47 	dashboard 	determine
+48 	deadbolt 	dictator
+49 	deckhand 	dinosaur
+4A 	dogsled 	direction
+4B 	dragnet 	disable
+4C 	drainage 	disbelief
+4D 	dreadful 	disruptive
+4E 	drifter 	distortion
+4F 	dropper 	document
+50 	drumbeat 	embezzle
+51 	drunken 	enchanting
+52 	Dupont 	enrollment
+53 	dwelling 	enterprise
+54 	eating 	equation
+55 	edict 	equipment
+56 	egghead 	escapade
+57 	eightball 	Eskimo
+58 	endorse 	everyday
+59 	endow 	examine
+5A 	enlist 	existence
+5B 	erase 	exodus
+5C 	escape 	fascinate
+5D 	exceed 	filament
+5E 	eyeglass 	finicky
+5F 	eyetooth 	forever
+60 	facial 	fortitude
+61 	fallout 	frequency
+62 	flagpole 	gadgetry
+63 	flatfoot 	Galveston
+64 	flytrap 	getaway
+65 	fracture 	glossary
+66 	framework 	gossamer
+67 	freedom 	graduate
+68 	frighten 	gravity
+69 	gazelle 	guitarist
+6A 	Geiger 	hamburger
+6B 	glitter 	Hamilton
+6C 	glucose 	handiwork
+6D 	goggles 	hazardous
+6E 	goldfish 	headwaters
+6F 	gremlin 	hemisphere
+70 	guidance 	hesitate
+71 	hamlet 	hideaway
+72 	highchair 	holiness
+73 	hockey 	hurricane
+74 	indoors 	hydraulic
+75 	indulge 	impartial
+76 	inverse 	impetus
+77 	involve 	inception
+78 	island 	indigo
+79 	jawbone 	inertia
+7A 	keyboard 	infancy
+7B 	kickoff 	inferno
+7C 	kiwi 	informant
+7D 	klaxon 	insincere
+7E 	locale 	insurgent
+7F 	lockup 	integrate
+80 	merit 	intention
+81 	minnow 	inventive
+82 	miser 	Istanbul
+83 	Mohawk 	Jamaica
+84 	mural 	Jupiter
+85 	music 	leprosy
+86 	necklace 	letterhead
+87 	Neptune 	liberty
+88 	newborn 	maritime
+89 	nightbird 	matchmaker
+8A 	Oakland 	maverick
+8B 	obtuse 	Medusa
+8C 	offload 	megaton
+8D 	optic 	microscope
+8E 	orca 	microwave
+8F 	payday 	midsummer
+90 	peachy 	millionaire
+91 	pheasant 	miracle
+92 	physique 	misnomer
+93 	playhouse 	molasses
+94 	Pluto 	molecule
+95 	preclude 	Montana
+96 	prefer 	monument
+97 	preshrunk 	mosquito
+98 	printer 	narrative
+99 	prowler 	nebula
+9A 	pupil 	newsletter
+9B 	puppy 	Norwegian
+9C 	python 	October
+9D 	quadrant 	Ohio
+9E 	quiver 	onlooker
+9F 	quota 	opulent
+A0 	ragtime 	Orlando
+A1 	ratchet 	outfielder
+A2 	rebirth 	Pacific
+A3 	reform 	pandemic
+A4 	regain 	Pandora
+A5 	reindeer 	paperweight
+A6 	rematch 	paragon
+A7 	repay 	paragraph
+A8 	retouch 	paramount
+A9 	revenge 	passenger
+AA 	reward 	pedigree
+AB 	rhythm 	Pegasus
+AC 	ribcage 	penetrate
+AD 	ringbolt 	perceptive
+AE 	robust 	performance
+AF 	rocker 	pharmacy
+B0 	ruffled 	phonetic
+B1 	sailboat 	photograph
+B2 	sawdust 	pioneer
+B3 	scallion 	pocketful
+B4 	scenic 	politeness
+B5 	scorecard 	positive
+B6 	Scotland 	potato
+B7 	seabird 	processor
+B8 	select 	provincial
+B9 	sentence 	proximate
+BA 	shadow 	puberty
+BB 	shamrock 	publisher
+BC 	showgirl 	pyramid
+BD 	skullcap 	quantity
+BE 	skydive 	racketeer
+BF 	slingshot 	rebellion
+C0 	slowdown 	recipe
+C1 	snapline 	recover
+C2 	snapshot 	repellent
+C3 	snowcap 	replica
+C4 	snowslide 	reproduce
+C5 	solo 	resistor
+C6 	southward 	responsive
+C7 	soybean 	retraction
+C8 	spaniel 	retrieval
+C9 	spearhead 	retrospect
+CA 	spellbind 	revenue
+CB 	spheroid 	revival
+CC 	spigot 	revolver
+CD 	spindle 	sandalwood
+CE 	spyglass 	sardonic
+CF 	stagehand 	Saturday
+D0 	stagnate 	savagery
+D1 	stairway 	scavenger
+D2 	standard 	sensation
+D3 	stapler 	sociable
+D4 	steamship 	souvenir
+D5 	sterling 	specialist
+D6 	stockman 	speculate
+D7 	stopwatch 	stethoscope
+D8 	stormy 	stupendous
+D9 	sugar 	supportive
+DA 	surmount 	surrender
+DB 	suspense 	suspicious
+DC 	sweatband 	sympathy
+DD 	swelter 	tambourine
+DE 	tactics 	telephone
+DF 	talon 	therapist
+E0 	tapeworm 	tobacco
+E1 	tempest 	tolerance
+E2 	tiger 	tomorrow
+E3 	tissue 	torpedo
+E4 	tonic 	tradition
+E5 	topmost 	travesty
+E6 	tracker 	trombonist
+E7 	transit 	truncated
+E8 	trauma 	typewriter
+E9 	treadmill 	ultimate
+EA 	Trojan 	undaunted
+EB 	trouble 	underfoot
+EC 	tumor 	unicorn
+ED 	tunnel 	unify
+EE 	tycoon 	universe
+EF 	uncut 	unravel
+F0 	unearth 	upcoming
+F1 	unwind 	vacancy
+F2 	uproot 	vagabond
+F3 	upset 	vertigo
+F4 	upshot 	Virginia
+F5 	vapor 	visitor
+F6 	village 	vocalist
+F7 	virus 	voyager
+F8 	Vulcan 	warranty
+F9 	waffle 	Waterloo
+FA 	wallet 	whimsical
+FB 	watchword 	Wichita
+FC 	wayside 	Wilmington
+FD 	willow 	Wyoming
+FE 	woodlark 	yesteryear
+FF 	Zulu 	Yucatan 
diff --git a/fw/src/rand_stm32.c b/fw/src/rand_stm32.c
new file mode 100644
index 0000000..87bea8f
--- /dev/null
+++ b/fw/src/rand_stm32.c
@@ -0,0 +1,135 @@
+/* Quick-and-dirty cryptographic RNG based on BLAKE2s
+ *
+ * This system uses a 32-byte BLAKE2s hash as internal state seeded by somewhat random post-powerup SRAM content, the
+ * unique device ID and the program flash contents. This seed state is mixed with values from the hardware RNG for each
+ * 32-byte block of output data.
+ *
+ * The RNG's chaining looks like the following, with H(...) being the BLAKE2s hash function, | being binary
+ * concatenation and hw_rng(...) being the hardware RNG. c and e are the fixed extraction and chain string constants
+ * defined below.
+ *
+ * Seed:    state    = H(SRAM | FLASH | hw_rng(64 byte))
+ *
+ * Extract: state    = H(state | c | hw_rng(64 byte)) block[0] = H(state | e)
+ *          state    = H(state | c | hw_rng(64 byte)) block[1] = H(state | e)
+ *          [...]
+ *          state    = H(state | c | hw_rng(64 byte)) block[n] = H(state | e)
+ *          state    = H(state | c | hw_rng(64 byte))
+ *
+ *
+ * Graphically, with C = H( state | c | rng )   being the chaining function
+ *               and X = H( state | e )         being the extraction function
+ * this becomes:
+ *
+ *            rng      rng              rng      rng
+ *             |        |                |        |
+ *             v        v                v        v
+ * state ---> [C] ---> [C] -- . . . --> [C] ---> [C] ---> new state
+ *             |        |                |
+ *             v        v                v
+ *            [X]      [X]              [X]
+ *             |        |                |
+ *             v        v                v
+ *            out[0]   out[1]   . . .   out[n]
+ *
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <libopencm3/stm32/f4/rng.h>
+
+#include "usart_helpers.h"
+#include "rand_stm32.h"
+#include "tracing.h"
+
+#include "crypto/noise-c/src/protocol/internal.h"
+#include "crypto/noise-c/src/crypto/blake2/blake2s.h"
+
+/* FIXME persist state in backup sram */
+extern unsigned _ram_start, _ram_end, _rom_start, _rom_end;
+static uint8_t global_stm_rand_state[BLAKE2S_HASH_SIZE];
+
+static uint32_t stm32_read_rng_raw(void) {
+    if ((RNG_SR & (RNG_SR_SEIS | RNG_SR_CEIS)) || !(RNG_CR & RNG_CR_RNGEN)) {
+        LOG_PRINTF("RNG error detected, bailing out.\n");
+        exit(1);
+    }
+
+    while (!(RNG_SR & RNG_SR_DRDY))
+        ;
+
+    return RNG_DR;
+}
+
+static void rng_seed_blake(BLAKE2s_context_t *bc) {
+    /* This pulls out 64 bytes. Even though the resulting BLAKE2s hash only is 32 bytes large, the internal state of
+     * BLAKE2s is larger. Also I don't quite trust the STM32F4's hardware RNG. */
+    for (int i=0; i<16; i++) {
+        uint32_t val = stm32_read_rng_raw();
+        BLAKE2s_update(bc, &val, sizeof(val));
+    }
+}
+
+void rand_init() {
+    RNG_CR |= RNG_CR_RNGEN;
+    BLAKE2s_context_t bc;
+    BLAKE2s_reset(&bc);
+
+    /* Seed with entire SRAM area */
+    BLAKE2s_update(&bc, &_ram_start, &_ram_end - &_ram_start);
+    /* Seed with entire flash area. This includes the device unique ID if it has not been overwritten. */
+    BLAKE2s_update(&bc, &_rom_start, &_rom_end - &_rom_start);
+    /* Seed with 64 bytes of handware RNG input */
+    rng_seed_blake(&bc);
+    /* FIXME use ADC to seeed */
+
+    BLAKE2s_finish(&bc, global_stm_rand_state);
+    /* FIXME make sure this is not optimized out */
+    memset(&bc, 0, sizeof(bc));
+}
+
+const char *extraction_constant = "Blake2 RNG extraction constant";
+const char *chain_constant = "Blake2 RNG chaining constant";
+
+void noise_rand_bytes(void *bytes, size_t size) {
+    TRACING_SET(TR_RNG);
+    BLAKE2s_context_t out_ctx, chain_ctx;
+    uint8_t *out = (uint8_t *)bytes;
+    uint8_t hash_buf[BLAKE2S_HASH_SIZE];
+
+    for (size_t wr_pos = 0; wr_pos<size; wr_pos += BLAKE2S_HASH_SIZE) {
+        BLAKE2s_reset(&chain_ctx);
+        BLAKE2s_update(&chain_ctx, global_stm_rand_state, sizeof(global_stm_rand_state));
+        BLAKE2s_update(&chain_ctx, chain_constant, strlen(chain_constant));
+        rng_seed_blake(&chain_ctx);
+        BLAKE2s_finish(&chain_ctx, global_stm_rand_state);
+
+        BLAKE2s_reset(&out_ctx);
+        BLAKE2s_update(&out_ctx, global_stm_rand_state, sizeof(global_stm_rand_state));
+        BLAKE2s_update(&out_ctx, extraction_constant, strlen(extraction_constant));
+        BLAKE2s_finish(&out_ctx, hash_buf);
+
+        size_t rem = size-wr_pos;
+        memcpy(&out[wr_pos], hash_buf, rem < BLAKE2S_HASH_SIZE ? rem : BLAKE2S_HASH_SIZE);
+    }
+
+    BLAKE2s_reset(&chain_ctx);
+    BLAKE2s_update(&chain_ctx, global_stm_rand_state, sizeof(global_stm_rand_state));
+    BLAKE2s_update(&chain_ctx, chain_constant, strlen(chain_constant));
+    rng_seed_blake(&chain_ctx);
+    BLAKE2s_finish(&chain_ctx, global_stm_rand_state);
+
+    /* FIXME make sure this is not optimized out */
+    memset(&out_ctx, 0, sizeof(out_ctx));
+    memset(&chain_ctx, 0, sizeof(chain_ctx));
+    memset(hash_buf, 0, sizeof(hash_buf));
+    TRACING_CLEAR(TR_RNG);
+}
+
+#ifdef ED25519_CUSTOMRANDOM /* We are building against ed25519-donna, which needs a random function */
+void ed25519_randombytes_unsafe(void *p, size_t len) {
+    noise_rand_bytes(p, len);
+}
+#endif
diff --git a/fw/src/rand_stm32.h b/fw/src/rand_stm32.h
new file mode 100644
index 0000000..3e89ec3
--- /dev/null
+++ b/fw/src/rand_stm32.h
@@ -0,0 +1,11 @@
+#ifndef __RAND_STM32_H__
+#define __RAND_STM32_H__
+
+#include <stdint.h>
+#include <unistd.h>
+
+#define BLAKE2S_HASH_SIZE 32
+
+void rand_init(void);
+
+#endif
diff --git a/fw/src/tinyprintf.c b/fw/src/tinyprintf.c
new file mode 100644
index 0000000..0f9ec4e
--- /dev/null
+++ b/fw/src/tinyprintf.c
@@ -0,0 +1,523 @@
+/*
+File: tinyprintf.c
+
+Copyright (C) 2004  Kustaa Nyholm
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+*/
+
+#include "tinyprintf.h"
+
+
+/*
+ * Configuration
+ */
+
+/* Enable long int support */
+#define PRINTF_LONG_SUPPORT
+
+/* Enable long long int support (implies long int support) */
+#define PRINTF_LONG_LONG_SUPPORT
+
+/* Enable %z (size_t) support */
+#define PRINTF_SIZE_T_SUPPORT
+
+/*
+ * Configuration adjustments
+ */
+#ifdef PRINTF_SIZE_T_SUPPORT
+#include <sys/types.h>
+#endif
+
+#ifdef PRINTF_LONG_LONG_SUPPORT
+# define PRINTF_LONG_SUPPORT
+#endif
+
+/* __SIZEOF_<type>__ defined at least by gcc */
+#ifdef __SIZEOF_POINTER__
+# define SIZEOF_POINTER __SIZEOF_POINTER__
+#endif
+#ifdef __SIZEOF_LONG_LONG__
+# define SIZEOF_LONG_LONG __SIZEOF_LONG_LONG__
+#endif
+#ifdef __SIZEOF_LONG__
+# define SIZEOF_LONG __SIZEOF_LONG__
+#endif
+#ifdef __SIZEOF_INT__
+# define SIZEOF_INT __SIZEOF_INT__
+#endif
+
+#ifdef __GNUC__
+# define _TFP_GCC_NO_INLINE_  __attribute__ ((noinline))
+#else
+# define _TFP_GCC_NO_INLINE_
+#endif
+
+/*
+ * Implementation
+ */
+struct param {
+    char lz:1;          /**<  Leading zeros */
+    char alt:1;         /**<  alternate form */
+    char uc:1;          /**<  Upper case (for base16 only) */
+    char align_left:1;  /**<  0 == align right (default), 1 == align left */
+    unsigned int width; /**<  field width */
+    char sign;          /**<  The sign to display (if any) */
+    unsigned int base;  /**<  number base (e.g.: 8, 10, 16) */
+    char *bf;           /**<  Buffer to output */
+};
+
+
+#ifdef PRINTF_LONG_LONG_SUPPORT
+static void _TFP_GCC_NO_INLINE_ ulli2a(
+    unsigned long long int num, struct param *p)
+{
+    int n = 0;
+    unsigned long long int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void lli2a(long long int num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    ulli2a(num, p);
+}
+#endif
+
+#ifdef PRINTF_LONG_SUPPORT
+static void uli2a(unsigned long int num, struct param *p)
+{
+    int n = 0;
+    unsigned long int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void li2a(long num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    uli2a(num, p);
+}
+#endif
+
+static void ui2a(unsigned int num, struct param *p)
+{
+    int n = 0;
+    unsigned int d = 1;
+    char *bf = p->bf;
+    while (num / d >= p->base)
+        d *= p->base;
+    while (d != 0) {
+        int dgt = num / d;
+        num %= d;
+        d /= p->base;
+        if (n || dgt > 0 || d == 0) {
+            *bf++ = dgt + (dgt < 10 ? '0' : (p->uc ? 'A' : 'a') - 10);
+            ++n;
+        }
+    }
+    *bf = 0;
+}
+
+static void i2a(int num, struct param *p)
+{
+    if (num < 0) {
+        num = -num;
+        p->sign = '-';
+    }
+    ui2a(num, p);
+}
+
+static int a2d(char ch)
+{
+    if (ch >= '0' && ch <= '9')
+        return ch - '0';
+    else if (ch >= 'a' && ch <= 'f')
+        return ch - 'a' + 10;
+    else if (ch >= 'A' && ch <= 'F')
+        return ch - 'A' + 10;
+    else
+        return -1;
+}
+
+static char a2u(char ch, const char **src, int base, unsigned int *nump)
+{
+    const char *p = *src;
+    unsigned int num = 0;
+    int digit;
+    while ((digit = a2d(ch)) >= 0) {
+        if (digit > base)
+            break;
+        num = num * base + digit;
+        ch = *p++;
+    }
+    *src = p;
+    *nump = num;
+    return ch;
+}
+
+static void putchw(void *putp, putcf putf, struct param *p)
+{
+    char ch;
+    int n = p->width;
+    char *bf = p->bf;
+
+    /* Number of filling characters */
+    while (*bf++ && n > 0)
+        n--;
+    if (p->sign)
+        n--;
+    if (p->alt && p->base == 16)
+        n -= 2;
+    else if (p->alt && p->base == 8)
+        n--;
+
+    /* Fill with space to align to the right, before alternate or sign */
+    if (!p->lz && !p->align_left) {
+        while (n-- > 0)
+            putf(putp, ' ');
+    }
+
+    /* print sign */
+    if (p->sign)
+        putf(putp, p->sign);
+
+    /* Alternate */
+    if (p->alt && p->base == 16) {
+        putf(putp, '0');
+        putf(putp, (p->uc ? 'X' : 'x'));
+    } else if (p->alt && p->base == 8) {
+        putf(putp, '0');
+    }
+
+    /* Fill with zeros, after alternate or sign */
+    if (p->lz) {
+        while (n-- > 0)
+            putf(putp, '0');
+    }
+
+    /* Put actual buffer */
+    bf = p->bf;
+    while ((ch = *bf++))
+        putf(putp, ch);
+
+    /* Fill with space to align to the left, after string */
+    if (!p->lz && p->align_left) {
+        while (n-- > 0)
+            putf(putp, ' ');
+    }
+}
+
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
+{
+    struct param p;
+#ifdef PRINTF_LONG_SUPPORT
+    char bf[23];  /* long = 64b on some architectures */
+#else
+    char bf[12];  /* int = 32b on some architectures */
+#endif
+    char ch;
+    p.bf = bf;
+
+    while ((ch = *(fmt++))) {
+        if (ch != '%') {
+            putf(putp, ch);
+        } else {
+#ifdef PRINTF_LONG_SUPPORT
+            char lng = 0;  /* 1 for long, 2 for long long */
+#endif
+            /* Init parameter struct */
+            p.lz = 0;
+            p.alt = 0;
+            p.width = 0;
+            p.align_left = 0;
+            p.sign = 0;
+
+            /* Flags */
+            while ((ch = *(fmt++))) {
+                switch (ch) {
+                case '-':
+                    p.align_left = 1;
+                    continue;
+                case '0':
+                    p.lz = 1;
+                    continue;
+                case '#':
+                    p.alt = 1;
+                    continue;
+                default:
+                    break;
+                }
+                break;
+            }
+
+            /* Width */
+            if (ch >= '0' && ch <= '9') {
+                ch = a2u(ch, &fmt, 10, &(p.width));
+            }
+
+            /* We accept 'x.y' format but don't support it completely:
+             * we ignore the 'y' digit => this ignores 0-fill
+             * size and makes it == width (ie. 'x') */
+            if (ch == '.') {
+              p.lz = 1;  /* zero-padding */
+              /* ignore actual 0-fill size: */
+              do {
+                ch = *(fmt++);
+              } while ((ch >= '0') && (ch <= '9'));
+            }
+
+#ifdef PRINTF_SIZE_T_SUPPORT
+# ifdef PRINTF_LONG_SUPPORT
+            if (ch == 'z') {
+                ch = *(fmt++);
+                if (sizeof(size_t) == sizeof(unsigned long int))
+                    lng = 1;
+#  ifdef PRINTF_LONG_LONG_SUPPORT
+                else if (sizeof(size_t) == sizeof(unsigned long long int))
+                    lng = 2;
+#  endif
+            } else
+# endif
+#endif
+
+#ifdef PRINTF_LONG_SUPPORT
+            if (ch == 'l') {
+                ch = *(fmt++);
+                lng = 1;
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (ch == 'l') {
+                  ch = *(fmt++);
+                  lng = 2;
+                }
+#endif
+            }
+#endif
+            switch (ch) {
+            case 0:
+                goto abort;
+            case 'u':
+                p.base = 10;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    ulli2a(va_arg(va, unsigned long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    uli2a(va_arg(va, unsigned long int), &p);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'd':
+            case 'i':
+                p.base = 10;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    lli2a(va_arg(va, long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    li2a(va_arg(va, long int), &p);
+                else
+#endif
+                    i2a(va_arg(va, int), &p);
+                putchw(putp, putf, &p);
+                break;
+#ifdef SIZEOF_POINTER
+            case 'p':
+                p.alt = 1;
+# if defined(SIZEOF_INT) && SIZEOF_POINTER <= SIZEOF_INT
+                lng = 0;
+# elif defined(SIZEOF_LONG) && SIZEOF_POINTER <= SIZEOF_LONG
+                lng = 1;
+# elif defined(SIZEOF_LONG_LONG) && SIZEOF_POINTER <= SIZEOF_LONG_LONG
+                lng = 2;
+# endif
+#endif
+            __attribute__((fallthrough));
+            case 'x':
+            __attribute__((fallthrough));
+            case 'X':
+                p.base = 16;
+                p.uc = (ch == 'X')?1:0;
+#ifdef PRINTF_LONG_SUPPORT
+#ifdef PRINTF_LONG_LONG_SUPPORT
+                if (2 == lng)
+                    ulli2a(va_arg(va, unsigned long long int), &p);
+                else
+#endif
+                  if (1 == lng)
+                    uli2a(va_arg(va, unsigned long int), &p);
+                else
+#endif
+                    ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'o':
+                p.base = 8;
+                ui2a(va_arg(va, unsigned int), &p);
+                putchw(putp, putf, &p);
+                break;
+            case 'c':
+                putf(putp, (char)(va_arg(va, int)));
+                break;
+            case 's':
+                p.bf = va_arg(va, char *);
+                putchw(putp, putf, &p);
+                p.bf = bf;
+                break;
+            case '%':
+                putf(putp, ch);
+            default:
+                break;
+            }
+        }
+    }
+ abort:;
+}
+
+#if TINYPRINTF_DEFINE_TFP_PRINTF
+static putcf stdout_putf;
+static void *stdout_putp;
+
+void init_printf(void *putp, putcf putf)
+{
+    stdout_putf = putf;
+    stdout_putp = putp;
+}
+
+void tfp_printf(char *fmt, ...)
+{
+    va_list va;
+    va_start(va, fmt);
+    tfp_format(stdout_putp, stdout_putf, fmt, va);
+    va_end(va);
+}
+#endif
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+struct _vsnprintf_putcf_data
+{
+  size_t dest_capacity;
+  char *dest;
+  size_t num_chars;
+};
+
+static void _vsnprintf_putcf(void *p, char c)
+{
+  struct _vsnprintf_putcf_data *data = (struct _vsnprintf_putcf_data*)p;
+  if (data->num_chars < data->dest_capacity)
+    data->dest[data->num_chars] = c;
+  data->num_chars ++;
+}
+
+int tfp_vsnprintf(char *str, size_t size, const char *format, va_list ap)
+{
+  struct _vsnprintf_putcf_data data;
+
+  if (size < 1)
+    return 0;
+
+  data.dest = str;
+  data.dest_capacity = size-1;
+  data.num_chars = 0;
+  tfp_format(&data, _vsnprintf_putcf, format, ap);
+
+  if (data.num_chars < data.dest_capacity)
+    data.dest[data.num_chars] = '\0';
+  else
+    data.dest[data.dest_capacity] = '\0';
+
+  return data.num_chars;
+}
+
+int tfp_snprintf(char *str, size_t size, const char *format, ...)
+{
+  va_list ap;
+  int retval;
+
+  va_start(ap, format);
+  retval = tfp_vsnprintf(str, size, format, ap);
+  va_end(ap);
+  return retval;
+}
+
+struct _vsprintf_putcf_data
+{
+  char *dest;
+  size_t num_chars;
+};
+
+static void _vsprintf_putcf(void *p, char c)
+{
+  struct _vsprintf_putcf_data *data = (struct _vsprintf_putcf_data*)p;
+  data->dest[data->num_chars++] = c;
+}
+
+int tfp_vsprintf(char *str, const char *format, va_list ap)
+{
+  struct _vsprintf_putcf_data data;
+  data.dest = str;
+  data.num_chars = 0;
+  tfp_format(&data, _vsprintf_putcf, format, ap);
+  data.dest[data.num_chars] = '\0';
+  return data.num_chars;
+}
+
+int tfp_sprintf(char *str, const char *format, ...)
+{
+  va_list ap;
+  int retval;
+
+  va_start(ap, format);
+  retval = tfp_vsprintf(str, format, ap);
+  va_end(ap);
+  return retval;
+}
+#endif
diff --git a/fw/src/tinyprintf.h b/fw/src/tinyprintf.h
new file mode 100644
index 0000000..a769f4a
--- /dev/null
+++ b/fw/src/tinyprintf.h
@@ -0,0 +1,186 @@
+/*
+File: tinyprintf.h
+
+Copyright (C) 2004  Kustaa Nyholm
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+
+This library is really just two files: 'tinyprintf.h' and 'tinyprintf.c'.
+
+They provide a simple and small (+400 loc) printf functionality to
+be used in embedded systems.
+
+I've found them so useful in debugging that I do not bother with a
+debugger at all.
+
+They are distributed in source form, so to use them, just compile them
+into your project.
+
+Two printf variants are provided: printf and the 'sprintf' family of
+functions ('snprintf', 'sprintf', 'vsnprintf', 'vsprintf').
+
+The formats supported by this implementation are:
+'c' 'd' 'i' 'o' 'p' 'u' 's' 'x' 'X'.
+
+Zero padding and field width are also supported.
+
+If the library is compiled with 'PRINTF_SUPPORT_LONG' defined, then
+the long specifier is also supported. Note that this will pull in some
+long math routines (pun intended!) and thus make your executable
+noticeably longer. Likewise with 'PRINTF_LONG_LONG_SUPPORT' for the
+long long specifier, and with 'PRINTF_SIZE_T_SUPPORT' for the size_t
+specifier.
+
+The memory footprint of course depends on the target CPU, compiler and
+compiler options, but a rough guesstimate (based on a H8S target) is about
+1.4 kB for code and some twenty 'int's and 'char's, say 60 bytes of stack space.
+Not too bad. Your mileage may vary. By hacking the source code you can
+get rid of some hundred bytes, I'm sure, but personally I feel the balance of
+functionality and flexibility versus  code size is close to optimal for
+many embedded systems.
+
+To use the printf, you need to supply your own character output function,
+something like :
+
+void putc ( void* p, char c)
+{
+    while (!SERIAL_PORT_EMPTY) ;
+    SERIAL_PORT_TX_REGISTER = c;
+}
+
+Before you can call printf, you need to initialize it to use your
+character output function with something like:
+
+init_printf(NULL,putc);
+
+Notice the 'NULL' in 'init_printf' and the parameter 'void* p' in 'putc',
+the NULL (or any pointer) you pass into the 'init_printf' will eventually be
+passed to your 'putc' routine. This allows you to pass some storage space (or
+anything really) to the character output function, if necessary.
+This is not often needed but it was implemented like that because it made
+implementing the sprintf function so neat (look at the source code).
+
+The code is re-entrant, except for the 'init_printf' function, so it is safe
+to call it from interrupts too, although this may result in mixed output.
+If you rely on re-entrancy, take care that your 'putc' function is re-entrant!
+
+The printf and sprintf functions are actually macros that translate to
+'tfp_printf' and 'tfp_sprintf' when 'TINYPRINTF_OVERRIDE_LIBC' is set
+(default). Setting it to 0 makes it possible to use them along with
+'stdio.h' printf's in a single source file. When
+'TINYPRINTF_OVERRIDE_LIBC' is set, please note that printf/sprintf are
+not function-like macros, so if you have variables or struct members
+with these names, things will explode in your face.  Without variadic
+macros this is the best we can do to wrap these function. If it is a
+problem, just give up the macros and use the functions directly, or
+rename them.
+
+It is also possible to avoid defining tfp_printf and/or tfp_sprintf by
+clearing 'TINYPRINTF_DEFINE_TFP_PRINTF' and/or
+'TINYPRINTF_DEFINE_TFP_SPRINTF' to 0. This allows for example to
+export only tfp_format, which is at the core of all the other
+functions.
+
+For further details see source code.
+
+regs Kusti, 23.10.2004
+*/
+
+#ifndef __TFP_PRINTF__
+#define __TFP_PRINTF__
+
+#include <stdarg.h>
+
+/* Global configuration */
+
+/* Set this to 0 if you do not want to provide tfp_printf */
+#ifndef TINYPRINTF_DEFINE_TFP_PRINTF
+# define TINYPRINTF_DEFINE_TFP_PRINTF 1
+#endif
+
+/* Set this to 0 if you do not want to provide
+   tfp_sprintf/snprintf/vsprintf/vsnprintf */
+#ifndef TINYPRINTF_DEFINE_TFP_SPRINTF
+# define TINYPRINTF_DEFINE_TFP_SPRINTF 1
+#endif
+
+/* Set this to 0 if you do not want tfp_printf and
+   tfp_{vsn,sn,vs,s}printf to be also available as
+   printf/{vsn,sn,vs,s}printf */
+#ifndef TINYPRINTF_OVERRIDE_LIBC
+# define TINYPRINTF_OVERRIDE_LIBC 1
+#endif
+
+/* Optional external types dependencies */
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+# include <sys/types.h>  /* size_t */
+#endif
+
+/* Declarations */
+
+#ifdef __GNUC__
+# define _TFP_SPECIFY_PRINTF_FMT(fmt_idx,arg1_idx) \
+    __attribute__((format (printf, fmt_idx, arg1_idx)))
+#else
+# define _TFP_SPECIFY_PRINTF_FMT(fmt_idx,arg1_idx)
+#endif
+
+#ifdef  __cplusplus
+extern "C" {
+#endif
+
+typedef void (*putcf) (void *, char);
+
+/*
+   'tfp_format' really is the central function for all tinyprintf. For
+   each output character after formatting, the 'putf' callback is
+   called with 2 args:
+     - an arbitrary void* 'putp' param defined by the user and
+       passed unmodified from 'tfp_format',
+     - the character.
+   The 'tfp_printf' and 'tfp_sprintf' functions simply define their own
+   callback and pass to it the right 'putp' it is expecting.
+*/
+void tfp_format(void *putp, putcf putf, const char *fmt, va_list va);
+
+#if TINYPRINTF_DEFINE_TFP_SPRINTF
+int tfp_vsnprintf(char *str, size_t size, const char *fmt, va_list ap);
+int tfp_snprintf(char *str, size_t size, const char *fmt, ...) \
+     _TFP_SPECIFY_PRINTF_FMT(3, 4);
+int tfp_vsprintf(char *str, const char *fmt, va_list ap);
+int tfp_sprintf(char *str, const char *fmt, ...) \
+    _TFP_SPECIFY_PRINTF_FMT(2, 3);
+# if TINYPRINTF_OVERRIDE_LIBC
+#  define vsnprintf tfp_vsnprintf
+#  define snprintf tfp_snprintf
+#  define vsprintf tfp_vsprintf
+#  define sprintf tfp_sprintf
+# endif
+#endif
+
+#if TINYPRINTF_DEFINE_TFP_PRINTF
+void init_printf(void *putp, putcf putf);
+void tfp_printf(char *fmt, ...) _TFP_SPECIFY_PRINTF_FMT(1, 2);
+# if TINYPRINTF_OVERRIDE_LIBC
+#  define printf tfp_printf
+# endif
+#endif
+
+#ifdef  __cplusplus
+}
+#endif
+
+#endif
diff --git a/fw/src/tracing.h b/fw/src/tracing.h
new file mode 100644
index 0000000..1970556
--- /dev/null
+++ b/fw/src/tracing.h
@@ -0,0 +1,25 @@
+#ifndef __TRACING_H__
+#define __TRACING_H__
+
+#include <libopencm3/stm32/gpio.h>
+
+#ifndef VERIFICATION
+#define TRACING_SET(i) gpio_set(GPIOD, (1<<i))
+#define TRACING_CLEAR(i) gpio_clear(GPIOD, (1<<i))
+#else
+#define TRACING_SET(i) ((void)0)
+#define TRACING_CLEAR(i) ((void)0)
+#endif
+
+enum tracing_channels {
+    TR_HID_MESSAGE_HANDLER = 0,
+    TR_DEBUG_OUT_DMA_IRQ = 1,
+    TR_HOST_IF_DMA_IRQ = 2,
+    TR_HOST_IF_USART_IRQ = 3,
+    TR_USBH_POLL = 4,
+    TR_HOST_PKT_HANDLER = 5,
+    TR_NOISE_HANDSHAKE = 6,
+    TR_RNG = 7,
+};
+
+#endif
diff --git a/fw/src/usart_helpers.c b/fw/src/usart_helpers.c
new file mode 100644
index 0000000..0cfc2d5
--- /dev/null
+++ b/fw/src/usart_helpers.c
@@ -0,0 +1,149 @@
+/*

+ * This file is part of the libusbhost library

+ * hosted at http://github.com/libusbhost/libusbhost

+ *

+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>

+ *

+ *

+ * libusbhost is free software: you can redistribute it and/or modify

+ * it under the terms of the GNU Lesser General Public License as published by

+ * the Free Software Foundation, either version 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This library is distributed in the hope that it will be useful,

+ * but WITHOUT ANY WARRANTY; without even the implied warranty of

+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+ * GNU Lesser General Public License for more details.

+ *

+ * You should have received a copy of the GNU Lesser General Public License

+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.

+ *

+ */

+

+

+#include "usart_helpers.h"

+#define TINYPRINTF_OVERRIDE_LIBC 0

+#define TINYPRINTF_DEFINE_TFP_SPRINTF 0

+#include "tinyprintf.h"

+#include "cobs.h"

+

+#include <string.h>

+#include <stdarg.h>

+#include <stdlib.h>

+#include <stdio.h>

+#include <libopencm3/stm32/usart.h>

+#include <libopencm3/stm32/dma.h>

+#include <libopencm3/cm3/nvic.h>

+#include <libopencmsis/core_cm3.h>

+

+

+void usart_fprintf(struct dma_usart_file *f, const char *str, ...) {

+	va_list va;

+	va_start(va, str);

+	tfp_format(f, (void (*)(void *, char c))putf, str, va);

+	va_end(va);

+    flush(f);

+}

+

+void usart_init(uint32_t arg_usart, uint32_t baudrate) {

+	usart_set_baudrate(arg_usart, baudrate);

+	usart_set_databits(arg_usart, 8);

+	usart_set_flow_control(arg_usart, USART_FLOWCONTROL_NONE);

+	usart_set_mode(arg_usart, USART_MODE_TX | USART_MODE_RX);

+	usart_set_parity(arg_usart, USART_PARITY_NONE);

+	usart_set_stopbits(arg_usart, USART_STOPBITS_1);

+	usart_enable(arg_usart);

+}

+

+void usart_dma_init(struct dma_usart_file *f) {

+    usart_init(f->usart, f->baudrate);

+

+    f->buf->xfr_start = -1,

+    f->buf->xfr_end = 0,

+    f->buf->wr_pos = 0,

+

+    dma_stream_reset(f->dma, f->stream);

+    dma_channel_select(f->dma, f->stream, DMA_SxCR_CHSEL(f->channel));

+	dma_set_peripheral_address(f->dma, f->stream, (uint32_t)&USART_DR(f->usart));

+	dma_set_transfer_mode(f->dma, f->stream, DMA_SxCR_DIR_MEM_TO_PERIPHERAL);

+	dma_enable_memory_increment_mode(f->dma, f->stream);

+	dma_set_peripheral_size(f->dma, f->stream, DMA_SxCR_PSIZE_8BIT);

+	dma_set_memory_size(f->dma, f->stream, DMA_SxCR_MSIZE_8BIT);

+	dma_set_priority(f->dma, f->stream, DMA_SxCR_PL_VERY_HIGH);

+	dma_enable_transfer_complete_interrupt(f->dma, f->stream);

+	dma_enable_fifo_error_interrupt(f->dma, f->stream);

+    usart_enable_tx_dma(f->usart);

+}

+

+void usart_kickoff_dma(uint32_t dma, uint8_t stream, volatile uint8_t *buf, size_t len) {

+    /* initiate transmission of new buffer */

+	dma_set_memory_address(dma, stream, (uint32_t)buf); /* select active buffer address */

+	dma_set_number_of_data(dma, stream, len);

+	dma_enable_stream(dma, stream);

+}

+

+void schedule_dma(volatile struct dma_usart_file *f) {

+    volatile struct dma_buf *buf = f->buf;

+

+    uint32_t xfr_len, xfr_start = buf->xfr_end;

+    if (buf->wr_pos > xfr_start) /* no wraparound */

+        xfr_len = buf->wr_pos - xfr_start;

+    else /* wraparound */

+        xfr_len = buf->len - xfr_start; /* schedule transfer until end of buffer */

+

+    buf->xfr_start = xfr_start;

+    buf->xfr_end = (xfr_start + xfr_len) % buf->len; /* handle wraparound */

+    usart_kickoff_dma(f->dma, f->stream, buf->data + xfr_start, xfr_len);

+}

+

+int dma_fifo_push(volatile struct dma_buf *buf, char c) {

+    if (buf->wr_pos == buf->xfr_start)

+        return -EBUSY;

+

+    buf->data[buf->wr_pos] = c;

+    buf->wr_pos = (buf->wr_pos + 1) % buf->len;

+    return 0;

+}

+

+int putf(void *file, char c) {

+    volatile struct dma_usart_file *f = (struct dma_usart_file *)file;

+

+    nvic_disable_irq(f->irqn);

+    /* push char to fifo, busy-loop if stalled to wait for USART to empty fifo via DMA */

+    while (dma_fifo_push(f->buf, c) == -EBUSY) {

+        nvic_enable_irq(f->irqn);

+        flush(f);

+        nvic_disable_irq(f->irqn);

+    }

+    nvic_enable_irq(f->irqn);

+    return 0;

+}

+

+int putb(void *file, const uint8_t *buf, size_t len) {

+    volatile struct dma_usart_file *f = (struct dma_usart_file *)file;

+

+    nvic_disable_irq(f->irqn);

+    for (size_t i=0; i<len; i++) {

+        /* push char to fifo, busy-loop if stalled to wait for USART to empty fifo via DMA */

+        while (dma_fifo_push(f->buf, buf[i]) == -EBUSY) {

+            nvic_enable_irq(f->irqn);

+            nvic_disable_irq(f->irqn);

+        }

+    }

+    nvic_enable_irq(f->irqn);

+    return 0;

+}

+

+void flush(void *file) {

+    volatile struct dma_usart_file *f = (struct dma_usart_file *)file;

+

+    nvic_disable_irq(f->irqn);

+    /* If the DMA stream is idle right now, schedule a transfer */

+    if (!(DMA_SCR(f->dma, f->stream) & DMA_SxCR_EN)) { /* DMA is not running */

+            //&& !dma_get_interrupt_flag(f->dma, f->stream, DMA_TCIF)/* DMA interrupt is clear */) {

+        dma_clear_interrupt_flags(f->dma, f->stream, DMA_TCIF);

+        schedule_dma(f);

+    }

+    nvic_enable_irq(f->irqn);

+}

+

diff --git a/fw/src/usart_helpers.h b/fw/src/usart_helpers.h
new file mode 100644
index 0000000..531652a
--- /dev/null
+++ b/fw/src/usart_helpers.h
@@ -0,0 +1,88 @@
+/*

+ * This file is part of the libusbhost library

+ * hosted at http://github.com/libusbhost/libusbhost

+ *

+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>

+ *

+ *

+ * libusbhost is free software: you can redistribute it and/or modify

+ * it under the terms of the GNU Lesser General Public License as published by

+ * the Free Software Foundation, either version 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This library is distributed in the hope that it will be useful,

+ * but WITHOUT ANY WARRANTY; without even the implied warranty of

+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+ * GNU Lesser General Public License for more details.

+ *

+ * You should have received a copy of the GNU Lesser General Public License

+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.

+ *

+ */

+

+#ifndef USBH_USART_HELPERS_H

+#define USBH_USART_HELPERS_H

+

+#include "usbh_core.h"

+#include <stdint.h>

+#include <stdarg.h>

+#include <errno.h>

+

+BEGIN_DECLS

+

+struct dma_buf {

+    uint32_t xfr_start; /* Start index of running DMA transfer */

+    uint32_t xfr_end; /* End index of running DMA transfer plus one */

+    uint32_t wr_pos; /* Next index to be written */

+    uint32_t len;

+    uint8_t data[0];

+};

+

+struct dma_usart_file {

+    uint32_t usart;

+    uint32_t baudrate;

+    uint32_t dma;

+    uint8_t stream;

+    uint8_t channel;

+    uint8_t irqn;

+    volatile struct dma_buf *buf;

+};

+

+

+extern struct dma_usart_file *debug_out;

+

+

+void usart_init(uint32_t usart, uint32_t baudrate);

+void usart_fprintf(struct dma_usart_file *f, const char *str, ...);

+void usart_fifo_push(uint8_t c);

+

+void usart_dma_init(struct dma_usart_file *f);

+void usart_kickoff_dma(uint32_t dma, uint8_t stream, volatile uint8_t *buf, size_t len);

+void schedule_dma(volatile struct dma_usart_file *f);

+int dma_fifo_push(volatile struct dma_buf *buf, char c);

+int putf(void *file, char c);

+int putb(void *file, const uint8_t *buf, size_t len);

+void flush(void *file);

+

+/* This macro abomination templates a bunch of dma-specific register/constant names from preprocessor macros passed in

+ * from cmake. */

+#define DMA_PASTE(num) DMA ## num

+#define DMA(num) DMA_PASTE(num)

+

+#define NVIC_DMA_IRQ_PASTE(dma, stream) NVIC_ ## DMA ## dma ## _ ## STREAM ## stream ## _IRQ

+#define NVIC_DMA_IRQ(dma, stream) NVIC_DMA_IRQ_PASTE(dma, stream)

+

+#define DMA_ISR_PASTE(dma, stream) DMA ## dma ## _ ## STREAM ## stream ## _IRQHandler

+#define DMA_ISR(dma, stream) DMA_ISR_PASTE(dma, stream)

+

+#ifdef USART_DEBUG

+#define LOG_PRINTF(format, ...) usart_fprintf(debug_out, format, ##__VA_ARGS__);

+#else

+#define LOG_PRINTF(dummy, ...) ((void)dummy)

+#endif

+

+#define UNUSED(var) ((void)var)

+

+END_DECLS

+

+#endif

diff --git a/fw/src/usbh_core.c b/fw/src/usbh_core.c
new file mode 100644
index 0000000..1a60285
--- /dev/null
+++ b/fw/src/usbh_core.c
@@ -0,0 +1,726 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "usbh_config.h"
+#include "usbh_lld_stm32f4.h"
+#include "driver/usbh_device_driver.h"
+#include "usart_helpers.h"
+
+#include <libopencm3/stm32/gpio.h>
+#include <libopencm3/usb/usbstd.h>
+
+#include <stddef.h>
+
+static struct {
+	bool enumeration_run;
+	const usbh_low_level_driver_t * const *lld_drivers;
+	const usbh_dev_driver_t * const *dev_drivers;
+	int8_t address_temporary;
+} usbh_data = {};
+
+static void set_enumeration(void)
+{
+	usbh_data.enumeration_run = true;
+}
+
+static void reset_enumeration(void)
+{
+	usbh_data.enumeration_run = false;
+}
+
+static bool enumeration(void)
+{
+	return usbh_data.enumeration_run;
+}
+
+void device_remove(usbh_device_t *dev)
+{
+	if (dev->drv && dev->drvdata) {
+		dev->drv->remove(dev->drvdata);
+	}
+	dev->address = -1;
+	dev->drv = NULL;
+	dev->drvdata = NULL;
+}
+
+/**
+ *
+ */
+static bool find_driver(usbh_device_t *dev, const usbh_dev_driver_info_t * device_info)
+{
+
+#define CHECK_PARTIAL_COMPATIBILITY(what) \
+	if (usbh_data.dev_drivers[i]->info->what != -1\
+	&& device_info->what != usbh_data.dev_drivers[i]->info->what) {\
+		i++;\
+		continue;\
+	}
+
+	int i = 0;
+
+	while (usbh_data.dev_drivers[i]) {
+
+		CHECK_PARTIAL_COMPATIBILITY(ifaceClass);
+		CHECK_PARTIAL_COMPATIBILITY(ifaceSubClass);
+		CHECK_PARTIAL_COMPATIBILITY(ifaceProtocol);
+		CHECK_PARTIAL_COMPATIBILITY(deviceClass);
+		CHECK_PARTIAL_COMPATIBILITY(deviceSubClass);
+		CHECK_PARTIAL_COMPATIBILITY(deviceProtocol);
+		CHECK_PARTIAL_COMPATIBILITY(idVendor);
+		CHECK_PARTIAL_COMPATIBILITY(idProduct);
+
+		dev->drv = usbh_data.dev_drivers[i];
+		dev->drvdata = dev->drv->init(dev);
+		if (!dev->drvdata) {
+			LOG_PRINTF("Unable to initialize device driver at index %d\n", i);
+			i++;
+			continue;
+		}
+		return true;
+	}
+	return false;
+#undef CHECK_PARTIAL_COMPATIBILITY
+}
+
+
+static void device_register(void *descriptors, uint16_t descriptors_len, usbh_device_t *dev)
+{
+	uint32_t i = 0;
+	uint8_t *buf = (uint8_t *)descriptors;
+
+	dev->drv = NULL;
+	dev->drvdata = NULL;
+
+	uint8_t desc_len = buf[i];
+	uint8_t desc_type = buf[i + 1];
+
+	usbh_dev_driver_info_t device_info;
+	if (desc_type == USB_DT_DEVICE) {
+		struct usb_device_descriptor *device_desc = (void*)&buf[i];
+		LOG_PRINTF("DEVICE DESCRIPTOR\n");
+		device_info.deviceClass = device_desc->bDeviceClass;
+		device_info.deviceSubClass = device_desc->bDeviceSubClass;
+		device_info.deviceProtocol = device_desc->bDeviceProtocol;
+		device_info.idVendor = device_desc->idVendor;
+		device_info.idProduct = device_desc->idProduct;
+	} else {
+		LOG_PRINTF("INVALID descriptors pointer - fatal error");
+		return;
+	}
+
+
+	while (i < descriptors_len) {
+		desc_len = buf[i];
+		desc_type = buf[i + 1];
+		switch (desc_type) {
+		case USB_DT_INTERFACE:
+		{
+			LOG_PRINTF("INTERFACE_DESCRIPTOR\n");
+			struct usb_interface_descriptor *iface = (void*)&buf[i];
+			device_info.ifaceClass = iface->bInterfaceClass;
+			device_info.ifaceSubClass = iface->bInterfaceSubClass;
+			device_info.ifaceProtocol = iface->bInterfaceProtocol;
+			if (find_driver(dev, &device_info)) {
+				int k = 0;
+				while (k < descriptors_len) {
+					desc_len = buf[k];
+					void *drvdata = dev->drvdata;
+					LOG_PRINTF("[%d]", buf[k+1]);
+					if (dev->drv->analyze_descriptor(drvdata, &buf[k])) {
+						LOG_PRINTF("Device Initialized\n");
+						return;
+					}
+
+					if (desc_len == 0) {
+						LOG_PRINTF("Problem occured while parsing complete configuration descriptor");
+						return;
+					}
+					k += desc_len;
+				}
+				LOG_PRINTF("Device driver isn't compatible with this device\n");
+				device_remove(dev);
+			} else {
+				LOG_PRINTF("No compatible driver has been found for interface #%d\n", iface->bInterfaceNumber);
+			}
+		}
+			break;
+		default:
+			break;
+		}
+
+		if (desc_len == 0) {
+			LOG_PRINTF("PROBLEM WITH PARSE %d\n",i);
+			return;
+		}
+		i += desc_len;
+	}
+	LOG_PRINTF("Device NOT Initialized\n");
+}
+
+void usbh_init(const usbh_low_level_driver_t * const low_level_drivers[], const usbh_dev_driver_t * const device_drivers[])
+{
+	if (!low_level_drivers) {
+		return;
+	}
+
+	usbh_data.lld_drivers = (const usbh_low_level_driver_t **)low_level_drivers;
+	usbh_data.dev_drivers = device_drivers;
+
+	uint32_t k = 0;
+	while (usbh_data.lld_drivers[k]) {
+		LOG_PRINTF("Initialization low-level driver with index=%d\n", k);
+
+		usbh_device_t * usbh_device =
+			((usbh_generic_data_t *)(usbh_data.lld_drivers[k])->driver_data)->usbh_device;
+		uint32_t i;
+		for (i = 0; i < USBH_MAX_DEVICES; i++) {
+			//~ LOG_PRINTF("%p ", &usbh_device[i]);
+			usbh_device[i].address = -1;
+			usbh_device[i].drv = 0;
+			usbh_device[i].drvdata = 0;
+		}
+		usbh_data.lld_drivers[k]->init(usbh_data.lld_drivers[k]->driver_data);
+
+		k++;
+	}
+
+}
+
+static void device_xfer_control_write_setup(const void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
+{
+	usbh_packet_t packet;
+
+	packet.data.out = data;
+	packet.datalen = datalen;
+	packet.address = dev->address;
+	packet.endpoint_address = 0;
+	packet.endpoint_size_max = dev->packet_size_max0;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_CONTROL;
+	packet.control_type = USBH_CONTROL_TYPE_SETUP;
+	packet.speed = dev->speed;
+	packet.callback = callback;
+	packet.callback_arg = dev;
+	packet.toggle = &dev->toggle0;
+
+	usbh_write(dev, &packet);
+	LOG_PRINTF("WR-setup@device...%d \n", dev->address);
+}
+
+static void device_xfer_control_write_data(const void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
+{
+	usbh_packet_t packet;
+
+	packet.data.out = data;
+	packet.datalen = datalen;
+	packet.address = dev->address;
+	packet.endpoint_address = 0;
+	packet.endpoint_size_max = dev->packet_size_max0;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_CONTROL;
+	packet.control_type = USBH_CONTROL_TYPE_DATA;
+	packet.speed = dev->speed;
+	packet.callback = callback;
+	packet.callback_arg = dev;
+	packet.toggle = &dev->toggle0;
+
+	usbh_write(dev, &packet);
+	LOG_PRINTF("WR-data@device...%d \n", dev->address);
+}
+
+static void device_xfer_control_read(void *data, uint16_t datalen, usbh_packet_callback_t callback, usbh_device_t *dev)
+{
+	usbh_packet_t packet;
+
+	packet.data.in = data;
+	packet.datalen = datalen;
+	packet.address = dev->address;
+	packet.endpoint_address = 0;
+	packet.endpoint_size_max = dev->packet_size_max0;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_CONTROL;
+	packet.speed = dev->speed;
+	packet.callback = callback;
+	packet.callback_arg = dev;
+	packet.toggle = &dev->toggle0;
+
+	usbh_read(dev, &packet);
+	LOG_PRINTF("RD@device...%d |  \n", dev->address);
+}
+
+
+static void control_state_machine(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	switch (dev->control.state) {
+	case USBH_CONTROL_STATE_SETUP:
+		if (cb_data.status != USBH_PACKET_CALLBACK_STATUS_OK) {
+			dev->control.state = USBH_CONTROL_STATE_NONE;
+			// Unable to deliver setup control packet - this is a fatal error
+			usbh_packet_callback_data_t ret_data;
+			ret_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;
+			ret_data.transferred_length = 0;
+			dev->control.callback(dev, ret_data);
+			break;
+		}
+		if (dev->control.setup_data.bmRequestType & USB_REQ_TYPE_IN) {
+			dev->control.state = USBH_CONTROL_STATE_DATA;
+			device_xfer_control_read(dev->control.data.in, dev->control.data_length, control_state_machine, dev);
+		} else {
+			if (dev->control.data_length == 0) {
+				dev->control.state = USBH_CONTROL_STATE_STATUS;
+				device_xfer_control_read(NULL, 0, control_state_machine, dev);
+			} else {
+				dev->control.state = USBH_CONTROL_STATE_DATA;
+				device_xfer_control_write_data(dev->control.data.out, dev->control.data_length, control_state_machine, dev);
+			}
+		}
+		break;
+
+	case USBH_CONTROL_STATE_DATA:
+		if (dev->control.setup_data.bmRequestType & USB_REQ_TYPE_IN) {
+			dev->control.state = USBH_CONTROL_STATE_NONE;
+			dev->control.callback(dev, cb_data);
+		} else {
+			if (cb_data.status != USBH_PACKET_CALLBACK_STATUS_OK) {
+				dev->control.state = USBH_CONTROL_STATE_NONE;
+				// Unable to deliver data control packet - this is a fatal error
+				usbh_packet_callback_data_t ret_data;
+				ret_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;
+				ret_data.transferred_length = 0;
+				dev->control.callback(dev, ret_data);
+				break;
+			}
+
+			if (dev->control.data_length == 0) {
+				// we should be in status state when the length of data is zero
+				LOG_PRINTF("Control logic error\n");
+				dev->control.state = USBH_CONTROL_STATE_NONE;
+				dev->control.callback(dev, cb_data);
+			} else {
+				dev->control.state = USBH_CONTROL_STATE_STATUS;
+				device_xfer_control_read(NULL, 0, control_state_machine, dev);
+			}
+		}
+		break;
+
+	case USBH_CONTROL_STATE_STATUS:
+		dev->control.state = USBH_CONTROL_STATE_NONE;
+		dev->control.callback(dev, cb_data);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void device_control(usbh_device_t *dev, usbh_packet_callback_t callback, const struct usb_setup_data *setup_data, void *data)
+{
+	if (dev->control.state != USBH_CONTROL_STATE_NONE) {
+		LOG_PRINTF("ERROR: Use of control state machine while not idle\n");
+		return;
+	}
+
+	dev->control.state = USBH_CONTROL_STATE_SETUP;
+	dev->control.callback = callback;
+	dev->control.data.out = data;
+	dev->control.data_length = setup_data->wLength;
+	dev->control.setup_data = *setup_data;
+	device_xfer_control_write_setup(&dev->control.setup_data, sizeof(dev->control.setup_data), control_state_machine, dev);
+}
+
+
+bool usbh_enum_available(void)
+{
+	return !enumeration();
+}
+
+/**
+ * Returns 0 on error
+ * device otherwise
+ */
+usbh_device_t *usbh_get_free_device(const usbh_device_t *dev)
+{
+	const usbh_low_level_driver_t *lld = dev->lld;
+	usbh_generic_data_t *lld_data = lld->driver_data;
+	usbh_device_t *usbh_device = lld_data->usbh_device;
+
+	uint8_t i;
+	LOG_PRINTF("DEV ADDRESS%d\n", dev->address);
+	for (i = 0; i < USBH_MAX_DEVICES; i++) {
+		if (usbh_device[i].address < 0) {
+			LOG_PRINTF("\t\t\t\t\tFOUND: %d", i);
+			usbh_device[i].address = i+1;
+			return &usbh_device[i];
+		} else {
+			LOG_PRINTF("address: %d\n\n\n", usbh_device[i].address);
+		}
+	}
+
+	return 0;
+}
+
+static void device_enumeration_finish(usbh_device_t *dev)
+{
+	reset_enumeration();
+	dev->state = USBH_ENUM_STATE_FIRST;
+}
+
+static void device_enumeration_terminate(usbh_device_t *dev)
+{
+	dev->address = -1;
+	device_enumeration_finish(dev);
+}
+
+#define CONTINUE_WITH(en) \
+	dev->state = en;\
+	device_enumerate(dev, cb_data);
+
+static void device_enumerate(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	const usbh_low_level_driver_t *lld = dev->lld;
+	usbh_generic_data_t *lld_data = lld->driver_data;
+	uint8_t *usbh_buffer = lld_data->usbh_buffer;
+//	LOG_PRINTF("\nSTATE: %d\n", state);
+	switch (dev->state) {
+	case USBH_ENUM_STATE_SET_ADDRESS:
+		switch (cb_data.status) {
+		case USBH_PACKET_CALLBACK_STATUS_OK:
+			if (dev->address == 0) {
+				dev->address = usbh_data.address_temporary;
+				LOG_PRINTF("Assigned address: %d\n", dev->address);
+			}
+			CONTINUE_WITH(USBH_ENUM_STATE_DEVICE_DT_READ_SETUP);
+			break;
+
+		default:
+			device_enumeration_terminate(dev);
+			ERROR(cb_data.status);
+			break;
+		}
+		break;
+
+	case USBH_ENUM_STATE_DEVICE_DT_READ_SETUP:
+		{
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_DEVICE << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = USB_DT_DEVICE_SIZE;
+
+			dev->state = USBH_ENUM_STATE_DEVICE_DT_READ_COMPLETE;
+			device_control(dev, device_enumerate, &setup_data, &usbh_buffer[0]);
+		}
+		break;
+
+	case USBH_ENUM_STATE_DEVICE_DT_READ_COMPLETE:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				{
+					struct usb_device_descriptor *ddt =
+							(struct usb_device_descriptor *)&usbh_buffer[0];
+					dev->packet_size_max0 = ddt->bMaxPacketSize0;
+					LOG_PRINTF("Found device with vid=0x%04x pid=0x%04x\n", ddt->idVendor, ddt->idProduct);
+					LOG_PRINTF("class=0x%02x subclass=0x%02x protocol=0x%02x\n", ddt->bDeviceClass, ddt->bDeviceSubClass, ddt->bDeviceProtocol);
+					CONTINUE_WITH(USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_SETUP)
+				}
+				break;
+
+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+				if (cb_data.transferred_length >= 8) {
+					struct usb_device_descriptor *ddt =
+						(struct usb_device_descriptor *)&usbh_buffer[0];
+					dev->packet_size_max0 = ddt->bMaxPacketSize0;
+					CONTINUE_WITH(USBH_ENUM_STATE_DEVICE_DT_READ_SETUP);
+				} else {
+					device_enumeration_terminate(dev);
+					ERROR(cb_data.status);
+				}
+				break;
+
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_SETUP:
+		{
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_CONFIGURATION << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = dev->packet_size_max0;
+
+			dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ;
+			device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
+				device_enumerate, dev);
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_COMPLETE;
+				device_xfer_control_read(&usbh_buffer[USB_DT_DEVICE_SIZE],
+					dev->packet_size_max0, device_enumerate, dev);
+				break;
+
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_HEADER_READ_COMPLETE:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				CONTINUE_WITH(USBH_ENUM_STATE_CONFIGURATION_DT_READ_SETUP);
+				break;
+
+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+				if (cb_data.transferred_length >= USB_DT_CONFIGURATION_SIZE) {
+					struct usb_config_descriptor *cdt =
+						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+					if (cb_data.transferred_length == cdt->wTotalLength) {
+						LOG_PRINTF("Configuration descriptor read complete. length: %d\n", cdt->wTotalLength);
+						CONTINUE_WITH(USBH_ENUM_STATE_SET_CONFIGURATION_SETUP);
+					}
+				}
+				break;
+
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_READ_SETUP:
+		{
+			struct usb_config_descriptor *cdt =
+				(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+			struct usb_setup_data setup_data;
+			LOG_PRINTF("Getting complete configuration descriptor of length: %d bytes\n", cdt->wTotalLength);
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_CONFIGURATION << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = cdt->wTotalLength;
+
+			dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_READ;
+			device_xfer_control_write_setup(&setup_data, sizeof(setup_data),
+				device_enumerate, dev);
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_READ:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				{
+					struct usb_config_descriptor *cdt =
+						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+					dev->state = USBH_ENUM_STATE_CONFIGURATION_DT_READ_COMPLETE;
+					device_xfer_control_read(&usbh_buffer[USB_DT_DEVICE_SIZE],
+						cdt->wTotalLength, device_enumerate, dev);
+				}
+				break;
+
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+		}
+		break;
+
+	case USBH_ENUM_STATE_CONFIGURATION_DT_READ_COMPLETE:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				{
+					struct usb_config_descriptor *cdt =
+						(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+					LOG_PRINTF("Configuration descriptor read complete. length: %d\n", cdt->wTotalLength);
+					CONTINUE_WITH(USBH_ENUM_STATE_SET_CONFIGURATION_SETUP);
+
+				}
+				break;
+
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+
+		}
+		break;
+
+	case USBH_ENUM_STATE_SET_CONFIGURATION_SETUP:
+		{
+			struct usb_config_descriptor *cdt =
+				(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_STANDARD | USB_REQ_TYPE_DEVICE;
+			setup_data.bRequest = USB_REQ_SET_CONFIGURATION;
+			setup_data.wValue = cdt->bConfigurationValue;
+			setup_data.wIndex = 0;
+			setup_data.wLength = 0;
+
+			dev->state = USBH_ENUM_STATE_SET_CONFIGURATION_COMPLETE;
+			device_control(dev, device_enumerate, &setup_data, 0);
+		}
+		break;
+
+	case USBH_ENUM_STATE_SET_CONFIGURATION_COMPLETE:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				CONTINUE_WITH(USBH_ENUM_STATE_FIND_DRIVER);
+				break;
+
+			default:
+				device_enumeration_terminate(dev);
+				ERROR(cb_data.status);
+				break;
+			}
+		}
+		break;
+
+	case USBH_ENUM_STATE_FIND_DRIVER:
+		{
+			struct usb_config_descriptor *cdt =
+				(struct usb_config_descriptor *)&usbh_buffer[USB_DT_DEVICE_SIZE];
+			device_register(usbh_buffer, cdt->wTotalLength + USB_DT_DEVICE_SIZE, dev);
+
+			device_enumeration_finish(dev);
+		}
+		break;
+
+	default:
+		LOG_PRINTF("Error: Unknown state "__FILE__"/%d\n", __LINE__);
+		break;
+	}
+}
+
+void device_enumeration_start(usbh_device_t *dev)
+{
+	set_enumeration();
+
+	// save address
+	uint8_t address = dev->address;
+	dev->address = 0;
+
+	if (dev->speed == USBH_SPEED_LOW) {
+		dev->packet_size_max0 = 8;
+	} else {
+		dev->packet_size_max0 = 64;
+	}
+
+	usbh_data.address_temporary = address;
+
+	LOG_PRINTF("\n\n\n ENUMERATION OF DEVICE@%d STARTED \n\n", address);
+
+	dev->state = USBH_ENUM_STATE_SET_ADDRESS;
+	struct usb_setup_data setup_data;
+
+	setup_data.bmRequestType = USB_REQ_TYPE_STANDARD | USB_REQ_TYPE_DEVICE;
+	setup_data.bRequest = USB_REQ_SET_ADDRESS;
+	setup_data.wValue = address;
+	setup_data.wIndex = 0;
+	setup_data.wLength = 0;
+
+	device_control(dev, device_enumerate, &setup_data, 0);
+}
+
+/**
+ * Should be called with at least 1kHz frequency
+ *
+ */
+void usbh_poll(uint32_t time_curr_us)
+{
+	uint32_t k = 0;
+	while (usbh_data.lld_drivers[k]) {
+		usbh_device_t *usbh_device =
+			((usbh_generic_data_t *)(usbh_data.lld_drivers[k]->driver_data))->usbh_device;
+		usbh_generic_data_t *lld_data = usbh_data.lld_drivers[k]->driver_data;
+
+		enum USBH_POLL_STATUS poll_status =
+			usbh_data.lld_drivers[k]->poll(lld_data, time_curr_us);
+
+		switch (poll_status) {
+		case USBH_POLL_STATUS_DEVICE_CONNECTED:
+			// New device found
+			LOG_PRINTF("\nDEVICE FOUND\n");
+			usbh_device[0].lld = usbh_data.lld_drivers[k];
+			usbh_device[0].speed = usbh_data.lld_drivers[k]->root_speed(lld_data);
+			usbh_device[0].address = 1;
+			usbh_device[0].control.state = USBH_CONTROL_STATE_NONE;
+
+			device_enumeration_start(&usbh_device[0]);
+			break;
+
+		case USBH_POLL_STATUS_DEVICE_DISCONNECTED:
+			{
+				usbh_device[0].control.state = USBH_CONTROL_STATE_NONE;
+				uint32_t i;
+				for (i = 0; i < USBH_MAX_DEVICES; i++) {
+					device_remove(&usbh_device[i]);
+				}
+			}
+			break;
+
+		default:
+			break;
+		}
+
+		if (lld_data->usbh_device[0].drv && usbh_device[0].drvdata) {
+			usbh_device[0].drv->poll(usbh_device[0].drvdata, time_curr_us);
+		}
+
+		k++;
+	}
+}
+
+void usbh_read(usbh_device_t *dev, usbh_packet_t *packet)
+{
+	const usbh_low_level_driver_t *lld = dev->lld;
+	lld->read(lld->driver_data, packet);
+}
+
+void usbh_write(usbh_device_t *dev, const usbh_packet_t *packet)
+{
+	const usbh_low_level_driver_t *lld = dev->lld;
+	lld->write(lld->driver_data, packet);
+}
+
diff --git a/fw/src/usbh_driver_ac_midi.c b/fw/src/usbh_driver_ac_midi.c
new file mode 100644
index 0000000..96cf383
--- /dev/null
+++ b/fw/src/usbh_driver_ac_midi.c
@@ -0,0 +1,364 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2016 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+ 
+#include "driver/usbh_device_driver.h"
+#include "usbh_driver_ac_midi_private.h"
+#include "usart_helpers.h"
+
+#include <stddef.h>
+
+#include <libopencm3/usb/midi.h>
+#include <libopencm3/usb/audio.h>
+#include <libopencm3/usb/usbstd.h>
+
+static midi_device_t midi_device[USBH_AC_MIDI_MAX_DEVICES];
+static const midi_config_t *midi_config = NULL;
+static bool initialized = false;
+
+void midi_driver_init(const midi_config_t *config)
+{
+	uint32_t i;
+	midi_config = config;
+	for (i = 0; i < USBH_AC_MIDI_MAX_DEVICES; i++) {
+		midi_device[i].state = 0;
+	}
+	initialized = true;
+}
+/**
+ *
+ *
+ */
+static void *init(usbh_device_t *usbh_dev)
+{
+	if (!midi_config || !initialized) {
+		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
+		return 0;
+	}
+	uint32_t i;
+	midi_device_t *drvdata = NULL;
+
+	// find free data space for midi device
+	for (i = 0; i < USBH_AC_MIDI_MAX_DEVICES; i++) {
+		if (midi_device[i].state == 0) {
+			drvdata = &midi_device[i];
+			drvdata->device_id = i;
+			drvdata->endpoint_in_address = 0;
+			drvdata->endpoint_out_address = 0;
+			drvdata->endpoint_in_toggle = 0;
+			drvdata->endpoint_out_toggle = 0;
+			drvdata->usbh_device = usbh_dev;
+			drvdata->write_callback_user = NULL;
+			drvdata->sending = false;
+			break;
+		}
+	}
+
+	return drvdata;
+}
+
+/**
+ * Returns true if all needed data are parsed
+ */
+static bool analyze_descriptor(void *drvdata, void *descriptor)
+{
+	midi_device_t *midi = drvdata;
+	uint8_t desc_type = ((uint8_t *)descriptor)[1];
+	switch (desc_type) {
+	case USB_DT_CONFIGURATION:
+		{
+			struct usb_config_descriptor *cfg =
+				(struct usb_config_descriptor*)descriptor;
+			midi->buffer[0] = cfg->bConfigurationValue;
+		}
+		break;
+	case USB_DT_DEVICE:
+		break;
+	case USB_DT_INTERFACE:
+		break;
+	case USB_DT_ENDPOINT:
+		{
+			struct usb_endpoint_descriptor *ep =
+				(struct usb_endpoint_descriptor*)descriptor;
+			if ((ep->bmAttributes&0x03) == USB_ENDPOINT_ATTR_BULK) {
+				uint8_t epaddr = ep->bEndpointAddress;
+				if (epaddr & (1<<7)) {
+					midi->endpoint_in_address = epaddr&0x7f;
+					if (ep->wMaxPacketSize < USBH_AC_MIDI_BUFFER) {
+						midi->endpoint_in_maxpacketsize = ep->wMaxPacketSize;
+					} else {
+						midi->endpoint_in_maxpacketsize = USBH_AC_MIDI_BUFFER;
+					}
+				} else {
+					midi->endpoint_out_address = epaddr;
+					midi->endpoint_out_maxpacketsize = ep->wMaxPacketSize;
+				}
+
+				if (midi->endpoint_in_address && midi->endpoint_out_address) {
+					midi->state = 1;
+					return true;
+				}
+			}
+		}
+		break;
+
+	case USB_AUDIO_DT_CS_ENDPOINT:
+	{
+		struct usb_midi_in_jack_descriptor *midi_in_jack_desc =
+			(struct usb_midi_in_jack_descriptor *) descriptor;
+		(void)midi_in_jack_desc;
+	}
+		break;
+	// TODO Class Specific descriptors
+	default:
+		break;
+	}
+	return false;
+}
+
+static void midi_in_message(midi_device_t *midi, const uint8_t datalen)
+{
+	uint8_t i = 0;
+	if (midi_config->read_callback) {
+		for (i = 0; i < datalen; i += 4) {
+
+//			uint8_t cable_number = (midi->buffer[i] & 0xf0) >> 4;
+			uint8_t code_id = midi->buffer[i]&0xf;
+
+			uint8_t *ptrdata = &midi->buffer[i];
+			if (code_id < 2) {
+				continue;
+			}
+			midi_config->read_callback(midi->device_id, ptrdata);
+		}
+	}
+}
+
+static void event(usbh_device_t *dev, usbh_packet_callback_data_t status)
+{
+	midi_device_t *midi = (midi_device_t *)dev->drvdata;
+	switch (midi->state) {
+	case 26:
+		{
+			switch (status.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				midi_in_message(midi, midi->endpoint_in_maxpacketsize);
+				midi->state = 25;
+				break;
+
+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+				midi_in_message(midi, status.transferred_length);
+				midi->state = 25;
+				break;
+
+			default:
+				LOG_PRINTF("FATAL ERROR, MIDI DRIVER DEAD \n");
+				//~ dev->drv->remove();
+				midi->state = 0;
+				break;
+			}
+		}
+		break;
+
+	case 102:
+		{
+			midi->state = 101;
+			LOG_PRINTF("\n CAN'T TOUCH THIS... ignoring data\n");
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+static void read_midi_in(void *drvdata, const uint8_t nextstate)
+{
+	midi_device_t *midi = drvdata;
+	usbh_packet_t packet;
+
+	packet.address = midi->usbh_device->address;
+	packet.data.in = &midi->buffer[0];
+	packet.datalen = midi->endpoint_in_maxpacketsize;
+	packet.endpoint_address = midi->endpoint_in_address;
+	packet.endpoint_size_max = midi->endpoint_in_maxpacketsize;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_BULK;
+	packet.speed = midi->usbh_device->speed;
+	packet.callback = event;
+	packet.callback_arg = midi->usbh_device;
+	packet.toggle = &midi->endpoint_in_toggle;
+
+	midi->state = nextstate;
+	usbh_read(midi->usbh_device,&packet);
+}
+
+/**
+ * 
+ *  @param t_us global time us
+ */
+static void poll(void *drvdata, uint32_t t_us)
+{
+	(void)drvdata;
+
+	midi_device_t *midi = drvdata;
+	switch (midi->state) {
+
+	/// Upon configuration, some controllers send additional error data
+	/// case 100, 101, 102 cares for ignoring those data
+	case 100:
+		{
+			midi->time_us_config = t_us;
+			midi->state = 101;
+		}
+		break;
+
+	case 101:
+		{
+			read_midi_in(drvdata, 102);
+		}
+		break;
+
+	case 102:
+		{
+			// if elapsed MIDI initial delay microseconds
+			if (t_us - midi->time_us_config > MIDI_INITIAL_DELAY) {
+				midi->state = 26;
+			}
+		}
+		break;
+
+	case 25:
+		{
+			read_midi_in(drvdata, 26);
+		}
+		break;
+
+	case 1:
+		{
+			midi->state = 100;
+
+			midi->endpoint_in_toggle = 0;
+			LOG_PRINTF("\nMIDI CONFIGURED\n");
+
+			// Notify user
+			if (midi_config->notify_connected) {
+				midi_config->notify_connected(midi->device_id);
+			}
+		}
+		break;
+	}
+}
+
+// don't call directly
+static void write_callback(usbh_device_t *dev, usbh_packet_callback_data_t status)
+{
+	(void)status;
+	midi_device_t *midi = (midi_device_t *)dev->drvdata;
+
+	if (midi->sending) {
+		midi->sending = false;
+		const midi_write_callback_t callback = midi->write_callback_user;
+		if (!callback) {
+			return;
+		}
+
+		if (status.status & USBH_PACKET_CALLBACK_STATUS_OK) {
+			callback(midi->write_packet.datalen);
+		} else {
+			if (status.status & USBH_PACKET_CALLBACK_STATUS_ERRSIZ) {
+				const uint32_t length = status.transferred_length;
+				callback(length);
+			} else {
+				callback(0);
+			}
+		}
+	}
+}
+
+void usbh_midi_write(uint8_t device_id, const void *data, uint32_t length, midi_write_callback_t callback)
+{
+	// bad device_id handling
+	if (device_id >= USBH_AC_MIDI_MAX_DEVICES) {
+		return;
+	}
+
+	midi_device_t *midi = &midi_device[device_id];
+
+	// device with provided device_id is not alive
+	if (midi->state == 0) {
+		return;
+	}
+
+	usbh_device_t *dev = midi->usbh_device;
+	if (midi->endpoint_out_address == 0) {
+		return;
+	}
+
+	midi->sending = true;
+	midi->write_callback_user = callback;
+
+	midi->write_packet.data.out = data;
+	midi->write_packet.datalen = length;
+	midi->write_packet.address = dev->address;
+	midi->write_packet.endpoint_address = midi->endpoint_out_address;
+	midi->write_packet.endpoint_size_max = midi->endpoint_out_maxpacketsize;
+	midi->write_packet.endpoint_type = USBH_ENDPOINT_TYPE_BULK;
+	midi->write_packet.speed = dev->speed;
+	midi->write_packet.callback = write_callback;
+	midi->write_packet.callback_arg = midi->usbh_device;
+	midi->write_packet.toggle = &midi->endpoint_out_toggle;
+
+
+	usbh_write(dev, &midi->write_packet);
+}
+
+static void remove(void *drvdata)
+{
+	midi_device_t *midi = drvdata;
+
+	if (midi_config->notify_disconnected) {
+		midi_config->notify_disconnected(midi->device_id);
+	}
+
+	midi->state = 0;
+	midi->endpoint_in_address = 0;
+	midi->endpoint_out_address = 0;
+}
+
+static const usbh_dev_driver_info_t usbh_midi_driver_info = {
+	.deviceClass = -1,
+	.deviceSubClass = -1,
+	.deviceProtocol = -1,
+	.idVendor = -1,
+	.idProduct = -1,
+	.ifaceClass = 0x01,
+	.ifaceSubClass = 0x03,
+	.ifaceProtocol = -1,
+};
+
+const usbh_dev_driver_t usbh_midi_driver = {
+	.init = init,
+	.analyze_descriptor = analyze_descriptor,
+	.poll = poll,
+	.remove = remove,
+	.info = &usbh_midi_driver_info
+};
diff --git a/fw/src/usbh_driver_ac_midi_private.h b/fw/src/usbh_driver_ac_midi_private.h
new file mode 100644
index 0000000..b92ee97
--- /dev/null
+++ b/fw/src/usbh_driver_ac_midi_private.h
@@ -0,0 +1,52 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2016 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef USBH_DRIVER_AC_MIDI_PRIVATE_
+#define USBH_DRIVER_AC_MIDI_PRIVATE_
+
+#include "driver/usbh_device_driver.h"
+#include "usbh_driver_ac_midi.h"
+
+#include <stdint.h>
+
+
+#define MIDI_INITIAL_DELAY 	(100000)
+
+struct _midi_device {
+	usbh_device_t *usbh_device;
+	uint8_t buffer[USBH_AC_MIDI_BUFFER];
+	uint16_t endpoint_in_maxpacketsize;
+	uint16_t endpoint_out_maxpacketsize;
+	uint8_t endpoint_in_address;
+	uint8_t endpoint_out_address;
+	uint8_t state;
+	uint8_t endpoint_in_toggle;
+	uint8_t endpoint_out_toggle;
+	uint8_t device_id;
+	bool sending;
+	midi_write_callback_t write_callback_user;
+	usbh_packet_t write_packet;
+	// Timestamp at sending config command
+	uint32_t time_us_config;
+};
+typedef struct _midi_device midi_device_t;
+#endif
diff --git a/fw/src/usbh_driver_gp_xbox.c b/fw/src/usbh_driver_gp_xbox.c
new file mode 100644
index 0000000..957cb3e
--- /dev/null
+++ b/fw/src/usbh_driver_gp_xbox.c
@@ -0,0 +1,368 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+
+#include "usart_helpers.h"
+#include "usbh_driver_gp_xbox.h"
+#include "driver/usbh_device_driver.h"
+
+#include <stdint.h>
+#include <libopencm3/usb/usbstd.h>
+
+enum STATES {
+	STATE_INACTIVE,
+	STATE_INITIAL,
+	STATE_READING_REQUEST,
+	STATE_READING_COMPLETE,
+};
+
+#define GP_XBOX_CORRECT_TRANSFERRED_LENGTH 20
+
+struct _gp_xbox_device {
+	usbh_device_t *usbh_device;
+	uint8_t buffer[USBH_GP_XBOX_BUFFER];
+	uint16_t endpoint_in_maxpacketsize;
+	uint8_t endpoint_in_address;
+	enum STATES state_next;
+	uint8_t endpoint_in_toggle;
+	uint8_t device_id;
+	uint8_t configuration_value;
+};
+typedef struct _gp_xbox_device gp_xbox_device_t;
+
+static gp_xbox_device_t gp_xbox_device[USBH_GP_XBOX_MAX_DEVICES];
+static const gp_xbox_config_t *gp_xbox_config;
+
+static bool initialized = false;
+static void read_gp_xbox_in(gp_xbox_device_t *gp_xbox);
+
+void gp_xbox_driver_init(const gp_xbox_config_t *config)
+{
+	if (!config) {
+		return;
+	}
+	initialized = true;
+	uint32_t i;
+	gp_xbox_config = config;
+	for (i = 0; i < USBH_GP_XBOX_MAX_DEVICES; i++) {
+		gp_xbox_device[i].state_next = STATE_INACTIVE;
+	}
+}
+
+/**
+ *
+ *
+ */
+static void *init(usbh_device_t *usbh_dev)
+{
+	if (!initialized) {
+		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
+		return 0;
+	}
+
+	uint32_t i;
+	gp_xbox_device_t *drvdata = 0;
+
+	// find free data space for gp_xbox device
+	for (i = 0; i < USBH_GP_XBOX_MAX_DEVICES; i++) {
+		if (gp_xbox_device[i].state_next == STATE_INACTIVE) {
+			drvdata = &gp_xbox_device[i];
+			drvdata->device_id = i;
+			drvdata->endpoint_in_address = 0;
+			drvdata->endpoint_in_toggle = 0;
+			drvdata->usbh_device = usbh_dev;
+			break;
+		}
+	}
+
+	return drvdata;
+}
+
+/**
+ * Returns true if all needed data are parsed
+ */
+static bool analyze_descriptor(void *drvdata, void *descriptor)
+{
+	gp_xbox_device_t *gp_xbox = (gp_xbox_device_t *)drvdata;
+	uint8_t desc_type = ((uint8_t *)descriptor)[1];
+	switch (desc_type) {
+	case USB_DT_CONFIGURATION:
+		{
+			struct usb_config_descriptor *cfg = (struct usb_config_descriptor*)descriptor;
+			gp_xbox->configuration_value = cfg->bConfigurationValue;
+		}
+		break;
+	case USB_DT_DEVICE:
+		break;
+	case USB_DT_INTERFACE:
+		break;
+	case USB_DT_ENDPOINT:
+		{
+			struct usb_endpoint_descriptor *ep = (struct usb_endpoint_descriptor*)descriptor;
+			if ((ep->bmAttributes&0x03) == USB_ENDPOINT_ATTR_INTERRUPT) {
+				uint8_t epaddr = ep->bEndpointAddress;
+				if (epaddr & (1<<7)) {
+					gp_xbox->endpoint_in_address = epaddr&0x7f;
+					if (ep->wMaxPacketSize < USBH_GP_XBOX_BUFFER) {
+						gp_xbox->endpoint_in_maxpacketsize = ep->wMaxPacketSize;
+					} else {
+						gp_xbox->endpoint_in_maxpacketsize = USBH_GP_XBOX_BUFFER;
+					}
+				}
+
+				if (gp_xbox->endpoint_in_address) {
+					gp_xbox->state_next = STATE_INITIAL;
+					return true;
+				}
+			}
+		}
+		break;
+	// TODO Class Specific descriptors
+	default:
+		break;
+	}
+	return false;
+}
+
+static void parse_data(usbh_device_t *dev)
+{
+	gp_xbox_device_t *gp_xbox = (gp_xbox_device_t *)dev->drvdata;
+
+	uint8_t *packet = gp_xbox->buffer;
+
+	gp_xbox_packet_t gp_xbox_packet;
+	gp_xbox_packet.buttons = 0;
+
+	// DPAD
+	const uint8_t data1 = packet[2];
+	const uint8_t data2 = packet[3];
+	if (data1 & (1 << 0)) {
+		gp_xbox_packet.buttons |= GP_XBOX_DPAD_TOP;
+	}
+
+	if (data1 & (1 << 1)) {
+		gp_xbox_packet.buttons |= GP_XBOX_DPAD_BOTTOM;
+	}
+
+	if (data1 & (1 << 2)) {
+		gp_xbox_packet.buttons |= GP_XBOX_DPAD_LEFT;
+	}
+
+	if (data1 & (1 << 3)) {
+		gp_xbox_packet.buttons |= GP_XBOX_DPAD_RIGHT;
+	}
+
+	// Start + select
+
+	if (data1 & (1 << 4)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_START;
+	}
+
+	if (data1 & (1 << 5)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_SELECT;
+	}
+
+	// axis buttons
+
+	if (data1 & (1 << 6)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_AXIS_LEFT;
+	}
+
+	if (data1 & (1 << 7)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_AXIS_RIGHT;
+	}
+
+	// buttons ABXY
+
+	if (data2 & (1 << 4)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_A;
+	}
+
+	if (data2 & (1 << 5)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_B;
+	}
+
+	if (data2 & (1 << 6)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_X;
+	}
+
+	if (data2 & (1 << 7)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_Y;
+	}
+
+	// buttons rear
+
+	if (data2 & (1 << 0)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_LT;
+	}
+
+	if (data2 & (1 << 1)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_RT;
+	}
+
+	if (data2 & (1 << 2)) {
+		gp_xbox_packet.buttons |= GP_XBOX_BUTTON_XBOX;
+	}
+
+	// rear levers
+
+	gp_xbox_packet.axis_rear_left = packet[4];
+	gp_xbox_packet.axis_rear_right = packet[5];
+	gp_xbox_packet.axis_left_x = packet[7]*256 + packet[6];
+	gp_xbox_packet.axis_left_y = packet[9]*256 + packet[8];
+	gp_xbox_packet.axis_right_x = packet[11]*256 + packet[10];
+	gp_xbox_packet.axis_right_y = packet[13]*256 + packet[12];
+
+	// call update callback
+	if (gp_xbox_config->update) {
+		gp_xbox_config->update(gp_xbox->device_id, gp_xbox_packet);
+	}
+}
+
+static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	gp_xbox_device_t *gp_xbox = (gp_xbox_device_t *)dev->drvdata;
+	switch (gp_xbox->state_next) {
+	case STATE_READING_COMPLETE:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				parse_data(dev);
+				gp_xbox->state_next = STATE_READING_REQUEST;
+				break;
+
+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+				if (cb_data.transferred_length == GP_XBOX_CORRECT_TRANSFERRED_LENGTH) {
+					parse_data(dev);
+				}
+				gp_xbox->state_next = STATE_READING_REQUEST;
+				break;
+
+			default:
+				ERROR(cb_data.status);
+				gp_xbox->state_next = STATE_INACTIVE;
+				break;
+			}
+		}
+		break;
+
+	case STATE_INACTIVE:
+		{
+			LOG_PRINTF("XBOX inactive");
+		}
+		break;
+	default:
+		{
+			LOG_PRINTF("Unknown state\n");
+		}
+		break;
+	}
+}
+
+
+static void read_gp_xbox_in(gp_xbox_device_t *gp_xbox)
+{
+	usbh_packet_t packet;
+
+	packet.address = gp_xbox->usbh_device->address;
+	packet.data.in = &gp_xbox->buffer[0];
+	packet.datalen = gp_xbox->endpoint_in_maxpacketsize;
+	packet.endpoint_address = gp_xbox->endpoint_in_address;
+	packet.endpoint_size_max = gp_xbox->endpoint_in_maxpacketsize;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_INTERRUPT;
+	packet.speed = gp_xbox->usbh_device->speed;
+	packet.callback = event;
+	packet.callback_arg = gp_xbox->usbh_device;
+	packet.toggle = &gp_xbox->endpoint_in_toggle;
+
+	gp_xbox->state_next = STATE_READING_COMPLETE;
+	usbh_read(gp_xbox->usbh_device, &packet);
+
+	// LOG_PRINTF("@gp_xbox EP1 |  \n");
+}
+
+/**
+ * \param time_curr_us - monotically rising time (see usbh_hubbed.h)
+ *		unit is microseconds
+ */
+static void poll(void *drvdata, uint32_t time_curr_us)
+{
+	(void)time_curr_us;
+
+	gp_xbox_device_t *gp_xbox = (gp_xbox_device_t *)drvdata;
+
+	switch (gp_xbox->state_next) {
+	case STATE_READING_REQUEST:
+		{
+			read_gp_xbox_in(gp_xbox);
+		}
+		break;
+
+	case STATE_INITIAL:
+		{
+			gp_xbox->state_next = STATE_READING_REQUEST;
+			gp_xbox->endpoint_in_toggle = 0;
+			LOG_PRINTF("\ngp_xbox CONFIGURED\n");
+			if (gp_xbox_config->notify_connected) {
+				gp_xbox_config->notify_connected(gp_xbox->device_id);
+			}
+		}
+		break;
+
+	default:
+		{
+			// do nothing - probably transfer is in progress
+		}
+		break;
+	}
+}
+
+static void remove(void *drvdata)
+{
+	LOG_PRINTF("Removing xbox\n");
+
+	gp_xbox_device_t *gp_xbox = (gp_xbox_device_t *)drvdata;
+	if (gp_xbox_config->notify_disconnected) {
+		gp_xbox_config->notify_disconnected(gp_xbox->device_id);
+	}
+	gp_xbox->state_next = STATE_INACTIVE;
+	gp_xbox->endpoint_in_address = 0;
+}
+
+static const usbh_dev_driver_info_t driver_info = {
+	.deviceClass = 0xff,
+	.deviceSubClass = 0xff,
+	.deviceProtocol = 0xff,
+	.idVendor = 0x045e,
+	.idProduct = 0x028e,
+	.ifaceClass = 0xff,
+	.ifaceSubClass = 93,
+	.ifaceProtocol = 0x01
+};
+
+const usbh_dev_driver_t usbh_gp_xbox_driver = {
+	.init = init,
+	.analyze_descriptor = analyze_descriptor,
+	.poll = poll,
+	.remove = remove,
+	.info = &driver_info
+};
diff --git a/fw/src/usbh_driver_hid.c b/fw/src/usbh_driver_hid.c
new file mode 100644
index 0000000..4893354
--- /dev/null
+++ b/fw/src/usbh_driver_hid.c
@@ -0,0 +1,412 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2016 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "usbh_core.h"
+#include "driver/usbh_device_driver.h"
+#include "usbh_driver_hid.h"
+#include "usart_helpers.h"
+
+#include <libopencm3/usb/usbstd.h>
+#include <libopencm3/usb/hid.h>
+#include <stdint.h>
+#include <stddef.h>
+
+#define USB_HID_SET_REPORT 0x09
+#define USB_HID_SET_IDLE 0x0A
+
+enum STATES {
+	STATE_INACTIVE,
+	STATE_READING_REQUEST,
+	STATE_READING_COMPLETE_AND_CHECK_REPORT,
+	STATE_SET_REPORT_EMPTY_READ,
+	STATE_GET_REPORT_DESCRIPTOR_READ_SETUP,// configuration is complete at this point. We write request
+	STATE_GET_REPORT_DESCRIPTOR_READ_COMPLETE,// after the read finishes, we parse that descriptor
+	STATE_SET_IDLE,
+	STATE_SET_IDLE_COMPLETE,
+};
+
+enum REPORT_STATE {
+	REPORT_STATE_NULL,
+	REPORT_STATE_READY,
+	REPORT_STATE_PENDING,
+};
+
+struct _hid_device {
+	usbh_device_t *usbh_device;
+	uint8_t buffer[USBH_HID_BUFFER];
+	uint16_t endpoint_in_maxpacketsize;
+	uint8_t endpoint_in_address;
+	enum STATES state_next;
+	uint8_t endpoint_in_toggle;
+	uint8_t device_id;
+	uint8_t configuration_value;
+	uint16_t report0_length;
+	enum REPORT_STATE report_state;
+	uint8_t report_data[USBH_HID_REPORT_BUFFER];
+	uint8_t report_data_length;
+	enum HID_TYPE hid_type;
+	uint8_t interface_number;
+};
+typedef struct _hid_device hid_device_t;
+
+struct hid_report_decriptor {
+	struct usb_hid_descriptor header;
+	struct _report_descriptor_info {
+		uint8_t bDescriptorType;
+		uint16_t wDescriptorLength;
+	} __attribute__((packed)) report_descriptors_info[];
+} __attribute__((packed));
+
+static hid_device_t hid_device[USBH_HID_MAX_DEVICES];
+static hid_config_t hid_config;
+
+static bool initialized = false;
+
+void hid_driver_init(const hid_config_t *config)
+{
+	uint32_t i;
+
+	initialized = true;
+
+	hid_config = *config;
+	for (i = 0; i < USBH_HID_MAX_DEVICES; i++) {
+		hid_device[i].state_next = STATE_INACTIVE;
+	}
+}
+
+static void *init(usbh_device_t *usbh_dev)
+{
+	if (!initialized) {
+		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);
+		return 0;
+	}
+
+	uint32_t i;
+	hid_device_t *drvdata = NULL;
+
+	// find free data space for HID device
+	for (i = 0; i < USBH_HID_MAX_DEVICES; i++) {
+		if (hid_device[i].state_next == STATE_INACTIVE) {
+			drvdata = &hid_device[i];
+			drvdata->device_id = i;
+			drvdata->endpoint_in_address = 0;
+			drvdata->endpoint_in_toggle = 0;
+			drvdata->report0_length = 0;
+			drvdata->usbh_device = usbh_dev;
+			drvdata->report_state = REPORT_STATE_NULL;
+			drvdata->hid_type = HID_TYPE_NONE;
+			break;
+		}
+	}
+
+	return drvdata;
+}
+
+static void parse_report_descriptor(hid_device_t *hid, const uint8_t *buffer, uint32_t length)
+{
+	// TODO
+	// Do some parsing!
+	// add some checks
+	hid->report_state = REPORT_STATE_READY;
+
+	// TODO: parse this from buffer!
+	hid->report_data_length = 1;
+	(void)buffer;
+	(void)length;
+}
+
+/**
+ * Returns true if all needed data are parsed
+ */
+static bool analyze_descriptor(void *drvdata, void *descriptor)
+{
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	uint8_t desc_type = ((uint8_t *)descriptor)[1];
+	switch (desc_type) {
+	case USB_DT_CONFIGURATION:
+		{
+			const struct usb_config_descriptor * cfg = (const struct usb_config_descriptor*)descriptor;
+			hid->configuration_value = cfg->bConfigurationValue;
+		}
+		break;
+
+	case USB_DT_DEVICE:
+		{
+			const struct usb_device_descriptor *devDesc = (const struct usb_device_descriptor *)descriptor;
+			(void)devDesc;
+		}
+		break;
+
+	case USB_DT_INTERFACE:
+		{
+			const struct usb_interface_descriptor *ifDesc = (const struct usb_interface_descriptor *)descriptor;
+			if (ifDesc->bInterfaceProtocol == 0x01) {
+				hid->hid_type = HID_TYPE_KEYBOARD;
+				hid->interface_number = ifDesc->bInterfaceNumber;
+			} else if (ifDesc->bInterfaceProtocol == 0x02) {
+				hid->hid_type = HID_TYPE_MOUSE;
+				hid->interface_number = ifDesc->bInterfaceNumber;
+			}
+		}
+		break;
+
+	case USB_DT_ENDPOINT:
+		{
+			const struct usb_endpoint_descriptor *ep = (const struct usb_endpoint_descriptor *)descriptor;
+			if ((ep->bmAttributes&0x03) == USB_ENDPOINT_ATTR_INTERRUPT) {
+				uint8_t epaddr = ep->bEndpointAddress;
+				if (epaddr & (1<<7)) {
+					hid->endpoint_in_address = epaddr&0x7f;
+					if (ep->wMaxPacketSize < USBH_HID_BUFFER) {
+						hid->endpoint_in_maxpacketsize = ep->wMaxPacketSize;
+					} else {
+						hid->endpoint_in_maxpacketsize = USBH_HID_BUFFER;
+					}
+				}
+			}
+		}
+		break;
+
+	case USB_DT_HID:
+		{
+			const struct hid_report_decriptor *desc = (const struct hid_report_decriptor *)descriptor;
+			if (desc->header.bNumDescriptors > 0 && desc->report_descriptors_info[0].bDescriptorType == USB_DT_REPORT) {
+				hid->report0_length = desc->report_descriptors_info[0].wDescriptorLength;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	if (hid->endpoint_in_address && hid->report0_length) {
+		hid->state_next = STATE_GET_REPORT_DESCRIPTOR_READ_SETUP;
+		return true;
+	}
+
+	return false;
+}
+
+static void report_event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	(void)cb_data;// UNUSED
+
+	hid_device_t *hid = (hid_device_t *)dev->drvdata;
+	hid->report_state = REPORT_STATE_READY;
+}
+
+static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)
+{
+	hid_device_t *hid = (hid_device_t *)dev->drvdata;
+
+	switch (hid->state_next) {
+	case STATE_READING_COMPLETE_AND_CHECK_REPORT:
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:
+				if (hid_config.hid_in_message_handler) {
+					hid_config.hid_in_message_handler(hid->device_id, hid->buffer, cb_data.transferred_length);
+				}
+				hid->state_next = STATE_READING_REQUEST;
+				break;
+
+			default:
+				ERROR(cb_data.status);
+				hid->state_next = STATE_INACTIVE;
+				break;
+			}
+		}
+		break;
+
+	case STATE_GET_REPORT_DESCRIPTOR_READ_COMPLETE: // read complete, SET_IDLE to 0
+		{
+			switch (cb_data.status) {
+			case USBH_PACKET_CALLBACK_STATUS_OK:
+				LOG_PRINTF("READ REPORT COMPLETE \n");
+				hid->state_next = STATE_READING_REQUEST;
+				hid->endpoint_in_toggle = 0;
+
+				parse_report_descriptor(hid, hid->buffer, cb_data.transferred_length);
+				break;
+
+			default:
+				ERROR(cb_data.status);
+				hid->state_next = STATE_INACTIVE;
+				break;
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+
+static void read_hid_in_endpoint(void *drvdata)
+{
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	usbh_packet_t packet;
+
+	packet.address = hid->usbh_device->address;
+	packet.data.in = &hid->buffer[0];
+	packet.datalen = hid->endpoint_in_maxpacketsize;
+	packet.endpoint_address = hid->endpoint_in_address;
+	packet.endpoint_size_max = hid->endpoint_in_maxpacketsize;
+	packet.endpoint_type = USBH_ENDPOINT_TYPE_INTERRUPT;
+	packet.speed = hid->usbh_device->speed;
+	packet.callback = event;
+	packet.callback_arg = hid->usbh_device;
+	packet.toggle = &hid->endpoint_in_toggle;
+
+	hid->state_next = STATE_READING_COMPLETE_AND_CHECK_REPORT;
+	usbh_read(hid->usbh_device, &packet);
+}
+
+/**
+ * @param time_curr_us - monotically rising time
+ *		unit is microseconds
+ * @see usbh_poll()
+ */
+static void poll(void *drvdata, uint32_t time_curr_us)
+{
+	(void)time_curr_us;
+
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	usbh_device_t *dev = hid->usbh_device;
+	switch (hid->state_next) {
+	case STATE_READING_REQUEST:
+		{
+			read_hid_in_endpoint(drvdata);
+		}
+		break;
+
+	case STATE_GET_REPORT_DESCRIPTOR_READ_SETUP:
+		{
+			hid->endpoint_in_toggle = 0;
+			// We support only the first report descriptor with index 0
+
+			// limit the size of the report descriptor!
+			if (hid->report0_length > USBH_HID_BUFFER) {
+				hid->report0_length = USBH_HID_BUFFER;
+			}
+
+			struct usb_setup_data setup_data;
+
+			setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_INTERFACE;
+			setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;
+			setup_data.wValue = USB_DT_REPORT << 8;
+			setup_data.wIndex = 0;
+			setup_data.wLength = hid->report0_length;
+
+			hid->state_next = STATE_GET_REPORT_DESCRIPTOR_READ_COMPLETE;
+			device_control(dev, event, &setup_data, hid->buffer);
+		}
+		break;
+
+	default:
+		// do nothing - probably transfer is in progress
+		break;
+	}
+}
+
+static void remove(void *drvdata)
+{
+	hid_device_t *hid = (hid_device_t *)drvdata;
+	hid->state_next = STATE_INACTIVE;
+	hid->endpoint_in_address = 0;
+}
+
+bool hid_set_report(uint8_t device_id, uint8_t val)
+{
+	if (device_id >= USBH_HID_MAX_DEVICES) {
+		LOG_PRINTF("invalid device id\n");
+		return false;
+	}
+
+	hid_device_t *hid = &hid_device[device_id];
+	if (hid->report_state != REPORT_STATE_READY) {
+		LOG_PRINTF("reporting is not ready\n");
+		// store and update afterwards
+		return false;
+	}
+
+	if (hid->report_data_length == 0) {
+		LOG_PRINTF("reporting is not available (report len=0)\n");
+		return false;
+	}
+
+	struct usb_setup_data setup_data;
+	setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE;
+	setup_data.bRequest = USB_HID_SET_REPORT;
+	setup_data.wValue = 0x02 << 8;
+	setup_data.wIndex = hid->interface_number;
+	setup_data.wLength = hid->report_data_length;
+
+	hid->report_data[0] = val;
+
+	hid->report_state = REPORT_STATE_PENDING;
+	device_control(hid->usbh_device, report_event, &setup_data, &hid->report_data);
+	return true;
+}
+
+bool hid_is_connected(uint8_t device_id)
+{
+	if (device_id >= USBH_HID_MAX_DEVICES) {
+		LOG_PRINTF("is connected: invalid device id\n");
+		return false;
+	}
+	return hid_device[device_id].state_next == STATE_INACTIVE;
+}
+
+
+enum HID_TYPE hid_get_type(uint8_t device_id)
+{
+	if (hid_is_connected(device_id)) {
+		return HID_TYPE_NONE;
+	}
+	return hid_device[device_id].hid_type;
+}
+
+static const usbh_dev_driver_info_t driver_info = {
+	.deviceClass = -1,
+	.deviceSubClass = -1,
+	.deviceProtocol = -1,
+	.idVendor = -1,
+	.idProduct = -1,
+	.ifaceClass = 0x03, // HID class
+	.ifaceSubClass = -1,
+	.ifaceProtocol = -1, // Do not care
+};
+
+const usbh_dev_driver_t usbh_hid_driver = {
+	.init = init,
+	.analyze_descriptor = analyze_descriptor,
+	.poll = poll,
+	.remove = remove,
+	.info = &driver_info
+};
+
+
+
diff --git a/fw/src/usbh_driver_hub.c b/fw/src/usbh_driver_hub.c
new file mode 100644
index 0000000..3f51c68
--- /dev/null
+++ b/fw/src/usbh_driver_hub.c
@@ -0,0 +1,675 @@
+/*

+ * This file is part of the libusbhost library

+ * hosted at http://github.com/libusbhost/libusbhost

+ *

+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>

+ *

+ *

+ * libusbhost is free software: you can redistribute it and/or modify

+ * it under the terms of the GNU Lesser General Public License as published by

+ * the Free Software Foundation, either version 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This library is distributed in the hope that it will be useful,

+ * but WITHOUT ANY WARRANTY; without even the implied warranty of

+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+ * GNU Lesser General Public License for more details.

+ *

+ * You should have received a copy of the GNU Lesser General Public License

+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.

+ *

+ */

+

+#include "usbh_driver_hub_private.h"

+#include "driver/usbh_device_driver.h"

+#include "usart_helpers.h"

+#include "usbh_config.h"

+

+#include <stddef.h>

+#include <stdint.h>

+

+static hub_device_t hub_device[USBH_MAX_HUBS];

+

+static bool initialized = false;

+

+void hub_driver_init(void)

+{

+	uint32_t i;

+

+	initialized = true;

+

+	for (i = 0; i < USBH_MAX_HUBS; i++) {

+		hub_device[i].device[0] = 0;

+		hub_device[i].ports_num = 0;

+		hub_device[i].current_port = CURRENT_PORT_NONE;

+	}

+}

+

+static void *init(usbh_device_t *usbh_dev)

+{

+	if (!initialized) {

+		LOG_PRINTF("\n%s/%d : driver not initialized\n", __FILE__, __LINE__);

+		return 0;

+	}

+

+	uint32_t i;

+	hub_device_t *drvdata = NULL;

+	// find free data space for hub device

+	for (i = 0; i < USBH_MAX_HUBS; i++) {

+		if (hub_device[i].device[0] == 0) {

+			break;

+		}

+	}

+	LOG_PRINTF("{%d}",i);

+	if (i == USBH_MAX_HUBS) {

+		LOG_PRINTF("Unable to initialize hub driver");

+		return 0;

+	}

+

+	drvdata = &hub_device[i];

+	drvdata->state = EVENT_STATE_NONE;

+	drvdata->ports_num = 0;

+	drvdata->device[0] = usbh_dev;

+	drvdata->busy = 0;

+	drvdata->endpoint_in_address = 0;

+	drvdata->endpoint_in_maxpacketsize = 0;

+

+	return drvdata;

+}

+

+/**

+ * @returns true if all needed data are parsed

+ */

+static bool analyze_descriptor(void *drvdata, void *descriptor)

+{

+	hub_device_t *hub = (hub_device_t *)drvdata;

+	uint8_t desc_type = ((uint8_t *)descriptor)[1];

+	switch (desc_type) {

+	case USB_DT_ENDPOINT:

+		{

+			struct usb_endpoint_descriptor *ep = (struct usb_endpoint_descriptor *)descriptor;

+			if ((ep->bmAttributes&0x03) == USB_ENDPOINT_ATTR_INTERRUPT) {

+				uint8_t epaddr = ep->bEndpointAddress;

+				if (epaddr & (1<<7)) {

+					hub->endpoint_in_address = epaddr&0x7f;

+					hub->endpoint_in_maxpacketsize = ep->wMaxPacketSize;

+				}

+			}

+			LOG_PRINTF("ENDPOINT DESCRIPTOR FOUND\n");

+		}

+		break;

+

+	case USB_DT_HUB:

+		{

+			struct usb_hub_descriptor *desc = (struct usb_hub_descriptor *)descriptor;

+			if ( desc->head.bNbrPorts <= USBH_HUB_MAX_DEVICES) {

+				hub->ports_num = desc->head.bNbrPorts;

+			} else {

+				LOG_PRINTF("INCREASE NUMBER OF ENABLED PORTS\n");

+				hub->ports_num = USBH_HUB_MAX_DEVICES;

+			}

+			LOG_PRINTF("HUB DESCRIPTOR FOUND \n");

+		}

+		break;

+

+	default:

+		LOG_PRINTF("TYPE: %02X \n",desc_type);

+		break;

+	}

+

+	if (hub->endpoint_in_address) {

+		hub->state = EVENT_STATE_INITIAL;

+		LOG_PRINTF("end enum");

+		return true;

+	}

+	return false;

+}

+

+// Enumerate

+static void event(usbh_device_t *dev, usbh_packet_callback_data_t cb_data)

+{

+	hub_device_t *hub = (hub_device_t *)dev->drvdata;

+

+	LOG_PRINTF("\nHUB->STATE = %d\n", hub->state);

+	switch (hub->state) {

+	case EVENT_STATE_POLL:

+		switch (cb_data.status) {

+		case USBH_PACKET_CALLBACK_STATUS_OK:

+			{

+				uint8_t i;

+				uint8_t *buf = hub->buffer;

+				uint32_t psc = 0;	// Limit: up to 4 bytes...

+				for (i = 0; i < cb_data.transferred_length; i++) {

+					psc += buf[i] << (i*8);

+				}

+				int8_t port = 0;

+

+				LOG_PRINTF("psc:%d\n",psc);

+				// Driver error... port not found

+				if (!psc) {

+					// Continue reading status change endpoint

+					hub->state = EVENT_STATE_POLL_REQ;

+					break;

+				}

+

+				for (i = 0; i <= hub->ports_num; i++) {

+					if (psc & (1<<i)) {

+						port = i;

+						psc = 0;

+						break;

+					}

+				}

+

+				if (hub->current_port >= 1) {

+					if (hub->current_port != port) {

+						LOG_PRINTF("X");

+						hub->state = EVENT_STATE_POLL_REQ;

+						break;

+					}

+				}

+				struct usb_setup_data setup_data;

+				// If regular port event, else hub event

+				if (port) {

+					setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+				} else {

+					setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

+				}

+

+				setup_data.bRequest = USB_REQ_GET_STATUS;

+				setup_data.wValue = 0;

+				setup_data.wIndex = port;

+				setup_data.wLength = 4;

+				hub->state = EVENT_STATE_GET_STATUS_COMPLETE;

+

+				hub->current_port = port;

+				LOG_PRINTF("\n\nPORT FOUND: %d\n", port);

+				device_control(dev, event, &setup_data, &hub->hub_and_port_status[port]);

+			}

+			break;

+

+		default:

+			ERROR(cb_data.status);

+			hub->state = EVENT_STATE_NONE;

+			break;

+

+		case USBH_PACKET_CALLBACK_STATUS_EAGAIN:

+

+			// In case of EAGAIN error, retry read on status endpoint

+			hub->state = EVENT_STATE_POLL_REQ;

+			LOG_PRINTF("HUB: Retrying...\n");

+			break;

+		}

+		break;

+

+	case EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE:// Hub descriptor found

+		{

+			switch (cb_data.status) {

+			case USBH_PACKET_CALLBACK_STATUS_OK:

+				{

+					struct usb_hub_descriptor *hub_descriptor =

+						(struct usb_hub_descriptor *)hub->buffer;

+

+					// Check size

+					if (hub_descriptor->head.bDescLength > hub->desc_len) {

+						struct usb_setup_data setup_data;

+						hub->desc_len = hub_descriptor->head.bDescLength;

+

+						setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

+						setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;

+						setup_data.wValue = 0x29<<8;

+						setup_data.wIndex = 0;

+						setup_data.wLength = hub->desc_len;

+

+						hub->state = EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE;

+						device_control(dev, event, &setup_data, hub->buffer);

+						break;

+					} else if (hub_descriptor->head.bDescLength == hub->desc_len) {

+						hub->ports_num = hub_descriptor->head.bNbrPorts;

+

+						hub->state = EVENT_STATE_ENABLE_PORTS;

+						hub->index = 0;

+						cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

+						event(dev, cb_data);

+					} else {

+						//try again

+					}

+				}

+				break;

+

+			case USBH_PACKET_CALLBACK_STATUS_ERRSIZ:

+				{

+					LOG_PRINTF("->\t\t\t\t\t ERRSIZ: deschub\n");

+					struct usb_hub_descriptor*hub_descriptor =

+						(struct usb_hub_descriptor *)hub->buffer;

+

+					if (cb_data.transferred_length >= sizeof(struct usb_hub_descriptor_head)) {

+						if (cb_data.transferred_length == hub_descriptor->head.bDescLength) {

+							// Process HUB descriptor

+							if ( hub_descriptor->head.bNbrPorts <= USBH_HUB_MAX_DEVICES) {

+								hub->ports_num = hub_descriptor->head.bNbrPorts;

+							} else {

+								LOG_PRINTF("INCREASE NUMBER OF ENABLED PORTS\n");

+								hub->ports_num = USBH_HUB_MAX_DEVICES;

+							}

+							hub->state = EVENT_STATE_ENABLE_PORTS;

+							hub->index = 0;

+

+							cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

+							event(dev, cb_data);

+						}

+					}

+				}

+				break;

+

+			default:

+				ERROR(cb_data.status);

+				break;

+			}

+		}

+		break;

+

+	case EVENT_STATE_ENABLE_PORTS:// enable ports

+		{

+			switch (cb_data.status) {

+			case USBH_PACKET_CALLBACK_STATUS_OK:

+				if (hub->index < hub->ports_num) {

+					hub->index++;

+					struct usb_setup_data setup_data;

+

+					LOG_PRINTF("[!%d!]",hub->index);

+					setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+					setup_data.bRequest = HUB_REQ_SET_FEATURE;

+					setup_data.wValue = HUB_FEATURE_PORT_POWER;

+					setup_data.wIndex = hub->index;

+					setup_data.wLength = 0;

+

+					device_control(dev, event, &setup_data, 0);

+				} else {

+					// TODO:

+					// Delay Based on hub descriptor field bPwr2PwrGood

+					// delay_ms_busy_loop(200);

+

+					LOG_PRINTF("\nHUB CONFIGURED & PORTS POWERED\n");

+

+					// get device status

+					struct usb_setup_data setup_data;

+

+					setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

+					setup_data.bRequest = USB_REQ_GET_STATUS;

+					setup_data.wValue = 0;

+					setup_data.wIndex = 0;

+					setup_data.wLength = 4;

+

+					hub->state = EVENT_STATE_GET_PORT_STATUS;

+					hub->index = 0;

+					device_control(dev, event, &setup_data, hub->buffer);

+				}

+				break;

+

+			default:

+				ERROR(cb_data.status);

+				break;

+			}

+		}

+		break;

+

+	case EVENT_STATE_GET_PORT_STATUS:

+		{

+			switch (cb_data.status) {

+			case USBH_PACKET_CALLBACK_STATUS_OK:

+				{

+					if (hub->index < hub->ports_num) {

+						//TODO: process data contained in hub->buffer

+

+						struct usb_setup_data setup_data;

+

+						setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+						setup_data.bRequest = USB_REQ_GET_STATUS;

+						setup_data.wValue = 0;

+						setup_data.wIndex = ++hub->index;

+						setup_data.wLength = 4;

+

+						hub->state = EVENT_STATE_GET_PORT_STATUS;

+						device_control(dev, event, &setup_data, hub->buffer);

+					} else {

+						hub->busy = 0;

+						hub->state = EVENT_STATE_POLL_REQ;

+					}

+

+				}

+				break;

+

+			default:

+				ERROR(cb_data.status);

+				break;

+			}

+		}

+		break;

+

+	case EVENT_STATE_GET_STATUS_COMPLETE:

+		{

+			switch (cb_data.status) {

+			case USBH_PACKET_CALLBACK_STATUS_OK:

+				{

+					int8_t port = hub->current_port;

+					LOG_PRINTF("|%d",port);

+

+

+					// Get Port status, else Get Hub status

+					if (port) {

+						uint16_t stc = hub->hub_and_port_status[port].stc;

+

+						// Connection status changed

+						if (stc & (1<<HUB_FEATURE_PORT_CONNECTION)) {

+

+							// Check, whether device is in connected state

+							if (!hub->device[port]) {

+								if (!usbh_enum_available() || hub->busy) {

+									LOG_PRINTF("\n\t\t\tCannot enumerate %d %d\n", !usbh_enum_available(), hub->busy);

+									hub->state = EVENT_STATE_POLL_REQ;

+									break;

+								}

+							}

+

+							// clear feature C_PORT_CONNECTION

+							struct usb_setup_data setup_data;

+

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+							setup_data.bRequest = HUB_REQ_CLEAR_FEATURE;

+							setup_data.wValue = HUB_FEATURE_C_PORT_CONNECTION;

+							setup_data.wIndex = port;

+							setup_data.wLength = 0;

+

+							hub->state = EVENT_STATE_PORT_RESET_REQ;

+							device_control(dev, event, &setup_data, 0);

+

+						} else if(stc & (1<<HUB_FEATURE_PORT_RESET)) {

+							// clear feature C_PORT_RESET

+							// Reset processing is complete, enumerate device

+							struct usb_setup_data setup_data;

+

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+							setup_data.bRequest = HUB_REQ_CLEAR_FEATURE;

+							setup_data.wValue = HUB_FEATURE_C_PORT_RESET;

+							setup_data.wIndex = port;

+							setup_data.wLength = 0;

+

+							hub->state = EVENT_STATE_PORT_RESET_COMPLETE;

+

+							LOG_PRINTF("RESET");

+							device_control(dev, event, &setup_data, 0);

+						} else {

+							LOG_PRINTF("another STC %d\n", stc);

+						}

+					} else {

+						hub->state = EVENT_STATE_POLL_REQ;

+						LOG_PRINTF("HUB status change\n");

+					}

+				}

+				break;

+

+			default:

+				ERROR(cb_data.status);

+				// continue

+				hub->state = EVENT_STATE_POLL_REQ;

+				break;

+			}

+		}

+		break;

+	case EVENT_STATE_PORT_RESET_REQ:

+		{

+			switch (cb_data.status) {

+			case USBH_PACKET_CALLBACK_STATUS_OK:

+				{

+					int8_t port = hub->current_port;

+					uint16_t stc = hub->hub_and_port_status[port].stc;

+					if (!hub->device[port]) {

+						if ((stc) & (1<<HUB_FEATURE_PORT_CONNECTION)) {

+							struct usb_setup_data setup_data;

+

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+							setup_data.bRequest = HUB_REQ_SET_FEATURE;

+							setup_data.wValue = HUB_FEATURE_PORT_RESET;

+							setup_data.wIndex = port;

+							setup_data.wLength = 0;

+

+							hub->state = EVENT_STATE_GET_PORT_STATUS;

+

+							LOG_PRINTF("CONN");

+

+							hub->busy = 1;

+							device_control(dev, event, &setup_data, 0);

+						}

+					} else {

+						LOG_PRINTF("\t\t\t\tDISCONNECT EVENT\n");

+						device_remove(hub->device[port]);

+

+						hub->device[port] = 0;

+						hub->current_port = CURRENT_PORT_NONE;

+						hub->state = EVENT_STATE_POLL_REQ;

+						hub->busy = 0;

+					}

+				}

+				break;

+

+			default:

+				ERROR(cb_data.status);

+				// continue

+				hub->state = EVENT_STATE_POLL_REQ;

+				break;

+			}

+		}

+		break;

+	case EVENT_STATE_PORT_RESET_COMPLETE:	// RESET COMPLETE, start enumeration

+		{

+			switch (cb_data.status) {

+			case USBH_PACKET_CALLBACK_STATUS_OK:

+				{

+					LOG_PRINTF("\nPOLL\n");

+					int8_t port = hub->current_port;

+					uint16_t sts = hub->hub_and_port_status[port].sts;

+

+

+					if (sts & (1<<HUB_FEATURE_PORT_ENABLE)) {

+						hub->device[port] = usbh_get_free_device(dev);

+

+						if (!hub->device[port]) {

+							LOG_PRINTF("\nFATAL ERROR\n");

+							return;// DEAD END

+						}

+						if ((sts & (1<<(HUB_FEATURE_PORT_LOWSPEED))) &&

+							!(sts & (1<<(HUB_FEATURE_PORT_HIGHSPEED)))) {

+#define DISABLE_LOW_SPEED

+#ifdef DISABLE_LOW_SPEED

+							LOG_PRINTF("Low speed device");

+

+							// Disable Low speed device immediately

+							struct usb_setup_data setup_data;

+

+							setup_data.bmRequestType = USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE | USB_REQ_TYPE_ENDPOINT;

+							setup_data.bRequest = HUB_REQ_CLEAR_FEATURE;

+							setup_data.wValue = HUB_FEATURE_PORT_ENABLE;

+							setup_data.wIndex = port;

+							setup_data.wLength = 0;

+

+							// After write process another devices, poll for events

+							//Expecting all ports are powered (constant/non-changeable after init)

+							hub->state = EVENT_STATE_GET_PORT_STATUS;

+

+							hub->current_port = CURRENT_PORT_NONE;

+							device_control(dev, event, &setup_data, 0);

+#else

+							hub->device[port]->speed = USBH_SPEED_LOW;

+							LOG_PRINTF("Low speed device");

+							hub->timestamp_us = hub->time_curr_us;

+							hub->state = EVENT_STATE_SLEEP_500_MS; // schedule wait for 500ms

+#endif

+						} else if (!(sts & (1<<(HUB_FEATURE_PORT_LOWSPEED))) &&

+							!(sts & (1<<(HUB_FEATURE_PORT_HIGHSPEED)))) {

+							hub->device[port]->speed = USBH_SPEED_FULL;

+							LOG_PRINTF("Full speed device");

+							hub->timestamp_us = hub->time_curr_us;

+							hub->state = EVENT_STATE_SLEEP_500_MS; // schedule wait for 500ms

+						}

+

+

+					} else {

+						LOG_PRINTF("%s:%d Do not know what to do, when device is disabled after reset\n", __FILE__, __LINE__);

+						hub->state = EVENT_STATE_POLL_REQ;

+						return;

+					}

+				}

+				break;

+

+			default:

+				ERROR(cb_data.status);

+				// continue

+				hub->state = EVENT_STATE_POLL_REQ;

+				break;

+			}

+		}

+		break;

+	default:

+		LOG_PRINTF("UNHANDLED EVENT %d\n",hub->state);

+		break;

+	}

+}

+

+static void read_ep1(void *drvdata)

+{

+	hub_device_t *hub = (hub_device_t *)drvdata;

+	usbh_packet_t packet;

+

+	packet.address = hub->device[0]->address;

+	packet.data.in = hub->buffer;

+	packet.datalen = hub->endpoint_in_maxpacketsize;

+	packet.endpoint_address = hub->endpoint_in_address;

+	packet.endpoint_size_max = hub->endpoint_in_maxpacketsize;

+	packet.endpoint_type = USBH_ENDPOINT_TYPE_INTERRUPT;

+	packet.speed = hub->device[0]->speed;

+	packet.callback = event;

+	packet.callback_arg = hub->device[0];

+	packet.toggle = &hub->endpoint_in_toggle;

+

+	hub->state = EVENT_STATE_POLL;

+	usbh_read(hub->device[0], &packet);

+	LOG_PRINTF("@hub %d/EP1 |  \n", hub->device[0]->address);

+

+}

+

+/**

+ * @param time_curr_us - monotically rising time

+ *		unit is microseconds

+ * @see usbh_poll()

+ */

+static void poll(void *drvdata, uint32_t time_curr_us)

+{

+	hub_device_t *hub = (hub_device_t *)drvdata;

+	usbh_device_t *dev = hub->device[0];

+

+	hub->time_curr_us = time_curr_us;

+

+	switch (hub->state) {

+	case EVENT_STATE_POLL_REQ:

+		{

+			if (usbh_enum_available()) {

+				read_ep1(hub);

+			} else {

+				LOG_PRINTF("enum not available\n");

+			}

+		}

+		break;

+

+	case EVENT_STATE_INITIAL:

+		{

+			if (hub->ports_num) {

+				hub->index = 0;

+				hub->state = EVENT_STATE_ENABLE_PORTS;

+				LOG_PRINTF("No need to get HUB DESC\n");

+				event(dev, (usbh_packet_callback_data_t){0, 0});

+			} else {

+				hub->endpoint_in_toggle = 0;

+

+				struct usb_setup_data setup_data;

+				hub->desc_len = hub->device[0]->packet_size_max0;

+

+				setup_data.bmRequestType = USB_REQ_TYPE_IN | USB_REQ_TYPE_CLASS | USB_REQ_TYPE_DEVICE;

+				setup_data.bRequest = USB_REQ_GET_DESCRIPTOR;

+				setup_data.wValue = 0x29 << 8;

+				setup_data.wIndex = 0;

+				setup_data.wLength = hub->desc_len;

+

+				hub->state = EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE;

+				device_control(dev, event, &setup_data, hub->buffer);

+				LOG_PRINTF("DO Need to get HUB DESC\n");

+			}

+		}

+		break;

+	case EVENT_STATE_SLEEP_500_MS:

+		if (hub->time_curr_us - hub->timestamp_us > 500000) {

+			int8_t port = hub->current_port;

+			LOG_PRINTF("PORT: %d\n", port);

+			LOG_PRINTF("NEW device at address: %d\n", hub->device[port]->address);

+			hub->device[port]->lld = hub->device[0]->lld;

+

+			device_enumeration_start(hub->device[port]);

+			hub->current_port = CURRENT_PORT_NONE;

+

+			// Maybe error, when assigning address is taking too long

+			//

+			// Detail:

+			// USB hub cannot enable another port while the device

+			// the current one is also in address state (has address==0)

+			// Only one device on bus can have address==0

+			hub->busy = 0;

+

+			hub->state = EVENT_STATE_POLL_REQ;

+		}

+		break;

+	default:

+		break;

+	}

+

+	if (usbh_enum_available()) {

+		uint32_t i;

+		for (i = 1; i < USBH_HUB_MAX_DEVICES + 1; i++) {

+			if (hub->device[i]) {

+				if (hub->device[i]->drv && hub->device[i]->drvdata) {

+					hub->device[i]->drv->poll(hub->device[i]->drvdata, time_curr_us);

+				}

+			}

+		}

+	}

+}

+static void remove(void *drvdata)

+{

+	hub_device_t *hub = (hub_device_t *)drvdata;

+	uint8_t i;

+

+	hub->state = EVENT_STATE_NONE;

+	hub->endpoint_in_address = 0;

+	hub->busy = 0;

+	for (i = 0; i < USBH_HUB_MAX_DEVICES + 1; i++) {

+		hub->device[i] = 0;

+

+	}

+}

+

+static const usbh_dev_driver_info_t driver_info = {

+	.deviceClass = 0x09,

+	.deviceSubClass = -1,

+	.deviceProtocol = -1,

+	.idVendor = -1,

+	.idProduct = -1,

+	.ifaceClass = 0x09,

+	.ifaceSubClass = -1,

+	.ifaceProtocol = -1

+};

+

+const usbh_dev_driver_t usbh_hub_driver = {

+	.init = init,

+	.analyze_descriptor = analyze_descriptor,

+	.poll = poll,

+	.remove = remove,

+	.info = &driver_info

+};

diff --git a/fw/src/usbh_driver_hub_private.h b/fw/src/usbh_driver_hub_private.h
new file mode 100644
index 0000000..8c9bc44
--- /dev/null
+++ b/fw/src/usbh_driver_hub_private.h
@@ -0,0 +1,123 @@
+/*
+ * This file is part of the libusbhost library
+ * hosted at http://github.com/libusbhost/libusbhost
+ *
+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>
+ *
+ *
+ * libusbhost is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef USBH_DRIVER_HUB_PRIVATE_
+#define USBH_DRIVER_HUB_PRIVATE_
+
+#include "usbh_config.h"
+#include "driver/usbh_device_driver.h"
+#include "usbh_driver_hub.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <libopencm3/usb/usbstd.h>
+
+
+// # HUB DEFINITIONS
+#define HUB_FEATURE_PORT_CONNECTION 0
+#define HUB_FEATURE_PORT_ENABLE 1
+#define HUB_FEATURE_PORT_SUSPEND 2
+#define HUB_FEATURE_PORT_OVERCURRENT 3
+#define HUB_FEATURE_PORT_RESET 4
+#define HUB_FEATURE_PORT_POWER 8
+#define HUB_FEATURE_PORT_LOWSPEED 9
+#define HUB_FEATURE_PORT_HIGHSPEED 10
+
+#define HUB_FEATURE_C_PORT_CONNECTION 16
+#define HUB_FEATURE_C_PORT_ENABLE 17
+#define HUB_FEATURE_C_PORT_SUSPEND 18
+#define HUB_FEATURE_C_PORT_OVERCURRENT 19
+#define HUB_FEATURE_C_PORT_RESET 20
+
+#define HUB_REQ_GET_STATUS 		0
+#define HUB_REQ_CLEAR_FEATURE 	1
+#define HUB_REQ_SET_FEATURE 	3
+#define HUB_REQ_GET_DESCRIPTOR 	6
+
+#define USB_DT_HUB 		(41)
+#define USB_DT_HUB_SIZE	(9)
+// Hub buffer: must be larger than hub descriptor
+#define USBH_HUB_BUFFER_SIZE	(USB_DT_HUB_SIZE)
+
+
+#define CURRENT_PORT_NONE -1
+
+enum EVENT_STATE {
+	EVENT_STATE_NONE,
+	EVENT_STATE_INITIAL,
+	EVENT_STATE_POLL_REQ,
+	EVENT_STATE_POLL,
+	EVENT_STATE_READ_HUB_DESCRIPTOR_COMPLETE,
+	EVENT_STATE_ENABLE_PORTS,
+	EVENT_STATE_GET_PORT_STATUS,
+	EVENT_STATE_PORT_RESET_REQ,
+	EVENT_STATE_PORT_RESET_COMPLETE,
+	EVENT_STATE_SLEEP_500_MS,
+	EVENT_STATE_GET_STATUS_COMPLETE,
+};
+
+struct _hub_device {
+	usbh_device_t *device[USBH_HUB_MAX_DEVICES + 1];
+	uint8_t buffer[USBH_HUB_BUFFER_SIZE];
+	uint16_t endpoint_in_maxpacketsize;
+	uint8_t endpoint_in_address;
+	uint8_t endpoint_in_toggle;
+	enum EVENT_STATE state;
+
+	uint8_t desc_len;
+	uint16_t ports_num;
+	int8_t index;
+	int8_t current_port;
+
+	struct {
+		uint16_t sts;
+		uint16_t stc;
+	} hub_and_port_status[USBH_HUB_MAX_DEVICES + 1];
+
+	bool busy;
+
+	uint32_t time_curr_us;
+	uint32_t timestamp_us;
+};
+
+typedef struct _hub_device hub_device_t;
+
+struct usb_hub_descriptor_head {
+	uint8_t bDescLength;
+	uint8_t bDescriptorType;
+	uint8_t bNbrPorts;
+	uint16_t wHubCharacteristics;
+	uint8_t bPwrOn2PwrGood;
+	uint8_t bHubContrCurrent;
+} __attribute__((packed));
+struct usb_hub_descriptor_body {
+	uint8_t bDeviceRemovable;
+	uint8_t PortPwrCtrlMask;
+} __attribute__((packed));
+
+// for hubs with up to 7 ports on hub
+struct usb_hub_descriptor {
+	struct usb_hub_descriptor_head head;
+	struct usb_hub_descriptor_body body[1];
+} __attribute__((packed));
+
+#endif
diff --git a/fw/src/usbh_lld_stm32f4.c b/fw/src/usbh_lld_stm32f4.c
new file mode 100644
index 0000000..3fcd51a
--- /dev/null
+++ b/fw/src/usbh_lld_stm32f4.c
@@ -0,0 +1,1041 @@
+/*

+ * This file is part of the libusbhost library

+ * hosted at http://github.com/libusbhost/libusbhost

+ *

+ * Copyright (C) 2015 Amir Hammad <amir.hammad@hotmail.com>

+ *

+ *

+ * libusbhost is free software: you can redistribute it and/or modify

+ * it under the terms of the GNU Lesser General Public License as published by

+ * the Free Software Foundation, either version 3 of the License, or

+ * (at your option) any later version.

+ *

+ * This library is distributed in the hope that it will be useful,

+ * but WITHOUT ANY WARRANTY; without even the implied warranty of

+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

+ * GNU Lesser General Public License for more details.

+ *

+ * You should have received a copy of the GNU Lesser General Public License

+ * along with this library.  If not, see <http://www.gnu.org/licenses/>.

+ *

+ */

+

+#include "driver/usbh_device_driver.h"

+#include "usbh_lld_stm32f4.h"

+#include "usart_helpers.h"

+

+#include <string.h>

+#include <stdint.h>

+#include <libopencm3/stm32/otg_hs.h>

+#include <libopencm3/stm32/otg_fs.h>

+

+

+

+/* Receive FIFO size in 32-bit words. */

+#define RX_FIFO_SIZE    (64)

+/* Transmit NON-periodic FIFO size in 32-bit words. */

+#define TX_NP_FIFO_SIZE (64)

+/* Transmit periodic FIFO size in 32-bit words. */

+#define TX_P_FIFO_SIZE  (64)

+

+enum CHANNEL_STATE {

+	CHANNEL_STATE_FREE = 0,

+	CHANNEL_STATE_WORK = 1

+};

+

+struct _channel {

+	enum CHANNEL_STATE state;

+	usbh_packet_t packet;

+	uint32_t data_index; //used in receive function

+};

+typedef struct _channel channel_t;

+

+enum DEVICE_STATE {

+	DEVICE_STATE_INIT = 0,

+	DEVICE_STATE_RUN = 1,

+	DEVICE_STATE_RESET = 2

+};

+

+enum DEVICE_POLL_STATE {

+	DEVICE_POLL_STATE_DISCONN = 0,

+	DEVICE_POLL_STATE_DEVCONN = 1,

+	DEVICE_POLL_STATE_DEVRST = 2,

+	DEVICE_POLL_STATE_RUN = 3

+};

+

+struct _usbh_lld_stm32f4_driver_data {

+	usbh_generic_data_t generic;

+	const uint32_t base;

+	channel_t *channels;

+	const uint8_t num_channels;

+

+	uint32_t poll_sequence;

+	enum DEVICE_POLL_STATE dpstate;

+	enum DEVICE_STATE state;

+	uint32_t state_prev;//for reset only

+	uint32_t time_curr_us;

+	uint32_t timestamp_us;

+};

+typedef struct _usbh_lld_stm32f4_driver_data usbh_lld_stm32f4_driver_data_t;

+

+

+

+/*

+ * Define correct REBASE. If only one driver is enabled use directly OTG base

+ *

+ */

+#if 	defined(USE_STM32F4_USBH_DRIVER_FS) || \

+		defined(USE_STM32F4_USBH_DRIVER_HS)

+

+#if 	defined(USE_STM32F4_USBH_DRIVER_FS) && \

+		defined(USE_STM32F4_USBH_DRIVER_HS)

+#define REBASE(reg)				MMIO32(dev->base + reg)

+#define REBASE_CH(reg, x)	MMIO32(dev->base + reg(x))

+#elif defined(USE_STM32F4_USBH_DRIVER_FS)

+#define REBASE(reg)				MMIO32(USB_OTG_FS_BASE + reg)

+#define REBASE_CH(reg, x)	MMIO32(USB_OTG_FS_BASE + reg(x))

+#elif defined(USE_STM32F4_USBH_DRIVER_HS)

+#define REBASE(reg)				MMIO32(USB_OTG_HS_BASE + reg)

+#define REBASE_CH(reg, x)	MMIO32(USB_OTG_HS_BASE + reg(x))

+#endif

+

+static int8_t get_free_channel(void *drvdata);

+static void channels_init(void *drvdata);

+static void rxflvl_handle(void *drvdata);

+static void free_channel(void *drvdata, uint8_t channel);

+

+

+

+

+

+static inline void reset_start(usbh_lld_stm32f4_driver_data_t *dev)

+{

+

+	// apply reset condition on port

+	REBASE(OTG_HPRT) |= OTG_HPRT_PRST;

+

+	// push current state to stack

+	dev->state_prev = dev->state;

+

+	// move to new state

+	dev->state = DEVICE_STATE_RESET;

+

+	// schedule disable reset condition after ~10ms

+	dev->timestamp_us = dev->time_curr_us;

+}

+

+/**

+ * Should be nonblocking

+ *

+ */

+static void init(void *drvdata)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	dev->state = DEVICE_STATE_INIT;

+	dev->poll_sequence = 0;

+	dev->timestamp_us = dev->time_curr_us;

+

+	//Disable interrupts first

+	REBASE(OTG_GAHBCFG) &= ~OTG_GAHBCFG_GINT;

+

+	// Select full speed phy

+	REBASE(OTG_GUSBCFG) |= OTG_GUSBCFG_PHYSEL;

+}

+

+static uint32_t usbh_to_stm32_endpoint_type(enum USBH_ENDPOINT_TYPE usbh_eptyp)

+{

+	switch (usbh_eptyp) {

+	case USBH_ENDPOINT_TYPE_CONTROL: return OTG_HCCHAR_EPTYP_CONTROL;

+	case USBH_ENDPOINT_TYPE_BULK: return OTG_HCCHAR_EPTYP_BULK;

+

+	// Use bulk transfer also for interrupt, since no difference is on protocol layer

+	// Except different behaviour of the core

+	case USBH_ENDPOINT_TYPE_INTERRUPT: return OTG_HCCHAR_EPTYP_BULK;

+	case USBH_ENDPOINT_TYPE_ISOCHRONOUS: return OTG_HCCHAR_EPTYP_ISOCHRONOUS;

+	default:

+		LOG_PRINTF("\n\n\n\nWRONG EP TYPE\n\n\n\n\n");

+		return OTG_HCCHAR_EPTYP_CONTROL;

+	}

+}

+

+static void stm32f4_usbh_port_channel_setup(

+	void *drvdata, uint32_t channel, uint32_t epdir)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+	uint32_t max_packet_size = channels[channel].packet.endpoint_size_max;

+	uint32_t address = channels[channel].packet.address;

+	uint32_t epnum = channels[channel].packet.endpoint_address;

+	uint32_t eptyp = usbh_to_stm32_endpoint_type(channels[channel].packet.endpoint_type);

+

+	uint32_t speed = 0;

+	if (channels[channel].packet.speed == USBH_SPEED_LOW) {

+		speed = OTG_HCCHAR_LSDEV;

+	}

+

+	REBASE_CH(OTG_HCCHAR, channel) = OTG_HCCHAR_CHENA |

+				(OTG_HCCHAR_DAD_MASK & (address << 22)) |

+				OTG_HCCHAR_MCNT_1 |

+				(OTG_HCCHAR_EPTYP_MASK & (eptyp)) |

+				(speed) |

+				(OTG_HCCHAR_EPDIR_MASK & epdir) |

+				(OTG_HCCHAR_EPNUM_MASK & (epnum << 11)) |

+				(OTG_HCCHAR_MPSIZ_MASK & max_packet_size);

+

+}

+

+

+/**

+ * TODO: Check for maximum datalength

+ */

+static void read(void *drvdata, usbh_packet_t *packet)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+

+	int8_t channel = get_free_channel(dev);

+	if (channel == -1) {

+		// BIG PROBLEM

+		LOG_PRINTF("FATAL ERROR IN, NO CHANNEL LEFT \n");

+		usbh_packet_callback_data_t cb_data;

+		cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+		cb_data.transferred_length = 0;

+		packet->callback(packet->callback_arg, cb_data);

+		return;

+	}

+

+	channels[channel].data_index = 0;

+	channels[channel].packet = *packet;

+

+	uint32_t dpid;

+	if (packet->toggle[0]) {

+		dpid = OTG_HCTSIZ_DPID_DATA1;

+	} else {

+		dpid = OTG_HCTSIZ_DPID_DATA0;

+	}

+

+	uint32_t num_packets;

+	if (packet->datalen) {

+		num_packets = ((packet->datalen - 1) / packet->endpoint_size_max) + 1;

+	} else {

+		num_packets = 0;

+	}

+

+	REBASE_CH(OTG_HCTSIZ, channel) = dpid | (num_packets << 19) | packet->datalen;

+

+	stm32f4_usbh_port_channel_setup(dev, channel, OTG_HCCHAR_EPDIR_IN);

+}

+

+/**

+ *

+ * 	Bug: datalen > max_packet_size ...

+ */

+static void write(void *drvdata, const usbh_packet_t *packet)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+

+	int8_t channel = get_free_channel(dev);

+

+	if (channel == -1) {

+		// BIG PROBLEM

+		LOG_PRINTF("FATAL ERROR OUT, NO CHANNEL LEFT \n");

+		usbh_packet_callback_data_t cb_data;

+		cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+		cb_data.transferred_length = 0;

+		packet->callback(packet->callback_arg, cb_data);

+		return;

+	}

+

+	channels[channel].data_index = 0;

+	channels[channel].packet = *packet;

+

+	uint32_t dpid;

+	if (packet->endpoint_type == USBH_ENDPOINT_TYPE_CONTROL) {

+		if (packet->control_type == USBH_CONTROL_TYPE_DATA) {

+			dpid = packet->toggle[0] ? OTG_HCTSIZ_DPID_DATA1 : OTG_HCTSIZ_DPID_DATA0;

+		} else {

+			dpid = OTG_HCTSIZ_DPID_MDATA;

+			packet->toggle[0] = 0;

+		}

+	} else if(packet->endpoint_type == USBH_ENDPOINT_TYPE_INTERRUPT) {

+		dpid = packet->toggle[0] ? OTG_HCTSIZ_DPID_DATA1 : OTG_HCTSIZ_DPID_DATA0;

+	} else if (packet->endpoint_type == USBH_ENDPOINT_TYPE_BULK) {

+		dpid = packet->toggle[0] ? OTG_HCTSIZ_DPID_DATA1 : OTG_HCTSIZ_DPID_DATA0;

+	} else {

+		dpid = OTG_HCTSIZ_DPID_DATA0; // ! TODO: BUG

+		ERROR("");

+	}

+

+	uint32_t num_packets;

+	if (packet->datalen) {

+		num_packets = ((packet->datalen - 1) / packet->endpoint_size_max) + 1;

+	} else {

+		num_packets = 1;

+	}

+	REBASE_CH(OTG_HCTSIZ, channel) = dpid | (num_packets << 19) | packet->datalen;

+

+	stm32f4_usbh_port_channel_setup(dev, channel, OTG_HCCHAR_EPDIR_OUT);

+

+	if (packet->endpoint_type == USBH_ENDPOINT_TYPE_CONTROL ||

+		packet->endpoint_type == USBH_ENDPOINT_TYPE_BULK) {

+

+		volatile uint32_t *fifo = &REBASE_CH(OTG_FIFO, channel) + RX_FIFO_SIZE;

+		const uint32_t * buf32 = packet->data.out;

+		int i;

+		LOG_PRINTF("\nSending[%d]: ", packet->datalen);

+		for(i = packet->datalen; i >= 4; i-=4) {

+			const uint8_t *buf8 = (const uint8_t *)buf32;

+			LOG_PRINTF("%02X %02X %02X %02X, ", buf8[0], buf8[1], buf8[2], buf8[3]);

+			*fifo++ = *buf32++;

+

+		}

+

+		if (i > 0) {

+			*fifo = *buf32&((1 << (8*i)) - 1);

+			uint8_t *buf8 = (uint8_t *)buf32;

+			while (i--) {

+				LOG_PRINTF("%02X ", *buf8++);

+			}

+		}

+		LOG_PRINTF("\n");

+

+	} else {

+		volatile uint32_t *fifo = &REBASE_CH(OTG_FIFO, channel) +

+			RX_FIFO_SIZE + TX_NP_FIFO_SIZE;

+		const uint32_t * buf32 = packet->data.out;

+		int i;

+		for(i = packet->datalen; i > 0; i-=4) {

+			*fifo++ = *buf32++;

+		}

+	}

+	LOG_PRINTF("->WRITE %08X\n", REBASE_CH(OTG_HCCHAR, channel));

+}

+

+static void rxflvl_handle(void *drvdata)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+	uint32_t rxstsp = REBASE(OTG_GRXSTSP);

+	uint8_t channel = rxstsp&0xf;

+	uint32_t len = (rxstsp>>4) & 0x1ff;

+	if ((rxstsp&OTG_GRXSTSP_PKTSTS_MASK) == OTG_GRXSTSP_PKTSTS_IN) {

+		uint8_t *data = channels[channel].packet.data.in;

+		uint32_t *buf32 = (uint32_t *)&data[channels[channel].data_index];

+

+		int32_t i;

+		uint32_t extra;

+		if (!len) {

+			return;

+		}

+		// Receive data from fifo

+		volatile uint32_t *fifo = &REBASE_CH(OTG_FIFO, channel);

+		for (i = len; i > 4; i -= 4) {

+			*buf32++ = *fifo++;

+		}

+		extra = *fifo;

+

+		memcpy(buf32, &extra, i);

+		channels[channel].data_index += len;

+

+		// If transfer not complete, Enable channel to continue

+		if ( channels[channel].data_index < channels[channel].packet.datalen) {

+			if (len == channels[channel].packet.endpoint_size_max) {

+				REBASE_CH(OTG_HCCHAR, channel) |= OTG_HCCHAR_CHENA;

+				//LOG_PRINTF("CHENA[%d/%d] ", channels[channel].data_index, channels[channel].packet.datalen);

+			}

+

+		}

+

+	} else if ((rxstsp&OTG_GRXSTSP_PKTSTS_MASK) == OTG_GRXSTSP_PKTSTS_IN_COMP) {

+/*

+#ifdef USART_DEBUG

+		uint32_t i;

+		LOG_PRINTF("\nDATA: ");

+		for (i = 0; i < channels[channel].data_index; i++) {

+			uint8_t *data = channels[channel].packet.data.in;

+			LOG_PRINTF("%02X ", data[i]);

+		}

+#endif

+*/

+	} else if ((rxstsp&OTG_GRXSTSP_PKTSTS_MASK) == OTG_GRXSTSP_PKTSTS_CHH) {

+

+	} else {

+

+	}

+}

+

+

+static enum USBH_POLL_STATUS poll_run(usbh_lld_stm32f4_driver_data_t *dev)

+{

+	channel_t *channels = dev->channels;

+

+	if (dev->dpstate == DEVICE_POLL_STATE_DISCONN) {

+		REBASE(OTG_GINTSTS) = REBASE(OTG_GINTSTS);

+		// Check for connection of device

+		if ((REBASE(OTG_HPRT) & OTG_HPRT_PCDET)  &&

+			(REBASE(OTG_HPRT) & OTG_HPRT_PCSTS) ) {

+

+			dev->dpstate = DEVICE_POLL_STATE_DEVCONN;

+			dev->timestamp_us = dev->time_curr_us;

+			return USBH_POLL_STATUS_NONE;

+		}

+	}

+

+	if (dev->dpstate == DEVICE_POLL_STATE_DEVCONN) {

+		// May be other condition, e.g. Debounce done,

+		// using 0.5s wait by default

+		if (dev->time_curr_us - dev->timestamp_us < 500000) {

+			return USBH_POLL_STATUS_NONE;

+		}

+

+		if ((REBASE(OTG_HPRT) & OTG_HPRT_PCDET)  &&

+			(REBASE(OTG_HPRT) & OTG_HPRT_PCSTS) ) {

+			if ((REBASE(OTG_HPRT) & OTG_HPRT_PSPD_MASK) == OTG_HPRT_PSPD_FULL) {

+				REBASE(OTG_HFIR) = (REBASE(OTG_HFIR) & ~OTG_HFIR_FRIVL_MASK) | 48000;

+				if ((REBASE(OTG_HCFG) & OTG_HCFG_FSLSPCS_MASK) != OTG_HCFG_FSLSPCS_48MHz) {

+					REBASE(OTG_HCFG) = (REBASE(OTG_HCFG) & ~OTG_HCFG_FSLSPCS_MASK) | OTG_HCFG_FSLSPCS_48MHz;

+					LOG_PRINTF("\n Reset Full-Speed \n");

+				}

+				channels_init(dev);

+				dev->dpstate = DEVICE_POLL_STATE_DEVRST;

+				reset_start(dev);

+

+			} else if ((REBASE(OTG_HPRT) & OTG_HPRT_PSPD_MASK) == OTG_HPRT_PSPD_LOW) {

+				REBASE(OTG_HFIR) = (REBASE(OTG_HFIR) & ~OTG_HFIR_FRIVL_MASK) | 6000;

+				if ((REBASE(OTG_HCFG) & OTG_HCFG_FSLSPCS_MASK) != OTG_HCFG_FSLSPCS_6MHz) {

+					REBASE(OTG_HCFG) = (REBASE(OTG_HCFG) & ~OTG_HCFG_FSLSPCS_MASK) | OTG_HCFG_FSLSPCS_6MHz;

+					LOG_PRINTF("\n Reset Low-Speed \n");

+				}

+

+				channels_init(dev);

+				dev->dpstate = DEVICE_POLL_STATE_DEVRST;

+				reset_start(dev);

+			}

+			return USBH_POLL_STATUS_NONE;

+		}

+	}

+

+	if (dev->dpstate == DEVICE_POLL_STATE_DEVRST) {

+		if (dev->time_curr_us - dev->timestamp_us < 210000) {

+			return USBH_POLL_STATUS_NONE;

+		} else {

+			dev->dpstate = DEVICE_POLL_STATE_RUN;

+		}

+	}

+

+	// ELSE RUN

+

+	if (REBASE(OTG_GINTSTS) & OTG_GINTSTS_SOF) {

+		REBASE(OTG_GINTSTS) = OTG_GINTSTS_SOF;

+	}

+

+	while (REBASE(OTG_GINTSTS) & OTG_GINTSTS_RXFLVL) {

+		//receive data

+		rxflvl_handle(dev);

+	}

+

+	if (REBASE(OTG_GINTSTS) & OTG_GINTSTS_HPRTINT) {

+		if (REBASE(OTG_HPRT) & OTG_HPRT_PENCHNG) {

+			uint32_t hprt = REBASE(OTG_HPRT);

+			// Clear Interrupt

+			// HARDWARE BUG - not mentioned in errata

+			// To clear interrupt write 0 to PENA

+			// To disable port write 1 to PENCHNG

+			REBASE(OTG_HPRT) &= ~OTG_HPRT_PENA;

+			//LOG_PRINTF("PENCHNG");

+			if ((hprt & OTG_HPRT_PENA)) {

+				return USBH_POLL_STATUS_DEVICE_CONNECTED;

+			}

+

+		}

+

+		if (REBASE(OTG_HPRT) & OTG_HPRT_POCCHNG) {

+			// TODO: Check for functionality

+			REBASE(OTG_HPRT) |= OTG_HPRT_POCCHNG;

+			//LOG_PRINTF("POCCHNG");

+		}

+	}

+

+	if (REBASE(OTG_GINTSTS) & OTG_GINTSTS_DISCINT) {

+		REBASE(OTG_GINTSTS) = OTG_GINTSTS_DISCINT;

+		//LOG_PRINTF("DISCINT");

+

+		/*

+		 * When the voltage drops, DISCINT interrupt is generated although

+		 * Device is connected, so there is no need to reinitialize channels.

+		 * Often, DISCINT is bad interpreted upon insertion of device

+		 */

+		if (!(REBASE(OTG_HPRT) & OTG_HPRT_PCSTS)) {

+			//LOG_PRINTF("discint processsing...");

+			channels_init(dev);

+		}

+		REBASE(OTG_GINTSTS) = REBASE(OTG_GINTSTS);

+		dev->dpstate = DEVICE_POLL_STATE_DISCONN;

+		return USBH_POLL_STATUS_DEVICE_DISCONNECTED;

+	}

+

+	if (REBASE(OTG_GINTSTS) & OTG_GINTSTS_HCINT) {

+		uint32_t channel;

+

+		for(channel = 0; channel < dev->num_channels; channel++)

+		{

+			if (channels[channel].state != CHANNEL_STATE_WORK ||

+				!(REBASE(OTG_HAINT)&(1<<channel))) {

+				continue;

+			}

+			uint32_t hcint = REBASE_CH(OTG_HCINT, channel);

+			uint8_t eptyp = channels[channel].packet.endpoint_type;

+

+			// Write

+			if (!(REBASE_CH(OTG_HCCHAR, channel)&OTG_HCCHAR_EPDIR_IN)) {

+

+				if (hcint & OTG_HCINT_NAK) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_NAK;

+					//LOG_PRINTF("NAK\n");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EAGAIN;

+					cb_data.transferred_length = channels[channel].data_index;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+

+				}

+

+				if (hcint & OTG_HCINT_ACK) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_ACK;

+					//LOG_PRINTF("ACK");

+					if (eptyp == USBH_ENDPOINT_TYPE_CONTROL) {

+						channels[channel].packet.toggle[0] = 1;

+					} else {

+						channels[channel].packet.toggle[0] ^= 1;

+					}

+				}

+

+				if (hcint & OTG_HCINT_XFRC) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_XFRC;

+					//LOG_PRINTF("XFRC\n");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

+					cb_data.transferred_length = channels[channel].data_index;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+					continue;

+				}

+

+				if (hcint & OTG_HCINT_FRMOR) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_FRMOR;

+					//LOG_PRINTF("FRMOR");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+					cb_data.transferred_length = 0;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+				}

+

+				if (hcint & OTG_HCINT_TXERR) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_TXERR;

+					//LOG_PRINTF("TXERR");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EAGAIN;

+					cb_data.transferred_length = 0;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+

+

+				}

+

+				if (hcint & OTG_HCINT_STALL) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_STALL;

+					//LOG_PRINTF("STALL");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+					cb_data.transferred_length = 0;

+

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+				}

+

+				if (hcint & OTG_HCINT_CHH) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_CHH;

+					//LOG_PRINTF("CHH");

+

+					free_channel(dev, channel);

+				}

+			} else { // Read

+

+				if (hcint & OTG_HCINT_NAK) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_NAK;

+					if (eptyp == USBH_ENDPOINT_TYPE_CONTROL) {

+						 //LOG_PRINTF("NAK");

+					}

+

+					REBASE_CH(OTG_HCCHAR, channel) |= OTG_HCCHAR_CHENA;

+

+				}

+

+				if (hcint & OTG_HCINT_DTERR) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_DTERR;

+					//LOG_PRINTF("DTERR");

+				}

+

+				if (hcint & OTG_HCINT_ACK) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_ACK;

+					//LOG_PRINTF("ACK");

+

+					channels[channel].packet.toggle[0] ^= 1;

+

+				}

+

+

+

+				if (hcint & OTG_HCINT_XFRC) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_XFRC;

+					//LOG_PRINTF("XFRC\n");

+

+					free_channel(dev, channel);

+					usbh_packet_callback_data_t cb_data;

+					if (channels[channel].data_index == channels[channel].packet.datalen) {

+						cb_data.status = USBH_PACKET_CALLBACK_STATUS_OK;

+					} else {

+						cb_data.status = USBH_PACKET_CALLBACK_STATUS_ERRSIZ;

+					}

+					cb_data.transferred_length = channels[channel].data_index;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+

+					continue;

+				}

+

+				if (hcint & OTG_HCINT_BBERR) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_BBERR;

+					//LOG_PRINTF("BBERR");

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+					cb_data.transferred_length = 0;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+				}

+

+				if (hcint & OTG_HCINT_FRMOR) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_FRMOR;

+					//LOG_PRINTF("FRMOR");

+

+				}

+

+				if (hcint & OTG_HCINT_TXERR) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_TXERR;

+					//LOG_PRINTF("TXERR");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+					cb_data.transferred_length = 0;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+

+				}

+

+				if (hcint & OTG_HCINT_STALL) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_STALL;

+					//LOG_PRINTF("STALL");

+

+					free_channel(dev, channel);

+

+					usbh_packet_callback_data_t cb_data;

+					cb_data.status = USBH_PACKET_CALLBACK_STATUS_EFATAL;

+					cb_data.transferred_length = 0;

+

+					channels[channel].packet.callback(

+						channels[channel].packet.callback_arg,

+						cb_data);

+

+				}

+				if (hcint & OTG_HCINT_CHH) {

+					REBASE_CH(OTG_HCINT, channel) = OTG_HCINT_CHH;

+					//LOG_PRINTF("CHH");

+					free_channel(dev, channel);

+				}

+

+			}

+		}

+	}

+

+	if (REBASE(OTG_GINTSTS) & OTG_GINTSTS_MMIS) {

+		REBASE(OTG_GINTSTS) = OTG_GINTSTS_MMIS;

+		//LOG_PRINTF("Mode mismatch");

+	}

+

+	if (REBASE(OTG_GINTSTS) & OTG_GINTSTS_IPXFR) {

+		REBASE(OTG_GINTSTS) = OTG_GINTSTS_IPXFR;

+		//LOG_PRINTF("IPXFR");

+	}

+

+	return USBH_POLL_STATUS_NONE;

+}

+

+/*

+ * Sequence numbers are hardcoded, since it is used

+ * locally in poll_init() function.

+ * If value of poll_sequence is needed elsewhere, enum must be defined.

+ *

+ */

+static void poll_init(usbh_lld_stm32f4_driver_data_t *dev)

+{

+	//=======================================

+

+	int done = 0;

+	/* Wait for AHB idle. */

+	switch (dev->poll_sequence) {

+	case 0:// wait until AHBIDL is set

+		if (REBASE(OTG_GRSTCTL) & OTG_GRSTCTL_AHBIDL) {

+			done = 1;

+		}

+		break;

+

+	case 1:// wait 1ms and issue core soft reset

+

+		// needs delay to not hang?? Do not know why.

+		// Maybe after AHBIDL is set, it needs to set up some things

+		if (dev->time_curr_us - dev->timestamp_us > 1000) {

+			REBASE(OTG_GRSTCTL) |= OTG_GRSTCTL_CSRST;

+			done = 1;

+		}

+		break;

+

+	case 2:// wait until core soft reset processing is done

+		if (!(REBASE(OTG_GRSTCTL) & OTG_GRSTCTL_CSRST)) {

+			done = 1;

+		}

+		break;

+

+	case 3:// wait for 50ms

+		if (dev->time_curr_us - dev->timestamp_us > 50000) {

+			done = 1;

+		}

+		break;

+

+	case 4:// wait until AHBIDL is set and power up the USB

+		if (REBASE(OTG_GRSTCTL) & OTG_GRSTCTL_AHBIDL) {

+			REBASE(OTG_GCCFG) = OTG_GCCFG_VBUSASEN | OTG_GCCFG_VBUSBSEN |

+					OTG_GCCFG_NOVBUSSENS | OTG_GCCFG_PWRDWN;

+			done = 1;

+		}

+		break;

+

+	case 5:// wait for 50ms and force host only mode

+		if (dev->time_curr_us - dev->timestamp_us > 50000) {

+

+			// Core initialized

+			// Force host only mode.

+			REBASE(OTG_GUSBCFG) |= OTG_GUSBCFG_FHMOD;

+			done = 1;

+		}

+		break;

+

+	case 6:// wait for 200ms and reset PHY clock start reset processing

+		if (dev->time_curr_us - dev->timestamp_us > 200000) {

+			/* Restart the PHY clock. */

+			REBASE(OTG_PCGCCTL) = 0;

+

+			REBASE(OTG_HCFG) = 	(REBASE(OTG_HCFG) & ~OTG_HCFG_FSLSPCS_MASK) |

+							OTG_HCFG_FSLSPCS_48MHz;

+

+			// Start reset processing

+			REBASE(OTG_HPRT) |= OTG_HPRT_PRST;

+

+			done = 1;

+

+		}

+		break;

+

+	case 7:// wait for reset processing to be done(12ms), disable PRST

+		if (dev->time_curr_us - dev->timestamp_us > 12000) {

+

+			REBASE(OTG_HPRT) &= ~OTG_HPRT_PRST;

+			done = 1;

+		}

+		break;

+

+	case 8:// wait 12ms after PRST was disabled, configure fifo

+		if (dev->time_curr_us - dev->timestamp_us > 12000) {

+

+			REBASE(OTG_HCFG) &= ~OTG_HCFG_FSLSS;

+

+			REBASE(OTG_GRXFSIZ) = RX_FIFO_SIZE;

+			REBASE(OTG_GNPTXFSIZ) = (TX_NP_FIFO_SIZE << 16) |

+								RX_FIFO_SIZE;

+			REBASE(OTG_HPTXFSIZ) = (TX_P_FIFO_SIZE << 16) |

+								(RX_FIFO_SIZE +	TX_NP_FIFO_SIZE);

+

+			// FLUSH RX FIFO

+			REBASE(OTG_GRSTCTL) |= OTG_GRSTCTL_RXFFLSH;

+

+			done = 1;

+		}

+		break;

+

+	case 9: // wait to RX FIFO become flushed, flush TX

+		if (!(REBASE(OTG_GRSTCTL) & OTG_GRSTCTL_RXFFLSH)) {

+			REBASE(OTG_GRSTCTL) |= OTG_GRSTCTL_TXFFLSH | (0x10 << 6);

+

+			done = 1;

+		}

+		break;

+

+	case 10: // wait to TX FIFO become flushed

+		if (!(REBASE(OTG_GRSTCTL) & OTG_GRSTCTL_TXFFLSH)) {

+

+			channels_init(dev);

+

+			REBASE(OTG_GOTGINT) |= 1 << 19;

+			REBASE(OTG_GINTMSK) = 0;

+			REBASE(OTG_GINTSTS) = ~0;

+			REBASE(OTG_HPRT) |= OTG_HPRT_PPWR;

+

+			done = 1;

+		}

+		break;

+

+	case 11: // wait 200ms

+		if (dev->time_curr_us - dev->timestamp_us > 200000) {

+

+			// Uncomment to enable Interrupt generation

+			REBASE(OTG_GAHBCFG) |= OTG_GAHBCFG_GINT;

+

+			LOG_PRINTF("INIT COMPLETE\n");

+

+			// Finish

+			dev->state = DEVICE_STATE_RUN;

+			dev->dpstate = DEVICE_POLL_STATE_DISCONN;

+

+			done = 1;

+		}

+	}

+

+	if (done) {

+		dev->poll_sequence++;

+		dev->timestamp_us = dev->time_curr_us;

+		//LOG_PRINTF("\t\t POLL SEQUENCE %d\n", dev->poll_sequence);

+	}

+

+}

+

+static void poll_reset(usbh_lld_stm32f4_driver_data_t *dev)

+{

+	if (dev->time_curr_us - dev->timestamp_us > 10000) {

+		REBASE(OTG_HPRT) &= ~OTG_HPRT_PRST;

+		dev->state = dev->state_prev;

+		dev->state_prev = DEVICE_STATE_RESET;

+

+		//LOG_PRINTF("RESET");

+	} else {

+		LOG_PRINTF("waiting %d < %d\n",dev->time_curr_us, dev->timestamp_us);

+	}

+}

+

+static enum USBH_POLL_STATUS poll(void *drvdata, uint32_t time_curr_us)

+{

+	(void)time_curr_us;

+

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	enum USBH_POLL_STATUS ret = USBH_POLL_STATUS_NONE;

+

+	dev->time_curr_us = time_curr_us;

+

+	switch (dev->state) {

+	case DEVICE_STATE_RUN:

+		ret = poll_run(dev);

+		break;

+

+	case DEVICE_STATE_INIT:

+		poll_init(dev);

+		break;

+

+	case DEVICE_STATE_RESET:

+		poll_reset(dev);

+		break;

+

+	default:

+		break;

+	}

+

+	return ret;

+

+}

+

+

+/**

+ *

+ * Returns positive free channel id

+ * 	otherwise -1 for error

+ */

+static int8_t get_free_channel(void *drvdata)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+	uint32_t i = 0;

+	for (i = 0; i < dev->num_channels; i++) {

+		if (dev->channels[i].state == CHANNEL_STATE_FREE &&

+			!(REBASE_CH(OTG_HCCHAR, i) & OTG_HCCHAR_CHENA)) {

+			channels[i].state = CHANNEL_STATE_WORK;

+			REBASE_CH(OTG_HCINT, i) = ~0;

+			REBASE_CH(OTG_HCINTMSK, i) |= OTG_HCINTMSK_ACKM | OTG_HCINTMSK_NAKM |

+				OTG_HCINTMSK_TXERRM | OTG_HCINTMSK_XFRCM |

+				OTG_HCINTMSK_DTERRM | OTG_HCINTMSK_BBERRM |

+				OTG_HCINTMSK_CHHM | OTG_HCINTMSK_STALLM |

+				OTG_HCINTMSK_FRMORM;

+			REBASE(OTG_HAINTMSK) |= (1 << i);

+			return i;

+		}

+	}

+	return -1;

+}

+

+/*

+ * Do not clear callback and callback data, so channel can be freed even before callback is called

+ * This saves number of active channels: When one transfer ends, in callback driver can write/read to this channel again (indirectly)

+ */

+static void free_channel(void *drvdata, uint8_t channel)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	channel_t *channels = dev->channels;

+

+	if (REBASE_CH(OTG_HCCHAR, channel) & OTG_HCCHAR_CHENA) {

+		REBASE_CH(OTG_HCCHAR, channel) |= OTG_HCCHAR_CHDIS;

+		REBASE_CH(OTG_HCINT, channel) = ~0;

+		LOG_PRINTF("\nDisabling channel %d\n", channel);

+	} else {

+		channels[channel].state = CHANNEL_STATE_FREE;

+	}

+}

+/**

+ * Init channels

+ */

+static void channels_init(void *drvdata)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+

+	uint32_t i = 0;

+	for (i = 0; i < dev->num_channels; i++) {

+		REBASE_CH(OTG_HCINT, i) = ~0;

+		REBASE_CH(OTG_HCINTMSK, i) = 0x7ff;

+		free_channel(dev, i);

+	}

+

+	// Enable interrupt mask bits for all channels

+	REBASE(OTG_HAINTMSK) = (1 << dev->num_channels) - 1;

+}

+

+/**

+ * Get speed of connected device

+ *

+ */

+static enum USBH_SPEED root_speed(void *drvdata)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = drvdata;

+	(void)dev;

+	uint32_t hprt_speed = REBASE(OTG_HPRT) & OTG_HPRT_PSPD_MASK;

+	if (hprt_speed == OTG_HPRT_PSPD_LOW) {

+		return USBH_SPEED_LOW;

+	} else if(hprt_speed == OTG_HPRT_PSPD_FULL) {

+		return USBH_SPEED_FULL;

+	} else if(hprt_speed == OTG_HPRT_PSPD_HIGH) {

+		return USBH_SPEED_HIGH;

+	} else {

+		// Should not happen(let the compiler be happy)

+		return USBH_SPEED_FULL;

+	}

+}

+#endif // if defined otg_hs or otg_fs

+

+

+#ifdef USART_DEBUG

+

+/**

+ * Just for debug

+ */

+void print_channels(const void *lld)

+{

+	usbh_lld_stm32f4_driver_data_t *dev = ((usbh_low_level_driver_t *)lld)->driver_data;

+	channel_t *channels = dev->channels;

+	int32_t i;

+	LOG_PRINTF("\nCHANNELS: \n");

+	for (i = 0;i < dev->num_channels;i++) {

+		LOG_PRINTF("%4d %4d %4d %08X\n", channels[i].state, channels[i].packet.address, channels[i].packet.datalen, MMIO32(dev->base + OTG_HCINT(i)));

+	}

+}

+#endif

+

+// USB Full Speed - OTG_FS

+#if defined(USE_STM32F4_USBH_DRIVER_FS)

+#define NUM_CHANNELS_FS		(8)

+static channel_t channels_fs[NUM_CHANNELS_FS];

+static usbh_lld_stm32f4_driver_data_t driver_data_fs = {

+	.base = USB_OTG_FS_BASE,

+	.channels = channels_fs,

+	.num_channels = NUM_CHANNELS_FS

+};

+const usbh_low_level_driver_t usbh_lld_stm32f4_driver_fs = {

+	.init = init,

+	.poll = poll,

+	.read = read,

+	.write = write,

+	.root_speed = root_speed,

+	.driver_data = &driver_data_fs

+};

+#endif

+

+// USB High Speed - OTG_HS

+#if defined(USE_STM32F4_USBH_DRIVER_HS)

+#define NUM_CHANNELS_HS		(12)

+static channel_t channels_hs[NUM_CHANNELS_HS];

+static usbh_lld_stm32f4_driver_data_t driver_data_hs = {

+	.base = USB_OTG_HS_BASE,

+	.channels = channels_hs,

+	.num_channels = NUM_CHANNELS_HS

+};

+

+const usbh_low_level_driver_t usbh_lld_stm32f4_driver_hs = {

+	.init = init,

+	.poll = poll,

+	.read = read,

+	.write = write,

+	.root_speed = root_speed,

+	.driver_data = &driver_data_hs

+};

+

+#endif

diff --git a/fw/src/words.c b/fw/src/words.c
new file mode 100644
index 0000000..387d23b
--- /dev/null
+++ b/fw/src/words.c
@@ -0,0 +1,521 @@
+
+#include "words.h"
+
+const char * const even[256] = {
+    "aardvark",     /* 00 */
+    "absurd",       /* 01 */
+    "accrue",       /* 02 */
+    "acme",         /* 03 */
+    "adrift",       /* 04 */
+    "adult",        /* 05 */
+    "afflict",      /* 06 */
+    "ahead",        /* 07 */
+    "aimless",      /* 08 */
+    "Algol",        /* 09 */
+    "allow",        /* 0A */
+    "alone",        /* 0B */
+    "ammo",         /* 0C */
+    "ancient",      /* 0D */
+    "apple",        /* 0E */
+    "artist",       /* 0F */
+    "assume",       /* 10 */
+    "Athens",       /* 11 */
+    "atlas",        /* 12 */
+    "Aztec",        /* 13 */
+    "baboon",       /* 14 */
+    "backfield",    /* 15 */
+    "backward",     /* 16 */
+    "banjo",        /* 17 */
+    "beaming",      /* 18 */
+    "bedlamp",      /* 19 */
+    "beehive",      /* 1A */
+    "beeswax",      /* 1B */
+    "befriend",     /* 1C */
+    "Belfast",      /* 1D */
+    "berserk",      /* 1E */
+    "billiard",     /* 1F */
+    "bison",        /* 20 */
+    "blackjack",    /* 21 */
+    "blockade",     /* 22 */
+    "blowtorch",    /* 23 */
+    "bluebird",     /* 24 */
+    "bombast",      /* 25 */
+    "bookshelf",    /* 26 */
+    "brackish",     /* 27 */
+    "breadline",    /* 28 */
+    "breakup",      /* 29 */
+    "brickyard",    /* 2A */
+    "briefcase",    /* 2B */
+    "Burbank",      /* 2C */
+    "button",       /* 2D */
+    "buzzard",      /* 2E */
+    "cement",       /* 2F */
+    "chairlift",    /* 30 */
+    "chatter",      /* 31 */
+    "checkup",      /* 32 */
+    "chisel",       /* 33 */
+    "choking",      /* 34 */
+    "chopper",      /* 35 */
+    "Christmas",    /* 36 */
+    "clamshell",    /* 37 */
+    "classic",      /* 38 */
+    "classroom",    /* 39 */
+    "cleanup",      /* 3A */
+    "clockwork",    /* 3B */
+    "cobra",        /* 3C */
+    "commence",     /* 3D */
+    "concert",      /* 3E */
+    "cowbell",      /* 3F */
+    "crackdown",    /* 40 */
+    "cranky",       /* 41 */
+    "crowfoot",     /* 42 */
+    "crucial",      /* 43 */
+    "crumpled",     /* 44 */
+    "crusade",      /* 45 */
+    "cubic",        /* 46 */
+    "dashboard",    /* 47 */
+    "deadbolt",     /* 48 */
+    "deckhand",     /* 49 */
+    "dogsled",      /* 4A */
+    "dragnet",      /* 4B */
+    "drainage",     /* 4C */
+    "dreadful",     /* 4D */
+    "drifter",      /* 4E */
+    "dropper",      /* 4F */
+    "drumbeat",     /* 50 */
+    "drunken",      /* 51 */
+    "Dupont",       /* 52 */
+    "dwelling",     /* 53 */
+    "eating",       /* 54 */
+    "edict",        /* 55 */
+    "egghead",      /* 56 */
+    "eightball",    /* 57 */
+    "endorse",      /* 58 */
+    "endow",        /* 59 */
+    "enlist",       /* 5A */
+    "erase",        /* 5B */
+    "escape",       /* 5C */
+    "exceed",       /* 5D */
+    "eyeglass",     /* 5E */
+    "eyetooth",     /* 5F */
+    "facial",       /* 60 */
+    "fallout",      /* 61 */
+    "flagpole",     /* 62 */
+    "flatfoot",     /* 63 */
+    "flytrap",      /* 64 */
+    "fracture",     /* 65 */
+    "framework",    /* 66 */
+    "freedom",      /* 67 */
+    "frighten",     /* 68 */
+    "gazelle",      /* 69 */
+    "Geiger",       /* 6A */
+    "glitter",      /* 6B */
+    "glucose",      /* 6C */
+    "goggles",      /* 6D */
+    "goldfish",     /* 6E */
+    "gremlin",      /* 6F */
+    "guidance",     /* 70 */
+    "hamlet",       /* 71 */
+    "highchair",    /* 72 */
+    "hockey",       /* 73 */
+    "indoors",      /* 74 */
+    "indulge",      /* 75 */
+    "inverse",      /* 76 */
+    "involve",      /* 77 */
+    "island",       /* 78 */
+    "jawbone",      /* 79 */
+    "keyboard",     /* 7A */
+    "kickoff",      /* 7B */
+    "kiwi",         /* 7C */
+    "klaxon",       /* 7D */
+    "locale",       /* 7E */
+    "lockup",       /* 7F */
+    "merit",        /* 80 */
+    "minnow",       /* 81 */
+    "miser",        /* 82 */
+    "Mohawk",       /* 83 */
+    "mural",        /* 84 */
+    "music",        /* 85 */
+    "necklace",     /* 86 */
+    "Neptune",      /* 87 */
+    "newborn",      /* 88 */
+    "nightbird",    /* 89 */
+    "Oakland",      /* 8A */
+    "obtuse",       /* 8B */
+    "offload",      /* 8C */
+    "optic",        /* 8D */
+    "orca",         /* 8E */
+    "payday",       /* 8F */
+    "peachy",       /* 90 */
+    "pheasant",     /* 91 */
+    "physique",     /* 92 */
+    "playhouse",    /* 93 */
+    "Pluto",        /* 94 */
+    "preclude",     /* 95 */
+    "prefer",       /* 96 */
+    "preshrunk",    /* 97 */
+    "printer",      /* 98 */
+    "prowler",      /* 99 */
+    "pupil",        /* 9A */
+    "puppy",        /* 9B */
+    "python",       /* 9C */
+    "quadrant",     /* 9D */
+    "quiver",       /* 9E */
+    "quota",        /* 9F */
+    "ragtime",      /* A0 */
+    "ratchet",      /* A1 */
+    "rebirth",      /* A2 */
+    "reform",       /* A3 */
+    "regain",       /* A4 */
+    "reindeer",     /* A5 */
+    "rematch",      /* A6 */
+    "repay",        /* A7 */
+    "retouch",      /* A8 */
+    "revenge",      /* A9 */
+    "reward",       /* AA */
+    "rhythm",       /* AB */
+    "ribcage",      /* AC */
+    "ringbolt",     /* AD */
+    "robust",       /* AE */
+    "rocker",       /* AF */
+    "ruffled",      /* B0 */
+    "sailboat",     /* B1 */
+    "sawdust",      /* B2 */
+    "scallion",     /* B3 */
+    "scenic",       /* B4 */
+    "scorecard",    /* B5 */
+    "Scotland",     /* B6 */
+    "seabird",      /* B7 */
+    "select",       /* B8 */
+    "sentence",     /* B9 */
+    "shadow",       /* BA */
+    "shamrock",     /* BB */
+    "showgirl",     /* BC */
+    "skullcap",     /* BD */
+    "skydive",      /* BE */
+    "slingshot",    /* BF */
+    "slowdown",     /* C0 */
+    "snapline",     /* C1 */
+    "snapshot",     /* C2 */
+    "snowcap",      /* C3 */
+    "snowslide",    /* C4 */
+    "solo",         /* C5 */
+    "southward",    /* C6 */
+    "soybean",      /* C7 */
+    "spaniel",      /* C8 */
+    "spearhead",    /* C9 */
+    "spellbind",    /* CA */
+    "spheroid",     /* CB */
+    "spigot",       /* CC */
+    "spindle",      /* CD */
+    "spyglass",     /* CE */
+    "stagehand",    /* CF */
+    "stagnate",     /* D0 */
+    "stairway",     /* D1 */
+    "standard",     /* D2 */
+    "stapler",      /* D3 */
+    "steamship",    /* D4 */
+    "sterling",     /* D5 */
+    "stockman",     /* D6 */
+    "stopwatch",    /* D7 */
+    "stormy",       /* D8 */
+    "sugar",        /* D9 */
+    "surmount",     /* DA */
+    "suspense",     /* DB */
+    "sweatband",    /* DC */
+    "swelter",      /* DD */
+    "tactics",      /* DE */
+    "talon",        /* DF */
+    "tapeworm",     /* E0 */
+    "tempest",      /* E1 */
+    "tiger",        /* E2 */
+    "tissue",       /* E3 */
+    "tonic",        /* E4 */
+    "topmost",      /* E5 */
+    "tracker",      /* E6 */
+    "transit",      /* E7 */
+    "trauma",       /* E8 */
+    "treadmill",    /* E9 */
+    "Trojan",       /* EA */
+    "trouble",      /* EB */
+    "tumor",        /* EC */
+    "tunnel",       /* ED */
+    "tycoon",       /* EE */
+    "uncut",        /* EF */
+    "unearth",      /* F0 */
+    "unwind",       /* F1 */
+    "uproot",       /* F2 */
+    "upset",        /* F3 */
+    "upshot",       /* F4 */
+    "vapor",        /* F5 */
+    "village",      /* F6 */
+    "virus",        /* F7 */
+    "Vulcan",       /* F8 */
+    "waffle",       /* F9 */
+    "wallet",       /* FA */
+    "watchword",    /* FB */
+    "wayside",      /* FC */
+    "willow",       /* FD */
+    "woodlark",     /* FE */
+    "Zulu"          /* FF */
+};
+
+const char * const odd[256] = {
+    "aardvark",     /* 00 */
+    "absurd",       /* 01 */
+    "accrue",       /* 02 */
+    "acme",         /* 03 */
+    "adrift",       /* 04 */
+    "adult",        /* 05 */
+    "afflict",      /* 06 */
+    "ahead",        /* 07 */
+    "aimless",      /* 08 */
+    "Algol",        /* 09 */
+    "allow",        /* 0A */
+    "alone",        /* 0B */
+    "ammo",         /* 0C */
+    "ancient",      /* 0D */
+    "apple",        /* 0E */
+    "artist",       /* 0F */
+    "assume",       /* 10 */
+    "Athens",       /* 11 */
+    "atlas",        /* 12 */
+    "Aztec",        /* 13 */
+    "baboon",       /* 14 */
+    "backfield",    /* 15 */
+    "backward",     /* 16 */
+    "banjo",        /* 17 */
+    "beaming",      /* 18 */
+    "bedlamp",      /* 19 */
+    "beehive",      /* 1A */
+    "beeswax",      /* 1B */
+    "befriend",     /* 1C */
+    "Belfast",      /* 1D */
+    "berserk",      /* 1E */
+    "billiard",     /* 1F */
+    "bison",        /* 20 */
+    "blackjack",    /* 21 */
+    "blockade",     /* 22 */
+    "blowtorch",    /* 23 */
+    "bluebird",     /* 24 */
+    "bombast",      /* 25 */
+    "bookshelf",    /* 26 */
+    "brackish",     /* 27 */
+    "breadline",    /* 28 */
+    "breakup",      /* 29 */
+    "brickyard",    /* 2A */
+    "briefcase",    /* 2B */
+    "Burbank",      /* 2C */
+    "button",       /* 2D */
+    "buzzard",      /* 2E */
+    "cement",       /* 2F */
+    "chairlift",    /* 30 */
+    "chatter",      /* 31 */
+    "checkup",      /* 32 */
+    "chisel",       /* 33 */
+    "choking",      /* 34 */
+    "chopper",      /* 35 */
+    "Christmas",    /* 36 */
+    "clamshell",    /* 37 */
+    "classic",      /* 38 */
+    "classroom",    /* 39 */
+    "cleanup",      /* 3A */
+    "clockwork",    /* 3B */
+    "cobra",        /* 3C */
+    "commence",     /* 3D */
+    "concert",      /* 3E */
+    "cowbell",      /* 3F */
+    "crackdown",    /* 40 */
+    "cranky",       /* 41 */
+    "crowfoot",     /* 42 */
+    "crucial",      /* 43 */
+    "crumpled",     /* 44 */
+    "crusade",      /* 45 */
+    "cubic",        /* 46 */
+    "dashboard",    /* 47 */
+    "deadbolt",     /* 48 */
+    "deckhand",     /* 49 */
+    "dogsled",      /* 4A */
+    "dragnet",      /* 4B */
+    "drainage",     /* 4C */
+    "dreadful",     /* 4D */
+    "drifter",      /* 4E */
+    "dropper",      /* 4F */
+    "drumbeat",     /* 50 */
+    "drunken",      /* 51 */
+    "Dupont",       /* 52 */
+    "dwelling",     /* 53 */
+    "eating",       /* 54 */
+    "edict",        /* 55 */
+    "egghead",      /* 56 */
+    "eightball",    /* 57 */
+    "endorse",      /* 58 */
+    "endow",        /* 59 */
+    "enlist",       /* 5A */
+    "erase",        /* 5B */
+    "escape",       /* 5C */
+    "exceed",       /* 5D */
+    "eyeglass",     /* 5E */
+    "eyetooth",     /* 5F */
+    "facial",       /* 60 */
+    "fallout",      /* 61 */
+    "flagpole",     /* 62 */
+    "flatfoot",     /* 63 */
+    "flytrap",      /* 64 */
+    "fracture",     /* 65 */
+    "framework",    /* 66 */
+    "freedom",      /* 67 */
+    "frighten",     /* 68 */
+    "gazelle",      /* 69 */
+    "Geiger",       /* 6A */
+    "glitter",      /* 6B */
+    "glucose",      /* 6C */
+    "goggles",      /* 6D */
+    "goldfish",     /* 6E */
+    "gremlin",      /* 6F */
+    "guidance",     /* 70 */
+    "hamlet",       /* 71 */
+    "highchair",    /* 72 */
+    "hockey",       /* 73 */
+    "indoors",      /* 74 */
+    "indulge",      /* 75 */
+    "inverse",      /* 76 */
+    "involve",      /* 77 */
+    "island",       /* 78 */
+    "jawbone",      /* 79 */
+    "keyboard",     /* 7A */
+    "kickoff",      /* 7B */
+    "kiwi",         /* 7C */
+    "klaxon",       /* 7D */
+    "locale",       /* 7E */
+    "lockup",       /* 7F */
+    "merit",        /* 80 */
+    "minnow",       /* 81 */
+    "miser",        /* 82 */
+    "Mohawk",       /* 83 */
+    "mural",        /* 84 */
+    "music",        /* 85 */
+    "necklace",     /* 86 */
+    "Neptune",      /* 87 */
+    "newborn",      /* 88 */
+    "nightbird",    /* 89 */
+    "Oakland",      /* 8A */
+    "obtuse",       /* 8B */
+    "offload",      /* 8C */
+    "optic",        /* 8D */
+    "orca",         /* 8E */
+    "payday",       /* 8F */
+    "peachy",       /* 90 */
+    "pheasant",     /* 91 */
+    "physique",     /* 92 */
+    "playhouse",    /* 93 */
+    "Pluto",        /* 94 */
+    "preclude",     /* 95 */
+    "prefer",       /* 96 */
+    "preshrunk",    /* 97 */
+    "printer",      /* 98 */
+    "prowler",      /* 99 */
+    "pupil",        /* 9A */
+    "puppy",        /* 9B */
+    "python",       /* 9C */
+    "quadrant",     /* 9D */
+    "quiver",       /* 9E */
+    "quota",        /* 9F */
+    "ragtime",      /* A0 */
+    "ratchet",      /* A1 */
+    "rebirth",      /* A2 */
+    "reform",       /* A3 */
+    "regain",       /* A4 */
+    "reindeer",     /* A5 */
+    "rematch",      /* A6 */
+    "repay",        /* A7 */
+    "retouch",      /* A8 */
+    "revenge",      /* A9 */
+    "reward",       /* AA */
+    "rhythm",       /* AB */
+    "ribcage",      /* AC */
+    "ringbolt",     /* AD */
+    "robust",       /* AE */
+    "rocker",       /* AF */
+    "ruffled",      /* B0 */
+    "sailboat",     /* B1 */
+    "sawdust",      /* B2 */
+    "scallion",     /* B3 */
+    "scenic",       /* B4 */
+    "scorecard",    /* B5 */
+    "Scotland",     /* B6 */
+    "seabird",      /* B7 */
+    "select",       /* B8 */
+    "sentence",     /* B9 */
+    "shadow",       /* BA */
+    "shamrock",     /* BB */
+    "showgirl",     /* BC */
+    "skullcap",     /* BD */
+    "skydive",      /* BE */
+    "slingshot",    /* BF */
+    "slowdown",     /* C0 */
+    "snapline",     /* C1 */
+    "snapshot",     /* C2 */
+    "snowcap",      /* C3 */
+    "snowslide",    /* C4 */
+    "solo",         /* C5 */
+    "southward",    /* C6 */
+    "soybean",      /* C7 */
+    "spaniel",      /* C8 */
+    "spearhead",    /* C9 */
+    "spellbind",    /* CA */
+    "spheroid",     /* CB */
+    "spigot",       /* CC */
+    "spindle",      /* CD */
+    "spyglass",     /* CE */
+    "stagehand",    /* CF */
+    "stagnate",     /* D0 */
+    "stairway",     /* D1 */
+    "standard",     /* D2 */
+    "stapler",      /* D3 */
+    "steamship",    /* D4 */
+    "sterling",     /* D5 */
+    "stockman",     /* D6 */
+    "stopwatch",    /* D7 */
+    "stormy",       /* D8 */
+    "sugar",        /* D9 */
+    "surmount",     /* DA */
+    "suspense",     /* DB */
+    "sweatband",    /* DC */
+    "swelter",      /* DD */
+    "tactics",      /* DE */
+    "talon",        /* DF */
+    "tapeworm",     /* E0 */
+    "tempest",      /* E1 */
+    "tiger",        /* E2 */
+    "tissue",       /* E3 */
+    "tonic",        /* E4 */
+    "topmost",      /* E5 */
+    "tracker",      /* E6 */
+    "transit",      /* E7 */
+    "trauma",       /* E8 */
+    "treadmill",    /* E9 */
+    "Trojan",       /* EA */
+    "trouble",      /* EB */
+    "tumor",        /* EC */
+    "tunnel",       /* ED */
+    "tycoon",       /* EE */
+    "uncut",        /* EF */
+    "unearth",      /* F0 */
+    "unwind",       /* F1 */
+    "uproot",       /* F2 */
+    "upset",        /* F3 */
+    "upshot",       /* F4 */
+    "vapor",        /* F5 */
+    "village",      /* F6 */
+    "virus",        /* F7 */
+    "Vulcan",       /* F8 */
+    "waffle",       /* F9 */
+    "wallet",       /* FA */
+    "watchword",    /* FB */
+    "wayside",      /* FC */
+    "willow",       /* FD */
+    "woodlark",     /* FE */
+    "Zulu",         /* FF */
+};
+
diff --git a/fw/src/words.h b/fw/src/words.h
new file mode 100644
index 0000000..0bbd3c4
--- /dev/null
+++ b/fw/src/words.h
@@ -0,0 +1,7 @@
+#ifndef __ADJECTIVES_H__
+#define __ADJECTIVES_H__
+
+extern const char * const even[256];
+extern const char * const odd[256];
+
+#endif