static void *__smalloc_pool(struct pool *pool, size_t size)
{
size_t nr_blocks;
unsigned int i;
unsigned int offset;
unsigned int last_idx;
void *ret = NULL;
fio_mutex_down(pool->lock);
nr_blocks = size_to_blocks(size);
if (nr_blocks > pool->free_blocks)
goto fail;
i = pool->next_non_full;
last_idx = 0;
offset = -1U;
while (i < pool->nr_blocks) {
unsigned int idx;
if (pool->bitmap[i] == -1U) {
i++;
pool->next_non_full = i;
last_idx = 0;
continue;
}
idx = find_next_zero(pool->bitmap[i], last_idx);
if (!blocks_free(pool, i, idx, nr_blocks)) {
idx += nr_blocks;
if (idx < SMALLOC_BPI)
last_idx = idx;
else {
last_idx = 0;
while (idx >= SMALLOC_BPI) {
i++;
idx -= SMALLOC_BPI;
}
}
continue;
}
set_blocks(pool, i, idx, nr_blocks);
offset = i * SMALLOC_BPL + idx * SMALLOC_BPB;
break;
}
if (i < pool->nr_blocks) {
pool->free_blocks -= nr_blocks;
ret = pool->map + offset;
}
fail:
fio_mutex_up(pool->lock);
return ret;
}
int nes_loadstate(int fp)
{
stateheader_t header;
blocknes_t *bnes;
blockcpu_t *cpu;
blockppu_t *ppu;
blockapu_t *apu;
blockvram_t *vram;
blocksvram_t *svram;
blocksram_t *sram;
blockwram_t *wram;
blockmapper_t *mapper;
blockdisk_t *disk;
int i;
void *block;
int blocktype;
//load state header
if(blocks_loadheader(fp,&header) != 0)
return(1);
//enumerate thru all the state blocks
while((block = blocks_load(fp,&blocktype)) != 0) {
switch(blocktype) {
case B_NES:
bnes = (blocknes_t*)block;
memcpy(nes->ram,bnes->ram,0x800);
memcpy(nes->nametables[0],bnes->nametables + 0x000,0x400);
memcpy(nes->nametables[1],bnes->nametables + 0x400,0x400);
memcpy(nes->nametables[2],bnes->nametables + 0x800,0x400);
memcpy(nes->nametables[3],bnes->nametables + 0xC00,0x400);
memcpy(nes->pal,bnes->palette,32);
nes->strobe = bnes->strobe;
for(i=0;i<32;i++)
pal_write(i,nes->pal[i]);
break;
case B_CPU:
cpu = (blockcpu_t*)block;
nes->cpu.a = cpu->a;
nes->cpu.x = cpu->x;
nes->cpu.y = cpu->y;
nes->cpu.s = cpu->s;
nes->cpu.f = cpu->f;
nes->cpu.pc = cpu->pc;
nes->cpu.totalcycles = cpu->cycles;
nes->cpu.needirq = cpu->irq;
break;
case B_PPU:
ppu = (blockppu_t*)block;
nes->scanline = ppu->scanline;
nes->ppu.ctrl0 = ppu->control1;
nes->ppu.ctrl1 = ppu->control2;
nes->ppu.status = ppu->status;
nes->ppu.temp = ppu->temp;
nes->ppu.scroll = ppu->scroll;
nes->ppu.scrollx = ppu->finex;
nes->ppu.toggle = ppu->toggle;
nes->ppu.buf = ppu->buffer;
nes->ppu.latch = ppu->latch;
nes->ppu.spraddr = ppu->spraddr;
memcpy(nes->sprmem,ppu->spriteram,0x100);
ppucycles = ppu->cycles;
ppuframes = ppu->frames;
break;
case B_APU:
apu = (blockapu_t*)block;
for(i=0;i<0x16;i++)
apu_write(0x4000 + i,apu->regs[i]);
break;
case B_VRAM:
vram = (blockvram_t*)block;
if(nes->rom->vram) {
memcpy(nes->rom->vram,vram->data,nes->rom->vramsize);
for(i=0;i<0x2000;i++)
ppumem_write(i,ppumem_read(i));
}
break;
case B_SVRAM:
svram = (blocksvram_t*)block;
if(nes->rom->svram)
memcpy(nes->rom->svram,svram->data,nes->rom->svramsize);
break;
case B_SRAM:
sram = (blocksram_t*)block;
if(nes->rom->sram)
memcpy(nes->rom->sram,sram->data,nes->rom->sramsize);
break;
case B_WRAM:
wram = (blockwram_t*)block;
if(nes->rom->wram)
memcpy(nes->rom->wram,wram->data,nes->rom->wramsize);
break;
case B_MAPR:
mapper = (blockmapper_t*)block;
if(nes->mapper->state)
nes->mapper->state(STATE_LOAD,mapper->data);
break;
case B_DISK:
disk = (blockdisk_t*)block;
for(i=0;i<(nes->rom->disksides * 65500);i++)
nes->rom->diskdata[i] = nes->rom->orig_diskdata[i] ^ disk->data[i];
break;
}
blocks_free(block);
log_message("loading block '%s'\n",blockids[blocktype]);
}
log_message("loaded state, %ld bytes\n",file_tell(fp));
return(0);
}
DiskImage *di_load_image(char *name) {
FILE *file;
unsigned int filesize;
size_t l, read;
DiskImage *di;
/* open image */
if (fopen_s(&file, name, "rb") != 0) {
return NULL;
}
/* get file size*/
if (fseek(file, 0, SEEK_END)) {
fclose(file);
return NULL;
}
filesize = ftell(file);
fseek(file, 0, SEEK_SET);
if ((di = malloc(sizeof(*di))) == NULL) {
fclose(file);
return NULL;
}
di->size = filesize;
/* allocate buffer for image */
if ((di->image = malloc(filesize)) == NULL) {
free(di);
fclose(file);
return NULL;
}
/* read file into buffer */
read = 0;
while (read < filesize) {
l = fread(di->image, 1, filesize - read, file);
if (l) {
read += l;
} else {
free(di->image);
free(di);
fclose(file);
return NULL;
}
}
fclose(file);
di->errinfo = NULL;
/* check image type */
switch (filesize) {
case D64ERRSIZE: /* D64 with error info */
di->errinfo = &(di->image[D64SIZE]);
case D64SIZE: /* standard D64 */
di->type = D64;
di->bam.track = 18;
di->bam.sector = 0;
di->dir = di->bam;
break;
case D71ERRSIZE: /* D71 with error info */
di->errinfo = &(di->image[D71SIZE]);
case D71SIZE:
di->type = D71;
di->bam.track = 18;
di->bam.sector = 0;
di->bam2.track = 53;
di->bam2.sector = 0;
di->dir = di->bam;
break;
case D81ERRSIZE: /* D81 with error info */
di->errinfo = &(di->image[D81SIZE]);
case D81SIZE:
di->type = D81;
di->bam.track = 40;
di->bam.sector = 1;
di->bam2.track = 40;
di->bam2.sector = 2;
di->dir.track = 40;
di->dir.sector = 0;
break;
default:
free(di->image);
free(di);
return NULL;
}
size_t filenameLen = strlen(name) + 1;
if ((di->filename = malloc(filenameLen)) == NULL) {
free(di->image);
free(di);
return NULL;
}
strcpy_s(di->filename, filenameLen, name);
di->openfiles = 0;
di->blocksfree = blocks_free(di);
di->modified = 0;
di->interleave = interleave(di->type);
set_status(di, 254, 0, 0);
return di;
}
DiskImage *di_create_image(char *name, int size) {
DiskImage *di;
if ((di = malloc(sizeof(*di))) == NULL) {
return NULL;
}
/* allocate buffer for image */
if ((di->image = malloc(size)) == NULL) {
free(di);
return NULL;
}
memset(di->image, 0, size);
di->size = size;
/* check image type */
switch (size) {
case D64ERRSIZE: /* D64 with error info */
di->errinfo = &(di->image[D64SIZE]);
case D64SIZE: /* standard D64 */
di->type = D64;
di->bam.track = 18;
di->bam.sector = 0;
di->dir = di->bam;
break;
case D71ERRSIZE: /* D71 with error info */
di->errinfo = &(di->image[D71SIZE]);
case D71SIZE:
di->type = D71;
di->bam.track = 18;
di->bam.sector = 0;
di->bam2.track = 53;
di->bam2.sector = 0;
di->dir = di->bam;
break;
case D81ERRSIZE: /* D81 with error info */
di->errinfo = &(di->image[D81SIZE]);
case D81SIZE:
di->type = D81;
di->bam.track = 40;
di->bam.sector = 1;
di->bam2.track = 40;
di->bam2.sector = 2;
di->dir.track = 40;
di->dir.sector = 0;
break;
default:
free(di->image);
free(di);
return NULL;
}
size_t filenameLen = strlen(name) + 1;
if ((di->filename = malloc(filenameLen)) == NULL) {
free(di->image);
free(di);
return NULL;
}
strcpy_s(di->filename, filenameLen, name);
di->openfiles = 0;
di->blocksfree = blocks_free(di);
di->modified = 1;
di->interleave = interleave(di->type);
set_status(di, 254, 0, 0);
return di;
}
int di_format(DiskImage *di, unsigned char *rawname, unsigned char *rawid) {
unsigned char *p;
TrackSector ts;
di->modified = 1;
/* erase disk */
if (rawid) {
memset(di->image, 0, di->size);
}
switch (di->type) {
case D64:
/* get ptr to bam */
p = di_get_ts_addr(di, di->bam);
/* setup header */
p[0] = 18;
p[1] = 1;
p[2] = 'A';
p[3] = 0;
/* clear bam */
memset(p + 4, 0, 0x8c);
/* free blocks */
for (ts.track = 1; ts.track <= di_tracks(di->type); ++ts.track) {
for (ts.sector = 0; ts.sector < di_sectors_per_track(di->type, ts.track); ++ts.sector) {
di_free_ts(di, ts);
}
}
/* allocate bam and dir */
ts.track = 18;
ts.sector = 0;
di_alloc_ts(di, ts);
ts.sector = 1;
di_alloc_ts(di, ts);
/* copy name */
memcpy(p + 0x90, rawname, 16);
/* set id */
memset(p + 0xa0, 0xa0, 2);
if (rawid) {
memcpy(p + 0xa2, rawid, 2);
}
memset(p + 0xa4, 0xa0, 7);
p[0xa5] = '2';
p[0xa6] = 'A';
/* clear unused bytes */
memset(p + 0xab, 0, 0x55);
/* clear first dir block */
memset(p + 256, 0, 256);
p[257] = 0xff;
break;
case D71:
/* get ptr to bam2 */
p = di_get_ts_addr(di, di->bam2);
/* clear bam2 */
memset(p, 0, 256);
/* get ptr to bam */
p = di_get_ts_addr(di, di->bam);
/* setup header */
p[0] = 18;
p[1] = 1;
p[2] = 'A';
p[3] = 0x80;
/* clear bam */
memset(p + 4, 0, 0x8c);
/* clear bam2 counters */
memset(p + 0xdd, 0, 0x23);
/* free blocks */
for (ts.track = 1; ts.track <= di_tracks(di->type); ++ts.track) {
if (ts.track != 53) {
for (ts.sector = 0; ts.sector < di_sectors_per_track(di->type, ts.track); ++ts.sector) {
di_free_ts(di, ts);
}
}
}
/* allocate bam and dir */
ts.track = 18;
ts.sector = 0;
di_alloc_ts(di, ts);
ts.sector = 1;
di_alloc_ts(di, ts);
/* copy name */
memcpy(p + 0x90, rawname, 16);
/* set id */
memset(p + 0xa0, 0xa0, 2);
if (rawid) {
memcpy(p + 0xa2, rawid, 2);
}
memset(p + 0xa4, 0xa0, 7);
p[0xa5] = '2';
p[0xa6] = 'A';
/* clear unused bytes */
memset(p + 0xab, 0, 0x32);
/* clear first dir block */
memset(p + 256, 0, 256);
p[257] = 0xff;
break;
case D81:
/* get ptr to bam */
p = di_get_ts_addr(di, di->bam);
/* setup header */
p[0] = 0x28;
p[1] = 0x02;
p[2] = 0x44;
p[3] = 0xbb;
p[6] = 0xc0;
/* set id */
if (rawid) {
memcpy(p + 4, rawid, 2);
}
/* clear bam */
memset(p + 7, 0, 0xfa);
/* get ptr to bam2 */
p = di_get_ts_addr(di, di->bam2);
/* setup header */
p[0] = 0x00;
p[1] = 0xff;
p[2] = 0x44;
p[3] = 0xbb;
p[6] = 0xc0;
/* set id */
if (rawid) {
memcpy(p + 4, rawid, 2);
}
/* clear bam2 */
memset(p + 7, 0, 0xfa);
/* free blocks */
for (ts.track = 1; ts.track <= di_tracks(di->type); ++ts.track) {
for (ts.sector = 0; ts.sector < di_sectors_per_track(di->type, ts.track); ++ts.sector) {
di_free_ts(di, ts);
}
}
/* allocate bam and dir */
ts.track = 40;
ts.sector = 0;
di_alloc_ts(di, ts);
ts.sector = 1;
di_alloc_ts(di, ts);
ts.sector = 2;
di_alloc_ts(di, ts);
ts.sector = 3;
di_alloc_ts(di, ts);
/* get ptr to dir */
p = di_get_ts_addr(di, di->dir);
/* copy name */
memcpy(p + 4, rawname, 16);
/* set id */
memset(p + 0x14, 0xa0, 2);
if (rawid) {
memcpy(p + 0x16, rawid, 2);
}
memset(p + 0x18, 0xa0, 5);
p[0x19] = '3';
p[0x1a] = 'D';
/* clear unused bytes */
memset(p + 0x1d, 0, 0xe3);
/* clear first dir block */
memset(p + 768, 0, 256);
p[769] = 0xff;
break;
}
di->blocksfree = blocks_free(di);
return set_status(di, 0, 0, 0);
}
