#include <errno.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <unistd.h>
FILE *out;
void *trymmap (void *start, size_t length, int prot, int flags, int fd, off_t offset)
{
    char *p;
    int aa;
    fprintf (out, "mmap(%X, %X, %X, %X, %X, %X) ... ", (unsigned int)start, length, prot, flags, fd, (unsigned int)offset);
    p = mmap (start, length, prot, flags, fd, offset);
    if (p == (char *)0xFFFFFFFF)
    {
        aa = errno;
        fprintf (out, "failed. errno = %d\n", aa);
    }
    else
    {
        fprintf (out, "OK! (%X)\n", (unsigned int)p);
    }
    return p;
}
int main(int argc, char *argv[]) {
    //open fb0 and mem
    out = stdout;
    int fb0 = open("/dev/fb0", O_RDWR);
    int mem = open("/dev/mem", O_RDWR);
    
    //find physical address of fb
    struct fb_fix_screeninfo fixed_info;
    ioctl(fb0, FBIOGET_FSCREENINFO, &fixed_info); 
    //mmap fb to screen
    unsigned short *screen = trymmap(0, 320*240*2, PROT_WRITE, MAP_SHARED, mem, fixed_info.smem_start);
    //mmap regs
    unsigned long *reg1l = trymmap(0, 0x10000,   PROT_READ|PROT_WRITE, MAP_SHARED, mem, 0xc0000000);
    unsigned short *reg1s = (unsigned short *)reg1l;
    unsigned long *reg2l = trymmap(0, 0x10000,   PROT_READ|PROT_WRITE, MAP_SHARED, mem, 0xE0020000);
    unsigned short *reg2s = (unsigned short *)reg2l;
    reg1s[0x28DA>>1]=0x04AB; // 16bpp
    reg1s[0x290C>>1]=320*2;  // line width in bytes
    // draw something in (0,0)-(31, 31)
    int i, j; 
    for (i=0; i<32; i++) {
        for (j=0; j<32; j++) {
            screen[i*320 + j] = i<<11 | j<<6 | (i+j)>>1;
        }
    }
    sleep(3);
    reg1l[0x090a>>2] = 0xFFFFFFFF;
 
    // copy (0,0)-(31, 31) in (32,32)-(63,63)
    // Dest control
    reg2l[0x0000 >> 2] = 0 << 6 |
                        1 << 5 |
                        0 << 0;
    // Dest base
    reg2l[0x0004 >> 2] = fixed_info.smem_start + (32*2)*320 + (32*2);
    // Dest stride
    reg2l[0x0008 >> 2] = 320 * 2;
    // Source control
    reg2l[0x000C >> 2] = 1 << 8 |
                        1 << 7 |
                        1 << 5 |
                        0 << 0;
                        
    // Source base
    reg2l[0x0010 >> 2] = fixed_info.smem_start;
    // Source stride
    reg2l[0x0014 >> 2] = 320 * 2;
    
    // Source FG
    reg2l[0x0018 >> 2] = 0x0000FFFF;
    // Source BG
    reg2l[0x001C >> 2] = 0x00000000;
    // Pattern control
    reg2l[0x0020 >> 2] = 0x01 >> 6 |
                        0x01 >> 5 |
                        0x02 >> 3 |
                        0x00 >> 0;
    // Pattern reg
    reg2l[0x0080 >> 2] = 0xFFFFFFFF;
    // Pattern FG
    reg2l[0x0024 >> 2] = 0x0000FFFF;
     
    // Pattern BG
    reg2l[0x0028 >> 2] = 0x0000FFFF;
    // Size 
    reg2l[0x002C >> 2] = 32 << 16 | 32 << 0;
    // Control reg
    reg2l[0x0030 >> 2] = 0x00 << 16 |
                        0x00 << 11 |
                        0x01 << 10 |
                        0x01 << 9 |
                        0x01 << 8 |
                        0xFF << 0;
    sleep(3);
    munmap(reg1l, 0x10000);
    munmap(reg2l, 0x10000);
    munmap(screen, 320*240*2);
    close(mem);
    close(fb0);
    chdir("/usr/gp2x");                                          //go to menu
    execl("gp2xmenu","gp2xmenu",NULL);
}