diff --git a/src/btstack_hsm.c b/src/btstack_hsm.c
index 11b9ae25e..d18197bd4 100644
--- a/src/btstack_hsm.c
+++ b/src/btstack_hsm.c
@@ -76,6 +76,7 @@ static btstack_hsm_state_t btstack_hsm_get_super( btstack_hsm_t * const me, btst
 
 static btstack_hsm_event_t const entry_evt = { BTSTACK_HSM_ENTRY_SIG };
 static btstack_hsm_event_t const exit_evt = { BTSTACK_HSM_EXIT_SIG };
+static btstack_hsm_event_t const init_evt  = { BTSTACK_HSM_INIT_SIG };
 
 void btstack_hsm_init(btstack_hsm_t * const me, btstack_hsm_event_t const * const e) {
     btstack_assert(me->state != NULL);
@@ -83,21 +84,19 @@ void btstack_hsm_init(btstack_hsm_t * const me, btstack_hsm_event_t const * cons
     btstack_hsm_state_handler_t target = me->state;
     btstack_hsm_state_t status = me->temp(me, e);
     btstack_assert( status == BTSTACK_HSM_TRAN_STATUS );
-    static btstack_hsm_event_t const init_evt = { BTSTACK_HSM_INIT_SIG };
 
-    int_fast8_t level;
     btstack_hsm_state_handler_t *root_path = me->path;
     memset(root_path, 0, sizeof(btstack_hsm_state_handler_t)*me->depth);
 
     do {
-        level = 0;
+        int_fast8_t level = 0;
         btstack_hsm_state_handler_t current = me->temp;
         for(; current != target; current=me->temp, level++ ) {
             root_path[level] = current;
             btstack_hsm_get_super( me, current );
         }
-        for(; level>0;) {
-            root_path[--level]( me, &entry_evt );
+        for(--level; level>=0;--level) {
+            root_path[level]( me, &entry_evt );
         }
         target = root_path[0];
         status = target( me, &init_evt );
@@ -106,10 +105,22 @@ void btstack_hsm_init(btstack_hsm_t * const me, btstack_hsm_event_t const * cons
     me->state = target;
 }
 
-static void btstack_hsm_handler_super_cache( btstack_hsm_t * const me, btstack_hsm_state_handler_t cache[], int idx, btstack_hsm_state_handler_t handler ) {
-    if( cache[idx] == NULL ) {
-        cache[idx] = handler;
+static void btstack_hsm_handler_super_cache(
+        btstack_hsm_t * const me,
+        btstack_hsm_state_handler_t cache[],
+        int idx, btstack_hsm_state_handler_t handler ) {
+
+    if( idx == me->depth ) {
         btstack_hsm_get_super(me, handler);
+        if( me->temp != btstack_hsm_top ) {
+            log_error("state machine has higher depth (%d) than specified!", me->depth);
+            btstack_assert( 0 );
+        }
+        return;
+    }
+    if( cache[idx] == NULL ) {
+        btstack_hsm_get_super(me, handler);
+        cache[idx] = me->temp;
     } else {
         me->temp = cache[idx];
     }
@@ -117,34 +128,54 @@ static void btstack_hsm_handler_super_cache( btstack_hsm_t * const me, btstack_h
 
 btstack_hsm_state_t btstack_hsm_dispatch(btstack_hsm_t * const me, btstack_hsm_event_t const * const e) {
     btstack_hsm_state_t status;
-    btstack_hsm_state_handler_t current = me->state;
+    btstack_hsm_state_handler_t current;
+    // forward event to next hierarchy level if not handled in current state
+    me->temp = me->state;
     do {
+        current = me->temp;
         status = current(me, e);
         // if the state doesn't handle the event try at the super state too
         if( status == BTSTACK_HSM_UNHANDLED_STATUS ) {
             status = btstack_hsm_get_super( me, current );
         }
-        current = me->temp;
     } while( status == BTSTACK_HSM_SUPER_STATUS );
 
     // if we don't switch states we are done now
     if( status != BTSTACK_HSM_TRAN_STATUS ) {
         return status;
     }
-    btstack_hsm_state_handler_t source = me->state;
-    btstack_hsm_state_handler_t target = me->temp;
+    // save the destination of the previous transition
+    btstack_hsm_state_handler_t dest = me->temp;
+
+    // if the transaction came from an higher hierarchical level, go there
+    btstack_hsm_state_handler_t target = current;
+    current = me->state;
+    for(; current != target; current = me->temp) {
+        current( me, &exit_evt );
+        btstack_hsm_get_super( me, current );
+    }
+
+    btstack_hsm_state_handler_t source = target;
+    target = dest;
     btstack_hsm_state_handler_t *root_path = me->path;
     memset(root_path, 0, sizeof(btstack_hsm_state_handler_t)*me->depth);
 
-    // why should that be possible/necessary?
-    btstack_assert( source != target );
+    // the state handlers form a single linked list with the default transition pointing to the previous hierarchy level,
+    // so if we only record the previous pointer we miss the first element of the list to reproduce it fully.
+    // So now the array contains head->prev->prev->prev...
+    root_path[0] = target;
 
+    // self transition
+    if( source == target ) {
+        source( me, &exit_evt );
+        target( me, &entry_evt );
+    }
     // handle entry/exit edges
     int_fast8_t level = 0;
     bool lca_found = false;
     // calculates the lowest common ancestor of the state graph
     for(; source != btstack_hsm_top; source=me->temp) {
-        level = 0;
+        level = 1;
         for(current=target; current != btstack_hsm_top; current=me->temp, ++level ) {
             if( current == source ) {
                 lca_found = true;
@@ -160,8 +191,22 @@ btstack_hsm_state_t btstack_hsm_dispatch(btstack_hsm_t * const me, btstack_hsm_e
         btstack_hsm_get_super( me, source );
     }
 
-    for(level--; level > 0; ) {
-        root_path[--level]( me, &entry_evt );
+    // handle entry in reverse order
+    for(level-=2; level >= 0; --level) {
+        root_path[level]( me, &entry_evt );
+    }
+    // initial transitions are only allowed to point deeper into the state machine hierarchy,
+    // so we only follow this direction here, deeper.
+    for(; target( me, &init_evt ) == BTSTACK_HSM_TRAN_STATUS ;) {
+        current = me->temp;
+        for(level = 0; current != target; current=me->temp, ++level ) {
+            root_path[level] = current;
+            btstack_hsm_get_super( me, current );
+        }
+        for(--level; level >= 0; --level) {
+            root_path[level]( me, &entry_evt );
+        }
+        target = root_path[0];
     }
     me->state = target;
     return status;