ADPCM decoder + mode7 effect for GBA

gistfile1.c

#include <stdint.h>

#include "gba.h"

#define F3M_FREQ 16384
#define F3M_BUFLEN 273
#define ABLK_MAX 0x200

static uint8_t obuf[3][F3M_BUFLEN];
static int ablk_pos = 0;
static int ablk_pred = 0;
static int ablk_step = 0;
static int ablk_len = 512-8;
static uint32_t ablk_val = 0;
static uint8_t *adpcm_data;

const int8_t ima_index_table[16] = {
  -1, -1, -1, -1, 2, 4, 6, 8,
  -1, -1, -1, -1, 2, 4, 6, 8,
}; 

const int16_t ima_step_table[89] = { 
  7, 8, 9, 10, 11, 12, 13, 14, 16, 17, 
  19, 21, 23, 25, 28, 31, 34, 37, 41, 45, 
  50, 55, 60, 66, 73, 80, 88, 97, 107, 118, 
  130, 143, 157, 173, 190, 209, 230, 253, 279, 307,
  337, 371, 408, 449, 494, 544, 598, 658, 724, 796,
  876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066, 
  2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358,
  5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899, 
  15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767,
};

const int16_t lsintab[256] = {
0,100,200,301,401,501,601,700,799,897,995,1092,1189,1284,1379,1474,1567,1659,1751,1841,1930,2018,2105,2191,2275,2358,2439,2519,2598,2675,2750,2824,2896,2966,3034,3101,3166,3229,3289,3348,3405,3460,3513,3563,3612,3658,3702,3744,3784,3821,3856,3889,3919,3947,3973,3996,4017,4035,4051,4065,4076,4084,4091,4094,4096,4094,4091,4084,4076,4065,4051,4035,4017,3996,3973,3947,3919,3889,3856,3821,3784,3744,3702,3658,3612,3563,3513,3460,3405,3348,3289,3229,3166,3101,3034,2966,2896,2824,2750,2675,2598,2519,2439,2358,2275,2191,2105,2018,1930,1841,1751,1659,1567,1474,1379,1284,1189,1092,995,897,799,700,601,501,401,301,200,100,0,-100,-200,-301,-401,-501,-601,-700,-799,-897,-995,-1092,-1189,-1284,-1379,-1474,-1567,-1659,-1751,-1841,-1930,-2018,-2105,-2191,-2275,-2358,-2439,-2519,-2598,-2675,-2750,-2824,-2896,-2966,-3034,-3101,-3166,-3229,-3289,-3348,-3405,-3460,-3513,-3563,-3612,-3658,-3702,-3744,-3784,-3821,-3856,-3889,-3919,-3947,-3973,-3996,-4017,-4035,-4051,-4065,-4076,-4084,-4091,-4094,-4096,-4094,-4091,-4084,-4076,-4065,-4051,-4035,-4017,-3996,-3973,-3947,-3919,-3889,-3856,-3821,-3784,-3744,-3702,-3658,-3612,-3563,-3513,-3460,-3405,-3348,-3289,-3229,-3166,-3101,-3034,-2966,-2896,-2824,-2750,-2675,-2598,-2519,-2439,-2358,-2275,-2191,-2105,-2018,-1930,-1841,-1751,-1659,-1567,-1474,-1379,-1284,-1189,-1092,-995,-897,-799,-700,-601,-501,-401,-301,-200,-100
};

const uint64_t tile_src[] = {
	0x0101010101010101ULL,
	0x0101010101010101ULL,
	0x0101010101010101ULL,
	0x0101010101010101ULL,
	0x0101010101010101ULL,
	0x0101010101010101ULL,
	0x0101010101010101ULL,
	0x0101010101010101ULL,

	0x0101010101010101ULL,
	0x0102020202020201ULL,
	0x0102020202020201ULL,
	0x0102020202020201ULL,
	0x0102020202020201ULL,
	0x0102020202020201ULL,
	0x0102020202020201ULL,
	0x0101010101010101ULL,
};

volatile int mod_bump = 0;
uint16_t mode7tab[160][8];
//uint16_t mulinv[];

int32_t camposx = 0;
int32_t camposy = 0;
int32_t camrot = 0;
int32_t cam_tilt_cycle = 0;

static void adpcm_update(uint8_t *buf)
{
	int i;

	for(i = 0; i < F3M_BUFLEN; i++)
	{
		if(ablk_pos == 0)
		{
			// Read header
			ablk_pred = (int)*(int16_t *)adpcm_data; adpcm_data += 2;
			ablk_step = (int)*(uint8_t *)adpcm_data; adpcm_data += 2;
			if(ablk_step > 88) ablk_step = 88;
		}

		if((ablk_pos & 7) == 0)
		{
			// Read sample
			ablk_val = *(uint32_t *)adpcm_data; adpcm_data += 4;
		}

		// Read val
		int val = (ablk_val>>((ablk_pos&7)*4))&0xF;
		int mag = (((val&7)<<1)+1) * (int)ima_step_table[ablk_step];
		mag >>= 3;
		if(val&8) ablk_pred -= mag;
		else ablk_pred += mag;
		if(ablk_pred < -0x8000) ablk_pred = -0x8000;
		if(ablk_pred >  0x7FFF) ablk_pred =  0x7FFF;
		ablk_step += ima_index_table[val];
		if(ablk_step < 0) ablk_step = 0;
		if(ablk_step > 88) ablk_step = 88;

		// Emit
		*(buf++) = ablk_pred>>8;

		// Advance
		ablk_pos++;

		if(ablk_pos >= ablk_len) ablk_pos = 0;
	}
}

static void update_dma_mode7(void)
{
	int sy;

	int32_t rs0 = lsintab[((camrot>>8)+0)&255];
	int32_t rs1 = lsintab[((camrot>>8)+1)&255];
	int32_t rc0 = lsintab[((camrot>>8)+64)&255];
	int32_t rc1 = lsintab[((camrot>>8)+65)&255];
	int32_t rt0 = lsintab[((cam_tilt_cycle>>8)+0)&255];
	int32_t rt1 = lsintab[((cam_tilt_cycle>>8)+1)&255];

	int32_t cam_fx = ((rs1*(camrot&255)) + rs0*(256-(camrot&255)));
	int32_t cam_fz = ((rc1*(camrot&255)) + rc0*(256-(camrot&255)));
	int32_t horiz_tilt = cam_tilt_cycle+40;

	int horiz_span = 18;
	int ht0 = 0;//horiz_tilt - horiz_span;
	int ht1 = horiz_tilt + horiz_span;
	if(ht1 > 0 && ht0 < 159)
	{
		if(ht0 < 0) ht0 = 0;
		if(ht1 > 159) ht1 = 159;
		WIN0H = (0<<8) | 240;
		//int hw = 80;
		//WIN0H = ((120-hw)<<8) | (120+hw);
		WIN0V = (ht0<<8) | (ht1+1);
		WININ = 0;//(1<<1);
		WINOUT = (1<<2);
		DISPCNT |= (1<<13);
		//DISPCNT &= ~(1<<(8+2));
	} else {
		DISPCNT &= ~(1<<13);
		//DISPCNT |= (1<<(8+2));
	}

	for(sy = 0; sy < 160; sy++)
	{
		// Trace ray + get width
		int32_t dist = ((sy-horiz_tilt)<<12);
		if(dist < 0) dist = -dist;

		// TODO: faster 1/z
		// Calculate
		int32_t dsin = ((cam_fx<<10)/dist);
		int32_t dcos = ((cam_fz<<10)/dist);

		int32_t dx = dcos>>6;
		int32_t dy = -(dsin>>6);
		int32_t start_x = camposx>>12;
		int32_t start_y = camposy>>12;
		start_x += -(dx*120) + (dsin<<1);
		start_y += -(dy*120) + (dcos<<1);

		start_x += 0x20000;
		start_y += 0x20000;
		start_x &= 0x3FFFF;
		start_y &= 0x3FFFF;
		start_x -= 0x20000;
		start_y -= 0x20000;

		// Apply
		mode7tab[sy][0] = dx;
		mode7tab[sy][1] = 0;
		mode7tab[sy][2] = dy;
		mode7tab[sy][3] = 0;
		*(uint32_t *)(&(mode7tab[sy][4])) = start_x;
		*(uint32_t *)(&(mode7tab[sy][6])) = start_y;

		if(sy == 0)
		{
			// Do first write
			*(uint64_t *)&BG2PA = *(uint64_t *)&(mode7tab[0][0]);
			*(uint64_t *)&BG2X = *(uint64_t *)&(mode7tab[0][4]);
		}
	}
	
	cam_fx >>= 8;
	cam_fz >>= 8;

	camposx += cam_fx<<8;
	camposy += cam_fz<<8;

	uint16_t k = KEYINPUT;
	if(k & KEY_UP) cam_tilt_cycle -= 1;
	if(k & KEY_DOWN) cam_tilt_cycle += 1;
	if(k & KEY_LEFT) camrot += 250;
	if(k & KEY_RIGHT) camrot -= 250;
}

static void isr_handler(void)
{
	//asm("mov pc, lr;\n");
	uint16_t isr_state = IF;

	if(isr_state & 0x0001)
	{
		if(mod_bump != 0)
		{
			// Start DMA1 (sound/music)
			DMA1CNT_H = 0x0000;
			DMA1SAD = obuf[0];
			DMA1DAD = &FIFO_A;
			DMA1CNT_H = 0xB600;

			mod_bump = 0;
		}

		// Start DMA0
		DMA0CNT_H = 0x0000;
		DMA0CNT_L = 4;
		DMA0SAD = mode7tab[1];
		DMA0DAD = &BG2PA;
		DMA0CNT_H = 0
			| (3<<5)
			| (0<<7)
			| (1<<9)
			| (1<<10)
			| (2<<12)
			| (1<<15)
			| 0;

		// Acknowledge vblank
		IF = 0x0001;
		ISR_flags |= 0x0001;

		// Update DMA
		update_dma_mode7();
	}

	//asm("mov r0, #0x08000000 ; bx r0 ;\n");
}

static void wait_timer()
{
	while(!(ISR_flags & 0x0001)) {}
	ISR_flags &= ~0x0001;
}

void _start(void)
{
	int i;

	//WAITCNT = 0x431B;

	BG2CNT = 0x2000;
	DISPCNT = 0x0401;

	BG2X = 0x00000000;
	BG2Y = 0x00000000;
	BG2PA = 0x0100; BG2PB = 0x0000;
	BG2PC = 0x0000; BG2PD = 0x0100;
	
	IME = 0;
	ISR_funct = isr_handler;
	IE = 0x0001;
	DISPSTAT |= 0x0008;
	update_dma_mode7();
	IME = 1;

	// Set palette
	VPAL0[0] = 0x000F;
	VPAL0[1] = 0x0000;
	VPAL0[2] = 0x7FFF;

	// Load tiles
	for(i = 0; i < 8*2; i++)
		VRAM0D64[i] = tile_src[i];

	// Set up audio
	adpcm_data = *(uint8_t **)0x080000C4;
	adpcm_data += 0x3C;
	adpcm_update(obuf[0]);
	adpcm_update(obuf[1]);

	// Init sound properly
	SOUNDCNT_X = (1<<7);
	SOUNDCNT_L = (7<<0) | (7<<4);
	SOUNDCNT_H = (2<<0) | (1<<2) | (1<<11) | (3<<8);

	TM0CNT_H = 0;

	DMA1SAD = obuf[0];
	DMA1DAD = &FIFO_A;
	DMA1CNT_H = 0xB600;

	TM0CNT_L = 0x10000-((1<<24)/F3M_FREQ);
	TM0CNT_H = 0x0080;

	//while((*(uint8_t **)0x080000C4)[0] != 'R');

	for(;;)
	{
		wait_timer();
		adpcm_update(obuf[0]);
		// copy a bit to the end
		((uint32_t *)(obuf[2]))[0] = ((uint32_t *)(obuf[0]))[0];
		((uint32_t *)(obuf[2]))[1] = ((uint32_t *)(obuf[0]))[1];
		((uint32_t *)(obuf[2]))[2] = ((uint32_t *)(obuf[0]))[2];
		((uint32_t *)(obuf[2]))[3] = ((uint32_t *)(obuf[0]))[3];
		mod_bump = 1;
		wait_timer();
		adpcm_update(obuf[1]);
	}

	for(;;) {}
}