C2SoftAomEnc: Add support for RGBA1010102 to YUV420Planar16 format

Bug: 267373580
Test: atest android.mediav2.cts.CodecEncoderSurfaceTest

Change-Id: Ifdaef70bda6e33af76c86bdfbffdb9120dfd4214
diff --git a/media/codec2/components/base/SimpleC2Component.cpp b/media/codec2/components/base/SimpleC2Component.cpp
index 32f8fa8..55a1164 100644
--- a/media/codec2/components/base/SimpleC2Component.cpp
+++ b/media/codec2/components/base/SimpleC2Component.cpp
@@ -452,6 +452,60 @@
     }
 }
 
+static const int16_t bt709Matrix_10bit[2][3][3] = {
+    { { 218, 732, 74 }, { -117, -395, 512 }, { 512, -465, -47 } }, /* RANGE_FULL */
+    { { 186, 627, 63 }, { -103, -345, 448 }, { 448, -407, -41 } }, /* RANGE_LIMITED */
+};
+
+static const int16_t bt2020Matrix_10bit[2][3][3] = {
+    { { 269, 694, 61 }, { -143, -369, 512 }, { 512, -471, -41 } }, /* RANGE_FULL */
+    { { 230, 594, 52 }, { -125, -323, 448 }, { 448, -412, -36 } }, /* RANGE_LIMITED */
+};
+
+void convertRGBA1010102ToYUV420Planar16(uint16_t* dstY, uint16_t* dstU, uint16_t* dstV,
+                                        const uint32_t* srcRGBA, size_t srcRGBStride, size_t width,
+                                        size_t height, C2Color::matrix_t colorMatrix,
+                                        C2Color::range_t colorRange) {
+    uint16_t r, g, b;
+    int32_t i32Y, i32U, i32V;
+    uint16_t zeroLvl =  colorRange == C2Color::RANGE_FULL ? 0 : 64;
+    uint16_t maxLvlLuma =  colorRange == C2Color::RANGE_FULL ? 1023 : 940;
+    uint16_t maxLvlChroma =  colorRange == C2Color::RANGE_FULL ? 1023 : 960;
+    // set default range as limited
+    if (colorRange != C2Color::RANGE_FULL) {
+        colorRange = C2Color::RANGE_LIMITED;
+    }
+    const int16_t(*weights)[3] = (colorMatrix == C2Color::MATRIX_BT709)
+                                         ? bt709Matrix_10bit[colorRange - 1]
+                                         : bt2020Matrix_10bit[colorRange - 1];
+
+    for (size_t y = 0; y < height; ++y) {
+        for (size_t x = 0; x < width; ++x) {
+            b = (srcRGBA[x]  >> 20) & 0x3FF;
+            g = (srcRGBA[x]  >> 10) & 0x3FF;
+            r = srcRGBA[x] & 0x3FF;
+
+            i32Y = ((r * weights[0][0] + g * weights[0][1] + b * weights[0][2] + 512) >> 10) +
+                   zeroLvl;
+            dstY[x] = CLIP3(zeroLvl, i32Y, maxLvlLuma);
+            if (y % 2 == 0 && x % 2 == 0) {
+                i32U = ((r * weights[1][0] + g * weights[1][1] + b * weights[1][2] + 512) >> 10) +
+                       512;
+                i32V = ((r * weights[2][0] + g * weights[2][1] + b * weights[2][2] + 512) >> 10) +
+                       512;
+                dstU[x >> 1] = CLIP3(zeroLvl, i32U, maxLvlChroma);
+                dstV[x >> 1] = CLIP3(zeroLvl, i32V, maxLvlChroma);
+            }
+        }
+        srcRGBA += srcRGBStride;
+        dstY += width;
+        if (y % 2 == 0) {
+            dstU += width / 2;
+            dstV += width / 2;
+        }
+    }
+}
+
 std::unique_ptr<C2Work> SimpleC2Component::WorkQueue::pop_front() {
     std::unique_ptr<C2Work> work = std::move(mQueue.front().work);
     mQueue.pop_front();