Massive update (Xv output, 2d HW compositing, VDE ABI sync)

18 files changed, 2566 insertions, 378 deletions

diff --git a/src/Makefile.am b/src/Makefile.am
index 72620f9..70513d7 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -1,4 +1,6 @@
-AM_CFLAGS = -Wall $(X11_CFLAGS) $(PIXMAN_CFLAGS) $(DRM_CFLAGS) $(CFLAG_VISIBILITY)
+AM_CFLAGS = -Wall -pthread \
+	    $(X11_CFLAGS) $(PIXMAN_CFLAGS) $(DRM_CFLAGS) $(XV_CFLAGS) \
+	    $(CFLAG_VISIBILITY)
 
 vdpau_tegra_libdir = $(libdir)/vdpau
 vdpau_tegra_lib_LTLIBRARIES = libvdpau_tegra.la
@@ -12,10 +14,13 @@ libvdpau_tegra_la_SOURCES = vdpau_tegra.c \
                             presentation_queue.c \
                             decoder.c \
                             dmabuf.c \
-                            bitstream.c
+                            bitstream.c \
+                            host1x-gr2d.c \
+                            host1x-pixelbuffer.c \
+                            tegra_stream.c
 
 libvdpau_tegra_la_LDFLAGS = -version-info 1:0:0 -module -Wl,-z,defs
-libvdpau_tegra_la_LIBADD  = -lm $(X11_LIBS) $(PIXMAN_LIBS) $(DRM_LIBS)
+libvdpau_tegra_la_LIBADD  = -lm $(X11_LIBS) $(PIXMAN_LIBS) $(DRM_LIBS) $(XV_LIBS)
 
 pkgconfigdir = ${libdir}/pkgconfig
 pkgconfig_DATA = vdpau-tegra.pc
diff --git a/src/atomic.h b/src/atomic.h
new file mode 100644
index 0000000..a199546
--- /dev/null
+++ b/src/atomic.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright © 2009 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Chris Wilson <chris@chris-wilson.co.uk>
+ *
+ */
+
+#ifndef _ATOMICS_H
+#define _ATOMICS_H
+
+#define atomic_read(x) ((x)->atomic)
+#define atomic_set(x, val) ((x)->atomic = (val))
+#define atomic_inc(x) ((void) __sync_fetch_and_add (&(x)->atomic, 1))
+#define atomic_inc_return(x) (__sync_add_and_fetch (&(x)->atomic, 1))
+#define atomic_dec_and_test(x) (__sync_add_and_fetch (&(x)->atomic, -1) == 0)
+#define atomic_add(x, v) ((void) __sync_add_and_fetch(&(x)->atomic, (v)))
+#define atomic_dec(x, v) ((void) __sync_sub_and_fetch(&(x)->atomic, (v)))
+#define atomic_cmpxchg(x, oldv, newv) __sync_val_compare_and_swap (&(x)->atomic, oldv, newv)
+
+#endif
diff --git a/src/decoder.c b/src/decoder.c
index a52bf47..cdc4648 100644
--- a/src/decoder.c
+++ b/src/decoder.c
@@ -72,7 +72,7 @@ static VdpStatus copy_bitstream_to_dmabuf(tegra_decoder *dec,
             return VDP_STATUS_INVALID_STRUCT_VERSION;
         }
 
-        assert(bytes < MAX_BITSTREAM_SIZE);
+        assert(bytes <= MAX_BITSTREAM_SIZE);
 
         memcpy(bitstream, bufs[i].bitstream, bufs[i].bitstream_bytes);
 
@@ -289,7 +289,7 @@ static VdpStatus tegra_decode_h264(tegra_decoder *dec, tegra_surface *surf,
 {
     struct tegra_vde_h264_decoder_ctx ctx;
     struct tegra_vde_h264_frame dpb_frames[17];
-    tegra_device *dev = get_device(dec->device);
+    tegra_device *dev = dec->dev;
     int32_t max_frame_num               = 1 << (info->log2_max_frame_num_minus4 + 4);
     int32_t delim_pic_order_cnt         = INT32_MAX;
     int ref_frames_with_earlier_poc_num = 0;
@@ -303,7 +303,7 @@ static VdpStatus tegra_decode_h264(tegra_decoder *dec, tegra_surface *surf,
     }
 
     if (info->entropy_coding_mode_flag) {
-        fprintf(stderr, "%s: CABAC decoding unimplemented\n", __func__);
+        ErrorMsg("CABAC decoding unimplemented\n");
         return VDP_STATUS_NO_IMPLEMENTATION;
     }
 
@@ -335,6 +335,7 @@ static VdpStatus tegra_decode_h264(tegra_decoder *dec, tegra_surface *surf,
     }
 
     ctx.bitstream_data_fd                   = dec->bitstream_data_fd;
+    ctx.bitstream_data_offset               = 0;
     ctx.dpb_frames_nb                       = 1 + refs_num;
     ctx.dpb_frames_ptr                      = (uintptr_t) dpb_frames;
     ctx.dpb_ref_frames_with_earlier_poc_nb  = ref_frames_with_earlier_poc_num;
@@ -358,6 +359,8 @@ static VdpStatus tegra_decode_h264(tegra_decoder *dec, tegra_surface *surf,
         return VDP_STATUS_ERROR;
     }
 
+    host1x_pixelbuffer_check_guard(surf->pixbuf);
+
     return VDP_STATUS_OK;
 }
 
@@ -442,6 +445,7 @@ VdpStatus vdp_decoder_create(VdpDevice device,
         is_baseline_profile = 1;
         break;
     case VDP_DECODER_PROFILE_H264_MAIN:
+    case VDP_DECODER_PROFILE_H264_HIGH: /* MPlayer compatibility */
         break;
     default:
         return VDP_STATUS_INVALID_DECODER_PROFILE;
@@ -472,9 +476,9 @@ VdpStatus vdp_decoder_create(VdpDevice device,
         return VDP_STATUS_RESOURCES;
     }
 
-    dec->device = device;
-    dec->width = width;
-    dec->height = height;
+    dec->dev = dev;
+    dec->width = ALIGN(width, 16);
+    dec->height = ALIGN(height, 16);
     dec->bitstream_data_fd = dmabuf_fd;
     dec->is_baseline_profile = is_baseline_profile;
 
@@ -493,6 +497,7 @@ VdpStatus vdp_decoder_destroy(VdpDecoder decoder)
 
     set_decoder(decoder, NULL);
 
+    close(dec->bitstream_data_fd);
     drm_tegra_bo_unref(dec->bitstream_bo);
     free(dec);
 
@@ -503,6 +508,12 @@ VdpStatus vdp_decoder_get_parameters(VdpDecoder decoder,
                                      VdpDecoderProfile *profile,
                                      uint32_t *width, uint32_t *height)
 {
+    if (width)
+        *width = ALIGN(*width, 16);
+
+    if (height)
+        *height = ALIGN(*height, 16);
+
     return VDP_STATUS_OK;
 }
 
@@ -528,8 +539,6 @@ VdpStatus vdp_decoder_render(VdpDecoder decoder,
 
     ret = tegra_decode_h264(dec, surf, picture_info);
 
-    assert(ret == VDP_STATUS_OK);
-
     if (ret != VDP_STATUS_OK) {
         return ret;
     }
diff --git a/src/host1x-gr2d.c b/src/host1x-gr2d.c
new file mode 100644
index 0000000..afa0216
--- /dev/null
+++ b/src/host1x-gr2d.c
@@ -0,0 +1,564 @@
+/*
+ * Copyright (c) 2012, 2013 Erik Faye-Lund
+ * Copyright (c) 2013 Thierry Reding
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#include "vdpau_tegra.h"
+
+#define FLOAT_TO_FIXED_6_12(fp) \
+    (((int32_t) (fp * 4096.0f + 0.5f)) & ((1 << 18) - 1))
+
+#define FLOAT_TO_FIXED_2_7(fp) \
+    ((((int32_t) (fp * 128.0f)) & 0x300) | \
+        (((int32_t) (fabs(fp) * 128.0f + 0.5f)) & ((1 << 8) - 1)))
+
+#define FLOAT_TO_FIXED_1_7(fp) \
+    ((((int32_t) (fp * 128.0f)) & 0x100) | \
+        (((int32_t) (fabs(fp) * 128.0f + 0.5f)) & ((1 << 8) - 1)))
+
+#define FLOAT_TO_FIXED_0_8(fp) \
+    (((int32_t) (fp * 256.0f + 0.5f)) & ((1 << 8) - 1))
+
+int host1x_gr2d_clear(struct tegra_stream *stream,
+                      struct host1x_pixelbuffer *pixbuf,
+                      uint32_t color)
+{
+    return host1x_gr2d_clear_rect(stream, pixbuf, color, 0, 0,
+                                  pixbuf->width, pixbuf->height);
+}
+
+int host1x_gr2d_clear_rect(struct tegra_stream *stream,
+                           struct host1x_pixelbuffer *pixbuf,
+                           uint32_t color,
+                           unsigned x, unsigned y,
+                           unsigned width, unsigned height)
+{
+    unsigned tiled = 0;
+    int err;
+
+    if (x + width > pixbuf->width)
+        return -EINVAL;
+
+    if (y + height > pixbuf->height)
+        return -EINVAL;
+
+    switch (pixbuf->layout) {
+    case PIX_BUF_LAYOUT_TILED_16x16:
+        tiled = 1;
+    case PIX_BUF_LAYOUT_LINEAR:
+        break;
+    default:
+        host1x_error("Invalid layout %u\n", pixbuf->layout);
+        return -EINVAL;
+    }
+
+    err = tegra_stream_begin(stream);
+    if (err < 0)
+        return err;
+
+    tegra_stream_push_setclass(stream, HOST1X_CLASS_GR2D);
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x09, 9));
+    tegra_stream_push(stream, 0x0000003a);
+    tegra_stream_push(stream, 0x00000000);
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x1e, 7));
+    tegra_stream_push(stream, 0x00000000); /* controlsecond */
+    tegra_stream_push(stream, /* controlmain */
+                      (PIX_BUF_FORMAT_BYTES(pixbuf->format) >> 1) << 16 |
+                      1 << 6 | /* srcsld */
+                      1 << 2 /* turbofill */);
+    tegra_stream_push(stream, 0x000000cc);
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x2b, 9));
+    tegra_stream_push_reloc(stream, pixbuf->bo, 0);
+    tegra_stream_push(stream, pixbuf->pitch);
+    tegra_stream_push(stream, HOST1X_OPCODE_NONINCR(0x35, 1));
+    tegra_stream_push(stream, color);
+    tegra_stream_push(stream, HOST1X_OPCODE_NONINCR(0x46, 1));
+    tegra_stream_push(stream, tiled << 20); /* tilemode */
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x38, 5));
+    tegra_stream_push(stream, height << 16 | width);
+    tegra_stream_push(stream, y << 16 | x);
+
+    err = tegra_stream_end(stream);
+    if (err < 0)
+        return err;
+
+    err = tegra_stream_flush(stream);
+    if (err < 0)
+        return err;
+
+    host1x_pixelbuffer_check_guard(pixbuf);
+
+    return 0;
+}
+
+int host1x_gr2d_clear_rect_clipped(struct tegra_stream *stream,
+                                   struct host1x_pixelbuffer *pixbuf,
+                                   uint32_t color,
+                                   unsigned x, unsigned y,
+                                   unsigned width, unsigned height,
+                                   unsigned clip_x0, unsigned clip_y0,
+                                   unsigned clip_x1, unsigned clip_y1,
+                                   bool draw_outside)
+{
+    unsigned tiled = 0;
+    int err;
+
+    if (x + width > pixbuf->width)
+        return -EINVAL;
+
+    if (y + height > pixbuf->height)
+        return -EINVAL;
+
+    switch (pixbuf->layout) {
+    case PIX_BUF_LAYOUT_TILED_16x16:
+        tiled = 1;
+    case PIX_BUF_LAYOUT_LINEAR:
+        break;
+    default:
+        host1x_error("Invalid layout %u\n", pixbuf->layout);
+        return -EINVAL;
+    }
+
+    err = tegra_stream_begin(stream);
+    if (err < 0)
+        return err;
+
+    tegra_stream_push_setclass(stream, HOST1X_CLASS_GR2D);
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x09, 9));
+    tegra_stream_push(stream, 0x0000003a);
+    tegra_stream_push(stream, 0x00000000);
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x1e, 7));
+    tegra_stream_push(stream, /* controlsecond */
+                      (draw_outside ? 3 : 1) << 21 /* clip inside/outside */);
+    tegra_stream_push(stream, /* controlmain */
+                      (PIX_BUF_FORMAT_BYTES(pixbuf->format) >> 1) << 16 |
+                      1 << 6 /* srcsld */);
+    tegra_stream_push(stream, 0x000000cc);
+    tegra_stream_push(stream, HOST1X_OPCODE_INCR(0x22, 2));
+    tegra_stream_push(stream, clip_y0 << 16 | clip_x0);
+    tegra_stream_push(stream, clip_y1 << 16 | clip_x1);
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x2b, 9));
+    tegra_stream_push_reloc(stream, pixbuf->bo, 0);
+    tegra_stream_push(stream, pixbuf->pitch);
+    tegra_stream_push(stream, HOST1X_OPCODE_NONINCR(0x35, 1));
+    tegra_stream_push(stream, color);
+    tegra_stream_push(stream, HOST1X_OPCODE_NONINCR(0x46, 1));
+    tegra_stream_push(stream, tiled << 20); /* tilemode */
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x38, 5));
+    tegra_stream_push(stream, height << 16 | width);
+    tegra_stream_push(stream, y << 16 | x);
+
+    err = tegra_stream_end(stream);
+    if (err < 0)
+        return err;
+
+    err = tegra_stream_flush(stream);
+    if (err < 0)
+        return err;
+
+    host1x_pixelbuffer_check_guard(pixbuf);
+
+    return 0;
+}
+
+int host1x_gr2d_blit(struct tegra_stream *stream,
+                     struct host1x_pixelbuffer *src,
+                     struct host1x_pixelbuffer *dst,
+                     unsigned int sx, unsigned int sy,
+                     unsigned int dx, unsigned int dy,
+                     unsigned int width, int height)
+{
+    unsigned src_tiled = 0;
+    unsigned dst_tiled = 0;
+    unsigned yflip = 0;
+    unsigned xdir = 0;
+    unsigned ydir = 0;
+    int err;
+
+    if (PIX_BUF_FORMAT_BYTES(src->format) !=
+        PIX_BUF_FORMAT_BYTES(dst->format))
+    {
+        host1x_error("Unequal bytes size\n");
+        return -EINVAL;
+    }
+
+    switch (src->layout) {
+    case PIX_BUF_LAYOUT_TILED_16x16:
+        src_tiled = 1;
+    case PIX_BUF_LAYOUT_LINEAR:
+        break;
+    default:
+        host1x_error("Invalid src layout %u\n", src->layout);
+        return -EINVAL;
+    }
+
+    switch (dst->layout) {
+    case PIX_BUF_LAYOUT_TILED_16x16:
+        dst_tiled = 1;
+    case PIX_BUF_LAYOUT_LINEAR:
+        break;
+    default:
+        host1x_error("Invalid dst layout %u\n", dst->layout);
+        return -EINVAL;
+    }
+
+    if (height < 0) {
+        yflip = 1;
+        height = -height;
+    }
+
+    if (sx + width > src->width ||
+        dx + width > dst->width ||
+        sy + height > src->height ||
+        dy + height > dst->height) {
+        host1x_error("Coords out of range\n");
+        return -EINVAL;
+    }
+
+    if (src != dst)
+        goto yflip_setup;
+
+    if (sx >= dx + width || sx + width <= dx)
+        goto yflip_setup;
+
+    if (sy >= dy + height || sy + height <= dy)
+        goto yflip_setup;
+
+    if (dx > sx) {
+        xdir = 1;
+        sx += width - 1;
+        dx += width - 1;
+    }
+
+    if (dy > sy) {
+        ydir = 1;
+        sy += height - 1;
+        dy += height - 1;
+    }
+
+yflip_setup:
+    if (yflip && !ydir)
+        dy += height - 1;
+
+    err = tegra_stream_begin(stream);
+    if (err < 0)
+        return err;
+
+    tegra_stream_push_setclass(stream, HOST1X_CLASS_GR2D);
+
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x009, 9));
+    tegra_stream_push(stream, 0x0000003a); /* trigger */
+    tegra_stream_push(stream, 0x00000000); /* cmdsel */
+
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x01e, 0x7));
+    tegra_stream_push(stream, 0x00000000); /* controlsecond */
+    /*
+     * [20:20] source color depth (0: mono, 1: same)
+     * [17:16] destination color depth (0: 8 bpp, 1: 16 bpp, 2: 32 bpp)
+     */
+    tegra_stream_push(stream, /* controlmain */
+                      1 << 20 |
+                      (PIX_BUF_FORMAT_BYTES(dst->format) >> 1) << 16 |
+                      yflip << 14 | ydir << 10 | xdir << 9);
+    tegra_stream_push(stream, 0x000000cc); /* ropfade */
+
+    tegra_stream_push(stream, HOST1X_OPCODE_NONINCR(0x046, 1));
+    /*
+     * [20:20] destination write tile mode (0: linear, 1: tiled)
+     * [ 0: 0] tile mode Y/RGB (0: linear, 1: tiled)
+     */
+    tegra_stream_push(stream, dst_tiled << 20 | src_tiled); /* tilemode */
+
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x02b, 0xe149));
+    tegra_stream_push_reloc(stream, dst->bo, 0); /* dstba */
+    tegra_stream_push(stream, dst->pitch); /* dstst */
+    tegra_stream_push_reloc(stream, src->bo, 0); /* srcba */
+    tegra_stream_push(stream, src->pitch); /* srcst */
+    tegra_stream_push(stream, height << 16 | width); /* dstsize */
+    tegra_stream_push(stream, sy << 16 | sx); /* srcps */
+    tegra_stream_push(stream, dy << 16 | dx); /* dstps */
+
+    err = tegra_stream_end(stream);
+    if (err < 0)
+        return err;
+
+    err = tegra_stream_flush(stream);
+    if (err < 0)
+        return err;
+
+    host1x_pixelbuffer_check_guard(dst);
+
+    return 0;
+}
+
+static uint32_t sb_offset(struct host1x_pixelbuffer *pixbuf,
+                          uint32_t xpos, uint32_t ypos)
+{
+    uint32_t offset;
+    uint32_t bytes_per_pixel = PIX_BUF_FORMAT_BYTES(pixbuf->format);
+    uint32_t pixels_per_line = pixbuf->pitch / bytes_per_pixel;
+    uint32_t xb;
+
+    if (pixbuf->layout == PIX_BUF_LAYOUT_LINEAR) {
+        offset = ypos * pixbuf->pitch;
+        offset += xpos * bytes_per_pixel;
+    } else {
+        xb = xpos * bytes_per_pixel;
+        offset = 16 * pixels_per_line * (ypos / 16);
+        offset += 256 * (xb / 16);
+        offset += 16 * (ypos % 16);
+        offset += xb % 16;
+    }
+
+    return offset;
+}
+
+int host1x_gr2d_surface_blit(struct tegra_stream *stream,
+                             struct host1x_pixelbuffer *src,
+                             struct host1x_pixelbuffer *dst,
+                             VdpCSCMatrix cscmat,
+                             unsigned int sx, unsigned int sy,
+                             unsigned int src_width, unsigned int src_height,
+                             unsigned int dx, unsigned int dy,
+                             unsigned int dst_width, int dst_height)
+{
+    float inv_scale_x;
+    float inv_scale_y;
+    unsigned src_tiled = 0;
+    unsigned dst_tiled = 0;
+    unsigned yflip = 0;
+    unsigned src_fmt;
+    unsigned dst_fmt;
+    unsigned hftype;
+    unsigned vftype;
+    unsigned vfen;
+    int err;
+
+    switch (src->layout) {
+    case PIX_BUF_LAYOUT_TILED_16x16:
+        src_tiled = 1;
+    case PIX_BUF_LAYOUT_LINEAR:
+        break;
+    default:
+        host1x_error("Invalid src layout %u\n", src->layout);
+        return -EINVAL;
+    }
+
+    switch (dst->layout) {
+    case PIX_BUF_LAYOUT_TILED_16x16:
+        dst_tiled = 1;
+    case PIX_BUF_LAYOUT_LINEAR:
+        break;
+    default:
+        host1x_error("Invalid dst layout %u\n", dst->layout);
+        return -EINVAL;
+    }
+
+    /*
+     * GR2DSB doesn't support this format. Not sure that this is fine
+     * to do, but scaled result looks correct.
+     */
+    if (src->format == dst->format &&
+        src->format == PIX_BUF_FMT_RGBA8888) {
+        src_fmt = 14;
+        dst_fmt = 14;
+        goto coords_check;
+    }
+
+    switch (src->format) {
+    case PIX_BUF_FMT_ABGR8888:
+        src_fmt = 14;
+        break;
+    case PIX_BUF_FMT_ARGB8888:
+        src_fmt = 15;
+        break;
+    case PIX_BUF_FMT_YV12:
+        src_fmt = 0;
+        break;
+    default:
+        host1x_error("Invalid src format %u\n", src->format);
+        return -EINVAL;
+    }
+
+    switch (dst->format) {
+    case PIX_BUF_FMT_ABGR8888:
+        dst_fmt = 14;
+        break;
+    case PIX_BUF_FMT_ARGB8888:
+        dst_fmt = 15;
+        break;
+    default:
+        host1x_error("Invalid dst format %u\n", dst->format);
+        return -EINVAL;
+    }
+
+coords_check:
+    if (dst_height < 0) {
+        yflip = 1;
+        dst_height = -dst_height;
+    }
+
+    if (sx + src_width > src->width ||
+        dx + dst_width > dst->width ||
+        sy + src_height > src->height ||
+        dy + dst_height > dst->height) {
+        host1x_error("Coords out of range\n");
+        return -EINVAL;
+    }
+
+    inv_scale_x = (src_width) / (float)(dst_width);
+    inv_scale_y = (src_height) / (float)(dst_height);
+
+    if (inv_scale_y > 64.0f || inv_scale_y < 1.0f / 4096.0f) {
+        host1x_error("Unsupported Y scale\n");
+        return -EINVAL;
+    }
+
+    if (inv_scale_x > 64.0f || inv_scale_x < 1.0f / 4096.0f) {
+        host1x_error("Unsupported X scale\n");
+        return -EINVAL;
+    }
+
+    if (inv_scale_x == 1.0f)
+        hftype = 7;
+    else if (inv_scale_x < 1.0f)
+        hftype = 0;
+    else if (inv_scale_x < 1.3f)
+        hftype = 1;
+    else if (inv_scale_x < 2.0f)
+        hftype = 3;
+    else
+        hftype = 6;
+
+    if (inv_scale_y == 1.0f) {
+        vftype = 0;
+        vfen = 0;
+    } else {
+        vfen = 1;
+
+        if (inv_scale_y < 1.0f)
+            vftype = 0;
+        else if (inv_scale_y < 1.3f)
+            vftype = 1;
+        else if (inv_scale_y < 2.0f)
+            vftype = 2;
+        else
+            vftype = 3;
+    }
+
+    err = tegra_stream_begin(stream);
+    if (err < 0)
+        return err;
+
+    tegra_stream_push_setclass(stream, HOST1X_CLASS_GR2D_SB);
+
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x009, 0xF09));
+    tegra_stream_push(stream, 0x00000038); /* trigger */
+    tegra_stream_push(stream, 0x00000001); /* cmdsel */
+    tegra_stream_push(stream, FLOAT_TO_FIXED_6_12(inv_scale_y)); /* vdda */
+    tegra_stream_push(stream, FLOAT_TO_FIXED_0_8(sy)); /* vddaini */
+    tegra_stream_push(stream, FLOAT_TO_FIXED_6_12(inv_scale_x)); /* hdda */
+    tegra_stream_push(stream, FLOAT_TO_FIXED_0_8(sx)); /* hddainils */
+
+    /* CSC RGB -> RGB coefficients */
+    if (src->format != PIX_BUF_FMT_YV12) {
+        tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x15, 0x787));
+
+        tegra_stream_push(stream,
+                /* cvr */ FLOAT_TO_FIXED_2_7(1.0f) << 12 |
+                /* cub */ FLOAT_TO_FIXED_2_7(1.0f)); /* cscfirst */
+        tegra_stream_push(stream,
+                /* cyx */ FLOAT_TO_FIXED_1_7(1.0f) << 24 |
+                /* cur */ FLOAT_TO_FIXED_2_7(0.0f) << 12 |
+                /* cug */ FLOAT_TO_FIXED_1_7(0.0f)); /* cscsecond */
+        tegra_stream_push(stream,
+                /* cvb */ FLOAT_TO_FIXED_2_7(0.0f) << 16 |
+                /* cvg */ FLOAT_TO_FIXED_1_7(0.0f)); /* cscthird */
+    } else {
+        tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x15, 0x7E7));
+
+        tegra_stream_push(stream,
+                /* yos */ (-16) << 24 |
+                /* cvr */ FLOAT_TO_FIXED_2_7(cscmat[0][2]) << 12 |
+                /* cub */ FLOAT_TO_FIXED_2_7(cscmat[2][1])); /* cscfirst */
+        tegra_stream_push(stream,
+                /* cyx */ FLOAT_TO_FIXED_1_7(cscmat[0][0]) << 24 |
+                /* cur */ FLOAT_TO_FIXED_2_7(cscmat[0][1]) << 12 |
+                /* cug */ FLOAT_TO_FIXED_1_7(cscmat[1][1])); /* cscsecond */
+        tegra_stream_push(stream,
+                /* cvb */ FLOAT_TO_FIXED_2_7(cscmat[2][2]) << 16 |
+                /* cvg */ FLOAT_TO_FIXED_1_7(cscmat[1][2])); /* cscthird */
+
+        tegra_stream_push_reloc(stream, src->bos[1], 0); /* uba */
+        tegra_stream_push_reloc(stream, src->bos[2], 0); /* vba */
+    }
+
+    tegra_stream_push(stream, dst_fmt << 8 | src_fmt); /* sbformat */
+    tegra_stream_push(stream, /* controlsb */
+                      hftype << 20 | vfen << 18 | vftype << 16 |
+                      (3 << 8) /* uvst */ |
+                      ((src->format == PIX_BUF_FMT_YV12) << 5) /* imode */);
+    tegra_stream_push(stream, 0x00000000); /* controlsecond */
+    /*
+     * [20:20] source color depth (0: mono, 1: same)
+     * [17:16] destination color depth (0: 8 bpp, 1: 16 bpp, 2: 32 bpp)
+     */
+    tegra_stream_push(stream, /* controlmain */
+                      1 << 28 | 1 << 27 |
+                      (PIX_BUF_FORMAT_BYTES(dst->format) >> 1) << 16 |
+                      yflip << 14);
+
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x044, 0x35));
+    tegra_stream_push(stream, src->pitch_uv); /* uvstride */
+    /*
+     * [20:20] destination write tile mode (0: linear, 1: tiled)
+     * [ 0: 0] tile mode Y/RGB (0: linear, 1: tiled)
+     */
+    tegra_stream_push(stream, dst_tiled << 20 | src_tiled); /* tilemode */
+    tegra_stream_push_reloc(stream, src->bo, /* srcba_sb_surfbase */
+                            sb_offset(src, sx, sy));
+    tegra_stream_push_reloc(stream, dst->bo, /* dstba_sb_surfbase */
+                            sb_offset(dst, dx, dy) +
+                                yflip * dst->pitch * (dst_height - 1));
+
+    tegra_stream_push(stream, HOST1X_OPCODE_MASK(0x02b, 0x3149));
+    tegra_stream_push_reloc(stream, dst->bo, /* dstba */
+                            sb_offset(dst, dx, dy) +
+                                yflip * dst->pitch * (dst_height - 1));
+    tegra_stream_push(stream, dst->pitch); /* dstst */
+    tegra_stream_push_reloc(stream, src->bo, /* srcba */
+                            sb_offset(src, sx, sy));
+    tegra_stream_push(stream, src->pitch); /* srcst */
+    tegra_stream_push(stream, (src_height - 1) << 16 | src_width); /* srcsize */
+    tegra_stream_push(stream, (dst_height - 1) << 16 | dst_width); /* dstsize */
+
+    err = tegra_stream_end(stream);
+    if (err < 0)
+        return err;
+
+    err = tegra_stream_flush(stream);
+    if (err < 0)
+        return err;
+
+    host1x_pixelbuffer_check_guard(dst);
+
+    return 0;
+}
diff --git a/src/host1x-pixelbuffer.c b/src/host1x-pixelbuffer.c
new file mode 100644
index 0000000..a1552ed
--- /dev/null
+++ b/src/host1x-pixelbuffer.c
@@ -0,0 +1,291 @@
+/*
+ * Copyright (c) 2012, 2013 Erik Faye-Lund
+ * Copyright (c) 2013 Avionic Design GmbH
+ * Copyright (c) 2013 Thierry Reding
+ * Copyright (c) 2017 Dmitry Osipenko
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#include "vdpau_tegra.h"
+
+#define PIXBUF_GUARD_PATTERN    0xF5132803
+
+static bool pixbuf_guard_disabled = true;
+
+struct host1x_pixelbuffer *host1x_pixelbuffer_create(struct drm_tegra *drm,
+                                                     unsigned width,
+                                                     unsigned height,
+                                                     unsigned pitch,
+                                                     unsigned pitch_uv,
+                                                     enum pixel_format format,
+                                                     enum layout_format layout)
+{
+    struct host1x_pixelbuffer *pixbuf;
+    uint32_t flags = 0;
+    uint32_t bo_size;
+    int ret;
+
+    pixbuf = calloc(1, sizeof(*pixbuf));
+    if (!pixbuf)
+        return NULL;
+
+    if (layout == PIX_BUF_LAYOUT_TILED_16x16) {
+        pitch = ALIGN(pitch, 256);
+        pitch_uv = ALIGN(pitch_uv, 256);
+    }
+
+    if (width * PIX_BUF_FORMAT_BYTES(format) > pitch) {
+        host1x_error("Invalid pitch\n");
+        return NULL;
+    }
+
+    if (format == PIX_BUF_FMT_YV12) {
+        if (width * PIX_BUF_FORMAT_BYTES(format) / 2 > pitch_uv) {
+            host1x_error("Invalid UV pitch\n");
+            return NULL;
+        }
+    }
+
+    pixbuf->pitch = pitch;
+    pixbuf->pitch_uv = pitch_uv;
+    pixbuf->width = width;
+    pixbuf->height = height;
+    pixbuf->format = format;
+    pixbuf->layout = layout;
+
+    if (layout == PIX_BUF_LAYOUT_TILED_16x16)
+        height = ALIGN(height, 16);
+
+    bo_size = pitch * height;
+
+    if (!pixbuf_guard_disabled) {
+        pixbuf->guard_offset[0] = bo_size;
+        bo_size += PIXBUF_GUARD_AREA_SIZE;
+    }
+
+    ret = drm_tegra_bo_new(&pixbuf->bo, drm, flags, bo_size);
+    if (ret < 0) {
+        host1x_error("Failed to allocate BO size %u\n", bo_size);
+        goto error_cleanup;
+    }
+
+    if (format == PIX_BUF_FMT_YV12) {
+        bo_size = pitch_uv * height / 2;
+
+        if (!pixbuf_guard_disabled) {
+            pixbuf->guard_offset[1] = bo_size;
+            pixbuf->guard_offset[2] = bo_size;
+            bo_size += PIXBUF_GUARD_AREA_SIZE;
+        }
+
+        ret = drm_tegra_bo_new(&pixbuf->bos[1], drm, flags, bo_size);
+        if (ret < 0) {
+            host1x_error("Failed to allocate Cb BO size %u\n", bo_size);
+            goto error_cleanup;
+        }
+
+        ret = drm_tegra_bo_new(&pixbuf->bos[2], drm, flags, bo_size);
+        if (ret < 0){
+            host1x_error("Failed to allocate Cr BO size %u\n", bo_size);
+            goto error_cleanup;
+        }
+    }
+
+    host1x_pixelbuffer_setup_guard(pixbuf);
+
+    return pixbuf;
+
+error_cleanup:
+    drm_tegra_bo_unref(pixbuf->bos[0]);
+    drm_tegra_bo_unref(pixbuf->bos[1]);
+    drm_tegra_bo_unref(pixbuf->bos[2]);
+    free(pixbuf);
+
+    return NULL;
+}
+
+void host1x_pixelbuffer_free(struct host1x_pixelbuffer *pixbuf)
+{
+    drm_tegra_bo_unref(pixbuf->bos[0]);
+    drm_tegra_bo_unref(pixbuf->bos[1]);
+    drm_tegra_bo_unref(pixbuf->bos[2]);
+    free(pixbuf);
+}
+
+int host1x_pixelbuffer_load_data(struct drm_tegra *drm,
+                                 struct tegra_stream *stream,
+                                 struct host1x_pixelbuffer *pixbuf,
+                                 void *data,
+                                 unsigned data_pitch,
+                                 unsigned long data_size,
+                                 enum pixel_format data_format,
+                                 enum layout_format data_layout)
+{
+    struct host1x_pixelbuffer *tmp;
+    bool blit = false;
+    void *map;
+    int ret;
+
+    if (pixbuf->format != data_format)
+        return -1;
+
+    if (pixbuf->layout != data_layout)
+        blit = true;
+
+    if (pixbuf->pitch != data_pitch)
+        blit = true;
+
+    if (blit) {
+        tmp = host1x_pixelbuffer_create(drm,
+                                        pixbuf->width, pixbuf->height,
+                                        data_pitch, 0, data_format,
+                                        data_layout);
+        if (!tmp)
+            return -1;
+    } else {
+        tmp = pixbuf;
+    }
+
+    ret = drm_tegra_bo_map(tmp->bo, &map);
+    if (ret < 0)
+        return ret;
+
+    memcpy(map, data, data_size);
+
+    if (blit) {
+        ret = host1x_gr2d_blit(stream, tmp, pixbuf,
+                               0, 0, 0, 0,
+                               pixbuf->width, pixbuf->height);
+        host1x_pixelbuffer_free(tmp);
+    }
+
+    drm_tegra_bo_unmap(pixbuf->bo);
+
+    return ret;
+}
+
+static int host1x_pixelbuffer_setup_bo_guard(struct drm_tegra_bo *bo,
+                                             uint32_t guard_offset)
+{
+    volatile uint32_t *guard;
+    unsigned i;
+    int ret;
+
+    ret = drm_tegra_bo_map(bo, (void**)&guard);
+    if (ret < 0)
+        return ret;
+
+    guard = (void*)guard + guard_offset;
+
+    for (i = 0; i < PIXBUF_GUARD_AREA_SIZE / 4; i++)
+        guard[i] = PIXBUF_GUARD_PATTERN + i;
+
+    ret = drm_tegra_bo_unmap(bo);
+    if (ret < 0)
+        return ret;
+
+    return 0;
+}
+
+int host1x_pixelbuffer_setup_guard(struct host1x_pixelbuffer *pixbuf)
+{
+    unsigned i;
+    int ret;
+
+    if (pixbuf_guard_disabled)
+        return 0;
+
+    for (i = 0; i < PIX_BUF_FORMAT_PLANES_NB(pixbuf->format); i++) {
+        ret = host1x_pixelbuffer_setup_bo_guard(pixbuf->bos[i],
+                                                pixbuf->guard_offset[i]);
+        if (ret < 0) {
+            host1x_error("Pixbuf guard setup failed %d\n", ret);
+            return ret;
+        }
+    }
+
+    return 0;
+}
+
+static int host1x_pixelbuffer_check_bo_guard(struct host1x_pixelbuffer *pixbuf,
+                                             struct drm_tegra_bo *bo,
+                                             uint32_t guard_offset)
+{
+    volatile uint32_t *guard;
+    bool smashed = false;
+    uint32_t value;
+    unsigned i;
+    int ret;
+
+    ret = drm_tegra_bo_map(bo, (void**)&guard);
+    if (ret < 0)
+        return ret;
+
+    guard = (void*)guard + guard_offset;
+
+    for (i = 0; i < PIXBUF_GUARD_AREA_SIZE / 4; i++) {
+        value = guard[i];
+
+        if (value != PIXBUF_GUARD_PATTERN + i) {
+            host1x_error("Guard[%d of %d] smashed, 0x%08X != 0x%08X\n",
+                         i, PIXBUF_GUARD_AREA_SIZE / 4 - 1,
+                         value, PIXBUF_GUARD_PATTERN + i);
+            smashed = true;
+        }
+    }
+
+    if (smashed) {
+        host1x_error("Pixbuf %p: width %u, height %u, pitch %u, format %u\n",
+                     pixbuf, pixbuf->width, pixbuf->height,
+                     pixbuf->pitch, pixbuf->format);
+        abort();
+    }
+
+    ret = drm_tegra_bo_unmap(bo);
+    if (ret < 0)
+        return ret;
+
+    return 0;
+}
+
+int host1x_pixelbuffer_check_guard(struct host1x_pixelbuffer *pixbuf)
+{
+    unsigned i;
+    int ret;
+
+    if (pixbuf_guard_disabled)
+        return 0;
+
+    for (i = 0; i < PIX_BUF_FORMAT_PLANES_NB(pixbuf->format); i++) {
+        ret = host1x_pixelbuffer_check_bo_guard(pixbuf, pixbuf->bos[i],
+                                                pixbuf->guard_offset[i]);
+        if (ret < 0) {
+            host1x_error("Pixbuf guard check failed %d\n", ret);
+            return ret;
+        }
+    }
+
+    return 0;
+}
+
+void host1x_pixelbuffer_disable_bo_guard(void)
+{
+    pixbuf_guard_disabled = true;
+}
diff --git a/src/host1x.h b/src/host1x.h
new file mode 100644
index 0000000..da44b78
--- /dev/null
+++ b/src/host1x.h
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2012, 2013 Erik Faye-Lund
+ * Copyright (c) 2013 Avionic Design GmbH
+ * Copyright (c) 2013 Thierry Reding
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef HOST1X_H
+#define HOST1X_H
+
+#define HOST1X_OPCODE_SETCL(offset, classid, mask) \
+    ((0x0 << 28) | (((offset) & 0xfff) << 16) | (((classid) & 0x3ff) << 6) | ((mask) & 0x3f))
+#define HOST1X_OPCODE_INCR(offset, count) \
+    ((0x1 << 28) | (((offset) & 0xfff) << 16) | ((count) & 0xffff))
+#define HOST1X_OPCODE_NONINCR(offset, count) \
+    ((0x2 << 28) | (((offset) & 0xfff) << 16) | ((count) & 0xffff))
+#define HOST1X_OPCODE_MASK(offset, mask) \
+    ((0x3 << 28) | (((offset) & 0xfff) << 16) | ((mask) & 0xffff))
+#define HOST1X_OPCODE_IMM(offset, data) \
+    ((0x4 << 28) | (((offset) & 0xfff) << 16) | ((data) & 0xffff))
+#define HOST1X_OPCODE_EXTEND(subop, value) \
+    ((0xe << 28) | (((subop) & 0xf) << 24) | ((value) & 0xffffff))
+
+#define HOST1X_CLASS_GR2D       0x51
+#define HOST1X_CLASS_GR2D_SB    0x52
+
+#define host1x_error(fmt, args...) \
+    fprintf(stderr, "ERROR: %s: %d: " fmt, __func__, __LINE__, ##args)
+
+#define PIX_BUF_FMT(id, bpp, planes_nb) \
+    ((planes_nb) << 16 | (id) << 8 | (bpp))
+
+#define PIX_BUF_FORMAT_BITS(f) \
+    ((f) & 0xff)
+
+#define PIX_BUF_FORMAT_BYTES(f) \
+    (PIX_BUF_FORMAT_BITS(f) >> 3)
+
+#define PIX_BUF_FORMAT_PLANES_NB(f) \
+    (((f) >> 16) & 3)
+
+enum pixel_format {
+    PIX_BUF_FMT_A8            = PIX_BUF_FMT(0, 8, 1),
+    PIX_BUF_FMT_L8            = PIX_BUF_FMT(1, 8, 1),
+    PIX_BUF_FMT_S8            = PIX_BUF_FMT(2, 8, 1),
+    PIX_BUF_FMT_LA88          = PIX_BUF_FMT(3, 16, 1),
+    PIX_BUF_FMT_RGB565        = PIX_BUF_FMT(4, 16, 1),
+    PIX_BUF_FMT_RGBA5551      = PIX_BUF_FMT(5, 16, 1),
+    PIX_BUF_FMT_RGBA4444      = PIX_BUF_FMT(6, 16, 1),
+    PIX_BUF_FMT_D16_LINEAR    = PIX_BUF_FMT(7, 16, 1),
+    PIX_BUF_FMT_D16_NONLINEAR = PIX_BUF_FMT(8, 16, 1),
+    PIX_BUF_FMT_RGBA8888      = PIX_BUF_FMT(9, 32, 1),
+    PIX_BUF_FMT_RGBA_FP32     = PIX_BUF_FMT(10, 32, 1),
+    PIX_BUF_FMT_ARGB8888      = PIX_BUF_FMT(11, 32, 1),
+    PIX_BUF_FMT_ABGR8888      = PIX_BUF_FMT(12, 32, 1),
+    PIX_BUF_FMT_YV12          = PIX_BUF_FMT(13, 8, 3),
+};
+
+enum layout_format {
+    PIX_BUF_LAYOUT_LINEAR,
+    PIX_BUF_LAYOUT_TILED_16x16,
+};
+
+struct host1x_pixelbuffer {
+    union {
+        struct drm_tegra_bo *bo;
+        struct drm_tegra_bo *bos[3];
+    };
+    enum pixel_format format;
+    enum layout_format layout;
+    unsigned width;
+    unsigned height;
+    unsigned pitch;
+    unsigned pitch_uv;
+    uint32_t guard_offset[3];
+};
+
+#define PIXBUF_GUARD_AREA_SIZE    0x4000
+
+struct host1x_pixelbuffer *host1x_pixelbuffer_create(struct drm_tegra *drm,
+                                                     unsigned width,
+                                                     unsigned height,
+                                                     unsigned pitch,
+                                                     unsigned pitch_uv,
+                                                     enum pixel_format format,
+                                                     enum layout_format layout);
+
+void host1x_pixelbuffer_free(struct host1x_pixelbuffer *pixbuf);
+
+int host1x_pixelbuffer_load_data(struct drm_tegra *drm,
+                                 struct tegra_stream *stream,
+                                 struct host1x_pixelbuffer *pixbuf,
+                                 void *data,
+                                 unsigned data_pitch,
+                                 unsigned long data_size,
+                                 enum pixel_format data_format,
+                                 enum layout_format data_layout);
+
+int host1x_pixelbuffer_setup_guard(struct host1x_pixelbuffer *pixbuf);
+
+int host1x_pixelbuffer_check_guard(struct host1x_pixelbuffer *pixbuf);
+
+void host1x_pixelbuffer_disable_bo_guard(void);
+
+int host1x_gr2d_clear(struct tegra_stream *stream,
+                      struct host1x_pixelbuffer *pixbuf,
+                      uint32_t color);
+
+int host1x_gr2d_clear_rect(struct tegra_stream *stream,
+                           struct host1x_pixelbuffer *pixbuf,
+                           uint32_t color,
+                           unsigned x, unsigned y,
+                           unsigned width, unsigned height);
+
+int host1x_gr2d_clear_rect_clipped(struct tegra_stream *stream,
+                                   struct host1x_pixelbuffer *pixbuf,
+                                   uint32_t color,
+                                   unsigned x, unsigned y,
+                                   unsigned width, unsigned height,
+                                   unsigned clip_x0, unsigned clip_y0,
+                                   unsigned clip_x1, unsigned clip_y1,
+                                   bool draw_outside);
+
+int host1x_gr2d_blit(struct tegra_stream *stream,
+                     struct host1x_pixelbuffer *src,
+                     struct host1x_pixelbuffer *dst,
+                     unsigned int sx, unsigned int sy,
+                     unsigned int dx, unsigned int dy,
+                     unsigned int width, int height);
+
+int host1x_gr2d_surface_blit(struct tegra_stream *stream,
+                             struct host1x_pixelbuffer *src,
+                             struct host1x_pixelbuffer *dst,
+                             VdpCSCMatrix cscmat,
+                             unsigned int sx, unsigned int sy,
+                             unsigned int src_width, unsigned int src_height,
+                             unsigned int dx, unsigned int dy,
+                             unsigned int dst_width, int dst_height);
+#endif
diff --git a/src/presentation_queue.c b/src/presentation_queue.c
index bd7d126..9f61787 100644
--- a/src/presentation_queue.c
+++ b/src/presentation_queue.c
@@ -19,14 +19,225 @@
 
 #include "vdpau_tegra.h"
 
-static uint64_t get_time(void)
+static VdpTime get_time(void)
 {
     struct timespec tp;
 
     if (clock_gettime(CLOCK_MONOTONIC, &tp) == -1)
-        return 0;
+        abort();
 
-    return (uint64_t)tp.tv_sec * 1000000000ULL + (uint64_t)tp.tv_nsec;
+    return (VdpTime)tp.tv_sec * 1000000000ULL + (VdpTime)tp.tv_nsec;
+}
+
+static void * x11_thr(void *opaque)
+{
+    tegra_pq *pq = opaque;
+    tegra_pqt *pqt = pq->pqt;
+    tegra_device *dev = pqt->dev;
+    XEvent event;
+    int x = 0, y = 0, width = 0, height = 0;
+
+    while (true) {
+        if (!pq->exit &&
+            !XCheckWindowEvent(dev->display, pqt->drawable,
+                               StructureNotifyMask, &event)) {
+            usleep(100000);
+            continue;
+        }
+
+        if (pq->exit)
+            break;
+
+        switch (event.type) {
+        case ConfigureNotify:
+            if (x != event.xconfigure.x)
+                break;
+
+            if (y != event.xconfigure.y)
+                break;
+
+            if (width != event.xconfigure.width)
+                break;
+
+            if (height != event.xconfigure.height)
+                break;
+
+        default:
+            continue;
+        }
+
+        x = event.xconfigure.x;
+        y = event.xconfigure.y;
+        width = event.xconfigure.width;
+        height = event.xconfigure.height;
+
+        pthread_mutex_lock(&pq->lock);
+
+        if (pqt->disp_surf) {
+                XvPutImage(dev->display, dev->xv_port,
+                           pqt->drawable, pqt->gc,
+                           pqt->disp_surf->xv_img,
+                           0, 0,
+                           pqt->disp_surf->disp_width,
+                           pqt->disp_surf->disp_height,
+                           0, 0,
+                           pqt->disp_surf->disp_width,
+                           pqt->disp_surf->disp_height);
+
+                XSync(dev->display, 0);
+        }
+
+        pthread_mutex_unlock(&pq->lock);
+    }
+
+    return NULL;
+}
+
+static void pqt_display_surface_to_idle_state(tegra_pqt *pqt)
+{
+    if (!pqt->disp_surf) {
+        return;
+    }
+
+    pthread_mutex_lock(&pqt->disp_surf->lock);
+
+    pqt->disp_surf->status = VDP_PRESENTATION_QUEUE_STATUS_IDLE;
+    pthread_cond_signal(&pqt->disp_surf->idle_cond);
+
+    pthread_mutex_unlock(&pqt->disp_surf->lock);
+
+    unref_surface(pqt->disp_surf);
+
+    pqt->disp_surf = NULL;
+}
+
+static void * presentation_queue_thr(void *opaque)
+{
+    tegra_pq *pq = opaque;
+    tegra_pqt *pqt = pq->pqt;
+    tegra_device *dev = pqt->dev;
+    tegra_surface *surf, *tmp;
+    struct timespec tp;
+    VdpTime time = UINT64_MAX;
+    int ret;
+
+    while (true) {
+        pthread_mutex_lock(&pq->lock);
+
+        if (time == UINT64_MAX) {
+            clock_gettime(CLOCK_MONOTONIC, &tp);
+            tp.tv_sec += 9999999;
+        } else {
+            memset(&tp, 0, sizeof(tp));
+            tp.tv_sec = time / 1000000000ULL;
+            tp.tv_nsec = time - tp.tv_sec * 1000000000ULL;
+        }
+
+        ret = pthread_cond_timedwait(&pq->cond, &pq->lock, &tp);
+
+        if (pq->exit) {
+            LIST_FOR_EACH_ENTRY_SAFE(surf, tmp, &pq->surf_list, list_item) {
+                pthread_mutex_lock(&surf->lock);
+
+                surf->status = VDP_PRESENTATION_QUEUE_STATUS_IDLE;
+                surf->first_presentation_time = 0;
+                pthread_cond_signal(&surf->idle_cond);
+
+                LIST_DEL(&surf->list_item);
+                pthread_mutex_unlock(&surf->lock);
+
+                unref_surface(surf);
+            }
+
+            pqt_display_surface_to_idle_state(pqt);
+
+            pthread_mutex_unlock(&pq->lock);
+
+            return NULL;
+        }
+
+        if (ret == ETIMEDOUT) {
+            time = (VdpTime)tp.tv_sec * 1000000000ULL + (VdpTime)tp.tv_nsec;
+
+            LIST_FOR_EACH_ENTRY_SAFE(surf, tmp, &pq->surf_list, list_item) {
+                pthread_mutex_lock(&surf->lock);
+
+                if (surf->earliest_presentation_time > time) {
+                    pthread_mutex_unlock(&surf->lock);
+                    continue;
+                }
+
+                if (surf->earliest_presentation_time < time) {
+                    surf->status = VDP_PRESENTATION_QUEUE_STATUS_IDLE;
+                    surf->first_presentation_time = 0;
+                    pthread_cond_signal(&surf->idle_cond);
+
+                    goto del_surface;
+                }
+
+                XvPutImage(dev->display, dev->xv_port,
+                           pqt->drawable, pqt->gc,
+                           surf->xv_img,
+                           0, 0,
+                           surf->disp_width,
+                           surf->disp_height,
+                           0, 0,
+                           surf->disp_width,
+                           surf->disp_height);
+
+                XSync(dev->display, 0);
+
+                surf->first_presentation_time = get_time();
+                surf->status = VDP_PRESENTATION_QUEUE_STATUS_VISIBLE;
+
+                if (pqt->disp_surf != surf) {
+                    pqt_display_surface_to_idle_state(pqt);
+                    pqt->disp_surf = surf;
+                }
+
+                ref_surface(surf);
+del_surface:
+                LIST_DEL(&surf->list_item);
+                pthread_mutex_unlock(&surf->lock);
+                unref_surface(surf);
+            }
+        }
+
+        time = UINT64_MAX;
+
+        if (LIST_IS_EMPTY(&pq->surf_list)) {
+            pthread_mutex_unlock(&pq->lock);
+            continue;
+        }
+
+        LIST_FOR_EACH_ENTRY(surf, &pq->surf_list, list_item) {
+            pthread_mutex_lock(&surf->lock);
+
+            if (surf->earliest_presentation_time < time)
+                time = surf->earliest_presentation_time;
+
+            pthread_mutex_unlock(&surf->lock);
+        }
+
+        pthread_mutex_unlock(&pq->lock);
+    }
+
+    return NULL;
+}
+
+VdpStatus unref_queue_target(tegra_pqt *pqt)
+{
+    tegra_device *dev = pqt->dev;
+
+    if (!atomic_dec_and_test(&pqt->refcnt))
+        return VDP_STATUS_OK;
+
+    if (pqt->gc != None)
+        XFreeGC(dev->display, pqt->gc);
+
+    free(pqt);
+
+    return VDP_STATUS_OK;
 }
 
 VdpStatus vdp_presentation_queue_target_destroy(
@@ -40,9 +251,7 @@ VdpStatus vdp_presentation_queue_target_destroy(
 
     set_presentation_queue_target(presentation_queue_target, NULL);
 
-    free(pqt);
-
-    return VDP_STATUS_OK;
+    return unref_queue_target(pqt);
 }
 
 VdpStatus vdp_presentation_queue_create(
@@ -54,6 +263,9 @@ VdpStatus vdp_presentation_queue_create(
     tegra_pqt *pqt = get_presentation_queue_target(presentation_queue_target);
     tegra_pq *pq;
     VdpPresentationQueue i;
+    pthread_condattr_t cond_attrs;
+    pthread_attr_t thread_attrs;
+    int ret;
 
     if (dev == NULL || pqt == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
@@ -77,7 +289,57 @@ VdpStatus vdp_presentation_queue_create(
         return VDP_STATUS_RESOURCES;
     }
 
-    pq->presentation_queue_target = presentation_queue_target;
+    ret = pthread_mutex_init(&pq->lock, NULL);
+    if (ret != 0) {
+        ErrorMsg("pthread_mutex_init failed\n");
+        return VDP_STATUS_RESOURCES;
+    }
+
+    pthread_condattr_init(&cond_attrs);
+    pthread_condattr_setclock(&cond_attrs, CLOCK_MONOTONIC);
+
+    ret = pthread_cond_init(&pq->cond, &cond_attrs);
+    if (ret != 0) {
+        ErrorMsg("pthread_cond_init failed\n");
+        return VDP_STATUS_RESOURCES;
+    }
+
+    pthread_condattr_destroy(&cond_attrs);
+
+    LIST_INITHEAD(&pq->surf_list);
+    pq->pqt = pqt;
+
+    pthread_attr_init(&thread_attrs);
+    pthread_attr_setdetachstate(&thread_attrs, PTHREAD_CREATE_JOINABLE);
+
+    ret = pthread_create(&pq->disp_thread, &thread_attrs,
+                         presentation_queue_thr, pq);
+    if (ret != 0) {
+        ErrorMsg("pthread_create failed\n");
+        return VDP_STATUS_RESOURCES;
+    }
+
+    pthread_attr_destroy(&thread_attrs);
+
+    /*
+     * XXX: Unfortunately this doesn't work with MPlayer. Any player that
+     * doesn't invoke XInitThreads() may crash.
+     */
+    if (0) {
+        pthread_attr_init(&thread_attrs);
+        pthread_attr_setdetachstate(&thread_attrs, PTHREAD_CREATE_JOINABLE);
+
+        ret = pthread_create(&pq->x11_thread, &thread_attrs, x11_thr, pq);
+        if (ret != 0) {
+            ErrorMsg("pthread_create failed\n");
+            return VDP_STATUS_RESOURCES;
+        }
+
+        pthread_attr_destroy(&thread_attrs);
+    }
+
+    atomic_inc(&pqt->refcnt);
+    ref_device(dev);
 
     *presentation_queue = i;
 
@@ -88,13 +350,31 @@ VdpStatus vdp_presentation_queue_destroy(
                                         VdpPresentationQueue presentation_queue)
 {
     tegra_pq *pq = get_presentation_queue(presentation_queue);
+    tegra_pqt *pqt;
+    tegra_device *dev;
 
     if (pq == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
 
+    pqt = pq->pqt;
+    dev = pqt->dev;
+
     set_presentation_queue(presentation_queue, NULL);
 
+    pthread_mutex_lock(&pq->lock);
+
+    pq->exit = true;
+    pthread_cond_signal(&pq->cond);
+
+    pthread_mutex_unlock(&pq->lock);
+
+    if (0) {
+        pthread_join(pq->x11_thread, NULL);
+    }
+    pthread_join(pq->disp_thread, NULL);
+    unref_queue_target(pqt);
+    unref_device(dev);
     free(pq);
 
     return VDP_STATUS_OK;
@@ -152,44 +432,77 @@ VdpStatus vdp_presentation_queue_display(
     tegra_pq *pq = get_presentation_queue(presentation_queue);
     tegra_pqt *pqt;
     tegra_device *dev;
+    VdpTime time;
 
-    if (surf == NULL || pq == NULL) {
+    if (pq == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    pqt = get_presentation_queue_target(pq->presentation_queue_target);
+    pqt = pq->pqt;
+    dev = pqt->dev;
 
-    if (pqt == NULL) {
-        return VDP_STATUS_INVALID_HANDLE;
-    }
+    /* This will happen on surface allocation failure. */
+    if (surf == NULL) {
+        time = get_time();
+
+        if (earliest_presentation_time > time) {
+            usleep((earliest_presentation_time - time) / 1000);
+        }
 
-    dev = get_device(pqt->device);
+        pthread_mutex_lock(&pq->lock);
+
+        pqt_display_surface_to_idle_state(pqt);
+
+        pthread_cond_signal(&pq->cond);
+        pthread_mutex_unlock(&pq->lock);
 
-    if (dev == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    assert(surf->img != NULL);
-
-    if (surf->pix_disp != NULL) {
-        /* Display uses other pixel format, conversion is required.  */
-        pixman_image_composite(PIXMAN_OP_SRC,
-                               surf->pix,
-                               NULL,
-                               surf->pix_disp,
-                               0, 0,
-                               0, 0,
-                               0, 0,
-                               clip_width, clip_height);
+    pthread_mutex_lock(&pq->lock);
+    pthread_mutex_lock(&surf->lock);
+
+    assert(surf->idle_hack ||
+           surf->status == VDP_PRESENTATION_QUEUE_STATUS_IDLE);
+
+    surf->disp_width  = clip_width  ?: surf->xv_img->width;
+    surf->disp_height = clip_height ?: surf->xv_img->height;
+    surf->idle_hack = false;
+
+    /* XXX: X11 app won't survive threading without XInitThreads() */
+    if (earliest_presentation_time == 0) {
+        XvPutImage(dev->display, dev->xv_port,
+                   pqt->drawable, pqt->gc,
+                   surf->xv_img,
+                   0, 0,
+                   surf->disp_width,
+                   surf->disp_height,
+                   0, 0,
+                   surf->disp_width,
+                   surf->disp_height);
+
+        XSync(dev->display, 0);
+
+        surf->status = VDP_PRESENTATION_QUEUE_STATUS_VISIBLE;
+        surf->first_presentation_time = get_time();
+        surf->idle_hack = true;
+
+        pthread_mutex_unlock(&surf->lock);
+        pthread_mutex_unlock(&pq->lock);
+
+        return VDP_STATUS_OK;
     }
 
-    XPutImage(dev->display, pqt->drawable, pqt->gc,
-              surf->img,
-              0, 0,
-              0, 0,
-              clip_width, clip_height);
+    ref_surface(surf);
+    LIST_ADDTAIL(&surf->list_item, &pq->surf_list);
+
+    surf->status = VDP_PRESENTATION_QUEUE_STATUS_QUEUED;
+    surf->earliest_presentation_time = earliest_presentation_time;
 
-    XSync(dev->display, 0);
+    pthread_mutex_unlock(&surf->lock);
+
+    pthread_cond_signal(&pq->cond);
+    pthread_mutex_unlock(&pq->lock);
 
     return VDP_STATUS_OK;
 }
@@ -203,10 +516,22 @@ VdpStatus vdp_presentation_queue_block_until_surface_idle(
     tegra_pq *pq = get_presentation_queue(presentation_queue);
 
     if (surf == NULL || pq == NULL) {
+        *first_presentation_time = get_time();
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    *first_presentation_time = get_time();
+    ref_surface(surf);
+
+    pthread_mutex_lock(&surf->lock);
+
+    if (!surf->idle_hack && surf->status != VDP_PRESENTATION_QUEUE_STATUS_IDLE)
+        pthread_cond_wait(&surf->idle_cond, &surf->lock);
+
+    *first_presentation_time = surf->first_presentation_time;
+
+    pthread_mutex_unlock(&surf->lock);
+
+    unref_surface(surf);
 
     return VDP_STATUS_OK;
 }
@@ -221,11 +546,12 @@ VdpStatus vdp_presentation_queue_query_surface_status(
     tegra_pq *pq = get_presentation_queue(presentation_queue);
 
     if (surf == NULL || pq == NULL) {
+        *first_presentation_time = get_time();
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    *status = VDP_PRESENTATION_QUEUE_STATUS_VISIBLE;
-    *first_presentation_time = get_time();
+    *status = surf->status;
+    *first_presentation_time = surf->first_presentation_time;
 
     return VDP_STATUS_OK;
 }
@@ -244,11 +570,6 @@ VdpStatus vdp_presentation_queue_target_create_x11(
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    XSetWindowBackground(dev->display, drawable,
-                         BlackPixel(dev->display, dev->screen));
-    XClearWindow(dev->display, drawable);
-    XSync(dev->display, 0);
-
     pthread_mutex_lock(&global_lock);
 
     for (i = 0; i < MAX_PRESENTATION_QUEUE_TARGETS_NB; i++) {
@@ -267,7 +588,8 @@ VdpStatus vdp_presentation_queue_target_create_x11(
         return VDP_STATUS_RESOURCES;
     }
 
-    pqt->device = device;
+    atomic_set(&pqt->refcnt, 1);
+    pqt->dev = dev;
     pqt->drawable = drawable;
     pqt->gc = XCreateGC(dev->display, drawable, 0, &values);
 
diff --git a/src/surface.c b/src/surface.c
index 2a86955..8b3cba4 100644
--- a/src/surface.c
+++ b/src/surface.c
@@ -36,56 +36,23 @@ static uint32_t get_unused_surface_id(void)
     return id;
 }
 
-static struct drm_tegra_bo *alloc_plane(struct drm_tegra *drm, void **map,
-                                        int *dmabuf_fd, int size)
-{
-    struct drm_tegra_bo *bo;
-    uint32_t fd;
-    int ret;
-
-    ret = drm_tegra_bo_new(&bo, drm, 0, size);
-
-    if (ret < 0) {
-        return NULL;
-    }
-
-    if (map) {
-        ret = drm_tegra_bo_map(bo, map);
-
-        if (ret < 0) {
-            drm_tegra_bo_unref(bo);
-            return NULL;
-        }
-    }
-
-    ret = drm_tegra_bo_to_dmabuf(bo, &fd);
-
-    if (ret < 0) {
-        drm_tegra_bo_unref(bo);
-        return NULL;
-    }
-
-    *dmabuf_fd = fd;
-
-    return bo;
-}
-
 uint32_t create_surface(tegra_device *dev,
                         uint32_t width,
                         uint32_t height,
-                        pixman_format_code_t pfmt,
+                        VdpRGBAFormat rgba_format,
                         int output,
                         int video)
 {
     tegra_surface *surf                 = calloc(1, sizeof(tegra_surface));
     pixman_image_t *pix                 = NULL;
-    pixman_image_t *pix_disp            = NULL;
-    XImage *img                         = NULL;
+    XvImage *xv_img                     = NULL;
     struct tegra_vde_h264_frame *frame  = NULL;
-    void *data                          = NULL;
-    void *xrgb_data                     = NULL;
+    struct host1x_pixelbuffer *pixbuf   = NULL;
     uint32_t surface_id                 = VDP_INVALID_HANDLE;
     uint32_t stride                     = width * 4;
+    uint32_t *bo_flinks                 = NULL;
+    uint32_t *pitches                   = NULL;
+    int ret;
 
     if (surf == NULL) {
         return VDP_INVALID_HANDLE;
@@ -105,16 +72,52 @@ uint32_t create_surface(tegra_device *dev,
         goto err_cleanup;
     }
 
-    xrgb_data = data = malloc(stride * height);
+    if (!video){
+        pixman_format_code_t pfmt;
+        enum pixel_format pixbuf_fmt;
+        void *data;
 
-    if (data == NULL) {
-        goto err_cleanup;
-    }
+        switch (rgba_format) {
+        case VDP_RGBA_FORMAT_R8G8B8A8:
+            pixbuf_fmt = PIX_BUF_FMT_ABGR8888;
+            pfmt = PIXMAN_a8b8g8r8;
+            break;
 
-    pix = pixman_image_create_bits_no_clear(pfmt, width, height, data, stride);
+        case VDP_RGBA_FORMAT_B8G8R8A8:
+            pixbuf_fmt = PIX_BUF_FMT_ARGB8888;
+            pfmt = PIXMAN_a8r8g8b8;
+            break;
 
-    if (pix == NULL) {
-        goto err_cleanup;
+        default:
+            goto err_cleanup;
+        }
+
+        pixbuf = host1x_pixelbuffer_create(dev->drm, width, height,
+                                           stride, 0,
+                                           pixbuf_fmt,
+                                           PIX_BUF_LAYOUT_LINEAR);
+        if (pixbuf == NULL) {
+            goto err_cleanup;
+        }
+
+        ret = drm_tegra_bo_map(pixbuf->bo, &data);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        ret = drm_tegra_bo_forbid_caching(pixbuf->bo);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        pix = pixman_image_create_bits_no_clear(pfmt, width, height,
+                                                data, stride);
+
+        if (pix == NULL) {
+            goto err_cleanup;
+        }
     }
 
     if (video) {
@@ -124,131 +127,226 @@ uint32_t create_surface(tegra_device *dev,
             goto err_cleanup;
         }
 
-        assert(dev != NULL);
+        frame->y_fd = -1;
+        frame->cb_fd = -1;
+        frame->cr_fd = -1;
+        frame->aux_fd = -1;
+
+        pixbuf = host1x_pixelbuffer_create(dev->drm,
+                                           width, ALIGN(height, 16),
+                                           ALIGN(width, 16),
+                                           ALIGN(width / 2, 8),
+                                           PIX_BUF_FMT_YV12,
+                                           PIX_BUF_LAYOUT_LINEAR);
+        if (pixbuf == NULL) {
+            goto err_cleanup;
+        }
+
+        surf->y_bo  = pixbuf->bos[0];
+        surf->cb_bo = pixbuf->bos[1];
+        surf->cr_bo = pixbuf->bos[2];
+
+        /* luma plane */
+
+        ret = drm_tegra_bo_to_dmabuf(surf->y_bo, (uint32_t *) &frame->y_fd);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        ret = drm_tegra_bo_map(surf->y_bo, &surf->y_data);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        ret = drm_tegra_bo_forbid_caching(surf->y_bo);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        /* blue plane */
+
+        ret = drm_tegra_bo_to_dmabuf(surf->cb_bo, (uint32_t *) &frame->cb_fd);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        ret = drm_tegra_bo_map(surf->cb_bo, &surf->cb_data);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        ret = drm_tegra_bo_forbid_caching(surf->cb_bo);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
+
+        /* red plane */
+
+        ret = drm_tegra_bo_to_dmabuf(surf->cr_bo, (uint32_t *) &frame->cr_fd);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
 
-        surf->y_bo = alloc_plane(dev->drm, &surf->y_data, &frame->y_fd,
-                                 width * height);
+        ret = drm_tegra_bo_map(surf->cr_bo, &surf->cr_data);
 
-        if (surf->y_bo == NULL) {
+        if (ret < 0) {
             goto err_cleanup;
         }
 
-        surf->cb_bo = alloc_plane(dev->drm, &surf->cb_data, &frame->cb_fd,
-                                  width * height / 4);
+        ret = drm_tegra_bo_forbid_caching(surf->cr_bo);
 
-        if (surf->cb_bo == NULL) {
+        if (ret < 0) {
             goto err_cleanup;
         }
 
-        surf->cr_bo = alloc_plane(dev->drm, &surf->cr_data, &frame->cr_fd,
-                                  width * height / 4);
+        /* aux stuff */
 
-        if (surf->cr_bo == NULL) {
+        ret = drm_tegra_bo_new(&surf->aux_bo, dev->drm, 0,
+                               ALIGN(width, 16) * ALIGN(height, 16) / 4);
+        if (ret < 0) {
             goto err_cleanup;
         }
 
-        surf->aux_bo = alloc_plane(dev->drm, NULL, &frame->aux_fd,
-                                   width * height / 4);
+        ret = drm_tegra_bo_to_dmabuf(surf->aux_bo, (uint32_t *) &frame->aux_fd);
 
-        if (surf->aux_bo == NULL) {
+        if (ret < 0) {
             goto err_cleanup;
         }
 
-        frame->flags = FLAG_IS_VALID;
+        ret = drm_tegra_bo_forbid_caching(surf->aux_bo);
+
+        if (ret < 0) {
+            goto err_cleanup;
+        }
     }
 
     if (output) {
-        if (pfmt != PIXMAN_x8r8g8b8 && pfmt != PIXMAN_a8r8g8b8) {
-            xrgb_data = malloc(stride * height);
+        int format_id = -1;
 
-            pix_disp = pixman_image_create_bits_no_clear(
-                            PIXMAN_x8r8g8b8, width, height, xrgb_data, stride);
+        switch (rgba_format) {
+        case VDP_RGBA_FORMAT_R8G8B8A8:
+            format_id = FOURCC_PASSTHROUGH_XBGR8888;
+            break;
 
-            assert(pix_disp != NULL);
+        case VDP_RGBA_FORMAT_B8G8R8A8:
+            format_id = FOURCC_PASSTHROUGH_XRGB8888;
+            break;
+        }
 
-            if (pix_disp == NULL) {
-                goto err_cleanup;
-            }
+        xv_img = XvCreateImage(dev->display, dev->xv_port,
+                               format_id, NULL, width, height);
+        if (xv_img == NULL) {
+            ErrorMsg("XvCreateImage failed\n");
+            goto err_cleanup;
         }
 
-        img = XCreateImage(dev->display, NULL, 24, ZPixmap, 0,
-                           xrgb_data,
-                           width, height,
-                           8,
-                           stride);
+        assert(xv_img->data_size == PASSTHROUGH_DATA_SIZE);
 
-        assert(img != NULL);
+        xv_img->data = calloc(1, xv_img->data_size);
 
-        if (img == NULL) {
+        if (xv_img->data == NULL) {
             goto err_cleanup;
         }
+
+        bo_flinks = (uint32_t *) xv_img->data;
+
+        if (drm_tegra_bo_get_name(pixbuf->bo, &bo_flinks[0]) != 0) {
+            ErrorMsg("drm_tegra_bo_get_name failed\n");
+            goto err_cleanup;
+        }
+
+        pitches = (uint32_t *) (xv_img->data + 12);
+
+        pitches[0] = pixbuf->pitch;
+    }
+
+    ret = pthread_mutex_init(&surf->lock, NULL);
+    if (ret != 0) {
+        ErrorMsg("pthread_mutex_init failed\n");
+        goto err_cleanup;
+    }
+
+    ret = pthread_cond_init(&surf->idle_cond, NULL);
+    if (ret != 0) {
+        ErrorMsg("pthread_cond_init failed\n");
+        goto err_cleanup;
     }
 
-    surf->rawdata = data;
+    atomic_set(&surf->refcnt, 1);
+    surf->status = VDP_PRESENTATION_QUEUE_STATUS_IDLE;
     surf->pix = pix;
-    surf->pix_disp = pix_disp;
-    surf->img = img;
+    surf->xv_img = xv_img;
     surf->frame = frame;
     surf->flags = video ? SURFACE_VIDEO : 0;
+    surf->pixbuf = pixbuf;
+    surf->dev = dev;
+
+    ref_device(dev);
 
     return surface_id;
 
 err_cleanup:
     set_surface(surface_id, NULL);
 
-    if (pix_disp != NULL && pix_disp != pix) {
-        pixman_image_unref(pix_disp);
+    if (xv_img != NULL) {
+        free(xv_img->data);
+        XFree(xv_img);
     }
 
     if (pix != NULL) {
         pixman_image_unref(pix);
     }
 
-    if (frame != NULL) {
-        drm_tegra_bo_unref(surf->y_bo);
-        drm_tegra_bo_unref(surf->cb_bo);
-        drm_tegra_bo_unref(surf->cr_bo);
-        drm_tegra_bo_unref(surf->aux_bo);
+    if (pixbuf!= NULL) {
+        host1x_pixelbuffer_free(pixbuf);
     }
 
-    if (xrgb_data != data) {
-        free(xrgb_data);
+    if (frame != NULL) {
+        drm_tegra_bo_unref(surf->aux_bo);
+        close(frame->y_fd);
+        close(frame->cb_fd);
+        close(frame->cr_fd);
+        close(frame->aux_fd);
     }
 
     free(frame);
-    free(data);
     free(surf);
 
     return VDP_INVALID_HANDLE;
 }
 
-VdpStatus destroy_surface(tegra_surface *surf)
+void ref_surface(tegra_surface *surf)
 {
-    pixman_bool_t ret;
+    atomic_inc(&surf->refcnt);
+}
 
-    if (surf->frame != NULL) {
-        drm_tegra_bo_unref(surf->y_bo);
-        drm_tegra_bo_unref(surf->cb_bo);
-        drm_tegra_bo_unref(surf->cr_bo);
-        drm_tegra_bo_unref(surf->aux_bo);
+VdpStatus unref_surface(tegra_surface *surf)
+{
+    if (!atomic_dec_and_test(&surf->refcnt)) {
+        return VDP_STATUS_OK;
     }
 
-    ret = pixman_image_unref(surf->pix);
-
-    assert(ret != 0);
-
-    if (surf->img != NULL) {
-        XDestroyImage(surf->img);
+    if (surf->frame != NULL) {
+        drm_tegra_bo_unref(surf->aux_bo);
+        close(surf->frame->y_fd);
+        close(surf->frame->cb_fd);
+        close(surf->frame->cr_fd);
+        close(surf->frame->aux_fd);
     }
 
-    if (surf->pix_disp != NULL) {
-        ret = pixman_image_unref(surf->pix_disp);
-        assert(ret != 0);
+    if (surf->xv_img != NULL) {
+        free(surf->xv_img->data);
+        XFree(surf->xv_img);
     }
 
-    if (surf->pix_disp != NULL) {
-        free(surf->rawdata);
-    }
+    unref_device(surf->dev);
 
     free(surf->frame);
     free(surf);
@@ -256,53 +354,25 @@ VdpStatus destroy_surface(tegra_surface *surf)
     return VDP_STATUS_OK;
 }
 
-static void convert_yv12_to_xrgb(uint8_t *restrict src_y_,
-                                 uint8_t *restrict src_cb,
-                                 uint8_t *restrict src_cr,
-                                 uint8_t *restrict dest_xrgb,
-                                 int width, int height,
-                                 VdpCSCMatrix cscmat)
+VdpStatus destroy_surface(tegra_surface *surf)
 {
-    float red, green, blue;
-    int y_, cb, cr;
-    int p_y, p_c;
-    int cx, cy;
-    int x, y;
-
-#define _X  3
-#define _R  2
-#define _G  1
-#define _B  0
-
-#define _MAX(x, y) (((x) > (y)) ? (x) : (y))
-#define _MIN(x, y) (((x) < (y)) ? (x) : (y))
-#define _CLAMP(c) (_MIN(255, _MAX(c, 0)))
-
-    for (cy = 0, y = 0; y < height; y++) {
-        cy = y / 2;
+    pthread_mutex_lock(&surf->lock);
 
-        for (x = 0; x < width; x++) {
-            cx = x / 2;
+    surf->earliest_presentation_time = 0;
 
-            p_y = x + width * y;
-            p_c = cx + width / 2 * cy;
-
-            y_ = src_y_[p_y];
-            cb = src_cb[p_c] - 128;
-            cr = src_cr[p_c] - 128;
+    if (surf->pixbuf != NULL) {
+        host1x_pixelbuffer_free(surf->pixbuf);
+        surf->pixbuf = NULL;
+    }
 
-            red   = y_ + cscmat[0][1] * cb + cscmat[0][2] * cr;
-            green = y_ + cscmat[1][1] * cb + cscmat[1][2] * cr;
-            blue  = y_ + cscmat[2][1] * cb + cscmat[2][2] * cr;
+    if (surf->pix != NULL) {
+        pixman_image_unref(surf->pix);
+        surf->pix = NULL;
+    }
 
-            dest_xrgb[_R] = _CLAMP(red);
-            dest_xrgb[_G] = _CLAMP(green);
-            dest_xrgb[_B] = _CLAMP(blue);
-            dest_xrgb[_X] = 255;
+    pthread_mutex_unlock(&surf->lock);
 
-            dest_xrgb += 4;
-        }
-    }
+    return unref_surface(surf);
 }
 
 int sync_video_frame_dmabufs(tegra_surface *surf, enum frame_sync type)
@@ -388,41 +458,3 @@ int sync_video_frame_dmabufs(tegra_surface *surf, enum frame_sync type)
 
     return 0;
 }
-
-int convert_video_surf(tegra_surface *surf, VdpCSCMatrix cscmat)
-{
-    pixman_image_t *pix = surf->pix;
-    int ret;
-
-    if (!(surf->flags & SURFACE_VIDEO)) {
-        return 0;
-    }
-
-    if (!(surf->flags & SURFACE_YUV_UNCONVERTED)) {
-        return 0;
-    }
-
-    ret = sync_video_frame_dmabufs(surf, READ_START);
-
-    if (ret) {
-        return ret;
-    }
-
-    convert_yv12_to_xrgb(surf->y_data, surf->cb_data, surf->cr_data,
-                         (void *) pixman_image_get_data(pix),
-                         pixman_image_get_width(pix),
-                         pixman_image_get_height(pix),
-                         cscmat);
-
-    surf->flags &= ~SURFACE_YUV_UNCONVERTED;
-
-    ret = sync_video_frame_dmabufs(surf, READ_END);
-
-    if (ret) {
-        return ret;
-    }
-
-    surf->flags &= ~SURFACE_DATA_NEEDS_SYNC;
-
-    return 0;
-}
diff --git a/src/surface_bitmap.c b/src/surface_bitmap.c
index f5800b8..e1cd208 100644
--- a/src/surface_bitmap.c
+++ b/src/surface_bitmap.c
@@ -55,7 +55,6 @@ VdpStatus vdp_bitmap_surface_create(VdpDevice device,
                                     VdpBitmapSurface *surface)
 {
     tegra_device *dev = get_device(device);
-    pixman_format_code_t pfmt;
 
     if (dev == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
@@ -63,16 +62,13 @@ VdpStatus vdp_bitmap_surface_create(VdpDevice device,
 
     switch (rgba_format) {
     case VDP_RGBA_FORMAT_R8G8B8A8:
-        pfmt = PIXMAN_a8r8g8b8;
-        break;
     case VDP_RGBA_FORMAT_B8G8R8A8:
-        pfmt = PIXMAN_a8b8g8r8;
         break;
     default:
         return VDP_STATUS_INVALID_RGBA_FORMAT;
     }
 
-    *surface = create_surface(NULL, width, height, pfmt, 0, 0);
+    *surface = create_surface(dev, width, height, rgba_format, 0, 0);
 
     if (*surface == VDP_INVALID_HANDLE) {
         return VDP_STATUS_RESOURCES;
@@ -84,17 +80,14 @@ VdpStatus vdp_bitmap_surface_create(VdpDevice device,
 VdpStatus vdp_bitmap_surface_destroy(VdpBitmapSurface surface)
 {
     tegra_surface *surf = get_surface(surface);
-    VdpStatus ret;
 
     if (surf == NULL) {
         return VDP_INVALID_HANDLE;
     }
 
-    ret = destroy_surface(surf);
-
     set_surface(surface, NULL);
 
-    return ret;
+    return destroy_surface(surf);
 }
 
 VdpStatus vdp_bitmap_surface_get_parameters(VdpBitmapSurface surface,
@@ -175,5 +168,7 @@ VdpStatus vdp_bitmap_surface_put_bits_native(VdpBitmapSurface surface,
 
     assert(ret != 0);
 
+    host1x_pixelbuffer_check_guard(surf->pixbuf);
+
     return VDP_STATUS_OK;
 }
diff --git a/src/surface_mixer.c b/src/surface_mixer.c
index fe05849..6e96bf0 100644
--- a/src/surface_mixer.c
+++ b/src/surface_mixer.c
@@ -25,12 +25,12 @@ VdpStatus vdp_video_mixer_query_feature_support(VdpDevice device,
 {
     tegra_device *dev = get_device(device);
 
+    *is_supported = VDP_FALSE;
+
     if (dev == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    *is_supported = VDP_FALSE;
-
     return VDP_STATUS_OK;
 }
 
@@ -41,10 +41,6 @@ VdpStatus vdp_video_mixer_query_parameter_support(
 {
     tegra_device *dev = get_device(device);
 
-    if (dev == NULL) {
-        return VDP_STATUS_INVALID_HANDLE;
-    }
-
     switch (parameter)
     {
     case VDP_VIDEO_MIXER_PARAMETER_CHROMA_TYPE:
@@ -58,6 +54,10 @@ VdpStatus vdp_video_mixer_query_parameter_support(
         break;
     }
 
+    if (dev == NULL) {
+        return VDP_STATUS_INVALID_HANDLE;
+    }
+
     return VDP_STATUS_OK;
 }
 
@@ -68,12 +68,21 @@ VdpStatus vdp_video_mixer_query_attribute_support(
 {
     tegra_device *dev = get_device(device);
 
+    switch (attribute) {
+    case VDP_VIDEO_MIXER_ATTRIBUTE_CSC_MATRIX:
+    case VDP_VIDEO_MIXER_ATTRIBUTE_BACKGROUND_COLOR:
+        *is_supported = VDP_TRUE;
+        break;
+
+    default:
+        *is_supported = VDP_FALSE;
+        break;
+    }
+
     if (dev == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    *is_supported = VDP_FALSE;
-
     return VDP_STATUS_OK;
 }
 
@@ -161,6 +170,22 @@ VdpStatus vdp_video_mixer_create(VdpDevice device,
         return VDP_STATUS_INVALID_HANDLE;
     }
 
+    while (parameter_count--) {
+        switch (parameters[parameter_count]) {
+        case VDP_VIDEO_MIXER_PARAMETER_CHROMA_TYPE:
+        {
+            const VdpChromaType *chromatype = parameter_values[parameter_count];
+
+            if (*chromatype != VDP_CHROMA_TYPE_420) {
+                return VDP_STATUS_ERROR;
+            }
+            break;
+        }
+        default:
+            break;
+        }
+    }
+
     pthread_mutex_lock(&global_lock);
 
     for (i = 0; i < MAX_MIXERS_NB; i++) {
@@ -196,7 +221,7 @@ VdpStatus vdp_video_mixer_set_feature_enables(
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    return (feature_count == 0) ? VDP_STATUS_OK : VDP_STATUS_ERROR;
+    return VDP_STATUS_OK;
 }
 
 VdpStatus vdp_video_mixer_set_attribute_values(
@@ -206,6 +231,7 @@ VdpStatus vdp_video_mixer_set_attribute_values(
                                         void const *const *attribute_values)
 {
     tegra_mixer *mix = get_mixer(mixer);
+    const VdpColor *color;
 
     if (mix == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
@@ -216,6 +242,12 @@ VdpStatus vdp_video_mixer_set_attribute_values(
         case VDP_VIDEO_MIXER_ATTRIBUTE_CSC_MATRIX:
             memcpy(&mix->csc_matrix, attribute_values[count],
                    sizeof(VdpCSCMatrix));
+            break;
+
+        case VDP_VIDEO_MIXER_ATTRIBUTE_BACKGROUND_COLOR:
+            color = attribute_values[count];
+            mix->bg_color = *color;
+            break;
         }
     }
 
@@ -230,6 +262,10 @@ VdpStatus vdp_video_mixer_get_feature_support(
 {
     tegra_mixer *mix = get_mixer(mixer);
 
+    while (feature_count--) {
+        feature_supports[feature_count] = VDP_FALSE;
+    }
+
     if (mix == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
@@ -316,37 +352,102 @@ VdpStatus vdp_video_mixer_render(
 {
     tegra_surface *dest_surf = get_surface(destination_surface);
     tegra_mixer *mix = get_mixer(mixer);
-    tegra_surface *surf;
+    tegra_device *dev;
+    uint32_t vid_width, vid_height;
+    uint32_t vid_x0, vid_y0;
+    uint32_t bg_width, bg_height;
+    uint32_t bg_x0, bg_y0;
+    uint32_t bg_color;
+    bool draw_background = false;
 
     if (dest_surf == NULL || mix == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
     }
 
-    vdp_output_surface_render_bitmap_surface(
-                                destination_surface,
-                                destination_rect,
-                                background_surface,
-                                background_source_rect,
-                                NULL,
-                                NULL,
-                                VDP_OUTPUT_SURFACE_RENDER_ROTATE_0);
+    dev = dest_surf->dev;
+
+    assert(dest_surf->idle_hack ||
+           dest_surf->status == VDP_PRESENTATION_QUEUE_STATUS_IDLE);
+
+    if (destination_video_rect != NULL) {
+        vid_width = destination_video_rect->x1 - destination_video_rect->x0;
+        vid_height = destination_video_rect->y1 - destination_video_rect->y0;
+        vid_x0 = destination_video_rect->x0;
+        vid_y0 = destination_video_rect->y0;
+    } else {
+        vid_width = dest_surf->pixbuf->width;
+        vid_height = dest_surf->pixbuf->height;
+        vid_x0 = 0;
+        vid_y0 = 0;
+    }
 
-    if (video_surface_current != VDP_INVALID_HANDLE) {
-        surf = get_surface(video_surface_current);
+    if (background_source_rect != NULL) {
+        bg_width = background_source_rect->x1 - background_source_rect->x0;
+        bg_height = background_source_rect->y1 - background_source_rect->y0;
+        bg_x0 = background_source_rect->x0;
+        bg_y0 = background_source_rect->y0;
+    } else {
+        bg_width = dest_surf->pixbuf->width;
+        bg_height = dest_surf->pixbuf->height;
+        bg_x0 = 0;
+        bg_y0 = 0;
+    }
+
+    if (vid_height < bg_height)
+        draw_background = true;
+
+    if (vid_width < bg_width)
+        draw_background = true;
+
+    if (vid_y0 != bg_y0)
+        draw_background = true;
+
+    if (vid_x0 != bg_x0)
+        draw_background = true;
+
+    if (draw_background) {
+        bg_color = (int)(mix->bg_color.alpha * 255) << 24;
 
-        if (convert_video_surf(surf, mix->csc_matrix)) {
-            return VDP_STATUS_ERROR;
+        switch (dest_surf->pixbuf->format) {
+        case PIX_BUF_FMT_ARGB8888:
+            bg_color |= (int)(mix->bg_color.red * 255) << 16;
+            bg_color |= (int)(mix->bg_color.green * 255) << 8;
+            bg_color |= (int)(mix->bg_color.blue * 255) << 0;
+            break;
+
+        case PIX_BUF_FMT_ABGR8888:
+            bg_color |= (int)(mix->bg_color.blue * 255) << 16;
+            bg_color |= (int)(mix->bg_color.green * 255) << 8;
+            bg_color |= (int)(mix->bg_color.red * 255) << 0;
+            break;
+
+        default:
+            abort();
         }
+
+        host1x_gr2d_clear_rect_clipped(dev->stream,
+                                       dest_surf->pixbuf,
+                                       bg_color,
+                                       bg_x0, bg_y0,
+                                       bg_width, bg_height,
+                                       vid_x0, vid_y0,
+                                       vid_x0 + vid_width,
+                                       vid_y0 + vid_height,
+                                       true);
     }
 
-    vdp_output_surface_render_bitmap_surface(
-                                destination_surface,
-                                destination_video_rect,
-                                video_surface_current,
-                                video_source_rect,
-                                NULL,
-                                NULL,
-                                VDP_OUTPUT_SURFACE_RENDER_ROTATE_0);
+    host1x_gr2d_surface_blit(dev->stream,
+                             get_surface(video_surface_current)->pixbuf,
+                             dest_surf->pixbuf,
+                             mix->csc_matrix,
+                             video_source_rect->x0,
+                             video_source_rect->y0,
+                             video_source_rect->x1 - video_source_rect->x0,
+                             video_source_rect->y1 - video_source_rect->y0,
+                             vid_x0,
+                             vid_y0,
+                             vid_width,
+                             vid_height);
 
     while (layer_count--) {
         if (layers[layer_count].struct_version != VDP_LAYER_VERSION) {
diff --git a/src/surface_output.c b/src/surface_output.c
index 0c19d12..8bd8927 100644
--- a/src/surface_output.c
+++ b/src/surface_output.c
@@ -90,7 +90,6 @@ VdpStatus vdp_output_surface_create(VdpDevice device,
                                     VdpOutputSurface *surface)
 {
     tegra_device *dev = get_device(device);
-    pixman_format_code_t pfmt;
 
     if (dev == NULL) {
         return VDP_STATUS_INVALID_HANDLE;
@@ -98,16 +97,13 @@ VdpStatus vdp_output_surface_create(VdpDevice device,
 
     switch (rgba_format) {
     case VDP_RGBA_FORMAT_R8G8B8A8:
-        pfmt = PIXMAN_a8r8g8b8;
-        break;
     case VDP_RGBA_FORMAT_B8G8R8A8:
-        pfmt = PIXMAN_a8b8g8r8;
         break;
     default:
         return VDP_STATUS_INVALID_RGBA_FORMAT;
     }
 
-    *surface = create_surface(dev, width, height, pfmt, 1, 0);
+    *surface = create_surface(dev, width, height, rgba_format, 1, 0);
 
     if (*surface == VDP_INVALID_HANDLE) {
         return VDP_STATUS_RESOURCES;
@@ -211,7 +207,7 @@ VdpStatus vdp_output_surface_render_bitmap_surface(
     pixman_transform_t transform;
     void *dst_data;
     uint32_t src_width, src_height;
-    uint32_t src_x0, src_y0 ;
+    uint32_t src_x0, src_y0;
     uint32_t dst_width, dst_height;
     uint32_t dst_x0, dst_y0;
     int need_scale = 0;
@@ -222,6 +218,9 @@ VdpStatus vdp_output_surface_render_bitmap_surface(
         return VDP_STATUS_INVALID_HANDLE;
     }
 
+    assert(dst_surf->idle_hack ||
+           dst_surf->status == VDP_PRESENTATION_QUEUE_STATUS_IDLE);
+
     src_surf = get_surface(source_surface);
 
     dst_pix  = dst_surf->pix;
@@ -245,6 +244,15 @@ VdpStatus vdp_output_surface_render_bitmap_surface(
     }
 
     if (source_surface == VDP_INVALID_HANDLE) {
+        ret = host1x_gr2d_clear_rect(dst_surf->dev->stream,
+                                     dst_surf->pixbuf,
+                                     0xFFFFFFFF,
+                                     dst_x0, dst_y0,
+                                     dst_width, dst_height);
+        if (ret == 0) {
+            return VDP_STATUS_OK;
+        }
+
         ret = pixman_fill(dst_data,
                           pixman_image_get_stride(dst_pix) / 4,
                           PIXMAN_FORMAT_BPP(pfmt),
@@ -298,6 +306,18 @@ VdpStatus vdp_output_surface_render_bitmap_surface(
         need_scale = 1;
     }
 
+    if (!need_rotate) {
+        host1x_gr2d_surface_blit(dst_surf->dev->stream,
+                                 src_surf->pixbuf,
+                                 dst_surf->pixbuf,
+                                 NULL,
+                                 src_x0, src_y0,
+                                 src_width, src_height,
+                                 dst_x0, dst_y0,
+                                 dst_width, dst_height);
+        return VDP_STATUS_OK;
+    }
+
     if (need_scale || need_rotate) {
         pixman_transform_init_identity(&transform);
 
diff --git a/src/surface_video.c b/src/surface_video.c
index a2db183..b2dc8db 100644
--- a/src/surface_video.c
+++ b/src/surface_video.c
@@ -102,11 +102,11 @@ VdpStatus vdp_video_surface_get_parameters(VdpVideoSurface surface,
     }
 
     if (width != NULL) {
-        *width = pixman_image_get_width(surf->pix);
+        *width = surf->pixbuf->width;
     }
 
     if (width != NULL) {
-        *height = pixman_image_get_height(surf->pix);
+        *height = surf->pixbuf->height;
     }
 
     return VDP_STATUS_OK;
@@ -144,12 +144,12 @@ VdpStatus vdp_video_surface_get_bits_y_cb_cr(
         return ret;
     }
 
-    width  = pixman_image_get_width(surf->pix);
-    height = pixman_image_get_height(surf->pix);
+    width  = surf->pixbuf->width;
+    height = surf->pixbuf->height;
 
     /* Copy luma plane.  */
     ret = pixman_blt(surf->y_data, dst_y,
-                     width / 4, destination_pitches[0] / 4,
+                     surf->pixbuf->pitch / 4, destination_pitches[0] / 4,
                      8, 8,
                      0, 0,
                      0, 0,
@@ -158,7 +158,7 @@ VdpStatus vdp_video_surface_get_bits_y_cb_cr(
 
     /* Copy chroma blue plane.  */
     ret = pixman_blt(surf->cb_data, dst_cb,
-                     width / 2 / 4, destination_pitches[1] / 4,
+                     surf->pixbuf->pitch_uv / 4, destination_pitches[1] / 4,
                      8, 8,
                      0, 0,
                      0, 0,
@@ -167,7 +167,7 @@ VdpStatus vdp_video_surface_get_bits_y_cb_cr(
 
     /* Copy chroma red plane.  */
     ret = pixman_blt(surf->cr_data, dst_cr,
-                     width / 2 / 4, destination_pitches[2] / 4,
+                     surf->pixbuf->pitch_uv / 4, destination_pitches[2] / 4,
                      8, 8,
                      0, 0,
                      0, 0,
@@ -217,12 +217,12 @@ VdpStatus vdp_video_surface_put_bits_y_cb_cr(
         return ret;
     }
 
-    width  = pixman_image_get_width(surf->pix);
-    height = pixman_image_get_height(surf->pix);
+    width  = surf->pixbuf->width;
+    height = surf->pixbuf->height;
 
     /* Copy luma plane.  */
     ret = pixman_blt(src_y, surf->y_data,
-                     source_pitches[0] / 4, width / 4,
+                     source_pitches[0] / 4, surf->pixbuf->pitch / 4,
                      8, 8,
                      0, 0,
                      0, 0,
@@ -231,7 +231,7 @@ VdpStatus vdp_video_surface_put_bits_y_cb_cr(
 
     /* Copy chroma blue plane.  */
     ret = pixman_blt(src_cb, surf->cb_data,
-                     source_pitches[1] / 4, width / 2 / 4,
+                     source_pitches[1] / 4, surf->pixbuf->pitch_uv / 4,
                      8, 8,
                      0, 0,
                      0, 0,
@@ -240,13 +240,15 @@ VdpStatus vdp_video_surface_put_bits_y_cb_cr(
 
     /* Copy chroma red plane.  */
     ret = pixman_blt(src_cr, surf->cr_data,
-                     source_pitches[2] / 4, width / 2 / 4,
+                     source_pitches[2] / 4, surf->pixbuf->pitch_uv / 4,
                      8, 8,
                      0, 0,
                      0, 0,
                      width / 2, height / 2);
     assert(ret != 0);
 
+    host1x_pixelbuffer_check_guard(surf->pixbuf);
+
     ret = sync_video_frame_dmabufs(surf, WRITE_END);
 
     if (ret) {
diff --git a/src/tegra_stream.c b/src/tegra_stream.c
new file mode 100644
index 0000000..a1c8aac
--- /dev/null
+++ b/src/tegra_stream.c
@@ -0,0 +1,275 @@
+/*
+ * Copyright (c) 2016-2017 Dmitry Osipenko <digetx@gmail.com>
+ * Copyright (C) 2012-2013 NVIDIA Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS\n", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Arto Merilainen <amerilainen@nvidia.com>
+ */
+
+#include "vdpau_tegra.h"
+
+/*
+ * tegra_stream_create(channel)
+ *
+ * Create a stream for given channel. This function preallocates several
+ * command buffers for later usage to improve performance. Streams are
+ * used for generating command buffers opcode by opcode using
+ * tegra_stream_push().
+ */
+
+int tegra_stream_create(struct drm_tegra *drm,
+                        struct drm_tegra_channel *channel,
+                        struct tegra_stream *stream,
+                        uint32_t words_num)
+{
+    stream->status    = TEGRADRM_STREAM_FREE;
+    stream->channel   = channel;
+    stream->num_words = words_num ?: 1;
+
+    return 0;
+}
+
+/*
+ * tegra_stream_destroy(stream)
+ *
+ * Destroy the given stream object. All resrouces are released.
+ */
+
+void tegra_stream_destroy(struct tegra_stream *stream)
+{
+    if (!stream)
+        return;
+
+    drm_tegra_job_free(stream->job);
+}
+
+/*
+ * tegra_stream_flush(stream, fence)
+ *
+ * Send the current contents of stream buffer. The stream must be
+ * synchronized correctly (we cannot send partial streams). If
+ * pointer to fence is given, the fence will contain the syncpoint value
+ * that is reached when operations in the buffer are finished.
+ */
+
+int tegra_stream_flush(struct tegra_stream *stream)
+{
+    struct drm_tegra_fence *fence;
+    int result = 0;
+
+    if (!stream)
+        return -1;
+
+    /* Reflushing is fine */
+    if (stream->status == TEGRADRM_STREAM_FREE)
+        return 0;
+
+    /* Return error if stream is constructed badly */
+    if (stream->status != TEGRADRM_STREAM_READY) {
+        result = -1;
+        goto cleanup;
+    }
+
+    result = drm_tegra_job_submit(stream->job, &fence);
+    if (result != 0) {
+        ErrorMsg("drm_tegra_job_submit() failed %d\n", result);
+        result = -1;
+        goto cleanup;
+    }
+
+    result = drm_tegra_fence_wait_timeout(fence, 1000);
+    if (result != 0) {
+        ErrorMsg("drm_tegra_fence_wait_timeout() failed %d\n", result);
+        result = -1;
+    }
+
+    drm_tegra_fence_free(fence);
+
+cleanup:
+    drm_tegra_job_free(stream->job);
+
+    stream->job = NULL;
+    stream->status = TEGRADRM_STREAM_FREE;
+
+    return result;
+}
+
+/*
+ * tegra_stream_begin(stream, num_words, fence, num_fences, num_syncpt_incrs,
+ *          num_relocs, class_id)
+ *
+ * Start constructing a stream.
+ *  - num_words refer to the maximum number of words the stream can contain.
+ *  - fence is a pointer to a table that contains syncpoint preconditions
+ *    before the stream execution can start.
+ *  - num_fences indicate the number of elements in the fence table.
+ *  - num_relocs indicate the number of memory references in the buffer.
+ *  - class_id refers to the class_id that is selected in the beginning of a
+ *    stream. If no class id is given, the default class id (=usually the
+ *    client device's class) is selected.
+ *
+ * This function verifies that the current buffer has enough room for holding
+ * the whole stream (this is computed using num_words and num_relocs). The
+ * function blocks until the stream buffer is ready for use.
+ */
+
+int tegra_stream_begin(struct tegra_stream *stream)
+{
+    int ret;
+
+    /* check stream and its state */
+    if (!(stream && stream->status == TEGRADRM_STREAM_FREE)) {
+        ErrorMsg("Stream status isn't FREE\n");
+        return -1;
+    }
+
+    ret = drm_tegra_job_new(&stream->job, stream->channel);
+    if (ret != 0) {
+        ErrorMsg("drm_tegra_job_new() failed %d\n", ret);
+        return -1;
+    }
+
+    ret = drm_tegra_pushbuf_new(&stream->buffer.pushbuf, stream->job);
+    if (ret != 0) {
+        ErrorMsg("drm_tegra_pushbuf_new() failed %d\n", ret);
+        drm_tegra_job_free(stream->job);
+        return -1;
+    }
+
+    ret = drm_tegra_pushbuf_prepare(stream->buffer.pushbuf, stream->num_words);
+    if (ret != 0) {
+        ErrorMsg("drm_tegra_pushbuf_prepare() failed %d\n", ret);
+        drm_tegra_job_free(stream->job);
+        return -1;
+    }
+
+    stream->class_id = 0;
+    stream->status = TEGRADRM_STREAM_CONSTRUCT;
+
+    return 0;
+}
+
+/*
+ * tegra_stream_push_reloc(stream, h, offset)
+ *
+ * Push a memory reference to the stream.
+ */
+
+int tegra_stream_push_reloc(struct tegra_stream *stream,
+                            struct drm_tegra_bo *bo,
+                            unsigned offset)
+{
+    int ret;
+
+    if (!(stream && stream->status == TEGRADRM_STREAM_CONSTRUCT)) {
+        ErrorMsg("Stream status isn't CONSTRUCT\n");
+        return -1;
+    }
+
+    ret = drm_tegra_pushbuf_relocate(stream->buffer.pushbuf,
+                                     bo, offset, 0);
+    if (ret != 0) {
+        stream->status = TEGRADRM_STREAM_CONSTRUCTION_FAILED;
+        ErrorMsg("drm_tegra_pushbuf_relocate() failed %d\n", ret);
+        return -1;
+    }
+
+    return 0;
+}
+
+/*
+ * tegra_stream_push(stream, word)
+ *
+ * Push a single word to given stream.
+ */
+
+int tegra_stream_push(struct tegra_stream *stream, uint32_t word)
+{
+    int ret;
+
+    if (!(stream && stream->status == TEGRADRM_STREAM_CONSTRUCT)) {
+        ErrorMsg("Stream status isn't CONSTRUCT\n");
+        return -1;
+    }
+
+    ret = drm_tegra_pushbuf_prepare(stream->buffer.pushbuf, 1);
+    if (ret != 0) {
+        stream->status = TEGRADRM_STREAM_CONSTRUCTION_FAILED;
+        ErrorMsg("drm_tegra_pushbuf_prepare() failed %d\n", ret);
+        return -1;
+    }
+
+    *stream->buffer.pushbuf->ptr++ = word;
+
+    return 0;
+}
+
+/*
+ * tegra_stream_push_setclass(stream, class_id)
+ *
+ * Push "set class" opcode to the stream. Do nothing if the class is already
+ * active
+ */
+
+int tegra_stream_push_setclass(struct tegra_stream *stream, unsigned class_id)
+{
+    int result;
+
+    if (stream->class_id == class_id)
+        return 0;
+
+    result = tegra_stream_push(stream, HOST1X_OPCODE_SETCL(0, class_id, 0));
+
+    if (result == 0)
+        stream->class_id = class_id;
+
+    return result;
+}
+
+/*
+ * tegra_stream_end(stream)
+ *
+ * Mark end of stream. This function pushes last syncpoint increment for
+ * marking end of stream.
+ */
+
+int tegra_stream_end(struct tegra_stream *stream)
+{
+    int ret;
+
+    if (!(stream && stream->status == TEGRADRM_STREAM_CONSTRUCT)) {
+        ErrorMsg("Stream status isn't CONSTRUCT\n");
+        return -1;
+    }
+
+    ret = drm_tegra_pushbuf_sync(stream->buffer.pushbuf,
+                                 DRM_TEGRA_SYNCPT_COND_OP_DONE);
+    if (ret != 0) {
+        stream->status = TEGRADRM_STREAM_CONSTRUCTION_FAILED;
+        ErrorMsg("drm_tegra_pushbuf_sync() failed %d\n", ret);
+        return -1;
+    }
+
+    stream->status = TEGRADRM_STREAM_READY;
+
+    return 0;
+}
diff --git a/src/tegra_stream.h b/src/tegra_stream.h
new file mode 100644
index 0000000..315b54c
--- /dev/null
+++ b/src/tegra_stream.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2016-2017 Dmitry Osipenko <digetx@gmail.com>
+ * Copyright (C) 2012-2013 NVIDIA Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Arto Merilainen <amerilainen@nvidia.com>
+ */
+
+#ifndef TEGRA_STREAM_H_
+#define TEGRA_STREAM_H_
+
+enum tegra_stream_status {
+    TEGRADRM_STREAM_FREE,
+    TEGRADRM_STREAM_CONSTRUCT,
+    TEGRADRM_STREAM_CONSTRUCTION_FAILED,
+    TEGRADRM_STREAM_READY,
+};
+
+struct tegra_command_buffer {
+    struct drm_tegra_pushbuf *pushbuf;
+};
+
+struct tegra_stream {
+    enum tegra_stream_status status;
+
+    struct drm_tegra_job *job;
+    struct drm_tegra_channel *channel;
+
+    struct tegra_command_buffer buffer;
+    int num_words;
+    uint32_t class_id;
+};
+
+/* Stream operations */
+int tegra_stream_create(struct drm_tegra *drm,
+                        struct drm_tegra_channel *channel,
+                        struct tegra_stream *stream,
+                        uint32_t words_num);
+void tegra_stream_destroy(struct tegra_stream *stream);
+int tegra_stream_begin(struct tegra_stream *stream);
+int tegra_stream_end(struct tegra_stream *stream);
+int tegra_stream_flush(struct tegra_stream *stream);
+int tegra_stream_push(struct tegra_stream *stream, uint32_t word);
+int tegra_stream_push_setclass(struct tegra_stream *stream, unsigned class_id);
+int tegra_stream_push_reloc(struct tegra_stream *stream,
+                            struct drm_tegra_bo *bo, unsigned offset);
+
+#endif
diff --git a/src/uapi/tegra-vde.h b/src/uapi/tegra-vde.h
index c4ae422..ed1fa87 100644
--- a/src/uapi/tegra-vde.h
+++ b/src/uapi/tegra-vde.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2016 Dmitry Osipenko <digetx@gmail.com>
+ * Copyright (C) 2016-2017 Dmitry Osipenko <digetx@gmail.com>
  * All Rights Reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining a
@@ -33,46 +33,49 @@ extern "C" {
 #endif
 
 #define FLAG_IS_B_FRAME		(1 << 0)
-#define FLAG_IS_P_FRAME		(1 << 1)
-#define FLAG_IS_REFERENCE	(1 << 2)
-#define FLAG_IS_VALID		(1 << 7)
+#define FLAG_IS_REFERENCE	(1 << 1)
 
 struct tegra_vde_h264_frame {
-    __s32 y_fd;
-    __s32 cb_fd;
-    __s32 cr_fd;
-    __s32 aux_fd;
-    __u32 frame_num;
-    __u8  flags;
+	__s32 y_fd;
+	__s32 cb_fd;
+	__s32 cr_fd;
+	__s32 aux_fd;
+	__u32 y_offset;
+	__u32 cb_offset;
+	__u32 cr_offset;
+	__u32 aux_offset;
+	__u32 frame_num;
+	__u32 flags;
 } __attribute__((packed));
 
 struct tegra_vde_h264_decoder_ctx {
-    __s32 bitstream_data_fd;
+	__s32 bitstream_data_fd;
+	__u32 bitstream_data_offset;
 
-    __u8  dpb_frames_nb;
-    __u32 dpb_frames_ptr;
-    __u8  dpb_ref_frames_with_earlier_poc_nb;
+	__u32 dpb_frames_ptr;
+	__u8  dpb_frames_nb;
+	__u8  dpb_ref_frames_with_earlier_poc_nb;
 
-    // SPS
-    __u8  is_baseline_profile;
-    __u8  level_idc;
-    __u8  log2_max_pic_order_cnt_lsb;
-    __u8  log2_max_frame_num;
-    __u8  pic_order_cnt_type;
-    __u8  direct_8x8_inference_flag;
-    __u8  pic_width_in_mbs;
-    __u8  pic_height_in_mbs;
+	// SPS
+	__u8  is_baseline_profile;
+	__u8  level_idc;
+	__u8  log2_max_pic_order_cnt_lsb;
+	__u8  log2_max_frame_num;
+	__u8  pic_order_cnt_type;
+	__u8  direct_8x8_inference_flag;
+	__u8  pic_width_in_mbs;
+	__u8  pic_height_in_mbs;
 
-    // PPS
-    __u8  pic_init_qp;
-    __u8  deblocking_filter_control_present_flag;
-    __u8  constrained_intra_pred_flag;
-    __u8  chroma_qp_index_offset;
-    __u8  pic_order_present_flag;
+	// PPS
+	__u8  pic_init_qp;
+	__u8  deblocking_filter_control_present_flag;
+	__u8  constrained_intra_pred_flag;
+	__u8  chroma_qp_index_offset;
+	__u8  pic_order_present_flag;
 
-    // Slice header
-    __u8  num_ref_idx_l0_active_minus1;
-    __u8  num_ref_idx_l1_active_minus1;
+	// Slice header
+	__u8  num_ref_idx_l0_active_minus1;
+	__u8  num_ref_idx_l1_active_minus1;
 } __attribute__((packed));
 
 #define VDE_IOCTL_BASE			'v'
diff --git a/src/util_double_list.h b/src/util_double_list.h
new file mode 100644
index 0000000..7e48b26
--- /dev/null
+++ b/src/util_double_list.h
@@ -0,0 +1,143 @@
+/*
+ *
+ * Copyright 2006 Tungsten Graphics, Inc., Bismarck, ND. USA.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+
+/**
+ * \file
+ * List macros heavily inspired by the Linux kernel
+ * list handling. No list looping yet.
+ *
+ * Is not threadsafe, so common operations need to
+ * be protected using an external mutex.
+ */
+#ifndef _U_DOUBLE_LIST_H_
+#define _U_DOUBLE_LIST_H_
+
+#include <stddef.h>
+
+struct list_head
+{
+    struct list_head *prev;
+    struct list_head *next;
+};
+
+static inline void list_inithead(struct list_head *item)
+{
+    item->prev = item;
+    item->next = item;
+}
+
+static inline void list_add(struct list_head *item, struct list_head *list)
+{
+    item->prev = list;
+    item->next = list->next;
+    list->next->prev = item;
+    list->next = item;
+}
+
+static inline void list_addtail(struct list_head *item, struct list_head *list)
+{
+    item->next = list;
+    item->prev = list->prev;
+    list->prev->next = item;
+    list->prev = item;
+}
+
+static inline void list_replace(struct list_head *from, struct list_head *to)
+{
+    to->prev = from->prev;
+    to->next = from->next;
+    from->next->prev = to;
+    from->prev->next = to;
+}
+
+static inline void list_del(struct list_head *item)
+{
+    item->prev->next = item->next;
+    item->next->prev = item->prev;
+}
+
+static inline void list_delinit(struct list_head *item)
+{
+    item->prev->next = item->next;
+    item->next->prev = item->prev;
+    item->next = item;
+    item->prev = item;
+}
+
+#define LIST_INITHEAD(__item) list_inithead(__item)
+#define LIST_ADD(__item, __list) list_add(__item, __list)
+#define LIST_ADDTAIL(__item, __list) list_addtail(__item, __list)
+#define LIST_REPLACE(__from, __to) list_replace(__from, __to)
+#define LIST_DEL(__item) list_del(__item)
+#define LIST_DELINIT(__item) list_delinit(__item)
+
+#define LIST_ENTRY(__type, __item, __field)   \
+    ((__type *)(((char *)(__item)) - offsetof(__type, __field)))
+
+#define LIST_FIRST_ENTRY(__ptr, __type, __field)   \
+    LIST_ENTRY(__type, (__ptr)->next, __field)
+
+#define LIST_LAST_ENTRY(__ptr, __type, __field)   \
+    LIST_ENTRY(__type, (__ptr)->prev, __field)
+
+#define LIST_IS_EMPTY(__list)                   \
+    ((__list)->next == (__list))
+
+#ifndef container_of
+#define container_of(ptr, sample, member)				\
+    (void *)((char *)(ptr)						\
+	     - ((char *)&((typeof(sample))0)->member))
+#endif
+
+#define LIST_FOR_EACH_ENTRY(pos, head, member)				\
+   for (pos = container_of((head)->next, pos, member);			\
+	&pos->member != (head);						\
+	pos = container_of(pos->member.next, pos, member))
+
+#define LIST_FOR_EACH_ENTRY_SAFE(pos, storage, head, member)	\
+   for (pos = container_of((head)->next, pos, member),			\
+	storage = container_of(pos->member.next, pos, member);	\
+	&pos->member != (head);						\
+	pos = storage, storage = container_of(storage->member.next, storage, member))
+
+#define LIST_FOR_EACH_ENTRY_SAFE_REV(pos, storage, head, member)	\
+   for (pos = container_of((head)->prev, pos, member),			\
+	storage = container_of(pos->member.prev, pos, member);		\
+	&pos->member != (head);						\
+	pos = storage, storage = container_of(storage->member.prev, storage, member))
+
+#define LIST_FOR_EACH_ENTRY_FROM(pos, start, head, member)		\
+   for (pos = container_of((start), pos, member);			\
+	&pos->member != (head);						\
+	pos = container_of(pos->member.next, pos, member))
+
+#define LIST_FOR_EACH_ENTRY_FROM_REV(pos, start, head, member)		\
+   for (pos = container_of((start), pos, member);			\
+	&pos->member != (head);						\
+	pos = container_of(pos->member.prev, pos, member))
+
+#endif /*_U_DOUBLE_LIST_H_*/
diff --git a/src/vdpau_tegra.c b/src/vdpau_tegra.c
index c330607..bda1f91 100644
--- a/src/vdpau_tegra.c
+++ b/src/vdpau_tegra.c
@@ -31,7 +31,7 @@ static tegra_pq      * tegra_pqs[MAX_PRESENTATION_QUEUES_NB];
 tegra_device * get_device(VdpDevice device)
 {
     if (device >= MAX_DEVICES_NB ) {
-        fprintf(stderr, "%s: Invalid handle %u\n", __func__, device);
+        ErrorMsg("%s: Invalid handle %u\n", __func__, device);
         return NULL;
     }
 
@@ -225,27 +225,32 @@ VdpStatus vdp_generate_csc_matrix(VdpProcamp *procamp,
         return VDP_STATUS_INVALID_POINTER;
     }
 
-    // BT.601 table
     switch (standard) {
     case VDP_COLOR_STANDARD_ITUR_BT_601:
+        (*csc_matrix)[0][0] =  1.164f;
         (*csc_matrix)[0][1] =  0.000f;
-        (*csc_matrix)[0][2] =  1.403f;
+        (*csc_matrix)[0][2] =  1.596f;
 
-        (*csc_matrix)[1][1] = -0.344f;
-        (*csc_matrix)[1][2] = -0.714f;
+        (*csc_matrix)[1][0] =  1.164f;
+        (*csc_matrix)[1][1] = -0.392f;
+        (*csc_matrix)[1][2] = -0.813f;
 
-        (*csc_matrix)[2][1] =  1.773f;
+        (*csc_matrix)[2][0] =  1.164f;
+        (*csc_matrix)[2][1] =  2.017f;
         (*csc_matrix)[2][2] =  0.000f;
         break;
     case VDP_COLOR_STANDARD_ITUR_BT_709:
-        (*csc_matrix)[0][1] =  0.0000f;
-        (*csc_matrix)[0][2] =  1.5701f;
+        (*csc_matrix)[0][0] =  1.164f;
+        (*csc_matrix)[0][1] =  0.000f;
+        (*csc_matrix)[0][2] =  1.793f;
 
-        (*csc_matrix)[1][1] = -0.1870f;
-        (*csc_matrix)[1][2] = -0.4664f;
+        (*csc_matrix)[1][0] =  1.164f;
+        (*csc_matrix)[1][1] = -0.213f;
+        (*csc_matrix)[1][2] = -0.533f;
 
-        (*csc_matrix)[2][1] =  1.8556f;
-        (*csc_matrix)[2][2] =  0.0000f;
+        (*csc_matrix)[2][0] =  1.164f;
+        (*csc_matrix)[2][1] =  2.112f;
+        (*csc_matrix)[2][2] =  0.000f;
         break;
     default:
         abort();
@@ -268,9 +273,6 @@ VdpStatus vdp_generate_csc_matrix(VdpProcamp *procamp,
         (*csc_matrix)[i][0] = procamp->contrast;
         (*csc_matrix)[i][1] = u;
         (*csc_matrix)[i][2] = v;
-        (*csc_matrix)[i][3] = - (u + v) / 2;
-        (*csc_matrix)[i][3] += 0.5f - procamp->contrast / 2;
-        (*csc_matrix)[i][3] += procamp->brightness;
     }
 
     return VDP_STATUS_OK;
@@ -368,6 +370,28 @@ VdpStatus vdp_preemption_callback_register(VdpDevice device,
     return VDP_STATUS_OK;
 }
 
+void ref_device(tegra_device *dev)
+{
+    atomic_inc(&dev->refcnt);
+}
+
+VdpStatus unref_device(tegra_device *dev)
+{
+    if (!atomic_dec_and_test(&dev->refcnt))
+        return VDP_STATUS_OK;
+
+    XvUngrabPort(dev->display, dev->xv_port, CurrentTime);
+    tegra_stream_destroy(dev->stream);
+    drm_tegra_channel_close(dev->gr2d);
+    drm_tegra_close(dev->drm);
+    close(dev->vde_fd);
+    close(dev->drm_fd);
+    free(dev->stream);
+    free(dev);
+
+    return VDP_STATUS_OK;
+}
+
 VdpStatus vdp_device_destroy(VdpDevice device)
 {
     tegra_device *dev = get_device(device);
@@ -378,12 +402,7 @@ VdpStatus vdp_device_destroy(VdpDevice device)
 
     tegra_devices[device] = NULL;
 
-    drm_tegra_close(dev->drm);
-    close(dev->vde_fd);
-    close(dev->drm_fd);
-    free(dev);
-
-    return VDP_STATUS_OK;
+    return unref_device(dev);
 }
 
 EXPORTED VdpStatus vdp_imp_device_create_x11(Display *display,
@@ -392,7 +411,14 @@ EXPORTED VdpStatus vdp_imp_device_create_x11(Display *display,
                                              VdpGetProcAddress **get_proc_address)
 {
     struct drm_tegra *drm = NULL;
+    struct drm_tegra_channel *gr2d = NULL;
+    struct tegra_stream *stream = NULL;
+    XvAdaptorInfo *adaptor_info = NULL;
+    XvImageFormatValues *fmt;
     VdpDevice i;
+    unsigned int ver, rel, req, ev, err;
+    unsigned int num_adaptors;
+    int num_formats;
     int vde_fd = -1;
     int drm_fd = -1;
     int ret;
@@ -411,11 +437,75 @@ EXPORTED VdpStatus vdp_imp_device_create_x11(Display *display,
 
     ret = drm_tegra_new(&drm, drm_fd);
     if (ret < 0) {
+        ErrorMsg("Tegra DRM not detected\n");
+        goto err_cleanup;
+    }
+
+    ret = drm_tegra_channel_open(&gr2d, drm, DRM_TEGRA_GR2D);
+    if (ret < 0) {
+        ErrorMsg("failed to open 2D channel: %d\n", ret);
+        goto err_cleanup;
+    }
+
+    stream = calloc(1, sizeof(*stream));
+    if (!stream) {
+        ErrorMsg("failed to allocate command stream\n");
+        goto err_cleanup;
+    }
+
+    ret = tegra_stream_create(drm, gr2d, stream, 0);
+    if (ret < 0) {
+        ErrorMsg("failed to create command stream: %d\n", ret);
+        goto err_cleanup;
+    }
+
+    ret = XvQueryExtension(display, &ver, &rel, &req, &ev, &err);
+    if (ret != Success) {
+        ErrorMsg("Xv is disabled in the Xorg driver\n");
         goto err_cleanup;
     }
 
+    ret = XvQueryAdaptors(display, DefaultRootWindow(display),
+                          &num_adaptors, &adaptor_info);
+    if (ret != Success) {
+        goto err_cleanup;
+    }
+
+    while (num_adaptors--) {
+        if (adaptor_info[num_adaptors].num_ports != 1)
+            continue;
+
+        if (!(adaptor_info[num_adaptors].type & XvImageMask))
+            continue;
+
+        fmt = XvListImageFormats(display, adaptor_info[num_adaptors].base_id,
+                                 &num_formats);
+
+        while (num_formats--) {
+            if (!strncmp(fmt[num_formats].guid, "PASSTHROUGH_YV12", 16) &&
+                    fmt[num_formats].id == FOURCC_PASSTHROUGH_YV12) {
+                goto xv_detected;
+            }
+        }
+
+        XFree(fmt);
+    }
+
+    ErrorMsg("Opentegra Xv undetected\n");
+
+    goto err_cleanup;
+
+xv_detected:
     pthread_mutex_lock(&global_lock);
 
+    XFree(fmt);
+
+    ret = XvGrabPort(display, adaptor_info[num_adaptors].base_id, CurrentTime);
+    if (ret != Success) {
+        ErrorMsg("Xv port is busy\n");
+        goto err_cleanup;
+    }
+
     for (i = 0; i < MAX_DEVICES_NB; i++) {
         if (tegra_devices[i] == NULL) {
             tegra_devices[i] = calloc(1, sizeof(tegra_device));
@@ -429,21 +519,33 @@ EXPORTED VdpStatus vdp_imp_device_create_x11(Display *display,
         goto err_cleanup;
     }
 
+    atomic_set(&tegra_devices[i]->refcnt, 1);
+    tegra_devices[i]->xv_port = adaptor_info[num_adaptors].base_id;
     tegra_devices[i]->display = display;
     tegra_devices[i]->screen = screen;
     tegra_devices[i]->vde_fd = vde_fd;
     tegra_devices[i]->drm_fd = drm_fd;
+    tegra_devices[i]->stream = stream;
+    tegra_devices[i]->gr2d = gr2d;
     tegra_devices[i]->drm = drm;
 
     *device = i;
     *get_proc_address = vdp_get_proc_address;
 
+    XvFreeAdaptorInfo(adaptor_info);
+
     return VDP_STATUS_OK;
 
 err_cleanup:
+    tegra_stream_destroy(stream);
+    drm_tegra_channel_close(gr2d);
     drm_tegra_close(drm);
     close(drm_fd);
     close(vde_fd);
+    free(stream);
+
+    if (adaptor_info)
+        XvFreeAdaptorInfo(adaptor_info);
 
     return VDP_STATUS_RESOURCES;
 }
diff --git a/src/vdpau_tegra.h b/src/vdpau_tegra.h
index d683724..94fb629 100644
--- a/src/vdpau_tegra.h
+++ b/src/vdpau_tegra.h
@@ -30,7 +30,9 @@
 #include <limits.h>
 #include <math.h>
 #include <pthread.h>
+#include <stdbool.h>
 #include <stdio.h>
+#include <stdint.h>
 #include <stdlib.h>
 #include <string.h>
 #include <sys/ioctl.h>
@@ -40,13 +42,19 @@
 
 #include <pixman.h>
 #include <vdpau/vdpau_x11.h>
+
 #include <X11/Xutil.h>
+#include <X11/extensions/Xvlib.h>
 
 #include <libdrm/tegra_drm.h>
 #include <libdrm/tegra.h>
 #include <xf86drm.h>
 
+#include "atomic.h"
 #include "bitstream.h"
+#include "tegra_stream.h"
+#include "host1x.h"
+#include "util_double_list.h"
 #include "uapi/dma-buf.h"
 #include "uapi/tegra-vde.h"
 
@@ -58,26 +66,87 @@
 
 #define TEGRA_VDPAU_INTERFACE_VERSION 1
 
-#define MAX_DEVICES_NB                      32
-#define MAX_DECODERS_NB                     8
+#define MAX_DEVICES_NB                      1
+#define MAX_DECODERS_NB                     1
 #define MAX_MIXERS_NB                       16
 #define MAX_SURFACES_NB                     256
 #define MAX_PRESENTATION_QUEUE_TARGETS_NB   32
 #define MAX_PRESENTATION_QUEUES_NB          128
 
+#define SURFACE_VIDEO               (1 << 0)
+#define SURFACE_YUV_UNCONVERTED     (1 << 1)
+#define SURFACE_DATA_NEEDS_SYNC     (1 << 2)
+
+#define PASSTHROUGH_DATA_SIZE   36
+
+#define FOURCC_PASSTHROUGH_YV12     (('1' << 24) + ('2' << 16) + ('V' << 8) + 'Y')
+#define FOURCC_PASSTHROUGH_XRGB565  (('1' << 24) + ('B' << 16) + ('G' << 8) + 'R')
+#define FOURCC_PASSTHROUGH_XRGB8888 (('X' << 24) + ('B' << 16) + ('G' << 8) + 'R')
+#define FOURCC_PASSTHROUGH_XBGR8888 (('X' << 24) + ('R' << 16) + ('G' << 8) + 'B')
+
+#define __align_mask(value, mask)  (((value) + (mask)) & ~(mask))
+#define ALIGN(value, alignment)    __align_mask(value, (typeof(value))((alignment) - 1))
+
+#define ErrorMsg(fmt, args...) \
+    fprintf(stderr, "%s:%d/%s(): " fmt, \
+            __FILE__, __LINE__, __func__, ##args)
+
+typedef struct {
+    int atomic;
+} atomic_t;
+
 extern pthread_mutex_t global_lock;
 
 typedef struct tegra_device {
     struct drm_tegra *drm;
+    struct drm_tegra_channel *gr2d;
+    struct tegra_stream *stream;
     Display *display;
+    XvPortID xv_port;
+    atomic_t refcnt;
     int screen;
     int vde_fd;
     int drm_fd;
 } tegra_device;
 
+typedef struct tegra_surface {
+    tegra_device *dev;
+
+    struct tegra_vde_h264_frame *frame;
+    int32_t pic_order_cnt;
+
+    pixman_image_t *pix;
+    XvImage *xv_img;
+    uint32_t flags;
+
+    void *y_data;
+    void *cb_data;
+    void *cr_data;
+
+    struct host1x_pixelbuffer *pixbuf;
+    struct drm_tegra_bo *y_bo;
+    struct drm_tegra_bo *cb_bo;
+    struct drm_tegra_bo *cr_bo;
+    struct drm_tegra_bo *aux_bo;
+
+    uint32_t disp_width;
+    uint32_t disp_height;
+
+    VdpPresentationQueueStatus status;
+    VdpTime first_presentation_time;
+    VdpTime earliest_presentation_time;
+
+    atomic_t refcnt;
+    struct list_head list_item;
+    pthread_cond_t idle_cond;
+    pthread_mutex_t lock;
+
+    bool idle_hack;
+} tegra_surface;
+
 typedef struct tegra_decoder {
     struct drm_tegra_bo *bitstream_bo;
-    VdpDevice device;
+    tegra_device *dev;
     bitstream_reader reader;
     int bitstream_data_fd;
     void *bitstream_data;
@@ -88,44 +157,34 @@ typedef struct tegra_decoder {
 
 typedef struct tegra_mixer {
     VdpCSCMatrix csc_matrix;
+    VdpColor bg_color;
 } tegra_mixer;
 
 typedef struct tegra_pqt {
-    VdpDevice device;
+    tegra_device *dev;
+    tegra_surface *disp_surf;
     Drawable drawable;
     GC gc;
+    atomic_t refcnt;
 } tegra_pqt;
 
 typedef struct tegra_pq {
-    VdpPresentationQueueTarget presentation_queue_target;
+    tegra_pqt *pqt;
     VdpColor background_color;
-} tegra_pq;
-
-#define SURFACE_VIDEO           (1 << 0)
-#define SURFACE_YUV_UNCONVERTED (1 << 1)
-#define SURFACE_DATA_NEEDS_SYNC (1 << 2)
 
-typedef struct tegra_surface {
-    struct tegra_vde_h264_frame *frame;
-    int32_t pic_order_cnt;
-
-    pixman_image_t *pix;
-    pixman_image_t *pix_disp;
-    XImage *img;
-    void *rawdata;
-    uint32_t flags;
+    struct list_head surf_list;
+    pthread_mutex_t lock;
+    pthread_cond_t cond;
+    pthread_t disp_thread;
+    pthread_t x11_thread;
+    bool exit;
+} tegra_pq;
 
-    void *y_data;
-    void *cb_data;
-    void *cr_data;
+tegra_device * get_device(VdpDevice device);
 
-    struct drm_tegra_bo *y_bo;
-    struct drm_tegra_bo *cb_bo;
-    struct drm_tegra_bo *cr_bo;
-    struct drm_tegra_bo *aux_bo;
-} tegra_surface;
+void ref_device(tegra_device *dev);
 
-tegra_device * get_device(VdpDevice device);
+VdpStatus unref_device(tegra_device *dev);
 
 tegra_decoder * get_decoder(VdpDecoder decoder);
 
@@ -152,13 +211,15 @@ void set_presentation_queue(VdpPresentationQueue presentation_queue,
 uint32_t create_surface(tegra_device *dev,
                         uint32_t width,
                         uint32_t height,
-                        pixman_format_code_t pfmt,
+                        VdpRGBAFormat rgba_format,
                         int output,
                         int video);
 
 VdpStatus destroy_surface(tegra_surface *surf);
 
-int convert_video_surf(tegra_surface *surf, VdpCSCMatrix cscmat);
+void ref_surface(tegra_surface *surf);
+
+VdpStatus unref_surface(tegra_surface *surf);
 
 int sync_dmabuf_write_start(int dmabuf_fd);