diff --git a/fb/Cargo.toml b/fb/Cargo.toml new file mode 100644 index 0000000..697c5ed --- /dev/null +++ b/fb/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "fb" +version = "0.1.0" +edition = "2024" + +[dependencies] +libc = "0.2.177" +memmap2 = "0.9.10" diff --git a/fb/README.md b/fb/README.md new file mode 100644 index 0000000..2462255 --- /dev/null +++ b/fb/README.md @@ -0,0 +1,6 @@ +This is a simple test outputting to /dev/fb0 on the CC2 + +``` +cross build --release --target armv7-unknown-linux-musleabihf +sshpass -p MTY4ODE2 scp ./target/armv7-unknown-linux-musleabihf/release/fb root@:/root/ +``` diff --git a/fb/disp.ko b/fb/disp.ko new file mode 100644 index 0000000..167f4a9 Binary files /dev/null and b/fb/disp.ko differ diff --git a/fb/src/framebuffer.rs b/fb/src/framebuffer.rs new file mode 100644 index 0000000..04db0b9 --- /dev/null +++ b/fb/src/framebuffer.rs @@ -0,0 +1,388 @@ +use memmap2::{MmapMut, MmapOptions}; +use std::fs::{File, OpenOptions}; +use std::io::{self, Error, ErrorKind}; +use std::os::unix::io::AsRawFd; + +const FBIOGET_VSCREENINFO: libc::c_ulong = 0x4600; +const FBIOGET_FSCREENINFO: libc::c_ulong = 0x4602; + +#[repr(C)] +#[derive(Debug, Copy, Clone)] +struct FbFixScreenInfo { + id: [u8; 16], + smem_start: libc::c_ulong, + smem_len: u32, + fb_type: u32, + type_aux: u32, + visual: u32, + xpanstep: u16, + ypanstep: u16, + ywrapstep: u16, + line_length: u32, + mmio_start: libc::c_ulong, + mmio_len: u32, + accel: u32, + capabilities: u16, + reserved: [u16; 2], +} + +#[repr(C)] +#[derive(Debug, Copy, Clone)] +struct FbVarScreenInfo { + xres: u32, + yres: u32, + xres_virtual: u32, + yres_virtual: u32, + xoffset: u32, + yoffset: u32, + bits_per_pixel: u32, + grayscale: u32, + red: FbBitfield, + green: FbBitfield, + blue: FbBitfield, + transp: FbBitfield, + nonstd: u32, + activate: u32, + height: u32, + width: u32, + accel_flags: u32, + pixclock: u32, + left_margin: u32, + right_margin: u32, + upper_margin: u32, + lower_margin: u32, + hsync_len: u32, + vsync_len: u32, + sync: u32, + vmode: u32, + rotate: u32, + colorspace: u32, + reserved: [u32; 4], +} + +#[repr(C)] +#[derive(Debug, Copy, Clone)] +struct FbBitfield { + offset: u32, + length: u32, + msb_right: u32, +} + +#[derive(Debug, Copy, Clone)] +enum PixelFormat { + Rgb565, + Native32, + Native24, + Native16, +} + +pub struct FrameBuffer { + _file: File, + fb_map: MmapMut, + pub xres: u32, + pub yres: u32, + xoffset: u32, + yoffset: u32, + line_length: usize, + bytes_per_pixel: usize, + red: FbBitfield, + green: FbBitfield, + blue: FbBitfield, + transp: FbBitfield, + format: PixelFormat, +} + +impl FrameBuffer { + pub fn new(device: &str) -> io::Result { + let file = OpenOptions::new().read(true).write(true).open(device)?; + + let mut fix_info = unsafe { std::mem::zeroed::() }; + ioctl_read(file.as_raw_fd(), FBIOGET_FSCREENINFO, &mut fix_info)?; + + let mut var_info = unsafe { std::mem::zeroed::() }; + ioctl_read(file.as_raw_fd(), FBIOGET_VSCREENINFO, &mut var_info)?; + + let bytes_per_pixel = match var_info.bits_per_pixel { + 16 => 2, + 24 => 3, + 32 => 4, + bpp => { + return Err(Error::new( + ErrorKind::Unsupported, + format!("unsupported framebuffer depth: {bpp} bpp"), + )); + } + }; + + if fix_info.smem_len == 0 { + return Err(Error::new( + ErrorKind::InvalidData, + "framebuffer reports zero memory length", + )); + } + + let line_length = fix_info.line_length as usize; + if line_length == 0 { + return Err(Error::new( + ErrorKind::InvalidData, + "framebuffer reports zero line length", + )); + } + + let format = detect_format( + var_info.bits_per_pixel, + var_info.red, + var_info.green, + var_info.blue, + ); + let fb_map = unsafe { + MmapOptions::new() + .len(fix_info.smem_len as usize) + .map_mut(&file)? + }; + + Ok(FrameBuffer { + _file: file, + fb_map, + xres: var_info.xres, + yres: var_info.yres, + xoffset: var_info.xoffset, + yoffset: var_info.yoffset, + line_length, + bytes_per_pixel, + red: var_info.red, + green: var_info.green, + blue: var_info.blue, + transp: var_info.transp, + format, + }) + } + + pub fn display_frame(&mut self, frame: &[u8], width: u32, height: u32) -> io::Result<()> { + self.display_frame_at(frame, width, height, 0, 0) + } + + pub fn display_frame_at( + &mut self, + frame: &[u8], + width: u32, + height: u32, + offset_x: i32, + offset_y: i32, + ) -> io::Result<()> { + let expected_len = rgb888_len(width, height)?; + if frame.len() < expected_len { + return Err(Error::new( + ErrorKind::InvalidInput, + format!( + "frame is too small for {width}x{height} RGB888: {} < {expected_len}", + frame.len() + ), + )); + } + + let Some(clip) = Clip::new(width, height, self.xres, self.yres, offset_x, offset_y) else { + return Ok(()); + }; + + let xoffset = self.xoffset as usize; + let yoffset = self.yoffset as usize; + let line_length = self.line_length; + let bpp = self.bytes_per_pixel; + let red = self.red; + let green = self.green; + let blue = self.blue; + let transp = self.transp; + let format = self.format; + + for row in 0..clip.height { + let src_y = clip.src_y + row; + let dst_y = clip.dst_y + row; + let src_start = ((src_y * width + clip.src_x) * 3) as usize; + let src_end = src_start + (clip.width * 3) as usize; + let src = &frame[src_start..src_end]; + + let dst_start = + (dst_y as usize + yoffset) * line_length + (clip.dst_x as usize + xoffset) * bpp; + let dst_end = dst_start + clip.width as usize * bpp; + if dst_end > self.fb_map.len() { + return Err(Error::new( + ErrorKind::InvalidData, + "visible framebuffer area exceeds mapped memory", + )); + } + + write_rgb888_row( + src, + &mut self.fb_map[dst_start..dst_end], + format, + red, + green, + blue, + transp, + ); + } + + // fbdev mappings are device memory. The writes above are the update; + // msync can return EINVAL on framebuffer mappings even after pixels land. + Ok(()) + } +} + +fn ioctl_read(fd: std::os::fd::RawFd, request: libc::c_ulong, value: &mut T) -> io::Result<()> { + let result = unsafe { libc::ioctl(fd, request.try_into().unwrap(), value as *mut T) }; + if result < 0 { + Err(io::Error::last_os_error()) + } else { + Ok(()) + } +} + +fn detect_format( + bits_per_pixel: u32, + red: FbBitfield, + green: FbBitfield, + blue: FbBitfield, +) -> PixelFormat { + match bits_per_pixel { + 16 if red.offset == 11 + && red.length == 5 + && green.offset == 5 + && green.length == 6 + && blue.offset == 0 + && blue.length == 5 => + { + PixelFormat::Rgb565 + } + 16 => PixelFormat::Native16, + 24 => PixelFormat::Native24, + 32 => PixelFormat::Native32, + _ => unreachable!("unsupported framebuffer depth was rejected earlier"), + } +} + +fn rgb888_len(width: u32, height: u32) -> io::Result { + width + .checked_mul(height) + .and_then(|pixels| pixels.checked_mul(3)) + .map(|len| len as usize) + .ok_or_else(|| Error::new(ErrorKind::InvalidInput, "RGB888 frame dimensions overflow")) +} + +fn write_rgb888_row( + src: &[u8], + dst: &mut [u8], + format: PixelFormat, + red: FbBitfield, + green: FbBitfield, + blue: FbBitfield, + transp: FbBitfield, +) { + match format { + PixelFormat::Rgb565 => write_rgb565_row(src, dst), + PixelFormat::Native16 => write_native_row::<2>(src, dst, red, green, blue, transp), + PixelFormat::Native24 => write_native_row::<3>(src, dst, red, green, blue, transp), + PixelFormat::Native32 => write_native_row::<4>(src, dst, red, green, blue, transp), + } +} + +fn write_rgb565_row(src: &[u8], dst: &mut [u8]) { + for (rgb, out) in src.chunks_exact(3).zip(dst.chunks_exact_mut(2)) { + let r = (rgb[0] as u16 >> 3) << 11; + let g = (rgb[1] as u16 >> 2) << 5; + let b = rgb[2] as u16 >> 3; + out.copy_from_slice(&(r | g | b).to_ne_bytes()); + } +} + +fn write_native_row( + src: &[u8], + dst: &mut [u8], + red: FbBitfield, + green: FbBitfield, + blue: FbBitfield, + transp: FbBitfield, +) { + for (rgb, out) in src.chunks_exact(3).zip(dst.chunks_exact_mut(N)) { + let pixel = pack_pixel(rgb[0], rgb[1], rgb[2], red, green, blue, transp); + out.copy_from_slice(&pixel.to_ne_bytes()[..N]); + } +} + +fn pack_pixel( + r: u8, + g: u8, + b: u8, + red: FbBitfield, + green: FbBitfield, + blue: FbBitfield, + transp: FbBitfield, +) -> u32 { + pack_channel(r, red) | pack_channel(g, green) | pack_channel(b, blue) | pack_alpha(transp) +} + +fn pack_channel(value: u8, field: FbBitfield) -> u32 { + if field.length == 0 { + return 0; + } + + let max = (1u32 << field.length) - 1; + let scaled = (value as u32 * max + 127) / 255; + scaled << field.offset +} + +fn pack_alpha(field: FbBitfield) -> u32 { + if field.length == 0 { + return 0; + } + + ((1u32 << field.length) - 1) << field.offset +} + +struct Clip { + src_x: u32, + src_y: u32, + dst_x: u32, + dst_y: u32, + width: u32, + height: u32, +} + +impl Clip { + fn new( + src_width: u32, + src_height: u32, + dst_width: u32, + dst_height: u32, + offset_x: i32, + offset_y: i32, + ) -> Option { + if src_width == 0 || src_height == 0 || dst_width == 0 || dst_height == 0 { + return None; + } + + let src_x = offset_x.saturating_neg().max(0) as u32; + let src_y = offset_y.saturating_neg().max(0) as u32; + let dst_x = offset_x.max(0) as u32; + let dst_y = offset_y.max(0) as u32; + + if src_x >= src_width || src_y >= src_height || dst_x >= dst_width || dst_y >= dst_height { + return None; + } + + let width = (src_width - src_x).min(dst_width - dst_x); + let height = (src_height - src_y).min(dst_height - dst_y); + if width == 0 || height == 0 { + return None; + } + + Some(Clip { + src_x, + src_y, + dst_x, + dst_y, + width, + height, + }) + } +} diff --git a/fb/src/main.rs b/fb/src/main.rs new file mode 100644 index 0000000..0a2318c --- /dev/null +++ b/fb/src/main.rs @@ -0,0 +1,41 @@ +use std::error::Error; + +use crate::framebuffer::FrameBuffer; + +mod framebuffer; + +fn main() -> Result<(), Box> { + let mut fb = FrameBuffer::new("/dev/fb0")?; + + println!("Display started at resolution ({},{})!", fb.xres, fb.yres); + + let buffer_size = fb.xres * fb.yres; + + let mut imgbuffer; + + let mut t: u8 = 0; + + loop { + imgbuffer = (0..buffer_size) + .map(|n| { + let x = n % fb.xres; + let y = n / fb.xres; + let r = scale_to_u8(x, fb.xres).wrapping_add(t); + let g = scale_to_u8(y, fb.yres).wrapping_add(t); + + (r, g, 0u8) + }) + .flat_map(|(r, g, b)| [r, g, b]) + .collect::>(); + + fb.display_frame(&imgbuffer, fb.xres, fb.yres)?; + + t = t.wrapping_add(1); + } + + // Ok(()) +} + +fn scale_to_u8(value: u32, size: u32) -> u8 { + ((value as u64 * 255) / (size - 1) as u64) as u8 +}