IVGCVSW-3436 Create the Periodic Counter Selection Command Handler

Change-Id: Ia6fe19db5aebe82bb00dcbab17e16633befda0a5
Signed-off-by: Ferran Balaguer <ferran.balaguer@arm.com>
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 10326cb..ef79ee1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -446,6 +446,8 @@
     src/profiling/ProfilingStateMachine.hpp
     src/profiling/ProfilingService.cpp
     src/profiling/ProfilingService.hpp
+    src/profiling/PeriodicCounterSelectionCommandHandler.cpp
+    src/profiling/PeriodicCounterSelectionCommandHandler.hpp
     third-party/half/half.hpp
     )
 
diff --git a/src/profiling/Packet.cpp b/src/profiling/Packet.cpp
index d0650a2..44d5ac1 100644
--- a/src/profiling/Packet.cpp
+++ b/src/profiling/Packet.cpp
@@ -31,7 +31,7 @@
     return m_Length;
 }
 
-const char* Packet::GetData()
+const char* Packet::GetData() const
 {
     return m_Data;
 }
diff --git a/src/profiling/Packet.hpp b/src/profiling/Packet.hpp
index b350f7c..c5e7f3c 100644
--- a/src/profiling/Packet.hpp
+++ b/src/profiling/Packet.hpp
@@ -35,7 +35,7 @@
     uint32_t GetPacketFamily() const;
     uint32_t GetPacketId() const;
     uint32_t GetLength() const;
-    const char* GetData();
+    const char* GetData() const;
 
     uint32_t GetPacketClass() const;
     uint32_t GetPacketType() const;
diff --git a/src/profiling/PeriodicCounterSelectionCommandHandler.cpp b/src/profiling/PeriodicCounterSelectionCommandHandler.cpp
new file mode 100644
index 0000000..9be37fc
--- /dev/null
+++ b/src/profiling/PeriodicCounterSelectionCommandHandler.cpp
@@ -0,0 +1,70 @@
+//
+// Copyright © 2019 Arm Ltd. All rights reserved.
+// SPDX-License-Identifier: MIT
+//
+
+#include "PeriodicCounterSelectionCommandHandler.hpp"
+#include "ProfilingUtils.hpp"
+
+#include <boost/numeric/conversion/cast.hpp>
+
+namespace armnn
+{
+
+namespace profiling
+{
+
+using namespace std;
+using boost::numeric_cast;
+
+void PeriodicCounterSelectionCommandHandler::ParseData(const Packet& packet, CaptureData& captureData)
+{
+    std::vector<uint16_t> counterIds;
+    uint32_t sizeOfUint32 = numeric_cast<uint32_t>(sizeof(uint32_t));
+    uint32_t sizeOfUint16 = numeric_cast<uint32_t>(sizeof(uint16_t));
+    uint32_t offset = 0;
+
+    if (packet.GetLength() > 0)
+    {
+        if (packet.GetLength() >= 4)
+        {
+            captureData.SetCapturePeriod(ReadUint32(reinterpret_cast<const unsigned char*>(packet.GetData()), offset));
+
+            unsigned int counters = (packet.GetLength() - 4) / 2;
+
+            if (counters > 0)
+            {
+                counterIds.reserve(counters);
+                offset += sizeOfUint32;
+                for(unsigned int pos = 0; pos < counters; ++pos)
+                {
+                    counterIds.emplace_back(ReadUint16(reinterpret_cast<const unsigned char*>(packet.GetData()),
+                                            offset));
+                    offset += sizeOfUint16;
+                }
+            }
+
+            captureData.SetCounterIds(counterIds);
+        }
+    }
+}
+
+void PeriodicCounterSelectionCommandHandler::operator()(const Packet& packet)
+{
+    CaptureData captureData;
+
+    ParseData(packet, captureData);
+
+    vector<uint16_t> counterIds = captureData.GetCounterIds();
+
+    m_CaptureDataHolder.SetCaptureData(captureData.GetCapturePeriod(), counterIds);
+
+    m_CaptureThread.Start();
+
+    // Write packet to Counter Stream Buffer
+    m_SendCounterPacket.SendPeriodicCounterSelectionPacket(captureData.GetCapturePeriod(), captureData.GetCounterIds());
+}
+
+} // namespace profiling
+
+} // namespace armnn
\ No newline at end of file
diff --git a/src/profiling/PeriodicCounterSelectionCommandHandler.hpp b/src/profiling/PeriodicCounterSelectionCommandHandler.hpp
new file mode 100644
index 0000000..e247e77
--- /dev/null
+++ b/src/profiling/PeriodicCounterSelectionCommandHandler.hpp
@@ -0,0 +1,50 @@
+//
+// Copyright © 2019 Arm Ltd. All rights reserved.
+// SPDX-License-Identifier: MIT
+//
+
+#pragma once
+
+#include "Packet.hpp"
+#include "CommandHandlerFunctor.hpp"
+#include "Holder.hpp"
+#include "SendCounterPacket.hpp"
+#include "IPeriodicCounterCapture.hpp"
+
+#include <vector>
+#include <thread>
+#include <atomic>
+
+namespace armnn
+{
+
+namespace profiling
+{
+
+class PeriodicCounterSelectionCommandHandler : public CommandHandlerFunctor
+{
+
+public:
+    PeriodicCounterSelectionCommandHandler(uint32_t packetId, uint32_t version, Holder& captureDataHolder,
+                                           IPeriodicCounterCapture& captureThread,
+                                           ISendCounterPacket& sendCounterPacket)
+    : CommandHandlerFunctor(packetId, version),
+    m_CaptureDataHolder(captureDataHolder),
+    m_CaptureThread(captureThread),
+    m_SendCounterPacket(sendCounterPacket)
+    {}
+
+    void operator()(const Packet& packet) override;
+
+
+private:
+    Holder& m_CaptureDataHolder;
+    IPeriodicCounterCapture& m_CaptureThread;
+    ISendCounterPacket& m_SendCounterPacket;
+    void ParseData(const Packet& packet, CaptureData& captureData);
+};
+
+} // namespace profiling
+
+} // namespace armnn
+
diff --git a/src/profiling/test/ProfilingTests.cpp b/src/profiling/test/ProfilingTests.cpp
index 25fae6c..7f7fe1c 100644
--- a/src/profiling/test/ProfilingTests.cpp
+++ b/src/profiling/test/ProfilingTests.cpp
@@ -12,12 +12,17 @@
 #include "../PacketVersionResolver.hpp"
 #include "../ProfilingService.hpp"
 #include "../ProfilingStateMachine.hpp"
+#include "../PeriodicCounterSelectionCommandHandler.hpp"
 #include "../ProfilingUtils.hpp"
 #include "../SocketProfilingConnection.hpp"
+#include "../IPeriodicCounterCapture.hpp"
+#include "SendCounterPacketTests.hpp"
 
 #include <Runtime.hpp>
 
+
 #include <boost/test/unit_test.hpp>
+#include <boost/numeric/conversion/cast.hpp>
 
 #include <cstdint>
 #include <cstring>
@@ -531,6 +536,109 @@
     BOOST_TEST(uid1 != uid2);
 }
 
+BOOST_AUTO_TEST_CASE(CounterSelectionCommandHandlerParseData)
+{
+    using boost::numeric_cast;
+
+    class TestCaptureThread : public IPeriodicCounterCapture
+    {
+        void Start() override {};
+    };
+
+    const uint32_t packetId = 0x40000;
+
+    uint32_t version = 1;
+    Holder holder;
+    TestCaptureThread captureThread;
+    MockBuffer mockBuffer(512);
+    SendCounterPacket sendCounterPacket(mockBuffer);
+
+    uint32_t sizeOfUint32 = numeric_cast<uint32_t>(sizeof(uint32_t));
+    uint32_t sizeOfUint16 = numeric_cast<uint32_t>(sizeof(uint16_t));
+
+    // Data with period and counters
+    uint32_t period1 = 10;
+    uint32_t dataLength1 = 8;
+    unsigned char data1[dataLength1];
+    uint32_t offset = 0;
+
+    WriteUint32(data1, offset, period1);
+    offset += sizeOfUint32;
+    WriteUint16(data1, offset, 4000);
+    offset += sizeOfUint16;
+    WriteUint16(data1, offset, 5000);
+
+    Packet packetA(packetId, dataLength1, reinterpret_cast<const char*>(data1));
+
+    PeriodicCounterSelectionCommandHandler commandHandler(packetId, version, holder, captureThread,
+                                                          sendCounterPacket);
+    commandHandler(packetA);
+
+    std::vector<uint16_t> counterIds = holder.GetCaptureData().GetCounterIds();
+
+    BOOST_TEST(holder.GetCaptureData().GetCapturePeriod() == period1);
+    BOOST_TEST(counterIds.size() == 2);
+    BOOST_TEST(counterIds[0] == 4000);
+    BOOST_TEST(counterIds[1] == 5000);
+
+    unsigned int size = 0;
+
+    const unsigned char* readBuffer = mockBuffer.GetReadBuffer(size);
+
+    offset = 0;
+
+    uint32_t headerWord0 = ReadUint32(readBuffer, offset);
+    offset += sizeOfUint32;
+    uint32_t headerWord1 = ReadUint32(readBuffer, offset);
+    offset += sizeOfUint32;
+    uint32_t period = ReadUint32(readBuffer, offset);
+
+    BOOST_TEST(((headerWord0 >> 26) & 0x3F) == 0);  // packet family
+    BOOST_TEST(((headerWord0 >> 16) & 0x3FF) == 4); // packet id
+    BOOST_TEST(headerWord1 == 8);                   // data lenght
+    BOOST_TEST(period == 10);                       // capture period
+
+    uint16_t counterId = 0;
+    offset += sizeOfUint32;
+    counterId = ReadUint16(readBuffer, offset);
+    BOOST_TEST(counterId == 4000);
+    offset += sizeOfUint16;
+    counterId = ReadUint16(readBuffer, offset);
+    BOOST_TEST(counterId == 5000);
+
+    // Data with period only
+    uint32_t period2 = 11;
+    uint32_t dataLength2 = 4;
+    unsigned char data2[dataLength2];
+
+    WriteUint32(data2, 0, period2);
+
+    Packet packetB(packetId, dataLength2, reinterpret_cast<const char*>(data2));
+
+    commandHandler(packetB);
+
+    counterIds = holder.GetCaptureData().GetCounterIds();
+
+    BOOST_TEST(holder.GetCaptureData().GetCapturePeriod() == period2);
+    BOOST_TEST(counterIds.size() == 0);
+
+    readBuffer = mockBuffer.GetReadBuffer(size);
+
+    offset = 0;
+
+    headerWord0 = ReadUint32(readBuffer, offset);
+    offset += sizeOfUint32;
+    headerWord1 = ReadUint32(readBuffer, offset);
+    offset += sizeOfUint32;
+    period = ReadUint32(readBuffer, offset);
+
+    BOOST_TEST(((headerWord0 >> 26) & 0x3F) == 0);  // packet family
+    BOOST_TEST(((headerWord0 >> 16) & 0x3FF) == 4); // packet id
+    BOOST_TEST(headerWord1 == 4);                   // data lenght
+    BOOST_TEST(period == 11);                       // capture period
+
+}
+
 BOOST_AUTO_TEST_CASE(CheckSocketProfilingConnection)
 {
     // Check that creating a SocketProfilingConnection results in an exception as the Gator UDS doesn't exist.
diff --git a/src/profiling/test/SendCounterPacketTests.cpp b/src/profiling/test/SendCounterPacketTests.cpp
index 626a5a6..4c4620f 100644
--- a/src/profiling/test/SendCounterPacketTests.cpp
+++ b/src/profiling/test/SendCounterPacketTests.cpp
@@ -1,11 +1,12 @@
 //
-// Copyright © 2017 Arm Ltd. All rights reserved.
+// Copyright © 2019 Arm Ltd. All rights reserved.
 // SPDX-License-Identifier: MIT
 //
 
-#include "../SendCounterPacket.hpp"
 #include "../ProfilingUtils.hpp"
 #include "../EncodeVersion.hpp"
+#include "../SendCounterPacket.hpp"
+#include "SendCounterPacketTests.hpp"
 
 #include <armnn/Exceptions.hpp>
 
@@ -17,91 +18,6 @@
 
 BOOST_AUTO_TEST_SUITE(SendCounterPacketTests)
 
-using namespace armnn::profiling;
-
-class MockBuffer : public IBufferWrapper
-{
-public:
-    MockBuffer(unsigned int size)
-    : m_BufferSize(size),
-      m_Buffer(std::make_unique<unsigned char[]>(size)) {}
-
-    unsigned char* Reserve(unsigned int requestedSize, unsigned int& reservedSize) override
-    {
-        if (requestedSize > m_BufferSize)
-        {
-            reservedSize = m_BufferSize;
-        }
-        else
-        {
-            reservedSize = requestedSize;
-        }
-
-        return m_Buffer.get();
-    }
-
-    void Commit(unsigned int size) override {}
-
-    const unsigned char* GetReadBuffer(unsigned int& size) override
-    {
-        size = static_cast<unsigned int>(strlen(reinterpret_cast<const char*>(m_Buffer.get())) + 1);
-        return m_Buffer.get();
-    }
-
-    void Release( unsigned int size) override {}
-
-private:
-    unsigned int m_BufferSize;
-    std::unique_ptr<unsigned char[]> m_Buffer;
-};
-
-class MockSendCounterPacket : public ISendCounterPacket
-{
-public:
-    MockSendCounterPacket(IBufferWrapper& sendBuffer) : m_Buffer(sendBuffer) {}
-
-    void SendStreamMetaDataPacket() override
-    {
-        std::string message("SendStreamMetaDataPacket");
-        unsigned int reserved = 0;
-        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
-        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
-    }
-
-    void SendCounterDirectoryPacket(const CounterDirectory& counterDirectory) override
-    {
-        std::string message("SendCounterDirectoryPacket");
-        unsigned int reserved = 0;
-        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
-        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
-    }
-
-    void SendPeriodicCounterCapturePacket(uint64_t timestamp,
-                                          const std::vector<std::pair<uint16_t, uint32_t>>& values) override
-    {
-        std::string message("SendPeriodicCounterCapturePacket");
-        unsigned int reserved = 0;
-        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
-        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
-    }
-
-    void SendPeriodicCounterSelectionPacket(uint32_t capturePeriod,
-                                            const std::vector<uint16_t>& selectedCounterIds) override
-    {
-        std::string message("SendPeriodicCounterSelectionPacket");
-        unsigned int reserved = 0;
-        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
-        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
-        m_Buffer.Commit(reserved);
-    }
-
-    void SetReadyToRead() override
-    {}
-
-private:
-    IBufferWrapper& m_Buffer;
-};
-
 BOOST_AUTO_TEST_CASE(MockSendCounterPacketTest)
 {
     unsigned int size = 0;
diff --git a/src/profiling/test/SendCounterPacketTests.hpp b/src/profiling/test/SendCounterPacketTests.hpp
new file mode 100644
index 0000000..a22d02b
--- /dev/null
+++ b/src/profiling/test/SendCounterPacketTests.hpp
@@ -0,0 +1,101 @@
+//
+// Copyright © 2019 Arm Ltd. All rights reserved.
+// SPDX-License-Identifier: MIT
+//
+
+#pragma once
+
+#include "../SendCounterPacket.hpp"
+#include "../ProfilingUtils.hpp"
+
+#include <armnn/Exceptions.hpp>
+
+#include <boost/test/unit_test.hpp>
+
+#include <chrono>
+#include <iostream>
+
+using namespace armnn::profiling;
+
+class MockBuffer : public IBufferWrapper
+{
+public:
+    MockBuffer(unsigned int size)
+            : m_BufferSize(size),
+              m_Buffer(std::make_unique<unsigned char[]>(size)) {}
+
+    unsigned char* Reserve(unsigned int requestedSize, unsigned int& reservedSize) override
+    {
+        if (requestedSize > m_BufferSize)
+        {
+            reservedSize = m_BufferSize;
+        }
+        else
+        {
+            reservedSize = requestedSize;
+        }
+
+        return m_Buffer.get();
+    }
+
+    void Commit(unsigned int size) override {}
+
+    const unsigned char* GetReadBuffer(unsigned int& size) override
+    {
+        size = static_cast<unsigned int>(strlen(reinterpret_cast<const char*>(m_Buffer.get())) + 1);
+        return m_Buffer.get();
+    }
+
+    void Release( unsigned int size) override {}
+
+private:
+    unsigned int m_BufferSize;
+    std::unique_ptr<unsigned char[]> m_Buffer;
+};
+
+class MockSendCounterPacket : public ISendCounterPacket
+{
+public:
+    MockSendCounterPacket(IBufferWrapper& sendBuffer) : m_Buffer(sendBuffer) {}
+
+    void SendStreamMetaDataPacket() override
+    {
+        std::string message("SendStreamMetaDataPacket");
+        unsigned int reserved = 0;
+        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
+        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
+    }
+
+    void SendCounterDirectoryPacket(const CounterDirectory& counterDirectory) override
+    {
+        std::string message("SendCounterDirectoryPacket");
+        unsigned int reserved = 0;
+        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
+        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
+    }
+
+    void SendPeriodicCounterCapturePacket(uint64_t timestamp,
+                                          const std::vector<std::pair<uint16_t, uint32_t>>& values) override
+    {
+        std::string message("SendPeriodicCounterCapturePacket");
+        unsigned int reserved = 0;
+        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
+        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
+    }
+
+    void SendPeriodicCounterSelectionPacket(uint32_t capturePeriod,
+                                            const std::vector<uint16_t>& selectedCounterIds) override
+    {
+        std::string message("SendPeriodicCounterSelectionPacket");
+        unsigned int reserved = 0;
+        unsigned char* buffer = m_Buffer.Reserve(1024, reserved);
+        memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
+        m_Buffer.Commit(reserved);
+    }
+
+    void SetReadyToRead() override
+    {}
+
+private:
+    IBufferWrapper& m_Buffer;
+};