![](/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
Diff: uvc/uvccfg.cpp
- Revision:
- 0:1ed23ab1345f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/uvc/uvccfg.cpp Tue Jun 26 14:27:45 2012 +0000 @@ -0,0 +1,401 @@ +#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; +}