mfc: Use CAMIF/FIMC for hardware color format conversion and de-tiling

5 files changed, 878 insertions, 85 deletions

diff --git a/sys/mfc/Makefile.am b/sys/mfc/Makefile.am
index d88dc182b..4126b1ef1 100644
--- a/sys/mfc/Makefile.am
+++ b/sys/mfc/Makefile.am
@@ -2,11 +2,13 @@ plugin_LTLIBRARIES = libgstmfc.la
 
 libgstmfc_la_SOURCES = \
 	mfc_decoder/mfc_decoder.c \
+	fimc/fimc.c \
 	gstmfc.c \
 	gstmfcdec.c
 
 noinst_HEADERS = \
 	mfc_decoder/mfc_decoder.h \
+	fimc/fimc.h \
 	gstmfcdec.h
 
 libgstmfc_la_CFLAGS = \
diff --git a/sys/mfc/fimc/fimc.c b/sys/mfc/fimc/fimc.c
new file mode 100644
index 000000000..40a72c79f
--- /dev/null
+++ b/sys/mfc/fimc/fimc.c
@@ -0,0 +1,592 @@
+/*
+ * Copyright (c) 2012 Collabora Ltd.
+ *   Author: Sebastian Dröge <sebastian.droege@collabora.co.uk>
+ *
+ * This library is licensed under 2 different licenses and you
+ * can choose to use it under the terms of any one of them. The
+ * two licenses are the Apache License 2.0 and the LGPL.
+ *
+ * Apache License 2.0:
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * LGPL:
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ *
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <string.h>
+#include <linux/videodev2.h>
+
+/* For logging */
+#include <gst/gst.h>
+GST_DEBUG_CATEGORY (fimc_debug);
+#define GST_CAT_DEFAULT fimc_debug
+
+#include "fimc.h"
+
+struct _Fimc
+{
+  int fd;
+
+  struct v4l2_capability caps;
+
+  int set_src;
+  FimcColorFormat src_format;
+  struct v4l2_format src_fmt;
+  struct v4l2_crop src_crop;
+  struct v4l2_requestbuffers src_requestbuffers;
+
+  int set_dst;
+  FimcColorFormat dst_format;
+  struct v4l2_format dst_fmt;
+  struct v4l2_crop dst_crop;
+  struct v4l2_requestbuffers dst_requestbuffers;
+
+  struct v4l2_plane dst_planes[3];
+  struct v4l2_buffer dst_buffer;
+  void *dst_buffer_data[3];
+  int dst_buffer_size[3];
+};
+
+#define FIMC_PATH "/dev/video4"
+
+static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
+static int fimc_in_use;
+
+void
+fimc_init_debug (void)
+{
+  GST_DEBUG_CATEGORY_INIT (fimc_debug, "fimc", 0, "FIMC library");
+}
+
+Fimc *
+fimc_new (void)
+{
+  Fimc *fimc;
+
+  pthread_mutex_lock (&mutex);
+  if (fimc_in_use) {
+    GST_ERROR ("Rejected because FIMC is already in use");
+    pthread_mutex_unlock (&mutex);
+    return NULL;
+  }
+  fimc_in_use = 1;
+  pthread_mutex_unlock (&mutex);
+
+  fimc = calloc (1, sizeof (Fimc));
+
+  fimc->fd = open (FIMC_PATH, O_RDWR, 0);
+  if (fimc->fd == -1) {
+    GST_ERROR ("Unable to open FIMC device node: %d", errno);
+    fimc_free (fimc);
+    return NULL;
+  }
+
+  /* check capabilities */
+
+  if (ioctl (fimc->fd, VIDIOC_QUERYCAP, &fimc->caps) < 0) {
+    GST_ERROR ("Unable to query capabilities: %d", errno);
+    fimc_free (fimc);
+    return NULL;
+  }
+
+  if ((fimc->caps.capabilities & V4L2_CAP_STREAMING) == 0 ||
+      (fimc->caps.capabilities & V4L2_CAP_VIDEO_OUTPUT_MPLANE) == 0 ||
+      (fimc->caps.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) == 0) {
+    GST_ERROR ("Required capabilities not available");
+    fimc_free (fimc);
+    return NULL;
+  }
+
+  return fimc;
+}
+
+void
+fimc_free (Fimc * fimc)
+{
+  int i;
+
+  for (i = 0; i < 3; i++) {
+    if (fimc->dst_buffer_data[i])
+      munmap (fimc->dst_buffer_data[i], fimc->dst_buffer_size[i]);
+  }
+
+  if (fimc->fd != -1)
+    close (fimc->fd);
+
+  pthread_mutex_lock (&mutex);
+  fimc_in_use = 0;
+  pthread_mutex_unlock (&mutex);
+  free (fimc);
+}
+
+static int
+fimc_color_format_to_v4l2 (FimcColorFormat format)
+{
+  switch (format) {
+    case FIMC_COLOR_FORMAT_YUV420SPT:
+      return V4L2_PIX_FMT_NV12MT;
+    case FIMC_COLOR_FORMAT_YUV420SP:
+      return V4L2_PIX_FMT_NV12M;
+    case FIMC_COLOR_FORMAT_YUV420P:
+      return V4L2_PIX_FMT_YUV420M;
+    default:
+      break;
+  }
+
+  return -1;
+}
+
+static int
+fimc_color_format_get_nplanes (FimcColorFormat format)
+{
+  switch (format) {
+    case FIMC_COLOR_FORMAT_YUV420SPT:
+    case FIMC_COLOR_FORMAT_YUV420SP:
+      return 2;
+    case FIMC_COLOR_FORMAT_YUV420P:
+      return 3;
+    default:
+      break;
+  }
+
+  return -1;
+}
+
+static int
+fimc_color_format_get_component_height (FimcColorFormat format, int c,
+    int height)
+{
+  switch (format) {
+    case FIMC_COLOR_FORMAT_YUV420SPT:
+    case FIMC_COLOR_FORMAT_YUV420SP:
+    case FIMC_COLOR_FORMAT_YUV420P:
+      if (c == 0)
+        return height;
+      else
+        return (height + 1) / 2;
+    default:
+      break;
+  }
+
+  return -1;
+}
+
+int
+fimc_set_src_format (Fimc * fimc, FimcColorFormat format, int width, int height,
+    int stride[3], int crop_left, int crop_top, int crop_width, int crop_height)
+{
+  struct v4l2_format fmt;
+  struct v4l2_crop crop;
+  struct v4l2_requestbuffers requestbuffers;
+  struct v4l2_control control;
+  int i;
+
+  /* Check if something changed */
+  if (fimc->set_src &&
+      fimc->src_fmt.fmt.pix_mp.width == width &&
+      fimc->src_fmt.fmt.pix_mp.height == height &&
+      fimc->src_fmt.fmt.pix_mp.pixelformat == fimc_color_format_to_v4l2 (format)
+      && fimc->src_crop.c.left == crop_left && fimc->src_crop.c.top == crop_top
+      && fimc->src_crop.c.width == crop_width
+      && fimc->src_crop.c.height == crop_height) {
+    if (fimc->src_requestbuffers.memory == V4L2_MEMORY_USERPTR
+        && fimc->src_fmt.fmt.pix_mp.plane_fmt[0].bytesperline == stride[0]
+        && fimc->src_fmt.fmt.pix_mp.plane_fmt[1].bytesperline == stride[1]
+        && fimc->src_fmt.fmt.pix_mp.plane_fmt[2].bytesperline == stride[2]) {
+      GST_DEBUG ("Nothing has changed");
+      return 0;
+    }
+  }
+
+  /* Something has changed */
+  fimc->set_src = 0;
+
+  memset (&fmt, 0, sizeof (fmt));
+  memset (&crop, 0, sizeof (crop));
+  memset (&requestbuffers, 0, sizeof (requestbuffers));
+  memset (&control, 0, sizeof (control));
+
+  fimc->src_format = format;
+
+  fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  fmt.fmt.pix_mp.width = width;
+  fmt.fmt.pix_mp.height = height;
+  fmt.fmt.pix_mp.pixelformat = fimc_color_format_to_v4l2 (format);
+  fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;
+  fmt.fmt.pix_mp.num_planes = fimc_color_format_get_nplanes (format);
+
+  for (i = 0; i < fmt.fmt.pix_mp.num_planes; i++) {
+    fmt.fmt.pix_mp.plane_fmt[i].bytesperline = stride[i];
+    fmt.fmt.pix_mp.plane_fmt[i].sizeimage =
+        fimc_color_format_get_component_height (format, i, height) * stride[i];
+  }
+
+  if (ioctl (fimc->fd, VIDIOC_S_FMT, &fmt) < 0) {
+    GST_ERROR ("Failed to set src format: %d", errno);
+    return -1;
+  }
+
+  fimc->src_fmt = fmt;
+
+  crop.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  crop.c.left = crop_left;
+  crop.c.top = crop_top;
+  crop.c.width = crop_width;
+  crop.c.height = crop_height;
+
+  if (ioctl (fimc->fd, VIDIOC_S_CROP, &crop) < 0) {
+    GST_ERROR ("Failed to set src crop: %d", errno);
+    return -1;
+  }
+
+  fimc->src_crop = crop;
+
+  requestbuffers.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  requestbuffers.memory = V4L2_MEMORY_USERPTR;
+  requestbuffers.count = 1;
+
+  if (ioctl (fimc->fd, VIDIOC_REQBUFS, &requestbuffers) < 0) {
+    GST_ERROR ("Failed to request src buffers: %d", errno);
+    return -1;
+  }
+
+  fimc->src_requestbuffers = requestbuffers;
+
+  if (requestbuffers.count < 1) {
+    GST_ERROR ("Got %d buffers instead of %d", requestbuffers.count, 1);
+    return -1;
+  }
+
+  control.id = V4L2_CID_ROTATE;
+  control.value = 0;
+
+  if (ioctl (fimc->fd, VIDIOC_S_CTRL, &control) < 0) {
+    GST_ERROR ("Failed to set rotation to 0: %d", errno);
+    return -1;
+  }
+
+  fimc->set_src = 1;
+
+  return 0;
+}
+
+int
+fimc_set_dst_format (Fimc * fimc, FimcColorFormat format, int width, int height,
+    int stride[3], int crop_left, int crop_top, int crop_width, int crop_height)
+{
+  struct v4l2_format fmt;
+  struct v4l2_crop crop;
+  struct v4l2_requestbuffers requestbuffers;
+  struct v4l2_control control;
+  int i;
+
+  /* Check if something changed */
+  if (fimc->set_dst &&
+      fimc->dst_fmt.fmt.pix_mp.width == width &&
+      fimc->dst_fmt.fmt.pix_mp.height == height &&
+      fimc->dst_fmt.fmt.pix_mp.pixelformat == fimc_color_format_to_v4l2 (format)
+      && fimc->dst_crop.c.left == crop_left && fimc->dst_crop.c.top == crop_top
+      && fimc->dst_crop.c.width == crop_width
+      && fimc->dst_crop.c.height == crop_height) {
+    if (stride) {
+      if (fimc->dst_requestbuffers.memory == V4L2_MEMORY_USERPTR &&
+          fimc->dst_fmt.fmt.pix_mp.plane_fmt[0].bytesperline == stride[0] &&
+          fimc->dst_fmt.fmt.pix_mp.plane_fmt[1].bytesperline == stride[1] &&
+          fimc->dst_fmt.fmt.pix_mp.plane_fmt[2].bytesperline == stride[2]) {
+        GST_DEBUG ("Nothing has changed");
+        return 0;
+      }
+    } else {
+      if (fimc->dst_requestbuffers.memory == V4L2_MEMORY_MMAP) {
+        GST_DEBUG ("Nothing has changed");
+        return 0;
+      }
+    }
+  }
+
+  /* Something has changed */
+  fimc->set_dst = 0;
+  for (i = 0; i < 3; i++) {
+    if (fimc->dst_buffer_data[i])
+      munmap (fimc->dst_buffer_data[i], fimc->dst_buffer_size[i]);
+    fimc->dst_buffer_data[i] = NULL;
+    fimc->dst_buffer_size[i] = 0;
+  }
+
+  memset (&fmt, 0, sizeof (fmt));
+  memset (&crop, 0, sizeof (crop));
+  memset (&requestbuffers, 0, sizeof (requestbuffers));
+  memset (&control, 0, sizeof (control));
+
+  fimc->dst_format = format;
+
+  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  fmt.fmt.pix_mp.width = width;
+  fmt.fmt.pix_mp.height = height;
+  fmt.fmt.pix_mp.pixelformat = fimc_color_format_to_v4l2 (format);
+  fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;
+  fmt.fmt.pix_mp.num_planes = fimc_color_format_get_nplanes (format);
+
+  if (stride) {
+    for (i = 0; i < fmt.fmt.pix_mp.num_planes; i++) {
+      fmt.fmt.pix_mp.plane_fmt[i].bytesperline = stride[i];
+      fmt.fmt.pix_mp.plane_fmt[i].sizeimage =
+          fimc_color_format_get_component_height (format, i,
+          height) * stride[i];
+    }
+  }
+
+  if (ioctl (fimc->fd, VIDIOC_S_FMT, &fmt) < 0) {
+    GST_ERROR ("Failed to set dst format: %d", errno);
+    return -1;
+  }
+
+  fimc->dst_fmt = fmt;
+
+  crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  crop.c.left = crop_left;
+  crop.c.top = crop_top;
+  crop.c.width = crop_width;
+  crop.c.height = crop_height;
+
+  if (ioctl (fimc->fd, VIDIOC_S_CROP, &crop) < 0) {
+    GST_ERROR ("Failed to set dst crop: %d", errno);
+    return -1;
+  }
+
+  fimc->dst_crop = crop;
+
+  requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  requestbuffers.memory = stride ? V4L2_MEMORY_USERPTR : V4L2_MEMORY_MMAP;
+  requestbuffers.count = 1;
+
+  if (ioctl (fimc->fd, VIDIOC_REQBUFS, &requestbuffers) < 0) {
+    GST_ERROR ("Failed to request dst buffers: %d", errno);
+    return -1;
+  }
+
+  fimc->dst_requestbuffers = requestbuffers;
+
+  if (requestbuffers.count < 1) {
+    GST_ERROR ("Got %d buffers instead of %d", requestbuffers.count, 1);
+    return -1;
+  }
+
+  control.id = V4L2_CID_ROTATE;
+  control.value = 0;
+
+  if (ioctl (fimc->fd, VIDIOC_S_CTRL, &control) < 0) {
+    GST_ERROR ("Failed to set rotation to 0: %d", errno);
+    return -1;
+  }
+
+  fimc->set_dst = 1;
+
+  return 0;
+}
+
+int
+fimc_set_dst_format_direct (Fimc * fimc, FimcColorFormat format, int width,
+    int height, int crop_left, int crop_top, int crop_width, int crop_height,
+    void *dst[3], int stride[3])
+{
+  struct v4l2_plane planes[3];
+  struct v4l2_buffer buffer;
+  int i;
+
+  memset (planes, 0, sizeof (planes));
+  memset (&buffer, 0, sizeof (buffer));
+
+  if (fimc_set_dst_format (fimc, format, width, height, NULL, crop_left,
+          crop_top, crop_width, crop_height) < 0)
+    return -1;
+
+  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  buffer.memory = V4L2_MEMORY_MMAP;
+  buffer.index = 0;
+  buffer.length = fimc_color_format_get_nplanes (format);
+  buffer.m.planes = planes;
+
+  if (ioctl (fimc->fd, VIDIOC_QUERYBUF, &buffer) < 0) {
+    GST_ERROR ("Query of output buffers failed: %d", errno);
+    return -1;
+  }
+
+  fimc->dst_planes[0] = planes[0];
+  fimc->dst_planes[1] = planes[1];
+  fimc->dst_planes[2] = planes[2];
+  fimc->dst_buffer = buffer;
+  fimc->dst_buffer.m.planes = fimc->dst_planes;
+
+  for (i = 0; i < buffer.length; i++) {
+    fimc->dst_buffer_data[i] =
+        mmap (NULL, buffer.m.planes[i].length, PROT_READ | PROT_WRITE,
+        MAP_SHARED, fimc->fd, buffer.m.planes[i].m.mem_offset);
+
+    if (fimc->dst_buffer_data[i] == MAP_FAILED) {
+      GST_ERROR ("Failed to map output buffer %d", i);
+      return -1;
+    }
+    fimc->dst_buffer_size[i] = buffer.m.planes[i].length;
+
+    dst[i] = fimc->dst_buffer_data[i];
+    stride[i] = fimc->dst_fmt.fmt.pix_mp.plane_fmt[i].bytesperline;
+  }
+
+  return 0;
+}
+
+static int
+fimc_convert_internal (Fimc * fimc, void *src[3], void *dst[3], int direct)
+{
+  struct v4l2_plane planes[3];
+  struct v4l2_buffer buffer;
+  enum v4l2_buf_type type;
+  int i;
+
+  memset (planes, 0, sizeof (planes));
+  memset (&buffer, 0, sizeof (buffer));
+
+  buffer.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  buffer.memory = V4L2_MEMORY_USERPTR;
+  buffer.length = fimc->src_fmt.fmt.pix_mp.num_planes;
+  buffer.index = 0;
+  buffer.m.planes = planes;
+
+  for (i = 0; i < buffer.length; i++) {
+    buffer.m.planes[i].length = fimc->src_fmt.fmt.pix_mp.plane_fmt[i].sizeimage;
+    buffer.m.planes[i].m.userptr = (unsigned long) src[i];
+  }
+
+  if (ioctl (fimc->fd, VIDIOC_QBUF, &buffer) < 0) {
+    GST_ERROR ("Failed to queue input buffer: %d", errno);
+    return -1;
+  }
+
+  memset (planes, 0, sizeof (planes));
+  memset (&buffer, 0, sizeof (buffer));
+
+  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  buffer.memory = direct ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR;
+  buffer.length = fimc->dst_fmt.fmt.pix_mp.num_planes;
+  buffer.index = 0;
+  buffer.m.planes = planes;
+
+  for (i = 0; i < buffer.length; i++) {
+    buffer.m.planes[i].length = fimc->dst_fmt.fmt.pix_mp.plane_fmt[i].sizeimage;
+    if (direct)
+      buffer.m.planes[i].m.mem_offset = fimc->dst_planes[i].m.mem_offset;
+    else
+      buffer.m.planes[i].m.userptr = (unsigned long) dst[i];
+  }
+
+  if (ioctl (fimc->fd, VIDIOC_QBUF, &buffer) < 0) {
+    GST_ERROR ("Failed to queue output buffer: %d", errno);
+    return -1;
+  }
+
+  type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  if (ioctl (fimc->fd, VIDIOC_STREAMON, &type) < 0) {
+    GST_ERROR ("Activating input stream failed: %d", errno);
+    return -1;
+  }
+
+  type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  if (ioctl (fimc->fd, VIDIOC_STREAMON, &type) < 0) {
+    GST_ERROR ("Activating output stream failed: %d", errno);
+    return -1;
+  }
+
+  memset (planes, 0, sizeof (planes));
+  memset (&buffer, 0, sizeof (buffer));
+
+  buffer.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  buffer.memory = V4L2_MEMORY_USERPTR;
+  buffer.length = fimc->src_fmt.fmt.pix_mp.num_planes;
+  buffer.m.planes = planes;
+
+  if (ioctl (fimc->fd, VIDIOC_DQBUF, &buffer) < 0) {
+    GST_ERROR ("Failed to dequeue input buffer: %d", errno);
+    return -1;
+  }
+
+  memset (planes, 0, sizeof (planes));
+  memset (&buffer, 0, sizeof (buffer));
+
+  buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  buffer.memory = direct ? V4L2_MEMORY_MMAP : V4L2_MEMORY_USERPTR;
+  buffer.length = fimc->dst_fmt.fmt.pix_mp.num_planes;
+  buffer.m.planes = planes;
+
+  if (ioctl (fimc->fd, VIDIOC_DQBUF, &buffer) < 0) {
+    GST_ERROR ("Failed to dequeue output buffer: %d", errno);
+    return -1;
+  }
+
+  type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  if (ioctl (fimc->fd, VIDIOC_STREAMOFF, &type) < 0) {
+    GST_ERROR ("Deactivating input stream failed: %d", errno);
+    return -1;
+  }
+
+  type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  if (ioctl (fimc->fd, VIDIOC_STREAMOFF, &type) < 0) {
+    GST_ERROR ("Deactivating output stream failed: %d", errno);
+    return -1;
+  }
+
+  return 0;
+}
+
+int
+fimc_convert (Fimc * fimc, void *src[3], void *dst[3])
+{
+  if (!fimc->set_src || !fimc->set_dst ||
+      fimc->dst_requestbuffers.memory == V4L2_MEMORY_MMAP)
+    return -1;
+
+  return fimc_convert_internal (fimc, src, dst, 0);
+}
+
+int
+fimc_convert_direct (Fimc * fimc, void *src[3])
+{
+  if (!fimc->set_src || !fimc->set_dst ||
+      fimc->dst_requestbuffers.memory == V4L2_MEMORY_USERPTR)
+    return -1;
+
+  return fimc_convert_internal (fimc, src, NULL, 1);
+}
diff --git a/sys/mfc/fimc/fimc.h b/sys/mfc/fimc/fimc.h
new file mode 100644
index 000000000..e35c78b12
--- /dev/null
+++ b/sys/mfc/fimc/fimc.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2012 Collabora Ltd.
+ *   Author: Sebastian Dröge <sebastian.droege@collabora.co.uk>
+ *
+ * This library is licensed under 2 different licenses and you
+ * can choose to use it under the terms of any one of them. The
+ * two licenses are the Apache License 2.0 and the LGPL.
+ *
+ * Apache License 2.0:
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ * LGPL:
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ *
+ */
+
+#ifndef __FIMC_H__
+#define __FIMC_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct _Fimc Fimc;
+
+typedef enum {
+  FIMC_COLOR_FORMAT_YUV420SPT,
+  FIMC_COLOR_FORMAT_YUV420SP,
+  FIMC_COLOR_FORMAT_YUV420P
+} FimcColorFormat;
+
+void fimc_init_debug (void);
+
+Fimc * fimc_new (void);
+void fimc_free (Fimc * fimc);
+
+int fimc_set_src_format (Fimc *fimc, FimcColorFormat format, int width, int height, int stride[3], int crop_left, int crop_top, int crop_width, int crop_height);
+
+int fimc_set_dst_format (Fimc *fimc, FimcColorFormat format, int width, int height, int stride[3], int crop_left, int crop_top, int crop_width, int crop_height);
+int fimc_set_dst_format_direct (Fimc *fimc, FimcColorFormat format, int width, int height, int crop_left, int crop_top, int crop_width, int crop_height, void *dst[3], int stride[3]);
+
+int fimc_convert (Fimc *fimc, void *src[3], void *dst[3]);
+int fimc_convert_direct (Fimc *fimc, void *src[3]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __FIMC_H__ */
diff --git a/sys/mfc/gstmfcdec.c b/sys/mfc/gstmfcdec.c
index 5b709279f..384c4d34d 100644
--- a/sys/mfc/gstmfcdec.c
+++ b/sys/mfc/gstmfcdec.c
@@ -35,7 +35,7 @@ static gboolean gst_mfc_dec_stop (GstVideoDecoder * decoder);
 static gboolean gst_mfc_dec_set_format (GstVideoDecoder * decoder,
     GstVideoCodecState * state);
 static gboolean gst_mfc_dec_reset (GstVideoDecoder * decoder, gboolean hard);
-static gboolean gst_mfc_dec_finish (GstVideoDecoder * decoder);
+static GstFlowReturn gst_mfc_dec_finish (GstVideoDecoder * decoder);
 static GstFlowReturn gst_mfc_dec_handle_frame (GstVideoDecoder * decoder,
     GstVideoCodecFrame * frame);
 
@@ -68,6 +68,7 @@ gst_mfc_dec_class_init (GstMFCDecClass * klass)
   video_decoder_class = (GstVideoDecoderClass *) klass;
 
   mfc_dec_init_debug ();
+  fimc_init_debug ();
 
   gst_element_class_add_pad_template (element_class,
       gst_static_pad_template_get (&gst_mfc_dec_src_template));
@@ -104,17 +105,35 @@ static gboolean
 gst_mfc_dec_start (GstVideoDecoder * decoder)
 {
   GstMFCDec *self = GST_MFC_DEC (decoder);
+  Fimc *fimc;
 
   GST_DEBUG_OBJECT (self, "Starting");
 
+  self->width = 0;
+  self->height = 0;
+  self->crop_left = 0;
+  self->crop_top = 0;
+  self->crop_width = 0;
+  self->crop_height = 0;
+
   /* Initialize with H264 here, we chose the correct codec in set_format */
   self->context = mfc_dec_create (CODEC_TYPE_H264, 1);
   if (!self->context) {
     GST_ELEMENT_ERROR (self, LIBRARY, INIT,
         ("Failed to initialize MFC decoder context"), (NULL));
+    return FALSE;
+  }
+
+  fimc = fimc_new ();
+
+  if (!fimc) {
+    GST_ELEMENT_ERROR (self, LIBRARY, INIT,
+        ("Failed to initialize FIMC context"), (NULL));
+    return FALSE;
   }
+  self->fimc = fimc;
 
-  return self->context != NULL;
+  return TRUE;
 }
 
 static gboolean
@@ -135,6 +154,11 @@ gst_mfc_dec_stop (GstVideoDecoder * video_decoder)
   }
   self->initialized = FALSE;
 
+  if (self->fimc) {
+    fimc_free (self->fimc);
+    self->fimc = NULL;
+  }
+
   GST_DEBUG_OBJECT (self, "Stopped");
 
   return TRUE;
@@ -207,14 +231,14 @@ gst_mfc_dec_queue_input (GstMFCDec * self, GstBuffer * inbuf)
   if (inbuf) {
     gst_buffer_map (inbuf, &map, GST_MAP_READ);
 
-    mfc_inbuf_data = mfc_buffer_get_input_data (mfc_inbuf);
+    mfc_inbuf_data = (guint8 *) mfc_buffer_get_input_data (mfc_inbuf);
     g_assert (mfc_inbuf_data != NULL);
     mfc_inbuf_size = mfc_buffer_get_input_max_size (mfc_inbuf);
 
     GST_DEBUG_OBJECT (self, "Have input buffer %p with size %d", mfc_inbuf_data,
         mfc_inbuf_size);
 
-    if (mfc_inbuf_size < map.size)
+    if ((gsize) mfc_inbuf_size < map.size)
       goto too_small_inbuf;
 
     memcpy (mfc_inbuf_data, map.data, map.size);
@@ -270,7 +294,7 @@ gst_mfc_dec_get_earliest_frame (GstMFCDec * self)
   frames = gst_video_decoder_get_frames (GST_VIDEO_DECODER (self));
 
   for (l = frames; l; l = l->next) {
-    GstVideoCodecFrame *tmp = l->data;
+    GstVideoCodecFrame *tmp = (GstVideoCodecFrame *) l->data;
 
     if (!frame) {
       frame = tmp;
@@ -299,109 +323,179 @@ gst_mfc_dec_dequeue_output (GstMFCDec * self)
   gint crop_left, crop_top, crop_width, crop_height;
   GstVideoCodecState *state = NULL;
   gint64 deadline;
-
-  GST_DEBUG_OBJECT (self, "Dequeueing output");
+  Fimc *fimc = NULL;
+  GstVideoFrame vframe;
 
   if (!self->initialized) {
+    GST_DEBUG_OBJECT (self, "Initializing decoder");
     if ((mfc_ret = mfc_dec_init (self->context, 1)) < 0)
       goto initialize_error;
     self->initialized = TRUE;
   }
 
-  if ((mfc_ret = mfc_dec_output_available (self->context)) == 0)
-    return GST_FLOW_OK;
-  else if (mfc_ret < 0)
-    goto output_available_error;
-
-  mfc_dec_get_output_size (self->context, &width, &height);
-  mfc_dec_get_crop_size (self->context, &crop_left, &crop_top, &crop_width,
-      &crop_height);
+  while ((mfc_ret = mfc_dec_output_available (self->context)) > 0) {
+    GST_DEBUG_OBJECT (self, "Dequeueing output");
+
+    mfc_dec_get_output_size (self->context, &width, &height);
+    mfc_dec_get_crop_size (self->context, &crop_left, &crop_top, &crop_width,
+        &crop_height);
+
+    GST_DEBUG_OBJECT (self, "Have output buffer: width %d, height %d, "
+        "crop_left %d, crop_right %d, "
+        "crop_width %d, crop_height %d", width, height,
+        crop_left, crop_top, crop_width, crop_height);
+
+    if (self->width != width || self->height != height ||
+        self->crop_left != self->crop_left || self->crop_top != crop_top ||
+        self->crop_width != crop_width || self->crop_height != crop_height) {
+      fimc = self->fimc;
+
+      if (fimc_set_src_format (fimc, FIMC_COLOR_FORMAT_YUV420SPT, width, height,
+              NULL, crop_left, crop_top, crop_width, crop_height) < 0)
+        goto fimc_src_error;
+
+      if (fimc_set_dst_format_direct (fimc, FIMC_COLOR_FORMAT_YUV420SPT, width,
+              height, crop_left, crop_top, crop_width, crop_height, self->dst,
+              self->stride) < 0)
+        goto fimc_dst_error;
+
+      self->width = width;
+      self->height = height;
+      self->crop_left = crop_left;
+      self->crop_top = crop_top;
+      self->crop_width = crop_width;
+      self->crop_height = crop_height;
+    }
 
-  GST_DEBUG_OBJECT (self, "Have output buffer: width %d, height %d, "
-      "crop_left %d, crop_right %d, "
-      "crop_width %d, crop_height %d", width, height,
-      crop_left, crop_top, crop_width, crop_height);
+    state = gst_video_decoder_get_output_state (GST_VIDEO_DECODER (self));
+    if (!state || state->info.width != crop_width
+        || state->info.height != crop_height) {
+      GST_DEBUG_OBJECT (self, "Creating new output state");
 
-  state = gst_video_decoder_get_output_state (GST_VIDEO_DECODER (self));
-  if (!state || state->info.width != crop_width
-      || state->info.height != crop_height) {
-    GST_DEBUG_OBJECT (self, "Creating new output state");
+      if (state)
+        gst_video_codec_state_unref (state);
+      state =
+          gst_video_decoder_set_output_state (GST_VIDEO_DECODER (self),
+          GST_VIDEO_FORMAT_NV12, crop_width, crop_height, self->input_state);
+    }
 
-    if (state)
-      gst_video_codec_state_unref (state);
-    state =
-        gst_video_decoder_set_output_state (GST_VIDEO_DECODER (self),
-        GST_VIDEO_FORMAT_NV12, crop_width, crop_height, self->input_state);
-  }
+    if ((mfc_ret = mfc_dec_dequeue_output (self->context, &mfc_outbuf)) < 0) {
+      if (mfc_ret == -2) {
+        GST_DEBUG_OBJECT (self, "Timeout dequeueing output, trying again");
+        mfc_ret = mfc_dec_dequeue_output (self->context, &mfc_outbuf);
+      }
 
-  if ((mfc_ret = mfc_dec_dequeue_output (self->context, &mfc_outbuf)) < 0) {
-    if (mfc_ret == -2) {
-      GST_DEBUG_OBJECT (self, "Timeout dequeueing output, trying again");
-      mfc_ret = mfc_dec_dequeue_output (self->context, &mfc_outbuf);
+      if (mfc_ret < 0)
+        goto dequeue_error;
     }
 
-    if (mfc_ret < 0)
-      goto dequeue_error;
-  }
+    g_assert (mfc_outbuf != NULL);
 
-  g_assert (mfc_outbuf != NULL);
+    /* FIXME: Replace this by gst_video_decoder_get_frame() with an ID */
+    frame = gst_mfc_dec_get_earliest_frame (self);
 
-  /* FIXME: Replace this by gst_video_decoder_get_frame() with an ID */
-  frame = gst_mfc_dec_get_earliest_frame (self);
+    if (frame) {
+      deadline =
+          gst_video_decoder_get_max_decode_time (GST_VIDEO_DECODER (self),
+          frame);
+      if (deadline < 0) {
+        GST_LOG_OBJECT (self,
+            "Dropping too late frame: deadline %" G_GINT64_FORMAT, deadline);
+        ret = gst_video_decoder_drop_frame (GST_VIDEO_DECODER (self), frame);
+        goto done;
+      }
 
-  if (frame) {
-    deadline =
-        gst_video_decoder_get_max_decode_time (GST_VIDEO_DECODER (self), frame);
-    if (deadline < 0) {
-      GST_LOG_OBJECT (self,
-          "Dropping too late frame: deadline %" G_GINT64_FORMAT, deadline);
-      ret = gst_video_decoder_drop_frame (GST_VIDEO_DECODER (self), frame);
-      goto done;
+      if (!frame->output_buffer)
+        ret =
+            gst_video_decoder_allocate_output_frame (GST_VIDEO_DECODER (self),
+            frame);
 
-    }
-    ret =
-        gst_video_decoder_allocate_output_frame (GST_VIDEO_DECODER (self),
-        frame);
+      if (ret != GST_FLOW_OK)
+        goto alloc_error;
 
-    if (ret != GST_FLOW_OK)
-      goto alloc_error;
+      outbuf = frame->output_buffer;
+    } else {
+      outbuf =
+          gst_video_decoder_allocate_output_buffer (GST_VIDEO_DECODER (self));
 
-    outbuf = frame->output_buffer;
-  } else {
-    outbuf =
-        gst_video_decoder_allocate_output_buffer (GST_VIDEO_DECODER (self));
+      if (!outbuf) {
+        ret = GST_FLOW_ERROR;
+        goto alloc_error;
+      }
+    }
 
-    if (!outbuf) {
-      ret = GST_FLOW_ERROR;
-      goto alloc_error;
+    {
+      const guint8 *mfc_outbuf_comps[3] = { NULL, };
+      gint i, h, w, src_stride, dst_stride;
+      guint8 *dst_, *src_;
+
+      fimc = self->fimc;
+
+      mfc_buffer_get_output_data (mfc_outbuf, (void **) &mfc_outbuf_comps[0],
+          (void **) &mfc_outbuf_comps[1]);
+
+      if (fimc_convert_direct (fimc, (void **) mfc_outbuf_comps) < 0)
+        goto fimc_convert_error;
+
+      if (!gst_video_frame_map (&vframe, &state->info, outbuf, GST_MAP_WRITE))
+        goto frame_map_error;
+
+      dst_ = (guint8 *) GST_VIDEO_FRAME_PLANE_DATA (&vframe, 0);
+      src_ = self->dst[0];
+      h = GST_VIDEO_FRAME_COMP_HEIGHT (&vframe, 0);
+      w = GST_VIDEO_FRAME_COMP_WIDTH (&vframe, 0);
+      src_stride = self->stride[0];
+      dst_stride = GST_VIDEO_FRAME_PLANE_STRIDE (&vframe, 0);
+      for (i = 0; i < h; i++) {
+        memcpy (dst_, src_, w);
+        dst_ += dst_stride;
+        src_ += src_stride;
+      }
+
+      dst_ = (guint8 *) GST_VIDEO_FRAME_PLANE_DATA (&vframe, 1);
+      src_ = self->dst[1];
+      h = GST_VIDEO_FRAME_COMP_HEIGHT (&vframe, 1);
+      w = GST_VIDEO_FRAME_COMP_WIDTH (&vframe, 1);
+      src_stride = self->stride[1];
+      dst_stride = GST_VIDEO_FRAME_PLANE_STRIDE (&vframe, 1);
+      for (i = 0; i < h; i++) {
+        memcpy (dst_, src_, w * 2);
+        dst_ += dst_stride;
+        src_ += src_stride;
+      }
+
+      gst_video_frame_unmap (&vframe);
     }
-  }
 
-  /* TODO: Here now copy the mfc_outbuf to outbuf using the FIMC detiler */
+    if (frame) {
+      ret = gst_video_decoder_finish_frame (GST_VIDEO_DECODER (self), frame);
+      frame = NULL;
+      outbuf = NULL;
+    } else {
+      ret = gst_pad_push (GST_VIDEO_DECODER_SRC_PAD (self), outbuf);
+      outbuf = NULL;
+    }
 
-  if (frame) {
-    ret = gst_video_decoder_finish_frame (GST_VIDEO_DECODER (self), frame);
-    frame = NULL;
-  } else {
-    ret = gst_pad_push (GST_VIDEO_DECODER_SRC_PAD (self), outbuf);
-  }
+    if (ret != GST_FLOW_OK)
+      GST_INFO_OBJECT (self, "Pushing frame returned: %s",
+          gst_flow_get_name (ret));
 
-  if (ret != GST_FLOW_OK)
-    GST_INFO_OBJECT (self, "Pushing frame returned: %s",
-        gst_flow_get_name (ret));
+  done:
+    if (mfc_outbuf) {
+      if ((mfc_ret = mfc_dec_enqueue_output (self->context, mfc_outbuf)) < 0)
+        goto enqueue_error;
+    }
 
-done:
-  if (mfc_outbuf) {
-    if ((mfc_ret = mfc_dec_enqueue_output (self->context, mfc_outbuf)) < 0)
-      goto enqueue_error;
-  }
+    if (!frame && outbuf)
+      gst_buffer_unref (outbuf);
+    if (state)
+      gst_video_codec_state_unref (state);
+    if (frame)
+      gst_video_codec_frame_unref (frame);
 
-  if (!frame && outbuf)
-    gst_buffer_unref (outbuf);
-  if (state)
-    gst_video_codec_state_unref (state);
-  if (frame)
-    gst_video_codec_frame_unref (frame);
+    if (ret != GST_FLOW_OK)
+      break;
+  }
 
   return ret;
 
@@ -413,11 +507,18 @@ initialize_error:
     goto done;
   }
 
-output_available_error:
+fimc_src_error:
+  {
+    GST_ELEMENT_ERROR (self, LIBRARY, FAILED,
+        ("Failed to set FIMC source parameters"), (NULL));
+    ret = GST_FLOW_ERROR;
+    goto done;
+  }
+
+fimc_dst_error:
   {
     GST_ELEMENT_ERROR (self, LIBRARY, FAILED,
-        ("Failed to check if output is available"),
-        ("mfc_dec_output_available: %d", mfc_ret));
+        ("Failed to set FIMC destination parameters"), (NULL));
     ret = GST_FLOW_ERROR;
     goto done;
   }
@@ -439,6 +540,22 @@ alloc_error:
     goto done;
   }
 
+frame_map_error:
+  {
+    GST_ELEMENT_ERROR (self, CORE, FAILED, ("Failed to map output buffer"),
+        (NULL));
+    ret = GST_FLOW_ERROR;
+    goto done;
+  }
+
+fimc_convert_error:
+  {
+    GST_ELEMENT_ERROR (self, LIBRARY, FAILED,
+        ("Failed to convert via FIMC"), (NULL));
+    ret = GST_FLOW_ERROR;
+    goto done;
+  }
+
 enqueue_error:
   {
     GST_ELEMENT_ERROR (self, LIBRARY, FAILED,
diff --git a/sys/mfc/gstmfcdec.h b/sys/mfc/gstmfcdec.h
index 0645f5bfa..773af49b8 100644
--- a/sys/mfc/gstmfcdec.h
+++ b/sys/mfc/gstmfcdec.h
@@ -27,6 +27,7 @@
 #include <gst/video/gstvideodecoder.h>
 
 #include "mfc_decoder/mfc_decoder.h"
+#include "fimc/fimc.h"
 
 G_BEGIN_DECLS
 
@@ -52,6 +53,13 @@ struct _GstMFCDec
   GstVideoCodecState *input_state;
   struct mfc_dec_context* context;
   gboolean initialized;
+  
+  Fimc *fimc;
+  gint width, height;
+  gint crop_left, crop_top;
+  gint crop_width, crop_height;
+  void *dst[3];
+  int stride[3];
 };
 
 struct _GstMFCDecClass