From ae5eedb0b1ab295881750f005230a195aa2f8d26 Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Sun, 17 Oct 2021 03:24:34 +0800
Subject: [PATCH]  Replay/framereader: fix c3 qcamera padding problem (#22572)

---
 selfdrive/ui/SConscript            |  2 +-
 selfdrive/ui/replay/framereader.cc | 18 ++++--------------
 selfdrive/ui/replay/framereader.h  |  1 -
 3 files changed, 5 insertions(+), 16 deletions(-)

diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript
index eb919f761c..c5bdced7ac 100644
--- a/selfdrive/ui/SConscript
+++ b/selfdrive/ui/SConscript
@@ -111,7 +111,7 @@ if arch in ['x86_64', 'Darwin'] and os.path.exists(Dir("#tools/").get_abspath())
   replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"]
 
   replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs)
-  replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'swscale', 'bz2', 'curl'] + qt_libs
+  replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'swscale', 'bz2', 'curl', 'yuv'] + qt_libs
   qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs)
 
   qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs)
diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc
index 67dc5add01..8f578835d0 100644
--- a/selfdrive/ui/replay/framereader.cc
+++ b/selfdrive/ui/replay/framereader.cc
@@ -1,6 +1,7 @@
 #include "selfdrive/ui/replay/framereader.h"
 
 #include <cassert>
+#include "libyuv.h"
 
 static int ffmpeg_lockmgr_cb(void **arg, enum AVLockOp op) {
   std::mutex *mutex = (std::mutex *)*arg;
@@ -49,9 +50,6 @@ FrameReader::~FrameReader() {
   for (auto &f : frames_) {
     av_free_packet(&f.pkt);
   }
-  if (frmRgb_) {
-    av_frame_free(&frmRgb_);
-  }
   if (pCodecCtx_) {
     avcodec_close(pCodecCtx_);
     avcodec_free_context(&pCodecCtx_);
@@ -93,9 +91,6 @@ bool FrameReader::load(const std::string &url) {
                             SWS_BILINEAR, NULL, NULL, NULL);
   if (!sws_ctx_) return false;
 
-  frmRgb_ = av_frame_alloc();
-  if (!frmRgb_) return false;
-
   frames_.reserve(60 * 20);  // 20fps, one minute
   do {
     Frame &frame = frames_.emplace_back();
@@ -176,14 +171,9 @@ std::pair<uint8_t *, uint8_t *> FrameReader::decodeFrame(AVPacket *pkt) {
     for (k = 0; k < f->height / 2; k++) {
       memcpy(yuv_data + f->width * i + f->width / 2 * j + f->width / 2 * k, f->data[2] + f->linesize[2] * k, f->width / 2);
     }
-
-    int ret = avpicture_fill((AVPicture *)frmRgb_, rgb_data, AV_PIX_FMT_BGR24, f->width, f->height);
-    assert(ret > 0);
-    if (sws_scale(sws_ctx_, (const uint8_t **)f->data, f->linesize, 0, f->height, frmRgb_->data, frmRgb_->linesize) <= 0) {
-      delete[] rgb_data;
-      delete[] yuv_data;
-      rgb_data = yuv_data = nullptr;
-    }
+    uint8_t *u = yuv_data + f->width * f->height;
+    uint8_t *v = u + (f->width / 2) * (f->height / 2);
+    libyuv::I420ToRGB24(yuv_data, f->width, u, f->width / 2, v, f->width / 2, rgb_data, f->width * 3, f->width, f->height);
   }
   av_frame_free(&f);
   return {rgb_data, yuv_data};
diff --git a/selfdrive/ui/replay/framereader.h b/selfdrive/ui/replay/framereader.h
index ce72b4e5b9..cb383e19ca 100644
--- a/selfdrive/ui/replay/framereader.h
+++ b/selfdrive/ui/replay/framereader.h
@@ -41,7 +41,6 @@ private:
 
   AVFormatContext *pFormatCtx_ = nullptr;
   AVCodecContext *pCodecCtx_ = nullptr;
-  AVFrame *frmRgb_ = nullptr;
   struct SwsContext *sws_ctx_ = nullptr;
 
   std::mutex mutex_;