Initial revision

1 files changed, 523 insertions, 0 deletions

diff --git a/fbtools.c b/fbtools.c
new file mode 100644
index 0000000..a03f4d3
--- /dev/null
+++ b/fbtools.c
@@ -0,0 +1,523 @@
+/*
+ * some generic framebuffer device stuff
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <signal.h>
+#include <errno.h>
+#include <setjmp.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/wait.h>
+#include <sys/stat.h>
+
+#include <linux/kd.h>
+#include <linux/vt.h>
+#include <linux/fb.h>
+
+#include <asm/page.h>
+
+#include "fbtools.h"
+
+/* -------------------------------------------------------------------- */
+/* exported stuff                                                       */
+
+struct fb_fix_screeninfo   fb_fix;
+struct fb_var_screeninfo   fb_var;
+unsigned char             *fb_mem;
+int			   fb_mem_offset = 0;
+int                        fb_switch_state = FB_ACTIVE;
+
+/* -------------------------------------------------------------------- */
+/* internal variables                                                   */
+
+static int                       fb,tty;
+#if 0
+static int                       bpp,black,white;
+#endif
+
+static int                       orig_vt_no = 0;
+static struct vt_mode            vt_mode;
+
+static int                       kd_mode;
+static struct vt_mode            vt_omode;
+static struct termios            term;
+static struct fb_var_screeninfo  fb_ovar;
+static unsigned short            ored[256], ogreen[256], oblue[256];
+static struct fb_cmap            ocmap = { 0, 256, ored, ogreen, oblue };
+
+/* -------------------------------------------------------------------- */
+/* devices                                                              */
+
+struct DEVS {
+    char *fb0;
+    char *fbnr;
+    char *ttynr;
+};
+
+struct DEVS devs_default = {
+    fb0:   "/dev/fb0",
+    fbnr:  "/dev/fb%d",
+    ttynr: "/dev/tty%d",
+};
+struct DEVS devs_devfs = {
+    fb0:   "/dev/fb/0",
+    fbnr:  "/dev/fb/%d",
+    ttynr: "/dev/vc/%d",
+};
+struct DEVS *devices;
+
+static void dev_init(void)
+{
+    struct stat dummy;
+
+    if (NULL != devices)
+	return;
+    if (0 == stat("/dev/.devfsd",&dummy))
+	devices = &devs_devfs;
+    else
+	devices = &devs_default;
+}
+
+/* -------------------------------------------------------------------- */
+/* console switching                                                    */
+
+extern int debug;
+
+static void
+fb_switch_signal(int signal)
+{
+    if (signal == SIGUSR1) {
+	/* release */
+	fb_switch_state = FB_REL_REQ;
+	if (debug)
+	    write(2,"vt: SIGUSR1\n",12);
+    }
+    if (signal == SIGUSR2) {
+	/* acquisition */
+	fb_switch_state = FB_ACQ_REQ;
+	if (debug)
+	    write(2,"vt: SIGUSR2\n",12);
+    }
+}
+
+void
+fb_switch_release()
+{
+    ioctl(tty, VT_RELDISP, 1);
+    fb_switch_state = FB_INACTIVE;
+    if (debug)
+	write(2,"vt: release\n",12);
+}
+
+void
+fb_switch_acquire()
+{
+    ioctl(tty, VT_RELDISP, VT_ACKACQ);
+    fb_switch_state = FB_ACTIVE;
+    if (debug)
+	write(2,"vt: acquire\n",12);
+}
+
+int
+fb_switch_init()
+{
+    struct sigaction act,old;
+
+    memset(&act,0,sizeof(act));
+    act.sa_handler  = fb_switch_signal;
+    sigemptyset(&act.sa_mask);
+    sigaction(SIGUSR1,&act,&old);
+    sigaction(SIGUSR2,&act,&old);
+    
+    if (-1 == ioctl(tty,VT_GETMODE, &vt_mode)) {
+	perror("ioctl VT_GETMODE");
+	exit(1);
+    }
+    vt_mode.mode   = VT_PROCESS;
+    vt_mode.waitv  = 0;
+    vt_mode.relsig = SIGUSR1;
+    vt_mode.acqsig = SIGUSR2;
+    
+    if (-1 == ioctl(tty,VT_SETMODE, &vt_mode)) {
+	perror("ioctl VT_SETMODE");
+	exit(1);
+    }
+    return 0;
+}
+
+/* -------------------------------------------------------------------- */
+/* initialisation & cleanup                                             */
+
+void
+fb_memset (void *addr, int c, size_t len)
+{
+#if 1 /* defined(__powerpc__) */
+    unsigned int i, *p;
+    
+    i = (c & 0xff) << 8;
+    i |= i << 16;
+    len >>= 2;
+    for (p = addr; len--; p++)
+	*p = i;
+#else
+    memset(addr, c, len);
+#endif
+}
+
+static int
+fb_setmode(char *name)
+{
+    FILE *fp;
+    char line[80],label[32],value[16];
+    int  geometry=0, timings=0;
+    
+    /* load current values */
+    if (-1 == ioctl(fb,FBIOGET_VSCREENINFO,&fb_var)) {
+	perror("ioctl FBIOGET_VSCREENINFO");
+	exit(1);
+    }
+    
+    if (NULL == name)
+	return -1;
+    if (NULL == (fp = fopen("/etc/fb.modes","r")))
+	return -1;
+    while (NULL != fgets(line,79,fp)) {
+	if (1 == sscanf(line, "mode \"%31[^\"]\"",label) &&
+	    0 == strcmp(label,name)) {
+	    /* fill in new values */
+	    fb_var.sync  = 0;
+	    fb_var.vmode = 0;
+	    while (NULL != fgets(line,79,fp) &&
+		   NULL == strstr(line,"endmode")) {
+		if (5 == sscanf(line," geometry %d %d %d %d %d",
+				&fb_var.xres,&fb_var.yres,
+				&fb_var.xres_virtual,&fb_var.yres_virtual,
+				&fb_var.bits_per_pixel))
+		    geometry = 1;
+		if (7 == sscanf(line," timings %d %d %d %d %d %d %d",
+				&fb_var.pixclock,
+				&fb_var.left_margin,  &fb_var.right_margin,
+				&fb_var.upper_margin, &fb_var.lower_margin,
+				&fb_var.hsync_len,    &fb_var.vsync_len))
+		    timings = 1;
+		if (1 == sscanf(line, " hsync %15s",value) &&
+		    0 == strcasecmp(value,"high"))
+		    fb_var.sync |= FB_SYNC_HOR_HIGH_ACT;
+		if (1 == sscanf(line, " vsync %15s",value) &&
+		    0 == strcasecmp(value,"high"))
+		    fb_var.sync |= FB_SYNC_VERT_HIGH_ACT;
+		if (1 == sscanf(line, " csync %15s",value) &&
+		    0 == strcasecmp(value,"high"))
+		    fb_var.sync |= FB_SYNC_COMP_HIGH_ACT;
+		if (1 == sscanf(line, " extsync %15s",value) &&
+		    0 == strcasecmp(value,"true"))
+		    fb_var.sync |= FB_SYNC_EXT;
+		if (1 == sscanf(line, " laced %15s",value) &&
+		    0 == strcasecmp(value,"true"))
+		    fb_var.vmode |= FB_VMODE_INTERLACED;
+		if (1 == sscanf(line, " double %15s",value) &&
+		    0 == strcasecmp(value,"true"))
+		    fb_var.vmode |= FB_VMODE_DOUBLE;
+	    }
+	    /* ok ? */
+	    if (!geometry || !timings)
+		return -1;
+	    /* set */
+	    fb_var.xoffset = 0;
+	    fb_var.yoffset = 0;
+	    if (-1 == ioctl(fb,FBIOPUT_VSCREENINFO,&fb_var))
+		perror("ioctl FBIOPUT_VSCREENINFO");
+	    /* look what we have now ... */
+	    if (-1 == ioctl(fb,FBIOGET_VSCREENINFO,&fb_var)) {
+		perror("ioctl FBIOGET_VSCREENINFO");
+		exit(1);
+	    }
+	    return 0;
+	}
+    }
+    return -1;
+}
+
+static void
+fb_setvt(int vtno)
+{
+    struct vt_stat vts;
+    char vtname[12];
+    
+    if (vtno < 0) {
+	if (-1 == ioctl(tty,VT_OPENQRY, &vtno) || vtno == -1) {
+	    perror("ioctl VT_OPENQRY");
+	    exit(1);
+	}
+    }
+
+    vtno &= 0xff;
+    sprintf(vtname, devices->ttynr, vtno);
+    chown(vtname, getuid(), getgid());
+    if (-1 == access(vtname, R_OK | W_OK)) {
+	fprintf(stderr,"access %s: %s\n",vtname,strerror(errno));
+	exit(1);
+    }
+    switch (fork()) {
+    case 0:
+	break;
+    case -1:
+	perror("fork");
+	exit(1);
+    default:
+	exit(0);
+    }
+    close(tty);
+    close(0);
+    close(1);
+    close(2);
+    setsid();
+    open(vtname,O_RDWR);
+    dup(0);
+    dup(0);
+
+    if (-1 == ioctl(tty,VT_GETSTATE, &vts)) {
+	perror("ioctl VT_GETSTATE");
+	exit(1);
+    }
+    orig_vt_no = vts.v_active;
+    if (-1 == ioctl(tty,VT_ACTIVATE, vtno)) {
+	perror("ioctl VT_ACTIVATE");
+	exit(1);
+    }
+    if (-1 == ioctl(tty,VT_WAITACTIVE, vtno)) {
+	perror("ioctl VT_WAITACTIVE");
+	exit(1);
+    }
+}
+
+/* Hmm. radeonfb needs this. matroxfb doesn't. */
+static int fb_activate_current(int tty)
+{
+    struct vt_stat vts;
+    
+    if (-1 == ioctl(tty,VT_GETSTATE, &vts)) {
+	perror("ioctl VT_GETSTATE");
+	return -1;
+    }
+    if (-1 == ioctl(tty,VT_ACTIVATE, vts.v_active)) {
+	perror("ioctl VT_ACTIVATE");
+	return -1;
+    }
+    if (-1 == ioctl(tty,VT_WAITACTIVE, vts.v_active)) {
+	perror("ioctl VT_WAITACTIVE");
+	return -1;
+    }
+    return 0;
+}
+
+int
+fb_init(char *device, char *mode, int vt)
+{
+    char   fbdev[16];
+    struct vt_stat vts;
+
+    dev_init();
+    tty = 0;
+    if (vt != 0)
+	fb_setvt(vt);
+
+    if (-1 == ioctl(tty,VT_GETSTATE, &vts)) {
+	fprintf(stderr,"ioctl VT_GETSTATE: %s (not a linux console?)\n",
+		strerror(errno));
+	exit(1);
+    }
+    
+    if (NULL == device) {
+	device = getenv("FRAMEBUFFER");
+	if (NULL == device) {
+	    struct fb_con2fbmap c2m;
+	    if (-1 == (fb = open(devices->fb0,O_RDWR /* O_WRONLY */,0))) {
+		fprintf(stderr,"open %s: %s\n",devices->fb0,strerror(errno));
+		exit(1);
+	    }
+	    c2m.console = vts.v_active;
+	    if (-1 == ioctl(fb, FBIOGET_CON2FBMAP, &c2m)) {
+		perror("ioctl FBIOGET_CON2FBMAP");
+		exit(1);
+	    }
+	    close(fb);
+	    fprintf(stderr,"map: vt%02d => fb%d\n",
+		    c2m.console,c2m.framebuffer);
+	    sprintf(fbdev,devices->fbnr,c2m.framebuffer);
+	    device = fbdev;
+	}
+    }
+
+    /* get current settings (which we have to restore) */
+    if (-1 == (fb = open(device,O_RDWR /* O_WRONLY */))) {
+	fprintf(stderr,"open %s: %s\n",device,strerror(errno));
+	exit(1);
+    }
+    if (-1 == ioctl(fb,FBIOGET_VSCREENINFO,&fb_ovar)) {
+	perror("ioctl FBIOGET_VSCREENINFO");
+	exit(1);
+    }
+    if (-1 == ioctl(fb,FBIOGET_FSCREENINFO,&fb_fix)) {
+	perror("ioctl FBIOGET_FSCREENINFO");
+	exit(1);
+    }
+    if (fb_ovar.bits_per_pixel == 8 ||
+	fb_fix.visual == FB_VISUAL_DIRECTCOLOR) {
+	if (-1 == ioctl(fb,FBIOGETCMAP,&ocmap)) {
+	    perror("ioctl FBIOGETCMAP");
+	    exit(1);
+	}
+    }
+    if (-1 == ioctl(tty,KDGETMODE, &kd_mode)) {
+	perror("ioctl KDGETMODE");
+	exit(1);
+    }
+    if (-1 == ioctl(tty,VT_GETMODE, &vt_omode)) {
+	perror("ioctl VT_GETMODE");
+	exit(1);
+    }
+    tcgetattr(tty, &term);
+    
+    /* switch mode */
+    fb_setmode(mode);
+    
+    /* checks & initialisation */
+    if (-1 == ioctl(fb,FBIOGET_FSCREENINFO,&fb_fix)) {
+	perror("ioctl FBIOGET_FSCREENINFO");
+	exit(1);
+    }
+    if (fb_fix.type != FB_TYPE_PACKED_PIXELS) {
+	fprintf(stderr,"can handle only packed pixel frame buffers\n");
+	goto err;
+    }
+#if 0
+    switch (fb_var.bits_per_pixel) {
+    case 8:
+	white = 255; black = 0; bpp = 1;
+	break;
+    case 15:
+    case 16:
+	if (fb_var.green.length == 6)
+	    white = 0xffff;
+	else
+	    white = 0x7fff;
+	black = 0; bpp = 2;
+	break;
+    case 24:
+	white = 0xffffff; black = 0; bpp = fb_var.bits_per_pixel/8;
+	break;
+    case 32:
+	white = 0xffffff; black = 0; bpp = fb_var.bits_per_pixel/8;
+	fb_setpixels = fb_setpixels4;
+	break;
+    default:
+	fprintf(stderr, "Oops: %i bit/pixel ???\n",
+		fb_var.bits_per_pixel);
+	goto err;
+    }
+#endif
+    fb_mem_offset = (unsigned long)(fb_fix.smem_start) & (~PAGE_MASK);
+    fb_mem = mmap(NULL,fb_fix.smem_len+fb_mem_offset,
+		  PROT_READ|PROT_WRITE,MAP_SHARED,fb,0);
+    if (-1L == (long)fb_mem) {
+	perror("mmap");
+	goto err;
+    }
+    /* move viewport to upper left corner */
+    if (fb_var.xoffset != 0 || fb_var.yoffset != 0) {
+	fb_var.xoffset = 0;
+	fb_var.yoffset = 0;
+	if (-1 == ioctl(fb,FBIOPAN_DISPLAY,&fb_var)) {
+	    perror("ioctl FBIOPAN_DISPLAY");
+	    goto err;
+	}
+    }
+    if (-1 == ioctl(tty,KDSETMODE, KD_GRAPHICS)) {
+	perror("ioctl KDSETMODE");
+	goto err;
+    }
+    fb_activate_current(tty);
+
+    /* cls */
+    fb_memset(fb_mem+fb_mem_offset,0,fb_fix.smem_len);
+    return fb;
+
+ err:
+    fb_cleanup();
+    exit(1);
+}
+
+void
+fb_cleanup(void)
+{
+    /* restore console */
+    if (-1 == ioctl(fb,FBIOPUT_VSCREENINFO,&fb_ovar))
+	perror("ioctl FBIOPUT_VSCREENINFO");
+    if (-1 == ioctl(fb,FBIOGET_FSCREENINFO,&fb_fix))
+	perror("ioctl FBIOGET_FSCREENINFO");
+    if (fb_ovar.bits_per_pixel == 8 ||
+	fb_fix.visual == FB_VISUAL_DIRECTCOLOR) {
+	if (-1 == ioctl(fb,FBIOPUTCMAP,&ocmap))
+	    perror("ioctl FBIOPUTCMAP");
+    }
+    close(fb);
+
+    if (-1 == ioctl(tty,KDSETMODE, kd_mode))
+	perror("ioctl KDSETMODE");
+    if (-1 == ioctl(tty,VT_SETMODE, &vt_omode))
+	perror("ioctl VT_SETMODE");
+    if (orig_vt_no && -1 == ioctl(tty, VT_ACTIVATE, orig_vt_no))
+	perror("ioctl VT_ACTIVATE");
+    if (orig_vt_no && -1 == ioctl(tty, VT_WAITACTIVE, orig_vt_no))
+	perror("ioctl VT_WAITACTIVE");
+    tcsetattr(tty, TCSANOW, &term);
+    close(tty);
+}
+
+/* -------------------------------------------------------------------- */
+/* handle fatal errors                                                  */
+
+static jmp_buf fb_fatal_cleanup;
+
+static void
+fb_catch_exit_signal(int signal)
+{
+    siglongjmp(fb_fatal_cleanup,signal);
+}
+
+void
+fb_catch_exit_signals(void)
+{
+    struct sigaction act,old;
+    int termsig;
+
+    memset(&act,0,sizeof(act));
+    act.sa_handler = fb_catch_exit_signal;
+    sigemptyset(&act.sa_mask);
+    sigaction(SIGINT, &act,&old);
+    sigaction(SIGQUIT,&act,&old);
+    sigaction(SIGTERM,&act,&old);
+
+    sigaction(SIGABRT,&act,&old);
+    sigaction(SIGTSTP,&act,&old);
+
+    sigaction(SIGBUS, &act,&old);
+    sigaction(SIGILL, &act,&old);
+    sigaction(SIGSEGV,&act,&old);
+
+    if (0 == (termsig = sigsetjmp(fb_fatal_cleanup,0)))
+	return;
+
+    /* cleanup */
+    fb_cleanup();
+    fprintf(stderr,"Oops: %s\n",sys_siglist[termsig]);
+    exit(42);
+}