diff --git a/epicardium/main.c b/epicardium/main.c
index 66d3ad598e922fa30a8edc66fc32cdd7683cd631..9d7a578a83642802056a129bf7655419c090ed4f 100644
--- a/epicardium/main.c
+++ b/epicardium/main.c
@@ -24,7 +24,6 @@
 #include "task.h"
 
 TaskHandle_t dispatcher_task_id;
-TaskHandle_t ble_task_id;
 
 void vBleTask(void*pvParameters);
 
@@ -106,13 +105,14 @@ int main(void)
 		abort();
 	}
 
+    /* BLE */
 	if (xTaskCreate(
 		vBleTask,
 		(const char*)"BLE",
 		configMINIMAL_STACK_SIZE,
 		NULL,
-		tskIDLE_PRIORITY  + 3,
-		&ble_task_id
+		tskIDLE_PRIORITY  + 1,
+        NULL
 	) != pdPASS) {
 		printf("Failed to create BLE task!\n");
 		abort();
diff --git a/epicardium/modules/ble.c b/epicardium/modules/ble.c
index 230b51a97781118b54db32d78f9a3236b5a8d52b..ae56ddc698798027a5b70a7e24c019d382f2e445 100644
--- a/epicardium/modules/ble.c
+++ b/epicardium/modules/ble.c
@@ -12,9 +12,15 @@
 #include "mcr_regs.h"
 #include "hci_core.h"
 
+#include "FreeRTOS.h"
+#include "timers.h"
+
 #include <stdio.h>
 #include <string.h>
 
+/* Task ID for the ble handler */
+static TaskHandle_t ble_task_id = NULL;
+
 /* Number of WSF buffer pools */
 #define WSF_BUF_POOLS              6
 
@@ -65,28 +71,19 @@ static void myTrace(const char *pStr, va_list args)
     }
 }
 
+/*************************************************************************************************/
+/*!
+ *  \brief  Initialize WSF.
+ *
+ *  \return None.
+ */
+/*************************************************************************************************/
 static void WsfInit(void)
 {
     WsfTimerInit();
     WsfBufInit(sizeof(mainBufMem), (uint8_t*)mainBufMem, WSF_BUF_POOLS, mainPoolDesc);
     WsfTraceRegister(myTrace);
 }
-
-void MacInit(void)
-{
-    wsfHandlerId_t handlerId;
-
-    /* Initialize link layer. */
-    BbInit();
-    handlerId = WsfOsSetNextHandler(SchHandler);
-    SchInit(handlerId);
-    LlAdvSlaveInit();
-    LlConnSlaveInit();
-    handlerId = WsfOsSetNextHandler(LlHandler);
-    LlHandlerInit(handlerId);
-}
-
-
 /* TODO: WTF? */
 /*
  * In two-chip solutions, setting the address must wait until the HCI interface is initialized.
@@ -109,6 +106,67 @@ void SetAddress(uint8_t event)
     }
 }
 
+/*************************************************************************************************/
+/*!
+ *  \brief  Initialize MAC layer.
+ *
+ *  \param  None.
+ *
+ *  \return None.
+ */
+/*************************************************************************************************/
+extern int8_t tx_rfpower_on;
+void MacInit(void)
+{
+    wsfHandlerId_t handlerId;
+
+    /* Initialize link layer. */
+    BbInit();
+    handlerId = WsfOsSetNextHandler(SchHandler);
+    SchInit(handlerId);
+    LlAdvSlaveInit();
+    LlConnSlaveInit();
+    handlerId = WsfOsSetNextHandler(LlHandler);
+    LlHandlerInit(handlerId);
+}
+
+
+
+
+static StaticTimer_t x;
+TimerHandle_t timerWakeup;
+int lasttick = 0;
+
+static void vTimerCallback(xTimerHandle pxTimer)
+{
+    bool_t          timerRunning;
+    wsfTimerTicks_t time_to_next_expire;
+    do {
+        int tick = xTaskGetTickCount();
+        WsfTimerUpdate(tick - lasttick);
+        lasttick = tick;
+        time_to_next_expire = WsfTimerNextExpiration(&timerRunning);
+    } while (timerRunning && time_to_next_expire == 0);
+
+
+    if(timerRunning) {
+        printf("time_to_next_expire = %d\n", time_to_next_expire);
+        timerWakeup = xTimerCreateStatic(
+            "timerWakeup", /* name */
+            pdMS_TO_TICKS(time_to_next_expire), /* period/time */
+            pdFALSE, /* auto reload */
+            NULL, /* timer ID */
+            vTimerCallback, &x); /* callback */
+
+        if(timerWakeup != NULL) {
+            xTimerStart(timerWakeup, 0);
+        } else {
+            printf("could not create timer\n");
+        }
+    } else {
+        printf("No timer running\n");
+    }
+}
 
 
 static void ble_init(void)
@@ -121,14 +179,23 @@ static void ble_init(void)
 
     /* Register a handler for Application events */
     AppUiActionRegister(SetAddress);
+
+    lasttick = xTaskGetTickCount();
+    vTimerCallback(NULL);
 }
 
 
 void vBleTask(void*pvParameters)
 {
+	ble_task_id = xTaskGetCurrentTaskHandle();
+
     ble_init();
+    const TickType_t xDelay = 500 / portTICK_PERIOD_MS;
+
     while (1){
         // TODO: this need some timing and sleeping
         wsfOsDispatcher();
+        vTimerCallback(NULL);
+        vTaskDelay( xDelay );
     }
 }