opencsd: test: Add in CoreSight frame generator for test purposes

CoreSight frame generator takes in raw binary data along with a CoreSight Trace ID
and packs these into standard 16 byte formatted trace frames using the same format
as the CoreSight frame formatter.

This can be used in test programs to generate frames from raw packets.

Signed-off-by: Mike Leach <mike.leach@linaro.org>

2 files changed, 306 insertions, 0 deletions

diff --git a/decoder/include/common/cs_frame_mux_data.h b/decoder/include/common/cs_frame_mux_data.h
new file mode 100644
index 000000000000..efc58cb34932
--- /dev/null
+++ b/decoder/include/common/cs_frame_mux_data.h
@@ -0,0 +1,96 @@
+/*!
+ * \file       cs_frame_mux_data.h
+ * \brief      OpenCSD : Creates CSID muxed data frames for test data.
+ *
+ * \copyright  Copyright (c) 2024, ARM Limited. All Rights Reserved.
+ */
+
+ /*
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  * this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  * this list of conditions and the following disclaimer in the documentation
+  * and/or other materials provided with the distribution.
+  *
+  * 3. Neither the name of the copyright holder nor the names of its contributors
+  * may be used to endorse or promote products derived from this software without
+  * specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
+  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  */
+
+#ifndef ARM_CS_FRAME_MUX_DATA_H_INCLUDED
+#define ARM_CS_FRAME_MUX_DATA_H_INCLUDED
+
+
+#include <cstdio>
+#include <inttypes.h>
+#include <vector>
+
+class CSFrameMuxData
+{
+public:
+	CSFrameMuxData();
+	~CSFrameMuxData();
+
+
+	void initMux(const size_t frame_capacity = 16);	// initialise the muxer state with initial capacity for frames
+
+	/** creates CS data frames from input data associated with the ID.
+	 *
+	 * @in_data : current block of input data.
+	 * @in_size : size of input data.
+	 * @in_CSID : CS ID associated with this data.
+	 * @pad_end : pad last frame at end of data (otherwise wait for more input)
+	 *
+	 * calling with in_size == 0, and padEnd == true will cause any incomplete frame to be padded.
+	 *
+	 * return : bytes used
+	 */
+	int muxInData(const uint8_t* in_data, const uint32_t in_size, const uint8_t in_CSID, const bool padEnd);
+
+	/* extract complete frames into output buffer. Removes these frames from the current frame vector
+	 *  return bytes written to input buffer.
+	 */
+	int extractFrames(uint8_t* out_frame_buffer, const uint32_t out_size);
+
+	/* true if an incomplete frame exists */
+	const bool hasIncompleteFrame() const { return (bool)(curr_frame_idx != 0); };
+
+	/* number of complete frames in the buffer */
+	const int numFrames() const { return (cs_frames.size() / frame_size_bytes); };
+
+	/* direct access to frame buffer and size */
+	const uint8_t* getFrameBuffer() { return cs_frames.data(); };
+	size_t getFrameBufferSize() { return cs_frames.size(); };
+	void clearFrames(int nFrames); // remove copied frames
+
+
+private:
+	void copyCurrFrameToStack(); // copy the completed current frame to the frame stack
+	void setCSIDByte(const int frame_idx, const uint8_t newCSID, const bool nextDataNewID = true);
+	void padCurrFrame();
+
+	static const int frame_size_bytes = 16;
+
+	std::vector<uint8_t> cs_frames;	// completed frame data
+	int frames_since_id;	// coutn of frames since last ID
+	uint8_t curr_frame[frame_size_bytes];	// the current in progress frame.
+	int curr_frame_idx;		// index into current frame for the next data/id value
+	uint8_t curr_CSID;		// current CSID for the muxer
+};
+
+#endif // ARM_CS_FRAME_MUX_DATA_H_INCLUDED
diff --git a/decoder/source/cs_frame_mux_data.cpp b/decoder/source/cs_frame_mux_data.cpp
new file mode 100644
index 000000000000..865a4ba80885
--- /dev/null
+++ b/decoder/source/cs_frame_mux_data.cpp
@@ -0,0 +1,210 @@
+/*
+ * \file       cs_frame_mux_data.cpp
+ * \brief      OpenCSD : Class to take raw protocol data and mux into 16 byte coresight frames
+ *
+ * \copyright  Copyright (c) 2024, ARM Limited. All Rights Reserved.
+ */
+
+ /*
+  * Redistribution and use in source and binary forms, with or without modification,
+  * are permitted provided that the following conditions are met:
+  *
+  * 1. Redistributions of source code must retain the above copyright notice,
+  * this list of conditions and the following disclaimer.
+  *
+  * 2. Redistributions in binary form must reproduce the above copyright notice,
+  * this list of conditions and the following disclaimer in the documentation
+  * and/or other materials provided with the distribution.
+  *
+  * 3. Neither the name of the copyright holder nor the names of its contributors
+  * may be used to endorse or promote products derived from this software without
+  * specific prior written permission.
+  *
+  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND
+  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
+  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+  */
+
+#include <cstring>
+
+#include "common/cs_frame_mux_data.h"
+
+CSFrameMuxData::CSFrameMuxData()
+{
+	initMux();
+}
+
+CSFrameMuxData::~CSFrameMuxData()
+{
+
+}
+
+void CSFrameMuxData::initMux(const size_t frame_capacity /* = 16*/)
+{	
+	cs_frames.clear();
+	for (int i = 0; i < frame_size_bytes; i++)
+		curr_frame[i] = 0;
+	curr_frame_idx = 0;
+	curr_CSID = 0;
+	frames_since_id = 0;
+	// reserve 16 frames worth of data.
+	cs_frames.reserve(frame_capacity * frame_size_bytes);
+}
+
+void CSFrameMuxData::copyCurrFrameToStack()
+{
+	// copy current frame and clear
+	for (int i = 0; i < frame_size_bytes; i++)
+	{	
+		cs_frames.push_back(curr_frame[i]);
+		curr_frame[i] = 0;
+	}
+	curr_frame_idx = 0;
+	frames_since_id++;
+}
+
+void CSFrameMuxData::setCSIDByte(const int frame_idx, const uint8_t newCSID, const bool nextDataNewID /*= true*/)
+{	
+	curr_frame[frame_idx] = (newCSID << 1) | 0x1;
+	curr_CSID = newCSID;
+	frames_since_id = 0;
+
+	if (!nextDataNewID)
+	{
+		curr_frame[15] |= (uint8_t)0x1 << (frame_idx / 2);
+	}
+}
+
+// call if padding and incomplete frame. - curr_frame_idx != 0.
+void CSFrameMuxData::padCurrFrame()
+{
+	uint8_t z_bytes[14] = { 0 };
+	int bytesToPad;
+
+	// pad scenarios - just ID0 in byte 14
+	// 
+	if (curr_frame_idx == 14)
+	{
+		setCSIDByte(curr_frame_idx, 0);
+		copyCurrFrameToStack();
+	}
+	else 
+	{ 
+		// otherwise ID + n zero bytes.
+		bytesToPad = 14 - curr_frame_idx;
+		muxInData(z_bytes, bytesToPad, 0, false);
+	}
+}
+
+int CSFrameMuxData::muxInData(const uint8_t* in_data, const uint32_t in_size, const uint8_t in_CSID, const bool padEnd)
+{
+	bool newCSID = (bool)(in_CSID != curr_CSID);
+	uint32_t bytesProcessed = 0;
+	
+	while (bytesProcessed < in_size)
+	{
+		// odd index - Data byte - but handle ID change or flag byte
+		if (curr_frame_idx % 2)
+		{
+			// reached the flag byte - save the current frame and reset the index.
+			if (curr_frame_idx == 15)
+				copyCurrFrameToStack();
+			else
+			{
+				// data only - unless new ID
+				if (newCSID)
+				{
+					// CSID changed but data in the previous ID byte
+					// move to data to this byte and insert the ID in previous byte.
+					// flag data as being for previous ID
+					curr_frame[curr_frame_idx] = curr_frame[curr_frame_idx - 1];
+					setCSIDByte(curr_frame_idx - 1, in_CSID, false);
+				}
+				else
+				{
+					// simply add the data byte
+					curr_frame[curr_frame_idx] = in_data[bytesProcessed];
+					bytesProcessed++;
+				}
+				curr_frame_idx++;
+			}
+		}
+		// even index - ID / data byte
+		else
+		{
+			// periodically insert a trace ID
+			if ((curr_frame_idx == 0) && (frames_since_id >= 15))
+				newCSID = true;
+
+			// put ID in the ID/Data byte
+			if (newCSID)
+			{
+				setCSIDByte(curr_frame_idx, in_CSID);
+				newCSID = false;
+			}
+			else
+			{
+				// put data into the ID/Data byte. Save data bit 0 in flag byte
+				curr_frame[curr_frame_idx] = in_data[bytesProcessed] & 0xFE;
+				if (in_data[bytesProcessed] & 0x1)
+					curr_frame[15] |= (uint8_t)0x1 << (curr_frame_idx / 2);
+				bytesProcessed++;
+			}
+			curr_frame_idx++;
+		}
+	}
+
+	// exiting with all input data used, but reached flag byte
+	// so save the frame here
+	if (curr_frame_idx == 15)
+		copyCurrFrameToStack();
+
+	// pad if requested and incomplete frame.
+	if (padEnd && hasIncompleteFrame())
+		padCurrFrame();
+
+	return (int )bytesProcessed;
+}
+
+
+/* extract complete frames into output buffer. Removes these frames from the current frame vector
+ *  return bytes written to input buffer.
+ */
+int CSFrameMuxData::extractFrames(uint8_t* out_frame_buffer, const uint32_t out_size)
+{
+	int copyframes = out_size / frame_size_bytes;
+	int copybytes; 
+
+	// calculate number of frames that may be copied
+	if (copyframes > numFrames())
+		copyframes = numFrames();
+
+	copybytes = copyframes * frame_size_bytes;
+
+	if (copybytes)
+	{
+		// copy out the frames from the first to buffer size / end of frames - whichever larger
+		memcpy(out_frame_buffer, cs_frames.data(), copybytes);
+
+		// remove the copied frames from the buffer.
+		clearFrames(copyframes);
+	}
+
+	// return the number of bytes copied.
+	return copybytes;
+}
+
+// remove first nFrames from the buffer after extraction
+void CSFrameMuxData::clearFrames(int nFrames)
+{
+	if (nFrames > numFrames())
+		nFrames = numFrames();
+	cs_frames.erase(cs_frames.begin(), cs_frames.begin() + (nFrames * frame_size_bytes));
+}