![](/media/cache/profiles/bf201d5407a6509fa536afc4b380577e.50x50_q85.jpg)
BLE demo for mbed Ported RunningElectronics's SBDBT firmware for BLE. It can communicate with iOS
Dependencies: FatFileSystem mbed
Fork of BTstack by
uvc/uvccfg.cpp
- Committer:
- todotani
- Date:
- 2013-02-20
- Revision:
- 6:cf06ba884429
- Parent:
- 0:1ed23ab1345f
File content as of revision 6:cf06ba884429:
#include "mbed.h" #include "uvc.h" #define __DEBUG #include "mydbg.h" #include "stcamcfg.h" #include "Utils.h" #define DESCRIPTOR_TYPE_DEVICE 1 #define DESCRIPTOR_TYPE_CONFIGURATION 2 #define DESCRIPTOR_TYPE_STRING 3 #define DESCRIPTOR_TYPE_INTERFACE 4 #define DESCRIPTOR_TYPE_ENDPOINT 5 #define DESCRIPTOR_TYPE_INTERFACE_ASSOCIATION 0x0b #define DESCRIPTOR_TYPE_HID 0x21 #define DESCRIPTOR_TYPE_REPORT 0x22 #define DESCRIPTOR_TYPE_PHYSICAL 0x23 #define DESCRIPTOR_TYPE_CS_INTERFACE 0x24 #define DESCRIPTOR_TYPE_CS_ENDPOINT 0x25 #define DESCRIPTOR_TYPE_HUB 0x29 #define CLASS_AUDIO 0x02 #define CLASS_HUB 0x09 #define IF_EQ_THEN_PRINTF(A,B) if (A == B) {VERBOSE("%s\n", #A); #define ENDIF } #define AC_HEADER 0x01 #define AC_INPUT_TERMINAL 0x02 #define AC_OUTPUT_TERMINAL 0x03 #define AC_FEATURE_UNIT 0x06 // Input Terminal Types #define ITT_CAMERA 0x0201 void _parserAudioControl(uint8_t* buf, int len) { int subtype = buf[2]; IF_EQ_THEN_PRINTF(AC_HEADER, subtype) VERBOSE("ADC: %04x\n", LE16(buf+3)); VERBOSE("TotalLength: %d\n", LE16(buf+5)); VERBOSE("InCollection: %d\n", buf[7]); for (int n = 1; n <= buf[7]; n++) { VERBOSE("aInterfaceNr(%d): %d\n", n, buf[8+n-1]); } ENDIF IF_EQ_THEN_PRINTF(AC_INPUT_TERMINAL, subtype) VERBOSE("TerminalID: %d\n", buf[3]); VERBOSE("TerminalType: %04X\n", LE16(buf+4)); VERBOSE("AssocTermianl: %d\n", buf[6]); VERBOSE("NrChannels: %d\n", buf[7]); ENDIF IF_EQ_THEN_PRINTF(AC_OUTPUT_TERMINAL, subtype) VERBOSE("TerminalID: %d\n", buf[3]); VERBOSE("TerminalType: %04X\n", LE16(buf+4)); VERBOSE("AssocTermianl: %d\n", buf[6]); ENDIF IF_EQ_THEN_PRINTF(AC_FEATURE_UNIT, subtype) VERBOSE("UnitID: %d\n", buf[3]); VERBOSE("SourceID: %d\n", buf[4]); VERBOSE("ControlSize: %d\n", buf[5]); ENDIF } #define AS_GENERAL 0x01 #define AS_FORMAT_TYPE 0x02 void _parserAudioStream(uint8_t* buf, int len) { int subtype = buf[2]; IF_EQ_THEN_PRINTF(AS_GENERAL, subtype) VERBOSE("TerminalLink: %d\n", buf[3]); VERBOSE("Delay: %d\n", buf[4]); VERBOSE("FormatTag: %04x\n", LE16(buf+5)); ENDIF IF_EQ_THEN_PRINTF(AS_FORMAT_TYPE, subtype) VERBOSE("FormatType: %d\n", buf[3]); VERBOSE("NrChannels: %d\n", buf[4]); VERBOSE("SubFrameSize: %d\n", buf[5]); VERBOSE("BitResolution: %d\n", buf[6]); VERBOSE("SamFreqType: %d\n", buf[7]); VERBOSE("SamFreq(1): %d\n", LE24(buf+8)); ENDIF } #define CC_VIDEO 0x0e #define SC_VIDEOCONTROL 0x01 #define SC_VIDEOSTREAMING 0x02 #define VC_HEADER 0x01 #define VC_INPUT_TERMINAL 0x02 #define VC_OUTPUT_TERMINAL 0x03 #define VC_SELECTOR_UNIT 0x04 #define VC_PROCESSING_UNIT 0x05 #define VC_EXTENSION_UNIT 0x06 void _parserVideoControl(uint8_t* buf, int len) { int subtype = buf[2]; IF_EQ_THEN_PRINTF(VC_HEADER, subtype) VERBOSE("UVC: %04x\n", LE16(buf+3)); VERBOSE("TotalLength: %d\n", LE16(buf+5)); VERBOSE("ClockFrequency: %d\n", LE32(buf+7)); VERBOSE("InCollection: %d\n", buf[11]); VERBOSE("aInterfaceNr(1): %d\n", buf[12]); ENDIF IF_EQ_THEN_PRINTF(VC_INPUT_TERMINAL, subtype) VERBOSE("TerminalID: %d\n", buf[3]); uint16_t tt = LE16(buf+4); VERBOSE("TerminalType: %04X\n", tt); VERBOSE("AssocTerminal: %d\n", buf[6]); VERBOSE("Terminal: %d\n", buf[7]); if (tt == ITT_CAMERA) { // camera int bControlSize = buf[14]; VERBOSE("ControlSize: %d\n", bControlSize); for(int i = 0; i < bControlSize; i++) { uint8_t bControls = buf[15+i]; VERBOSE("Controls(%d): %02X\n", i, bControls); } } ENDIF IF_EQ_THEN_PRINTF(VC_OUTPUT_TERMINAL, subtype) VERBOSE("TerminalID: %d\n", buf[3]); VERBOSE("TerminalType: %04X\n", LE16(buf+4)); VERBOSE("AssocTerminal: %d\n", buf[6]); VERBOSE("SourceID: %d\n", buf[7]); VERBOSE("Terminal: %d\n", buf[8]); ENDIF IF_EQ_THEN_PRINTF(VC_SELECTOR_UNIT, subtype) VERBOSE("UnitID: %d\n", buf[3]); ENDIF IF_EQ_THEN_PRINTF(VC_PROCESSING_UNIT, subtype) VERBOSE("UnitID: %d\n", buf[3]); VERBOSE("SourceID: %d\n", buf[4]); VERBOSE("MaxMultiplier: %d\n", LE16(buf+5)); VERBOSE("ControlSize: %d\n", buf[7]); int pos = 8; for (int n = 1; n <= buf[7]; n++) { VERBOSE("Controls(%d): %02X\n", n , buf[pos]); pos++; } VERBOSE("Processing: %d\n", buf[pos]); pos++; VERBOSE("VideoStanders: %02X\n", buf[pos]); ENDIF IF_EQ_THEN_PRINTF(VC_EXTENSION_UNIT, subtype) VERBOSE("UnitID: %d\n", buf[3]); ENDIF } #define VS_INPUT_HEADER 0x01 #define VS_STILL_FRAME 0x03 #define VS_FORMAT_UNCOMPRESSED 0x04 #define VS_FRAME_UNCOMPRESSED 0x05 #define VS_FORMAT_MJPEG 0x06 #define VS_FRAME_MJPEG 0x07 #define VS_COLOR_FORMAT 0x0d void _parserVideoStream(struct stcamcfg* cfg, uint8_t* buf, int len) { int subtype = buf[2]; IF_EQ_THEN_PRINTF(VS_INPUT_HEADER, subtype) VERBOSE("NumFormats: %d\n", buf[3]); VERBOSE("TotalLength: %d\n", LE16(buf+4)); VERBOSE("EndpointAddress: %02X\n", buf[6]); VERBOSE("Info: %02X\n", buf[7]); VERBOSE("TerminalLink: %d\n", buf[8]); VERBOSE("StillCaptureMethod: %d\n", buf[9]); VERBOSE("TriggerSupport: %d\n", buf[10]); VERBOSE("TriggerUsage: %d\n", buf[11]); VERBOSE("ControlSize: %d\n", buf[12]); int pos = 13; for (int n = 1; n <= buf[12]; n++) { VERBOSE("Controls(%d): %02X\n", n, buf[pos]); pos++; } cfg->bEndpointAddress = buf[6]; ENDIF IF_EQ_THEN_PRINTF(VS_STILL_FRAME, subtype) VERBOSE("EndpointAdress: %02X\n", buf[3]); VERBOSE("NumImageSizePatterns: %d\n", buf[4]); int ptn = buf[4]; int pos = 5; for (int n = 1; n <= ptn; n++) { VERBOSE("Width(%d): %d\n", n, LE16(buf+pos)); VERBOSE("Height(%d): %d\n", n, LE16(buf+pos+2)); pos += 4; } VERBOSE("NumCompressPtn: %d\n", buf[pos]); ptn = buf[pos++]; for (int n = 1; n <= ptn; n++) { VERBOSE("Compress(%d): %d\n", n, buf[pos]); pos++; } ENDIF IF_EQ_THEN_PRINTF(VS_FORMAT_UNCOMPRESSED, subtype) VERBOSE("FormatIndex: %d\n", buf[3]); VERBOSE("NumFrameDescriptors: %d\n", buf[4]); uint32_t guid = LE32(buf+5); if (guid == 0x32595559) { VERBOSE("GUID: YUY2\n"); } else if (guid == 0x3231564e) { VERBOSE("GUID: NV12\n"); } else { VERBOSE("GUID: %08x\n", guid); } VERBOSE("DefaultFrameIndex: %d\n", buf[22]); if (cfg->payload == PAYLOAD_YUY2) { cfg->FormatIndex = buf[3]; } ENDIF IF_EQ_THEN_PRINTF(VS_FRAME_UNCOMPRESSED, subtype) VERBOSE("FrameIndex: %d\n", buf[3]); VERBOSE("Capabilites: %d\n", buf[4]); VERBOSE("Width: %d\n", LE16(buf+5)); VERBOSE("Height: %d\n", LE16(buf+7)); VERBOSE("MinBitRate: %d\n", LE32(buf+9)); VERBOSE("MaxBitRate: %d\n", LE32(buf+13)); VERBOSE("MaxVideoFrameBufferSize: %d\n", LE32(buf+17)); VERBOSE("DefaultFrameInterval: %d\n", LE32(buf+21)); VERBOSE("FrameIntervalType: %d\n", buf[25]); int it = buf[25]; uint32_t max_fi = 333333; // 30.0fps if (it == 0) { VERBOSE("FrameMinInterval: %d\n", buf[26]); VERBOSE("FrameMaxInterval: %d\n", buf[30]); VERBOSE("FrameIntervalStep: %d\n", buf[34]); } else { int pos = 26; for (int n = 1; n <= it; n++) { uint32_t fi = LE32(buf+pos); if (fi >= max_fi) { max_fi = fi; } float fps = 1e+7 / fi; VERBOSE("FrameInterval(%u): %d (%.1f fps)\n", n, fi, fps); pos += 4; } } if (cfg->payload == PAYLOAD_YUY2) { if (cfg->width == LE16(buf+5) && cfg->height == LE16(buf+7)) { cfg->FrameIndex = buf[3]; } if (cfg->dwFrameInterval == 0) { cfg->dwFrameInterval = max_fi; } } ENDIF IF_EQ_THEN_PRINTF(VS_FORMAT_MJPEG, subtype) VERBOSE("FormatIndex: %d\n", buf[3]); VERBOSE("NumFrameDescriptors: %d\n", buf[4]); VERBOSE("Flags: %d\n", buf[5]); VERBOSE("DefaultFrameIndex: %d\n", buf[6]); if (cfg->payload == PAYLOAD_MJPEG) { cfg->FormatIndex = buf[3]; } ENDIF IF_EQ_THEN_PRINTF(VS_FRAME_MJPEG, subtype) VERBOSE("FrameIndex: %d\n", buf[3]); VERBOSE("Capabilites: %d\n", buf[4]); VERBOSE("Width: %d\n", LE16(buf+5)); VERBOSE("Height: %d\n", LE16(buf+7)); VERBOSE("MinBitRate: %d\n", LE32(buf+9)); VERBOSE("MaxBitRate: %d\n", LE32(buf+13)); VERBOSE("MaxVideoFrameBufferSize: %d\n", LE32(buf+17)); VERBOSE("DefaultFrameInterval: %d\n", LE32(buf+21)); VERBOSE("FrameIntervalType: %d\n", buf[25]); int it = buf[25]; uint32_t max_fi = 333333; // 30.0fps if (it == 0) { VERBOSE("FrameMinInterval: %d\n", buf[26]); VERBOSE("FrameMaxInterval: %d\n", buf[30]); VERBOSE("FrameIntervalStep: %d\n", buf[34]); } else { int pos = 26; for (int n = 1; n <= it; n++) { uint32_t fi = LE32(buf+pos); if (fi >= max_fi) { max_fi = fi; } float fps = 1e+7 / fi; VERBOSE("FrameInterval(%u): %d (%.1f fps)\n", n, fi, fps); pos += 4; } } if (cfg->payload == PAYLOAD_MJPEG) { if (cfg->width == LE16(buf+5) && cfg->height == LE16(buf+7)) { cfg->FrameIndex = buf[3]; } if (cfg->dwFrameInterval == 0) { cfg->dwFrameInterval = max_fi; } } ENDIF IF_EQ_THEN_PRINTF(VS_COLOR_FORMAT, subtype) ENDIF } void _parserConfigurationDescriptor(struct stcamcfg* cfg, uint8_t* buf, int len) { //DBG("buf=%p len=%d\n", buf, len); //DBG_HEX(buf, len); int pos = 0; cfg->_IfClass = 0; cfg->_IfSubClass = 0; while (pos < len) { int type = buf[pos+1]; //DBG_BYTES(TYPE_Str(type), buf+pos, buf[pos]); IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_CONFIGURATION, type) VERBOSE("NumInterfaces: %d\n", buf[pos+4]); ENDIF IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_INTERFACE_ASSOCIATION, type) VERBOSE("FirstInterface: %d\n", buf[pos+2]); VERBOSE("InterfaceCount: %d\n", buf[pos+3]); VERBOSE("FunctionClass: %02X\n", buf[pos+4]); VERBOSE("FunctionSubClass: %02X\n", buf[pos+5]); VERBOSE("FunctionProtocol: %02X\n", buf[pos+6]); VERBOSE("Function: %d\n", buf[pos+7]); ENDIF IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_INTERFACE,type) VERBOSE("InterfaceNumber: %d\n", buf[pos+2]); VERBOSE("AlternateSetting: %d\n", buf[pos+3]); VERBOSE("NumEndpoint: %d\n", buf[pos+4]); VERBOSE("InterfaceClass: %02X\n", buf[pos+5]); VERBOSE("InterfaceSubClass: %02X\n", buf[pos+6]); VERBOSE("InterfaceProtocol: %02X\n", buf[pos+7]); VERBOSE("Interface: %d\n", buf[pos+8]); cfg->_If = buf[pos+2]; cfg->_Ifalt = buf[pos+3]; cfg->_IfClass = buf[pos+5]; cfg->_IfSubClass = buf[pos+6]; ENDIF IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_ENDPOINT, type) VERBOSE("EndpointAddress: %02X\n", buf[pos+2]); VERBOSE("Attributes: %02X\n", buf[pos+3]); VERBOSE("MaxPacketSize: %d\n", LE16(buf+pos+4)); VERBOSE("Interval: %d\n", buf[pos+6]); if (cfg->_IfClass == CC_VIDEO && cfg->_IfSubClass == SC_VIDEOSTREAMING) { if (cfg->bEndpointAddress == buf[pos+2]) { if (cfg->wMaxPacketSize == 0) { cfg->wMaxPacketSize = LE16(buf+pos+4); } if (cfg->wMaxPacketSize == LE16(buf+pos+4)) { cfg->bInterface = cfg->_If; cfg->bAlternate = cfg->_Ifalt; } } } ENDIF IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_CS_INTERFACE, type) IF_EQ_THEN_PRINTF(CC_VIDEO, cfg->_IfClass) IF_EQ_THEN_PRINTF(SC_VIDEOCONTROL, cfg->_IfSubClass) _parserVideoControl(buf+pos, buf[pos]); ENDIF IF_EQ_THEN_PRINTF(SC_VIDEOSTREAMING, cfg->_IfSubClass) _parserVideoStream(cfg, buf+pos, buf[pos]); ENDIF ENDIF if (cfg->_IfClass == CLASS_AUDIO) { if (cfg->_IfSubClass == 0x01) { _parserAudioControl(buf+pos, buf[pos]); } else if (cfg->_IfSubClass == 0x02) { _parserAudioStream(buf+pos, buf[pos]); } } ENDIF IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_HUB, type) ENDIF pos += buf[pos]; } } void uvc::_config(struct stcamcfg* cfg) { DBG_ASSERT(cfg); int index = 0; uint8_t temp[4]; int rc = m_pDev->GetDescriptor(USB_DESCRIPTOR_TYPE_CONFIGURATION, index, temp, sizeof(temp)); DBG_ASSERT(rc == USBERR_OK); DBG_BYTES("Config Descriptor 4bytes", temp, sizeof(temp)); DBG_ASSERT(temp[0] == 9); DBG_ASSERT(temp[1] == 0x02); int TotalLength = LE16(temp+2); DBG("TotalLength: %d\n", TotalLength); uint8_t* buf = new uint8_t[TotalLength]; DBG_ASSERT(buf); rc = m_pDev->GetDescriptor(USB_DESCRIPTOR_TYPE_CONFIGURATION, index, buf, TotalLength); DBG_ASSERT(rc == USBERR_OK); _parserConfigurationDescriptor(cfg, buf, TotalLength); DBG("cfg->FrameIndex=%d\n", cfg->FrameIndex); DBG("cfg->dwFrameInterval=%u\n", cfg->dwFrameInterval); //DBG_ASSERT(cfg->FormatIndex >= 1); //DBG_ASSERT(cfg->FormatIndex <= 2); //DBG_ASSERT(cfg->FrameIndex >= 1); //DBG_ASSERT(cfg->FrameIndex <= 6); delete[] buf; }