Add message_handler application

This is an example application for communication with Ethos-U kernel driver

Change-Id: I1d602682cbfdf26a2b082a56ee6c38e93a49ab8d

7 files changed, 988 insertions, 0 deletions

diff --git a/applications/CMakeLists.txt b/applications/CMakeLists.txt
index 1c8d2a7..11ed236 100644
--- a/applications/CMakeLists.txt
+++ b/applications/CMakeLists.txt
@@ -26,6 +26,8 @@ add_subdirectory(hello_world)
 
 add_subdirectory(threadx_demo)
 
+add_subdirectory(message_handler)
+
 if (CMAKE_CXX_COMPILER_ID STREQUAL "ARMClang")
     # Only armclang supported for now
     add_subdirectory(trustzone_inference)
diff --git a/applications/message_handler/CMakeLists.txt b/applications/message_handler/CMakeLists.txt
new file mode 100644
index 0000000..0cb95cc
--- /dev/null
+++ b/applications/message_handler/CMakeLists.txt
@@ -0,0 +1,42 @@
+#
+# Copyright (c) 2020-2021 Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+# Licensed under the Apache License, Version 2.0 (the License); you may
+# not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an AS IS BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+if (NOT TARGET freertos_kernel)
+    message("Not building ethosu_message_dispatcher, required freertos not built.")
+    return()
+endif()
+
+set(MESSAGE_HANDLER_ARENA_SIZE 2000000 CACHE STRING "Size of message handler tensor arena")
+
+ethosu_add_executable(message_handler
+    SOURCES
+        main.cpp
+        message_handler.cpp
+        message_queue.cpp
+    LIBRARIES
+        cmsis_device
+        $<$<TARGET_EXISTS:ethosu_core_driver>:ethosu_core_driver>
+        ethosu_mailbox
+        freertos_kernel
+        inference_process)
+
+target_include_directories(message_handler PRIVATE
+    ${LINUX_DRIVER_STACK_PATH}/kernel)
+
+target_compile_definitions(message_handler PRIVATE
+    TENSOR_ARENA_SIZE=${MESSAGE_HANDLER_ARENA_SIZE})
diff --git a/applications/message_handler/main.cpp b/applications/message_handler/main.cpp
new file mode 100644
index 0000000..9f7c7cc
--- /dev/null
+++ b/applications/message_handler/main.cpp
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2019-2021 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/****************************************************************************
+ * Includes
+ ****************************************************************************/
+
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "semphr.h"
+#include "task.h"
+
+#include <inttypes.h>
+#include <stdio.h>
+
+#include "ethosu_core_interface.h"
+#include "message_handler.hpp"
+#include "message_queue.hpp"
+#include <mailbox.hpp>
+
+#if defined(MHU_V2)
+#include <mhu_v2.hpp>
+#elif defined(MHU_JUNO)
+#include <mhu_juno.hpp>
+#else
+#include <mhu_dummy.hpp>
+#endif
+
+/* Disable semihosting */
+__asm(".global __use_no_semihosting\n\t");
+
+using namespace EthosU;
+using namespace MessageHandler;
+
+/****************************************************************************
+ * Defines
+ ****************************************************************************/
+
+// Nr. of tasks to process inferences with, reserves driver & runs inference (Normally 1 per NPU, but not a must)
+#if defined(ETHOSU_NPU_COUNT)
+constexpr size_t NUM_PARALLEL_TASKS = ETHOSU_NPU_COUNT;
+#else
+constexpr size_t NUM_PARALLEL_TASKS = 1;
+#endif
+
+// TensorArena static initialisation
+constexpr size_t arenaSize = TENSOR_ARENA_SIZE / NUM_PARALLEL_TASKS;
+
+__attribute__((section(".bss.tensor_arena"), aligned(16))) uint8_t tensorArena[NUM_PARALLEL_TASKS][arenaSize];
+
+// Message queue from remote host
+__attribute__((section("ethosu_core_in_queue"))) MessageQueue::Queue<1000> inputMessageQueue;
+
+// Message queue to remote host
+__attribute__((section("ethosu_core_out_queue"))) MessageQueue::Queue<1000> outputMessageQueue;
+
+namespace {
+// Queue used to pass inference requests to the inference runner task
+QueueHandle_t inferenceQueue;
+
+// Queue for message responses to the remote host
+QueueHandle_t outputQueue;
+
+// Mailbox driver
+#ifdef MHU_V2
+Mailbox::MHUv2 mailbox(MHU_TX_BASE_ADDRESS, MHU_RX_BASE_ADDRESS); // txBase, rxBase
+#elif defined(MHU_JUNO)
+Mailbox::MHUJuno mailbox(MHU_BASE_ADDRESS);
+#else
+Mailbox::MHUDummy mailbox;
+#endif
+
+} // namespace
+
+/****************************************************************************
+ * Mutex & Semaphore
+ ****************************************************************************/
+
+extern "C" {
+
+void *ethosu_mutex_create(void) {
+    return xSemaphoreCreateMutex();
+}
+
+void ethosu_mutex_lock(void *mutex) {
+    SemaphoreHandle_t handle = reinterpret_cast<SemaphoreHandle_t>(mutex);
+    xSemaphoreTake(handle, portMAX_DELAY);
+}
+
+void ethosu_mutex_unlock(void *mutex) {
+    SemaphoreHandle_t handle = reinterpret_cast<SemaphoreHandle_t>(mutex);
+    xSemaphoreGive(handle);
+}
+
+void *ethosu_semaphore_create(void) {
+    return xSemaphoreCreateBinary();
+}
+
+void ethosu_semaphore_take(void *sem) {
+    SemaphoreHandle_t handle = reinterpret_cast<SemaphoreHandle_t>(sem);
+    xSemaphoreTake(handle, portMAX_DELAY);
+}
+
+void ethosu_semaphore_give(void *sem) {
+    SemaphoreHandle_t handle = reinterpret_cast<SemaphoreHandle_t>(sem);
+    xSemaphoreGiveFromISR(handle, NULL);
+}
+}
+
+/****************************************************************************
+ * Application
+ ****************************************************************************/
+
+namespace {
+
+void mailboxIrqHandler() {
+    mailbox.handleMessage();
+}
+
+void inferenceTask(void *pvParameters) {
+    printf("Starting inference task\n");
+
+    uint8_t *arena = reinterpret_cast<uint8_t *>(pvParameters);
+    InferenceHandler process(arena, arenaSize, inferenceQueue, outputQueue);
+    process.run();
+}
+
+void inputMessageTask(void *pvParameters) {
+    (void)pvParameters;
+
+    printf("Starting input message task\n");
+
+    IncomingMessageHandler process(*inputMessageQueue.toQueue(), mailbox, inferenceQueue, outputQueue);
+    process.run();
+}
+
+void outputMessageTask(void *pvParameters) {
+    (void)pvParameters;
+
+    printf("Starting output message task\n");
+
+    MessageHandler::OutgoingMessageHandler process(*outputMessageQueue.toQueue(), mailbox, outputQueue);
+    process.run();
+}
+
+} // namespace
+
+// FreeRTOS application. NOTE: Additional tasks may require increased heap size.
+int main() {
+    BaseType_t ret;
+
+#ifdef MHU_IRQ
+    // Register mailbox interrupt handler
+    NVIC_SetVector((IRQn_Type)MHU_IRQ, (uint32_t)&mailboxIrqHandler);
+    NVIC_EnableIRQ((IRQn_Type)MHU_IRQ);
+#endif
+
+    if (!mailbox.verifyHardware()) {
+        printf("Failed to verify mailbox hardware\n");
+        return 1;
+    }
+
+    // Create message queues for inter process communication
+    inferenceQueue = xQueueCreate(10, sizeof(ethosu_core_inference_req));
+    outputQueue    = xQueueCreate(10, sizeof(OutputMessage));
+
+    // Task for handling incoming messages from the remote host
+    ret = xTaskCreate(inputMessageTask, "inputMessageTask", 512, nullptr, 2, nullptr);
+    if (ret != pdPASS) {
+        printf("Failed to create 'inputMessageTask'\n");
+        return ret;
+    }
+
+    // Task for handling outgoing messages resposes to the remote host
+    ret = xTaskCreate(outputMessageTask, "outputMessageTask", 512, nullptr, 2, nullptr);
+    if (ret != pdPASS) {
+        printf("Failed to create 'outputMessageTask'\n");
+        return ret;
+    }
+
+    // One inference task for each NPU
+    for (size_t n = 0; n < NUM_PARALLEL_TASKS; n++) {
+        ret = xTaskCreate(inferenceTask, "inferenceTask", 8 * 1024, &tensorArena[n], 3, nullptr);
+        if (ret != pdPASS) {
+            printf("Failed to create 'inferenceTask%d'\n", n);
+            return ret;
+        }
+    }
+
+    // Start Scheduler
+    vTaskStartScheduler();
+
+    return 1;
+}
diff --git a/applications/message_handler/message_handler.cpp b/applications/message_handler/message_handler.cpp
new file mode 100644
index 0000000..7401546
--- /dev/null
+++ b/applications/message_handler/message_handler.cpp
@@ -0,0 +1,406 @@
+/*
+ * Copyright (c) 2020-2021 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "message_handler.hpp"
+
+#include "cmsis_compiler.h"
+
+#ifdef ETHOSU
+#include <ethosu_driver.h>
+#endif
+
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "semphr.h"
+
+#include <cstring>
+#include <inttypes.h>
+
+using namespace EthosU;
+using namespace MessageQueue;
+
+namespace MessageHandler {
+
+/****************************************************************************
+ * IncomingMessageHandler
+ ****************************************************************************/
+
+IncomingMessageHandler::IncomingMessageHandler(ethosu_core_queue &_messageQueue,
+                                               Mailbox::Mailbox &_mailbox,
+                                               QueueHandle_t _inferenceQueue,
+                                               QueueHandle_t _outputQueue) :
+    messageQueue(_messageQueue),
+    mailbox(_mailbox), inferenceQueue(_inferenceQueue), outputQueue(_outputQueue) {
+    mailbox.registerCallback(handleIrq, reinterpret_cast<void *>(this));
+    semaphore = xSemaphoreCreateBinary();
+}
+
+void IncomingMessageHandler::run() {
+    while (true) {
+        // Wait for event
+        xSemaphoreTake(semaphore, portMAX_DELAY);
+
+        // Handle all messages in queue
+        while (handleMessage()) {}
+    }
+}
+
+void IncomingMessageHandler::handleIrq(void *userArg) {
+    IncomingMessageHandler *_this = reinterpret_cast<IncomingMessageHandler *>(userArg);
+    xSemaphoreGive(_this->semaphore);
+}
+
+void IncomingMessageHandler::queueErrorAndResetQueue(ethosu_core_msg_err_type type, const char *message) {
+    OutputMessage msg(ETHOSU_CORE_MSG_ERR);
+    msg.data.error.type = type;
+
+    for (size_t i = 0; i < sizeof(msg.data.error.msg) && message[i]; i++) {
+        msg.data.error.msg[i] = message[i];
+    }
+
+    xQueueSend(outputQueue, &msg, portMAX_DELAY);
+    messageQueue.reset();
+}
+
+bool IncomingMessageHandler::handleMessage() {
+    struct ethosu_core_msg msg;
+
+    if (messageQueue.available() == 0) {
+        return false;
+    }
+
+    // Read msg header
+    // Only process a complete message header, else send error message
+    // and reset queue
+    if (!messageQueue.read(msg)) {
+        queueErrorAndResetQueue(ETHOSU_CORE_MSG_ERR_INVALID_SIZE, "Failed to read a complete header");
+        return false;
+    }
+
+    printf("Msg: header magic=%" PRIX32 ", type=%" PRIu32 ", length=%" PRIu32 "\n", msg.magic, msg.type, msg.length);
+
+    if (msg.magic != ETHOSU_CORE_MSG_MAGIC) {
+        printf("Msg: Invalid Magic\n");
+        queueErrorAndResetQueue(ETHOSU_CORE_MSG_ERR_INVALID_MAGIC, "Invalid magic");
+        return false;
+    }
+
+    switch (msg.type) {
+    case ETHOSU_CORE_MSG_PING: {
+        printf("Msg: Ping\n");
+
+        OutputMessage message(ETHOSU_CORE_MSG_PONG);
+        xQueueSend(outputQueue, &message, portMAX_DELAY);
+        break;
+    }
+    case ETHOSU_CORE_MSG_ERR: {
+        ethosu_core_msg_err error;
+
+        if (!messageQueue.read(error)) {
+            printf("ERROR: Msg: Failed to receive error message\n");
+        } else {
+            printf("Msg: Received an error response, type=%" PRIu32 ", msg=\"%s\"\n", error.type, error.msg);
+        }
+
+        messageQueue.reset();
+        return false;
+    }
+    case ETHOSU_CORE_MSG_VERSION_REQ: {
+        printf("Msg: Version request\n");
+
+        OutputMessage message(ETHOSU_CORE_MSG_VERSION_RSP);
+        xQueueSend(outputQueue, &message, portMAX_DELAY);
+        break;
+    }
+    case ETHOSU_CORE_MSG_CAPABILITIES_REQ: {
+        ethosu_core_capabilities_req capabilities;
+
+        if (!messageQueue.read(capabilities)) {
+            queueErrorAndResetQueue(ETHOSU_CORE_MSG_ERR_INVALID_PAYLOAD, "CapabilitiesReq. Failed to read payload");
+            break;
+        }
+
+        printf("Msg: Capabilities request.user_arg=0x%" PRIx64 "\n", capabilities.user_arg);
+
+        OutputMessage message(ETHOSU_CORE_MSG_CAPABILITIES_RSP);
+        message.data.userArg = capabilities.user_arg;
+        xQueueSend(outputQueue, &message, portMAX_DELAY);
+        break;
+    }
+    case ETHOSU_CORE_MSG_INFERENCE_REQ: {
+        ethosu_core_inference_req inference;
+
+        if (!messageQueue.read(inference)) {
+            queueErrorAndResetQueue(ETHOSU_CORE_MSG_ERR_INVALID_PAYLOAD, "InferenceReq. Failed to read payload");
+            break;
+        }
+
+        printf("Msg: InferenceReq. user_arg=0x%" PRIx64 ", network={0x%" PRIx32 ", %" PRIu32 "}\n",
+               inference.user_arg,
+               inference.network.ptr,
+               inference.network.size);
+
+        printf(", ifm_count=%" PRIu32 ", ifm=[", inference.ifm_count);
+        for (uint32_t i = 0; i < inference.ifm_count; ++i) {
+            if (i > 0) {
+                printf(", ");
+            }
+
+            printf("{0x%" PRIx32 ", %" PRIu32 "}", inference.ifm[i].ptr, inference.ifm[i].size);
+        }
+        printf("]");
+
+        printf(", ofm_count=%" PRIu32 ", ofm=[", inference.ofm_count);
+        for (uint32_t i = 0; i < inference.ofm_count; ++i) {
+            if (i > 0) {
+                printf(", ");
+            }
+
+            printf("{0x%" PRIx32 ", %" PRIu32 "}", inference.ofm[i].ptr, inference.ofm[i].size);
+        }
+        printf("]\n");
+
+        xQueueSend(inferenceQueue, &inference, portMAX_DELAY);
+        break;
+    }
+    default: {
+        char errMsg[128];
+
+        snprintf(&errMsg[0],
+                 sizeof(errMsg),
+                 "Msg: Unknown type: %" PRIu32 " with payload length %" PRIu32 " bytes\n",
+                 msg.type,
+                 msg.length);
+
+        queueErrorAndResetQueue(ETHOSU_CORE_MSG_ERR_UNSUPPORTED_TYPE, errMsg);
+
+        return false;
+    }
+    }
+
+    return true;
+}
+
+/****************************************************************************
+ * InferenceHandler
+ ****************************************************************************/
+
+InferenceHandler::InferenceHandler(uint8_t *tensorArena,
+                                   size_t arenaSize,
+                                   QueueHandle_t _inferenceQueue,
+                                   QueueHandle_t _outputQueue) :
+    inferenceQueue(_inferenceQueue),
+    outputQueue(_outputQueue), inference(tensorArena, arenaSize) {}
+
+void InferenceHandler::run() {
+    while (true) {
+        ethosu_core_inference_req req;
+
+        if (pdTRUE != xQueueReceive(inferenceQueue, &req, portMAX_DELAY)) {
+            continue;
+        }
+
+        OutputMessage msg(ETHOSU_CORE_MSG_INFERENCE_RSP);
+        runInference(req, msg.data.inference);
+
+        xQueueSend(outputQueue, &msg, portMAX_DELAY);
+    }
+}
+
+void InferenceHandler::runInference(ethosu_core_inference_req &req, ethosu_core_inference_rsp &rsp) {
+    /*
+     * Setup inference job
+     */
+
+    InferenceProcess::DataPtr networkModel(reinterpret_cast<void *>(req.network.ptr), req.network.size);
+
+    std::vector<InferenceProcess::DataPtr> ifm;
+    for (uint32_t i = 0; i < req.ifm_count; ++i) {
+        ifm.push_back(InferenceProcess::DataPtr(reinterpret_cast<void *>(req.ifm[i].ptr), req.ifm[i].size));
+    }
+
+    std::vector<InferenceProcess::DataPtr> ofm;
+    for (uint32_t i = 0; i < req.ofm_count; ++i) {
+        ofm.push_back(InferenceProcess::DataPtr(reinterpret_cast<void *>(req.ofm[i].ptr), req.ofm[i].size));
+    }
+
+    std::vector<InferenceProcess::DataPtr> expectedOutput;
+
+    std::vector<uint8_t> pmuEventConfig(ETHOSU_CORE_PMU_MAX);
+    for (uint32_t i = 0; i < ETHOSU_CORE_PMU_MAX; i++) {
+        pmuEventConfig[i] = req.pmu_event_config[i];
+    }
+
+    InferenceProcess::InferenceJob job(
+        "job", networkModel, ifm, ofm, expectedOutput, -1, pmuEventConfig, req.pmu_cycle_counter_enable);
+
+    /*
+     * Run inference
+     */
+
+    job.invalidate();
+    bool failed = inference.runJob(job);
+    job.clean();
+
+    /*
+     * Send inference response
+     */
+
+    rsp.user_arg  = req.user_arg;
+    rsp.ofm_count = job.output.size();
+    rsp.status    = failed ? ETHOSU_CORE_STATUS_ERROR : ETHOSU_CORE_STATUS_OK;
+
+    for (size_t i = 0; i < job.output.size(); ++i) {
+        rsp.ofm_size[i] = job.output[i].size;
+    }
+
+    for (size_t i = 0; i < job.pmuEventConfig.size(); i++) {
+        rsp.pmu_event_config[i] = job.pmuEventConfig[i];
+    }
+
+    for (size_t i = 0; i < job.pmuEventCount.size(); i++) {
+        rsp.pmu_event_count[i] = job.pmuEventCount[i];
+    }
+
+    rsp.pmu_cycle_counter_enable = job.pmuCycleCounterEnable;
+    rsp.pmu_cycle_counter_count  = job.pmuCycleCounterCount;
+}
+
+/****************************************************************************
+ * OutgoingMessageHandler
+ ****************************************************************************/
+
+OutgoingMessageHandler::OutgoingMessageHandler(ethosu_core_queue &_messageQueue,
+                                               Mailbox::Mailbox &_mailbox,
+                                               QueueHandle_t _outputQueue) :
+    messageQueue(_messageQueue),
+    mailbox(_mailbox), outputQueue(_outputQueue) {
+    readCapabilties(capabilities);
+}
+
+void OutgoingMessageHandler::run() {
+    while (true) {
+        OutputMessage message;
+        if (pdTRUE != xQueueReceive(outputQueue, &message, portMAX_DELAY)) {
+            continue;
+        }
+
+        switch (message.type) {
+        case ETHOSU_CORE_MSG_INFERENCE_RSP:
+            sendInferenceRsp(message.data.inference);
+            break;
+        case ETHOSU_CORE_MSG_CAPABILITIES_RSP:
+            sendCapabilitiesRsp(message.data.userArg);
+            break;
+        case ETHOSU_CORE_MSG_VERSION_RSP:
+            sendVersionRsp();
+            break;
+        case ETHOSU_CORE_MSG_PONG:
+            sendPong();
+            break;
+        case ETHOSU_CORE_MSG_ERR:
+            sendErrorRsp(message.data.error);
+            break;
+        default:
+            printf("Dropping unknown outcome of type %d\n", message.type);
+            break;
+        }
+    }
+}
+
+void OutgoingMessageHandler::sendPong() {
+    if (!messageQueue.write(ETHOSU_CORE_MSG_PONG)) {
+        printf("ERROR: Msg: Failed to write pong response. No mailbox message sent\n");
+    } else {
+        mailbox.sendMessage();
+    }
+}
+
+void OutgoingMessageHandler::sendVersionRsp() {
+    ethosu_core_msg_version version = {
+        ETHOSU_CORE_MSG_VERSION_MAJOR,
+        ETHOSU_CORE_MSG_VERSION_MINOR,
+        ETHOSU_CORE_MSG_VERSION_PATCH,
+        0,
+    };
+
+    if (!messageQueue.write(ETHOSU_CORE_MSG_VERSION_RSP, version)) {
+        printf("ERROR: Failed to write version response. No mailbox message sent\n");
+    } else {
+        mailbox.sendMessage();
+    }
+}
+
+void OutgoingMessageHandler::sendCapabilitiesRsp(uint64_t userArg) {
+    capabilities.user_arg = userArg;
+
+    if (!messageQueue.write(ETHOSU_CORE_MSG_CAPABILITIES_RSP, capabilities)) {
+        printf("ERROR: Failed to write capabilities response. No mailbox message sent\n");
+    } else {
+        mailbox.sendMessage();
+    }
+}
+
+void OutgoingMessageHandler::sendInferenceRsp(ethosu_core_inference_rsp &inference) {
+    if (!messageQueue.write(ETHOSU_CORE_MSG_INFERENCE_RSP, inference)) {
+        printf("ERROR: Msg: Failed to write inference response. No mailbox message sent\n");
+    } else {
+        mailbox.sendMessage();
+    }
+}
+
+void OutgoingMessageHandler::sendErrorRsp(ethosu_core_msg_err &error) {
+    printf("ERROR: Msg: \"%s\"\n", error.msg);
+
+    if (!messageQueue.write(ETHOSU_CORE_MSG_ERR, error)) {
+        printf("ERROR: Msg: Failed to write error response. No mailbox message sent\n");
+    } else {
+        mailbox.sendMessage();
+    }
+}
+
+void OutgoingMessageHandler::readCapabilties(ethosu_core_msg_capabilities_rsp &rsp) {
+    rsp = {0};
+
+#ifdef ETHOSU
+    struct ethosu_driver_version version;
+    ethosu_get_driver_version(&version);
+
+    struct ethosu_hw_info info;
+    struct ethosu_driver *drv = ethosu_reserve_driver();
+    ethosu_get_hw_info(drv, &info);
+    ethosu_release_driver(drv);
+
+    rsp.user_arg           = 0;
+    rsp.version_status     = info.version.version_status;
+    rsp.version_minor      = info.version.version_minor;
+    rsp.version_major      = info.version.version_major;
+    rsp.product_major      = info.version.product_major;
+    rsp.arch_patch_rev     = info.version.arch_patch_rev;
+    rsp.arch_minor_rev     = info.version.arch_minor_rev;
+    rsp.arch_major_rev     = info.version.arch_major_rev;
+    rsp.driver_patch_rev   = version.patch;
+    rsp.driver_minor_rev   = version.minor;
+    rsp.driver_major_rev   = version.major;
+    rsp.macs_per_cc        = info.cfg.macs_per_cc;
+    rsp.cmd_stream_version = info.cfg.cmd_stream_version;
+    rsp.custom_dma         = info.cfg.custom_dma;
+#endif
+}
+
+} // namespace MessageHandler
diff --git a/applications/message_handler/message_handler.hpp b/applications/message_handler/message_handler.hpp
new file mode 100644
index 0000000..5a195f0
--- /dev/null
+++ b/applications/message_handler/message_handler.hpp
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2020-2021 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MESSAGE_HANDLER_H
+#define MESSAGE_HANDLER_H
+
+#include "FreeRTOS.h"
+#include "queue.h"
+#include "semphr.h"
+
+#include "message_queue.hpp"
+#include <inference_process.hpp>
+#include <mailbox.hpp>
+
+#include <cstddef>
+#include <cstdio>
+#include <vector>
+
+namespace MessageHandler {
+
+class IncomingMessageHandler {
+public:
+    IncomingMessageHandler(EthosU::ethosu_core_queue &messageQueue,
+                           Mailbox::Mailbox &mailbox,
+                           QueueHandle_t inferenceQueue,
+                           QueueHandle_t outputQueue);
+    void run();
+
+private:
+    bool handleMessage();
+    void queueErrorAndResetQueue(EthosU::ethosu_core_msg_err_type type, const char *message);
+    static void handleIrq(void *userArg);
+
+    MessageQueue::QueueImpl messageQueue;
+    Mailbox::Mailbox &mailbox;
+    QueueHandle_t inferenceQueue;
+    QueueHandle_t outputQueue;
+    SemaphoreHandle_t semaphore;
+};
+
+class InferenceHandler {
+public:
+    InferenceHandler(uint8_t *tensorArena, size_t arenaSize, QueueHandle_t inferenceQueue, QueueHandle_t outputQueue);
+
+    void run();
+
+private:
+    void runInference(EthosU::ethosu_core_inference_req &req, EthosU::ethosu_core_inference_rsp &rsp);
+
+    QueueHandle_t inferenceQueue;
+    QueueHandle_t outputQueue;
+    InferenceProcess::InferenceProcess inference;
+};
+
+struct OutputMessage {
+    OutputMessage(EthosU::ethosu_core_msg_type _type = EthosU::ETHOSU_CORE_MSG_MAX) : type(_type) {}
+
+    EthosU::ethosu_core_msg_type type;
+    union {
+        EthosU::ethosu_core_inference_rsp inference;
+        EthosU::ethosu_core_msg_err error;
+        uint64_t userArg;
+    } data;
+};
+
+class OutgoingMessageHandler {
+public:
+    OutgoingMessageHandler(EthosU::ethosu_core_queue &messageQueue,
+                           Mailbox::Mailbox &mailbox,
+                           QueueHandle_t outputQueue);
+    void run();
+
+private:
+    void sendPong();
+    void sendErrorRsp(EthosU::ethosu_core_msg_err &error);
+    void sendVersionRsp();
+    void sendCapabilitiesRsp(uint64_t userArg);
+    void sendInferenceRsp(EthosU::ethosu_core_inference_rsp &inference);
+    void readCapabilties(EthosU::ethosu_core_msg_capabilities_rsp &rsp);
+
+    MessageQueue::QueueImpl messageQueue;
+    Mailbox::Mailbox &mailbox;
+    QueueHandle_t outputQueue;
+    EthosU::ethosu_core_msg_capabilities_rsp capabilities;
+};
+
+} // namespace MessageHandler
+
+#endif
diff --git a/applications/message_handler/message_queue.cpp b/applications/message_handler/message_queue.cpp
new file mode 100644
index 0000000..e896349
--- /dev/null
+++ b/applications/message_handler/message_queue.cpp
@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2020-2021 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "message_queue.hpp"
+
+#include <cstddef>
+#include <cstdio>
+#include <cstring>
+#include <inttypes.h>
+
+namespace MessageQueue {
+
+QueueImpl::QueueImpl(EthosU::ethosu_core_queue &_queue) : queue(_queue) {
+    cleanHeaderData();
+}
+
+bool QueueImpl::empty() const {
+    invalidateHeaderData();
+
+    return queue.header.read == queue.header.write;
+}
+
+size_t QueueImpl::available() const {
+    invalidateHeaderData();
+
+    size_t avail = queue.header.write - queue.header.read;
+
+    if (queue.header.read > queue.header.write) {
+        avail += queue.header.size;
+    }
+
+    return avail;
+}
+
+size_t QueueImpl::capacity() const {
+    return queue.header.size - available();
+}
+
+bool QueueImpl::read(uint8_t *dst, uint32_t length) {
+    const uint8_t *end = dst + length;
+
+    // Available will invalidate the cache
+    if (length > available()) {
+        return false;
+    }
+
+    uint32_t rpos = queue.header.read;
+
+    while (dst < end) {
+        *dst++ = queue.data[rpos];
+        rpos   = (rpos + 1) % queue.header.size;
+    }
+
+    queue.header.read = rpos;
+
+    cleanHeader();
+
+    return true;
+}
+
+bool QueueImpl::write(const Vec *vec, size_t length) {
+    size_t total = 0;
+
+    for (size_t i = 0; i < length; i++) {
+        total += vec[i].length;
+    }
+
+    invalidateHeader();
+
+    if (total > capacity()) {
+        return false;
+    }
+
+    uint32_t wpos = queue.header.write;
+
+    for (size_t i = 0; i < length; i++) {
+        const uint8_t *src = reinterpret_cast<const uint8_t *>(vec[i].base);
+        const uint8_t *end = src + vec[i].length;
+
+        while (src < end) {
+            queue.data[wpos] = *src++;
+            wpos             = (wpos + 1) % queue.header.size;
+        }
+    }
+
+    // Update the write position last
+    queue.header.write = wpos;
+
+    cleanHeaderData();
+
+    return true;
+}
+
+bool QueueImpl::write(const uint32_t type, const void *src, uint32_t length) {
+    EthosU::ethosu_core_msg msg = {ETHOSU_CORE_MSG_MAGIC, type, length};
+    Vec vec[2]                  = {{&msg, sizeof(msg)}, {src, length}};
+
+    return write(vec, 2);
+}
+
+// Skip to magic or end of queue
+void QueueImpl::reset() {
+    invalidateHeader();
+    queue.header.read = queue.header.write;
+    cleanHeader();
+}
+
+void QueueImpl::cleanHeader() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+#endif
+}
+
+void QueueImpl::cleanHeaderData() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+    uintptr_t queueDataPtr = reinterpret_cast<uintptr_t>(&queue.data[0]);
+    SCB_CleanDCache_by_Addr(reinterpret_cast<uint32_t *>(queueDataPtr & ~3), queue.header.size + (queueDataPtr & 3));
+#endif
+}
+
+void QueueImpl::invalidateHeader() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+#endif
+}
+
+void QueueImpl::invalidateHeaderData() const {
+#if defined(__DCACHE_PRESENT) && (__DCACHE_PRESENT == 1U)
+    SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t *>(&queue.header), sizeof(queue.header));
+    uintptr_t queueDataPtr = reinterpret_cast<uintptr_t>(&queue.data[0]);
+    SCB_InvalidateDCache_by_Addr(reinterpret_cast<uint32_t *>(queueDataPtr & ~3),
+                                 queue.header.size + (queueDataPtr & 3));
+#endif
+}
+} // namespace MessageQueue
diff --git a/applications/message_handler/message_queue.hpp b/applications/message_handler/message_queue.hpp
new file mode 100644
index 0000000..7c59e75
--- /dev/null
+++ b/applications/message_handler/message_queue.hpp
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2020-2021 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MESSAGE_QUEUE_H
+#define MESSAGE_QUEUE_H
+
+#include <cstddef>
+#include <ethosu_core_interface.h>
+
+namespace MessageQueue {
+
+template <uint32_t SIZE>
+struct Queue {
+    EthosU::ethosu_core_queue_header header;
+    uint8_t data[SIZE];
+
+    constexpr Queue() : header({SIZE, 0, 0}) {}
+
+    constexpr EthosU::ethosu_core_queue *toQueue() {
+        return reinterpret_cast<EthosU::ethosu_core_queue *>(&header);
+    }
+};
+
+class QueueImpl {
+public:
+    struct Vec {
+        const void *base;
+        size_t length;
+    };
+
+    QueueImpl(EthosU::ethosu_core_queue &queue);
+
+    bool empty() const;
+    size_t available() const;
+    size_t capacity() const;
+    void reset();
+    bool read(uint8_t *dst, uint32_t length);
+    template <typename T>
+    bool read(T &dst) {
+        return read(reinterpret_cast<uint8_t *>(&dst), sizeof(dst));
+    }
+    bool write(const Vec *vec, size_t length);
+    bool write(const uint32_t type, const void *src = nullptr, uint32_t length = 0);
+    template <typename T>
+    bool write(const uint32_t type, const T &src) {
+        return write(type, reinterpret_cast<const void *>(&src), sizeof(src));
+    }
+
+private:
+    void cleanHeader() const;
+    void cleanHeaderData() const;
+    void invalidateHeader() const;
+    void invalidateHeaderData() const;
+
+    EthosU::ethosu_core_queue &queue;
+};
+} // namespace MessageQueue
+
+#endif