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

/* It is supposed to work at 1Mbps */
#define CLK_DELAY() do { _delay_ms(0.0004); } while(0)

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, JTAG_TDI);	\
	else					\
	    pin_lo(JTAG_PORT, JTAG_TDI);	\
	CLK_DELAY();				\
	TCK_HI();				\
	CLK_DELAY();				\
    } while(0)
#define GET_TDO() (JTAG_PIN & _BV(JTAG_TDO))

void jtag_tms(char *buf, int nbits) {
    int i;
    int byteoff, bitoff;
    int bit;
    
    for(i = 0; i < nbits; i++) {
	byteoff = i / 8;
	bitoff = i % 8;
	bit = buf[byteoff] & (1 << bitoff);
	SEND_BIT(JTAG_TMS, bit);
	TCK_LO();
    }
}

void jtag_shift(char *buf, int nbits) {
    int i;
    int byteoff, bitoff;
    int bit;
    
    for(i = 0; i < nbits; i++) {
	byteoff = i / 8;
	bitoff = i % 8;
	bit = buf[byteoff] & (1 << bitoff);
	SEND_BIT(JTAG_TDI, bit);
	TCK_LO();
    }
}

void jtag_shift_inout(char *ibuf, char *obuf, int nbits) {
    int i;
    int byteoff, bitoff;
    int bit, tdo, bitmask;
    
    for(i = 0; i < nbits; i++) {
	byteoff = i / 8;
	bitoff = i % 8;
	bitmask = 1 << bitoff;
	
	bit = ibuf[byteoff] & bitmask;
	SEND_BIT(JTAG_TDI, bit);
	tdo = GET_TDO();
	TCK_LO();
	if(tdo)
	    obuf[byteoff] |= bitmask;
	else
	    obuf[byteoff] &= ~(bitmask);
    }
}