7 files changed, 613 insertions, 7 deletions

diff --git a/src/CdStream.cpp b/src/CdStream.cpp
new file mode 100644
index 00000000..f0cefda5
--- /dev/null
+++ b/src/CdStream.cpp
@@ -0,0 +1,529 @@
+#include <Windows.h>
+#include "common.h"
+#include "patcher.h"
+#include "CdStream.h"
+#include "rwcore.h"
+#include "RwHelper.h"
+
+#define CDDEBUG(f, ...)   debug ("%s: " f "\n", "cdvd_stream", __VA_ARGS__)
+#define CDTRACE(f, ...)   printf("%s: " f "\n", "cdvd_stream", __VA_ARGS__)
+
+struct CdReadInfo
+{
+	uint32 nSectorOffset;
+	uint32 nSectorsToRead;
+	void *pBuffer;
+	char field_C;
+	bool bLocked;
+	bool bInUse;
+	char _pad0;
+	int32 nStatus;
+	HANDLE hSemaphore;
+	HANDLE hFile;
+	OVERLAPPED Overlapped;
+};
+VALIDATE_SIZE(CdReadInfo, 0x30);
+
+char gCdImageNames[MAX_CDIMAGES+1][64];
+int32 gNumImages;
+int32 gNumChannels;
+
+HANDLE gImgFiles[MAX_CDIMAGES];
+
+HANDLE _gCdStreamThread;
+HANDLE gCdStreamSema;
+DWORD _gCdStreamThreadId;
+
+CdReadInfo *gpReadInfo;
+Queue gChannelRequestQ;
+
+int32 lastPosnRead;
+
+BOOL _gbCdStreamOverlapped;
+BOOL _gbCdStreamAsync;
+DWORD _gdwCdStreamFlags;
+
+
+void
+CdStreamInitThread(void)
+{
+	SetLastError(0);
+	
+	if ( gNumChannels > 0 )
+	{
+		for ( int32 i = 0; i < gNumChannels; i++ )
+		{
+			gpReadInfo[i].hSemaphore = CreateSemaphore(NULL, 0, 2, NULL);
+			
+			if ( gpReadInfo[i].hSemaphore == NULL )
+			{
+				CDTRACE("failed to create sync semaphore");
+				ASSERT(0);
+				return;
+			}
+		}
+	}
+	
+	gChannelRequestQ.items = (int32 *)LocalAlloc(LMEM_ZEROINIT, sizeof(int32) * (gNumChannels + 1));
+	gChannelRequestQ.head = 0;
+	gChannelRequestQ.tail = 0;
+	gChannelRequestQ.size = gNumChannels + 1;
+	ASSERT(gChannelRequestQ.items != NULL );
+	
+	gCdStreamSema = CreateSemaphore(NULL, 0, 5, "CdStream");
+	
+	if ( gCdStreamSema == NULL )
+	{
+		CDTRACE("failed to create stream semaphore");
+		ASSERT(0);
+		return;
+	}
+	
+	_gCdStreamThread = CreateThread(NULL, 64*1024/*64KB*/, CdStreamThread, NULL, CREATE_SUSPENDED, &_gCdStreamThreadId);
+	
+	if ( _gCdStreamThread == NULL )
+	{
+		CDTRACE("failed to create streaming thread");
+		ASSERT(0);
+		return;
+	}
+
+	SetThreadPriority(_gCdStreamThread, GetThreadPriority(GetCurrentThread()) - 1);
+		
+	ResumeThread(_gCdStreamThread);	
+}
+
+void
+CdStreamInit(int32 numChannels)
+{
+	DWORD SectorsPerCluster;
+	DWORD BytesPerSector;
+	DWORD NumberOfFreeClusters;
+	DWORD TotalNumberOfClusters;
+
+	GetDiskFreeSpace(NULL, &SectorsPerCluster, &BytesPerSector, &NumberOfFreeClusters, &TotalNumberOfClusters);
+	
+	_gdwCdStreamFlags = 0;
+	
+	if ( BytesPerSector <= CDSTREAM_SECTOR_SIZE )
+	{
+		_gdwCdStreamFlags |= FILE_FLAG_NO_BUFFERING;
+		debug("Using no buffered loading for streaming\n");
+	}
+	
+	_gbCdStreamOverlapped = TRUE;
+
+	_gdwCdStreamFlags |= FILE_FLAG_OVERLAPPED;
+	
+	_gbCdStreamAsync = FALSE;
+	
+	void *pBuffer = (void *)RwMallocAlign(CDSTREAM_SECTOR_SIZE, BytesPerSector);
+	ASSERT( pBuffer != NULL );
+	
+	SetLastError(0);
+	
+	gNumImages = 0;
+	
+	gNumChannels = numChannels;
+	
+	gpReadInfo = (CdReadInfo *)LocalAlloc(LMEM_ZEROINIT, sizeof(CdReadInfo) * numChannels);
+	ASSERT( gpReadInfo != NULL );
+	
+	CDDEBUG("read info %p", gpReadInfo);
+	
+	CdStreamAddImage("MODELS\\GTA3.IMG");
+	
+	int32 nStatus = CdStreamRead(0, pBuffer, 0, 1);
+	
+	CdStreamRemoveImages();
+	
+	if ( nStatus == STREAM_SUCCESS )
+	{
+		_gbCdStreamAsync = TRUE;
+		
+		debug("Using async loading for streaming\n");
+	}
+	else
+	{
+		_gdwCdStreamFlags &= ~FILE_FLAG_OVERLAPPED;
+		
+		_gbCdStreamOverlapped = FALSE;
+		
+		_gbCdStreamAsync = TRUE;
+		
+		debug("Using sync loading for streaming\n");
+	}
+	
+	CdStreamInitThread();
+	
+	ASSERT( pBuffer != NULL );
+	RwFreeAlign(pBuffer);
+}
+
+uint32
+GetGTA3ImgSize(void)
+{
+	ASSERT( gImgFiles[0] != NULL );
+	return (uint32)GetFileSize(gImgFiles[0], NULL);
+}
+
+void
+CdStreamShutdown(void)
+{
+	if ( _gbCdStreamAsync )
+	{
+		LocalFree(gChannelRequestQ.items);
+		CloseHandle(gCdStreamSema);
+		CloseHandle(_gCdStreamThread);
+		
+		for ( int32 i = 0; i < gNumChannels; i++ )
+			CloseHandle(gpReadInfo[i].hSemaphore);
+	}
+	
+	LocalFree(gpReadInfo);
+}
+
+
+
+int32
+CdStreamRead(int32 channel, void *buffer, uint32 offset, uint32 size)
+{
+	ASSERT( channel < gNumChannels );
+	ASSERT( buffer != NULL );
+	
+	lastPosnRead = size + offset;
+	
+	ASSERT( _GET_INDEX(offset) < MAX_CDIMAGES );
+	HANDLE hImage = gImgFiles[_GET_INDEX(offset)];
+	ASSERT( hImage != NULL );
+	
+	
+	CdReadInfo *pChannel = &gpReadInfo[channel];
+	ASSERT( pChannel != NULL );
+	
+	pChannel->hFile = hImage;
+	
+	SetLastError(0);
+	
+	if ( _gbCdStreamAsync )
+	{
+		if ( pChannel->nSectorsToRead != 0 || pChannel->bInUse )
+			return STREAM_NONE;
+		
+		pChannel->nStatus = STREAM_NONE;
+		pChannel->nSectorOffset = _GET_OFFSET(offset);
+		pChannel->nSectorsToRead = size;
+		pChannel->pBuffer = buffer;
+		pChannel->bLocked = 0;
+		
+		AddToQueue(&gChannelRequestQ, channel);
+		
+		if ( !ReleaseSemaphore(gCdStreamSema, 1, NULL) )
+			printf("Signal Sema Error\n");
+		
+		return STREAM_SUCCESS;
+	}
+	
+	if ( _gbCdStreamOverlapped )
+	{
+		ASSERT( channel < gNumChannels );
+		CdReadInfo *pChannel = &gpReadInfo[channel];
+		ASSERT( pChannel != NULL );
+		
+		pChannel->Overlapped.Offset = _GET_OFFSET(offset) * CDSTREAM_SECTOR_SIZE;
+		
+		if ( !ReadFile(hImage, buffer, size * CDSTREAM_SECTOR_SIZE, NULL, &pChannel->Overlapped)
+					&& GetLastError() != ERROR_IO_PENDING )
+			return STREAM_NONE;
+		else
+			return STREAM_SUCCESS;
+	}
+	
+	SetFilePointer(hImage, _GET_OFFSET(offset) * CDSTREAM_SECTOR_SIZE, NULL, FILE_BEGIN);
+	
+	DWORD NumberOfBytesRead;
+	
+	if ( !ReadFile(hImage, buffer, size * CDSTREAM_SECTOR_SIZE, &NumberOfBytesRead, NULL) )
+		return STREAM_NONE;
+	else
+		return STREAM_SUCCESS;
+}
+
+int32
+CdStreamGetStatus(int32 channel)
+{
+	ASSERT( channel < gNumChannels );
+	CdReadInfo *pChannel = &gpReadInfo[channel];
+	ASSERT( pChannel != NULL );
+	
+	if ( _gbCdStreamAsync )
+	{
+		if ( pChannel->bInUse )
+			return STREAM_READING;
+		
+		if ( pChannel->nSectorsToRead != 0 )
+			return STREAM_WAITING;
+		
+		if ( pChannel->nStatus != STREAM_NONE )
+		{
+			int32 status = pChannel->nStatus;
+			
+			pChannel->nStatus = STREAM_NONE;
+			
+			return status;
+		}
+		
+		return STREAM_NONE;
+	}
+	
+	if ( _gbCdStreamOverlapped )
+	{
+		ASSERT( pChannel->hFile != NULL );
+		if ( WaitForSingleObjectEx(pChannel->hFile, 0, TRUE) == WAIT_OBJECT_0 )
+			return STREAM_NONE;
+		else
+			return STREAM_READING;
+	}
+	
+	return STREAM_NONE;
+}
+
+int32
+CdStreamGetLastPosn(void)
+{
+	return lastPosnRead;
+}
+
+int32
+CdStreamSync(int32 channel)
+{
+	ASSERT( channel < gNumChannels );
+	CdReadInfo *pChannel = &gpReadInfo[channel];
+	ASSERT( pChannel != NULL );
+	
+	if ( _gbCdStreamAsync )
+	{
+		if ( pChannel->nSectorsToRead != 0 )
+		{
+			pChannel->bLocked = true;
+
+			ASSERT( pChannel->hSemaphore != NULL );
+
+			WaitForSingleObject(pChannel->hSemaphore, INFINITE);
+		}
+
+		pChannel->bInUse = false;
+		
+		return pChannel->nStatus;
+	}
+	
+	DWORD NumberOfBytesTransferred;
+
+	if ( _gbCdStreamOverlapped && pChannel->hFile )
+	{
+		ASSERT(pChannel->hFile != NULL );
+		if ( GetOverlappedResult(pChannel->hFile, &pChannel->Overlapped, &NumberOfBytesTransferred, TRUE) )
+			return STREAM_NONE;
+		else
+			return STREAM_ERROR;
+	}
+	
+	return STREAM_NONE;
+}
+
+void
+AddToQueue(Queue *queue, int32 item)
+{
+	ASSERT( queue != NULL );
+	ASSERT( queue->items != NULL );
+	queue->items[queue->tail] = item;
+	
+	queue->tail = (queue->tail + 1) % queue->size;
+	
+	if ( queue->head == queue->tail )
+		debug("Queue is full\n");
+}
+
+int32 
+GetFirstInQueue(Queue *queue)
+{
+	ASSERT( queue != NULL );
+	if ( queue->head == queue->tail )
+		return -1;
+
+	ASSERT( queue->items != NULL );
+	return queue->items[queue->head];
+}
+
+void
+RemoveFirstInQueue(Queue *queue)
+{	
+	ASSERT( queue != NULL );
+	if ( queue->head == queue->tail )
+	{
+		debug("Queue is empty\n");
+		return;
+	}
+
+	queue->head = (queue->head + 1) % queue->size;
+}
+
+DWORD
+WINAPI CdStreamThread(LPVOID lpThreadParameter)
+{
+	debug("Created cdstream thread\n");
+	
+	while ( true )
+	{
+		WaitForSingleObject(gCdStreamSema, INFINITE);
+		
+		int32 channel = GetFirstInQueue(&gChannelRequestQ);
+		ASSERT( channel < gNumChannels );
+		
+		CdReadInfo *pChannel = &gpReadInfo[channel];
+		ASSERT( pChannel != NULL );
+		
+		pChannel->bInUse = true;
+		
+		if ( pChannel->nStatus == STREAM_NONE )
+		{
+			if ( _gbCdStreamOverlapped )
+			{
+				pChannel->Overlapped.Offset = pChannel->nSectorOffset * CDSTREAM_SECTOR_SIZE;
+				
+				ASSERT(pChannel->hFile != NULL );
+				ASSERT(pChannel->pBuffer != NULL );
+				
+				DWORD NumberOfBytesTransferred;
+				
+				if ( ReadFile(pChannel->hFile,
+						pChannel->pBuffer,
+						pChannel->nSectorsToRead * CDSTREAM_SECTOR_SIZE,
+						NULL,
+						&pChannel->Overlapped) )
+				{
+					pChannel->nStatus = STREAM_NONE;
+				}
+				else if ( GetLastError() == ERROR_IO_PENDING
+						&& GetOverlappedResult(pChannel->hFile, &pChannel->Overlapped, &NumberOfBytesTransferred, TRUE) )
+				{
+					pChannel->nStatus = STREAM_NONE;
+				}
+				else
+				{
+					pChannel->nStatus = STREAM_ERROR;
+				}
+			}
+			else
+			{
+				ASSERT(pChannel->hFile != NULL );
+				ASSERT(pChannel->pBuffer != NULL );
+				
+				SetFilePointer(pChannel->hFile, pChannel->nSectorOffset * CDSTREAM_SECTOR_SIZE, NULL, FILE_BEGIN);
+
+				DWORD NumberOfBytesRead;
+				if ( ReadFile(pChannel->hFile,
+						pChannel->pBuffer,
+						pChannel->nSectorsToRead * CDSTREAM_SECTOR_SIZE,
+						&NumberOfBytesRead,
+						NULL) )
+				{
+					pChannel->nStatus = STREAM_NONE;
+				}
+			}
+		}
+		
+		RemoveFirstInQueue(&gChannelRequestQ);
+		
+		pChannel->nSectorsToRead = 0;
+		
+		if ( pChannel->bLocked )
+		{
+			ASSERT( pChannel->hSemaphore != NULL );
+			ReleaseSemaphore(pChannel->hSemaphore, 1, NULL);
+		}
+		
+		pChannel->bInUse = false;
+	}
+}
+
+bool
+CdStreamAddImage(char const *path)
+{
+	ASSERT(path != NULL);
+	ASSERT(gNumImages < MAX_CDIMAGES);
+	
+	SetLastError(0);
+	
+	gImgFiles[gNumImages] = CreateFile(path,
+									GENERIC_READ,
+									FILE_SHARE_READ,
+									NULL,
+									OPEN_EXISTING,
+									_gdwCdStreamFlags | FILE_FLAG_RANDOM_ACCESS | FILE_ATTRIBUTE_READONLY,
+									NULL);
+	
+	ASSERT( gImgFiles[gNumImages] != NULL );
+	if ( gImgFiles[gNumImages] == NULL )
+		return false;
+	
+	strcpy(gCdImageNames[gNumImages], path);
+	
+	gNumImages++;
+	
+	return true;
+}
+
+char *
+CdStreamGetImageName(int32 cd)
+{
+	ASSERT(cd < MAX_CDIMAGES);
+	if ( gImgFiles[cd] != NULL )
+		return gCdImageNames[cd];
+	
+	return NULL;
+}
+
+void
+CdStreamRemoveImages(void)
+{	
+	for ( int32 i = 0; i < gNumChannels; i++ )
+		CdStreamSync(i);
+	
+	for ( int32 i = 0; i < gNumImages; i++ )
+	{
+		SetLastError(0);
+		
+		CloseHandle(gImgFiles[i]);
+		gImgFiles[i] = NULL;
+	}
+	
+	gNumImages = 0;
+}
+
+int32
+CdStreamGetNumImages(void)
+{	
+	return gNumImages;
+}
+
+
+STARTPATCHES
+	InjectHook(0x405B50, CdStreamInitThread, PATCH_JUMP);
+	InjectHook(0x405C80, CdStreamInit, PATCH_JUMP);
+	//InjectHook(0x405DB0, debug, PATCH_JUMP);
+	InjectHook(0x405DC0, GetGTA3ImgSize, PATCH_JUMP);
+	InjectHook(0x405DD0, CdStreamShutdown, PATCH_JUMP);
+	InjectHook(0x405E40, CdStreamRead, PATCH_JUMP);
+	InjectHook(0x405F90, CdStreamGetStatus, PATCH_JUMP);
+	InjectHook(0x406000, CdStreamGetLastPosn, PATCH_JUMP);
+	InjectHook(0x406010, CdStreamSync, PATCH_JUMP);
+	InjectHook(0x4060B0, AddToQueue, PATCH_JUMP);
+	InjectHook(0x4060F0, GetFirstInQueue, PATCH_JUMP);
+	InjectHook(0x406110, RemoveFirstInQueue, PATCH_JUMP);
+	InjectHook(0x406140, CdStreamThread, PATCH_JUMP);
+	InjectHook(0x406270, CdStreamAddImage, PATCH_JUMP);
+	InjectHook(0x4062E0, CdStreamGetImageName, PATCH_JUMP);
+	InjectHook(0x406300, CdStreamRemoveImages, PATCH_JUMP);
+	InjectHook(0x406370, CdStreamGetNumImages, PATCH_JUMP);
+ENDPATCHES
\ No newline at end of file
diff --git a/src/CdStream.h b/src/CdStream.h
new file mode 100644
index 00000000..80c7874f
--- /dev/null
+++ b/src/CdStream.h
@@ -0,0 +1,47 @@
+#pragma once
+
+#define MAX_CDIMAGES 8
+#define CDSTREAM_SECTOR_SIZE 2048
+
+#define _GET_INDEX(a) (a >> 24)
+#define _GET_OFFSET(a)  (a & 0xFFFFFF)
+
+enum
+{
+	STREAM_NONE            = uint8( 0),
+	STREAM_SUCCESS         = uint8( 1),
+	STREAM_READING         = uint8(-1), // 0xFF,
+	STREAM_ERROR           = uint8(-2), // 0xFE,
+	STREAM_ERROR_NOCD      = uint8(-3), // 0xFD,
+	STREAM_ERROR_WRONGCD   = uint8(-4), // 0xFC,
+	STREAM_ERROR_OPENCD    = uint8(-5), // 0xFB,
+	STREAM_WAITING         = uint8(-6)  // 0xFA,
+};
+
+struct Queue
+{
+	int32 *items;
+	int32 head;
+	int32 tail;
+	int32 size;
+};
+
+VALIDATE_SIZE(Queue, 0x10);
+
+
+void CdStreamInitThread(void);
+void CdStreamInit(int32 numChannels);
+uint32 GetGTA3ImgSize(void);
+void CdStreamShutdown(void);
+int32 CdStreamRead(int32 channel, void *buffer, uint32 offset, uint32 size);
+int32 CdStreamGetStatus(int32 channel);
+int32 CdStreamGetLastPosn(void);
+int32 CdStreamSync(int32 channel);
+void AddToQueue(Queue *queue, int32 item);
+int32 GetFirstInQueue(Queue *queue);
+void RemoveFirstInQueue(Queue *queue);
+DWORD WINAPI CdStreamThread(LPVOID lpThreadParameter);
+bool CdStreamAddImage(char const *path);
+char *CdStreamGetImageName(int32 cd);
+void CdStreamRemoveImages(void);
+int32 CdStreamGetNumImages(void);
\ No newline at end of file
diff --git a/src/Pad.cpp b/src/Pad.cpp
index ab00e88f..a563e26c 100644
--- a/src/Pad.cpp
+++ b/src/Pad.cpp
@@ -1995,7 +1995,10 @@ STARTPATCHES
 	InjectHook(0x492720, CPad::UpdatePads, PATCH_JUMP);
 	InjectHook(0x492C60, &CPad::ProcessPCSpecificStuff, PATCH_JUMP);
 	InjectHook(0x492C70, &CPad::Update, PATCH_JUMP);
+#pragma warning( push )
+#pragma warning( disable : 4573)
 	InjectHook(0x492F00, (void (*)())CPad::DoCheats, PATCH_JUMP);
+#pragma warning( pop )
 	InjectHook(0x492F20, (void (CPad::*)(int16))&CPad::DoCheats, PATCH_JUMP);
 	InjectHook(0x492F30, CPad::StopPadsShaking, PATCH_JUMP);
 	InjectHook(0x492F50, &CPad::StopShaking, PATCH_JUMP);
diff --git a/src/RwHelper.cpp b/src/RwHelper.cpp
index 6a8c7530..5aa31e92 100644
--- a/src/RwHelper.cpp
+++ b/src/RwHelper.cpp
@@ -4,6 +4,34 @@
 #include "TimeCycle.h"
 #include "skeleton.h"
 
+void *
+RwMallocAlign(RwUInt32 size, RwUInt32 align)
+{
+	void *mem = (void *)malloc(size + align);
+	
+	ASSERT(mem != NULL);
+	
+	void *addr = (void *)((((RwUInt32)mem) + align) & ~(align - 1));
+	
+	ASSERT(addr != NULL);
+	
+	*(((void **)addr) - 1) = mem;
+	
+	return addr;
+}
+
+void
+RwFreeAlign(void *mem)
+{
+	ASSERT(mem != NULL);
+	
+	void *addr = *(((void **)mem) - 1);
+	
+	ASSERT(addr != NULL);
+	
+	free(addr);
+}
+
 void
 DefinedState(void)
 {
diff --git a/src/RwHelper.h b/src/RwHelper.h
index 2dbfd3ce..e0ec00a3 100644
--- a/src/RwHelper.h
+++ b/src/RwHelper.h
@@ -1,5 +1,8 @@
 #pragma once
 
+void *RwMallocAlign(RwUInt32 size, RwUInt32 align);
+void RwFreeAlign(void *mem);
+
 void DefinedState(void);
 RwFrame *GetFirstChild(RwFrame *frame);
 RwObject *GetFirstObject(RwFrame *frame);
diff --git a/src/animation/AnimBlendAssociation.cpp b/src/animation/AnimBlendAssociation.cpp
index eb7019ab..0e8d3d09 100644
--- a/src/animation/AnimBlendAssociation.cpp
+++ b/src/animation/AnimBlendAssociation.cpp
@@ -5,10 +5,7 @@
 #include "RpAnimBlend.h"
 #include "AnimManager.h"
 #include "AnimBlendAssociation.h"
-
-// TODO: implement those
-#define RwFreeAlign RwFree
-#define RwMallocAlign(sz, algn) RwMalloc(sz)
+#include "RwHelper.h"
 
 CAnimBlendAssociation::CAnimBlendAssociation(void)
 {
@@ -57,6 +54,7 @@ CAnimBlendAssociation::AllocateAnimBlendNodeArray(int n)
 void
 CAnimBlendAssociation::FreeAnimBlendNodeArray(void)
 {
+	ASSERT( nodes != NULL );
 	RwFreeAlign(nodes);
 }
 
diff --git a/src/animation/AnimBlendClumpData.cpp b/src/animation/AnimBlendClumpData.cpp
index 73e71246..06625eb5 100644
--- a/src/animation/AnimBlendClumpData.cpp
+++ b/src/animation/AnimBlendClumpData.cpp
@@ -1,10 +1,8 @@
 #include "common.h"
 #include "patcher.h"
 #include "AnimBlendClumpData.h"
+#include "RwHelper.h"
 
-// TODO: implement those
-#define RwFreeAlign RwFree
-#define RwMallocAlign(sz, algn) RwMalloc(sz)
 
 CAnimBlendClumpData::CAnimBlendClumpData(void)
 {