Framebuffer VSync problem

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <pthread.h>
#include <unistd.h>
#include <termios.h>
#include <linux/fb.h>
#include <sys/mman.h>
#include <linux/input.h>
#include <linux/keyboard.h>
#include <linux/kd.h>

int arg = 0;
int mfd = 0;
int kfd = 0;
size_t rb = 0;
size_t rk = 0;
struct input_event ev[64];
struct input_event ev2[64];
unsigned char data;
int mouse_read = 0;
unsigned char read_buffer[sizeof(struct input_event)*3];

int is_keyboard(int fd)
{
  int data;
  if(ioctl(fd, KDGKBTYPE, &data) != 0)
    return 0;

  return 1;
}

void* handle_input_mouse(void *ptr)
{
  for(;;)
    rb = read(mfd, ev, sizeof(ev));

  return NULL;
}

void* handle_input_keyboard(void *ptr)
{
  while(1)
    {
      rk = read(kfd, &data, sizeof(data));
    }
  return NULL;
}

int main()
{
  int fbfd = 0;
  int ttyfd = 0;
  int old_mode = -1;

  char *files_to_try[] = {"/dev/tty", "/dev/console", NULL};

  struct fb_var_screeninfo vinfo;
  struct fb_fix_screeninfo finfo;

  struct termios config;
  struct termios oldconfig;

  int squarex = 0, squarey = 0;

  long unsigned int screensize = 0;
  unsigned char *fbp = NULL;
  unsigned char *fbpbackup = NULL;
  unsigned char *fbpdb = NULL;
  int x = 0, y = 0;

  long int location = 0;
  pthread_t input_mouse, input_keyboard;

  if((fbfd = open("/dev/fb0", O_RDWR)) == -1)
    {
      puts("Error: cannot open framebuffer device.");
      exit(1);
    }
  puts("The framebuffer device was opened successfully.");

  if(ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo) == -1)
    {
      puts("Error reading fixed information.");
      exit(2);
    }

  if(ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo) == -1)
    {
      puts("Error reading variable information.");
      exit(3);
    }

  if((mfd = open("/dev/input/event3", O_RDONLY, 0)) < 0)
    {
      puts("Erorr opening mouse.");
      goto error;
    }

  int i;
  for(i = 0; files_to_try[i] != NULL; ++i)
    {
      kfd = open(files_to_try[i], O_RDONLY);
      if(kfd < 0) continue;

      if(is_keyboard(kfd)) break;

      close(kfd);
    }

  if(files_to_try[i] == NULL) return 1;

  if(ioctl(kfd, KDGKBMODE, &old_mode) != 0)
    {
      printf("Unable to query keyboard mode.\n");
      goto error;
    }

  if(ioctl(kfd, KDSKBMODE, K_MEDIUMRAW) != 0)
    {
      printf("Unable to set mediumraw mode.\n");
      goto error;
    }


  char *ret = ttyname(STDIN_FILENO);
  ttyfd = open(ret, O_RDWR | O_NOCTTY | O_NDELAY);

  if(ttyfd < 0)
    {
      puts("Failed to open tty");
      exit(6);
    }

  tcgetattr(ttyfd, &oldconfig);

  config = oldconfig;
  config.c_iflag = 0;
  config.c_lflag &= ~(ECHO | ICANON | ISIG);

  tcsetattr(ttyfd, TCSAFLUSH, &config);

  screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;

  squarex = vinfo.xres/2 - 50;
  squarey = vinfo.yres/2 - 50;

  fbp = (char*)mmap(NULL, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fbfd, 0);

  fbpbackup = (char*)malloc(screensize);
  fbpdb = (char*)malloc(screensize);
  memcpy(fbpbackup, fbp, screensize);

  if(fbp == MAP_FAILED)
    {

      exit(4);
    }

  printf("\e[?25l");
  fflush(stdout);
  int quit = 0;
  int draw = 0;



  memset(fbpdb, 0x0, screensize);
  for(x = squarex; x < squarex + 100; x++)
    for(y = squarey; y < squarey + 100; y++)
      {
        location = (x + vinfo.xoffset) * (vinfo.bits_per_pixel/8) + (y + vinfo.yoffset) * finfo.line_length;
        *(fbpdb + location + 0) = 0x0;
        *(fbpdb + location + 1) = 0x0;
        *(fbpdb + location + 2) = 0xff;
      }

  memcpy(fbp, fbpdb, screensize);

  pthread_create(&input_mouse, NULL, handle_input_mouse, NULL);
  pthread_create(&input_keyboard, NULL, handle_input_keyboard, NULL);
  ioctl(fbfd, _IOW('F', 309, int), 1);


  while(!quit)
    {
      int result;

      draw = 0;
      if(rb)
        {
          for(i = 0; i < rb/sizeof(struct input_event); ++i)
            {
              struct input_event *currev = &ev[i];
              if(EV_REL == currev -> type)
                {
                  if(currev -> code == REL_X)
                    squarex += currev -> value;
                  if(currev -> code == REL_Y)
                    squarey += currev -> value;

                  if(squarey < 0) squarey = 0;
                  if(squarey + 100 > vinfo.yres) squarey = vinfo.yres - 100;

                  if(squarex < 0) squarex = 0;
                  if(squarex + 100 > vinfo.xres) squarex = vinfo.xres - 100;

                  draw = 1;
                }
            }
        }

      rb = 0;

      if(rk)
        {
          if((data & 0x7F) == 1)
            {
              quit = 1;
              draw = 0;
              memset(fbp, 0x0, screensize);
            }
        }

      rk = 0;

      if(draw)
        {
          memset(fbpdb, 0x0, screensize);
          for(x = squarex; x < squarex + 100; x++)
            for(y = squarey; y < squarey + 100; y++)
              {
                location = (x + vinfo.xoffset) * (vinfo.bits_per_pixel/8) + (y + vinfo.yoffset) * finfo.line_length;
                *(fbpdb + location + 0) = 0x0;
                *(fbpdb + location + 1) = 0x0;
                *(fbpdb + location + 2) = 0xff;
              }
          draw = 0;


          ioctl(fbfd, FBIO_WAITFORVSYNC, &arg);
          memcpy(fbp, fbpdb, screensize);
        }
    }

  printf("\e[?25h");
  fflush(stdout);

  memcpy(fbp, fbpbackup, screensize);
  free(fbpbackup);
  free(fbpdb);

  munmap(fbp, screensize);
  close(fbfd);

  ioctl(kfd, KDSKBMODE, old_mode);
  tcsetattr(ttyfd, TCSAFLUSH, &oldconfig);

  return 0;


 error:

  if(old_mode != -1)
    {
      ioctl(kfd, KDSKBMODE, old_mode);
      tcsetattr(ttyfd, TCSAFLUSH, &oldconfig);
    }

  return 1;

}