From e3032cd897adf0f56de8bf1a9f8f4d82ec6254e0 Mon Sep 17 00:00:00 2001
From: Harald Welte <laforge@gnumonks.org>
Date: Fri, 27 May 2011 10:59:09 +0200
Subject: [PATCH] add matrix interleave for voice frames (untested)

---
 src/lower_mac/tetra_interleave.c | 23 +++++++++++++++++++++++
 src/lower_mac/tetra_interleave.h |  5 +++++
 2 files changed, 28 insertions(+)

diff --git a/src/lower_mac/tetra_interleave.c b/src/lower_mac/tetra_interleave.c
index 9b9b28f..635e5ee 100644
--- a/src/lower_mac/tetra_interleave.c
+++ b/src/lower_mac/tetra_interleave.c
@@ -57,3 +57,26 @@ void block_deinterleave(uint32_t K, uint32_t a, const uint8_t *in, uint8_t *out)
 		out[i-1] = in[k-1];
 	}
 }
+
+/* EN 300 395-2 Section 5.5.3 Matrix interleaving (voice */
+void matrix_interleave(uint32_t lines, uint32_t columns,
+			const uint8_t *in, uint8_t *out)
+{
+	int i, j;
+
+	for (i = 0; i < columns; i++) {
+		for (j = 0; j < lines; j++)
+			out[i*lines + columns] = in[j*columns + lines];
+	}
+}
+
+void matrix_deinterleave(uint32_t lines, uint32_t columns,
+			 const uint8_t *in, uint8_t *out)
+{
+	int i, j;
+
+	for (i = 0; i < columns; i++) {
+		for (j = 0; j < lines; j++)
+			out[j*columns + lines] = in[i*lines + columns];
+	}
+}
diff --git a/src/lower_mac/tetra_interleave.h b/src/lower_mac/tetra_interleave.h
index e08b80a..8fab647 100644
--- a/src/lower_mac/tetra_interleave.h
+++ b/src/lower_mac/tetra_interleave.h
@@ -9,4 +9,9 @@
 void block_interleave(uint32_t K, uint32_t a, const uint8_t *in, uint8_t *out);
 void block_deinterleave(uint32_t K, uint32_t a, const uint8_t *in, uint8_t *out);
 
+void matrix_interleave(uint32_t lines, uint32_t columns,
+			const uint8_t *in, uint8_t *out);
+void matrix_deinterleave(uint32_t lines, uint32_t columns,
+			 const uint8_t *in, uint8_t *out);
+
 #endif /* TETRA_INTERLEAVE_H */