PSP GU 2D Ortho + FPS

#include <pspdisplay.h>
#include <pspctrl.h>
#include <pspkernel.h>
#include <pspdebug.h>
#include <pspgu.h>
#include <pspgum.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define millis() (clock()/1000)

PSP_MODULE_INFO("GU", 0, 1, 1);

#define BUF_WIDTH  512
#define SCR_WIDTH  480
#define SCR_HEIGHT 272
#define GU_RGB(r,g,b) GU_RGBA(r,g,b,255)

#define printf pspDebugScreenPrintf
#define clearScreen() pspDebugScreenSetXY(0,0)

unsigned int __attribute__((aligned(16))) guList[262144];
void *fbp0;
//fps stuff
u32 frame_count;
time_t currentTime, lastTime, diffTime;

typedef struct
{
    unsigned int color;
    short x, y, z;
}FSVertex;

int  initGU(void);
void finishGU(void);
void initFPS();
void SetupProjection(void);
void guBegin(void);
void guEnd(void);
void FPS(float *fps_p);

void drawRectangle(short x, short y, short w, short h, unsigned int color);

int exit_callback(int arg1, int arg2, void *common);
int CallbackThread(SceSize args, void *argp);
int SetupCallbacks(void);

int main(void)
{
    SetupCallbacks();
    initGU();

    float fps = 0.0f;

    while(1)
    {
        guBegin();
        clearScreen();

        printf("FPS: %f   frame: %i diffTime: %lu", fps, frame_count, diffTime);

        drawRectangle(10, 10, 50, 20, GU_RGB(255,0,0));
        drawRectangle(109, 140, 20, 80, GU_RGB(0,0,255));

        guEnd();
        FPS(&fps);
    }

    finishGU();
    return 0;
}


void drawRectangle(short x, short y, short w, short h, unsigned int color)
{
    FSVertex *vertices = (FSVertex *)sceGuGetMemory(4 * sizeof(FSVertex)); //Allocate memory

    vertices[0] = (FSVertex){color, x, y};
    vertices[1] = (FSVertex){color, x, y+h};
    vertices[2] = (FSVertex){color, x+w, y};
    vertices[3] = (FSVertex){color, x+w, y+h};
    sceGuDrawArray(GU_TRIANGLE_STRIP, GU_COLOR_8888|GU_VERTEX_16BIT|GU_TRANSFORM_2D, 4, 0, vertices);

    sceKernelDcacheWritebackRange(vertices, 4 * sizeof(FSVertex)); //Free memory
}

int initGU(void)
{
    sceGuInit();
    sceGuStart(GU_DIRECT, guList);
    sceGuDrawBuffer(GU_PSM_8888, fbp0, BUF_WIDTH);
    sceGuDispBuffer(SCR_WIDTH, SCR_HEIGHT, (void*)0x88000, BUF_WIDTH);
    sceGuDepthBuffer((void*)0x110000, BUF_WIDTH);
    sceGuOffset(2048 - (SCR_WIDTH/2), 2048 - (SCR_HEIGHT/2));
    sceGuViewport(2048, 2048, SCR_WIDTH, SCR_HEIGHT);
    sceGuDepthRange(65535, 0);

    sceGuScissor(0, 0, SCR_WIDTH, SCR_HEIGHT);
    sceGuEnable(GU_SCISSOR_TEST);
    sceGuFrontFace(GU_CW);
    sceGuShadeModel(GU_SMOOTH);
    sceGuDisable(GU_TEXTURE_2D);

    //sceGuBlendFunc(GU_ADD, GU_SRC_ALPHA, GU_ONE_MINUS_SRC_ALPHA, 0, 0);
    sceGuFinish();

    sceGuSync(0, 0);
    sceDisplayWaitVblankStart();
    sceGuDisplay(GU_TRUE);

    SetupProjection();
    initFPS();

    pspDebugScreenInit();
    pspDebugScreenSetOffset((int)fbp0);

    return 1;
}

void finishGU(void)
{
    sceGuTerm();
}

void initFPS()
{
    currentTime = millis();
    lastTime = currentTime;
    frame_count = 0;
}

void FPS(float *fps_p)
{
    frame_count++;
    currentTime = millis();
    diffTime = currentTime - lastTime;
    if(diffTime >= 1000)
    {
        *fps_p = (float)frame_count/(diffTime/1000.0f);
        frame_count = 0;
        lastTime = millis();
    }
}

void SetupProjection(void)
{
    sceGumMatrixMode(GU_PROJECTION);
        sceGumLoadIdentity();
        sceGumOrtho(0, SCR_WIDTH, SCR_HEIGHT, 0, -1, 1); //2D
    sceGumMatrixMode(GU_VIEW);
        sceGumLoadIdentity();
    sceGumMatrixMode(GU_MODEL);
        sceGumLoadIdentity();
}


void guBegin(void)
{
    sceGuStart(GU_DIRECT, guList);
    sceGuClearColor(0);
    sceGuClearDepth(0);
    sceGuClear(GU_COLOR_BUFFER_BIT|GU_DEPTH_BUFFER_BIT);
    SetupProjection();
}

void guEnd(void)
{
    sceGuFinish();
    sceGuSync(0,0);
    sceDisplayWaitVblankStart();
    fbp0 = sceGuSwapBuffers();
}

/*callbacks*/

int exit_callback(int arg1, int arg2, void *common)
{
    sceKernelExitGame();
    return 0;
}

int CallbackThread(SceSize args, void *argp)
{
    int cbid;
    cbid = sceKernelCreateCallback("exit_callback", exit_callback, NULL);
    sceKernelRegisterExitCallback(cbid);
    sceKernelSleepThreadCB();
    return 0;
}

int SetupCallbacks(void)
{
    int thid = 0;
    thid = sceKernelCreateThread("callback_update_thread", CallbackThread, 0x11, 0xFA0, 0, 0);
    if(thid >= 0)
    {
        sceKernelStartThread(thid, 0, 0);
    }

    return thid;
}