Time MMC5 for multiplies

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>

#include "neslib.h"

#define NTADR(x,y)  ((0x2000|((y)<<5)|x))

#define MSB(x)      (((x)>>8))
#define LSB(x)      (((x)&0xff))

#define writereg8(reg_, val_) ( *(uint8_t*)(reg_) = val_ )
#define readreg8(reg_) ( *(uint8_t*)(reg_) )

#define writereg16(reg_, val_) ( *(uint16_t*)(reg_) = val_ )
#define readreg16(reg_) ( *(uint16_t*)(reg_) )

//////////////////////////////////////////////////////////////////////
// I hacked fceux to put whatever is written to memory location
// 0x4040 to stdout.  Hooray, now I can have printf style debugging
// in my program.
//////////////////////////////////////////////////////////////////////
#define xputc(ch) ( *(char*)0x4040 = ch )
#define measure() ( *(char*)0x4041 )
void write(int fd, const char *buf, int len)
{
    while(len--) {
        xputc(*buf++);
    }
}

#if 0
uint16_t mult(uint8_t a, uint8_t b) {
    writereg8(0x5205, a);
    writereg8(0x5206, b);
    return readreg16(0x5205);
}
#else
#define mult(a, b) ( \
    writereg8(0x5205, a), \
    writereg8(0x5206, b), \
    readreg16(0x5205) )
#endif

void main(void)
{
    uint8_t a, b;
    uint16_t c;
    char k;

    k = measure();
    a = 0;
    do {
        b = 0;
        do {
            c = mult(a, b);
            b = b + 1;
        } while(b);
        a = a + 1;
    } while(a);
    k = measure();

    for(;;) {}
}