IVGCVSW-3692 Implement SendPeriodicCounterCapturePacket() function

Change-Id: Ic976fc36955bec5e7721d1e34e89e7be79e23053
Signed-off-by: Francis Murtagh <francis.murtagh@arm.com>

6 files changed, 184 insertions, 14 deletions

diff --git a/src/profiling/ISendCounterPacket.hpp b/src/profiling/ISendCounterPacket.hpp
index eeec5d4b60..7f9e192b9d 100644
--- a/src/profiling/ISendCounterPacket.hpp
+++ b/src/profiling/ISendCounterPacket.hpp
@@ -16,6 +16,8 @@ namespace profiling
 class ISendCounterPacket
 {
 public:
+    using IndexValuePairsVector = std::vector<std::pair<uint16_t, uint32_t>>;
+
     /// Create and write a StreamMetaDataPacket in the buffer
     virtual void SendStreamMetaDataPacket() = 0;
 
@@ -23,8 +25,7 @@ public:
     virtual void SendCounterDirectoryPacket(const Category& category, const std::vector<Counter>& counters) = 0;
 
     /// Create and write a PeriodicCounterCapturePacket from the parameters to the buffer.
-    virtual void SendPeriodicCounterCapturePacket(uint64_t timestamp, const std::vector<uint32_t>& counterValues,
-                                                  const std::vector<uint16_t>& counterUids) = 0;
+    virtual void SendPeriodicCounterCapturePacket(uint64_t timestamp, const IndexValuePairsVector& values) = 0;
 
     /// Create and write a PeriodicCounterSelectionPacket from the parameters to the buffer.
     virtual void SendPeriodicCounterSelectionPacket(uint32_t capturePeriod,
diff --git a/src/profiling/ProfilingUtils.cpp b/src/profiling/ProfilingUtils.cpp
index 4dec7be6f2..b948026f42 100644
--- a/src/profiling/ProfilingUtils.cpp
+++ b/src/profiling/ProfilingUtils.cpp
@@ -13,6 +13,20 @@ namespace armnn
 namespace profiling
 {
 
+void WriteUint64(unsigned char* buffer, unsigned int offset, uint64_t value)
+{
+    BOOST_ASSERT(buffer);
+
+    buffer[offset]     = static_cast<unsigned char>(value & 0xFF);
+    buffer[offset + 1] = static_cast<unsigned char>((value >> 8) & 0xFF);
+    buffer[offset + 2] = static_cast<unsigned char>((value >> 16) & 0xFF);
+    buffer[offset + 3] = static_cast<unsigned char>((value >> 24) & 0xFF);
+    buffer[offset + 4] = static_cast<unsigned char>((value >> 32) & 0xFF);
+    buffer[offset + 5] = static_cast<unsigned char>((value >> 40) & 0xFF);
+    buffer[offset + 6] = static_cast<unsigned char>((value >> 48) & 0xFF);
+    buffer[offset + 7] = static_cast<unsigned char>((value >> 56) & 0xFF);
+}
+
 void WriteUint32(unsigned char* buffer, unsigned int offset, uint32_t value)
 {
     BOOST_ASSERT(buffer);
@@ -31,6 +45,23 @@ void WriteUint16(unsigned char* buffer, unsigned int offset, uint16_t value)
     buffer[offset + 1] = static_cast<unsigned char>((value >> 8) & 0xFF);
 }
 
+uint64_t ReadUint64(const unsigned char* buffer, unsigned int offset)
+{
+    BOOST_ASSERT(buffer);
+
+    uint64_t value = 0;
+    value = static_cast<uint64_t>(buffer[offset]);
+    value |= static_cast<uint64_t>(buffer[offset + 1]) << 8;
+    value |= static_cast<uint64_t>(buffer[offset + 2]) << 16;
+    value |= static_cast<uint64_t>(buffer[offset + 3]) << 24;
+    value |= static_cast<uint64_t>(buffer[offset + 4]) << 32;
+    value |= static_cast<uint64_t>(buffer[offset + 5]) << 40;
+    value |= static_cast<uint64_t>(buffer[offset + 6]) << 48;
+    value |= static_cast<uint64_t>(buffer[offset + 7]) << 56;
+
+    return value;
+}
+
 uint32_t ReadUint32(const unsigned char* buffer, unsigned int offset)
 {
     BOOST_ASSERT(buffer);
diff --git a/src/profiling/ProfilingUtils.hpp b/src/profiling/ProfilingUtils.hpp
index 12770aaee5..fe58ee13d1 100644
--- a/src/profiling/ProfilingUtils.hpp
+++ b/src/profiling/ProfilingUtils.hpp
@@ -5,6 +5,8 @@
 
 #pragma once
 
+#include <armnn/Exceptions.hpp>
+
 #include <stdint.h>
 
 namespace armnn
@@ -13,14 +15,23 @@ namespace armnn
 namespace profiling
 {
 
+void WriteUint64(unsigned char* buffer, unsigned int offset, uint64_t value);
+
 void WriteUint32(unsigned char* buffer, unsigned int offset, uint32_t value);
 
 void WriteUint16(unsigned char* buffer, unsigned int offset, uint16_t value);
 
+uint64_t ReadUint64(const unsigned char* buffer, unsigned int offset);
+
 uint32_t ReadUint32(const unsigned char* buffer, unsigned int offset);
 
 uint16_t ReadUint16(const unsigned char* buffer, unsigned int offset);
 
+class BufferExhaustion : public armnn::Exception
+{
+    using Exception::Exception;
+};
+
 } // namespace profiling
 
 } // namespace armnn
\ No newline at end of file
diff --git a/src/profiling/SendCounterPacket.cpp b/src/profiling/SendCounterPacket.cpp
index b99584cbf6..645b79196e 100644
--- a/src/profiling/SendCounterPacket.cpp
+++ b/src/profiling/SendCounterPacket.cpp
@@ -32,10 +32,57 @@ void SendCounterPacket::SendCounterDirectoryPacket(const Category& category, con
     throw armnn::UnimplementedException();
 }
 
-void SendCounterPacket::SendPeriodicCounterCapturePacket(uint64_t timestamp, const std::vector<uint32_t>& counterValues,
-                                                         const std::vector<uint16_t>& counterUids)
+void SendCounterPacket::SendPeriodicCounterCapturePacket(uint64_t timestamp, const IndexValuePairsVector& values)
 {
-    throw armnn::UnimplementedException();
+    uint32_t packetFamily = 1;
+    uint32_t packetClass = 0;
+    uint32_t packetType = 0;
+    uint32_t headerSize = numeric_cast<uint32_t>(2 * sizeof(uint32_t));
+    uint32_t bodySize = numeric_cast<uint32_t>((1 * sizeof(uint64_t)) +
+                                               (values.size() * (sizeof(uint16_t) + sizeof(uint32_t))));
+    uint32_t totalSize = headerSize + bodySize;
+    uint32_t offset = 0;
+    uint32_t reserved = 0;
+
+    unsigned char* writeBuffer = m_Buffer.Reserve(totalSize, reserved);
+
+    if (reserved < totalSize)
+    {
+        // Cancel the operation.
+        m_Buffer.Commit(0);
+        throw profiling::BufferExhaustion(
+                boost::str(boost::format("No space left in buffer. Unable to reserve (%1%) bytes.") % totalSize));
+    }
+
+    if (writeBuffer == nullptr)
+    {
+        // Cancel the operation.
+        m_Buffer.Commit(0);
+        throw RuntimeException("Error reserving buffer memory.");
+    }
+
+    // Create header.
+    WriteUint32(writeBuffer,
+                offset,
+                ((packetFamily & 0x3F) << 26) | ((packetClass & 0x3FF) << 19) | ((packetType & 0x3FFF) << 16));
+    offset += numeric_cast<uint32_t>(sizeof(uint32_t));
+    WriteUint32(writeBuffer, offset, bodySize);
+
+    // Copy captured Timestamp.
+    offset += numeric_cast<uint32_t>(sizeof(uint32_t));
+    WriteUint64(writeBuffer, offset, timestamp);
+
+    // Copy selectedCounterIds.
+    offset += numeric_cast<uint32_t>(sizeof(uint64_t));
+    for (const auto& pair: values)
+    {
+        WriteUint16(writeBuffer, offset, pair.first);
+        offset += numeric_cast<uint32_t>(sizeof(uint16_t));
+        WriteUint32(writeBuffer, offset, pair.second);
+        offset += numeric_cast<uint32_t>(sizeof(uint32_t));
+    }
+
+    m_Buffer.Commit(totalSize);
 }
 
 void SendCounterPacket::SendPeriodicCounterSelectionPacket(uint32_t capturePeriod,
@@ -44,7 +91,8 @@ void SendCounterPacket::SendPeriodicCounterSelectionPacket(uint32_t capturePerio
     uint32_t packetFamily = 0;
     uint32_t packetId = 4;
     uint32_t headerSize = numeric_cast<uint32_t>(2 * sizeof(uint32_t));
-    uint32_t bodySize = numeric_cast<uint32_t>((1 * sizeof(uint32_t)) + (selectedCounterIds.size() * sizeof(uint16_t)));
+    uint32_t bodySize   = numeric_cast<uint32_t>((1 * sizeof(uint32_t)) +
+                                                 (selectedCounterIds.size() * sizeof(uint16_t)));
     uint32_t totalSize = headerSize + bodySize;
     uint32_t offset = 0;
     uint32_t reserved = 0;
diff --git a/src/profiling/SendCounterPacket.hpp b/src/profiling/SendCounterPacket.hpp
index c1432b315d..453c0013e0 100644
--- a/src/profiling/SendCounterPacket.hpp
+++ b/src/profiling/SendCounterPacket.hpp
@@ -18,14 +18,15 @@ namespace profiling
 class SendCounterPacket : public ISendCounterPacket
 {
 public:
-    SendCounterPacket(IBufferWrapper& buffer) : m_Buffer(buffer), m_ReadyToRead(false) {}
+    using IndexValuePairsVector = std::vector<std::pair<uint16_t, uint32_t>>;
+
+    SendCounterPacket(IBufferWrapper& buffer) : m_Buffer(buffer), m_ReadyToRead(false) {};
 
     void SendStreamMetaDataPacket() override;
 
     void SendCounterDirectoryPacket(const Category& category, const std::vector<Counter>& counters) override;
 
-    void SendPeriodicCounterCapturePacket(uint64_t timestamp, const std::vector<uint32_t>& counterValues,
-                                          const std::vector<uint16_t>& counterUids) override;
+    void SendPeriodicCounterCapturePacket(uint64_t timestamp, const IndexValuePairsVector& values) override;
 
     void SendPeriodicCounterSelectionPacket(uint32_t capturePeriod,
                                             const std::vector<uint16_t>& selectedCounterIds) override;
diff --git a/src/profiling/test/SendCounterPacketTests.cpp b/src/profiling/test/SendCounterPacketTests.cpp
index 42a261bafb..89c05d1379 100644
--- a/src/profiling/test/SendCounterPacketTests.cpp
+++ b/src/profiling/test/SendCounterPacketTests.cpp
@@ -10,6 +10,7 @@
 
 #include <boost/test/unit_test.hpp>
 
+#include <chrono>
 #include <iostream>
 
 BOOST_AUTO_TEST_SUITE(SendCounterPacketTests)
@@ -73,8 +74,8 @@ public:
         memcpy(buffer, message.c_str(), static_cast<unsigned int>(message.size()) + 1);
     }
 
-    void SendPeriodicCounterCapturePacket(uint64_t timestamp, const std::vector<uint32_t>& counterValues,
-                                          const std::vector<uint16_t>& counterUids) override
+    void SendPeriodicCounterCapturePacket(uint64_t timestamp,
+                                          const std::vector<std::pair<uint16_t, uint32_t>>& values) override
     {
         std::string message("SendPeriodicCounterCapturePacket");
         unsigned int reserved = 0;
@@ -118,9 +119,9 @@ BOOST_AUTO_TEST_CASE(MockSendCounterPacketTest)
     BOOST_TEST(strcmp(buffer, "SendCounterDirectoryPacket") == 0);
 
     uint64_t timestamp = 0;
-    std::vector<uint32_t> counterValues;
-    std::vector<uint16_t> counterUids;
-    sendCounterPacket.SendPeriodicCounterCapturePacket(timestamp, counterValues, counterUids);
+    std::vector<std::pair<uint16_t, uint32_t>> indexValuePairs;
+
+    sendCounterPacket.SendPeriodicCounterCapturePacket(timestamp, indexValuePairs);
 
     BOOST_TEST(strcmp(buffer, "SendPeriodicCounterCapturePacket") == 0);
 
@@ -195,4 +196,81 @@ BOOST_AUTO_TEST_CASE(SendPeriodicCounterSelectionPacketTest)
     }
 }
 
+BOOST_AUTO_TEST_CASE(SendPeriodicCounterCapturePacketTest)
+{
+    // Error no space left in buffer
+    MockBuffer mockBuffer1(10);
+    SendCounterPacket sendPacket1(mockBuffer1);
+
+    auto captureTimestamp = std::chrono::steady_clock::now();
+    uint64_t time =  static_cast<uint64_t >(captureTimestamp.time_since_epoch().count());
+    std::vector<std::pair<uint16_t, uint32_t>> indexValuePairs;
+
+    BOOST_CHECK_THROW(sendPacket1.SendPeriodicCounterCapturePacket(time, indexValuePairs),
+                      BufferExhaustion);
+
+    // Packet without any counters
+    MockBuffer mockBuffer2(512);
+    SendCounterPacket sendPacket2(mockBuffer2);
+
+    sendPacket2.SendPeriodicCounterCapturePacket(time, indexValuePairs);
+    unsigned int sizeRead = 0;
+    const unsigned char* readBuffer2 = mockBuffer2.GetReadBuffer(sizeRead);
+
+    uint32_t headerWord0 = ReadUint32(readBuffer2, 0);
+    uint32_t headerWord1 = ReadUint32(readBuffer2, 4);
+    uint64_t readTimestamp = ReadUint64(readBuffer2, 8);
+
+    BOOST_TEST(((headerWord0 >> 26) & 0x3F) == 1);   // packet family
+    BOOST_TEST(((headerWord0 >> 19) & 0x3F) == 0);   // packet class
+    BOOST_TEST(((headerWord0 >> 16) & 0x3) == 0);    // packet type
+    BOOST_TEST(headerWord1 == 8);                    // data length
+    BOOST_TEST(time == readTimestamp);               // capture period
+
+    // Full packet message
+    MockBuffer mockBuffer3(512);
+    SendCounterPacket sendPacket3(mockBuffer3);
+
+    indexValuePairs.reserve(5);
+    indexValuePairs.emplace_back(std::make_pair<uint16_t, uint32_t >(0, 100));
+    indexValuePairs.emplace_back(std::make_pair<uint16_t, uint32_t >(1, 200));
+    indexValuePairs.emplace_back(std::make_pair<uint16_t, uint32_t >(2, 300));
+    indexValuePairs.emplace_back(std::make_pair<uint16_t, uint32_t >(3, 400));
+    indexValuePairs.emplace_back(std::make_pair<uint16_t, uint32_t >(4, 500));
+    sendPacket3.SendPeriodicCounterCapturePacket(time, indexValuePairs);
+    sizeRead = 0;
+    const unsigned char* readBuffer3 = mockBuffer3.GetReadBuffer(sizeRead);
+
+    headerWord0 = ReadUint32(readBuffer3, 0);
+    headerWord1 = ReadUint32(readBuffer3, 4);
+    uint64_t readTimestamp2 = ReadUint64(readBuffer3, 8);
+
+    BOOST_TEST(((headerWord0 >> 26) & 0x3F) == 1);   // packet family
+    BOOST_TEST(((headerWord0 >> 19) & 0x3F) == 0);   // packet class
+    BOOST_TEST(((headerWord0 >> 16) & 0x3) == 0);    // packet type
+    BOOST_TEST(headerWord1 == 38);                   // data length
+    BOOST_TEST(time == readTimestamp2);              // capture period
+
+    uint16_t counterIndex = 0;
+    uint32_t counterValue = 100;
+    uint32_t offset = 16;
+
+    // Counter Ids
+    for (auto it = indexValuePairs.begin(), end = indexValuePairs.end(); it != end; ++it)
+    {
+        // Check Counter Index
+        uint16_t readIndex = ReadUint16(readBuffer3, offset);
+        BOOST_TEST(counterIndex == readIndex);
+        counterIndex++;
+        offset += 2;
+
+        // Check Counter Value
+        uint32_t readValue = ReadUint32(readBuffer3, offset);
+        BOOST_TEST(counterValue == readValue);
+        counterValue += 100;
+        offset += 4;
+    }
+
+}
+
 BOOST_AUTO_TEST_SUITE_END()