File content as of revision 0:0faa55631ffe:

#include "mbed.h"
#include "BaseUsbProtocolAnalyzer.h"


uint8_t crc5usb(uint16_t data)
{
    uint16_t r = (data&0x7ff) ^ 0x1f;
    for(int j = 0; j < 11; j++) { // 11bit
        if (r & 1) {
            r = (r>>1) ^ 0x14;
        } else {
            r = r>>1;
        }
    }    
    return r ^ 0x1f;
}

uint16_t crc16usb(const uint8_t data[], int size)
{
    uint16_t r = 0xffff;
    for(int i = 0; i < size; i++) {
        r ^= data[i];
        for(int j = 0; j < 8; j++) {
            if (r & 1) {
                r = (r>>1) ^ 0xa001;
            } else {
                r = (r>>1);
            }
        }
    }
    return r ^ 0xffff;
}


BaseUsbProtocolAnalyzer::BaseUsbProtocolAnalyzer()
{
    init();
}

void BaseUsbProtocolAnalyzer::input(uint8_t data)
{
    char str[32];
    if (data == 0) { // EOP ?
        if (seq == SEQ_DATA) {
            output("[");
            for(int i = 0; i < data_count-2; i++) {
                snprintf(str, sizeof(str), "%s0x%02x", i == 0 ? "" : " ", packet.data[i]);
                output(str);
            }
            output("]");
            if (data_count >= 2) {
                uint16_t crc16 = packet.data[data_count-2]|(packet.data[data_count-1]<<8);
                bool crc16_err = (crc16usb(packet.data, data_count-2) == crc16);
                snprintf(str, sizeof(str), "[crc16=0x%04x%s]", crc16, crc16_err ? "" : " ERROR");
                output(str);
            }
        }
        output("[EOP]");
        init();
        return;
    }

    if (seq == SEQ_SYNC) {
        if (disp_cycle) {
            snprintf(str, sizeof(str), "(0x%02x)", data);
            output(str);
        }
        output("[SYNC]");
        packet.pid = (pid_t)data;
        switch(packet.pid) {
            case ACK:
                output("[ACK]");
                break;
            case NAK:
                output("[NAK]");
                break;
            case STALL:
                output("[STALL]");
                break;
            case IN:
                output("[IN]");
                bit_count = 7;
                break;
            case OUT:
                output("[OUT]");
                bit_count = 4;
                break;
            case SETUP:
                output("[SETUP]");
                bit_count = 7;
                break;
            case DATA0:
                output("[DATA0]");
                bit_count = 5;
                break;
            case DATA1:
                output("[DATA1]");
                bit_count = 7;
                break;
            default:
                snprintf(str, sizeof(str), "[pid=0x%02x]", packet.pid);
                output(str);
                break;
        }
        seq = SEQ_PID;
        return;
    }
    if (disp_cycle) {
        snprintf(str, sizeof(str), "(%d)", data);
        output(str);
    }
    int n = (data + CYCLE/2) / CYCLE - 1;
    uint8_t pat = 0;
    int len = 0;
    if (bit_stuffing) {
        const uint8_t table[] = {0x00,0x01,0x03,0x07,0x0f,0x1f,0x3f,0x7f,0xff};
        pat = table[n];
        len = n;
    } else {
        const uint8_t table[] = {0x00,0x02,0x06,0x0e,0x1e,0x3e,0x7e,0xfe};
        pat = table[n];    
        len = n + 1;
    }
    bit_stuffing = false;
    if (n >= 6) {
        bit_stuffing = true;
    }
    inputBit(pat, len);
}

void BaseUsbProtocolAnalyzer::inputBit(uint8_t pat, int len)
{
    if (disp_bit) {
        output('{');
        for(int i = 0; i < len; i++) {
            output(((pat>>i) & 1) ? '1' : '0');
        }
        output('}');
    }
    for(int i = 0; i < len; i++) {
        inputLSB((pat>>i) & 1);
    }
}

void BaseUsbProtocolAnalyzer::inputLSB(int lsb)
{
    char str[32];
    seq_t prev_seq = seq;
    switch(seq) {
        case SEQ_PID:
            if (++bit_count >= 8) {
                if (packet.pid == IN || packet.pid == OUT || packet.pid == SETUP) {
                    seq = SEQ_ADDR;
                } else if (packet.pid == DATA0 || packet.pid == DATA1) {
                    seq = SEQ_DATA;
                }
            }
            break;
        case SEQ_ADDR:
            packet.addr >>= 1;
            packet.addr |= lsb ? 0x40 : 0x00;
            if (++bit_count >= 7) {
                snprintf(str, sizeof(str), "[ADDR=%d]", packet.addr & 0x7f);
                output(str);
                seq = SEQ_ENDP;
            }    
            break;
        case SEQ_ENDP:
            packet.endp >>= 1;
            packet.endp |= lsb ? 0x08 : 0x00;
            if (++bit_count >= 4) {
                snprintf(str, sizeof(str), "[ENDP=%d]", packet.endp & 0x0f);
                output(str);
                seq = SEQ_CRC5;
            }
            break;
        case SEQ_CRC5:
            if (bit_count < 5) {
                packet.crc5 >>= 1;
                packet.crc5 |= lsb ? 0x10 : 0x00;
                if (++bit_count >= 5) {
                    uint16_t u = packet.crc5<<11|packet.endp<<7|packet.addr;
                    bool crc5_err = (crc5usb(u) == packet.crc5);
                    snprintf(str, sizeof(str), "[CRC5=0x%02x%s]", 
                        packet.crc5, crc5_err ? "" : " ERROR");
                    output(str);
                }
            }
            break;
        case SEQ_DATA:
            if (data_count < sizeof(packet.data)) {
                packet.data[data_count] >>= 1;
                packet.data[data_count] |= lsb ? 0x80 : 0x00;
                if (++bit_count >= 8) {
                    bit_count = 0;
                    data_count++;
                }
            }
            break;
    }
    if (prev_seq != seq) {
        bit_count = 0;
        data_count = 0;
    }
}

void BaseUsbProtocolAnalyzer::init()
{
    seq = SEQ_SYNC;
    bit_stuffing = false;
    bit_count = 0;
    disp_bit = false;
    disp_cycle = false;
}

void BaseUsbProtocolAnalyzer::output(const char* s)
{
    while(*s) {
        output(*s++);
    }
}