/*

CHANGED BY CHUI FOR MMU_HANDLE LIBRARY COMPATIBLE


CAUTION: DON'T REMOVE ORIGINAL SQ.C OF KOS TOOLCHAIN.
	 THIS SQ.C ONLY WORKS FOR MMU_HANDLE LIBRARY 

*/

#define USE_SQ_MMU

/* KallistiOS ##version##

   kernel/arch/dreamcast/hardware/sq.c
   (c)2001 Andrew Kieschnick
*/

#include <dc/sq.h>

/*
    Functions to clear, copy, and set memory using the sh4 store queues

    Based on code by Marcus Comstedt (store_q_clear from tatest)
*/

/* clears n bytes at dest, dest must be 32-byte aligned */
void sq_clr(void *dest, int n) {
	unsigned int *d = (unsigned int *)(void *)
#ifdef USE_SQ_MMU
		((0xe0000000 | (((unsigned)dest) & 0x03ffffe0))|((((unsigned)dest)>>3)&0x3800000));
#else
		(0xe0000000 | (((unsigned long)dest) & 0x03ffffe0));
#endif

	/* Set store queue memory area as desired */
#ifndef USE_SQ_MMU
	QACR0 = ((((unsigned int)dest)>>26)<<2)&0x1c;
	QACR1 = ((((unsigned int)dest)>>26)<<2)&0x1c;
#endif
    
	/* Fill both store queues with zeroes */
	d[0] = d[1] = d[2] = d[3] = d[4] = d[5] = d[6] = d[7] =
		d[8] = d[9] = d[10] = d[11] = d[12] = d[13] = d[14] = d[15] = 0;
    
	/* Write them as many times necessary */
	n>>=5;
	while(n--) {
		asm("pref @%0" : : "r" (d));
		d += 8;
	}
    
	/* Wait for both store queues to complete */
	d = (unsigned int *)0xe0000000;
	d[0] = d[8] = 0;
}

/* copies n bytes from src to dest, dest must be 32-byte aligned */
void * sq_cpy(void *dest, void *src, int n) {
	unsigned int *d = (unsigned int *)(void *)
#ifdef USE_SQ_MMU
		((0xe0000000 | (((unsigned)dest) & 0x03ffffe0))|((((unsigned)dest)>>3)&0x3800000));
#else
		(0xe0000000 | (((unsigned long)dest) & 0x03ffffe0));
#endif
	unsigned int *s = src;
    
	/* Set store queue memory area as desired */
#ifndef USE_SQ_MMU
	QACR0 = ((((unsigned int)dest)>>26)<<2)&0x1c;
	QACR1 = ((((unsigned int)dest)>>26)<<2)&0x1c;
#endif
    
	/* fill/write queues as many times necessary */
	n>>=5;
	while(n--) {
		asm("pref @%0" : : "r" (s + 8)); /* prefetch 32 bytes for next loop */
		d[0] = *(s++);
		d[1] = *(s++);
		d[2] = *(s++);
		d[3] = *(s++);
		d[4] = *(s++);
		d[5] = *(s++);
		d[6] = *(s++);
		d[7] = *(s++);
		asm("pref @%0" : : "r" (d));
		d += 8;
	}
	/* Wait for both store queues to complete */
	d = (unsigned int *)0xe0000000;
	d[0] = d[8] = 0;
    
	return dest;
}

/* fills n bytes at s with byte c, s must be 32-byte aligned */
void * sq_set(void *s, uint32 c, int n) {
	unsigned int *d = (unsigned int *)(void *)
#ifdef USE_SQ_MMU
		((0xe0000000 | (((unsigned)s) & 0x03ffffe0))|((((unsigned)s)>>3)&0x3800000));
#else
		(0xe0000000 | (((unsigned long)s) & 0x03ffffe0));
#endif
    
 	/* Set store queue memory area as desired */
#ifndef USE_SQ_MMU
	QACR0 = ((((unsigned int)s)>>26)<<2)&0x1c;
	QACR1 = ((((unsigned int)s)>>26)<<2)&0x1c;
#endif

	/* duplicate low 8-bits of c into high 24-bits */
	c = c & 0xff;
	c = (c << 24) | (c << 16) | (c << 8) | c;

	/* Fill both store queues with c */
	d[0] = d[1] = d[2] = d[3] = d[4] = d[5] = d[6] = d[7] =
		d[8] = d[9] = d[10] = d[11] = d[12] = d[13] = d[14] = d[15] = c;

	/* Write them as many times necessary */
	n>>=5;
	while(n--) {
		asm("pref @%0" : : "r" (d));
		d += 8;
	}

	/* Wait for both store queues to complete */
	d = (unsigned int *)0xe0000000;
	d[0] = d[8] = 0;

	return s;
}

/* fills n bytes at s with short c, s must be 32-byte aligned */
void * sq_set16(void *s, uint32 c, int n) {
	unsigned int *d = (unsigned int *)(void *)
#ifdef USE_SQ_MMU
		((0xe0000000 | (((unsigned)s) & 0x03ffffe0))|((((unsigned)s)>>3)&0x3800000));
#else
		(0xe0000000 | (((unsigned long)s) & 0x03ffffe0));
#endif
    
	/* Set store queue memory area as desired */
#ifndef USE_SQ_MMU
 	QACR0 = ((((unsigned int)s)>>26)<<2)&0x1c;
	QACR1 = ((((unsigned int)s)>>26)<<2)&0x1c;
#endif

	/* duplicate low 16-bits of c into high 16-bits */
	c = c & 0xffff;
	c = (c << 16) | c;

	/* Fill both store queues with c */
	d[0] = d[1] = d[2] = d[3] = d[4] = d[5] = d[6] = d[7] =
		d[8] = d[9] = d[10] = d[11] = d[12] = d[13] = d[14] = d[15] = c;

	/* Write them as many times necessary */
	n>>=5;
	while(n--) {
		asm("pref @%0" : : "r" (d));
		d += 8;
	}

	/* Wait for both store queues to complete */
	d = (unsigned int *)0xe0000000;
	d[0] = d[8] = 0;

	return s;
}

/* fills n bytes at s with int c, s must be 32-byte aligned */
void * sq_set32(void *s, uint32 c, int n) {
	unsigned int *d = (unsigned int *)(void *)
#ifdef USE_SQ_MMU
		((0xe0000000 | (((unsigned)s) & 0x03ffffe0))|((((unsigned)s)>>3)&0x3800000));
#else
		(0xe0000000 | (((unsigned long)s) & 0x03ffffe0));
#endif
    
	/* Set store queue memory area as desired */
#ifndef USE_SQ_MMU
	QACR0 = ((((unsigned int)s)>>26)<<2)&0x1c;
	QACR1 = ((((unsigned int)s)>>26)<<2)&0x1c;
#endif

	/* Fill both store queues with c */
	d[0] = d[1] = d[2] = d[3] = d[4] = d[5] = d[6] = d[7] =
		d[8] = d[9] = d[10] = d[11] = d[12] = d[13] = d[14] = d[15] = c;

	/* Write them as many times necessary */
	n>>=5;
	while(n--) {
		asm("pref @%0" : : "r" (d));
		d += 8;
	}

	/* Wait for both store queues to complete */
	d = (unsigned int *)0xe0000000;
	d[0] = d[8] = 0;

	return s;
}


