Diff: BaseUsbProtocolAnalyzer.cpp

Revision:

0:0faa55631ffe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BaseUsbProtocolAnalyzer.cpp	Wed Jun 18 01:33:32 2014 +0000
@@ -0,0 +1,222 @@
+#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++);
+    }
+}
+