Merge pull request #12 from rudihorn/decim_nv12_p030

Add support for NV12, P030 and linear decimation

Cargo.toml

@@ -10,7 +10,7 @@ drm = "0.4.0"
 nix = "0.20.0"
 drm-ffi = "0.1.0"
 drm-sys = "0.1.0"
-drm-fourcc = "2.0.0"
+drm-fourcc = "2.2.0"
 image = "0.23.14"
 flatbuffers = "2.0.0"
 byteorder = "1.4.3"

src/dump_image.rs

@@ -1,21 +1,27 @@
-use std::{mem::size_of, os::fd::AsRawFd, convert::TryFrom};
+use std::{convert::TryFrom, mem::size_of, os::fd::AsRawFd};
 
-use drm::SystemError;
 use drm::control::framebuffer::Handle;
+use drm::SystemError;
 use drm_fourcc::{DrmFourcc, DrmModifier};
-use image::RgbImage;
+use image::{GenericImage, RgbImage};
 use libc::close;
 use nix::sys::mman;
 
-use crate::{Card, ffi::{self, gem_close}, image_decoder::{decode_image, rgb565_to_rgb888, decode_tiled_small_image, decode_small_image_multichannel, decode_image_multichannel}};
-
+use crate::{
+    ffi::{self, gem_close},
+    image_decoder::{
+        decode_image, decode_image_multichannel, decode_small_image_multichannel,
+        decode_tiled_small_image, rgb565_to_rgb888, ToRgb, YUV420Pixel,
+    },
+    Card,
+};
 
 fn copy_buffer<T: Sized + Copy>(
     card: &Card,
-    handle : u32,
+    handle: u32,
     to: &mut [T],
-    verbose: bool) -> Result<(), SystemError> {
-
+    verbose: bool,
+) -> Result<(), SystemError> {
     let length = to.len() * size_of::<T>();
 
     let hfd = ffi::prime_handle_to_fd(card.as_raw_fd(), handle)?;
@@ -28,14 +34,7 @@ fn copy_buffer<T: Sized + Copy>(
     let prot = mman::ProtFlags::PROT_READ;
     let flags = mman::MapFlags::MAP_SHARED;
     unsafe {
-        let map = mman::mmap(
-            addr,
-            length as _,
-            prot,
-            flags,
-            hfd,
-            0,
-        ).unwrap();
+        let map = mman::mmap(addr, length as _, prot, flags, hfd, 0).unwrap();
 
         let mapping: &mut [T] = std::slice::from_raw_parts_mut(map as *mut _, to.len());
         to.copy_from_slice(mapping);
@@ -49,6 +48,138 @@ fn copy_buffer<T: Sized + Copy>(
     Ok(())
 }
 
+fn decimate_image_4(size: (usize, usize), image: &[u32], copy: &mut [u32]) {
+    let decim = (4, 4);
+    let newsize = (size.0 / decim.0, size.1 / decim.1);
+
+    for y in 0..newsize.1 {
+        let ty = decim.1 * y;
+        for x in 0..newsize.0 {
+            let tx = decim.0 * x;
+            copy[y * newsize.0 + x] = image[ty * size.0 + tx];
+        }
+    }
+}
+
+fn decode_p030_image(
+    card: &Card,
+    size: (usize, usize),
+    pitches: u32,
+    handle: u32,
+    modifier: u64,
+    offset: usize,
+    verbose: bool,
+) -> Result<RgbImage, SystemError> {
+
+    // We assume the DRM BROADCOM SAND128 format
+    if u64::from(drm_fourcc::DrmModifier::Broadcom_sand128) != modifier & !(0xFFFF << 8) {
+        panic!("Unsupported P030 modifier value");
+    }
+
+    let stride = 128 / 4; // each column is 128 bytes wide, we use 4 bytes per word
+    let colpx = 96;
+
+    let ypitch = pitches as usize / (32 / 8);
+    let ylines = ((modifier >> 8) & 0xFFFFFFFF) as usize;
+    let length = ylines * (size.0 / colpx) * stride;
+    let crcboffset = offset / 4; // offset of the CrCb information in each column
+
+    if verbose {
+        println!(
+            "P030, size: {:?}, lines: {}, pitches: {}, length: {}",
+            size, ylines, ypitch, length
+        );
+    }
+
+    let mut yplane = vec![0u32; length as _];
+    copy_buffer(card, handle, &mut yplane, verbose)?;
+
+    let decim = 3;
+    let mut img = RgbImage::new((size.0 / decim) as _, (size.1 / decim) as _);
+    for y in 0..size.1 / decim {
+        let ty = y * decim;
+        for x in 0..size.0 / decim {
+            let tx = x * decim;
+            let col = tx / colpx;
+            let col_offset = col * stride * ylines;
+            let x_mod = (tx % colpx) / decim;
+
+            let ypx = unsafe { yplane.get_unchecked(col_offset + ty * stride + x_mod) };
+            let rx = x_mod / 2 * 2;
+            let crcind = col_offset + crcboffset + ty / 2 * stride + rx;
+            let crcbpx = unsafe { yplane.get_unchecked(crcind + 1) };
+
+            let yuv = YUV420Pixel::new((ypx >> 2) as u8, (crcbpx >> 12) as u8, (crcbpx >> 2) as u8);
+
+            unsafe {
+                img.unsafe_put_pixel(x as _, y as _, yuv.rgb());
+            }
+        }
+    }
+
+    Ok(img)
+}
+
+fn decode_nv12_image(
+    card: &Card,
+    size: (usize, usize),
+    pitches: u32,
+    handle: u32,
+    modifier: u64,
+    offset: usize,
+    verbose: bool,
+) -> Result<RgbImage, SystemError> {
+
+    // We assume the DRM BROADCOM SAND128 format
+    if u64::from(drm_fourcc::DrmModifier::Broadcom_sand128) != modifier & !(0xFFFF << 8) {
+        panic!("Unsupported NV12 modifier value");
+    }
+
+    let stride = 128 / 4; // each column is 128 bytes wide, we use 4 bytes per word
+    let colpx = 128; // 1 byte per pixel
+
+    let ypitch = pitches as usize / (32 / 8);
+    let ylines = ((modifier >> 8) & 0xFFFFFFFF) as usize;
+    let length = ylines * (size.0 / colpx) * stride;
+    let crcboffset = offset / 4; // offset of the CrCb information in each column
+
+    if verbose {
+        println!(
+            "NV12, size: {:?}, lines: {}, pitches: {}, length: {}",
+            size, ylines, ypitch, length
+        );
+    }
+
+    let mut yplane = vec![0u32; length as _];
+    copy_buffer(card, handle, &mut yplane, verbose)?;
+
+    let decim : usize = 4;
+    let mut img = RgbImage::new((size.0 / decim) as _, (size.1 / decim) as _);
+    for y in 0..size.1 / decim {
+        let ty = y * decim;
+        for x in 0..size.0 / decim {
+            let tx = x * decim;
+            let col = tx / colpx;
+            let col_offset = col * stride * ylines;
+            let x_mod = (tx % colpx) / decim;
+
+            let ypx = unsafe { yplane.get_unchecked(col_offset + ty * stride + x_mod) };
+            let rx = x_mod / 2 * 2;
+            let crcind = col_offset + crcboffset + ty / 2 * stride + rx;
+            let crcbpx = unsafe { yplane.get_unchecked(crcind + 1) };
+
+            let yuv = YUV420Pixel::new((ypx >> 0) as u8, (crcbpx >> 0) as u8, (crcbpx >> 8) as u8);
+
+            unsafe {
+                img.unsafe_put_pixel(x as _, y as _, yuv.rgb());
+            }
+        }
+    }
+
+    Ok(img)
+}
+
+
 fn dump_linear_to_image(
     card: &Card,
     pitch: u32,
@@ -62,13 +193,24 @@ fn dump_linear_to_image(
     let length = pitch * size.1 / (bpp / 8);
 
     println!(
-        "size: {:?}, pitch: {}, bpp: {}, length: {}",
+        "linear, size: {:?}, pitch: {}, bpp: {}, length: {}",
         size, pitch, bpp, length
     );
     let mut copy = vec![0u32; length as _];
     copy_buffer(card, handle, &mut copy, verbose)?;
 
-    Ok(decode_image(copy.as_mut_slice(), pitch, size))
+    let mut dec = vec![0u32; (length / (4 * 4)) as _];
+    decimate_image_4(
+        (size.0 as _, size.1 as _),
+        copy.as_slice(),
+        dec.as_mut_slice(),
+    );
+
+    Ok(decode_image(
+        dec.as_mut_slice(),
+        pitch / 4,
+        (size.0 / 4, size.1 / 4),
+    ))
 }
 
 fn dump_rgb565_to_image(
@@ -84,7 +226,7 @@ fn dump_rgb565_to_image(
     let length = pitch * size.1 / (bpp / 8);
 
     println!(
-        "size: {:?}, pitch: {}, bpp: {}, length: {}",
+        "rgb565, size: {:?}, pitch: {}, bpp: {}, length: {}",
         size, pitch, bpp, length
     );
     let mut copy = vec![0u16; length as _];
@@ -110,7 +252,12 @@ fn dump_broadcom_tiled_to_image(
     let mut copy = vec![0; (length / 4) as _];
     copy_buffer(card, handle, &mut copy, verbose)?;
 
-    Ok(decode_tiled_small_image(copy.as_mut_slice(), tilesize, tiles, size))
+    Ok(decode_tiled_small_image(
+        copy.as_mut_slice(),
+        tilesize,
+        tiles,
+        size,
+    ))
 }
 
 fn dump_yuv420_to_image(
@@ -151,7 +298,11 @@ fn dump_yuv420_to_image(
     }
 }
 
-pub fn dump_framebuffer_to_image(card: &Card, fb: Handle, verbose: bool) -> Result<RgbImage, SystemError> {
+pub fn dump_framebuffer_to_image(
+    card: &Card,
+    fb: Handle,
+    verbose: bool,
+) -> Result<RgbImage, SystemError> {
     let fbinfo2 = ffi::fb_cmd2(card.as_raw_fd(), fb.into())?;
 
     if verbose {
@@ -160,6 +311,18 @@ pub fn dump_framebuffer_to_image(card: &Card, fb: Handle, verbose: bool) -> Resu
 
     let size = (fbinfo2.width, fbinfo2.height);
 
+    if fbinfo2.pixel_format == 808661072 {
+        return decode_p030_image(
+            card,
+            (size.0 as _, size.1 as _),
+            fbinfo2.pitches[0],
+            fbinfo2.handles[0],
+            fbinfo2.modifier[0],
+            fbinfo2.offsets[1] as _,
+            verbose,
+        );
+    }
+
     let fourcc = drm_fourcc::DrmFourcc::try_from(fbinfo2.pixel_format).unwrap();
     let modifier = drm_fourcc::DrmModifier::try_from(fbinfo2.modifier[0]).unwrap();
 
@@ -208,6 +371,15 @@ pub fn dump_framebuffer_to_image(card: &Card, fb: Handle, verbose: bool) -> Resu
             fbinfo2.handles[0],
             verbose,
         ),
+        DrmFourcc::Nv12 => decode_nv12_image(
+            card,
+            (size.0 as _, size.1 as _),
+            fbinfo2.pitches[0],
+            fbinfo2.handles[0],
+            fbinfo2.modifier[0],
+            fbinfo2.offsets[1] as _,
+            verbose
+        ),
 
         _ => panic!(
             "Unsupported framebuffer pixel format: {} {:x}",

src/image_decoder.rs

@@ -60,6 +60,18 @@ impl YUV420Pixel {
     }
 }
 
+fn clamp(v : i32) -> u8 {
+    if v > 0xFF {
+        0xFF
+    } else {
+        if v < 0 {
+            0
+        } else {
+            v as u8
+        }
+    }
+}
+
 impl ToRgb for YUV420Pixel {
     fn rgb(&self) -> Rgb<u8> {
         let y = self.dat[0] as i32;
@@ -73,18 +85,6 @@ impl ToRgb for YUV420Pixel {
         let g = (298 * c - 100 * d - 208 * e + 128) >> 8;
         let b = (298 * c + 516 * d + 128) >> 8;
 
-        let clamp = |v| {
-            if v > 0xFF {
-                0xFF
-            } else {
-                if v < 0 {
-                    0
-                } else {
-                    v as u8
-                }
-            }
-        };
-
         Rgb([clamp(r), clamp(g), clamp(b)])
     }
 }

src/main.rs

@@ -192,6 +192,7 @@ fn main() {
         loop {
             if let Some(fb) = find_framebuffer(&card, verbose) {
                 dump_and_send_framebuffer(&mut socket, &card, fb, verbose).unwrap();
+                thread::sleep(Duration::from_millis(1000/20));
             } else {
                 thread::sleep(Duration::from_secs(1));
             }