uvccfg.cpp

00001 #include "mbed.h"
00002 #include "uvc.h"
00003 #define __DEBUG
00004 #include "mydbg.h"
00005 #include "stcamcfg.h"
00006 #include "Utils.h"
00007 
00008 #define  DESCRIPTOR_TYPE_DEVICE        1
00009 #define  DESCRIPTOR_TYPE_CONFIGURATION 2
00010 #define  DESCRIPTOR_TYPE_STRING        3
00011 #define  DESCRIPTOR_TYPE_INTERFACE     4
00012 #define  DESCRIPTOR_TYPE_ENDPOINT      5
00013 
00014 #define DESCRIPTOR_TYPE_INTERFACE_ASSOCIATION 0x0b
00015 
00016 #define DESCRIPTOR_TYPE_HID          0x21
00017 #define DESCRIPTOR_TYPE_REPORT       0x22
00018 #define DESCRIPTOR_TYPE_PHYSICAL     0x23
00019 #define DESCRIPTOR_TYPE_CS_INTERFACE 0x24
00020 #define DESCRIPTOR_TYPE_CS_ENDPOINT  0x25
00021 #define DESCRIPTOR_TYPE_HUB          0x29
00022 
00023 #define CLASS_AUDIO 0x02
00024 #define CLASS_HUB   0x09
00025 
00026 #define IF_EQ_THEN_PRINTF(A,B) if (A == B) {VERBOSE("%s\n", #A);
00027 #define ENDIF }
00028 
00029 #define CC_AUDIO 0x01
00030 #define SC_AUDIOCONTROL   0x01
00031 #define SC_AUDIOSTREAMING 0x02
00032 
00033 #define AC_HEADER          0x01
00034 #define AC_INPUT_TERMINAL  0x02
00035 #define AC_OUTPUT_TERMINAL 0x03
00036 #define AC_FEATURE_UNIT    0x06
00037 
00038 // Input Terminal Types
00039 #define ITT_CAMERA 0x0201
00040 
00041 
00042 void _parserAudioControl(uint8_t* buf, int len) {
00043     int subtype = buf[2];
00044     IF_EQ_THEN_PRINTF(AC_HEADER, subtype)
00045     VERBOSE("ADC: %04x\n", LE16(buf+3));
00046     VERBOSE("TotalLength: %d\n", LE16(buf+5));
00047     VERBOSE("InCollection: %d\n", buf[7]);
00048     for (int n = 1; n <= buf[7]; n++) {
00049         VERBOSE("aInterfaceNr(%d): %d\n", n, buf[8+n-1]);
00050     }
00051     ENDIF
00052     IF_EQ_THEN_PRINTF(AC_INPUT_TERMINAL, subtype)
00053     VERBOSE("TerminalID: %d\n", buf[3]);
00054     VERBOSE("TerminalType: %04X\n", LE16(buf+4));
00055     VERBOSE("AssocTermianl: %d\n", buf[6]);
00056     VERBOSE("NrChannels: %d\n", buf[7]);
00057     ENDIF
00058     IF_EQ_THEN_PRINTF(AC_OUTPUT_TERMINAL, subtype)
00059     VERBOSE("TerminalID: %d\n", buf[3]);
00060     VERBOSE("TerminalType: %04X\n", LE16(buf+4));
00061     VERBOSE("AssocTermianl: %d\n", buf[6]);
00062     ENDIF
00063     IF_EQ_THEN_PRINTF(AC_FEATURE_UNIT, subtype)
00064     VERBOSE("UnitID: %d\n", buf[3]);
00065     VERBOSE("SourceID: %d\n", buf[4]);
00066     VERBOSE("ControlSize: %d\n", buf[5]);
00067     ENDIF
00068 }
00069 
00070 #define AS_GENERAL     0x01
00071 #define AS_FORMAT_TYPE 0x02
00072 
00073 void _parserAudioStream(uint8_t* buf, int len) {
00074     int subtype = buf[2];
00075     IF_EQ_THEN_PRINTF(AS_GENERAL, subtype)
00076     VERBOSE("TerminalLink: %d\n", buf[3]);
00077     VERBOSE("Delay: %d\n", buf[4]);
00078     VERBOSE("FormatTag: %04x\n", LE16(buf+5));
00079     ENDIF
00080     IF_EQ_THEN_PRINTF(AS_FORMAT_TYPE, subtype)
00081     VERBOSE("FormatType: %d\n", buf[3]);
00082     VERBOSE("NrChannels: %d\n", buf[4]);
00083     VERBOSE("SubFrameSize: %d\n", buf[5]);
00084     VERBOSE("BitResolution: %d\n", buf[6]);
00085     VERBOSE("SamFreqType: %d\n", buf[7]);
00086     VERBOSE("SamFreq(1): %d\n", LE24(buf+8));
00087     ENDIF
00088 }
00089 
00090 #define CC_VIDEO 0x0e
00091 
00092 #define SC_VIDEOCONTROL   0x01
00093 #define SC_VIDEOSTREAMING 0x02
00094 
00095 #define VC_HEADER          0x01
00096 #define VC_INPUT_TERMINAL  0x02
00097 #define VC_OUTPUT_TERMINAL 0x03
00098 #define VC_SELECTOR_UNIT   0x04
00099 #define VC_PROCESSING_UNIT 0x05
00100 #define VC_EXTENSION_UNIT  0x06
00101 
00102 void _parserVideoControl(struct stcamcfg* cfg, uint8_t* buf, int len) {
00103     int subtype = buf[2];
00104     IF_EQ_THEN_PRINTF(VC_HEADER, subtype)
00105         cfg->bcdUVC = LE16(buf+3);
00106         VERBOSE("UVC: %04x\n", cfg->bcdUVC);
00107         VERBOSE("TotalLength: %d\n", LE16(buf+5));
00108         VERBOSE("ClockFrequency: %d\n", LE32(buf+7));
00109         VERBOSE("InCollection: %d\n", buf[11]);
00110         VERBOSE("aInterfaceNr(1): %d\n", buf[12]);
00111     ENDIF
00112     IF_EQ_THEN_PRINTF(VC_INPUT_TERMINAL, subtype)
00113         VERBOSE("TerminalID: %d\n", buf[3]);
00114         uint16_t tt = LE16(buf+4);
00115         VERBOSE("TerminalType: %04X\n", tt);
00116         VERBOSE("AssocTerminal: %d\n", buf[6]);
00117         VERBOSE("Terminal: %d\n", buf[7]);
00118         if (tt == ITT_CAMERA) { // camera
00119             int bControlSize = buf[14];
00120             VERBOSE("ControlSize: %d\n", bControlSize);
00121             for(int i = 0; i < bControlSize; i++) {
00122             uint8_t bControls = buf[15+i];
00123                 VERBOSE("Controls(%d): %02X\n", i, bControls); 
00124             }
00125         }
00126     ENDIF
00127     IF_EQ_THEN_PRINTF(VC_OUTPUT_TERMINAL, subtype)
00128         VERBOSE("TerminalID: %d\n", buf[3]);
00129         VERBOSE("TerminalType: %04X\n", LE16(buf+4));
00130         VERBOSE("AssocTerminal: %d\n", buf[6]);
00131         VERBOSE("SourceID: %d\n", buf[7]);
00132         VERBOSE("Terminal: %d\n", buf[8]);
00133     ENDIF
00134     IF_EQ_THEN_PRINTF(VC_SELECTOR_UNIT, subtype)
00135         VERBOSE("UnitID: %d\n", buf[3]);
00136     ENDIF
00137     IF_EQ_THEN_PRINTF(VC_PROCESSING_UNIT, subtype)
00138         VERBOSE("UnitID: %d\n", buf[3]);
00139         VERBOSE("SourceID: %d\n", buf[4]);
00140         VERBOSE("MaxMultiplier: %d\n", LE16(buf+5));
00141         VERBOSE("ControlSize: %d\n", buf[7]);
00142         int pos = 8;
00143         for (int n = 1; n <= buf[7]; n++) {
00144             VERBOSE("Controls(%d): %02X\n", n , buf[pos]);
00145             pos++;
00146         }
00147         VERBOSE("Processing: %d\n", buf[pos]);
00148         pos++;
00149         VERBOSE("VideoStanders: %02X\n", buf[pos]);
00150     ENDIF
00151     IF_EQ_THEN_PRINTF(VC_EXTENSION_UNIT, subtype)
00152         VERBOSE("UnitID: %d\n", buf[3]);
00153     ENDIF
00154 }
00155 
00156 #define VS_INPUT_HEADER 0x01
00157 #define VS_STILL_FRAME  0x03
00158 #define VS_FORMAT_UNCOMPRESSED 0x04
00159 #define VS_FRAME_UNCOMPRESSED 0x05
00160 #define VS_FORMAT_MJPEG 0x06
00161 #define VS_FRAME_MJPEG  0x07
00162 #define VS_COLOR_FORMAT 0x0d
00163 
00164 void _parserVideoStream(struct stcamcfg* cfg, uint8_t* buf, int len) {
00165     int subtype = buf[2];
00166     IF_EQ_THEN_PRINTF(VS_INPUT_HEADER, subtype)
00167         VERBOSE("NumFormats: %d\n", buf[3]);
00168         VERBOSE("TotalLength: %d\n", LE16(buf+4));
00169         VERBOSE("EndpointAddress: %02X\n", buf[6]);
00170         VERBOSE("Info: %02X\n", buf[7]);
00171         VERBOSE("TerminalLink: %d\n", buf[8]);
00172         VERBOSE("StillCaptureMethod: %d\n", buf[9]);
00173         VERBOSE("TriggerSupport: %d\n", buf[10]);
00174         VERBOSE("TriggerUsage: %d\n", buf[11]);
00175         VERBOSE("ControlSize: %d\n", buf[12]);
00176         int pos = 13;
00177         for (int n = 1; n <= buf[12]; n++) {
00178             VERBOSE("Controls(%d): %02X\n", n, buf[pos]);
00179             pos++;
00180         }
00181         cfg->bEndpointAddress = buf[6];
00182     ENDIF
00183     IF_EQ_THEN_PRINTF(VS_STILL_FRAME, subtype)
00184     VERBOSE("EndpointAdress: %02X\n", buf[3]);
00185     VERBOSE("NumImageSizePatterns: %d\n", buf[4]);
00186     int ptn = buf[4];
00187     int pos = 5;
00188     for (int n = 1; n <= ptn; n++) {
00189         VERBOSE("Width(%d): %d\n", n, LE16(buf+pos));
00190         VERBOSE("Height(%d): %d\n", n, LE16(buf+pos+2));
00191         pos += 4;
00192     }
00193     VERBOSE("NumCompressPtn: %d\n", buf[pos]);
00194     ptn = buf[pos++];
00195     for (int n = 1; n <= ptn; n++) {
00196         VERBOSE("Compress(%d): %d\n", n, buf[pos]);
00197         pos++;
00198     }
00199     ENDIF
00200     IF_EQ_THEN_PRINTF(VS_FORMAT_UNCOMPRESSED, subtype)
00201         VERBOSE("FormatIndex: %d\n", buf[3]);
00202         VERBOSE("NumFrameDescriptors: %d\n", buf[4]);
00203         uint32_t guid = LE32(buf+5);
00204         if (guid == 0x32595559) {
00205             VERBOSE("GUID: YUY2\n");
00206         } else if (guid == 0x3231564e) {
00207             VERBOSE("GUID: NV12\n");
00208         } else {
00209             VERBOSE("GUID: %08x\n", guid);
00210         }
00211         VERBOSE("DefaultFrameIndex: %d\n", buf[22]);
00212         if (cfg->payload == PAYLOAD_YUY2) {
00213             cfg->FormatIndex = buf[3];
00214         }
00215     ENDIF
00216     IF_EQ_THEN_PRINTF(VS_FRAME_UNCOMPRESSED, subtype)
00217         VERBOSE("FrameIndex: %d\n", buf[3]);
00218         VERBOSE("Capabilites: %d\n", buf[4]);
00219         VERBOSE("Width: %d\n", LE16(buf+5));
00220         VERBOSE("Height: %d\n", LE16(buf+7));
00221         VERBOSE("MinBitRate: %d\n", LE32(buf+9));
00222         VERBOSE("MaxBitRate: %d\n", LE32(buf+13));
00223         VERBOSE("MaxVideoFrameBufferSize: %d\n", LE32(buf+17));
00224         VERBOSE("DefaultFrameInterval: %d\n", LE32(buf+21));
00225         VERBOSE("FrameIntervalType: %d\n", buf[25]);
00226         int it = buf[25];
00227         uint32_t max_fi = 333333; // 30.0fps
00228         if (it == 0) {
00229             VERBOSE("FrameMinInterval: %d\n", buf[26]);
00230             VERBOSE("FrameMaxInterval: %d\n", buf[30]);
00231             VERBOSE("FrameIntervalStep: %d\n", buf[34]);
00232         } else {
00233             int pos = 26;
00234             for (int n = 1; n <= it; n++) {
00235                 uint32_t fi = LE32(buf+pos);
00236                 if (fi >= max_fi) {
00237                     max_fi = fi;
00238                 }
00239                 float fps = 1e+7 / fi;
00240                 VERBOSE("FrameInterval(%u): %d (%.1f fps)\n", n, fi, fps);
00241                 pos += 4;
00242             }
00243         }
00244         if (cfg->payload == PAYLOAD_YUY2) {
00245             if (cfg->width == LE16(buf+5) && cfg->height == LE16(buf+7)) {
00246                 cfg->FrameIndex = buf[3];
00247             }
00248             if (cfg->dwFrameInterval == 0) {
00249                 cfg->dwFrameInterval = max_fi;
00250             }
00251         }
00252     ENDIF
00253     IF_EQ_THEN_PRINTF(VS_FORMAT_MJPEG, subtype)
00254         VERBOSE("FormatIndex: %d\n", buf[3]);
00255         VERBOSE("NumFrameDescriptors: %d\n", buf[4]);
00256         VERBOSE("Flags: %d\n", buf[5]);
00257         VERBOSE("DefaultFrameIndex: %d\n", buf[6]);
00258         if (cfg->payload == PAYLOAD_MJPEG) {
00259             cfg->FormatIndex = buf[3];
00260         }
00261     ENDIF
00262     IF_EQ_THEN_PRINTF(VS_FRAME_MJPEG, subtype)
00263         VERBOSE("FrameIndex: %d\n", buf[3]);
00264         VERBOSE("Capabilites: %d\n", buf[4]);
00265         VERBOSE("Width: %d\n", LE16(buf+5));
00266         VERBOSE("Height: %d\n", LE16(buf+7));
00267         VERBOSE("MinBitRate: %d\n", LE32(buf+9));
00268         VERBOSE("MaxBitRate: %d\n", LE32(buf+13));
00269         VERBOSE("MaxVideoFrameBufferSize: %d\n", LE32(buf+17));
00270         VERBOSE("DefaultFrameInterval: %d\n", LE32(buf+21));
00271         VERBOSE("FrameIntervalType: %d\n", buf[25]);
00272         int it = buf[25];
00273         uint32_t max_fi = 333333; // 30.0fps
00274         if (it == 0) {
00275             VERBOSE("FrameMinInterval: %d\n", buf[26]);
00276             VERBOSE("FrameMaxInterval: %d\n", buf[30]);
00277             VERBOSE("FrameIntervalStep: %d\n", buf[34]);
00278         } else {
00279             int pos = 26;
00280             for (int n = 1; n <= it; n++) {
00281                 uint32_t fi = LE32(buf+pos);
00282                 if (fi >= max_fi) {
00283                     max_fi = fi;
00284                 }
00285                 float fps = 1e+7 / fi;
00286                 VERBOSE("FrameInterval(%u): %d (%.1f fps)\n", n, fi, fps);
00287                 pos += 4;
00288             }
00289         }
00290         if (cfg->payload == PAYLOAD_MJPEG) {
00291             if (cfg->width == LE16(buf+5) && cfg->height == LE16(buf+7)) {
00292                 cfg->FrameIndex = buf[3];
00293             }
00294             if (cfg->dwFrameInterval == 0) {
00295                 cfg->dwFrameInterval = max_fi;
00296             }
00297         }
00298     ENDIF
00299     IF_EQ_THEN_PRINTF(VS_COLOR_FORMAT, subtype)
00300     ENDIF
00301 }
00302 
00303 void _parserConfigurationDescriptor(struct stcamcfg* cfg, uint8_t* buf, int len) {
00304     //DBG("buf=%p len=%d\n", buf, len);
00305     //DBG_HEX(buf, len);
00306     int pos = 0;
00307     cfg->_IfClass = 0;
00308     cfg->_IfSubClass = 0;
00309     while (pos < len) {
00310         int type = buf[pos+1];
00311         //DBG_BYTES(TYPE_Str(type), buf+pos, buf[pos]);
00312         IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_CONFIGURATION, type)
00313             VERBOSE("NumInterfaces: %d\n", buf[pos+4]);
00314         ENDIF
00315         IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_INTERFACE_ASSOCIATION, type)
00316             VERBOSE("FirstInterface: %d\n", buf[pos+2]);
00317             VERBOSE("InterfaceCount: %d\n", buf[pos+3]);
00318             VERBOSE("FunctionClass: %02X\n", buf[pos+4]);
00319             VERBOSE("FunctionSubClass: %02X\n", buf[pos+5]);
00320             VERBOSE("FunctionProtocol: %02X\n", buf[pos+6]);
00321             VERBOSE("Function: %d\n", buf[pos+7]);
00322         ENDIF
00323         IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_INTERFACE,type)
00324             VERBOSE("InterfaceNumber: %d\n", buf[pos+2]);
00325             VERBOSE("AlternateSetting: %d\n", buf[pos+3]);
00326             VERBOSE("NumEndpoint: %d\n", buf[pos+4]);
00327             VERBOSE("InterfaceClass: %02X\n", buf[pos+5]);
00328             VERBOSE("InterfaceSubClass: %02X\n", buf[pos+6]);
00329             VERBOSE("InterfaceProtocol: %02X\n", buf[pos+7]);
00330             VERBOSE("Interface: %d\n", buf[pos+8]);
00331             cfg->_If         = buf[pos+2];
00332             cfg->_Ifalt      = buf[pos+3];
00333             cfg->_IfClass    = buf[pos+5];
00334             cfg->_IfSubClass = buf[pos+6];
00335         ENDIF
00336         IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_ENDPOINT, type)
00337             VERBOSE("EndpointAddress: %02X\n", buf[pos+2]);
00338             VERBOSE("Attributes: %02X\n", buf[pos+3]);
00339             VERBOSE("MaxPacketSize: %d\n", LE16(buf+pos+4));
00340             VERBOSE("Interval: %d\n", buf[pos+6]);
00341             if (cfg->_IfClass == CC_VIDEO && cfg->_IfSubClass == SC_VIDEOSTREAMING) {
00342                 if (cfg->bEndpointAddress == buf[pos+2]) {
00343                     if (cfg->wMaxPacketSize == 0) {
00344                         cfg->wMaxPacketSize = LE16(buf+pos+4);
00345                     }    
00346                     if (cfg->wMaxPacketSize == LE16(buf+pos+4)) {
00347                         cfg->bInterface = cfg->_If;
00348                         cfg->bAlternate = cfg->_Ifalt;
00349                     }
00350                 }
00351             }
00352         ENDIF
00353         IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_CS_INTERFACE, type)
00354             IF_EQ_THEN_PRINTF(CC_VIDEO, cfg->_IfClass)
00355                 IF_EQ_THEN_PRINTF(SC_VIDEOCONTROL, cfg->_IfSubClass)
00356                     _parserVideoControl(cfg, buf+pos, buf[pos]);
00357                 ENDIF
00358                 IF_EQ_THEN_PRINTF(SC_VIDEOSTREAMING, cfg->_IfSubClass)
00359                     _parserVideoStream(cfg, buf+pos, buf[pos]);
00360                 ENDIF
00361             ENDIF
00362             IF_EQ_THEN_PRINTF(CC_AUDIO, cfg->_IfClass)
00363                 IF_EQ_THEN_PRINTF(SC_AUDIOCONTROL, cfg->_IfSubClass)
00364                     _parserAudioControl(buf+pos, buf[pos]);
00365                 ENDIF 
00366                 IF_EQ_THEN_PRINTF(SC_AUDIOSTREAMING, cfg->_IfSubClass)
00367                     _parserAudioStream(buf+pos, buf[pos]);
00368                 ENDIF
00369             ENDIF
00370         ENDIF
00371         IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_HUB, type)
00372         ENDIF
00373         pos += buf[pos];
00374     }
00375 }
00376 
00377 void uvc::_config(struct stcamcfg* cfg)
00378 {
00379     DBG_ASSERT(cfg);
00380     int index = 0;
00381     uint8_t temp[4];
00382     int rc = m_pDev->GetDescriptor(USB_DESCRIPTOR_TYPE_CONFIGURATION, index, temp, sizeof(temp));
00383     DBG_ASSERT(rc == USBERR_OK);
00384     DBG_BYTES("Config Descriptor 4bytes", temp, sizeof(temp));
00385     DBG_ASSERT(temp[0] == 9);
00386     DBG_ASSERT(temp[1] == 0x02);
00387     int TotalLength = LE16(temp+2);
00388     DBG("TotalLength: %d\n", TotalLength);
00389 
00390     uint8_t* buf = new uint8_t[TotalLength];
00391     DBG_ASSERT(buf);
00392     rc = m_pDev->GetDescriptor(USB_DESCRIPTOR_TYPE_CONFIGURATION, index, 
00393              buf, TotalLength);
00394     DBG_ASSERT(rc == USBERR_OK);
00395 
00396     _parserConfigurationDescriptor(cfg, buf, TotalLength);
00397 
00398     DBG("cfg->FormatIndex=%d\n", cfg->FormatIndex);
00399     DBG("cfg->FrameIndex=%d\n", cfg->FrameIndex);
00400     DBG("cfg->dwFrameInterval=%u\n", cfg->dwFrameInterval);
00401 
00402     //DBG_ASSERT(cfg->FormatIndex >= 1);
00403     //DBG_ASSERT(cfg->FormatIndex <= 2);
00404     //DBG_ASSERT(cfg->FrameIndex >= 1);
00405     //DBG_ASSERT(cfg->FrameIndex <= 6);
00406 
00407     delete[] buf;
00408 }