/*
 * Copyright(C) Paul und Scherer (mct.de/mct.net)
 *
 * This example demonstrates how to...
 *
 *  ... decode signals from a quadrature encoder.
 */

#include <stdio.h>
#include <sys/arm7tdmi.h>
#include <target.h>

#define QENC	(Intern_fio0pin3>>3&3)		// QENC input

extern char _bdata;				// data start
#define FIQ_VEC	(*((long *)&_bdata+15))		// FIQ vector

static long qp;					// QDEC pos counter

/*
 * Timer0 interrupt service (FIQ)
 *
 * Quadrature DECoder
 *
 * The encoder provides two signals (pulses),
 * which are phase-shifted by 90 degrees, and
 * so can take together 4 different states. A
 * special coding of these states (Gray code)
 * avoids multiple bit changes at transitions
 * (i.e. only ONE bit at most can change from
 * one state to another).
 *
 *  State code
 *  ----- ----
 *    1   00 =0
 *    2   01 =1
 *    3   11 =3
 *    4   10 =2
 *    1   00 =0
 *    .    .
 *    .    .
 *
 * The position counter is updated, according
 * to state transitions (+/- 1).
 *
 * The current and previous codes are used as
 * indices into an array, holding the offsets
 * to be added to the position counter (1, -1
 * or 0).
 */
static void t0_ack(void)
{
	Intern_t0ir	   = 1;			// clear int flag
	Intern_vicvectaddr = 0;			// reset VIC priority logic
}
static void __attribute__((interrupt))		// handle as ISR!
t0_isr(void)
{
	static const int qtbl[][4] =		// delta pos...
	{
		{ 0, -1,  1,  0},
		{ 1,  0,  0, -1},
		{-1,  0,  0,  1},
		{ 0,  1, -1,  0}
	};

	static int qc_old;			// QENC last code

	int qc = QENC;				// QENC cur code

	qp += qtbl[qc_old][qc];			// update pos
	qc_old = qc;				// save cur as old

	t0_ack();				// acknowledge int
}

int
main(void)
{
	long last = 0x80000000;			// last pos

	FIQ_VEC		    = (long)t0_isr;	// set ISR addr
	Intern_vicintenable = 0x10;		// enable t0 int
	Intern_vicintselect = 0x10;		// select t0 FIQ

	Intern_t0mr0  = _PCLK/100000-1;		// set MR0 to 10us
	Intern_t0mcr |= 3;			// int on MR0, reset
	Intern_t0tcr  = 1;			// go...

	Intern_scs	=  1;			// use fast I/O
	Intern_fio0mask = ~0x18000000;		// port active mask

	ENABLE_INTERRUPTS;			// enable core ints

	/*
	 * Zero adjust
	 */
	Intern_pcon = 1;			// sleep mode (wait for FIQ)
	qp = 0;					// zero pos

	/*
	 * Read the position and display changes.
	 */
	while (1) if (qp != last) printf("Pos: %ld\n", last = qp);
}