void dcache16(void);
void dcache_init16(void);
void write16(void);
void dcache32(void);
void dcache_init32(void);
void write32(void);
void timer_start(void);
volatile int timer_end(void);
void write_cp0_config(long);
volatile int read_cp0_config(void);
void dcache_index_writeback_inv(unsigned long);
void dcache_hit_writeback_inv(unsigned long);

asm("j main");

int main(void)
{
  int tim;
  int i;
  char buf[256];

  asm("mtc0 $0,$12");/* 割り込み禁止 */
  dcache16();
  dcache_init16();
  timer_start();
  for(i=0;i<0x100;i++)
    write16();
  tim = timer_end();
  sprintf(buf,"line size=16 time=%d\n",tim);
  CPU_UartPuts(buf);

  dcache32();
  dcache_init32();
  timer_start();
  for(i=0;i<0x100;i++)
    write32();
  tim = timer_end();
  sprintf(buf,"line size=32 time=%d\n",tim);
  CPU_UartPuts(buf);

  return 0;
}

void dcache16(void)
{
  int dat;
  dat = read_cp0_config();
  dat |= 0x10;
  dat ^= 0x10; /* DB bit をクリア */
  write_cp0_config(dat);
}

void dcache32(void)
{
  int dat;
  dat = read_cp0_config();
  dat |= 0x10; /* DB bit をセット */
  write_cp0_config(dat);
}

void dcache_init16(void)
{
  unsigned long s,e;
  for(s=0x80000000; s < 0x80002000; s+=16){
    dcache_index_writeback_inv(s+0x0000); /* way-0 */
    dcache_index_writeback_inv(s+0x2000); /* way-1 */
  }
}

void dcache_init32(void)
{
  unsigned long s,e;
  for(s=0x80000000; s < 0x80002000; s+=32){
    dcache_index_writeback_inv(s+0x0000); /* way-0 */
    dcache_index_writeback_inv(s+0x2000); /* way-1 */
  }
}

void dcache_index_writeback_inv(unsigned long addr)
{
  __asm__ __volatile__ (
    "cache 0x1,0(%0)\n\t" /* Index_WriteBack_Inv_D */
    : "=r" (addr) : "0" (addr));
}

void dcache_hit_writeback_inv(unsigned long addr)
{
  __asm__ __volatile__ (
    "cache 0x15,0(%0)\n\t" /* Hit_WriteBack_Inv_D */
    : "=r" (addr) : "0" (addr));
}

void write16(void)
{
  unsigned long s,e;
  int cnt=0;
  for(s=0x80400000; s < 0x80410000; s++){
    *(volatile unsigned char*)s = cnt;
    if((cnt & 0xf)==0xf)
      dcache_hit_writeback_inv(s-0xf);
    cnt++;
  }
}

void write32(void)
{
  unsigned long s,e;
  int cnt=0;
  for(s=0x80400000; s < 0x80410000; s++){
    *(volatile unsigned char*)s = cnt;
    if((cnt & 0x1f)==0x1f)
      dcache_hit_writeback_inv(s-0x1f);
    cnt++;
  }
}

void timer_start(void)
{
  __asm__ __volatile__ (
    "mtc0 $0,$9\n\t"
  );
}

volatile int timer_end(void)
{
  int dat;
  __asm__ __volatile__ (
    "mfc0 %0,$9\n\t"
    :  "=r" (dat) : "0" (dat));
  return dat;
}

void write_cp0_config(long dat)
{
  __asm__ __volatile__ (
    "mtc0 %0,$16\n\t"
    : "=r" (dat) : "0" (dat));
}

volatile int read_cp0_config(void)
{
  int dat;
  __asm__ __volatile__ (
    "mfc0 %0,$16\n\t"
    : "=r" (dat) : "0" (dat));
}