servo.cpp

#define PORTA [0x00]
#define PORTB [0x01]
#define PORTC [0x02]
#define PORTD [0x03]
#define DDRA [0x04]
#define DDRB [0x05]
#define DDRC [0x06]

#define PLMA [0x0A]

#define SCCR1 [0x0E]
#define SCCR2 [0x0F]

#define TIMER_CTRL [0x12]
#define INPUT_CAP_INT_EN	(0x80)
#define OUTPUT_CAP_INT_EN	(0x40)
#define TCAP1_HIGH		(0x02)
#define OUTPUT1_HIGH		(0x01)

#define TIMER_STATUS [0x13]

#define INPUT_CAP_LO [0x15]

#define OCR1_HI	[0x16]
#define OCR1_LO [0x17]
#define ACNT_HI [0x1A]
#define ACNT_LO [0x1B]

#define RAMCNT_LO [0x5B]
#define RAMCNT_HI [0x5C]
#define RAM_COUNTER word[0x5B]

void main()
{
	set_interrupt_mask();
	SP = 0xff;
	DDRB = 0;
	if (BIT3(PORTB))
		goto 11B;

	DDRA = 0;
	DDRC = 0;
	PORTB = 0x40;
	DDRB = 0x40;

	while (true);
}

11B:
	PORTA = 0x1C;
	PORTB = 0x53;
	PORTC = 0x04;
	DDRA = 0xFE;
	DDRB = 0xD1;
	DDRC = 0x04;
	for(int i = 0x50; i < 0x100; i++)
		[i] = 0;

	DDRB = 0xD5;
	[0x87] &= ~BIT6;
	[0xD4] = 0xaf;
	TIMER_CTRL = 0x00;
	clear_interrupt_mask();
	[0x7b] &= ~BIT0
	DDRB |= BIT3;
	DDRA &= ~BIT6;
	[0x5e] = 0x32;
	[0x96] = 0x0f;
	goto 15E;

156:
	[0x96] = 0x01;
	[0x90] = 0x02;

15E:
	[0x55] |= BIT2;
	PORTB &= ~BIT7;
	[0x0A] = 0xC1;
	[0x74] |= BIT7;

	while (true)
	{
		UINT8 A;
		call_0x0218();
		call_0x0236();
		call_0x11dd();
		call_0x0ec1();
		call_0x1039();
		call_0x170e();
		if ([0x52] & BIT1)
			call_0x08a4();
		if ([0x52] & BIT2)
			call_0x14b8();
		if ([0x52] & BIT3)
			call_0x0774();
		if ([0x55] & BIT2)
			call_0x08e0();
		if ([0x54] & BIT1)
			call_0x14ff();
		if ([0x54] & BIT7)
			call_0x0ff1();
		if (([0x55] & BIT3) && ([0x6f] & BIT1))
			A = call_0x081e();
		if (([0x54] & BIT2) && ([0x6f] & BIT0))
			call_0x1527();
		if (([0x54] & BIT5) && (([0x6f] & (BIT6 | BIT2 | BIT0)) == (BIT6 | BIT0)))
			call_0x0b3c();
		if (([0x54] & BIT6) && (([0x6f] & (BIT6 | BIT3 | BIT2 | BIT1)) == (BIT6 | BIT3 | BIT2 | BIT1)))
			call_0x0bed();
		call_0x0bed();
		call_0x01f5();
		call_0x08b4();
		call_0x0792();
		call_0x1323();
		call_0x0696();
		if (!([0x7b] & BIT0))
		{
			if ([0x52] & BIT0)
				call_1845();
			if ([0x52] & BIT4)
				call_183d();
			call_1851();
		}
	}
}

// 0x01f5

// 0x0218
call_0x0218()
{
	if (!([0x75] & BIT2) || ([0x52] & BIT2))
		return;
	
	A = [0x96];
	if (A == 0)
	{
		[0x55] |= BIT2;
		PORTB |= BIT0;
	}
	else if (A == 0x08)
	{
		PORTB |= BIT6 | BIT0;
		[0x75] &= ~BIT2;
		[0x6f] |= BIT3 | BIT2;
	}
}

// 0x0236
call_0x0236()
{
	if (!([0x52] & BIT5))
		return;

	if (!([0x6F] & BIT2))
	{
		[0x52] |= BIT3;
		[0x55] |= BIT2;
	}
	
	if (!([0x6F] & BIT1))
		return;

	[0x52] &= ~BIT5;
	[0x6F] &= ~BIT3;
	[0x54] |= BIT6;
	[0x73] &= ~BIT3;
	[0x73] |= BIT0;
	[0x74] |= BIT0;
}

// 0x0250
irqhandler_0x0250()
{
	[0x8F]--;
	if ([0x8F] == 0)
	{
		[0x5E] = 0x32;
		[0x87] |= (BIT5 | BIT6);
		PORTA = 0x1C;
		[0x96]++;
		TIMER_CTRL &= ~INPUT_CAP_INT_EN;
	}
	A = 0x15;
	return_from_interrupt();
}

// 0x0292
call_0x0292()
{
	// some sort of wait loop on port D bit 7 to be unset
	for (int A = 0x1A; A > 0 && !(PORTD & BIT7); A--);
}

// 0x0755
call_0x0755()
{
}

// 0x0774
call_0x0774()
{
	[0x52] &= ~BIT3;
	[0x72] &= ~BIT6;
	if (!([0x6F] & BIT2))
	{
		[0x6F] |= BIT2;
		[0x55] |= BIT2;
		if ([0x8C] != 0)
			return;
		[0x8C] = 0xAD;
	}
	else
	{
		if ([0x8C] != 0)
			return;
		[0x8C] = 0x91;
	}
	[0x8B] = 0;
}

// 0x0792
call_0x0792()
{
	UINT8 high_bits = [0x58] & (BIT6 | BIT7);
	if (high_bits == (BIT6 | BIT7))
	{
		[0x70] |= BIT0;
		[0x71] |= BIT3;
		[0x6F] |= BIT4;
	}
	else if (high_bits == 0)
	{
		if (([0x52] & (BIT6 | BIT7)) != 0)
			if (!([0x70] & BIT0))
				return;
		[0x70] &= ~BIT0;
		[0x71] &= ~BIT3;
		[0x52] &= ~(BIT6 | BIT7);
	}
	else
	{
		if(([0x52] & BIT6) || ([0x52] & BIT7))
		{
			call_0x07a3();
			[0x70] |= BIT0;
			if ([0x72] & BIT3)
				return;
			if ([0x71] & BIT3)
				return;
			if (!([0x6F] & BIT3))
				return;
			if (!([0x6F] & BIT6))
				return;
			if ([0x61] != 0)
				return;
			[0x72] |= BIT2;
			if (!([0x58] & BIT6))
				[0x72] &= ~BIT2;
			switch ([0x56] & 3)
			{
			case 0:
				[0x72] &= ~BIT6;
				[0x64] = 0xBC;
				[0x56]++;
				// intentional fall-through
			case 1:
				[0x61] = 0x19;
				break;
			case 2:
				[0x61] = 0x0C;
				break;
			case 3:
				A = 0x1E;
				X = 0;
				call_0x0cbe();
				[0x61] = 0x3E;
				return;
			}
			
			A = 0x01;
			X = 0x00;
			call_0x0cbe();
			if ([0x64] != 0)
				return;
			[0x64] = 0xFA;
			[0x56]++;
			return;
		}
	}

	[0x71] &= ~BIT4;
	[0x72] &= ~BIT3;
	[0x56] = 0;
	if ([0x6A] >= 0x3C)
		[0x6A] = 0;
}

// 0x081e
UINT8 call_0x081e()
{
	[0x6F] &= ~(BIT6 | BIT5 | BIT0);
	PORTA &= ~BIT7;
	PORTB &= ~(BIT2 | BIT0);
	DDRB |= BIT2;
	TIMER_CTRL &= ~INPUT_CAP_INT_EN;
	
	if (([0x79] & BIT2) || ([0x80] == 0 && [0x81] < 0xFA))
	{
		DDRC &= ~BIT5;
		[0x71] &= ~BIT1;
		if (([0x80] != 0) || ([0x81] != 0 && [0x81] > 0x05))
			goto_0x0867();
		else if ([0x81] != 0)
			[0x81]--;
		else
			[0x55] &= ~BIT3;
		[0x96] = 0x0B;
		[0x5E] = 0x08;
		TIMER_CTRL &= ~TCAP1_HIGH;
		return goto_0x0883(0);
	}
	else
	{
		PORTC &= ~BIT5;
		DDRC |= BIT5;
		[0x73] |= BIT1;
		return goto_0x0867();
	}
}

UINT8 goto_0x0867()
{
	[0x55] &= ~BIT3;
	[0x5A] = 0;
	[0x6E] = 0x05;
	[0x90] = 0x32;
	[0x96] = 0x0C;
	[0x5E] = 0x08;
	[0x8F] = 0x04;
	TIMER_CTRL |= TCAP1_HIGH;
	return goto_0x0883(2);
}

// 0x0883
UINT8 goto_0x0883(UINT8 newport)
{
	if (!([0x72] & BIT2))
		newport ^= 0x08;
	PORTA = newport;
	UINT16 counter1 = (ACNT_HI << 8) | ACNT_LO;
	g_ram_counter = counter1;
	counter1 += 0x384;
	OCR1_HI = (counter1 >> 8) & 0xff;
	OCR1_LO = counter1 & 0xff;
	TIMER_CTRL |= INPUT_CAP_INT_EN | OUTPUT_CAP_INT_EN;
	return TIMER_STATUS;
}

// 0x08a4
call_0x08a4()
{
	if ([0x70 & BIT5)
	{
		[0x70] &= ~BIT5;
		[0x6F] &= ~BIT4;
	}
	else
	{
		[0x70] |= BIT5;
		[0x6F] |= BIT4;
	}
	[0x52] &= ~BIT1;
}

// 0x08b4
call_0x08b4()
{
	if (!([0x6F] & (BIT7 | BIT6 | BIT4 | BIT3 | BIT0))
		return;
	if (!([0x71] & BIT0))
		return;
	if (!call_0x15c1())
		return;
	[0x54] |= BIT6;
	[0x6F] &= ~(BIT7 | BIT3);
	[0x73] |= BIT0;
	[0x73] &= ~BIT3;
	[0x74] |= BIT0;
	[0x71] &= ~BIT0;
	[0x67] = 0xFF;
}

// 0x08e0
call_0x08e0()
{
	if (([0x7B] & BIT0) && ([0x54] & BIT1))
		return;

	if (!([0x7B] & BIT0))
	{
		if ([0x98] != 5)
		{
			[0x52] |= BIT4;
			return;
		}
	}

	if (!([0x54] & BIT1))
	{
		if ([0x96] >= 0x0E)
			return;
		if ([0x96] == 0)
		{
			[0x96]++;
			[0x90] = 0x02;
		}
		else
		{
			if ([0x75] & BIT2)
			{
				[0x55] &= ~BIT2;
				return;
			}
			if ([0x6F] & BIT1)
			{
				if ([0x8C] > 0xAE)
				{
					[0x8C] = 0;
					[0x8B] = 0;
					[0x6A] = 0x37;
					goto_14b2();
				}
				[0x55] &= ~BIT2;
				if (!([0x6F] & BIT2))
					return;
				if ([0x8C] != 0)
					return;
				if ([0x91] != 0)
					return;
				[0x8C] = [0xAD];
				return;
			}
			if ([0x54] & BIT5)
				return;
			[0x54] |= BIT5;
			return;
		}
	}
}

// _0x093E
void _0x093E()
{
	set_interrupt_mask();
	[0x1FDF] = 0;
	PLMA = 0;
	TIMER_CTRL = 0;
	SCCR1 = 0;
	SCCR2 = 0;

	DDRC = 0x60;

	DDRA |= BIT4;
	DDRA |= BIT3;
	DDRA |= BIT2;
	DDRA |= BIT1;

	DDRA = 0xFF;
	DDRC = 0x60;

	while (true)
		if (PORTD & BIT7)
			PORTA |= BIT4;
		else
			PORTA &= ~BIT4;
		if (PORTD & BIT4)
			PORTA |= BIT3;
		else
			PORTA &= ~BIT3;
		if (PORTD & BIT3)
			PORTA |= BIT2;
		else
			PORTA &= ~BIT2;
		if (PORTD & BIT2)
			PORTA |= BIT1;
		else
			PORTA &= ~BIT1;
		
		UINT8 A = (PORTC >> 1) & 0x0F;
		if (A >= 0x09)
			A = 0;
		[0xB6] = A;
		A += A << 1;
		X = A;
		if (A == 8)
		{
			DDRB = 0x5D;
		}
		else
		{
			[0x75] &= ~BIT0;
			DDRB = 0x59;
		}
		
		switch (A)
		{
			case 0:	// 0x09c8
				PORTA |= (BIT7 | BIT6 | BIT5 | BIT0);
				PORTC |= BIT2;
				PORTB = 0x59;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				break;
				
			case 1:	// 0x09dc
				PORTA |= (BIT7 | BIT5 | BIT0);
				PORTA &= ~BIT6;
				PORTB = 0x51;
				PORTC |= BIT2;
				TIMER_CTRL &= ~OUTPUT1_HIGH;
				call_0x0b26();
				DDRC |= BIT6;
				if (PORTC & BIT7)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
				
			case 2:	// 0x09fa
				PORTA |= (BIT7 | BIT6 | BIT5 | BIT0);
				PORTB = 0x59;
				PORTC |= BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (PORTD & BIT7)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
				
			case 3:	// 0x0a16
				PORTA |= (BIT6 | BIT5 | BIT0);
				PORTA &= ~BIT7;
				PORTB = 0x59;
				PORTC |= BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (PORTD & BIT7)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
				
			case 4:	// 0x0a32
				PORTA |= (BIT7 | BIT6 | BIT5);
				PORTA &= ~BIT0;
				PORTB = 0x59;
				PORTC |= BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (PORTD & BIT1)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
				
			case 5:	// 0x0a4e
				PORTA |= (BIT7 | BIT6 | BIT0);
				PORTA &= ~BIT5;
				PORTB = 0x59;
				PORTC |= BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (PORTD & BIT0)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
				
			case 6:	// 0x0a6a
				PORTA |= (BIT7 | BIT6 | BIT5 | BIT0);
				PORTB = 0x19;
				PORTC |= BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (PORTD & BIT0)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
				
			case 7:	// 0x0a86
				PORTA |= (BIT7 | BIT6 | BIT5 | BIT0);
				PORTB = 0x58;
				PORTC |= BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (!(TIMER_STATUS & BIT0))
					break;
				A = TIMER_STATUS;
				A = INPUT_CAP_LO;
				if (TIMER_CTRL & TCAP1_HIGH)
				{
					PORTC |= BIT6;
					TIMER_CTRL &= TCAP1_HIGH;
				}
				else
				{
					PORTC &= ~BIT6;
					TIMER_CTRL |= TCAP1_HIGH;
				}
				break;
				
			case 8:	// 0x0aad
				PORTA |= (BIT7 | BIT6 | BIT5 | BIT0);
				PORTC |= BIT2;
				PORTB = 0x5B;
				if (!([0x75] & BIT0))
				{
					DDRB = 0x59;
					[0x75] |= BIT0;
				}
				DDRB = 0x5D;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				call_0x1527();
				if ([0x6A] != 0x07)
				{
					PORTC &= ~BIT2;
					break;
				}
				if (!([0x6F] & BIT6))
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0x99] != 0x01)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0x9A] != 0x00)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0x9B] != 0xA0)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0x9C] != 0x00)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0x9D] != 0x01)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0xA0] != 0x58)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0xA1] != 0x00)
				{
					PORTC &= ~BIT2;
					break;
				}
				if ([0xA2] != 0x00)
				{
					PORTC &= ~BIT2;
					break;
				}
				PORTC |= BIT2;
				break;
				
			case 9:	// 0x0b0a
				PORTA |= (BIT7 | BIT6 | BIT5 | BIT0);
				PORTB = 0x59;
				PORTC &= ~BIT2;
				TIMER_CTRL |= OUTPUT1_HIGH;
				call_0x0b26();
				if (PORTD & BIT7)
					PORTC |= BIT6;
				else
					PORTC &= ~BIT6;
				break;
		}
	}
	clear_interrupt_mask();
	return_from_interrupt();
}

// _0x0b26
UINT8 call_0x0b26()
{
	OCR1_HI = ACNT_HI;
	UINT16 A = ACNT_LO + 0x32;
	UINT8 X = TIMER_STATUS;

	OCR1_LO = A & 0xff;
	if (A & 0xff00)
		OCR1_HI++;
	return X;
}

// 0x0b3c
call_0x0b3c()
{
	if ([0x9A] != 0x00)
		goto_0x14b2(8);

	if (([0x99] & 0x0F) == 5)
	{
		if ([0x9B] != 0xB0)
			return;
		[0xBB] = [0xA0];
		[0xBC] = [0xA1];
		[0xBD] = [0xA2];
		[0x79] |= BIT1;
		[0x76] |= BIT4;
		return;
	}

	if ([0x9B] == 0xA0)
	{
		UINT8 _a1 = [0xA1];
		if (_a1 == 0x10)
		{
			[0x76] |= BIT1;
			[0x76] &= ~BIT3;
		}
		else if (_a1 == 0x00)
		{
			[0x76] |= BIT0;
			[0x76] &= ~BIT3;
		}
		else if (_a1 == 0x20)
		{
			[0x76] |= BIT2;
			[0x76] &= ~BIT3;
		}
		else
		{
			[0x76] |= BIT7;
			[0x76] &= ~BIT3;
		}
		if ([0xAD] != 0x00 && [0xAD] == [0x9B])
		{
			[0x72] |= BIT5;
			if ([0xA0] >= 2)
				[0x76] |= BIT6;
		}
		UINT8 A = [0x9B] ^ 0xA0;
		if (A < 3)
		{
			A += 0xAD;
			UINT8 X = A;
			[X] = [0xA0];
			if ([0x9B] & BIT1)
			{
				X++;
				[X] = [0xA1];
			}
		}
		A = [0xAD];
		if ([0xAD] == 0)
			return;
		if ([0xAE] == 0)
			return;
		if ([0xAF] == 0)
			if ([0xB0] == 0)
				return;
		if (([0x76] & 7) == 0)
			return;
		if (([0x76] & BIT0) && !([0x72] & BIT5))
			return;
		UINT8 X = 0xB0;
		call_0x1611();
		[0xB1] = X;
		[0xB2] = A;
		[0x6F] |= BIT3 | BIT1;
		[0x54] &= ~BIT5;
		[0x77] |= BIT0;
		if (!([0x79] & BIT1))
		{
			[0x6F] |= BIT2;
			return;
		}
		call_0x0755();
		return;
	}
}

// 0x0bed
call_0x0bed()
{
	if (!([0x6F] & BIT3))
	{
		if ([0x74] & (BIT2 | BIT0))
		{
			// 0x0c00
			[0x91] = [0x9A];
			if ([0x67] == 0)
			{
				[0x6A] = 0x1F;
				// 0x0c64
			}
			call_0x0d60();
			if (X == 0)
			{
				X = 0x4B;
				illegal_instruction_0x42();
			}
			goto_0x0cbe();
		}
		if ([0x71] & BIT4)
			return;
		if ([0x5F] != 0)
			return;
	}
	// 0x0c2a
}

// 0x0bed
goto_0x0bed()
{
}

// 0x0d60
call_0x0d60()
{
}

// 0x0ec1
call_0x0ec1()
{
}

// 0x0ff1
call_0x0ff1()
{
}
// 0x1039
call_0x1039()
{
}

// 0x11dd
call_0x11dd()
{
}

// 0x14b8
call_0x14b8()
{
}

// 0x14ff
call_0x14ff()
{
}

// 0x1527
call_0x1527()
{
}

// 0x15c1
bool call_0x15c1()
{
}

// 0x1611
call_0x1611()
{
}

// 0x170e
call_0x170e()
{
}

// 0x1845
call_0x183d()
{
}

// 0x1845
call_0x1845()
{
}