#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <sys/mman.h>

int main(int argc, char **argv)
{
  int fbfd = 0;
  struct fb_var_screeninfo vinfo;
  struct fb_fix_screeninfo finfo;
  struct fb_cmap cmap;
  FILE *cfp;
  unsigned char *fbp = 0;
  unsigned char *dfbp = 0;
  unsigned char *sfbp = 0;
  long int screensize = 0;
  int x = 0, y = 0, z=0, xpos = 0, ypos = 0;
  long int location = 0;
  char c;

  /* Open the file for reading and writing */
  fbfd = open("/dev/fb0", O_RDWR);
  if (!fbfd)
  {
    perror("open /dev/fb0");
    exit(1);
  }

  /* Get fixed screen information */
  if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo))
  {
    perror("finfo");
    exit(2);
  }

  /* Get variable screen information */
  if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo))
  {
    perror("vinfo");
    exit(3);
  }

  /* Get the colourmap */
  cmap.start = 0;
  cmap.len = 256;
  cmap.red = (unsigned short int *)malloc(256*sizeof(unsigned short int));
  cmap.green = (unsigned short int *)malloc(256*sizeof(unsigned short int));
  cmap.blue = (unsigned short int *)malloc(256*sizeof(unsigned short int));
  cmap.transp = (unsigned short int *)malloc(256*sizeof(unsigned short int));
  if (ioctl(fbfd, FBIOGETCMAP, &cmap))
  {
    perror("cmap");
    exit(4);
  }
  cfp = fopen("/tmp/cmap.txt", "w");
  if (cfp != NULL)
  {
    fprintf(cfp, "#    Red  Grn Blue Trsp\n");
    for (z=0; z<256; z++)
      fprintf(cfp, "%3d %4x %4x %4x %4x\n", z, cmap.red[z],
	      cmap.green[z], cmap.blue[z], cmap.transp[z]);
    fflush(cfp);
    fclose(cfp);
    printf("Saved colourmap values in /tmp/cmap.txt\n");
  }

  /* Figure out the size of the screen in bytes */
  screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;

  /* Map the device to memory */
  fbp = (unsigned char *)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED,
			      fbfd, 0);       
  if ((int)fbp == -1)
  {
    perror("mmap");
    exit(5);
  }
  /* display copy for drawing */
  dfbp = (unsigned char *)malloc(screensize*sizeof(unsigned char));
  if (dfbp <= 0)
  {
    perror("malloc dfbp");
    munmap(fbp, screensize);
    close(fbfd);
    exit(6);
  }
  /* saved screen for restoring */
  sfbp = (unsigned char *)malloc(screensize*sizeof(unsigned char));
  if (sfbp <= 0)
  {
    perror("malloc sfbp");
    free(dfbp);
    munmap(fbp, screensize);
    close(fbfd);
    exit(7);
  }
  /* save current screen contents */
  sleep(2);
  memcpy(sfbp, fbp, screensize);

  for (ypos=0; ypos<16; ypos++)
  {
    for (xpos=0; xpos<16; xpos++)
    {
      x = xpos * vinfo.xres / 16;
      y = ypos * vinfo.yres / 16;
      location = (x+vinfo.xoffset) * (vinfo.bits_per_pixel/8) +
                 (y+vinfo.yoffset) * finfo.line_length;

      for (z=0; z<(vinfo.yres/16); z++)
	memset(dfbp+location+(z*finfo.line_length),
	       (16*ypos)+xpos, vinfo.xres/16);
    }
  }
  memcpy(fbp, dfbp, screensize);
  read(0, &c, 1);
  /* restore original screen contents */
  memcpy(fbp, sfbp, screensize);

  free(dfbp);
  dfbp = NULL;
  munmap(fbp, screensize);
  close(fbfd);
  return 0;
}