broken image viewer for linux framebuffer

idump.c

#define _XOPEN_SOURCE 600
#include <errno.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <stimg.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>

const char *fb_path = "/dev/fb0";
char temp_file[] = "/tmp/idump.XXXXXX";

enum {
	DEBUG            = false,
	BITS_PER_BYTE    = 8,
	BUFSIZE          = 1024,
	CMAP_COLORS      = 256,
	CMAP_RED_SHIFT   = 5,
	CMAP_GREEN_SHIFT = 2,
	CMAP_BLUE_SHIFT  = 0,
	CMAP_RED_MASK    = 3,
	CMAP_GREEN_MASK  = 3,
	CMAP_BLUE_MASK   = 2
};

const uint32_t bit_mask[] = {
	0x00,
	0x01,       0x03,       0x07,       0x0F,
	0x1F,       0x3F,       0x7F,       0xFF,
	0x1FF,      0x3FF,      0x7FF,      0xFFF,
	0x1FFF,     0x3FFF,     0x7FFF,     0xFFFF,
	0x1FFFF,    0x3FFFF,    0x7FFFF,    0xFFFFF,
	0x1FFFFF,   0x3FFFFF,   0x7FFFFF,   0xFFFFFF,
	0x1FFFFFF,  0x3FFFFFF,  0x7FFFFFF,  0xFFFFFFF,
	0x1FFFFFFF, 0x3FFFFFFF, 0x7FFFFFFF, 0xFFFFFFFF,
};

struct framebuffer {
	char *fp;                       /* pointer of framebuffer (read only) */
	char *buf;                      /* copy of framebuffer */
	int fd;                         /* file descriptor of framebuffer */
	int width, height;              /* display resolution */
	long screen_size;               /* screen data size (byte) */
	int line_length;                /* line length (byte) */
	int bpp;                        /* BYTES per pixel */
	struct fb_cmap *cmap,           /* cmap for legacy framebuffer (8bpp pseudocolor) */
		*cmap_org;                  /* copy of default cmap */
	struct fb_var_screeninfo vinfo; /* display info: need for get_color() */
		
};

struct image {
	STIMG *data;
	int width;
	int height;
	int alpha;
};

/* error functions */
void error(char *str)
{
	perror(str);
	exit(EXIT_FAILURE);
}

void fatal(char *str)
{
	fprintf(stderr, "%s\n", str);
	exit(EXIT_FAILURE);
}

/* wrapper of C functions */
int eopen(const char *path, int flag)
{
	int fd;
	errno = 0;

	if ((fd = open(path, flag)) < 0) {
		fprintf(stderr, "cannot open \"%s\"\n", path);
		error("open");
	}

	return fd;
}

void eclose(int fd)
{
	errno = 0;

	if (close(fd) < 0)
		error("close");
}

void *emmap(int addr, size_t len, int prot, int flag, int fd, off_t offset)
{
	uint32_t *fp;
	errno = 0;

	if ((fp = (uint32_t *) mmap(0, len, prot, flag, fd, offset)) == MAP_FAILED)
		error("mmap");

	return fp;
}

void emunmap(void *ptr, size_t len)
{
	errno = 0;

	if (munmap(ptr, len) < 0)
		error("munmap");
}

void *ecalloc(size_t size)
{
	void *p;
	errno = 0;

	if ((p = calloc(1, size)) == NULL)
		error("calloc");

	return p;
}

/* some functions for Linux framebuffer */
int my_ceil(int val, int div)
{
	return (val + div - 1) / div;
}

uint32_t bit_reverse(uint32_t val, int bits)
{
    uint32_t ret = val;
    int shift = bits - 1;

    for (val >>= 1; val; val >>= 1) {
        ret <<= 1;
        ret |= val & 1;
        shift--;
    }

    return ret <<= shift;
}

void cmap_create(struct fb_cmap **cmap)
{
	*cmap           = (struct fb_cmap *) ecalloc(sizeof(struct fb_cmap));
	(*cmap)->start  = 0;
	(*cmap)->len    = CMAP_COLORS;
	(*cmap)->red    = (uint16_t *) ecalloc(sizeof(uint16_t) * CMAP_COLORS);
	(*cmap)->green  = (uint16_t *) ecalloc(sizeof(uint16_t) * CMAP_COLORS);
	(*cmap)->blue   = (uint16_t *) ecalloc(sizeof(uint16_t) * CMAP_COLORS);
	(*cmap)->transp = NULL;
}

void cmap_die(struct fb_cmap *cmap)
{
	if (cmap) {
		free(cmap->red);
		free(cmap->green);
		free(cmap->blue);
		free(cmap->transp);
		free(cmap);
	}
}

void cmap_init(struct framebuffer *fb, struct fb_var_screeninfo *vinfo)
{
	int i;
	uint8_t index;
	uint16_t r, g, b;

	if (ioctl(fb->fd, FBIOGETCMAP, fb->cmap_org) < 0) {
		/* not fatal, but we cannot restore original cmap */
		fprintf(stderr, "couldn't get original cmap\n");
		cmap_die(fb->cmap_org);
		fb->cmap_org = NULL; 
	}

	for (i = 0; i < CMAP_COLORS; i++) {
		/*
		in 8bpp mode, usually red and green have 3bit, blue has 1bit.
		so we will init cmap table like this.

		  index
		0000 0000 -> red [000] green [0 00] blue [00]
		...
		0110 1011 -> red [011] green [0 10] blue [11]
		...
		1111 1111 -> red [111] green [1 11] blue [11]

		these values are defined above:
		CMAP_RED_SHIFT   = 5
		CMAP_GREEN_SHIFT = 2
		CMAP_BLUE_SHIFT  = 0
		CMAP_RED_MASK    = 3
		CMAP_GREEN_MASK  = 3
		CMAP_BLUE_MASK   = 2
		*/
		index = (uint8_t) i;

		r = (index >> CMAP_RED_SHIFT)   & bit_mask[CMAP_RED_MASK];
		g = (index >> CMAP_GREEN_SHIFT) & bit_mask[CMAP_GREEN_MASK];
		b = (index >> CMAP_BLUE_SHIFT)  & bit_mask[CMAP_BLUE_MASK];

		/* cmap->{red,green,blue} has 16bit, so we should bulge */
		r = (double) r * bit_mask[16] / bit_mask[CMAP_RED_MASK];
		g = (double) g * bit_mask[16] / bit_mask[CMAP_GREEN_MASK];
		b = (double) b * bit_mask[16] / bit_mask[CMAP_BLUE_MASK];

		if (DEBUG)
			fprintf(stderr, "index:%.2X r:%.4X g:%.4X b:%.4X\n", index, r, g, b);

		*(fb->cmap->red + i) = (vinfo->red.msb_right) ?
			bit_reverse(r, 16) & bit_mask[16]: r;
		*(fb->cmap->green + i) = (vinfo->green.msb_right) ?
			bit_reverse(g, 16) & bit_mask[16]: g;
		*(fb->cmap->blue + i) = (vinfo->blue.msb_right) ?
			bit_reverse(b, 16) & bit_mask[16]: b;
	}

	if (ioctl(fb->fd, FBIOPUTCMAP, fb->cmap) < 0)
		fatal("ioctl: FBIOGET_VSCREENINFO failed");
}

void draw_cmap_table(struct framebuffer *fb)
{
	/* for debug */
	uint32_t c;
	int h, w, h_offset = 0, w_offset;
	enum {
		CELL_WIDTH = 8,
		CELL_HEIGHT = 16
	};

	for (c = 0; c < CMAP_COLORS; c++) {
		h_offset = (int) (c / 32) * CELL_HEIGHT;
		w_offset = (c % 32);

		for (h = 0; h < CELL_HEIGHT; h++) {
			for (w = 0; w < CELL_WIDTH; w++) {
				memcpy(fb->fp + (CELL_WIDTH * w_offset + w) * fb->bpp
					+ (h + h_offset) * fb->line_length, &c, fb->bpp);
			}
		}
	}
}

uint32_t get_color(struct fb_var_screeninfo *vinfo, uint8_t r, uint8_t g, uint8_t b)
{
	if (vinfo->bits_per_pixel == 8) {
		/*
		calculate cmap color index:
		we only use following bits
			[011]0 0100: red
			[110]0 0110: green
			[01]01 1011: red
		*/
		r = (r >> (BITS_PER_BYTE - CMAP_RED_MASK))   & bit_mask[CMAP_RED_MASK];
		g = (g >> (BITS_PER_BYTE - CMAP_GREEN_MASK)) & bit_mask[CMAP_GREEN_MASK];
		b = (b >> (BITS_PER_BYTE - CMAP_BLUE_MASK))  & bit_mask[CMAP_BLUE_MASK];

		return (uint8_t) (r << CMAP_RED_SHIFT) | (g << CMAP_GREEN_SHIFT) | (b << CMAP_BLUE_SHIFT);
	}

	r = r >> (BITS_PER_BYTE - vinfo->red.length);
	g = g >> (BITS_PER_BYTE - vinfo->green.length);
	b = b >> (BITS_PER_BYTE - vinfo->blue.length);

	if (vinfo->red.msb_right)
		r = bit_reverse(r, vinfo->red.length)
			& bit_mask[vinfo->red.length];
	if (vinfo->green.msb_right)
		g = bit_reverse(g, vinfo->green.length)
			& bit_mask[vinfo->green.length];
	if (vinfo->blue.msb_right)
		b = bit_reverse(b, vinfo->blue.length)
			& bit_mask[vinfo->blue.length];

	return (r << vinfo->red.offset)
		+ (g << vinfo->green.offset)
		+ (b << vinfo->blue.offset);
}

void fb_init(struct framebuffer *fb)
{
	char *path;
	struct fb_fix_screeninfo finfo;
	struct fb_var_screeninfo vinfo;

	if ((path = getenv("FRAMEBUFFER")) != NULL)
		fb->fd = eopen(path, O_RDWR);
	else
		fb->fd = eopen(fb_path, O_RDWR);

	if (ioctl(fb->fd, FBIOGET_FSCREENINFO, &finfo) < 0)
		fatal("ioctl: FBIOGET_FSCREENINFO failed");

	if (ioctl(fb->fd, FBIOGET_VSCREENINFO, &vinfo) < 0)
		fatal("ioctl: FBIOGET_VSCREENINFO failed");

	/* check screen offset and initialize because linux console change this */
	/*
	if (vinfo.xoffset != 0 || vinfo.yoffset != 0) {
		vinfo.xoffset = vinfo.yoffset = 0;
		ioctl(fb->fd, FBIOPUT_VSCREENINFO, &vinfo);
	}
	*/

	fb->width = vinfo.xres;
	fb->height = vinfo.yres;
	fb->screen_size = finfo.smem_len;
	fb->line_length = finfo.line_length;

	if ((finfo.visual == FB_VISUAL_TRUECOLOR || finfo.visual == FB_VISUAL_DIRECTCOLOR)
		&& (vinfo.bits_per_pixel == 15 || vinfo.bits_per_pixel == 16
		|| vinfo.bits_per_pixel == 24 || vinfo.bits_per_pixel == 32)) {
		fb->cmap = fb->cmap_org = NULL;
		fb->bpp = my_ceil(vinfo.bits_per_pixel, BITS_PER_BYTE);
	}
	else if (finfo.visual == FB_VISUAL_PSEUDOCOLOR
		&& vinfo.bits_per_pixel == 8) {
		cmap_create(&fb->cmap);
		cmap_create(&fb->cmap_org);
		cmap_init(fb, &vinfo);
		fb->bpp = 1;
	}
	else /* non packed pixel, mono color, grayscale: not implimented */
		fatal("unsupported framebuffer type");

	fb->fp = (char *) emmap(0, fb->screen_size, PROT_WRITE | PROT_READ, MAP_SHARED, fb->fd, 0);
	fb->buf = (char *) ecalloc(fb->screen_size);
	fb->vinfo = vinfo;
}

void fb_die(struct framebuffer *fb)
{
	cmap_die(fb->cmap);
	if (fb->cmap_org) {
		//ioctl(fb->fd, FBIOPUTCMAP, fb->cmap_org); /* not fatal */
		cmap_die(fb->cmap_org);
	}
	free(fb->buf);
	emunmap(fb->fp, fb->screen_size);
	eclose(fb->fd);
}

void remove_temp_file()
{
	extern char temp_file[]; /* global */
	remove(temp_file);
}

char *make_temp_file(char *template)
{
	int fd;
	ssize_t size, file_size = 0;
	char buf[BUFSIZE];
	errno = 0;

	if ((fd = mkstemp(template)) < 0) {
		perror("mkstemp");
		return NULL;
	}
	fprintf(stderr, "tmp file:%s\n", template);

	/* register cleanup function */
	if (atexit(remove_temp_file) != 0)
		fatal("atexit() failed\nmaybe temporary file remains...\n");

	if (fcntl(STDIN_FILENO, F_SETFL, O_NONBLOCK) == -1)
		fprintf(stderr, "couldn't set O_NONBLOCK flag\n");

	fprintf(stderr, "dump all stdin data to %s...", template);
	
	while ((size = read(STDIN_FILENO, buf, BUFSIZE)) > 0) {
		write(fd, buf, size);
		file_size += size;
	}

	fprintf(stderr, "done\n");
	eclose(fd);

	if (file_size == 0) {
		fprintf(stderr, "stdin is empty\n");
		fprintf(stderr, "usage: idump IMAGE or cat IMAGE | idump\n");
		exit(EXIT_SUCCESS);
	}

	return template;
}

void load_image(char *file, struct image *img)
{
	if ((img->data = stimg_load(file)) == NULL) {
		fprintf(stderr, "image load error: %s\n", file);
		exit(EXIT_FAILURE);
	}

	/* we MUST init this pointer manually.
		libstimg should init in stimg_load()... */
	img->data->next = NULL;
	img->width      = stimg_get_width(img->data);
	img->height     = stimg_get_height(img->data);
	img->alpha      = stimg_get_has_alpha(img->data);

	fprintf(stderr, "image width:%d height:%d alpha:%d\n",
		img->width, img->height, img->alpha);
}

void resize_image(int disp_width, int disp_height, struct image *img)
{
	double width_rate, height_rate, resize_rate;
	STIMG *tmp_img;

	width_rate = (disp_width < img->width) ? (double) disp_width / img->width: 1.0;
	height_rate = (disp_height < img->height) ? (double) disp_height / img->height: 1.0;
	/* we choose smaller rate to fit image to display */
	resize_rate = (width_rate < height_rate) ? width_rate: height_rate;

	if (DEBUG)
		fprintf(stderr, "width_rate:%.2lf height_rate:%.2lf\n",
			width_rate, height_rate);

	if (resize_rate < 1.0) {
		img->width  = resize_rate * img->width;
		img->height = resize_rate * img->height;
		fprintf(stderr, "resize_rate:%.2lf width:%d height:%d\n",
			resize_rate, img->width, img->height);

		tmp_img = stimg_resize(img->data, img->width, img->height, img->alpha);
		stimg_delete(img->data);
		img->data       = tmp_img;
		img->data->next = NULL;
	}
}

void draw_image(struct framebuffer *fb, int x_offset, int y_offset,
	int width, int height, int alpha, STIMG *stimg)
{
	/* TODO: check [xy]_offset (alyway zero now) */
	int w, h, offset, size;
	uint8_t r, g, b;
	uint32_t color;
	unsigned char *pixels;

	pixels = stimg_get_data(stimg);

	for (h = 0; h < height; h++) {
		for (w = 0; w < width; w++) {
			r = *pixels++;
			g = *pixels++;
			b = *pixels++;

			color = get_color(&fb->vinfo, r, g, b);

			/* update copy buffer */
			offset = (w + x_offset) * fb->bpp + (h + y_offset) * fb->line_length;
			memcpy(fb->buf + offset, &color, fb->bpp);

			if (alpha > 0)
				pixels++;
		}
		/* draw each scanline */
		if (width != fb->width) {
			offset = x_offset * fb->bpp + (h + y_offset) * fb->line_length;
			size = width * fb->bpp;
			memcpy(fb->fp + offset, fb->buf + offset, size);
		}
	}
	/* we can draw all image data at once! */
	if (width == fb->width) {
		offset = x_offset * fb->bpp + y_offset * fb->line_length;
		size = height * fb->line_length; 
		memcpy(fb->fp + offset, fb->buf + offset, size);
	}
}

int main(int argc, char **argv)
{
	extern char temp_file[]; /* global */
	char *file;
	struct framebuffer fb;
	struct image img;

	fb_init(&fb);

	if (argc < 2)
		file = make_temp_file(temp_file);
	else
		file = argv[1];

	load_image(file, &img);
	resize_image(fb.width, fb.height, &img);
	draw_image(&fb, 0, 0, img.width, img.height, img.alpha, img.data);

	stimg_delete(img.data);
	fb_die(&fb);

	return EXIT_SUCCESS;
}