#include <stdio.h>
#include <util/delay.h>
#include "jtag.h"
#include "avriotools.h"

/* It is supposed to work at 1Mbps */
#define CLK_DELAY()

void jtag_init(void) {
    pin_mode(&JTAG_PORT, JTAG_TCK, PM_OUTPUT);
    pin_mode(&JTAG_PORT, JTAG_TMS, PM_OUTPUT);
    pin_mode(&JTAG_PORT, JTAG_TDI, PM_OUTPUT);
    pin_mode(&JTAG_PORT, JTAG_TDO, PM_INPUT);
    pin_lo(JTAG_PORT, JTAG_TCK);
    pin_lo(JTAG_PORT, JTAG_TMS);
    pin_lo(JTAG_PORT, JTAG_TDI);
}

#define TCK_LO() pin_lo(JTAG_PORT, JTAG_TCK)
#define TCK_HI() pin_hi(JTAG_PORT, JTAG_TCK)
#define SEND_BIT(pin, bit)			\
    do {					\
	if(bit)					\
	    pin_hi(JTAG_PORT, pin);		\
	else					\
	    pin_lo(JTAG_PORT, pin);		\
	CLK_DELAY();				\
	TCK_HI();				\
	CLK_DELAY();				\
    } while(0)
#define GET_TDO() (JTAG_PIN & _BV(JTAG_TDO))

/*!
 * Before shifting registers, TAP controller must move to last state before
 * shift state.  The state machine transite to shift state when shifting
 * starts.
 */
void jtag_tms(char *buf, int nbits) {
    int i;
    int nbytes, byte;
    int byteoff, bitoff;
    int bit;
    
    nbytes = nbits / 8;
    for(i = 0; i < nbytes; i++) {
	byte = buf[i];
	
	bit = byte & 0x01;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x02;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x04;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x08;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x10;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x20;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x40;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
	
	bit = byte & 0x80;
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
    }
    
    byte = buf[i];
    nbits %= 8;
    for(i = 0; i < nbits; i++) {
	bit = byte & (1 << i);
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
    }
}

void jtag_shift(char *buf, int nbits) {
    int i;
    int nbytes, byte;
    int byteoff, bitoff;
    int bit;
    
    /* Transite to shift state.
     * \sa jtag_tms()
     */
    pin_lo(JTAG_PORT, JTAG_TMS);

    nbytes = nbits / 8;
    for(i = 0; i < nbytes; i++) {
	byte = buf[i];
	
	bit = byte & 0x01;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x02;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x04;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x08;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x10;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x20;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x40;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
	
	bit = byte & 0x80;
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
    }
    
    nbits %= 8;
    if(nbits) {
	byte = buf[i];
	for(i = 0; i < nbits; i++) {
	    bit = byte & (1 << i);
	    SEND_BIT(JTAG_TDI, bit);
	    TCK_LO();
	}
    }
}

void jtag_shift_inout(char *ibuf, char *obuf, int nbits) {
    int i, j;
    int nbytes, byte, obyte;
    int tdo;
    int byteoff, bitoff;
    int bit;
    
    /* Transite to shift state.
     * \sa jtag_tms()
     */
    pin_lo(JTAG_PORT, JTAG_TMS);

    nbytes = nbits / 8;
    for(i = 0; i < nbytes; i++) {
	byte = ibuf[i];
	obyte = 0;
	
	bit = byte & 0x01;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x01;
	else
	    obyte &= ~0x01;
	TCK_LO();
	
	bit = byte & 0x02;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x02;
	else
	    obyte &= ~0x02;
	TCK_LO();
	
	bit = byte & 0x04;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x04;
	else
	    obyte &= ~0x04;
	TCK_LO();
	
	bit = byte & 0x08;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x08;
	else
	    obyte &= ~0x08;
	TCK_LO();
	
	bit = byte & 0x10;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x10;
	else
	    obyte &= ~0x10;
	TCK_LO();
	
	bit = byte & 0x20;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x20;
	else
	    obyte &= ~0x20;
	TCK_LO();
	
	bit = byte & 0x40;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x40;
	else
	    obyte &= ~0x40;
	TCK_LO();
	
	bit = byte & 0x80;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	if(tdo)
	    obyte |= 0x80;
	else
	    obyte &= ~0x80;
	TCK_LO();

	obuf[i] = obyte;
    }
    
    nbits %= 8;
    if(nbits) {
	byte = ibuf[i];
	obyte = 0;
	for(j = 0; j < nbits; j++) {
	    bit = byte & (1 << j);
	    SEND_BIT(JTAG_TDI, bit);
	    tdo = GET_TDO();
	    if(tdo)
		obyte |= 1 << j;
	    else
		obyte &= ~(1 << j);
	    TCK_LO();
	}
	obuf[i] = obyte;
    }
}