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