usbloadergx/source/ramdisc/ramdisc.cpp

#include <string.h>
#include "ramdisc.h"

static inline u16 u8array_to_u16 (const u8* item, int offset)
{
	return ( item[offset] | (item[offset + 1] << 8));
}
static inline u32 u8array_to_u32 (const u8* item, int offset) 
{
	return ( item[offset] | (item[offset + 1] << 8) | (item[offset + 2] << 16) | (item[offset + 3] << 24) );
}
static inline void u16_to_u8array (u8* item, int offset, u16 value)
{
	item[offset] = (u8) value; item[offset + 1] = (u8)(value >> 8); 
}
static inline void u32_to_u8array (u8* item, int offset, u32 value)
{
	item[offset] = (u8) value;
	item[offset + 1] = (u8)(value >> 8);
	item[offset + 2] = (u8)(value >> 16);
	item[offset + 3] = (u8)(value >> 24);
} 


class padding
{
public:
	padding(u32 Start, u32 NumSectors)
	{
		start = Start;
		end = Start + NumSectors;
		data = new u8[NumSectors*512];
		next = 0;
	}
	~padding() { delete [] data;}

	u32 start;
	u32 end;
	u8 *data;
 	padding *next;
};

static u32 __ramdisk_sectorsOfPadding;
static padding *__ramdisk_data_start;
static padding *__ramdisk_data_end;

static bool __ramdisk_IsInserted(void)
{
	return __ramdisk_data_start!=NULL;
}
//forward decleration
static bool __ramdisk_WriteSectors(size_t Sector, size_t numSectors, u8 *Buffer);

bool initRAMDisc(u32 Size, u32 Padding)
{
	if(__ramdisk_data_start) return true; // is init

	if(Size > 16*1024*1024)	Size = 16*1024*1024; // maximum 16 MB
	if(Size < 16*1024)		Size = 16*1024; // minimum 16 MB
	if(Padding > Size)		Padding = Size; // Padding maximum =Disksize
	if(Padding < 4*1024)	Padding = 4*1024; // Padding minimum 4kB
	
	__ramdisk_sectorsOfPadding = Padding/512;

	__ramdisk_data_start = __ramdisk_data_end = new padding(0, __ramdisk_sectorsOfPadding);
	if(!__ramdisk_data_start) return false;

	// FAT12 Formatieren
	u8 sector[512] = {0, };
	sector[0x00d]	= 2; 	/*BPB_sectorsPerCluster*/
	sector[0x00e]	= 1; 	/*BPB_reservedSectors*/
	sector[0x010]	= 1; 	/*BPB_numFATs*/
	u16_to_u8array (sector, 0x011, 48);	/*BPB_rootEntries*/
	int num_sectors = Size/512;
	u16_to_u8array (sector, 0x013, num_sectors);	/*BPB_numSectorsSmall*/
	int num_clusters = (num_sectors-1-3) /2;
	int sectors_per_fat = (num_clusters * 3 + 1023) /1024;
	u16_to_u8array (sector, 0x016, sectors_per_fat);	/*BPB_sectorsPerFAT*/
	//u32_to_u8array (sector, 0x020, Size/512);	/*BPB_numSectors*/
	sector[0x036] 	= 'F';
	sector[0x037] 	= 'A';
	sector[0x038] 	= 'T';
	sector[0x1fe] 	= 0x55;
	sector[0x1ff] 	= 0xaa;
	if(!__ramdisk_WriteSectors(0, 1, sector))
		goto error;
	memset(sector, 0, 512);
	// clear FAT & rootDir
	for(int i=1; i<= sectors_per_fat+3/*sectors_per_rootDir*/; i++)
		if(!__ramdisk_WriteSectors(1, 1, sector))
			goto error;
	return true;
error:
	delete __ramdisk_data_start;
	__ramdisk_data_start = 0;
	return false;

}

void exitRAMDisc()
{
	while(__ramdisk_data_start)
	{
		padding *tmp = __ramdisk_data_start;
		__ramdisk_data_start = __ramdisk_data_start->next;
		delete tmp;	
	}
}
static u8 *__ramdisk_findSector(size_t Sector, size_t *Sectors)
{
	if(__ramdisk_data_start==NULL) return NULL;
	for(padding *tmp = __ramdisk_data_start; tmp; tmp=tmp->next)
	{
		if(tmp->start <= Sector && tmp->end >= Sector)
		{
			if(Sectors) *Sectors = 1+tmp->end-Sector;
			return &(tmp->data[(Sector-tmp->start)*512]);
		}
	}
	// not found -> add padding
	__ramdisk_data_end->next = new padding((Sector/__ramdisk_sectorsOfPadding)*__ramdisk_sectorsOfPadding, __ramdisk_sectorsOfPadding);
	if(__ramdisk_data_end->next)
	{
		__ramdisk_data_end = __ramdisk_data_end->next;
		return &( __ramdisk_data_end->data[(Sector-__ramdisk_data_end->start)*512]);
	}
	return 0;
}

static bool __ImplizitInit = false;
static bool __ramdisk_Startup(void)
{
	if(!__ramdisk_IsInserted())
	{
		// Std=8MB/64kB Padding
		return (__ImplizitInit = initRAMDisc(8*1024*1024, 64 * 1024));
	}
	return true;
}

static bool __ramdisk_ReadSectors(size_t Sector, size_t numSectors, u8 *Buffer)
{
	size_t num_sectors;
	while(numSectors)
	{
		if(u8 *buff = __ramdisk_findSector(Sector,&num_sectors))
		{
			if(num_sectors > numSectors) num_sectors = numSectors;
			memcpy(Buffer, buff, num_sectors * 512);
			numSectors -= num_sectors;
			Buffer+= num_sectors;
		}
		else
			return false;
	}
	return true;
}
static bool __ramdisk_WriteSectors(size_t Sector, size_t numSectors, u8 *Buffer)
{
	size_t num_sectors;
	while(numSectors)
	{
		if(u8 *buff = __ramdisk_findSector(Sector,&num_sectors))
		{
			if(num_sectors > numSectors) num_sectors = numSectors;
			memcpy(buff, Buffer, num_sectors * 512);
			numSectors -= num_sectors;
			Buffer+= num_sectors;
		}
		else
			return false;
	}
	return true;
}
static bool __ramdisk_ClearStatus(void)
{
	return true;
}
static bool __ramdisk_Shutdown(void)
{
	if(__ImplizitInit)
	{
		__ImplizitInit = false;
		exitRAMDisc();
	}
	return true;
}

const DISC_INTERFACE __io_ramdisk = {
	DEVICE_TYPE_RAM_DISK,
	FEATURE_MEDIUM_CANREAD | FEATURE_MEDIUM_CANWRITE | 0x1000,
	(FN_MEDIUM_STARTUP)&__ramdisk_Startup,
	(FN_MEDIUM_ISINSERTED)&__ramdisk_IsInserted,
	(FN_MEDIUM_READSECTORS)&__ramdisk_ReadSectors,
	(FN_MEDIUM_WRITESECTORS)&__ramdisk_WriteSectors,
	(FN_MEDIUM_CLEARSTATUS)&__ramdisk_ClearStatus, 
	(FN_MEDIUM_SHUTDOWN)&__ramdisk_Shutdown
};