diff --git a/.github/workflows/c-cpp.yml b/.github/workflows/c-cpp.yml
index 10c8857..4d7e08f 100644
--- a/.github/workflows/c-cpp.yml
+++ b/.github/workflows/c-cpp.yml
@@ -21,8 +21,8 @@ jobs:
     - name: make proto example
       run: make
       working-directory: example/linux/proto-api
-    - name: install libpaho-mqtt
-      run: sudo apt install libpaho-mqtt-dev
+    - name: install needed packages libpaho-mqtt
+      run: sudo apt-get update && sudo apt-get install -y libpaho-mqtt-dev libgtest-dev
     - name: make gateway example
       run: make
       working-directory: example/linux/gw-example
diff --git a/README.md b/README.md
index e2a8f64..80a2faf 100644
--- a/README.md
+++ b/README.md
@@ -22,6 +22,9 @@ An example on how to use and extend the library is available from:
 To run the tests you will to connect a device running WM's dual mcu api on
 your host machine.
 
+Tests require Google Test. On Debian and Ununtu, you can install the package
+libgtest-dev.
+
 Build the tests with
 
 ```shell
@@ -29,10 +32,11 @@ Build the tests with
     make
 ```
 
-Execute the test suite with
+When executing the tests, environment variables WPC_SERIAL_PORT and
+WPC_WPC_BAUD_RATE should be set. For example:
 
 ```shell
-    ./build/meshAPItest
+    WPC_SERIAL_PORT=/dev/ttyACM0 WPC_BAUD_RATE=125000 ./build/meshAPItest
 ```
 
 ## Contributing
diff --git a/test/callback_tests.cpp b/test/callback_tests.cpp
new file mode 100644
index 0000000..cb83fb2
--- /dev/null
+++ b/test/callback_tests.cpp
@@ -0,0 +1,433 @@
+#include "wpc_test.hpp"
+
+#include <cstring>
+#include <mutex>
+#include <chrono>
+#include <thread>
+
+class WpcCallbackTest : public WpcTest
+{
+public:
+    static void SetUpTestSuite()
+    {
+        ASSERT_NO_FATAL_FAILURE(WpcTest::SetUpTestSuite());
+
+        // Following parameters should be set in order to be able to start the
+        // stack, and stack should be stopped in order to be able to set them.
+        ASSERT_NO_FATAL_FAILURE(WpcTest::StopStack());
+        ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+        ASSERT_EQ(APP_RES_OK, WPC_set_node_address(2345));
+        ASSERT_EQ(APP_RES_OK, WPC_set_network_address(0x65432));
+        ASSERT_EQ(APP_RES_OK, WPC_set_network_channel(38));
+
+        // Set diagnostics interval to zero, so that data callbacks are not called.
+        const uint8_t TEST_CONFIG[] = { 0 };
+        ASSERT_EQ(APP_RES_OK, WPC_set_app_config_data(1, 0, TEST_CONFIG, sizeof(TEST_CONFIG)));
+
+        ASSERT_EQ(APP_RES_OK, WPC_start_stack());
+
+        // Wait a bit for possible earlier status messages to be ignored
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    }
+
+protected:
+    struct BaseCallbackHandler
+    {
+        void WaitForCallback(size_t max_seconds = 5)
+        {
+            for (size_t i = 0; i < 20 * max_seconds; i++) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(50));
+                std::lock_guard<std::mutex> lock(mutex);
+                if (callback_called) {
+                    return;
+                }
+            }
+
+            ADD_FAILURE() << "Callback was not called";
+        }
+
+        std::mutex mutex;
+        bool callback_called;
+    };
+
+    struct StackStatusCb : public BaseCallbackHandler
+    {
+        void reset()
+        {
+            std::lock_guard<std::mutex> lock(mutex);
+            callback_called = false;
+            status = 0;
+        }
+
+        uint8_t status;
+    };
+
+    struct ScanNeighborsCb : public BaseCallbackHandler
+    {
+        void reset()
+        {
+            std::lock_guard<std::mutex> lock(mutex);
+            callback_called = false;
+            status = 0;
+        }
+
+        uint8_t status;
+    };
+
+    struct AppConfigCb : public BaseCallbackHandler
+    {
+        void reset()
+        {
+            std::lock_guard<std::mutex> lock(mutex);
+            callback_called = false;
+            interval = 0;
+            std::memset(config, 0, sizeof(config));
+            app_config_size = 0;
+        }
+
+        uint16_t interval;
+        uint8_t config[UINT8_MAX];
+
+        uint8_t app_config_size;
+    };
+
+    struct DataSentCb : public BaseCallbackHandler
+    {
+        void reset()
+        {
+            std::lock_guard<std::mutex> lock(mutex);
+            callback_called = false;
+            pduid = 0;
+            result = 0;
+        }
+
+        uint16_t pduid;
+        uint8_t result;
+    };
+
+    struct DataReceivedCb : public BaseCallbackHandler
+    {
+        void reset()
+        {
+            std::lock_guard<std::mutex> lock(mutex);
+            callback_called = false;
+            bytes.clear();
+            qos = APP_QOS_NORMAL;
+            src_addr = 0;
+            dst_addr = 0;
+            expected_src_ep = 0;
+            expected_dst_ep = 0;
+        }
+
+        std::vector<uint8_t> bytes;
+        app_qos_e qos;
+        app_addr_t src_addr;
+        app_addr_t dst_addr;
+
+        uint8_t expected_src_ep;
+        uint8_t expected_dst_ep;
+    };
+
+    struct ConfigDataItemCb : public BaseCallbackHandler
+    {
+        void reset()
+        {
+            std::lock_guard<std::mutex> lock(mutex);
+            callback_called = false;
+            payload.clear();
+            expected_endpoint = 0;
+        }
+
+        std::vector<uint8_t> payload;
+
+        uint16_t expected_endpoint;
+    };
+
+    inline static StackStatusCb stack_status_cb;
+    inline static ScanNeighborsCb scan_neighbors_cb;
+    inline static AppConfigCb app_config_cb;
+    inline static DataSentCb data_sent_cb;
+    inline static DataReceivedCb data_received_cb;
+    inline static ConfigDataItemCb config_data_item_cb;
+
+    static void onStackStatusReceived(uint8_t status)
+    {
+        std::lock_guard<std::mutex> lock(stack_status_cb.mutex);
+        stack_status_cb.callback_called = true;
+
+        stack_status_cb.status = status;
+    }
+
+    static void onScanNeighborsDone(uint8_t status)
+    {
+        std::lock_guard<std::mutex> lock(scan_neighbors_cb.mutex);
+        scan_neighbors_cb.callback_called = true;
+
+        scan_neighbors_cb.status = status;
+    }
+
+    static void onAppConfigDataReceived(uint8_t seq, uint16_t interval, uint8_t* config)
+    {
+        std::lock_guard<std::mutex> lock(app_config_cb.mutex);
+        app_config_cb.callback_called = true;
+
+        app_config_cb.interval = interval;
+        std::memcpy(app_config_cb.config, config, app_config_cb.app_config_size);
+    }
+
+    static void onDataSent(uint16_t pduid, uint32_t buffering_delay, uint8_t result)
+    {
+        std::lock_guard<std::mutex> lock(data_sent_cb.mutex);
+        data_sent_cb.callback_called = true;
+
+        data_sent_cb.pduid = pduid;
+        data_sent_cb.result = result;
+    }
+
+    static bool onDataReceived(const uint8_t* bytes,
+                               size_t num_bytes,
+                               app_addr_t src_addr,
+                               app_addr_t dst_addr,
+                               app_qos_e qos,
+                               uint8_t src_ep,
+                               uint8_t dst_ep,
+                               uint32_t travel_time,
+                               uint8_t hop_count,
+                               unsigned long long timestamp_ms_epoch)
+    {
+        if (src_ep != data_received_cb.expected_src_ep || dst_ep != data_received_cb.expected_dst_ep) {
+            return false;
+        }
+
+        std::lock_guard<std::mutex> lock(data_received_cb.mutex);
+        data_received_cb.callback_called = true;
+
+        data_received_cb.bytes.resize(num_bytes);
+        std::memcpy(data_received_cb.bytes.data(), bytes, num_bytes);
+        data_received_cb.qos = qos;
+        data_received_cb.src_addr = src_addr;
+        data_received_cb.dst_addr = dst_addr;
+
+        return true;
+    }
+
+    static void onConfigDataItemReceived(const uint16_t endpoint,
+                                         const uint8_t* const payload,
+                                         const uint8_t size)
+    {
+        if (endpoint != config_data_item_cb.expected_endpoint) {
+            return;
+        }
+
+        std::lock_guard<std::mutex> lock(config_data_item_cb.mutex);
+        config_data_item_cb.callback_called = true;
+
+        config_data_item_cb.payload.resize(size);
+        std::memcpy(config_data_item_cb.payload.data(), payload, size);
+    }
+
+    void SetUp() override
+    {
+        stack_status_cb.reset();
+        scan_neighbors_cb.reset();
+        app_config_cb.reset();
+        data_sent_cb.reset();
+        data_received_cb.reset();
+        config_data_item_cb.reset();
+    }
+
+    void TearDown() override
+    {
+        WPC_unregister_from_stack_status();
+        WPC_unregister_from_scan_neighbors_done();
+        WPC_unregister_from_app_config_data();
+        WPC_unregister_for_data();
+        WPC_unregister_from_config_data_item();
+    }
+};
+
+TEST_F(WpcCallbackTest, testStackStatus)
+{
+    // Only testing stack stopping, because in some DualMCU versions indication
+    // for stack starting is not sent.
+
+    ASSERT_EQ(APP_RES_OK, WPC_register_for_stack_status(onStackStatusReceived));
+
+    ASSERT_EQ(APP_RES_OK, WPC_stop_stack());
+
+    ASSERT_NO_FATAL_FAILURE(stack_status_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(stack_status_cb.mutex);
+        // bit 0: 0 = running, 1 = sopped
+        ASSERT_EQ(1, (stack_status_cb.status & 1));
+    }
+
+    ASSERT_EQ(APP_RES_OK, WPC_start_stack());
+}
+
+TEST_F(WpcCallbackTest, testScanNeighbors)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_register_for_scan_neighbors_done(onScanNeighborsDone));
+
+    ASSERT_EQ(APP_RES_OK, WPC_start_scan_neighbors());
+
+    ASSERT_NO_FATAL_FAILURE(scan_neighbors_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(scan_neighbors_cb.mutex);
+        // 1 = scan done
+        ASSERT_EQ(1, scan_neighbors_cb.status);
+    }
+
+    app_nbors_t neighbors_list;
+    ASSERT_EQ(APP_RES_OK, WPC_get_neighbors(&neighbors_list));
+}
+
+TEST_F(WpcCallbackTest, testAppConfigData)
+{
+    {
+        std::lock_guard<std::mutex> lock(app_config_cb.mutex);
+        ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&app_config_cb.app_config_size));
+    }
+
+    ASSERT_EQ(APP_RES_OK, WPC_register_for_app_config_data(onAppConfigDataReceived));
+
+    const uint8_t TEST_CONFIG[] = { 0xAA, 0xBB, 0xCC, 0xDD };
+    ASSERT_EQ(APP_RES_OK, WPC_set_app_config_data(1, 600, TEST_CONFIG, sizeof(TEST_CONFIG)));
+
+    ASSERT_NO_FATAL_FAILURE(app_config_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(app_config_cb.mutex);
+        ASSERT_EQ(600, app_config_cb.interval);
+        ASSERT_EQ_ARRAY(TEST_CONFIG, app_config_cb.config, sizeof(TEST_CONFIG));
+    }
+}
+
+TEST_F(WpcCallbackTest, testSendDataWithSendCallback)
+{
+    const uint8_t TEST_DATA[] = { 0x10, 0x12, 0x14, 0x16, 0x18 };
+
+    // Send data to the sink itself
+    ASSERT_EQ(APP_RES_OK,
+              WPC_send_data(TEST_DATA, sizeof(TEST_DATA), 106, APP_ADDR_ANYSINK, APP_QOS_HIGH, 50, 50, onDataSent, 0));
+
+    ASSERT_NO_FATAL_FAILURE(data_sent_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(data_sent_cb.mutex);
+        // 0 = success, 1 = failure
+        ASSERT_EQ(0, data_sent_cb.result);
+        ASSERT_EQ(106, data_sent_cb.pduid);
+    }
+}
+
+TEST_F(WpcCallbackTest, testSendDataWithReceiveCallback)
+{
+    const uint8_t TEST_SRC_EP = 51;
+    const uint8_t TEST_DST_EP = 52;
+    const uint8_t TEST_DATA[] = { 0x10, 0x12, 0x14, 0x16, 0x18 };
+    const app_qos_e TEST_QOS = APP_QOS_HIGH;
+
+    net_addr_t address;
+    ASSERT_EQ(APP_RES_OK, WPC_get_node_address(&address));
+
+    {
+        std::lock_guard<std::mutex> lock(data_received_cb.mutex);
+        data_received_cb.expected_src_ep = TEST_SRC_EP;
+        data_received_cb.expected_dst_ep = TEST_DST_EP;
+    }
+
+    ASSERT_EQ(APP_RES_OK, WPC_register_for_data(onDataReceived));
+
+
+    // Send data to the sink itself
+    ASSERT_EQ(APP_RES_OK,
+              WPC_send_data(TEST_DATA,
+                            sizeof(TEST_DATA),
+                            10,
+                            APP_ADDR_ANYSINK,
+                            TEST_QOS,
+                            TEST_SRC_EP,
+                            TEST_DST_EP,
+                            NULL,
+                            0));
+
+    ASSERT_NO_FATAL_FAILURE(data_received_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(data_received_cb.mutex);
+        ASSERT_EQ(sizeof(TEST_DATA), data_received_cb.bytes.size());
+        ASSERT_EQ_ARRAY(TEST_DATA, (uint8_t*)data_received_cb.bytes.data(), sizeof(TEST_DATA));
+        ASSERT_EQ(TEST_QOS, data_received_cb.qos);
+        ASSERT_EQ(address, data_received_cb.src_addr);
+        ASSERT_EQ(address, data_received_cb.dst_addr);
+    }
+}
+
+TEST_F(WpcCallbackTest, testSendFragmentedDataWithReceiveCallback)
+{
+    const uint8_t TEST_SRC_EP = 73;
+    const uint8_t TEST_DST_EP = 81;
+    uint8_t TEST_DATA[500] = { 0 };
+    std::memset(TEST_DATA, 0xF1, sizeof(TEST_DATA));
+    const app_qos_e TEST_QOS = APP_QOS_HIGH;
+
+    net_addr_t address;
+    ASSERT_EQ(APP_RES_OK, WPC_get_node_address(&address));
+
+    {
+        std::lock_guard<std::mutex> lock(data_received_cb.mutex);
+        data_received_cb.expected_src_ep = TEST_SRC_EP;
+        data_received_cb.expected_dst_ep = TEST_DST_EP;
+    }
+
+    ASSERT_EQ(APP_RES_OK, WPC_register_for_data(onDataReceived));
+
+    // Send data to the sink itself
+    ASSERT_EQ(APP_RES_OK,
+              WPC_send_data(TEST_DATA,
+                            sizeof(TEST_DATA),
+                            20,
+                            APP_ADDR_ANYSINK,
+                            TEST_QOS,
+                            TEST_SRC_EP,
+                            TEST_DST_EP,
+                            NULL,
+                            0));
+
+    ASSERT_NO_FATAL_FAILURE(data_received_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(data_received_cb.mutex);
+        ASSERT_EQ(sizeof(TEST_DATA), data_received_cb.bytes.size());
+        ASSERT_EQ_ARRAY(TEST_DATA, (uint8_t*)data_received_cb.bytes.data(), sizeof(TEST_DATA));
+        ASSERT_EQ(TEST_QOS, data_received_cb.qos);
+        ASSERT_EQ(address, data_received_cb.src_addr);
+        ASSERT_EQ(address, data_received_cb.dst_addr);
+    }
+}
+
+TEST_F(WpcCallbackTest, testConfigDataItemCallback)
+{
+    const uint16_t TEST_ENDPOINT = 0x4156;
+    const uint8_t TEST_DATA[] = { 0x95, 0x85, 0x75 };
+
+    {
+        std::lock_guard<std::mutex> lock(data_received_cb.mutex);
+        config_data_item_cb.expected_endpoint = TEST_ENDPOINT;
+    }
+
+    ASSERT_EQ(APP_RES_OK, WPC_register_for_config_data_item(onConfigDataItemReceived));
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_ENDPOINT, TEST_DATA, sizeof(TEST_DATA)));
+
+    ASSERT_NO_FATAL_FAILURE(config_data_item_cb.WaitForCallback());
+
+    {
+        std::lock_guard<std::mutex> lock(config_data_item_cb.mutex);
+        ASSERT_EQ(sizeof(TEST_DATA), config_data_item_cb.payload.size());
+        ASSERT_EQ_ARRAY(TEST_DATA, (uint8_t*)config_data_item_cb.payload.data(), sizeof(TEST_DATA));
+    }
+}
diff --git a/test/cdd_tests.cpp b/test/cdd_tests.cpp
new file mode 100644
index 0000000..9f84ffa
--- /dev/null
+++ b/test/cdd_tests.cpp
@@ -0,0 +1,315 @@
+#include "wpc_test.hpp"
+#include <gtest/gtest.h>
+
+class WpcCddApiTest : public WpcTest
+{
+public:
+    static void SetUpTestSuite()
+    {
+        ASSERT_NO_FATAL_FAILURE(WpcTest::SetUpTestSuite());
+
+        // Stack should be stopped for factory reset, and it's not needed to be
+        // started for these tests.
+        ASSERT_NO_FATAL_FAILURE(WpcTest::StopStack());
+        ASSERT_EQ(APP_RES_OK, WPC_do_factory_reset());
+    }
+
+protected:
+    void SetUp() override
+    {
+        ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+    }
+
+    void TearDown() override
+    {
+        ASSERT_EQ(APP_RES_OK, WPC_do_factory_reset());
+    }
+
+    struct __attribute__((__packed__)) CddScratchpadDataItem
+    {
+        uint8_t seq;
+        uint16_t crc;
+        uint8_t action;
+        uint8_t param;
+    };
+
+#define STACK_PROFILE ISM_24
+#if STACK_PROFILE == ISM_24
+    const static uint16_t CDD_DIAG_INTERVAL_EP   = 0xF7FF;
+    const static uint16_t CDD_APP_CONFIG_EP      = 0xF9FF;
+    const static uint16_t CDD_SCRATCHPAD_DATA_EP = 0xFAFA;
+#elif STACK_PROFILE == DECT_NR
+    const static uint16_t CDD_DIAG_INTERVAL_EP   = 0xA029;
+    const static uint16_t CDD_APP_CONFIG_EP      = 0x4004;
+    const static uint16_t CDD_SCRATCHPAD_DATA_EP = 0x4002;
+#endif
+};
+
+
+TEST_F(WpcCddApiTest, testReadingAppConfigAndDiagWithCddApi)
+{
+    const uint8_t TEST_CONFIG[]               = { 0x11, 0x22, 0x33, 0x44, 0x55 };
+    const uint16_t TEST_DIAG_INTERVAL_SECONDS = 600;
+    const uint8_t EXPECTED_RAW_DIAG_INTERVAL  = 12;
+
+    ASSERT_EQ(APP_RES_OK,
+              WPC_set_app_config_data(1, TEST_DIAG_INTERVAL_SECONDS, TEST_CONFIG, sizeof(TEST_CONFIG)));
+
+    uint8_t app_config_size = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&app_config_size));
+    ASSERT_GT(app_config_size, 0);
+
+    {
+        uint8_t payload[UINT8_MAX] = { 0 };
+        uint8_t payload_size = 0;
+        ASSERT_EQ(APP_RES_OK, WPC_get_config_data_item(CDD_APP_CONFIG_EP, payload, sizeof(payload), &payload_size));
+        ASSERT_EQ(app_config_size, payload_size);
+        ASSERT_EQ_ARRAY(TEST_CONFIG, payload, sizeof(TEST_CONFIG));
+    }
+
+    {
+        uint8_t payload[UINT8_MAX] = { 0 };
+        uint8_t payload_size = 0;
+        ASSERT_EQ(APP_RES_OK, WPC_get_config_data_item(CDD_DIAG_INTERVAL_EP, payload, sizeof(payload), &payload_size));
+        ASSERT_EQ(1, payload_size);
+        ASSERT_EQ(EXPECTED_RAW_DIAG_INTERVAL, payload[0]);
+    }
+}
+
+TEST_F(WpcCddApiTest, testWritingAppConfigAndDiagWithCddApi)
+{
+    uint8_t app_config_size = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&app_config_size));
+    ASSERT_GT(app_config_size, 0);
+
+    const uint8_t TEST_CONFIG[UINT8_MAX] = { 0xDD, 0xAB, 0xCD, 0x99, 0x42 };
+    ASSERT_GE(sizeof(TEST_CONFIG), app_config_size);
+    const uint8_t RAW_DIAG_INTERVAL = 10;
+    const uint16_t EXPECTED_DIAG_INTERVAL_SECONDS = 120;
+
+    const uint8_t PAYLOAD[] = { RAW_DIAG_INTERVAL };
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(CDD_DIAG_INTERVAL_EP, PAYLOAD, sizeof(PAYLOAD)));
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(CDD_APP_CONFIG_EP, TEST_CONFIG, app_config_size));
+
+    uint8_t seq = 0;
+    uint16_t interval = 0;
+    uint8_t config[UINT8_MAX] = { 0 };
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data(&seq, &interval, config, sizeof(config)));
+    ASSERT_EQ(EXPECTED_DIAG_INTERVAL_SECONDS, interval);
+    ASSERT_EQ_ARRAY(TEST_CONFIG, config, app_config_size);
+}
+
+TEST_F(WpcCddApiTest, testReadingScratchpatTargetWithCddApi)
+{
+    const uint8_t TEST_SEQ    = 53;
+    const uint16_t TEST_CRC   = 0xABCD;
+    const uint8_t TEST_ACTION = 3;
+    const uint8_t TEST_PARAM  = 0x4A;
+
+    ASSERT_EQ(APP_RES_OK, WPC_write_target_scratchpad(TEST_SEQ, TEST_CRC, TEST_ACTION, TEST_PARAM));
+
+    uint8_t payload[255] = {0};
+    uint8_t payload_size = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_get_config_data_item(CDD_SCRATCHPAD_DATA_EP, payload, sizeof(payload), &payload_size));
+    ASSERT_EQ(sizeof(CddScratchpadDataItem), payload_size);
+    CddScratchpadDataItem* scratchpad_data = (CddScratchpadDataItem*) payload;
+    ASSERT_EQ(TEST_SEQ, scratchpad_data->seq);
+    ASSERT_EQ(TEST_CRC, scratchpad_data->crc);
+    ASSERT_EQ(TEST_ACTION, scratchpad_data->action);
+    ASSERT_EQ(TEST_PARAM, scratchpad_data->param);
+}
+
+TEST_F(WpcCddApiTest, testWritingScratchpatTargetWithCddApi)
+{
+    const CddScratchpadDataItem SCRATCHPAD_DATA = {
+        .seq = 71,
+        .crc = 0xFEEB,
+        .action = 3,
+        .param = 0xC2
+    };
+
+    ASSERT_EQ(APP_RES_OK,
+              WPC_set_config_data_item(CDD_SCRATCHPAD_DATA_EP,
+                                       (uint8_t const*) &SCRATCHPAD_DATA,
+                                       sizeof(SCRATCHPAD_DATA)));
+
+    uint8_t seq    = 0;
+    uint16_t crc   = 0;
+    uint8_t action = 0;
+    uint8_t param  = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_read_target_scratchpad(&seq, &crc, &action, &param));
+    ASSERT_EQ(SCRATCHPAD_DATA.seq, seq);
+    ASSERT_EQ(SCRATCHPAD_DATA.crc, crc);
+    ASSERT_EQ(SCRATCHPAD_DATA.action, action);
+    ASSERT_EQ(SCRATCHPAD_DATA.param, param);
+}
+
+TEST_F(WpcCddApiTest, testWritingInvalidScratchpatTargetWithCddApi)
+{
+    GTEST_SKIP() << "TODO re-enable once implemented on stack";
+
+    const CddScratchpadDataItem SCRATCHPAD_DATA = {
+        .seq = 71,
+        .crc = 0xFEEB,
+        .action = 250, // Invalid value
+        .param = 0xC2
+    };
+
+    ASSERT_EQ(APP_RES_DATA_ERROR,
+              WPC_set_config_data_item(CDD_SCRATCHPAD_DATA_EP,
+                                       (uint8_t const*) &SCRATCHPAD_DATA,
+                                       sizeof(SCRATCHPAD_DATA)));
+}
+
+TEST_F(WpcCddApiTest, testWritingInvalidDiagIntervalWithCddApi)
+{
+    GTEST_SKIP() << "TODO re-enable once implemented on stack";
+
+    const uint8_t TEST_DIAG_INTERVAL[] = { 0xFE };
+    ASSERT_EQ(APP_RES_DATA_ERROR,
+              WPC_set_config_data_item(CDD_DIAG_INTERVAL_EP, TEST_DIAG_INTERVAL, 1));
+}
+
+TEST_F(WpcCddApiTest, testWritingInvalidScratchpadTargetSizeWithCddApi)
+{
+    const uint8_t TEST_PAYLOAD[10] = { 0 };
+    ASSERT_EQ(APP_RES_DATA_ERROR,
+              WPC_set_config_data_item(CDD_SCRATCHPAD_DATA_EP, TEST_PAYLOAD, sizeof(TEST_PAYLOAD)));
+}
+
+TEST_F(WpcCddApiTest, testWritingInvalidDiagIntervalSizeWithCddApi)
+{
+    const uint8_t TEST_PAYLOAD[10] = { 0 };
+    ASSERT_EQ(APP_RES_DATA_ERROR,
+              WPC_set_config_data_item(CDD_DIAG_INTERVAL_EP, TEST_PAYLOAD, sizeof(TEST_PAYLOAD)));
+}
+
+TEST_F(WpcCddApiTest, testWritingInvalidAppConfigDataWithCddApi)
+{
+    // App config data contents can be anything, but the size should be correct
+
+    uint8_t app_config_size = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&app_config_size));
+    ASSERT_GT(app_config_size, 2);
+    const uint8_t INVALID_APP_CONFIG_SIZE = app_config_size - 1;
+
+    const uint8_t PAYLOAD[UINT8_MAX] = { 0 };
+    ASSERT_EQ(APP_RES_DATA_ERROR, WPC_set_config_data_item(CDD_APP_CONFIG_EP, PAYLOAD, INVALID_APP_CONFIG_SIZE));
+}
+
+TEST_F(WpcCddApiTest, settingItemShouldFailOnNonSinkNode)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_HEADNODE));
+
+    const uint8_t PAYLOAD[] = { 0xAA, 0xBB };
+    ASSERT_EQ(APP_RES_NOT_A_SINK, WPC_set_config_data_item(100, PAYLOAD, sizeof(PAYLOAD)));
+}
+
+TEST_F(WpcCddApiTest, gettingItemShouldWorkOnNonSinkNode)
+{
+    const uint8_t TEST_PAYLOAD[] = { 0x55, 0x11, 0xFA };
+    const uint16_t TEST_EP = 0x100;
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_EP, TEST_PAYLOAD, sizeof(TEST_PAYLOAD)));
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_HEADNODE));
+
+    uint8_t payload[UINT8_MAX] = { 0 };
+    uint8_t payload_size = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_get_config_data_item(TEST_EP, payload, sizeof(payload), &payload_size));
+    ASSERT_EQ(sizeof(TEST_PAYLOAD), payload_size);
+    ASSERT_EQ_ARRAY(TEST_PAYLOAD, payload, payload_size);
+}
+
+TEST_F(WpcCddApiTest, testSettingAndDeletingOptionalItem)
+{
+    const uint8_t TEST_PAYLOAD[] = { 0xF1, 0xD0, 0xA9, 0xB4 };
+    const uint16_t TEST_EP = 0x100;
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_EP, TEST_PAYLOAD, sizeof(TEST_PAYLOAD)));
+
+    {
+        uint8_t payload[UINT8_MAX] = { 0 };
+        uint8_t payload_size = 0;
+        ASSERT_EQ(APP_RES_OK, WPC_get_config_data_item(TEST_EP, payload, sizeof(payload), &payload_size));
+        ASSERT_EQ(sizeof(TEST_PAYLOAD), payload_size);
+        ASSERT_EQ_ARRAY(TEST_PAYLOAD, payload, payload_size);
+    }
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_EP, NULL, 0));
+
+    {
+        uint8_t payload[UINT8_MAX] = { 0 };
+        uint8_t payload_size = 0;
+        ASSERT_EQ(APP_RES_INVALID_VALUE, WPC_get_config_data_item(TEST_EP, payload, sizeof(payload), &payload_size));
+    }
+}
+
+TEST_F(WpcCddApiTest, settingTooLargePayloadShouldFail)
+{
+    const uint8_t TEST_PAYLOAD[250] = { 0 };
+    ASSERT_EQ(APP_RES_INTERNAL_ERROR, WPC_set_config_data_item(100, TEST_PAYLOAD, sizeof(TEST_PAYLOAD)));
+}
+
+TEST_F(WpcCddApiTest, testReplacingOptionalItem)
+{
+    const uint16_t TEST_EP = 0x100;
+    const uint8_t TEST_PAYLOAD_1[] = { 0xA1, 0xB1, 0xC1, 0xD1 };
+    const uint8_t TEST_PAYLOAD_2[] = { 0xA0, 0xB0 };
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_EP, TEST_PAYLOAD_1, sizeof(TEST_PAYLOAD_1)));
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_EP, TEST_PAYLOAD_2, sizeof(TEST_PAYLOAD_2)));
+
+    uint8_t payload[UINT8_MAX] = { 0 };
+    uint8_t payload_size = 0;
+    ASSERT_EQ(APP_RES_OK, WPC_get_config_data_item(TEST_EP, payload, sizeof(payload), &payload_size));
+    ASSERT_EQ(sizeof(TEST_PAYLOAD_2), payload_size);
+    ASSERT_EQ_ARRAY(TEST_PAYLOAD_2, payload, payload_size);
+}
+
+TEST_F(WpcCddApiTest, gettingNonExistingEndpointShouldFail)
+{
+    const uint16_t TEST_EP = 0xE999;
+
+    uint8_t payload[UINT8_MAX] = { 0 };
+    uint8_t payload_size = 0;
+    ASSERT_EQ(APP_RES_INVALID_VALUE, WPC_get_config_data_item(TEST_EP, payload, sizeof(payload), &payload_size));
+}
+
+TEST_F(WpcCddApiTest, factoryResetShouldRemoveOptionalItems)
+{
+    const uint8_t TEST_PAYLOAD[] = { 0xAA, 0xBB };
+    const uint16_t TEST_EP = 0x100;
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_config_data_item(TEST_EP, TEST_PAYLOAD, sizeof(TEST_PAYLOAD)));
+
+    ASSERT_EQ(APP_RES_OK, WPC_do_factory_reset());
+
+    uint8_t payload[UINT8_MAX] = { 0 };
+    uint8_t payload_size = 0;
+    ASSERT_EQ(APP_RES_INVALID_VALUE, WPC_get_config_data_item(TEST_EP, payload, sizeof(payload), &payload_size));
+    ASSERT_EQ(0, payload_size);
+}
+
+TEST_F(WpcCddApiTest, settingTooManyOptionalItemsShouldFail)
+{
+    const uint16_t MAX_ATTEMPTS = 50;
+    uint16_t detected_max_item_count = 0;
+    for (uint16_t ep = 1; ep <= MAX_ATTEMPTS; ep++)
+    {
+        const uint8_t TEST_PAYLOAD[] = { 0x43 };
+        const app_res_e res = WPC_set_config_data_item(ep, TEST_PAYLOAD, sizeof(TEST_PAYLOAD));
+        if (APP_RES_OK == res) {
+            detected_max_item_count++;
+        } else if (APP_RES_OUT_OF_MEMORY == res) {
+            break;
+        } else {
+            FAIL() << "Unexpected error occured";
+        }
+    }
+
+    ASSERT_LT(detected_max_item_count, MAX_ATTEMPTS);
+    ASSERT_GT(detected_max_item_count, 0);
+}
+
diff --git a/test/general_tests.cpp b/test/general_tests.cpp
new file mode 100644
index 0000000..792c439
--- /dev/null
+++ b/test/general_tests.cpp
@@ -0,0 +1,286 @@
+#include "wpc_test.hpp"
+
+class WpcGeneralTestStackOff : public WpcTest
+{
+public:
+    static void SetUpTestSuite()
+    {
+        ASSERT_NO_FATAL_FAILURE(WpcTest::SetUpTestSuite());
+        ASSERT_NO_FATAL_FAILURE(WpcTest::StopStack());
+    }
+};
+
+class WpcGeneralTestStackOn : public WpcTest
+{
+public:
+    static void SetUpTestSuite()
+    {
+        ASSERT_NO_FATAL_FAILURE(WpcTest::SetUpTestSuite());
+
+        // Following parameters should be set in order to be able to start the
+        // stack, and stack should be stopped in order to be able to set them.
+        ASSERT_NO_FATAL_FAILURE(WpcTest::StopStack());
+        ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+        ASSERT_EQ(APP_RES_OK, WPC_set_node_address(1234));
+        ASSERT_EQ(APP_RES_OK, WPC_set_network_address(0x654321));
+        ASSERT_EQ(APP_RES_OK, WPC_set_network_channel(38));
+        ASSERT_NO_FATAL_FAILURE(WpcTest::StartStack());
+    }
+};
+
+TEST_F(WpcGeneralTestStackOff, testFactoryReset)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_set_node_address(200));
+    ASSERT_EQ(APP_RES_OK, WPC_set_network_address(0x12123));
+    ASSERT_EQ(APP_RES_OK, WPC_set_network_channel(39));
+
+    ASSERT_EQ(APP_RES_OK, WPC_do_factory_reset());
+
+    app_addr_t node_address;
+    ASSERT_EQ(APP_RES_ATTRIBUTE_NOT_SET, WPC_get_node_address(&node_address));
+
+    app_addr_t network_address;
+    ASSERT_EQ(APP_RES_ATTRIBUTE_NOT_SET, WPC_get_network_address(&network_address));
+
+    net_channel_t network_channel;
+    ASSERT_EQ(APP_RES_ATTRIBUTE_NOT_SET, WPC_get_network_channel(&network_channel));
+}
+
+TEST_F(WpcGeneralTestStackOff, testSetCSAPAddressingAttributes)
+{
+    const app_role_t TEST_ROLE = APP_ROLE_HEADNODE;
+    const app_addr_t TEST_NODE_ADDRESS = 3211;
+    const app_addr_t TEST_NETWORK_ADDRESS = 45457;
+    const net_channel_t TEST_NETWORK_CHANNEL = 36;
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(TEST_ROLE));
+    ASSERT_EQ(APP_RES_OK, WPC_set_node_address(TEST_NODE_ADDRESS));
+    ASSERT_EQ(APP_RES_OK, WPC_set_network_address(TEST_NETWORK_ADDRESS));
+    ASSERT_EQ(APP_RES_OK, WPC_set_network_channel(TEST_NETWORK_CHANNEL));
+
+    app_role_t role;
+    ASSERT_EQ(APP_RES_OK, WPC_get_role(&role));
+    ASSERT_EQ(TEST_ROLE, role);
+
+    app_addr_t node_address;
+    ASSERT_EQ(APP_RES_OK, WPC_get_node_address(&node_address));
+    ASSERT_EQ(TEST_NODE_ADDRESS, node_address);
+
+    app_addr_t network_address;
+    ASSERT_EQ(APP_RES_OK, WPC_get_network_address(&network_address));
+    ASSERT_EQ(TEST_NETWORK_ADDRESS, network_address);
+
+    net_channel_t network_channel;
+    ASSERT_EQ(APP_RES_OK, WPC_get_network_channel(&network_channel));
+    ASSERT_EQ(TEST_NETWORK_CHANNEL, network_channel);
+}
+
+TEST_F(WpcGeneralTestStackOff, dumpCSAPAttributes)
+{
+    uint8_t mtu, pdus, scratch_seq, first_channel, last_channel, data_size;
+    uint16_t firmware[4], api_v, hw_magic, stack_profile;
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_mtu(&mtu));
+    RecordProperty("mtu_size", (long) mtu);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_pdu_buffer_size(&pdus));
+    RecordProperty("pdu_size", (long) pdus);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_scratchpad_sequence(&scratch_seq));
+    RecordProperty("scratch_seq", (long) scratch_seq);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_mesh_API_version(&api_v));
+    RecordProperty("api_v", (long) api_v);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_firmware_version(firmware));
+    RecordProperty("firmware_v_major", (long) firmware[0]);
+    RecordProperty("firmware_v_minor", (long) firmware[1]);
+    RecordProperty("firmware_v_maintenance", (long) firmware[2]);
+    RecordProperty("firmware_v_development", (long) firmware[3]);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_channel_limits(&first_channel, &last_channel));
+    RecordProperty("first_channel", (long) first_channel);
+    RecordProperty("last_channel", (long) last_channel);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&data_size));
+    RecordProperty("max_config_data_size", (long) data_size);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_hw_magic(&hw_magic));
+    RecordProperty("hw_magic", (long) hw_magic);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_stack_profile(&stack_profile));
+    RecordProperty("stack_profile", (long) stack_profile);
+}
+
+TEST_F(WpcGeneralTestStackOff, testCipherKey)
+{
+    bool key_set;
+
+    // Verify setting the key
+    const uint8_t key[16] = {
+        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
+    ASSERT_EQ(APP_RES_OK, WPC_set_cipher_key(key));
+
+    ASSERT_EQ(APP_RES_OK, WPC_is_cipher_key_set(&key_set));
+    ASSERT_TRUE(key_set);
+
+    // Verify removing the key
+    ASSERT_EQ(APP_RES_OK, WPC_remove_cipher_key());
+    ASSERT_EQ(APP_RES_OK, WPC_is_cipher_key_set(&key_set));
+    ASSERT_FALSE(key_set);
+}
+
+TEST_F(WpcGeneralTestStackOff, testAuthenticationKey)
+{
+    bool key_set;
+
+    // Verify setting the key
+    const uint8_t key[16] = {
+        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
+    ASSERT_EQ(APP_RES_OK, WPC_set_authentication_key(key));
+
+    ASSERT_EQ(APP_RES_OK, WPC_is_authentication_key_set(&key_set));
+    ASSERT_TRUE(key_set);
+
+    // Verify removing the key
+    ASSERT_EQ(APP_RES_OK, WPC_remove_authentication_key());
+    ASSERT_EQ(APP_RES_OK, WPC_is_authentication_key_set(&key_set));
+    ASSERT_FALSE(key_set);
+}
+
+TEST_F(WpcGeneralTestStackOff, testScratchpadTarget)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+
+    ASSERT_EQ(APP_RES_OK, WPC_write_target_scratchpad(12, 0x1234, 2, 13));
+
+    uint8_t target_seq;
+    uint16_t target_crc;
+    uint8_t action;
+    uint8_t param;
+    ASSERT_EQ(APP_RES_OK, WPC_read_target_scratchpad(&target_seq, &target_crc, &action, &param));
+
+    EXPECT_EQ(12, target_seq);
+    EXPECT_EQ(0x1234, target_crc);
+    EXPECT_EQ(2, action);
+    EXPECT_EQ(13, param);
+}
+
+TEST_F(WpcGeneralTestStackOff, gettingAppConfigShouldFailIfBufferIsTooSmall)
+{
+    uint8_t max_size;
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&max_size));
+    ASSERT_GT(max_size, 1);
+
+    uint8_t seq;
+    uint16_t interval;
+    uint8_t config[UINT8_MAX];
+    ASSERT_EQ(APP_RES_INVALID_VALUE, WPC_get_app_config_data(&seq, &interval, config, max_size - 1));
+}
+
+TEST_F(WpcGeneralTestStackOff, settingAppConfigShouldFailIfConfigTooLarge)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+
+    uint8_t max_size;
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&max_size));
+    ASSERT_LT(max_size, UINT8_MAX);
+
+    const uint8_t config[UINT8_MAX] = {0};
+    ASSERT_EQ(APP_RES_INVALID_VALUE, WPC_set_app_config_data(1, 30, config, max_size + 1));
+}
+
+TEST_F(WpcGeneralTestStackOff, testSettingAppConfigDataWithInvalidInterval)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+
+    const uint8_t INVALID_INTERVAL = 62;
+    const uint8_t config[1] = {0};
+    ASSERT_EQ(APP_RES_INVALID_DIAG_INTERVAL, WPC_set_app_config_data(1, INVALID_INTERVAL, config, sizeof(config)));
+}
+
+TEST_F(WpcGeneralTestStackOff, testSettingAppConfigData)
+{
+    ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+
+    uint8_t max_size;
+    ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data_size(&max_size));
+
+    uint8_t existing_seq;
+    uint16_t existing_interval;
+    uint8_t existing_config[UINT8_MAX];
+    ASSERT_EQ(APP_RES_OK,
+              WPC_get_app_config_data(&existing_seq,
+                                      &existing_interval,
+                                      existing_config,
+                                      sizeof(existing_config)));
+
+    uint8_t new_config[UINT8_MAX] = {0};
+    for (uint8_t i = 0; i < sizeof(new_config); i++) {
+        new_config[i] = i;
+    }
+    const uint16_t new_interval = 1800;
+    // Set new app config. On recent stack releases, sequence is ignored.
+    ASSERT_EQ(APP_RES_OK, WPC_set_app_config_data(100, new_interval, new_config, max_size));
+
+    {
+        uint8_t seq;
+        uint16_t interval;
+        uint8_t config[UINT8_MAX];
+        ASSERT_EQ(APP_RES_OK, WPC_get_app_config_data(&seq, &interval, config, sizeof(config)));
+        ASSERT_EQ(new_interval, interval);
+        ASSERT_EQ_ARRAY(new_config, config, max_size);
+    }
+}
+
+
+TEST_F(WpcGeneralTestStackOff, testSettingAccesCycleRange)
+{
+    const uint16_t RANGE_MIN = 2000;
+    const uint16_t RANGE_MAX = 4000;
+
+    ASSERT_EQ(APP_RES_OK, WPC_set_access_cycle_range(RANGE_MIN, RANGE_MAX));
+
+    {
+        uint16_t min, max;
+        ASSERT_EQ(APP_RES_OK, WPC_get_access_cycle_range(&min, &max));
+        ASSERT_EQ(RANGE_MIN, min);
+        ASSERT_EQ(RANGE_MAX, max);
+    }
+
+    {
+        uint16_t min, max = 0;
+        ASSERT_EQ(APP_RES_OK, WPC_get_access_cycle_limits(&min, &max));
+        ASSERT_NE(0, min);
+        ASSERT_NE(0, max);
+    }
+}
+
+TEST_F(WpcGeneralTestStackOn, testReadMSAPAttributes)
+{
+    uint8_t res8;
+    uint16_t res16;
+    uint32_t res32;
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_stack_status(&res8));
+    RecordProperty("stack_status", (long) res8);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_PDU_buffer_usage(&res8));
+    RecordProperty("pdu_buffer_usage", (long) res8);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_PDU_buffer_capacity(&res8));
+    RecordProperty("pdu_buffer_capacity", (long) res8);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_autostart(&res8));
+    RecordProperty("autostart", (long) res8);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_route_count(&res8));
+    RecordProperty("route_count", (long) res8);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_system_time(&res32));
+    RecordProperty("system_time", (long) res32);
+
+    ASSERT_EQ(APP_RES_OK, WPC_get_current_access_cycle(&res16));
+    RecordProperty("current_access_cycle", (long) res16);
+}
+
diff --git a/test/makefile b/test/makefile
index 5b24c33..5c38eb5 100644
--- a/test/makefile
+++ b/test/makefile
@@ -2,6 +2,7 @@
 
 # Toolchain
 CC  := gcc
+CXX := g++
 AS  := as
 LD  := ld
 AR  := ar
@@ -15,7 +16,7 @@ MESH_LIB_FOLDER := ../lib/
 MESH_LIB := $(MESH_LIB_FOLDER)build/mesh_api_lib.a
 
 # General compiler flags
-CFLAGS  := -std=gnu99 -Wall -Werror
+CFLAGS  := -std=c++17 -Wall -Werror
 
 # Targets definition
 MAIN_APP := meshAPItest
@@ -25,7 +26,7 @@ TARGET_APP := $(BUILDPREFIX)$(MAIN_APP)
 # Add Api header
 CFLAGS  += -I$(MESH_LIB_FOLDER)api
 # Add pthtread lib as needed by Mesh Lib
-LDFLAGS += -pthread
+LDFLAGS += -pthread -lgtest
 # Add Reentrant flag as using pthread
 CFLAGS  += -D_REENTRANT
 
@@ -33,28 +34,32 @@ CFLAGS  += -D_REENTRANT
 CFLAGS  += -I$(MESH_LIB_FOLDER)platform
 
 # Main app
-SOURCES := $(SOURCEPREFIX)test_app.c
+SOURCES := $(SOURCEPREFIX)test_main.cpp
 
 # Test cases
 CFLAGS  += -I$(SOURCEPREFIX)
-SOURCES += $(SOURCEPREFIX)test.c
+SOURCES += $(SOURCEPREFIX)wpc_test.cpp \
+	$(SOURCEPREFIX)general_tests.cpp     \
+	$(SOURCEPREFIX)scratchpad_tests.cpp  \
+	$(SOURCEPREFIX)callback_tests.cpp  \
+	$(SOURCEPREFIX)cdd_tests.cpp
 
 OBJECTS := $(patsubst $(SOURCEPREFIX)%,                     \
                   $(BUILDPREFIX)%,                          \
-                  $(SOURCES:.c=.o))
+                  $(SOURCES:.cpp=.o))
 
 # Functions
 
 # Also create the target directory if it does not exist
 define COMPILE
-	echo "  CC $(2)"
+	echo "  CXX $(2)"
 	mkdir -p $(dir $(1))
-	$(CC) $(CFLAGS) -c -o $(1) $(2)
+	$(CXX) $(CFLAGS) -c -o $(1) $(2)
 endef
 
 define LINK
 	echo "  Linking $(1)"
-	$(CC) $(CFLAGS) $(LDFLAGS) -o $(1) $(2) $(MESH_LIB)
+	$(CXX) $(CFLAGS) -o $(1) $(2) $(MESH_LIB) $(LDFLAGS)
 endef
 
 define CLEAN
@@ -88,7 +93,7 @@ clean:
 $(MESH_LIB):
 	make -C $(MESH_LIB_FOLDER)
 
-$(BUILDPREFIX)%.o: $(SOURCEPREFIX)%.c
+$(BUILDPREFIX)%.o: $(SOURCEPREFIX)%.cpp
 	$(call COMPILE,$@,$<)
 
 $(BUILDPREFIX)$(MAIN_APP): $(OBJECTS) $(MESH_LIB)
diff --git a/test/otap_files/.gitignore b/test/otap_files/.gitignore
deleted file mode 100644
index 8e3b96a..0000000
--- a/test/otap_files/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-downloaded.otap
diff --git a/test/scratchpad_tests.cpp b/test/scratchpad_tests.cpp
new file mode 100644
index 0000000..bbd8903
--- /dev/null
+++ b/test/scratchpad_tests.cpp
@@ -0,0 +1,154 @@
+#include <fstream>
+#include <filesystem>
+#include <gtest/gtest.h>
+#include "wpc_test.hpp"
+
+class WpcScratchpadTest : public WpcTest
+{
+public:
+    static void SetUpTestSuite()
+    {
+        ASSERT_NO_FATAL_FAILURE(WpcTest::SetUpTestSuite());
+        ASSERT_NO_FATAL_FAILURE(WpcTest::StopStack());
+        ASSERT_EQ(APP_RES_OK, WPC_set_role(APP_ROLE_SINK));
+        ASSERT_TRUE(std::filesystem::exists(OTAP_UPLOAD_FILE_PATH));
+    }
+
+protected:
+    inline static const std::filesystem::path OTAP_UPLOAD_FILE_PATH = "otap_files/dummy.otap";
+
+    std::vector<uint8_t> ReadUploadFile() const
+    {
+        const auto size = std::filesystem::file_size(OTAP_UPLOAD_FILE_PATH);
+        std::vector<uint8_t> buffer(size);
+
+        if (std::ifstream ifs(OTAP_UPLOAD_FILE_PATH, std::ios_base::binary); ifs) {
+            if (ifs.read((char*)buffer.data(), size)) {
+                return buffer;
+            }
+        }
+
+        ADD_FAILURE() << "Cannot open file " << OTAP_UPLOAD_FILE_PATH << ". "
+                      << "Please update OTAP_UPLOAD_FILE_PATH to a valid OTAP image";
+        return {};
+    }
+
+    void UploadScratchpadAndVerify(const std::vector<uint8_t>& buffer, const uint8_t seq_number) const
+    {
+        ASSERT_FALSE(buffer.empty());
+
+        ASSERT_EQ(APP_RES_OK, WPC_upload_local_scratchpad(buffer.size(), (uint8_t*)buffer.data(), seq_number));
+
+        ASSERT_NO_FATAL_FAILURE(VerifyUploadedScratchpad(seq_number, buffer.size()));
+    }
+
+    void VerifyUploadedScratchpad(const uint8_t seq_number, const uint32_t size) const
+    {
+        app_scratchpad_status_t status;
+        ASSERT_EQ(APP_RES_OK, WPC_get_local_scratchpad_status(&status));
+        ASSERT_EQ(size, status.scrat_len);
+        ASSERT_EQ(seq_number, status.scrat_seq_number);
+
+        uint32_t reported_size = 0;
+        ASSERT_EQ(APP_RES_OK, WPC_get_scratchpad_size(&reported_size));
+        ASSERT_EQ(size, reported_size);
+
+        uint8_t reported_seq = 0;
+        ASSERT_EQ(APP_RES_OK, WPC_get_scratchpad_sequence(&reported_seq));
+        ASSERT_EQ(seq_number, reported_seq);
+    }
+
+    void VerifyScratchpadsAreTheSame(const std::vector<uint8_t>& first, const std::vector<uint8_t>& second) const
+    {
+        std::vector<uint8_t> first_copy = { first };
+        std::vector<uint8_t> second_copy = { second };
+ 
+        // Scratchpad header (16 bytes starting at index 16) includes information
+        // that can be different when downloaded, so exclude that part when
+        // doing the comparison.
+        ASSERT_GE(first_copy.size(), 32);
+        ASSERT_GE(second_copy.size(), 32);
+        std::fill_n(first_copy.begin() + 16, 16, 0);
+        std::fill_n(second_copy.begin() + 16, 16, 0);
+
+        ASSERT_EQ(first_copy, second_copy);
+    }
+};
+
+TEST_F(WpcScratchpadTest, testUploadAndClearScratchpad)
+{
+    const auto buffer = ReadUploadFile();
+    ASSERT_NO_FATAL_FAILURE(UploadScratchpadAndVerify(buffer, 101));
+
+    ASSERT_EQ(APP_RES_OK, WPC_clear_local_scratchpad());
+
+    app_scratchpad_status_t status;
+    ASSERT_EQ(APP_RES_OK, WPC_get_local_scratchpad_status(&status));
+    ASSERT_EQ(0, status.scrat_len);
+    ASSERT_EQ(0, status.scrat_seq_number);
+
+    uint32_t size;
+    ASSERT_EQ(APP_RES_ATTRIBUTE_NOT_SET, WPC_get_scratchpad_size(&size));
+
+    uint8_t seq;
+    ASSERT_EQ(APP_RES_OK, WPC_get_scratchpad_sequence(&seq));
+    ASSERT_EQ(0, seq);
+}
+
+TEST_F(WpcScratchpadTest, testUploadAndDownloadScratchpadInBlocks)
+{
+    const auto upload_buffer = ReadUploadFile();
+
+    const uint32_t MAX_BLOCK_SIZE = 32;
+    ASSERT_LT(MAX_BLOCK_SIZE, upload_buffer.size());
+
+    const uint8_t TEST_SEQ = 99;
+    ASSERT_EQ(APP_RES_OK, WPC_start_local_scratchpad_update(upload_buffer.size(), TEST_SEQ));
+
+    for (size_t written = 0, remaining = upload_buffer.size(); remaining > 0;)
+    {
+        const uint32_t block_size = (remaining > MAX_BLOCK_SIZE) ? MAX_BLOCK_SIZE : remaining;
+        const uint8_t* block_ptr = (uint8_t*) upload_buffer.data() + written;
+
+        ASSERT_EQ(APP_RES_OK, WPC_upload_local_block_scratchpad(block_size, block_ptr, written));
+
+        written += block_size;
+        remaining -= block_size;
+    }
+
+    ASSERT_NO_FATAL_FAILURE(VerifyUploadedScratchpad(TEST_SEQ, upload_buffer.size()));
+
+    std::vector<uint8_t> download_buffer(upload_buffer.size());
+    for (size_t read = 0, remaining = upload_buffer.size(); remaining > 0;)
+    {
+        const uint32_t block_size = (remaining > MAX_BLOCK_SIZE) ? MAX_BLOCK_SIZE : remaining;
+        uint8_t* block_ptr = (uint8_t*) download_buffer.data() + read;
+
+        const app_res_e res = WPC_download_local_scratchpad(block_size, block_ptr, read);
+        ASSERT_NE(APP_RES_ACCESS_DENIED, res) << "Access denied when downloading scratchpad block. "
+                                                 "Make sure the dual MCU application was built with scratchpad "
+                                                 "reading enabled.";
+        ASSERT_EQ(APP_RES_OK, res);
+
+        read += block_size;
+        remaining -= block_size;
+    }
+
+    ASSERT_NO_FATAL_FAILURE(VerifyScratchpadsAreTheSame(upload_buffer, download_buffer));
+}
+
+TEST_F(WpcScratchpadTest, testUploadAndDownloadScratchpad)
+{
+    auto upload_buffer = ReadUploadFile();
+    ASSERT_NO_FATAL_FAILURE(UploadScratchpadAndVerify(upload_buffer, 201));
+
+    std::vector<uint8_t> download_buffer(upload_buffer.size());
+    const app_res_e res = WPC_download_local_scratchpad(download_buffer.size(), (uint8_t*)download_buffer.data(), 0);
+    ASSERT_NE(APP_RES_ACCESS_DENIED, res) << "Access denied when downloading scratchpad block. "
+                                             "Make sure the dual MCU application was built with scratchpad "
+                                             "reading enabled.";
+    ASSERT_EQ(APP_RES_OK, res);
+
+    ASSERT_NO_FATAL_FAILURE(VerifyScratchpadsAreTheSame(upload_buffer, download_buffer));
+}
+
diff --git a/test/test.c b/test/test.c
deleted file mode 100644
index 02552b4..0000000
--- a/test/test.c
+++ /dev/null
@@ -1,963 +0,0 @@
-/* Wirepas Oy licensed under Apache License, Version 2.0
- *
- * See file LICENSE for full license details.
- *
- */
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-
-#define LOG_MODULE_NAME "Test"
-#define MAX_LOG_LEVEL INFO_LOG_LEVEL
-#include "logger.h"
-
-#include "wpc.h"
-
-static int passedTestCount = 0;
-static int failedTestCount = 0;
-
-static bool setInitialState(app_role_t role,
-                            app_addr_t id,
-                            net_addr_t network_add,
-                            net_channel_t network_channel,
-                            bool start)
-{
-    WPC_stop_stack();
-
-    // Wait for stack to stop
-    usleep(3 * 1000 * 1000);
-
-    if (WPC_set_node_address(id) != APP_RES_OK)
-        return false;
-
-    if (WPC_set_role(role) != APP_RES_OK)
-        return false;
-
-    if (WPC_set_network_address(network_add) != APP_RES_OK)
-        return false;
-
-    if (WPC_set_network_channel(network_channel) != APP_RES_OK)
-        return false;
-
-    if (start && (WPC_start_stack() == APP_RES_OK))
-        return false;
-
-    return true;
-}
-
-static bool testFactoryReset()
-{
-    app_res_e res = WPC_stop_stack();
-    app_addr_t add;
-    if (res != APP_RES_STACK_ALREADY_STOPPED && res != APP_RES_OK)
-    {
-        LOGE("Cannot stop stack\n");
-        return false;
-    }
-
-    if (WPC_set_node_address(0x123456) != APP_RES_OK)
-    {
-        LOGE("Cannot set node address\n");
-        return false;
-    }
-
-    if (WPC_do_factory_reset() != APP_RES_OK)
-    {
-        LOGE("Cannot do factory reset\n");
-        return false;
-    }
-
-    // Check node address previously set is correctly removed
-    if (WPC_get_node_address(&add) != APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGE("Node address is still set\n");
-        return false;
-    }
-
-    return true;
-}
-
-static bool setAppconfig(uint8_t * config, uint16_t interval, uint8_t size)
-{
-    uint8_t cur_seq, new_seq = 0;
-    uint16_t cur_interval;
-    uint8_t cur_config[128];
-    app_res_e res;
-
-    if (size > sizeof(cur_config))
-    {
-        LOGE("Specified size (%d) is too big.\n", size);
-        return false;
-    }
-
-    res = WPC_get_app_config_data(&cur_seq, &cur_interval, cur_config, sizeof(cur_config));
-    if (res == APP_RES_OK)
-    {
-        new_seq = cur_seq + 1;
-    }
-    else
-    {
-        LOGE("Cannot get current app config seq. Result = 0x%02x.\n", res);
-        return false;
-    }
-
-    res = WPC_set_app_config_data(new_seq, interval, config, size);
-    if (res != APP_RES_OK)
-    {
-        LOGE("Cannot set new app config with seq = %d. Result = 0x%02x.\n", new_seq, res);
-        return false;
-    }
-
-    // Read back App config
-    res = WPC_get_app_config_data(&cur_seq, &cur_interval, cur_config, sizeof(cur_config));
-    if (res != APP_RES_OK)
-    {
-        LOGE("Cannot read back new app config. Result = 0x%02x.\n", res);
-        return false;
-    }
-
-    if (memcmp(config, cur_config, size) != 0)
-    {
-        LOGE("App config set differs from app config read.\n");
-        return false;
-    }
-
-    return true;
-}
-
-static bool dumpCSAPAttributes()
-{
-    uint8_t role;
-    net_channel_t channel;
-    app_addr_t node_address;
-    net_addr_t network;
-    uint8_t mtu, pdus, scratch_seq, first_channel, last_channel, data_size;
-    uint16_t firmware[4], api_v, hw_magic, stack_profile;
-
-    if (WPC_get_role(&role) == APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGW("Role not set\n");
-        role = 0;
-    }
-
-    if (WPC_get_node_address(&node_address) == APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGW("Node address not set\n");
-        node_address = 0;
-    }
-
-    if (WPC_get_network_address(&network) == APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGW("Network address not set\n");
-        network = 0;
-    }
-
-    if (WPC_get_network_channel(&channel) == APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGW("Network channel not set\n");
-        channel = 0;
-    }
-
-    LOGI("Role is 0x%02x / Add is %d / Network is 0x%06x (channel = %d)\n", role, node_address, network, channel);
-
-    WPC_get_mtu(&mtu);
-    LOGI("MTU size is %d\n", mtu);
-
-    WPC_get_pdu_buffer_size(&pdus);
-    LOGI("PDU size is %d\n", pdus);
-
-    WPC_get_scratchpad_sequence(&scratch_seq);
-    LOGI("Scratchpad seq is %d\n", scratch_seq);
-
-    WPC_get_mesh_API_version(&api_v);
-    LOGI("Mesh API version is %d\n", api_v);
-
-    WPC_get_firmware_version(firmware);
-    LOGI("Firmware version is %d:%d:%d:%d\n", firmware[0], firmware[1], firmware[2], firmware[3]);
-
-    WPC_get_channel_limits(&first_channel, &last_channel);
-    LOGI("Channel Limits are: [%d - %d]\n", first_channel, last_channel);
-
-    WPC_get_app_config_data_size(&data_size);
-    LOGI("Max config data size is %d\n", data_size);
-
-    WPC_get_hw_magic(&hw_magic);
-    LOGI("Radio hardware is %d\n", hw_magic);
-
-    WPC_get_stack_profile(&stack_profile);
-    LOGI("Stack profile is %d\n", stack_profile);
-
-    return true;
-}
-
-static bool testCipherKey()
-{
-    bool key_set;
-    if (WPC_is_cipher_key_set(&key_set) != APP_RES_OK)
-    {
-        return false;
-    }
-
-    if (key_set)
-    {
-        LOGW("Key already set\n");
-        // return false
-    }
-
-    LOGI("No cipher key\n");
-    uint8_t key[16] = {
-        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
-
-    LOGI("Set cipher key\n");
-    if (WPC_set_cipher_key(key) != APP_RES_OK)
-    {
-        LOGE("Cannot set key\n");
-        return false;
-    }
-
-    LOGI("Check cipher key set\n");
-    if (WPC_is_cipher_key_set(&key_set) != APP_RES_OK)
-    {
-        return false;
-    }
-
-    if (!key_set)
-    {
-        LOGE("Key is not set\n");
-        return false;
-    }
-
-    LOGI("Remove cipher key\n");
-    if (WPC_remove_cipher_key() != APP_RES_OK)
-    {
-        LOGE("Cannot remove key\n");
-        return false;
-    }
-
-    LOGI("Check cipher key not set\n");
-    if (WPC_is_cipher_key_set(&key_set) != APP_RES_OK)
-    {
-        return false;
-    }
-
-    if (key_set)
-    {
-        LOGE("Key is still set\n");
-        return false;
-    }
-
-    return true;
-}
-
-static bool testAuthenticationKey()
-{
-    bool key_set;
-    if (WPC_is_authentication_key_set(&key_set) != APP_RES_OK)
-    {
-        return false;
-    }
-
-    if (key_set)
-    {
-        LOGW("Key already set\n");
-        // return false
-    }
-
-    LOGI("No authentication key\n");
-    uint8_t key[16] = {
-        0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f};
-
-    LOGI("Set authentication key\n");
-    if (WPC_set_authentication_key(key) != APP_RES_OK)
-    {
-        LOGE("Cannot set key\n");
-        return false;
-    }
-
-    LOGI("Check authentication key set\n");
-    if (WPC_is_authentication_key_set(&key_set) != APP_RES_OK)
-    {
-        return false;
-    }
-
-    if (!key_set)
-    {
-        LOGE("Key is not set\n");
-        return false;
-    }
-
-    LOGI("Remove authentication key\n");
-    if (WPC_remove_authentication_key() != APP_RES_OK)
-    {
-        LOGE("Cannot remove key\n");
-        return false;
-    }
-
-    LOGI("Check authentication key not set\n");
-    if (WPC_is_authentication_key_set(&key_set) != APP_RES_OK)
-    {
-        return false;
-    }
-
-    if (key_set)
-    {
-        LOGE("Key is still set\n");
-        return false;
-    }
-
-    return true;
-}
-
-static bool testScratchpadTarget()
-{
-    uint8_t target_seq;
-    uint16_t target_crc;
-    uint8_t action;
-    uint8_t param;
-
-    // Read it a first time
-    if (WPC_read_target_scratchpad(&target_seq, &target_crc, &action, &param) != APP_RES_OK)
-    {
-        LOGE("Cannot read target scratchpad\n");
-        return false;
-    }
-
-    LOGI("Target is: %d, 0x%x, %d, %d\n", target_seq, target_crc, action, param);
-
-    app_res_e res = WPC_write_target_scratchpad(12, 0x1234, 2, 13);
-    if (res != APP_RES_OK)
-    {
-        LOGE("Cannot write target scratchpad %d\n", res);
-        return false;
-    }
-
-    LOGI("Write new target\n");
-
-    if (WPC_read_target_scratchpad(&target_seq, &target_crc, &action, &param))
-    {
-        LOGE("Cannot read target back scratchpad\n");
-        return false;
-    }
-
-    LOGI("Target read back is: %d, 0x%x, %d, %d\n", target_seq, target_crc, action, param);
-
-    if (target_seq != 12 || target_crc != 0x1234 || action != 2 || param != 13)
-    {
-        LOGE("Wrong read-back value\n");
-        return false;
-    }
-
-    return true;
-}
-
-void onDataSent(uint16_t pduid, uint32_t buffering_delay, uint8_t result)
-{
-    LOGI("Indication received for %d, delay=%d, result=%d\n", pduid, buffering_delay, result);
-}
-
-static bool testSendWithCallbacks()
-{
-    // Send 5 message to ourself
-    uint8_t data[] = "This is a test message #00 with ind\0";
-    for (int i = 0; i < 2; i++)
-    {
-        data[24] = i / 10 + 0x30;
-        data[25] = i % 10 + 0x30;
-        if (WPC_send_data(data, sizeof(data), i, APP_ADDR_ANYSINK, APP_QOS_HIGH, 50, 50, onDataSent, 0) !=
-            APP_RES_OK)
-        {
-            return false;
-        }
-    }
-    return true;
-}
-
-static bool testSendWithoutCallbacks()
-{
-    // Send 5 message to ourself
-    uint8_t data[] = "This is a test message #00\0";
-    for (int i = 2; i < 4; i++)
-    {
-        data[24] = i / 10 + 0x30;
-        data[25] = i % 10 + 0x30;
-        if (WPC_send_data(data, sizeof(data), i, APP_ADDR_ANYSINK, APP_QOS_HIGH, 50, 50, NULL, 0) != APP_RES_OK)
-        {
-            return false;
-        }
-    }
-    return true;
-}
-
-static bool testSendWithInitialTime()
-{
-    // Send 5 message to ourself
-    uint8_t data[] = "This is a test message #00\0";
-    for (int i = 4; i < 6; i++)
-    {
-        data[24] = i / 10 + 0x30;
-        data[25] = i % 10 + 0x30;
-        if (WPC_send_data(data, sizeof(data), i, APP_ADDR_ANYSINK, APP_QOS_HIGH, 50, 50, NULL, 300) !=
-            APP_RES_OK)
-        {
-            return false;
-        }
-    }
-    return true;
-}
-
-static void onAppConfigDataReceived(uint8_t seq, uint16_t interval, uint8_t * config_p)
-{
-    LOGI("AppConfig received %d, %d, %s\n", seq, interval, config_p);
-}
-
-static bool testAppConfigData()
-{
-    uint8_t new_config[128];
-    uint8_t max_size;
-
-    if (WPC_get_app_config_data_size(&max_size) != APP_RES_OK || (max_size > 128))
-    {
-        LOGE("Cannot get max app config size of bigger than 128 bytes\n");
-        return false;
-    }
-
-    for (uint8_t i = 0; i < max_size; i++)
-    {
-        new_config[i] = i;
-    }
-
-    if (!setAppconfig(new_config, 30, max_size))
-    {
-        LOGE("Cannot setAppConfig\n");
-        return false;
-    }
-
-    if (WPC_register_for_app_config_data(onAppConfigDataReceived) != APP_RES_OK)
-    {
-        LOGE("Cannot register for app_config\n");
-        return false;
-    }
-    return true;
-}
-
-static bool testMSAPAttributesStackOff()
-{
-    uint16_t min, max;
-    // Access cycle
-    int res = WPC_get_access_cycle_range(&min, &max);
-    if (res != APP_RES_OK && res != APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGE("Cannot get access cycle range\n");
-        return false;
-    }
-
-    if (res != APP_RES_ATTRIBUTE_NOT_SET)
-    {
-        LOGI("Access cycle range is %d - %d\n", min, max);
-    }
-
-    if (WPC_set_access_cycle_range(4000, 4000) != APP_RES_OK)
-    {
-        LOGE("Cannot set cycle range\n");
-        return false;
-    }
-    else
-    {
-        if (WPC_get_access_cycle_range(&min, &max) != APP_RES_OK)
-        {
-            LOGE("Cannot get cycle range\n");
-            return false;
-        }
-
-        if (min != 4000 || max != 4000)
-        {
-            LOGE("Cannot read back access cycle range set\n");
-            return false;
-        }
-    }
-    LOGI("Cycle range is now %d - %d\n", min, max);
-
-    if (WPC_get_access_cycle_limits(&min, &max) != APP_RES_OK)
-    {
-        LOGE("Cannot get cycle limits\n");
-        return false;
-    }
-    LOGI("Cycle range limits is %d - %d\n", min, max);
-    return true;
-}
-
-static bool testMSAPAttributesStackOn()
-{
-    app_res_e res;
-    uint8_t res8;
-    uint16_t res16;
-    uint32_t res32;
-
-    if (WPC_get_stack_status(&res8) != APP_RES_OK)
-    {
-        LOGE("Cannot get stack status\n");
-        return false;
-    }
-    LOGI("Stack status is %d\n", res8);
-
-    if (WPC_get_PDU_buffer_usage(&res8) != APP_RES_OK)
-    {
-        LOGE("Cannot get PDU buffer usage\n");
-        return false;
-    }
-    LOGI("PDU buffer usage is %d\n", res8);
-
-    if (WPC_get_PDU_buffer_capacity(&res8) != APP_RES_OK)
-    {
-        LOGE("Cannot get PDU buffer capacity\n");
-        return false;
-    }
-    LOGI("PDU buffer capacity is %d\n", res8);
-
-    // Battery remaining setting
-    res = WPC_get_remaining_energy(&res8);
-    if (res == APP_RES_INTERNAL_ERROR)
-    {
-        // Dualmcu API error code 1 while reading attributes
-        // means "Unsupported attribute id". That is mapped to
-        // APP_RES_INTERNAL_ERROR. Not ideal error code to
-        // indicate that the feature has been depricated or
-        // not implemented in this particular dualmcu implementation.
-        LOGI("Energy interface not implemented.\n");
-    }
-    else if (res != APP_RES_OK)
-    {
-        LOGE("Cannot get remaining battery usage. Result = 0x%02x.\n", res);
-        return false;
-    }
-    else
-    {
-        LOGI("Remaining battery is %d\n", res8);
-
-        if (WPC_set_remaining_energy(128) != APP_RES_OK)
-        {
-            LOGE("Cannot set remaining battery usage\n");
-            return false;
-        }
-        else
-        {
-            if (WPC_get_remaining_energy(&res8) != APP_RES_OK)
-            {
-                LOGE("Cannot get just set remaining battery usage\n");
-                return false;
-            }
-
-            if (res8 != 128)
-            {
-                LOGE("Expecting 128 but receive %d\n", res8);
-                return false;
-            }
-        }
-        LOGI("Able to set remaining battery to %d\n", res8);
-    }
-
-    if (WPC_get_autostart(&res8) != APP_RES_OK)
-    {
-        LOGE("Cannot get autostart setting\n");
-        return false;
-    }
-    LOGI("Autostart is %d\n", res8);
-
-    if (WPC_get_route_count(&res8) != APP_RES_OK)
-    {
-        LOGE("Cannot get route count\n");
-        return false;
-    }
-    LOGI("Route count is %d\n", res8);
-
-    if (WPC_get_system_time(&res32) != APP_RES_OK)
-    {
-        LOGE("Cannot get system time\n");
-        return false;
-    }
-    LOGI("System time is %d\n", res32);
-
-    if (WPC_get_current_access_cycle(&res16) != APP_RES_OK)
-    {
-        LOGE("Cannot get current access cycle\n");
-        return false;
-    }
-    LOGI("Current Access Cycle is %d\n", res16);
-
-    return true;
-}
-
-bool testClearScratchpad()
-{
-    app_scratchpad_status_t status;
-    if (WPC_clear_local_scratchpad() != APP_RES_OK)
-    {
-        LOGE("Cannot clear local scratchpad\n");
-        return false;
-    }
-
-    if (WPC_get_local_scratchpad_status(&status) != APP_RES_OK)
-    {
-        LOGE("Cannot get scratchpad status\n");
-        return false;
-    }
-
-    if (status.scrat_len != 0)
-    {
-        LOGE("Scratchpad is not cleared\n");
-        return false;
-    }
-
-    return true;
-}
-
-#define BLOCK_SIZE 128
-#define SEQ_NUMBER 50
-
-#define OTAP_UPLOAD_FILE_PATH "otap_files/dummy.otap"
-#define OTAP_DOWNLOAD_FILE_PATH "otap_files/downloaded.otap"
-
-bool testUploadScratchpad()
-{
-    FILE * fp;
-    app_scratchpad_status_t status;
-    long file_size = 0;
-    long written = 0;
-    const char * filename = OTAP_UPLOAD_FILE_PATH;
-    uint8_t block[BLOCK_SIZE];
-
-    fp = fopen(filename, "rb");
-    if (fp == NULL)
-    {
-        LOGE("Cannot open file %s. Please update OTAP_UPLOAD_FILE_PATH to a "
-             "valid "
-             "otap image\n",
-             filename);
-        return false;
-    }
-
-    /* Get size of binary */
-    fseek(fp, 0L, SEEK_END);
-    file_size = ftell(fp);
-    if (file_size <= 0)
-    {
-        LOGE("Cannot determine file size\n");
-        return false;
-    }
-    LOGI("Uploading otap file of %d bytes\n", file_size);
-
-    /* Set cursor to beginning of file */
-    fseek(fp, 0L, SEEK_SET);
-
-    /* Start scratchpad update*/
-    if (WPC_start_local_scratchpad_update(file_size, SEQ_NUMBER) != APP_RES_OK)
-    {
-        LOGE("Cannot start scratchpad update\n");
-        return false;
-    }
-
-    /* Send scratchpad image block by block */
-    LOGI("Start sending otap (it may take up to 1 minute)\n");
-    long remaining = file_size;
-    while (remaining > 0)
-    {
-        uint8_t block_size = (remaining > BLOCK_SIZE) ? BLOCK_SIZE : remaining;
-        size_t read;
-
-        read = fread(block, 1, block_size, fp);
-        if (read != block_size)
-        {
-            LOGE("Error while reading file asked = %d read = %d\n", block_size, read);
-            break;
-        }
-
-        /* Send the block */
-        if (WPC_upload_local_block_scratchpad(block_size, block, written) != APP_RES_OK)
-        {
-            LOGE("Cannot upload scratchpad block\n");
-            break;
-        }
-
-        written += block_size;
-        remaining -= block_size;
-    }
-
-    if (remaining > 0)
-    {
-        return false;
-    }
-
-    if (WPC_get_local_scratchpad_status(&status) != APP_RES_OK)
-    {
-        LOGE("Cannot get scratchpad status\n");
-        return false;
-    }
-
-    if (status.scrat_len != file_size)
-    {
-        LOGE("Scratchpad is not uploaded correctly (wrong size) %d vs %d\n",
-             status.scrat_len,
-             file_size);
-        return false;
-    }
-
-    if (status.scrat_seq_number != SEQ_NUMBER)
-    {
-        LOGE("Wrong seq number after uploading a scratchpad image \n");
-        return false;
-    }
-
-    return true;
-}
-
-bool testDownloadScratchpad()
-{
-    FILE * fp;
-    uint32_t scratchpad_size;
-    long read = 0;
-    const char * filename = OTAP_DOWNLOAD_FILE_PATH;
-    uint8_t block[BLOCK_SIZE];
-
-    fp = fopen(filename, "wb");
-    if (fp == NULL)
-    {
-        LOGE("Cannot open file %s. Please update OTAP_DOWNLOAD_FILE_PATH to a "
-             "valid path\n",
-             filename);
-        return false;
-    }
-
-    /* Get size of scratchpad */
-    if (WPC_get_scratchpad_size(&scratchpad_size) != APP_RES_OK)
-    {
-        LOGE("Cannot read scratchpad size\n");
-        return false;
-    }
-
-    LOGI("Downloading otap file of %d bytes\n", scratchpad_size);
-
-    /* Receive scratchpad image block by block */
-    LOGI("Start receiving otap (it may take up to 1 minute)\n");
-    uint32_t remaining = scratchpad_size;
-    while (remaining > 0)
-    {
-        uint8_t block_size = (remaining > BLOCK_SIZE) ? BLOCK_SIZE : remaining;
-
-        /* Receive the block */
-        const app_res_e download_res = WPC_download_local_scratchpad(block_size, block, read);
-        if (download_res != APP_RES_OK)
-        {
-            LOGE("Cannot download scratchpad block\n");
-            if (download_res == APP_RES_ACCESS_DENIED)
-            {
-                LOGE("Access denied when downloading scratchpad block. "
-                     "Make sure the dual MCU application was built with scratchpad reading enabled.\n");
-            }
-            break;
-        }
-
-        size_t written = fwrite(block, 1, block_size, fp);
-        if (written != block_size)
-        {
-            LOGE("Error while writing file asked = %d read = %d\n", block_size, written);
-            break;
-        }
-
-        read += block_size;
-        remaining -= block_size;
-    }
-
-    fclose(fp);
-
-    if (remaining > 0)
-    {
-        return false;
-    }
-
-    LOGI("Please compare files %s and %s\n", OTAP_UPLOAD_FILE_PATH, OTAP_DOWNLOAD_FILE_PATH);
-
-    return true;
-}
-
-static bool scan_done = false;
-
-static void onScanNeighborsDone(uint8_t status)
-{
-    LOGI("Scan neighbors is done res=%d\n", status);
-    scan_done = true;
-}
-
-static bool testScanNeighbors()
-{
-    app_nbors_t neighbors_list;
-
-    // Register for end of scan Neighbors
-    if (WPC_register_for_scan_neighbors_done(onScanNeighborsDone) != APP_RES_OK)
-    {
-        LOGE("Cannot register for remote status \n");
-        return false;
-    }
-
-    // Ask for a scan
-    if (WPC_start_scan_neighbors() != APP_RES_OK)
-    {
-        LOGE("Cannot start scan\n");
-        return false;
-    }
-
-    LOGI("Wait 5 seconds for scan result\n");
-    usleep(5 * 1000 * 1000);
-
-    // scan_done should be protected but we can assume
-    // that 5 sec is enough and no race can occur
-    if (!scan_done)
-    {
-        LOGE("Scan is not done\n");
-        return false;
-    }
-
-    if (WPC_get_neighbors(&neighbors_list) != APP_RES_OK)
-    {
-        LOGE("Cannot get neighbors list\n");
-        return false;
-    }
-
-    LOGI("Get neighbors done and node has %d neighbors\n", neighbors_list.number_of_neighbors);
-    if (neighbors_list.number_of_neighbors > 0)
-    {
-        LOGI("First node: %d, ch=%d, cost=%d, link=%d, type=%d, rssi=%d, "
-             "tx_power=%d, rx_power=%d, last_update=%d \n",
-             neighbors_list.nbors[0].add,
-             neighbors_list.nbors[0].channel,
-             neighbors_list.nbors[0].cost,
-             neighbors_list.nbors[0].link_rel,
-             neighbors_list.nbors[0].nbor_type,
-             neighbors_list.nbors[0].norm_rssi,
-             neighbors_list.nbors[0].tx_power,
-             neighbors_list.nbors[0].rx_power,
-             neighbors_list.nbors[0].last_update);
-    }
-
-    if (WPC_unregister_from_scan_neighbors_done() != APP_RES_OK)
-    {
-        LOGE("Cannot unregister from scan neighbors \n");
-        return false;
-    }
-
-    return true;
-}
-
-static void onStackStatusReceived(uint8_t status)
-{
-    LOGI("Stack status received %d\n", status);
-}
-
-static bool testStackStatus()
-{
-    if (WPC_register_for_stack_status(onStackStatusReceived) != APP_RES_OK)
-    {
-        return false;
-    }
-    return true;
-}
-
-// Macro to launch a test and check result
-#define RUN_TEST(_test_func_, _expected_result_)      \
-    do                                                \
-    {                                                 \
-        LOGI("### Starting test %s\n", #_test_func_); \
-        if (_test_func_() != _expected_result_)       \
-        {                                             \
-            LOGE("### Test is FAIL\n\n");             \
-            failedTestCount++;                        \
-        }                                             \
-        else                                          \
-        {                                             \
-            LOGI("### Test is PASS\n\n");             \
-            passedTestCount++;                        \
-        }                                             \
-    } while (0)
-
-int GetPassedTestCount()
-{
-    return passedTestCount;
-}
-
-int GetFailedTestCount()
-{
-    return failedTestCount;
-}
-
-int Test_runAll()
-{
-    uint8_t status;
-
-    RUN_TEST(testFactoryReset, true);
-
-    setInitialState(APP_ROLE_SINK, 1234, 0x654321, 5, false);
-
-    RUN_TEST(testScratchpadTarget, true);
-
-    RUN_TEST(dumpCSAPAttributes, true);
-
-    RUN_TEST(testAuthenticationKey, true);
-
-    RUN_TEST(testCipherKey, true);
-
-    RUN_TEST(testMSAPAttributesStackOff, true);
-
-    RUN_TEST(testStackStatus, true);
-
-    // Start the stack for following tests
-    WPC_start_stack();
-
-    // Ensure stack is started to avoid wrong test result
-    while (true)
-    {
-        if (WPC_get_stack_status(&status) != APP_RES_OK)
-        {
-            LOGI("Cannot read stack state\n");
-            continue;
-        }
-
-        if (status == 0)
-        {
-            break;
-        }
-        LOGI("Waiting for stack start\n");
-    }
-
-    RUN_TEST(testSendWithCallbacks, true);
-
-    RUN_TEST(testSendWithoutCallbacks, true);
-
-    RUN_TEST(testSendWithInitialTime, true);
-
-    RUN_TEST(testAppConfigData, true);
-
-    RUN_TEST(testMSAPAttributesStackOn, true);
-
-    RUN_TEST(testScanNeighbors, true);
-
-    return 0;
-}
-
-int Test_scratchpad()
-{
-    // Configure node as a sink
-    setInitialState(APP_ROLE_SINK, 1234, 0x654321, 5, false);
-
-    // Set app config
-    setAppconfig((uint8_t *) "Test scratchpad", 1800, 14);
-
-    RUN_TEST(testClearScratchpad, true);
-
-    RUN_TEST(testUploadScratchpad, true);
-
-    RUN_TEST(testDownloadScratchpad, true);
-
-    // Start the stack for following tests
-    WPC_start_stack();
-
-    return 0;
-}
diff --git a/test/test.h b/test/test.h
deleted file mode 100644
index 1969a82..0000000
--- a/test/test.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/* Wirepas Oy licensed under Apache License, Version 2.0
- *
- * See file LICENSE for full license details.
- *
- */
-
-/**
- * \brief   Run all the tests
- */
-int Test_runAll();
-
-/**
- * \brief   Test scratchpad functionality
- */
-int Test_scratchpad();
-
-/**
- * \brief   Returns number of passed test cases
- */
-int GetPassedTestCount();
-
-/**
- * \brief   Returns number of failed test cases
- */
-int GetFailedTestCount();
diff --git a/test/test_app.c b/test/test_app.c
deleted file mode 100644
index 6d4f4e9..0000000
--- a/test/test_app.c
+++ /dev/null
@@ -1,48 +0,0 @@
-/* Wirepas Oy licensed under Apache License, Version 2.0
- *
- * See file LICENSE for full license details.
- *
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include "test.h"
-#include "wpc.h"
-
-#define LOG_MODULE_NAME "TestApp"
-#define MAX_LOG_LEVEL INFO_LOG_LEVEL
-#include "logger.h"
-
-// Default serial port
-char * port_name = "/dev/ttyACM0";
-
-int main(int argc, char * argv[])
-{
-    unsigned long bitrate = DEFAULT_BITRATE;
-
-    if (argc > 1)
-    {
-        port_name = argv[1];
-    }
-
-    if (argc > 2)
-    {
-        bitrate = strtoul(argv[2], NULL, 0);
-    }
-
-    if (WPC_initialize(port_name, bitrate) != APP_RES_OK)
-        return -1;
-
-    Test_runAll();
-
-    Test_scratchpad();
-
-    WPC_close();
-
-    LOGI("Test summary: Passed: %d, Failed: %d\n", GetPassedTestCount(), GetFailedTestCount());
-
-    return GetFailedTestCount();
-}
diff --git a/test/test_main.cpp b/test/test_main.cpp
new file mode 100644
index 0000000..fa8dd7a
--- /dev/null
+++ b/test/test_main.cpp
@@ -0,0 +1,11 @@
+#include <gtest/gtest.h>
+
+#include "wpc_test.hpp"
+
+int main(int argc, char **argv)
+{
+    testing::InitGoogleTest(&argc, argv);
+    testing::AddGlobalTestEnvironment(new WpcTestEnvironment());
+    return RUN_ALL_TESTS();
+}
+
diff --git a/test/wpc_test.cpp b/test/wpc_test.cpp
new file mode 100644
index 0000000..d07c0b6
--- /dev/null
+++ b/test/wpc_test.cpp
@@ -0,0 +1,63 @@
+#include "wpc_test.hpp"
+
+std::string WpcTestEnvironment::GetSerialPort() const
+{
+    const auto envVar = std::getenv("WPC_SERIAL_PORT");
+    if (envVar != nullptr) {
+        return envVar;
+    } else {
+        return "/dev/ttyACM0";
+    }
+}
+
+unsigned long WpcTestEnvironment::GetBaudRate() const
+{
+    const auto envVar = std::getenv("WPC_BAUD_RATE");
+    if (envVar != nullptr) {
+        return atoi(envVar);
+    } else {
+        return 125000;
+    }
+}
+
+void WpcTestEnvironment::SetUp()
+{
+    const auto& serial_port = GetSerialPort();
+    const auto baud_rate = GetBaudRate();
+
+    if (WPC_initialize(serial_port.c_str(), baud_rate) != APP_RES_OK) {
+        std::cerr << "Could not initialize WPC with port: " << serial_port
+                  << " baud rate:" << baud_rate << std::endl;
+        is_initialized = false;
+    } else {
+        is_initialized = true;
+    }
+}
+
+void WpcTestEnvironment::TearDown()
+{
+    WPC_close();
+}
+
+bool WpcTestEnvironment::IsInitialized()
+{
+    return is_initialized;
+}
+
+void WpcTest::SetUpTestSuite()
+{
+    ASSERT_TRUE(WpcTestEnvironment::IsInitialized()) << "Environment is not initialized";
+}
+
+void WpcTest::StartStack()
+{
+    const auto res = WPC_start_stack();
+    ASSERT_TRUE(APP_RES_OK == res || APP_RES_STACK_ALREADY_STARTED == res);
+}
+
+void WpcTest::StopStack()
+{
+    const auto res = WPC_stop_stack();
+    ASSERT_TRUE(APP_RES_OK == res || APP_RES_STACK_ALREADY_STOPPED == res);
+}
+
diff --git a/test/wpc_test.hpp b/test/wpc_test.hpp
new file mode 100644
index 0000000..6f42371
--- /dev/null
+++ b/test/wpc_test.hpp
@@ -0,0 +1,33 @@
+#pragma once
+
+extern "C" {
+  #include <wpc.h>
+}
+
+#include <cstring>
+#include <gtest/gtest.h>
+
+#define ASSERT_EQ_ARRAY(first, second, size) \
+    ASSERT_EQ(0, std::memcmp(first, second, size));
+
+class WpcTestEnvironment : public testing::Environment
+{
+public:
+    void SetUp() override;
+    void TearDown() override;
+    static bool IsInitialized();
+private:
+    std::string GetSerialPort() const;
+    unsigned long GetBaudRate() const;
+    inline static bool is_initialized = false;
+};
+
+class WpcTest : public testing::Test
+{
+public:
+    static void SetUpTestSuite();
+protected:
+    static void StartStack();
+    static void StopStack();
+};
+