ATtinyのUSIでARM Serial Wire Debug(1)

#include <avr/iotnx4.h>

typedef union {
    uint32_t asU32;
    uint8_t asU8[4];
} u32u8_t;

/*SWD command definition*/
#define APnDP_DP (0)
#define APnDP_AP (1)
#define RnW_W (0)
#define RnW_R (1)

#define ACK_OK           (0x80)
#define ACK_WAIT         (0x40)
#define ACK_FAULT        (0x20)
#define ACK_DISCONNECT_H (0xE0)
#define ACK_DISCONNECT_L (0x00)
#define ACK_MASK         (0xE0)

#define ACK_READ_PARITY_MASK (0x10)
#define ACK_READ_PARITY_OK   (0x00)
#define ACK_READ_PARITY_ERR  (0x10)

#define ACK_ACCAP_ACKINFO_MASK  (0x0C)
#define ACK_ACCAP_ACKINFO_OK    (0x00) 
#define ACK_ACCAP_ACKINFO_FAIL1 (0x04) 
#define ACK_ACCAP_ACKINFO_FAIL2 (0x08) 
#define ACK_ACCAP_ACKINFO_FAIL3 (0x0C) 

#define PIN_SWDI (0x40)
#define PIN_SWDO (0x20)
#define PIN_SWCLK (0x10)
#define PIN_NRST (0x08)

volatile uint8_t txCompleted;
volatile uint8_t rxUSIDR;
uint8_t txBuf[32];
uint8_t rxBuf[32];

ISR(TIM1_COMPA_vect){
    USICR|=0x01;/*Toggle Clock Port Pin*/
}
ISR(USI_OVF_vect){
    TIMSK1&=~(0x02);/*clear Timer/Counter1 Output Compare A Match Interrupt Enable*/
    TCCR1B&=~(0x01);
    USISR=0x40;/*Write 1 to clear Counter Overflow Interrupt Flag*/
    txCompleted=1;
    rxUSIDR=USIDR;
}

uint8_t USISR_val[]={0xEE,0xEC,0xEA,0xE8,0xE6,0xE4,0xE2,0xE0};

void Send_or_Receive_Data(uint8_t tx_or_rx,uint8_t dataLength_in_bit,uint8_t NRST_const){
    uint8_t data_num_remain,byte_pos,data_num_tx;
    if(txCompleted==1){
        if(NRST_const){
            PORTA|=PIN_NRST;
        }else{
            PORTA&=~PIN_NRST;
        }
        data_num_remain=dataLength_in_bit;
        byte_pos=0;
        while(data_num_remain>0){
            data_num_tx=data_num_remain;
            if(data_num_remain>8){
                data_num_tx=8;
            }
            data_num_remain=data_num_remain-data_num_tx;
            if(tx_or_rx){
                USICR=0x5E;
                rxBuf[byte_pos]=0x00;
            }else{
                USICR=0x5A;
            }
            
            USIDR = txBuf[byte_pos];
            USISR=USISR_val[data_num_tx-1];
            txCompleted=0;
            TIFR1|=0x02;
            TCCR1B|=0x01;
            TIMSK1=0x02;
            while(txCompleted==0){
                //
            }
            rxBuf[byte_pos]=rxUSIDR<<(8-data_num_tx);
            byte_pos=byte_pos+1;
        }
    }else{
        rxBuf[0]=0x00;
    }
}

uint8_t flip_8bit(uint8_t src,uint8_t num_bit){
    uint8_t ret=0x00;
    if(src&0x80){ret|=0x01;}
    if(src&0x40){ret|=0x02;}
    if(src&0x20){ret|=0x04;}
    if(src&0x10){ret|=0x08;}
    if(src&0x08){ret|=0x10;}
    if(src&0x04){ret|=0x20;}
    if(src&0x02){ret|=0x40;}
    if(src&0x01){ret|=0x80;}
    return ret<<(8-num_bit);
}
void JTAG_to_SWD(uint8_t NRST_const){
    int i,j;
    for(i=0,j=0;j<7;i++,j++){
        txBuf[i]=0x00;
    }
    for(j=0;j<7;i++,j++){
        txBuf[i]=0xFF;
    }
    txBuf[i+1]=flip_8bit(0xE7,8);
    txBuf[i]=flip_8bit(0x9E,8);
    i=i+2;
    for(j=0;j<7;i++,j++){
        txBuf[i]=0xFF;
    }
    txBuf[i-1]=0xF0;
    Send_or_Receive_Data(0,i*8,NRST_const);
}
void SWD_SYNC(uint8_t NRST_const){
    txBuf[0]=0xF0;
    Send_or_Receive_Data(0,8,NRST_const);
}
int CheckParity32(uint8_t* data){
    uint32_t ret=-1;
    uint32_t i,j;
    uint8_t par=0;
    for(i=0;i<4;i++){
        for(j=0;j<8;j++){
            if((data[i]<>1)&0x18);
    txBuf[0]|=((parity&0x01)<<2);
    Send_or_Receive_Data(0,9,nres_const);
    Send_or_Receive_Data(1,3,nres_const);
    ack=(rxBuf[0]&ACK_MASK);
    if(ack==ACK_OK){
        Send_or_Receive_Data(1,34,nres_const);
        if(CheckParity32(rxBuf)==0){
            u32u8_rxData.asU8[0]=flip_8bit(rxBuf[0],8);
            u32u8_rxData.asU8[1]=flip_8bit(rxBuf[1],8);
            u32u8_rxData.asU8[2]=flip_8bit(rxBuf[2],8);
            u32u8_rxData.asU8[3]=flip_8bit(rxBuf[3],8);
            *rxData32=u32u8_rxData.asU32;
        }else{
            ack|=ACK_READ_PARITY_ERR;
        }
    }
    return ack;
}

uint8_t SWD_Read32_w_ACK_check(uint8_t APnDP,uint8_t ADDR,uint8_t nres_const,uint32_t* rxData32){
    uint8_t ack=0;
    uint8_t cntr=0;
    while((ack!=ACK_OK)&&(cntr<3)){
        ack=SWD_Read32(APnDP,ADDR,nres_const,rxData32);
        if(ack!=ACK_OK){
            SWD_SYNC(nres_const);
        }
        cntr=cntr+1;
    }
    return ack;
}

uint8_t READ_DP(uint8_t ADDR,uint8_t nres_const,uint32_t* rxData32){
    return SWD_Read32_w_ACK_check(APnDP_DP,ADDR,nres_const,rxData32);
}
void setup(void){
    DDRA|=(PIN_SWDO|PIN_SWCLK|PIN_NRST);
    DDRA&=~(PIN_SWDI);
    PORTA|=(PIN_SWDI|PIN_SWCLK|PIN_NRST);

    TCCR1A=0x00;
    OCR1A=0x0040;
    TCCR1B=0x08;
    txCompleted=1;
}

void loop(void){
    uint32_t rxData32;
    uint8_t ack;
    char buf[80];
    u32u8_t u32u8_data;

    JTAG_to_SWD(1);
    ack= READ_DP(0x0,1,&rxData32);
    ack= READ_DP(0x4,1,&rxData32);
    delay(200);

}