PSP: switched to using my memcpy

svn-id: r50982

7 files changed, 921 insertions, 904 deletions

diff --git a/backends/platform/psp/Makefile b/backends/platform/psp/Makefile
index fed558eaad..994d201f65 100644
--- a/backends/platform/psp/Makefile
+++ b/backends/platform/psp/Makefile
@@ -148,7 +148,8 @@ OBJS := powerman.o \
 	audio.o \
 	thread.o \
 	rtc.o \
-	mp3.o
+	mp3.o \
+	tests.o
 
 # Include common Scummvm makefile
 include $(srcdir)/Makefile.common
diff --git a/backends/platform/psp/memory.cpp b/backends/platform/psp/memory.cpp
index d66650aee5..1e7705e9e8 100644
--- a/backends/platform/psp/memory.cpp
+++ b/backends/platform/psp/memory.cpp
@@ -1,404 +1,499 @@
-/* ScummVM - Graphic Adventure Engine

- *

- * ScummVM is the legal property of its developers, whose names

- * are too numerous to list here. Please refer to the COPYRIGHT

- * file distributed with this source distribution.

- *

- * This program is free software; you can redistribute it and/or

- * modify it under the terms of the GNU General Public License

- * as published by the Free Software Foundation; either version 2

- * of the License, or (at your option) any later version.

-

- * This program is distributed in the hope that it will be useful,

- * but WITHOUT ANY WARRANTY; without even the implied warranty of

- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

- * GNU General Public License for more details.

-

- * You should have received a copy of the GNU General Public License

- * along with this program; if not, write to the Free Software

- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

- *

- * $URL: https://scummvm.svn.sourceforge.net/svnroot/scummvm/scummvm/trunk/backends/platform/psp/osys_psp.cpp $

- * $Id: osys_psp.cpp 46126 2009-11-24 14:18:46Z fingolfin $

- *

- */

-

-#include "common/scummsys.h"

-#include "common/singleton.h"

-#include "common/list.h"

-#include "backends/platform/psp/PSPPixelFormat.h"

-#include "backends/platform/psp/memory.h"

-

-// Class Copier --------------------------------------------------------------------------

-//#define __PSP_DEBUG_FUNCS__	/* For debugging the stack */

-//#define __PSP_DEBUG_PRINT__

-

-#include "backends/platform/psp/trace.h"

-

-//#define TEST_MEMORY_COPY

-

-// swapRB is used to swap red and blue in the display

-void PspMemory::copy(byte *dst, const byte *src, uint32 bytes) {

-	DEBUG_ENTER_FUNC();

-

-#ifdef TEST_MEMORY_COPY

-	uint32 debugBytes = bytes;

-	const byte *debugDst = dst, *debugSrc = src;

-#endif

-

-	PSP_DEBUG_PRINT("copy(): dst[%p], src[%p], bytes[%d]\n", dst, src, bytes);

-

-	// align the destination pointer first

-	uint32 prefixDst = (((uint32)dst) & 0x3);

-	

-	if (prefixDst) {

-		prefixDst = 4 - prefixDst;				// prefix only if we have address % 4 != 0	

-		PSP_DEBUG_PRINT("prefixDst[%d]\n", prefixDst);

-

-		bytes -= prefixDst;						// remember we assume bytes >= 4

-	

-		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY) {	// check if it's worthwhile to continue

-			copy8(dst, src, bytes);

-#ifdef TEST_MEMORY_COPY

-			testCopy(debugDst, debugSrc, debugBytes);

-#endif		

-			return;

-		}

-		

-		while (prefixDst--) {

-			*dst++ = *src++;

-		}		

-	}

-	

-	// check the source pointer alignment now

-	uint32 alignSrc = (((uint32)src) & 0x3);

-	

-	if (alignSrc) {						// we'll need to realign our reads

-		copy32Misaligned((uint32 *)dst, src, bytes, alignSrc);

-	} else {

-		copy32Aligned((uint32 *)dst, (uint32 *)src, bytes);

-	}

-

-#ifdef TEST_MEMORY_COPY

-	testCopy(debugDst, debugSrc, debugBytes);

-#endif		

-}

-

-void PspMemory::testCopy(const byte *debugDst, const byte *debugSrc, uint32 debugBytes) {

-	

-	bool mismatch = false;

-	PSP_INFO_PRINT("testing memcpy...");

-

-	for (uint32 i = 0; i < debugBytes; i++) {

-		if (debugDst[i] != debugSrc[i]) {

-			if (mismatch == false) {

-				PSP_DEBUG_PRINT_SAMELN("**** mismatch in copy! ****\n");

-				PSP_DEBUG_PRINT("dst[%p], src[%p], bytes[%u]\n", debugDst, debugSrc, debugBytes);

-				mismatch = true;

-			}

-			PSP_DEBUG_PRINT_SAMELN("%x!=%x ", debugSrc[i], debugDst[i]);

-		}

-	}

-	if (mismatch) {

-		PSP_DEBUG_PRINT("\n");

-	} else {

-		PSP_INFO_PRINT("ok\n");

-	}	

-}

-

-// 

-// used to swap red and blue

-void PspMemory::swap(uint16 *dst16, const uint16 *src16, uint32 bytes, PSPPixelFormat &format) {

-	DEBUG_ENTER_FUNC();

-

-	// align the destination pointer first

-	uint32 prefixDst = (((uint32)dst16) & 0x3);	// for swap, we can only have 2 or 0 as our prefix

-	

-	if (prefixDst) {

-		bytes -= prefixDst;						// remember we assume bytes > 4

-		*dst16++ = *src16++;

-		

-		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY * 2) { // check if it's worthwhile to continue

-			swap16(dst16, src16, bytes, format);

-			return;

-		}

-	}

-	

-	// check the source pointer alignment now

-	uint32 alignSrc = (((uint32)src16) & 0x3);

-	

-	if (alignSrc) {						// we'll need to realign our reads

-		PSP_DEBUG_PRINT("misaligned copy of %u bytes from %p to %p\n", bytes, src16, dst16);

-		swap32Misaligned((uint32 *)dst16, (uint16 *)src16, bytes, format);

-	} else {

-		swap32Aligned((uint32 *)dst16, (uint32 *)src16, bytes, format);

-	}

-}

-

-void PspMemory::copy32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes) {

-	PSP_DEBUG_PRINT("copy32Aligned(): dst32[%p], src32[%p], bytes[%d]\n", dst32, src32, bytes);

-

-	int words8 = bytes >> 5;

-	

-	// try blocks of 8 words at a time

-	if (words8) {

-		while (words8--) {

-			dst32[0] = src32[0];

-			dst32[1] = src32[1];

-			dst32[2] = src32[2];

-			dst32[3] = src32[3];

-			dst32[4] = src32[4];

-			dst32[5] = src32[5];

-			dst32[6] = src32[6];

-			dst32[7] = src32[7];			

-			dst32 += 8;

-			src32 += 8;

-		}				

-	}

-	

-	int words4 = (bytes & 0x1F) >> 4;

-	

-	// try blocks of 4 words at a time

-	if (words4) {

-		dst32[0] = src32[0];

-		dst32[1] = src32[1];

-		dst32[2] = src32[2];

-		dst32[3] = src32[3];

-		dst32 += 4;

-		src32 += 4;

-	}

-	

-	int bytesLeft = (bytes & 0xF);	// only look at bytes left after we did the above

-	int wordsLeft = bytesLeft >> 2;

-	

-	// now just do single words

-	while (wordsLeft) {

-		*dst32++ = *src32++;

-		wordsLeft--;

-	}

-

-	bytesLeft = bytes & 0x3;	// get remaining bytes

-

-	PSP_DEBUG_PRINT("bytesLeft[%d]\n", bytesLeft);

-

-	byte *dst = (byte *)dst32;

-	byte *src = (byte *)src32;

-	

-	while (bytesLeft--) {

-		*dst++ = *src++;

-	}

-}

-

-void PspMemory::swap32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes, PSPPixelFormat &format) {

-	DEBUG_ENTER_FUNC();

-	int words = bytes >> 2;

-	

-	// try blocks of 4 words at a time

-	for (; words - 4 >= 0; words -= 4) {

-		dst32[0] = format.swapRedBlue32(src32[0]);

-		dst32[1] = format.swapRedBlue32(src32[1]);

-		dst32[2] = format.swapRedBlue32(src32[2]);

-		dst32[3] = format.swapRedBlue32(src32[3]);

-		dst32 += 4;

-		src32 += 4;

-	}

-	

-	// now just do words

-	for (; words > 0; words--) {

-		*dst32++ = format.swapRedBlue32(*src32++);

-	}	

-

-	uint32 remainingBytes = bytes & 0x3;

-	

-	if (remainingBytes) {	// for swap, must be a 16 bit value

-		*((uint16 *)dst32) = format.swapRedBlue16(*((uint16 *)src32));	// only 1 short left

-	}

-}

-

-

-// More challenging -- need to shift

-// Assume dst is aligned

-void PspMemory::copy32Misaligned(uint32 *dst32, const byte *src, uint32 bytes, uint32 alignSrc) {

-	PSP_DEBUG_PRINT("copy32Misaligned: dst32[%p], src[%p], bytes[%d], alignSrc[%d]\n", dst32, src, bytes, alignSrc);

-	

-	uint32 *src32 = (uint32 *)(((uint32)src) & 0xFFFFFFFC);	// remove misalignment

-	uint32 offset;

-	

-	switch (alignSrc) {

-	case 1:

-		offset = misaligned32Detail(dst32, src32, bytes, alignSrc, 8, 24);

-		break;

-	case 2:

-		offset = misaligned32Detail(dst32, src32, bytes, alignSrc, 16, 16);

-		break;

-	case 3:

-		offset = misaligned32Detail(dst32, src32, bytes, alignSrc, 24, 8);

-		break;

-	}

-	

-	uint32 remainingBytes = bytes & 3;

-	

-	if (remainingBytes) {

-		byte *dst = (byte *)dst32;

-		src += offset;

-		dst += offset;

-		copy8(dst, src, remainingBytes);

-	}	

-}

-

-// returns offset in dst

-uint32 PspMemory::misaligned32Detail(uint32 *dst32, uint32 *src32, uint32 bytes, uint32 alignSrc, const uint32 shiftValue, const uint32 lastShiftValue) {

-	uint32 *origDst32 = dst32;

-	register uint32 dstWord, srcWord;

-	

-	PSP_DEBUG_PRINT("misaligned32Detail(): alignSrc[%d], dst32[%p], src32[%p], words[%d]\n", alignSrc, dst32, src32, words);

-	

-	// Try to do groups of 4 words

-	uint32 words4 = bytes >> 4;

-	

-	srcWord = src32[0];

-

-	while (words4--) {

-		dstWord = srcWord >> shiftValue;

-		srcWord = src32[1];

-		dstWord |= srcWord << lastShiftValue;

-		dst32[0] = dstWord;

-		dstWord = srcWord >> shiftValue;

-		srcWord = src32[2];

-		dstWord |= srcWord << lastShiftValue;

-		dst32[1] = dstWord;

-		dstWord = srcWord >> shiftValue;

-		srcWord = src32[3];

-		dstWord |= srcWord << lastShiftValue;

-		dst32[2] = dstWord;

-		dstWord = srcWord >> shiftValue;

-		srcWord = src32[4];

-		dstWord |= srcWord << lastShiftValue;

-		dst32[3] = dstWord;

-		src32 += 4;

-		dst32 += 4;

-	}

-	

-	uint32 words = (bytes & 0xF) >> 2;

-	

-	// we read one word ahead of what we write

-	// setup the first read

-	if (words) {

-		srcWord = *src32++;

-		

-		while (words--) {

-			dstWord = srcWord >> shiftValue;

-			srcWord = *src32++;

-			dstWord |= srcWord << lastShiftValue;

-			*dst32++ = dstWord;

-		}

-	}

-	

-	return (byte *)dst32 - (byte *)origDst32;

-}

-// More challenging -- need to shift

-// Assume dst is aligned

-void PspMemory::swap32Misaligned(uint32 *dst32, const uint16 *src16, uint32 bytes, PSPPixelFormat &format) {

-	DEBUG_ENTER_FUNC();

-	if (bytes < MIN_AMOUNT_FOR_MISALIGNED_COPY) {	// less than a certain number of bytes it's just not worth it

-		swap16((uint16 *)dst32, src16, bytes, format);

-		return;

-	}

-	

-	int words = bytes >> 2;

-	uint32 remainingBytes = bytes & 3;

-	

-	uint32 *src32 = (uint32 *)(((uint32)src16) & 0xFFFFFFFC);	// remove misalignment

-

-	// we read one word ahead of what we write

-	// setup the first read

-	uint32 lastWord = ((*src32++) >> 16) & 0xFFFF;

-	

-	for (; words; words--) {

-		uint32 srcWord = *src32++;

-		uint32 curWord = (srcWord >> 16) & 0xFFFF;

-		lastWord |= (srcWord & 0xFFFF) << 16;	// take the part of the src that belongs to this word		

-		*dst32++ = format.swapRedBlue32(lastWord);

-		lastWord = curWord;

-	}

-	

-	if (remainingBytes) {	// add in the remaining stuff

-		*(uint16 *)dst32 = format.swapRedBlue16((uint16)lastWord);

-	}

-}

-

-inline void PspMemory::copy16(uint16 *dst16, const uint16 *src16, uint32 bytes) {

-	PSP_DEBUG_PRINT("copy16(): dst16[%p], src16[%p], bytes[%d]\n", dst16, src16, bytes);

-	

-	uint32 shorts = bytes >> 1;

-	uint32 remainingBytes = bytes & 1;

-

-	for (; shorts > 0 ; shorts--) {

-		*dst16++ = *src16++;

-	}

-	if (remainingBytes)

-		*(byte *)dst16 = *(byte *)src16;

-}

-

-// Class VramAllocator -----------------------------------

-

-DECLARE_SINGLETON(VramAllocator)

-

-//#define __PSP_DEBUG_FUNCS__	/* For debugging the stack */

-//#define __PSP_DEBUG_PRINT__

-

-#include "backends/platform/psp/trace.h"

-

-

-void *VramAllocator::allocate(int32 size, bool smallAllocation /* = false */) {

-	DEBUG_ENTER_FUNC();

-	assert(size > 0);

-

-	byte *lastAddress = smallAllocation ? (byte *)VRAM_SMALL_ADDRESS : (byte *)VRAM_START_ADDRESS;

-	Common::List<Allocation>::iterator i;

-

-	// Find a block that fits, starting from the beginning

-	for (i = _allocList.begin(); i != _allocList.end(); ++i) {

-		byte *currAddress = (*i).address;

-

-		if (currAddress - lastAddress >= size) // We found a match

-			break;

-

-		if ((*i).getEnd() > lastAddress)

-			lastAddress = (byte *)(*i).getEnd();

-	}

-

-	if (lastAddress + size > (byte *)VRAM_END_ADDRESS) {

-		PSP_DEBUG_PRINT("No space for allocation of %d bytes. %d bytes already allocated.\n",

-		                size, _bytesAllocated);

-		return NULL;

-	}

-

-	_allocList.insert(i, Allocation(lastAddress, size));

-	_bytesAllocated += size;

-

-	PSP_DEBUG_PRINT("Allocated in VRAM, size %u at %p.\n", size, lastAddress);

-	PSP_DEBUG_PRINT("Total allocated %u, remaining %u.\n", _bytesAllocated, (2 * 1024 * 1024) - _bytesAllocated);

-

-	return lastAddress;

-}

-

-// Deallocate a block from VRAM

-void VramAllocator::deallocate(void *address) {

-	DEBUG_ENTER_FUNC();

-	address = (byte *)CACHED(address);	// Make sure all addresses are the same

-

-	Common::List<Allocation>::iterator i;

-

-	// Find the Allocator to deallocate

-	for (i = _allocList.begin(); i != _allocList.end(); ++i) {

-		if ((*i).address == address) {

-			_bytesAllocated -= (*i).size;

-			_allocList.erase(i);

-			PSP_DEBUG_PRINT("Deallocated address[%p], size[%u]\n", (*i).address, (*i).size);

-			return;

-		}

-	}

-

-	PSP_DEBUG_PRINT("Address[%p] not allocated.\n", address);

-}

+/* ScummVM - Graphic Adventure Engine
+ *
+ * ScummVM is the legal property of its developers, whose names
+ * are too numerous to list here. Please refer to the COPYRIGHT
+ * file distributed with this source distribution.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ * $URL: https://scummvm.svn.sourceforge.net/svnroot/scummvm/scummvm/trunk/backends/platform/psp/osys_psp.cpp $
+ * $Id: osys_psp.cpp 46126 2009-11-24 14:18:46Z fingolfin $
+ *
+ */
+
+#include "common/scummsys.h"
+#include "common/singleton.h"
+#include "common/list.h"
+#include "backends/platform/psp/PSPPixelFormat.h"
+#include "backends/platform/psp/memory.h"
+
+// Class Copier --------------------------------------------------------------------------
+//#define __PSP_DEBUG_FUNCS__	/* For debugging the stack */
+//#define __PSP_DEBUG_PRINT__
+
+#include "backends/platform/psp/trace.h"
+
+//#define TEST_MEMORY_COPY
+
+extern "C" {
+
+void *__wrap_memcpy(void *dst, void *src, size_t bytes) {
+	PspMemory::fastCopy((byte *)dst, (byte *)src, bytes);
+	return dst;
+}
+
+}
+
+void PspMemory::copy(byte *dst, const byte *src, uint32 bytes) {
+	DEBUG_ENTER_FUNC();
+
+#ifdef TEST_MEMORY_COPY
+	uint32 debugBytes = bytes;
+	const byte *debugDst = dst, *debugSrc = src;
+#endif
+
+	PSP_DEBUG_PRINT("copy(): dst[%p], src[%p], bytes[%d]\n", dst, src, bytes);
+
+	// align the destination pointer first
+	uint32 prefixDst = (((uint32)dst) & 0x3);
+	
+	if (prefixDst) {
+		prefixDst = 4 - prefixDst;				// prefix only if we have address % 4 != 0	
+		PSP_DEBUG_PRINT("prefixDst[%d]\n", prefixDst);
+
+		bytes -= prefixDst;						// remember we assume bytes >= 4
+		
+		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY) {	// check if it's worthwhile to continue
+			copy8(dst, src, bytes + prefixDst);
+#ifdef TEST_MEMORY_COPY
+			testCopy(debugDst, debugSrc, debugBytes);
+#endif		
+			return;
+		}
+		
+		while (prefixDst--) {
+			*dst++ = *src++;
+		}		
+	}
+	
+	// check the source pointer alignment now
+	uint32 alignSrc = (((uint32)src) & 0x3);
+	
+	if (alignSrc) {						// we'll need to realign our reads
+		copy32Misaligned((uint32 *)dst, src, bytes, alignSrc);
+	} else {
+		copy32Aligned((uint32 *)dst, (uint32 *)src, bytes);
+	}
+
+#ifdef TEST_MEMORY_COPY
+	testCopy(debugDst, debugSrc, debugBytes);
+#endif		
+}
+
+void PspMemory::testCopy(const byte *debugDst, const byte *debugSrc, uint32 debugBytes) {
+	
+	bool mismatch = false;
+	PSP_INFO_PRINT("testing fastCopy...");
+
+	for (uint32 i = 0; i < debugBytes; i++) {
+		if (debugDst[i] != debugSrc[i]) {
+			if (!mismatch) {
+				PSP_INFO_PRINT("**** mismatch in copy! ****\n");
+				PSP_INFO_PRINT("dst[%p], src[%p], bytes[%u]\n", debugDst, debugSrc, debugBytes);
+				mismatch = true;
+			}
+			PSP_INFO_PRINT("[%d]%x!=%x ", i, debugSrc[i], debugDst[i]);
+		}
+	}
+	if (mismatch) {
+		PSP_INFO_PRINT("\n");
+	} else {
+		PSP_INFO_PRINT("ok\n");
+	}	
+}
+
+// 
+// used to swap red and blue
+void PspMemory::swap(uint16 *dst16, const uint16 *src16, uint32 bytes, PSPPixelFormat &format) {
+	DEBUG_ENTER_FUNC();
+
+#ifdef TEST_MEMORY_COPY
+	uint32 debugBytes = bytes;
+	const uint16 *debugDst = dst16, *debugSrc = src16;
+#endif
+	
+	// align the destination pointer first
+	uint32 prefixDst = (((uint32)dst16) & 0x3);	// for swap, we can only have 2 or 0 as our prefix
+	
+	if (prefixDst) {
+		bytes -= prefixDst;						// remember we assume bytes > 4
+		*dst16++ = format.swapRedBlue16(*src16++);
+		
+		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY) { // check if it's worthwhile to continue
+			swap16(dst16, src16, bytes, format);
+
+#ifdef TEST_MEMORY_COPY
+			testSwap(debugDst, debugSrc, debugBytes, format);
+#endif		
+			return;
+		}
+	}
+	
+	// check the source pointer alignment now
+	uint32 alignSrc = (((uint32)src16) & 0x3);
+	
+	if (alignSrc) {						// we'll need to realign our reads
+		PSP_DEBUG_PRINT("misaligned copy of %u bytes from %p to %p\n", bytes, src16, dst16);
+		swap32Misaligned((uint32 *)dst16, src16, bytes, format);
+	} else {
+		swap32Aligned((uint32 *)dst16, (const uint32 *)src16, bytes, format);
+	}
+	
+#ifdef TEST_MEMORY_COPY
+	testSwap(debugDst, debugSrc, debugBytes, format);
+#endif		
+	
+}
+
+void PspMemory::testSwap(const uint16 *debugDst, const uint16 *debugSrc, uint32 debugBytes, PSPPixelFormat &format) {
+	
+	bool mismatch = false;
+	PSP_INFO_PRINT("testing fastSwap...");
+	
+	uint32 shorts = debugBytes >> 1;
+
+	for (uint32 i = 0; i < shorts; i++) {
+		if (debugDst[i] != format.swapRedBlue16(debugSrc[i])) {
+			if (!mismatch) {
+				PSP_INFO_PRINT("**** mismatch in swap! ****\n");
+				PSP_INFO_PRINT("dst[%p], src[%p], bytes[%u]\n", debugDst, debugSrc, debugBytes);
+				mismatch = true;
+			}
+			PSP_INFO_PRINT("[%d]%x!=%x ", i<<1, format.swapRedBlue16(debugSrc[i]), debugDst[i]);
+		}
+	}
+	if (mismatch) {
+		PSP_INFO_PRINT("\n");
+	} else {
+		PSP_INFO_PRINT("ok\n");
+	}	
+}
+
+
+void PspMemory::copy32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes) {
+	PSP_DEBUG_PRINT("copy32Aligned(): dst32[%p], src32[%p], bytes[%d]\n", dst32, src32, bytes);
+
+	int words8 = bytes >> 5;
+	
+	// try blocks of 8 words at a time
+	if (words8) {
+		while (words8--) {
+			uint32 a, b, c, d;
+			a = src32[0];
+			b = src32[1];
+			c = src32[2];
+			d = src32[3];
+			dst32[0] = a;
+			dst32[1] = b;
+			dst32[2] = c;
+			dst32[3] = d;
+			a = src32[4];
+			b = src32[5];
+			c = src32[6];
+			d = src32[7];
+			dst32[4] = a;
+			dst32[5] = b;
+			dst32[6] = c;
+			dst32[7] = d;
+			dst32 += 8;
+			src32 += 8;
+		}				
+	}
+	
+	int words4 = (bytes & 0x1F) >> 4;
+	
+	// try blocks of 4 words at a time
+	if (words4) {
+		uint32 a, b, c, d;
+		a = src32[0];
+		b = src32[1];
+		c = src32[2];
+		d = src32[3];
+		dst32[0] = a;
+		dst32[1] = b;
+		dst32[2] = c;
+		dst32[3] = d;
+		dst32 += 4;
+		src32 += 4;
+	}
+	
+	int bytesLeft = (bytes & 0xF);	// only look at bytes left after we did the above
+	int wordsLeft = bytesLeft >> 2;
+	
+	// now just do single words
+	while (wordsLeft) {
+		*dst32++ = *src32++;
+		wordsLeft--;
+	}
+
+	bytesLeft = bytes & 0x3;	// get remaining bytes
+
+	PSP_DEBUG_PRINT("bytesLeft[%d]\n", bytesLeft);
+
+	byte *dst = (byte *)dst32;
+	byte *src = (byte *)src32;
+	
+	while (bytesLeft--) {
+		*dst++ = *src++;
+	}
+}
+
+void PspMemory::swap32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes, PSPPixelFormat &format) {
+	DEBUG_ENTER_FUNC();
+	int words4 = bytes >> 4;
+	
+	// try blocks of 4 words at a time
+	while (words4--) {
+		uint32 a, b, c, d;
+		a = format.swapRedBlue32(src32[0]);
+		b = format.swapRedBlue32(src32[1]);
+		c = format.swapRedBlue32(src32[2]);
+		d = format.swapRedBlue32(src32[3]);
+		dst32[0] = a;
+		dst32[1] = b;
+		dst32[2] = c;
+		dst32[3] = d;
+		dst32 += 4;
+		src32 += 4;
+	}
+
+	uint32 bytesLeft = bytes & 0xF;
+	uint32 words = bytesLeft >> 2;
+	
+	// now just do words
+	while (words--) {
+		*dst32++ = format.swapRedBlue32(*src32++);
+	}	
+
+	bytesLeft = bytes & 0x3;
+	
+	if (bytesLeft) {	// for swap, can only be 1 short left
+		*((uint16 *)dst32) = format.swapRedBlue16(*((uint16 *)src32));
+	}
+}
+
+
+// More challenging -- need to shift
+// Assume dst is aligned
+void PspMemory::copy32Misaligned(uint32 *dst32, const byte *src, uint32 bytes, uint32 alignSrc) {
+	PSP_DEBUG_PRINT("copy32Misaligned: dst32[%p], src[%p], bytes[%d], alignSrc[%d]\n", dst32, src, bytes, alignSrc);
+	
+	uint32 *src32 = (uint32 *)(((uint32)src) & 0xFFFFFFFC);	// remove misalignment
+	uint32 offset;
+	
+	switch (alignSrc) {
+	case 1:
+		offset = misaligned32Detail(dst32, src32, bytes, alignSrc, 8, 24);
+		break;
+	case 2:
+		offset = misaligned32Detail(dst32, src32, bytes, alignSrc, 16, 16);
+		break;
+	default: /* 3 */
+		offset = misaligned32Detail(dst32, src32, bytes, alignSrc, 24, 8);
+		break;
+	}
+	
+	uint32 remainingBytes = bytes & 3;
+	
+	if (remainingBytes) {
+		byte *dst = (byte *)dst32;
+		src += offset;
+		dst += offset;
+		copy8(dst, src, remainingBytes);
+	}	
+}
+
+// returns offset in dst
+uint32 PspMemory::misaligned32Detail(uint32 *dst32, uint32 *src32, uint32 bytes, uint32 alignSrc, const uint32 shiftValue, const uint32 lastShiftValue) {
+	uint32 *origDst32 = dst32;
+	register uint32 dstWord, srcWord;
+	
+	PSP_DEBUG_PRINT("misaligned32Detail(): alignSrc[%d], dst32[%p], src32[%p], bytes[%d]\n", alignSrc, dst32, src32, bytes);
+	
+	// Try to do groups of 4 words
+	uint32 words4 = bytes >> 4;
+	
+	srcWord = src32[0];
+
+	while (words4--) {
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[1];
+		dstWord |= srcWord << lastShiftValue;
+		dst32[0] = dstWord;
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[2];
+		dstWord |= srcWord << lastShiftValue;
+		dst32[1] = dstWord;
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[3];
+		dstWord |= srcWord << lastShiftValue;
+		dst32[2] = dstWord;
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[4];
+		dstWord |= srcWord << lastShiftValue;
+		dst32[3] = dstWord;
+		src32 += 4;
+		dst32 += 4;
+	}
+	
+	uint32 words = (bytes & 0xF) >> 2;
+	
+	// we read one word ahead of what we write
+	// setup the first read
+	if (words) {
+		src32++;	// we already loaded the value, so just increment
+		
+		while (words--) {
+			dstWord = srcWord >> shiftValue;
+			srcWord = *src32++;
+			dstWord |= srcWord << lastShiftValue;
+			*dst32++ = dstWord;
+		}
+	}
+	
+	return (byte *)dst32 - (byte *)origDst32;
+}
+
+// More challenging -- need to shift
+// We assume dst is aligned
+void PspMemory::swap32Misaligned(uint32 *dst32, const uint16 *src16, uint32 bytes, PSPPixelFormat &format) {
+	DEBUG_ENTER_FUNC();
+
+	const uint32 shiftValue = 16;
+	uint32 *src32 = (uint32 *)(((uint32)src16) & 0xFFFFFFFC);	// remove misalignment
+	
+	// Try to do groups of 4 words
+	uint32 words4 = bytes >> 4;
+	uint32 srcWord = src32[0];	// preload
+
+	while (words4--) {
+		uint32 dstWord = srcWord >> shiftValue;
+		srcWord = src32[1];
+		dstWord |= srcWord << shiftValue;
+		dst32[0] = format.swapRedBlue32(dstWord);
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[2];
+		dstWord |= srcWord << shiftValue;
+		dst32[1] = format.swapRedBlue32(dstWord);
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[3];
+		dstWord |= srcWord << shiftValue;
+		dst32[2] = format.swapRedBlue32(dstWord);
+		dstWord = srcWord >> shiftValue;
+		srcWord = src32[4];
+		dstWord |= srcWord << shiftValue;
+		dst32[3] = format.swapRedBlue32(dstWord);
+		src32 += 4;
+		dst32 += 4;
+	}
+	
+	uint32 words = (bytes & 0xF) >> 2;
+	
+	// we read one word ahead of what we write
+	// setup the first read
+	if (words) {
+		//srcWord = *src32++;	// don't need this. already loaded
+		src32++;	// we already have the value loaded in
+		
+		while (words--) {
+			uint32 dstWord = srcWord >> shiftValue;
+			srcWord = *src32++;
+			dstWord |= srcWord << shiftValue;
+			*dst32++ = format.swapRedBlue32(dstWord);
+		}
+	}
+	
+	uint32 bytesLeft = bytes & 3;
+	
+	if (bytesLeft) {	// for swap, can only be 1 short left
+		*((uint16 *)dst32) = format.swapRedBlue16((uint16)(srcWord >> shiftValue));
+	}
+}
+
+inline void PspMemory::copy16(uint16 *dst16, const uint16 *src16, uint32 bytes) {
+	PSP_DEBUG_PRINT("copy16(): dst16[%p], src16[%p], bytes[%d]\n", dst16, src16, bytes);
+	
+	uint32 shorts = bytes >> 1;
+	uint32 remainingBytes = bytes & 1;
+
+	for (; shorts > 0 ; shorts--) {
+		*dst16++ = *src16++;
+	}
+	if (remainingBytes)
+		*(byte *)dst16 = *(byte *)src16;
+}
+
+// Class VramAllocator -----------------------------------
+
+DECLARE_SINGLETON(VramAllocator)
+
+//#define __PSP_DEBUG_FUNCS__	/* For debugging the stack */
+//#define __PSP_DEBUG_PRINT__
+
+#include "backends/platform/psp/trace.h"
+
+
+void *VramAllocator::allocate(int32 size, bool smallAllocation /* = false */) {
+	DEBUG_ENTER_FUNC();
+	assert(size > 0);
+
+	byte *lastAddress = smallAllocation ? (byte *)VRAM_SMALL_ADDRESS : (byte *)VRAM_START_ADDRESS;
+	Common::List<Allocation>::iterator i;
+
+	// Find a block that fits, starting from the beginning
+	for (i = _allocList.begin(); i != _allocList.end(); ++i) {
+		byte *currAddress = (*i).address;
+
+		if (currAddress - lastAddress >= size) // We found a match
+			break;
+
+		if ((*i).getEnd() > lastAddress)
+			lastAddress = (byte *)(*i).getEnd();
+	}
+
+	if (lastAddress + size > (byte *)VRAM_END_ADDRESS) {
+		PSP_DEBUG_PRINT("No space for allocation of %d bytes. %d bytes already allocated.\n",
+		                size, _bytesAllocated);
+		return NULL;
+	}
+
+	_allocList.insert(i, Allocation(lastAddress, size));
+	_bytesAllocated += size;
+
+	PSP_DEBUG_PRINT("Allocated in VRAM, size %u at %p.\n", size, lastAddress);
+	PSP_DEBUG_PRINT("Total allocated %u, remaining %u.\n", _bytesAllocated, (2 * 1024 * 1024) - _bytesAllocated);
+
+	return lastAddress;
+}
+
+// Deallocate a block from VRAM
+void VramAllocator::deallocate(void *address) {
+	DEBUG_ENTER_FUNC();
+	address = (byte *)CACHED(address);	// Make sure all addresses are the same
+
+	Common::List<Allocation>::iterator i;
+
+	// Find the Allocator to deallocate
+	for (i = _allocList.begin(); i != _allocList.end(); ++i) {
+		if ((*i).address == address) {
+			_bytesAllocated -= (*i).size;
+			_allocList.erase(i);
+			PSP_DEBUG_PRINT("Deallocated address[%p], size[%u]\n", (*i).address, (*i).size);
+			return;
+		}
+	}
+
+	PSP_DEBUG_PRINT("Address[%p] not allocated.\n", address);
+}
diff --git a/backends/platform/psp/memory.h b/backends/platform/psp/memory.h
index 81a77cca1c..c9b3d21cb6 100644
--- a/backends/platform/psp/memory.h
+++ b/backends/platform/psp/memory.h
@@ -1,131 +1,132 @@
-

-/* ScummVM - Graphic Adventure Engine

- *

- * ScummVM is the legal property of its developers, whose names

- * are too numerous to list here. Please refer to the COPYRIGHT

- * file distributed with this source distribution.

- *

- * This program is free software; you can redistribute it and/or

- * modify it under the terms of the GNU General Public License

- * as published by the Free Software Foundation; either version 2

- * of the License, or (at your option) any later version.

-

- * This program is distributed in the hope that it will be useful,

- * but WITHOUT ANY WARRANTY; without even the implied warranty of

- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

- * GNU General Public License for more details.

-

- * You should have received a copy of the GNU General Public License

- * along with this program; if not, write to the Free Software

- * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

- *

- * $URL: https://scummvm.svn.sourceforge.net/svnroot/scummvm/scummvm/trunk/backends/platform/psp/osys_psp.cpp $

- * $Id: osys_psp.cpp 46126 2009-11-24 14:18:46Z fingolfin $

- *

- */

-

-#ifndef PSP_MEMORY_H

-#define PSP_MEMORY_H

-

-#include "backends/platform/psp/psppixelformat.h"

-#include "common/list.h"

-

-#define UNCACHED(x)		((byte *)(((uint32)(x)) | 0x40000000))	/* make an uncached access */

-#define CACHED(x)		((byte *)(((uint32)(x)) & 0xBFFFFFFF))	/* make an uncached access into a cached one */

-

-#define MIN_AMOUNT_FOR_COMPLEX_COPY  8

-#define MIN_AMOUNT_FOR_MISALIGNED_COPY 8

-

-//#define __PSP_DEBUG_PRINT__

-

-#include "backends/platform/psp/trace.h"

-

-/**

- *	Class that does memory copying and swapping if needed

- */

-class PspMemory {

-private:

-	static void testCopy(const byte *debugDst, const byte *debugSrc, uint32 debugBytes);

-	static void copy(byte *dst, const byte *src, uint32 bytes);

-	static void swap(uint16 *dst16, const uint16 *src16, uint32 bytes, PSPPixelFormat &format);

-	static void copy32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes);

-	static void swap32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes, PSPPixelFormat &format);

-	static void copy32Misaligned(uint32 *dst32, const byte *src, uint32 bytes, uint32 alignSrc);

-	static uint32 misaligned32Detail(uint32 *dst32, uint32 *src32, uint32 bytes, uint32 alignSrc, const uint32 shiftValue, const uint32 lastShiftValue);

-	static void swap32Misaligned(uint32 *dst32, const uint16 *src16, uint32 bytes, PSPPixelFormat &format);

-	static void copy16(uint16 *dst, const uint16 *src, uint32 bytes);

-

-	// For swapping, we know that we have multiples of 16 bits

-	static void swap16(uint16 *dst16, const uint16 *src16, uint32 bytes, PSPPixelFormat &format) {

-		PSP_DEBUG_PRINT("swap16 called with dst16[%p], src16[%p], bytes[%d]\n", dst16, src16, bytes);

-		uint32 shorts = bytes >> 1;

-

-		for (; shorts > 0 ; shorts--) {

-			*dst16++ = format.swapRedBlue16(*src16++);

-		}

-	}

-	

-	static void copy8(byte *dst, const byte *src, uint32 bytes) {

-		PSP_DEBUG_PRINT("copy8 called with dst[%p], src[%p], bytes[%d]\n", dst, src, bytes);

-		while (bytes--) {

-			*dst++ = *src++;

-		}

-	}

-

-public:	

-	// This is the interface to the outside world

-	static void fastCopy(byte *dst, const byte *src, uint32 bytes) {

-		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY) {

-			copy8(dst, src, bytes);

-		} else {	// go to more powerful copy

-			copy(dst, src, bytes);

-		}

-	}

-

-	static void fastSwap(byte *dst, const byte *src, uint32 bytes, PSPPixelFormat &format) {

-		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY * 2) {

-			swap16((uint16 *)dst, (uint16 *)src, bytes, format);

-		} else {	// go to more powerful copy

-			swap((uint16 *)dst, (uint16 *)src, bytes, format);

-		}

-	}

-};

-

-/**

- *	Class that allocates memory in the VRAM

- */

-class VramAllocator : public Common::Singleton<VramAllocator> {

-public:

-	VramAllocator() : _bytesAllocated(0) {}

-	void *allocate(int32 size, bool smallAllocation = false);	// smallAllocation e.g. palettes

-	void deallocate(void *pointer);

-

-	static inline bool isAddressInVram(void *address) {

-		if ((uint32)(CACHED(address)) >= VRAM_START_ADDRESS && (uint32)(CACHED(address)) < VRAM_END_ADDRESS)

-			return true;

-		return false;

-	}

-

-

-private:

-	/**

-	 *	Used to allocate in VRAM

-	 */

-	struct Allocation {

-		byte *address;

-		uint32 size;

-		void *getEnd() { return address + size; }

-		Allocation(void *Address, uint32 Size) : address((byte *)Address), size(Size) {}

-		Allocation() : address(0), size(0) {}

-	};

-

-	enum {

-		VRAM_START_ADDRESS = 0x04000000,

-		VRAM_END_ADDRESS   = 0x04200000,

-		VRAM_SMALL_ADDRESS = VRAM_END_ADDRESS - (4 * 1024)	// 4K in the end for small allocations

-	};

-	Common::List <Allocation> _allocList;		// List of allocations

-	uint32 _bytesAllocated;

-};

-

-#endif /* PSP_MEMORY_H */

+
+/* ScummVM - Graphic Adventure Engine
+ *
+ * ScummVM is the legal property of its developers, whose names
+ * are too numerous to list here. Please refer to the COPYRIGHT
+ * file distributed with this source distribution.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+ *
+ * $URL: https://scummvm.svn.sourceforge.net/svnroot/scummvm/scummvm/trunk/backends/platform/psp/osys_psp.cpp $
+ * $Id: osys_psp.cpp 46126 2009-11-24 14:18:46Z fingolfin $
+ *
+ */
+
+#ifndef PSP_MEMORY_H
+#define PSP_MEMORY_H
+
+#include "backends/platform/psp/psppixelformat.h"
+#include "common/list.h"
+
+#define UNCACHED(x)		((byte *)(((uint32)(x)) | 0x40000000))	/* make an uncached access */
+#define CACHED(x)		((byte *)(((uint32)(x)) & 0xBFFFFFFF))	/* make an uncached access into a cached one */
+
+#define MIN_AMOUNT_FOR_COMPLEX_COPY  8
+#define MIN_AMOUNT_FOR_MISALIGNED_COPY 8
+
+//#define __PSP_DEBUG_PRINT__
+
+#include "backends/platform/psp/trace.h"
+
+/**
+ *	Class that does memory copying and swapping if needed
+ */
+class PspMemory {
+private:
+	static void testCopy(const byte *debugDst, const byte *debugSrc, uint32 debugBytes);
+	static void testSwap(const uint16 *debugDst, const uint16 *debugSrc, uint32 debugBytes, PSPPixelFormat &format);
+	static void copy(byte *dst, const byte *src, uint32 bytes);
+	static void swap(uint16 *dst16, const uint16 *src16, uint32 bytes, PSPPixelFormat &format);
+	static void copy32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes);
+	static void swap32Aligned(uint32 *dst32, const uint32 *src32, uint32 bytes, PSPPixelFormat &format);
+	static void copy32Misaligned(uint32 *dst32, const byte *src, uint32 bytes, uint32 alignSrc);
+	static uint32 misaligned32Detail(uint32 *dst32, uint32 *src32, uint32 bytes, uint32 alignSrc, const uint32 shiftValue, const uint32 lastShiftValue);
+	static void swap32Misaligned(uint32 *dst32, const uint16 *src16, uint32 bytes, PSPPixelFormat &format);
+	static void copy16(uint16 *dst, const uint16 *src, uint32 bytes);
+
+	// For swapping, we know that we have multiples of 16 bits
+	static void swap16(uint16 *dst16, const uint16 *src16, uint32 bytes, PSPPixelFormat &format) {
+		PSP_DEBUG_PRINT("swap16 called with dst16[%p], src16[%p], bytes[%d]\n", dst16, src16, bytes);
+		uint32 shorts = bytes >> 1;
+
+		while (shorts--) {
+			*dst16++ = format.swapRedBlue16(*src16++);
+		}
+	}
+	
+	static void copy8(byte *dst, const byte *src, uint32 bytes) {
+		PSP_DEBUG_PRINT("copy8 called with dst[%p], src[%p], bytes[%d]\n", dst, src, bytes);
+		while (bytes--) {
+			*dst++ = *src++;
+		}
+	}
+
+public:	
+	// This is the interface to the outside world
+	static void fastCopy(byte *dst, const byte *src, uint32 bytes) {
+		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY) {
+			copy8(dst, src, bytes);
+		} else {	// go to more powerful copy
+			copy(dst, src, bytes);
+		}
+	}
+
+	static void fastSwap(byte *dst, const byte *src, uint32 bytes, PSPPixelFormat &format) {
+		if (bytes < MIN_AMOUNT_FOR_COMPLEX_COPY * 2) {
+			swap16((uint16 *)dst, (uint16 *)src, bytes, format);
+		} else {	// go to more powerful copy
+			swap((uint16 *)dst, (uint16 *)src, bytes, format);
+		}
+	}
+};
+
+/**
+ *	Class that allocates memory in the VRAM
+ */
+class VramAllocator : public Common::Singleton<VramAllocator> {
+public:
+	VramAllocator() : _bytesAllocated(0) {}
+	void *allocate(int32 size, bool smallAllocation = false);	// smallAllocation e.g. palettes
+	void deallocate(void *pointer);
+
+	static inline bool isAddressInVram(void *address) {
+		if ((uint32)(CACHED(address)) >= VRAM_START_ADDRESS && (uint32)(CACHED(address)) < VRAM_END_ADDRESS)
+			return true;
+		return false;
+	}
+
+
+private:
+	/**
+	 *	Used to allocate in VRAM
+	 */
+	struct Allocation {
+		byte *address;
+		uint32 size;
+		void *getEnd() { return address + size; }
+		Allocation(void *Address, uint32 Size) : address((byte *)Address), size(Size) {}
+		Allocation() : address(0), size(0) {}
+	};
+
+	enum {
+		VRAM_START_ADDRESS = 0x04000000,
+		VRAM_END_ADDRESS   = 0x04200000,
+		VRAM_SMALL_ADDRESS = VRAM_END_ADDRESS - (4 * 1024)	// 4K in the end for small allocations
+	};
+	Common::List <Allocation> _allocList;		// List of allocations
+	uint32 _bytesAllocated;
+};
+
+#endif /* PSP_MEMORY_H */
diff --git a/backends/platform/psp/psp_main.cpp b/backends/platform/psp/psp_main.cpp
index b84702115f..dba9a8fc2b 100644
--- a/backends/platform/psp/psp_main.cpp
+++ b/backends/platform/psp/psp_main.cpp
@@ -44,15 +44,13 @@
 #include "backends/plugins/psp/psp-provider.h"
 #include "backends/platform/psp/psppixelformat.h"
 #include "backends/platform/psp/osys_psp.h"
-#include "backends/platform/psp/tests.h"
+#include "backends/platform/psp/tests.h"	/* for unit/speed tests */
 #include "backends/platform/psp/trace.h"
 
 #ifdef ENABLE_PROFILING
 	#include <pspprof.h>
 #endif
 
-#define ENABLE_TESTS	/* to enable tests of PSP architecture */
-
 /**
  * Define the module info section
  *
@@ -156,7 +154,7 @@ int SetupCallbacks(void) {
 #undef main
 int main(void) {
 	//change clock rate to 333mhz
-	scePowerSetClockFrequency(222, 222, 111);
+	scePowerSetClockFrequency(333, 333, 166);
 
 	PowerManager::instance();	// Setup power manager
 
@@ -172,10 +170,11 @@ int main(void) {
 	PluginManager::instance().addPluginProvider(new PSPPluginProvider());
 #endif
 
-#ifdef ENABLE_TESTS
+/* unit/speed tests */
+#if defined (PSP_ENABLE_UNIT_TESTS) || defined (PSP_ENABLE_SPEED_TESTS)	
 	PSP_INFO_PRINT("running tests\n");
-	tests();
-	sceKernelSleepThread();
+	psp_tests();
+	sceKernelSleepThread();	// that's it. That's all we're doing
 #endif
 	
 	int res = scummvm_main(argc, argv);
diff --git a/backends/platform/psp/tests.cpp b/backends/platform/psp/tests.cpp
index 25c07fd723..d1bdb9e640 100644
--- a/backends/platform/psp/tests.cpp
+++ b/backends/platform/psp/tests.cpp
@@ -23,6 +23,13 @@
  *
  */
 
+// PSP speed and unit tests. Activate in tests.h 
+// You may also want to build without any engines. 
+ 
+#include "backends/platform/psp/tests.h"
+
+#if defined (PSP_ENABLE_UNIT_TESTS) || defined (PSP_ENABLE_SPEED_TESTS)	
+ 
 #include "common/scummsys.h"
 #include <pspiofilemgr_fcntl.h>
 #include <pspiofilemgr_stat.h>
@@ -51,7 +58,36 @@
   
 #include "backends/platform/psp/trace.h" 
 
-void test_ticks() {
+class PspSpeedTests {
+public:
+	void tickSpeed();
+	void getMicrosSpeed();
+	void seekSpeed();
+	void msReadSpeed();
+	void threadFunctionsSpeed();
+	void semaphoreSpeed(); 
+	static int threadFunc(SceSize args, void *argp);
+	void semaphoreManyThreadSpeed();
+	void fastCopySpeed();
+	
+private:
+	enum {
+		MEMCPY_BUFFER_SIZE = 8192
+	};
+	static PspSemaphore _sem;	// semaphore
+
+	void readAndTime(uint32 bytes, char *buffer, FILE *file);
+	void seekAndTime(int bytes, int origin, FILE *file);
+	void fastCopySpecificSize(byte *dst, byte *src, uint32 bytes);
+	void fastCopyDifferentSizes(byte *dst, byte *src);
+	int getThreadIdSpeed();
+	void getPrioritySpeed();
+	void changePrioritySpeed(int id, int priority);
+};
+
+PspSemaphore PspSpeedTests::_sem(0);
+
+void PspSpeedTests::tickSpeed() {
 	uint32 ticksPerSecond = sceRtcGetTickResolution();
 	PSP_INFO_PRINT("ticksPerSecond[%d]\n", ticksPerSecond);
 
@@ -75,7 +111,7 @@ void test_ticks() {
 	PSP_INFO_PRINT("time2, year[%d] month[%d] day[%d] hour[%d] minutes[%d] seconds[%d] us[%d]\n", time2.year, time2.month, time2.day, time2.hour, time2.minutes, time2.seconds, time2.microseconds);
 }
 
-void test_getMicros() {
+void PspSpeedTests::getMicrosSpeed() {
 	uint32 time1, time2, time3, time4;
 	time1 = PspRtc::instance().getMicros();
 	time2 = PspRtc::instance().getMicros();
@@ -85,7 +121,7 @@ void test_getMicros() {
 	PSP_INFO_PRINT("getMicros() times: %d, %d, %d\n", time4-time3, time3-time2, time2-time1);
 }
 
-void read_and_time(uint32 bytes, char *buffer, FILE *file) {
+void PspSpeedTests::readAndTime(uint32 bytes, char *buffer, FILE *file) {
 	uint32 time1 = PspRtc::instance().getMicros();
 	// test minimal read
 	fread(buffer, bytes, 1, file);
@@ -101,18 +137,8 @@ void read_and_time(uint32 bytes, char *buffer, FILE *file) {
 	Reading 50 byte takes 8us / 11
 	Reading 100 byte takes 8us / 11
 	Reading 1000 byte takes 915us / 1131
-	Reading 1000 byte takes 911us
-	Reading 1000 byte takes 914us
-	Reading 1000 byte takes 943us
-	Reading 1000 byte takes 915us
-	Reading 1000 byte takes 923us
-	Reading 1000 byte takes 22us
-	Reading 1000 byte takes 916us 
-	Reading 1000 byte takes 913us / 1,120
-	Reading 1000 byte takes 909us / 1,122
 	Reading 2000 byte takes 1806us / 2,284
 	Reading 3000 byte takes 2697us / 3,374
-	Reading 4000 byte takes 3652us / 4,592
 	Reading 5000 byte takes 4551us / 5,544
 	Reading 6000 byte takes 5356us / 6,676
 	Reading 7000 byte takes 6800us / 8,358
@@ -120,47 +146,40 @@ void read_and_time(uint32 bytes, char *buffer, FILE *file) {
 	Reading 9000 byte takes 6782us / 8,563
 	Reading 10000 byte takes 8497us / 10,631
 	Reading 30000 byte takes 25995us / 32,473
-	Reading 50000 byte takes 42467us / 52,893
 	Reading 80000 byte takes 68457us / 85,291
 	Reading 100000 byte takes 85103us / 106,163
-	Reading 500000 byte takes 427337us / 531,679
-	Reading 1000000 byte takes 854831us / 1,063,107
 */
 // Function to test the impact of MS reads
 // These tests can't be done from shell - the cache screws them up
-void test_ms_reads() {
+void PspSpeedTests::msReadSpeed() {
 	FILE *file;
 	file = fopen("ms0:/psp/music/track1.mp3", "r");
 
 	char *buffer = (char *)malloc(2 * 1024 * 1024);
 	
-	read_and_time(1, buffer, file);
-	read_and_time(10, buffer, file);
-	read_and_time(50, buffer, file);
-	read_and_time(100, buffer, file);
-	for (int i = 0; i< 10; i++)
-		read_and_time(1000, buffer, file);
-	read_and_time(2000, buffer, file);
-	read_and_time(3000, buffer, file);
-	read_and_time(4000, buffer, file);
-	read_and_time(5000, buffer, file);
-	read_and_time(6000, buffer, file);
-	read_and_time(7000, buffer, file);
-	read_and_time(8000, buffer, file);
-	read_and_time(9000, buffer, file);	
-	read_and_time(10000, buffer, file);
-	read_and_time(30000, buffer, file);
-	read_and_time(50000, buffer, file);
-	read_and_time(80000, buffer, file);
-	read_and_time(100000, buffer, file);
-	read_and_time(500000, buffer, file);
-	read_and_time(1000000, buffer, file);	
+	readAndTime(1, buffer, file);
+	readAndTime(10, buffer, file);
+	readAndTime(50, buffer, file);
+	readAndTime(100, buffer, file);
+	readAndTime(1000, buffer, file);
+	readAndTime(2000, buffer, file);
+	readAndTime(3000, buffer, file);
+	readAndTime(5000, buffer, file);
+	readAndTime(6000, buffer, file);
+	readAndTime(7000, buffer, file);
+	readAndTime(8000, buffer, file);
+	readAndTime(9000, buffer, file);	
+	readAndTime(10000, buffer, file);
+	readAndTime(30000, buffer, file);
+	readAndTime(50000, buffer, file);
+	readAndTime(80000, buffer, file);
+	readAndTime(100000, buffer, file);
 	
 	fclose(file);
 	free(buffer);	
 }
  
-void seek_and_time(int bytes, int origin, FILE *file) {
+void PspSpeedTests::seekAndTime(int bytes, int origin, FILE *file) {
 	char buffer[1000];
 	
 	uint32 time1 = PspRtc::instance().getMicros();
@@ -202,26 +221,26 @@ Seeking 10000 byte from 2 took 3636us
 Reading 1000 bytes took 110us
 */
 
-void test_seeks() {
+void PspSpeedTests::seekSpeed() {
 	FILE *file;
 	file = fopen("ms0:/psp/music/track1.mp3", "r");
 
-	seek_and_time(0, SEEK_SET, file);
-	seek_and_time(5, SEEK_SET, file);
-	seek_and_time(1000, SEEK_SET, file);
-	seek_and_time(100, SEEK_SET, file);
-	seek_and_time(10000, SEEK_SET, file);
-	seek_and_time(-5, SEEK_CUR, file);
-	seek_and_time(-100, SEEK_CUR, file);
-	seek_and_time(100, SEEK_CUR, file);
-	seek_and_time(0, SEEK_END, file);
-	seek_and_time(-10000, SEEK_END, file);
+	seekAndTime(0, SEEK_SET, file);
+	seekAndTime(5, SEEK_SET, file);
+	seekAndTime(1000, SEEK_SET, file);
+	seekAndTime(100, SEEK_SET, file);
+	seekAndTime(10000, SEEK_SET, file);
+	seekAndTime(-5, SEEK_CUR, file);
+	seekAndTime(-100, SEEK_CUR, file);
+	seekAndTime(100, SEEK_CUR, file);
+	seekAndTime(0, SEEK_END, file);
+	seekAndTime(-10000, SEEK_END, file);
 
 	fclose(file);
 }
 
 // 222: 5-7us
-int testGetThreadId() {
+int PspSpeedTests::getThreadIdSpeed() {
 	uint32 time1 = PspRtc::instance().getMicros();
 	int threadId = sceKernelGetThreadId();
 	uint32 time2 = PspRtc::instance().getMicros();
@@ -232,7 +251,7 @@ int testGetThreadId() {
 }
 
 // 222: 4-5us
-void testGetPriority() {
+void PspSpeedTests::getPrioritySpeed() {
 	uint32 time1 = PspRtc::instance().getMicros();
 	int priority = sceKernelGetThreadCurrentPriority();
 	uint32 time2 = PspRtc::instance().getMicros();
@@ -241,7 +260,7 @@ void testGetPriority() {
 }
 
 // 222: 9-10us
-void testChangePriority(int id, int priority) {
+void PspSpeedTests::changePrioritySpeed(int id, int priority) {
 	uint32 time1 = PspRtc::instance().getMicros();
 	sceKernelChangeThreadPriority(id, priority);
 	uint32 time2 = PspRtc::instance().getMicros();
@@ -249,15 +268,16 @@ void testChangePriority(int id, int priority) {
 	PSP_INFO_PRINT("Changing thread priority to %d for id %d took %dus\n", priority, id, time2-time1);
 }
 
-void test_thread() {
+void PspSpeedTests::threadFunctionsSpeed() {
+	// very unscientific -- just ballpark
 	int id;
-	id = testGetThreadId();
-	testGetThreadId();
-	testGetPriority();
-	testGetPriority();
-	testChangePriority(id, 30);
-	testChangePriority(id, 35);
-	testChangePriority(id, 25);
+	id = getThreadIdSpeed();
+	getThreadIdSpeed();
+	getPrioritySpeed();
+	getPrioritySpeed();
+	changePrioritySpeed(id, 30);
+	changePrioritySpeed(id, 35);
+	changePrioritySpeed(id, 25);
 	
 	// test context switch time
 	for (int i=0; i<10; i++) {
@@ -268,118 +288,12 @@ void test_thread() {
 	}	
 }
  
-int test_mem_read(uint32 words) {
-	uint32 __attribute__((aligned(64))) buffer[1024];
-	register uint32 r = 0;
+void PspSpeedTests::semaphoreSpeed() { 
+	PspSemaphore sem(1);
 	
-	// uncached
-	uint32 *pBuffer = (uint32 *)UNCACHED(buffer);
-
 	uint32 time1 = PspRtc::instance().getMicros();
 	
-	for (int i=0; i<words; i++) {
-		r += *pBuffer;
-		pBuffer++;
-	}
-	uint32 time2 = PspRtc::instance().getMicros();
-	
-	PSP_INFO_PRINT("read of %d words from uncached took %dus\n", words, time2-time1);
-	
-	// flush cache
-	sceKernelDcacheWritebackInvalidateAll();
-	
-	// cached
-	pBuffer = buffer;
-
-	time1 = PspRtc::instance().getMicros();
-	
-	for (int i=0; i<words; i++) {
-		r += *pBuffer;
-		pBuffer++;
-	}
-	time2 = PspRtc::instance().getMicros();
-	
-	PSP_INFO_PRINT("read of %d words from cached took %dus\n", words, time2-time1);
-
-	return r;
-} 
-
-void test_mem_write(uint32 words) {
-	uint32 __attribute__((aligned(64))) buffer[1024];
-	register uint32 r = 0;
-	
-	// uncached
-	uint32 *pBuffer = (uint32 *)UNCACHED(buffer);
-
-	uint32 time1 = PspRtc::instance().getMicros();
-	
-	for (int i=0; i<words; i++) {
-		*pBuffer = r;
-		pBuffer++;
-	}
-	uint32 time2 = PspRtc::instance().getMicros();
-	
-	PSP_INFO_PRINT("write of %d words to uncached took %dus\n", words, time2-time1);
-	
-	// flush cache
-	sceKernelDcacheWritebackInvalidateAll();
-
-	// cached
-	pBuffer = buffer;
-	
-	time1 = PspRtc::instance().getMicros();
-	
-	for (int i=0; i<words; i++) {
-		*pBuffer = r;
-		pBuffer++;
-	}
-	time2 = PspRtc::instance().getMicros();
-	
-	PSP_INFO_PRINT("writeof %d words to cached took %dus\n", words, time2-time1);	
-}
-
-/*
-read of 1 words from uncached took 2us
-read of 1 words from cached took 3us
-read of 64 words from uncached took 12us
-read of 64 words from cached took 6us
-read of 65 words from uncached took 12us
-read of 65 words from cached took 6us
-read of 128 words from uncached took 24us
-read of 128 words from cached took 8us
-read of 1024 words from uncached took 168us
-read of 1024 words from cached took 50us
-write of 1 words to uncached took 2us
-writeof 1 words to cached took 2us
-write of 64 words to uncached took 4us
-writeof 64 words to cached took 5us
-write of 65 words to uncached took 5us
-writeof 65 words to cached took 5us
-write of 128 words to uncached took 5us
-writeof 128 words to cached took 8us
-write of 1024 words to uncached took 32us
-writeof 1024 words to cached took 45us
-*/ 
-void test_memory() {
-	test_mem_read(1);
-	test_mem_read(64);
-	test_mem_read(65);
-	test_mem_read(128);
-	test_mem_read(1024);
-	test_mem_write(1);
-	test_mem_write(64);
-	test_mem_write(65);
-	test_mem_write(128);
-	test_mem_write(1024);	
-}
-
-void test_semaphore() { 
-	// create semaphore
-	int sem = sceKernelCreateSema("testSemaphore", 0, 1, 1, 0);
-	
-	uint32 time1 = PspRtc::instance().getMicros();
-	
-	sceKernelWaitSema(sem, 1, 0);
+	sem.take();
 	
 	uint32 time2 = PspRtc::instance().getMicros();
 	
@@ -387,140 +301,97 @@ void test_semaphore() {
 	
 	uint32 time3 = PspRtc::instance().getMicros();
 	
-	sceKernelSignalSema(sem, 1);
+	sem.give();
 	
 	uint32 time4 = PspRtc::instance().getMicros();
 	PSP_INFO_PRINT("releasing semaphore took %d us\n", time4-time3);	//10us-55us
 }
 
-int globalSem = 0;	// semaphore
-
-int threadFunc(SceSize args, void *argp) {
+int PspSpeedTests::threadFunc(SceSize args, void *argp) {
 	PSP_INFO_PRINT("thread %x created.\n", sceKernelGetThreadId());
 	
-	sceKernelWaitSema(globalSem, 1, 0);	// grab semaphore
+	_sem.take();
+	
 	PSP_INFO_PRINT("grabbed semaphore. Quitting thread\n");
 	
 	return 0;
 }
 
-void test_semaphore_many_threads() {	
-	globalSem = sceKernelCreateSema("testSemaphore2", 0, 0, 255, 0);
-
+void PspSpeedTests::semaphoreManyThreadSpeed() {	
+	
 	// create 4 threads
 	for (int i=0; i<4; i++) {
-		int thid = sceKernelCreateThread("my_thread", threadFunc, 0x18, 0x10000, THREAD_ATTR_USER, NULL);
+		int thid = sceKernelCreateThread("my_thread", PspSpeedTests::threadFunc, 0x18, 0x10000, THREAD_ATTR_USER, NULL);
 		sceKernelStartThread(thid, 0, 0);
 	}
 		
 	PSP_INFO_PRINT("main thread. created threads\n");
 
-	SceKernelSemaInfo info;
-	int waitingThreads = 0;
-	while (waitingThreads < 4) {
-		sceKernelReferSemaStatus(globalSem, &info);
-		waitingThreads = info.numWaitThreads;
-		PSP_INFO_PRINT("main thread: waiting threads[%d]\n", waitingThreads);
+	uint32 threads = _sem.numOfWaitingThreads();
+	while (threads < 4) {
+		threads = _sem.numOfWaitingThreads();
+		PSP_INFO_PRINT("main thread: waiting threads[%d]\n", threads);
 	}
 	
-	PSP_INFO_PRINT("main: semaphore value[%d]\n", info.currentCount);
-	PSP_INFO_PRINT("main thread: waiting threads[%d]\n", info.numWaitThreads);
+	PSP_INFO_PRINT("main: semaphore value[%d]\n", _sem.getValue());
+	PSP_INFO_PRINT("main thread: waiting threads[%d]\n", _sem.numOfWaitingThreads());
 	
-	sceKernelSignalSema(globalSem, 4);
+	_sem.give(4);
 }
 
-void test_sce_ms_reads() { 
-	SceUID file;
-	char *buffer = (char *)malloc(2 * 1024 * 1024);
-	{
-		// get file size
-		SceIoStat stat;
-		
-		uint32 time1 = PspRtc::instance().getMicros();
-		
-		sceIoGetstat("ms0:/psp/music/track1.mp3", &stat);
-		
-		uint32 time2 = PspRtc::instance().getMicros();
-		
-		PSP_INFO_PRINT("getting size of %lld took %d us\n", stat.st_size, time2-time1); // 3180 us before an open
-	}
-	{
-		// open file
-		uint32 time1 = PspRtc::instance().getMicros();
-
-		file = sceIoOpen("ms0:/psp/music/track1.mp3", PSP_O_RDONLY, 0777);
-		
-		uint32 time2 = PspRtc::instance().getMicros();
-		
-		PSP_INFO_PRINT("opening scefile took %d us\n", time2-time1);	// 3355us (or 1223us if already got size)
-	}
-	{
-		// get file size
-		SceIoStat stat;
-		
-		uint32 time1 = PspRtc::instance().getMicros();
-		
-		sceIoGetstat("ms0:/psp/music/track1.mp3", &stat);
-		
-		uint32 time2 = PspRtc::instance().getMicros();
-		
-		PSP_INFO_PRINT("getting size of %lld took %d us\n", stat.st_size, time2-time1); // 1223us after an open
-	}
-	{
-		uint32 time1 = PspRtc::instance().getMicros();
+void PspSpeedTests::fastCopySpecificSize(byte *dst, byte *src, uint32 bytes) {
+	uint32 time1, time2;
+	uint32 fastcopyTime, memcpyTime;
+	const int iterations = 2000;
+	int intc;
 	
-		// asynchronous read
-		sceIoReadAsync(file, buffer, 100000);
-		
-		uint32 time2 = PspRtc::instance().getMicros();
-		
-		PSP_INFO_PRINT("async read of 100000 took %d us\n", time2-time1);  // 146us
-		
-		// poll for completion
-		SceInt64 result;
-		for (int i=0; i<10; i++) {
-			sceIoPollAsync(file, &result);
-			uint time3 = PspRtc::instance().getMicros();
-			PSP_INFO_PRINT("poll %d. result = %lld. Time = %d us.\n", i, result, time3-time1);
-			PspThread::delayMicros(100);
-		}	
+	intc = pspSdkDisableInterrupts();
 	
-	}
-	{
-		uint32 time1 = PspRtc::instance().getMicros();
-		sceIoClose(file);
-		uint32 time2 = PspRtc::instance().getMicros();
-		PSP_INFO_PRINT("file close took %d us\n", time2-time1);	// 230us
-	}
-	free(buffer);
-}
-
-#define MEMCPY_BUFFER_SIZE 4096
-
-void test_fastCopy_specific(byte *dst, byte *src, uint32 bytes) {
-	memset(dst, 0, bytes);
-	PspMemory::fastCopy(dst, src, bytes);
-	PSP_INFO_PRINT("\n");
+	time1 = PspRtc::instance().getMicros();
+	for (int i=0; i<iterations; i++) {
+		PspMemory::fastCopy(dst, src, bytes);
+	}	
+	time2 = PspRtc::instance().getMicros();
+	
+	pspSdkEnableInterrupts(intc);
+	
+	fastcopyTime = time2-time1;
+	
+	intc = pspSdkDisableInterrupts();
+	
+	time1 = PspRtc::instance().getMicros();
+	for (int i=0; i<iterations; i++) {
+		memcpy(dst, src, bytes);
+	}	
+	time2 = PspRtc::instance().getMicros();
+	
+	pspSdkEnableInterrupts(intc);
+	
+	memcpyTime = time2-time1;
+	
+	PSP_INFO_PRINT("%d bytes. memcpy[%d], fastcopy[%d]\n", bytes, memcpyTime, fastcopyTime);
 }
 
-void test_fastCopy_different_sizes(byte *dst, byte *src) {
-	test_fastCopy_specific(dst, src, 1);
-	test_fastCopy_specific(dst, src, 2);
-	test_fastCopy_specific(dst, src, 4);
-	test_fastCopy_specific(dst, src, 6);
-	test_fastCopy_specific(dst, src, 8);
-	test_fastCopy_specific(dst, src, 9);
-	test_fastCopy_specific(dst, src, 16);
-	test_fastCopy_specific(dst, src, 17);
-	test_fastCopy_specific(dst, src, 32);
-	test_fastCopy_specific(dst, src, 33);
-	test_fastCopy_specific(dst, src, 34);
-	test_fastCopy_specific(dst, src, 35);
-	test_fastCopy_specific(dst, src, 36);
+void PspSpeedTests::fastCopyDifferentSizes(byte *dst, byte *src) {
+	PSP_INFO_PRINT("\nsrc[%p], dst[%p]\n", src, dst);
+	fastCopySpecificSize(dst, src, 1);
+	fastCopySpecificSize(dst, src, 2);
+	fastCopySpecificSize(dst, src, 3);
+	fastCopySpecificSize(dst, src, 4);
+	fastCopySpecificSize(dst, src, 5);
+	fastCopySpecificSize(dst, src, 8);
+	fastCopySpecificSize(dst, src, 10);
+	fastCopySpecificSize(dst, src, 16);
+	fastCopySpecificSize(dst, src, 32);
+	fastCopySpecificSize(dst, src, 50);
+	fastCopySpecificSize(dst, src, 100);
+	fastCopySpecificSize(dst, src, 500);
+	fastCopySpecificSize(dst, src, 1024);
+	fastCopySpecificSize(dst, src, 2048);
 }
 
-void test_fastcopy() {
-	PSP_INFO_PRINT("running fastcopy test\n");
+void PspSpeedTests::fastCopySpeed() {
+	PSP_INFO_PRINT("running fastCopy speed test\n");
 
 	uint32 *bufferSrc32 = (uint32 *)memalign(16, MEMCPY_BUFFER_SIZE);
 	uint32 *bufferDst32 = (uint32 *)memalign(16, MEMCPY_BUFFER_SIZE);
@@ -536,84 +407,57 @@ void test_fastcopy() {
 	
 	byte *bufferSrc = ((byte *)bufferSrc32);
 	byte *bufferDst = ((byte *)bufferDst32);
-	
-	//test_fastCopy_different_sizes(bufferDst, bufferSrc);
-	//test_fastCopy_different_sizes(bufferDst+1, bufferSrc+1);
-	//test_fastCopy_different_sizes(bufferDst+2, bufferSrc+2);
-	//test_fastCopy_different_sizes(bufferDst+3, bufferSrc+3);
-	//test_fastCopy_different_sizes(bufferDst, bufferSrc+1);
-	//test_fastCopy_different_sizes(bufferDst, bufferSrc+2);
-	//test_fastCopy_different_sizes(bufferDst, bufferSrc+3);
-	test_fastCopy_different_sizes(bufferDst+1, bufferSrc+2);
-	test_fastCopy_different_sizes(bufferDst+1, bufferSrc+3);
-	test_fastCopy_different_sizes(bufferDst+2, bufferSrc+1);
-	test_fastCopy_different_sizes(bufferDst+2, bufferSrc+3);
-	test_fastCopy_different_sizes(bufferDst+3, bufferSrc+1);
-	test_fastCopy_different_sizes(bufferDst+3, bufferSrc+2);	
+
+	PSP_INFO_PRINT("\n\ndst and src cached: -----------------\n");
+	fastCopyDifferentSizes(bufferDst, bufferSrc);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc);
+	
+	PSP_INFO_PRINT("\n\ndst cached, src uncached: -----------------\n");
+	bufferSrc = UNCACHED(bufferSrc);
+	fastCopyDifferentSizes(bufferDst, bufferSrc);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc);
+
+	PSP_INFO_PRINT("\n\ndst uncached, src uncached: --------------\n");
+	bufferDst = UNCACHED(bufferDst);
+	fastCopyDifferentSizes(bufferDst, bufferSrc);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc);
+	
+	PSP_INFO_PRINT("\n\ndst uncached, src cached: -------------------\n");
+	bufferSrc = CACHED(bufferSrc);
+	fastCopyDifferentSizes(bufferDst, bufferSrc);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc);
+
 	
 	free(bufferSrc32);
 	free(bufferDst32);
 }
 
-void test_fastcopy_speed_specific(byte *dst, byte *src, uint32 bytes) {
-	byte *dstUncached = UNCACHED(dst);
-	byte *srcUncached = UNCACHED(src);
-//	byte *dstUncached = dst;
-//	byte *srcUncached = src;
-	
-	uint32 time1, time2;
-	const int iterations = 2000;
-	int intc;
-	
-	intc = pspSdkDisableInterrupts();
-	
-	time1 = PspRtc::instance().getMicros();
-	for (int i=0; i<iterations; i++) {
-		PspMemory::fastCopy(dstUncached, srcUncached, bytes);
-	}	
-	time2 = PspRtc::instance().getMicros();
-	
-	pspSdkEnableInterrupts(intc);
-	
-	PSP_INFO_PRINT("fastcppy() results: bytes[%d], iters[%d], time[%d]us\n",
-		bytes, iterations, time2-time1);
-	
-	intc = pspSdkDisableInterrupts();
-	
-	time1 = PspRtc::instance().getMicros();
-	for (int i=0; i<iterations; i++) {
-		memcpy(dstUncached, srcUncached, bytes);
-	}	
-	time2 = PspRtc::instance().getMicros();
-	
-	pspSdkEnableInterrupts(intc);
-	
-	PSP_INFO_PRINT("memcpy() results: bytes[%d], iters[%d], time[%d]us\n\n",
-		bytes, iterations, time2-time1);
+//-------Unit Tests -------------------------------
 
-}
+class PspUnitTests {
+public:
+	void testFastCopy();
 
-void test_fastcopy_speed_bunch(byte *dst, byte *src) {
-	PSP_INFO_PRINT("new test bunch ******************\n");
-	PSP_INFO_PRINT("src[%p], dst[%p]\n", src, dst);
-	test_fastcopy_speed_specific(dst, src, 1);
-	test_fastcopy_speed_specific(dst, src, 2);
-	test_fastcopy_speed_specific(dst, src, 3);
-	test_fastcopy_speed_specific(dst, src, 4);
-	test_fastcopy_speed_specific(dst, src, 5);
-	test_fastcopy_speed_specific(dst, src, 8);
-	test_fastcopy_speed_specific(dst, src, 10);
-	test_fastcopy_speed_specific(dst, src, 16);
-	test_fastcopy_speed_specific(dst, src, 32);
-	test_fastcopy_speed_specific(dst, src, 50);
-	test_fastcopy_speed_specific(dst, src, 100);
-	test_fastcopy_speed_specific(dst, src, 500);
-	test_fastcopy_speed_specific(dst, src, 1024);
-	test_fastcopy_speed_specific(dst, src, 2048);
-}
+private:
+	enum {
+		MEMCPY_BUFFER_SIZE = 8192
+	};
+	
+	void fastCopySpecificSize(byte *dst, byte *src, uint32 bytes, bool swap = false);
+	void fastCopyDifferentSizes(byte *dst, byte *src, bool swap = false);
+};	
 
-void test_fastcopy_speed() {
-	PSP_INFO_PRINT("running fastcopy speed test\n");
+void PspUnitTests::testFastCopy() {
+	PSP_INFO_PRINT("running fastcopy unit test ***********\n");
+	PSP_INFO_PRINT("this test requires the test flag to be on in fastCopy\n\n");
 
 	uint32 *bufferSrc32 = (uint32 *)memalign(16, MEMCPY_BUFFER_SIZE);
 	uint32 *bufferDst32 = (uint32 *)memalign(16, MEMCPY_BUFFER_SIZE);
@@ -630,27 +474,92 @@ void test_fastcopy_speed() {
 	byte *bufferSrc = ((byte *)bufferSrc32);
 	byte *bufferDst = ((byte *)bufferDst32);
 	
-	test_fastcopy_speed_bunch(bufferDst, bufferSrc);
-	test_fastcopy_speed_bunch(bufferDst+1, bufferSrc+1);
-	test_fastcopy_speed_bunch(bufferDst, bufferSrc+1);
-	test_fastcopy_speed_bunch(bufferDst+1, bufferSrc);
+	fastCopyDifferentSizes(bufferDst, bufferSrc, true);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+2, bufferSrc+2, true);
+	fastCopyDifferentSizes(bufferDst+3, bufferSrc+3);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+2, true);
+	fastCopyDifferentSizes(bufferDst+2, bufferSrc, true);
+	fastCopyDifferentSizes(bufferDst, bufferSrc+3);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+2);
+	fastCopyDifferentSizes(bufferDst+1, bufferSrc+3);
+	fastCopyDifferentSizes(bufferDst+2, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+2, bufferSrc+3);
+	fastCopyDifferentSizes(bufferDst+3, bufferSrc+1);
+	fastCopyDifferentSizes(bufferDst+3, bufferSrc+2);	
 	
 	free(bufferSrc32);
 	free(bufferDst32);
 }
+	
+void PspUnitTests::fastCopyDifferentSizes(byte *dst, byte *src, bool swap) {
+	fastCopySpecificSize(dst, src, 1);
+	fastCopySpecificSize(dst, src, 2, swap);
+	fastCopySpecificSize(dst, src, 4, swap);
+	fastCopySpecificSize(dst, src, 6, swap);
+	fastCopySpecificSize(dst, src, 8, swap);
+	fastCopySpecificSize(dst, src, 9);
+	fastCopySpecificSize(dst, src, 10, swap);
+	fastCopySpecificSize(dst, src, 11);
+	fastCopySpecificSize(dst, src, 12, swap);
+	fastCopySpecificSize(dst, src, 13);
+	fastCopySpecificSize(dst, src, 14, swap);
+	fastCopySpecificSize(dst, src, 15);	
+	fastCopySpecificSize(dst, src, 16, swap);
+	fastCopySpecificSize(dst, src, 17);
+	fastCopySpecificSize(dst, src, 18, swap);
+	fastCopySpecificSize(dst, src, 19);
+	fastCopySpecificSize(dst, src, 20, swap);
+	fastCopySpecificSize(dst, src, 32, swap);
+	fastCopySpecificSize(dst, src, 33);
+	fastCopySpecificSize(dst, src, 34, swap);
+	fastCopySpecificSize(dst, src, 35);
+	fastCopySpecificSize(dst, src, 36, swap);
+	fastCopySpecificSize(dst, src, 50, swap);
+	fastCopySpecificSize(dst, src, 100, swap);
+	fastCopySpecificSize(dst, src, 500, swap);
+	fastCopySpecificSize(dst, src, 1000, swap);
+}
 
+void PspUnitTests::fastCopySpecificSize(byte *dst, byte *src, uint32 bytes, bool swap) {
+	memset(dst, 0, bytes);
+	PspMemory::fastCopy(dst, src, bytes);
+	
+	if (swap) {	// test swap also
+		memset(dst, 0, bytes);
+		
+		// pixelformat for swap
+		PSPPixelFormat format;
+		format.set(PSPPixelFormat::Type_4444, true);
+	
+		PspMemory::fastSwap(dst, src, bytes, format);
+	}	
+}
 
- void tests() {
+void psp_tests() {
 	PSP_INFO_PRINT("in tests\n");
-	//test_ticks();
-	//test_getMicros();
-	//test_ms_reads();
-	//test_seeks();
-	//test_thread();
-	//test_memory();
-	//test_sce_ms_reads();
-	//test_semaphore();	
-	//test_semaphore_many_threads();
-	//test_fastcopy();
-	test_fastcopy_speed();
+	
+#ifdef PSP_ENABLE_SPEED_TESTS
+	// Speed tests
+	PspSpeedTests speedTests;
+	speedTests.tickSpeed();
+	speedTests.getMicrosSpeed();
+	speedTests.msReadSpeed();
+	speedTests.seekSpeed();
+	speedTests.msReadSpeed();
+	speedTests.threadFunctionsSpeed();
+	speedTests.semaphoreSpeed();	
+	speedTests.semaphoreManyThreadSpeed();
+	speedTests.fastCopySpeed();
+#endif	
+	
+#ifdef PSP_ENABLE_UNIT_TESTS
+	// Unit tests
+	PspUnitTests unitTests;
+	
+	unitTests.testFastCopy();
+#endif	
 }	
+
+#endif /* (PSP_ENABLE_UNIT_TESTS) || defined (PSP_ENABLE_SPEED_TESTS) */
\ No newline at end of file
diff --git a/backends/platform/psp/tests.h b/backends/platform/psp/tests.h
index 0a4fc074e3..1518acfb4c 100644
--- a/backends/platform/psp/tests.h
+++ b/backends/platform/psp/tests.h
@@ -23,4 +23,14 @@
  *
  */
 
-extern void tests();
\ No newline at end of file
+#ifndef _PSP_TESTS_H_ 
+#define _PSP_TESTS_H_
+
+//#define PSP_ENABLE_UNIT_TESTS		// run unit tests
+//#define PSP_ENABLE_SPEED_TESTS		// run speed tests
+
+#if defined (PSP_ENABLE_UNIT_TESTS) || defined (PSP_ENABLE_SPEED_TESTS)	
+void psp_tests();
+#endif
+
+#endif /* _PSP_TESTS_H_ */
\ No newline at end of file
diff --git a/backends/platform/psp/trace.h b/backends/platform/psp/trace.h
index 5c0ea86e42..625aa60772 100644
--- a/backends/platform/psp/trace.h
+++ b/backends/platform/psp/trace.h
@@ -30,6 +30,8 @@
 
 #include "common/str.h"
 
+#define __PSP_PRINT_TO_FILE_AND_SCREEN__
+
 /* Choose to print to file/screen/both */
 #ifdef __PSP_PRINT_TO_FILE__
 	#define __PSP_PRINT__(format,...)			PspDebugTrace(false, format, ## __VA_ARGS__)