![](/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@0:1ed23ab1345f, 2012-06-26 (annotated)
- Committer:
- va009039
- Date:
- Tue Jun 26 14:27:45 2012 +0000
- Revision:
- 0:1ed23ab1345f
fix overflow spp_service_buffer
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
va009039 | 0:1ed23ab1345f | 1 | #include "mbed.h" |
va009039 | 0:1ed23ab1345f | 2 | #include "uvc.h" |
va009039 | 0:1ed23ab1345f | 3 | #define __DEBUG |
va009039 | 0:1ed23ab1345f | 4 | #include "mydbg.h" |
va009039 | 0:1ed23ab1345f | 5 | #include "stcamcfg.h" |
va009039 | 0:1ed23ab1345f | 6 | #include "Utils.h" |
va009039 | 0:1ed23ab1345f | 7 | |
va009039 | 0:1ed23ab1345f | 8 | #define DESCRIPTOR_TYPE_DEVICE 1 |
va009039 | 0:1ed23ab1345f | 9 | #define DESCRIPTOR_TYPE_CONFIGURATION 2 |
va009039 | 0:1ed23ab1345f | 10 | #define DESCRIPTOR_TYPE_STRING 3 |
va009039 | 0:1ed23ab1345f | 11 | #define DESCRIPTOR_TYPE_INTERFACE 4 |
va009039 | 0:1ed23ab1345f | 12 | #define DESCRIPTOR_TYPE_ENDPOINT 5 |
va009039 | 0:1ed23ab1345f | 13 | |
va009039 | 0:1ed23ab1345f | 14 | #define DESCRIPTOR_TYPE_INTERFACE_ASSOCIATION 0x0b |
va009039 | 0:1ed23ab1345f | 15 | |
va009039 | 0:1ed23ab1345f | 16 | #define DESCRIPTOR_TYPE_HID 0x21 |
va009039 | 0:1ed23ab1345f | 17 | #define DESCRIPTOR_TYPE_REPORT 0x22 |
va009039 | 0:1ed23ab1345f | 18 | #define DESCRIPTOR_TYPE_PHYSICAL 0x23 |
va009039 | 0:1ed23ab1345f | 19 | #define DESCRIPTOR_TYPE_CS_INTERFACE 0x24 |
va009039 | 0:1ed23ab1345f | 20 | #define DESCRIPTOR_TYPE_CS_ENDPOINT 0x25 |
va009039 | 0:1ed23ab1345f | 21 | #define DESCRIPTOR_TYPE_HUB 0x29 |
va009039 | 0:1ed23ab1345f | 22 | |
va009039 | 0:1ed23ab1345f | 23 | #define CLASS_AUDIO 0x02 |
va009039 | 0:1ed23ab1345f | 24 | #define CLASS_HUB 0x09 |
va009039 | 0:1ed23ab1345f | 25 | |
va009039 | 0:1ed23ab1345f | 26 | #define IF_EQ_THEN_PRINTF(A,B) if (A == B) {VERBOSE("%s\n", #A); |
va009039 | 0:1ed23ab1345f | 27 | #define ENDIF } |
va009039 | 0:1ed23ab1345f | 28 | |
va009039 | 0:1ed23ab1345f | 29 | #define AC_HEADER 0x01 |
va009039 | 0:1ed23ab1345f | 30 | #define AC_INPUT_TERMINAL 0x02 |
va009039 | 0:1ed23ab1345f | 31 | #define AC_OUTPUT_TERMINAL 0x03 |
va009039 | 0:1ed23ab1345f | 32 | #define AC_FEATURE_UNIT 0x06 |
va009039 | 0:1ed23ab1345f | 33 | |
va009039 | 0:1ed23ab1345f | 34 | // Input Terminal Types |
va009039 | 0:1ed23ab1345f | 35 | #define ITT_CAMERA 0x0201 |
va009039 | 0:1ed23ab1345f | 36 | |
va009039 | 0:1ed23ab1345f | 37 | |
va009039 | 0:1ed23ab1345f | 38 | void _parserAudioControl(uint8_t* buf, int len) { |
va009039 | 0:1ed23ab1345f | 39 | int subtype = buf[2]; |
va009039 | 0:1ed23ab1345f | 40 | IF_EQ_THEN_PRINTF(AC_HEADER, subtype) |
va009039 | 0:1ed23ab1345f | 41 | VERBOSE("ADC: %04x\n", LE16(buf+3)); |
va009039 | 0:1ed23ab1345f | 42 | VERBOSE("TotalLength: %d\n", LE16(buf+5)); |
va009039 | 0:1ed23ab1345f | 43 | VERBOSE("InCollection: %d\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 44 | for (int n = 1; n <= buf[7]; n++) { |
va009039 | 0:1ed23ab1345f | 45 | VERBOSE("aInterfaceNr(%d): %d\n", n, buf[8+n-1]); |
va009039 | 0:1ed23ab1345f | 46 | } |
va009039 | 0:1ed23ab1345f | 47 | ENDIF |
va009039 | 0:1ed23ab1345f | 48 | IF_EQ_THEN_PRINTF(AC_INPUT_TERMINAL, subtype) |
va009039 | 0:1ed23ab1345f | 49 | VERBOSE("TerminalID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 50 | VERBOSE("TerminalType: %04X\n", LE16(buf+4)); |
va009039 | 0:1ed23ab1345f | 51 | VERBOSE("AssocTermianl: %d\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 52 | VERBOSE("NrChannels: %d\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 53 | ENDIF |
va009039 | 0:1ed23ab1345f | 54 | IF_EQ_THEN_PRINTF(AC_OUTPUT_TERMINAL, subtype) |
va009039 | 0:1ed23ab1345f | 55 | VERBOSE("TerminalID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 56 | VERBOSE("TerminalType: %04X\n", LE16(buf+4)); |
va009039 | 0:1ed23ab1345f | 57 | VERBOSE("AssocTermianl: %d\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 58 | ENDIF |
va009039 | 0:1ed23ab1345f | 59 | IF_EQ_THEN_PRINTF(AC_FEATURE_UNIT, subtype) |
va009039 | 0:1ed23ab1345f | 60 | VERBOSE("UnitID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 61 | VERBOSE("SourceID: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 62 | VERBOSE("ControlSize: %d\n", buf[5]); |
va009039 | 0:1ed23ab1345f | 63 | ENDIF |
va009039 | 0:1ed23ab1345f | 64 | } |
va009039 | 0:1ed23ab1345f | 65 | |
va009039 | 0:1ed23ab1345f | 66 | #define AS_GENERAL 0x01 |
va009039 | 0:1ed23ab1345f | 67 | #define AS_FORMAT_TYPE 0x02 |
va009039 | 0:1ed23ab1345f | 68 | |
va009039 | 0:1ed23ab1345f | 69 | void _parserAudioStream(uint8_t* buf, int len) { |
va009039 | 0:1ed23ab1345f | 70 | int subtype = buf[2]; |
va009039 | 0:1ed23ab1345f | 71 | IF_EQ_THEN_PRINTF(AS_GENERAL, subtype) |
va009039 | 0:1ed23ab1345f | 72 | VERBOSE("TerminalLink: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 73 | VERBOSE("Delay: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 74 | VERBOSE("FormatTag: %04x\n", LE16(buf+5)); |
va009039 | 0:1ed23ab1345f | 75 | ENDIF |
va009039 | 0:1ed23ab1345f | 76 | IF_EQ_THEN_PRINTF(AS_FORMAT_TYPE, subtype) |
va009039 | 0:1ed23ab1345f | 77 | VERBOSE("FormatType: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 78 | VERBOSE("NrChannels: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 79 | VERBOSE("SubFrameSize: %d\n", buf[5]); |
va009039 | 0:1ed23ab1345f | 80 | VERBOSE("BitResolution: %d\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 81 | VERBOSE("SamFreqType: %d\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 82 | VERBOSE("SamFreq(1): %d\n", LE24(buf+8)); |
va009039 | 0:1ed23ab1345f | 83 | ENDIF |
va009039 | 0:1ed23ab1345f | 84 | } |
va009039 | 0:1ed23ab1345f | 85 | |
va009039 | 0:1ed23ab1345f | 86 | #define CC_VIDEO 0x0e |
va009039 | 0:1ed23ab1345f | 87 | |
va009039 | 0:1ed23ab1345f | 88 | #define SC_VIDEOCONTROL 0x01 |
va009039 | 0:1ed23ab1345f | 89 | #define SC_VIDEOSTREAMING 0x02 |
va009039 | 0:1ed23ab1345f | 90 | |
va009039 | 0:1ed23ab1345f | 91 | #define VC_HEADER 0x01 |
va009039 | 0:1ed23ab1345f | 92 | #define VC_INPUT_TERMINAL 0x02 |
va009039 | 0:1ed23ab1345f | 93 | #define VC_OUTPUT_TERMINAL 0x03 |
va009039 | 0:1ed23ab1345f | 94 | #define VC_SELECTOR_UNIT 0x04 |
va009039 | 0:1ed23ab1345f | 95 | #define VC_PROCESSING_UNIT 0x05 |
va009039 | 0:1ed23ab1345f | 96 | #define VC_EXTENSION_UNIT 0x06 |
va009039 | 0:1ed23ab1345f | 97 | |
va009039 | 0:1ed23ab1345f | 98 | void _parserVideoControl(uint8_t* buf, int len) { |
va009039 | 0:1ed23ab1345f | 99 | int subtype = buf[2]; |
va009039 | 0:1ed23ab1345f | 100 | IF_EQ_THEN_PRINTF(VC_HEADER, subtype) |
va009039 | 0:1ed23ab1345f | 101 | VERBOSE("UVC: %04x\n", LE16(buf+3)); |
va009039 | 0:1ed23ab1345f | 102 | VERBOSE("TotalLength: %d\n", LE16(buf+5)); |
va009039 | 0:1ed23ab1345f | 103 | VERBOSE("ClockFrequency: %d\n", LE32(buf+7)); |
va009039 | 0:1ed23ab1345f | 104 | VERBOSE("InCollection: %d\n", buf[11]); |
va009039 | 0:1ed23ab1345f | 105 | VERBOSE("aInterfaceNr(1): %d\n", buf[12]); |
va009039 | 0:1ed23ab1345f | 106 | ENDIF |
va009039 | 0:1ed23ab1345f | 107 | IF_EQ_THEN_PRINTF(VC_INPUT_TERMINAL, subtype) |
va009039 | 0:1ed23ab1345f | 108 | VERBOSE("TerminalID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 109 | uint16_t tt = LE16(buf+4); |
va009039 | 0:1ed23ab1345f | 110 | VERBOSE("TerminalType: %04X\n", tt); |
va009039 | 0:1ed23ab1345f | 111 | VERBOSE("AssocTerminal: %d\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 112 | VERBOSE("Terminal: %d\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 113 | if (tt == ITT_CAMERA) { // camera |
va009039 | 0:1ed23ab1345f | 114 | int bControlSize = buf[14]; |
va009039 | 0:1ed23ab1345f | 115 | VERBOSE("ControlSize: %d\n", bControlSize); |
va009039 | 0:1ed23ab1345f | 116 | for(int i = 0; i < bControlSize; i++) { |
va009039 | 0:1ed23ab1345f | 117 | uint8_t bControls = buf[15+i]; |
va009039 | 0:1ed23ab1345f | 118 | VERBOSE("Controls(%d): %02X\n", i, bControls); |
va009039 | 0:1ed23ab1345f | 119 | } |
va009039 | 0:1ed23ab1345f | 120 | } |
va009039 | 0:1ed23ab1345f | 121 | ENDIF |
va009039 | 0:1ed23ab1345f | 122 | IF_EQ_THEN_PRINTF(VC_OUTPUT_TERMINAL, subtype) |
va009039 | 0:1ed23ab1345f | 123 | VERBOSE("TerminalID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 124 | VERBOSE("TerminalType: %04X\n", LE16(buf+4)); |
va009039 | 0:1ed23ab1345f | 125 | VERBOSE("AssocTerminal: %d\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 126 | VERBOSE("SourceID: %d\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 127 | VERBOSE("Terminal: %d\n", buf[8]); |
va009039 | 0:1ed23ab1345f | 128 | ENDIF |
va009039 | 0:1ed23ab1345f | 129 | IF_EQ_THEN_PRINTF(VC_SELECTOR_UNIT, subtype) |
va009039 | 0:1ed23ab1345f | 130 | VERBOSE("UnitID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 131 | ENDIF |
va009039 | 0:1ed23ab1345f | 132 | IF_EQ_THEN_PRINTF(VC_PROCESSING_UNIT, subtype) |
va009039 | 0:1ed23ab1345f | 133 | VERBOSE("UnitID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 134 | VERBOSE("SourceID: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 135 | VERBOSE("MaxMultiplier: %d\n", LE16(buf+5)); |
va009039 | 0:1ed23ab1345f | 136 | VERBOSE("ControlSize: %d\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 137 | int pos = 8; |
va009039 | 0:1ed23ab1345f | 138 | for (int n = 1; n <= buf[7]; n++) { |
va009039 | 0:1ed23ab1345f | 139 | VERBOSE("Controls(%d): %02X\n", n , buf[pos]); |
va009039 | 0:1ed23ab1345f | 140 | pos++; |
va009039 | 0:1ed23ab1345f | 141 | } |
va009039 | 0:1ed23ab1345f | 142 | VERBOSE("Processing: %d\n", buf[pos]); |
va009039 | 0:1ed23ab1345f | 143 | pos++; |
va009039 | 0:1ed23ab1345f | 144 | VERBOSE("VideoStanders: %02X\n", buf[pos]); |
va009039 | 0:1ed23ab1345f | 145 | ENDIF |
va009039 | 0:1ed23ab1345f | 146 | IF_EQ_THEN_PRINTF(VC_EXTENSION_UNIT, subtype) |
va009039 | 0:1ed23ab1345f | 147 | VERBOSE("UnitID: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 148 | ENDIF |
va009039 | 0:1ed23ab1345f | 149 | } |
va009039 | 0:1ed23ab1345f | 150 | |
va009039 | 0:1ed23ab1345f | 151 | #define VS_INPUT_HEADER 0x01 |
va009039 | 0:1ed23ab1345f | 152 | #define VS_STILL_FRAME 0x03 |
va009039 | 0:1ed23ab1345f | 153 | #define VS_FORMAT_UNCOMPRESSED 0x04 |
va009039 | 0:1ed23ab1345f | 154 | #define VS_FRAME_UNCOMPRESSED 0x05 |
va009039 | 0:1ed23ab1345f | 155 | #define VS_FORMAT_MJPEG 0x06 |
va009039 | 0:1ed23ab1345f | 156 | #define VS_FRAME_MJPEG 0x07 |
va009039 | 0:1ed23ab1345f | 157 | #define VS_COLOR_FORMAT 0x0d |
va009039 | 0:1ed23ab1345f | 158 | |
va009039 | 0:1ed23ab1345f | 159 | void _parserVideoStream(struct stcamcfg* cfg, uint8_t* buf, int len) { |
va009039 | 0:1ed23ab1345f | 160 | int subtype = buf[2]; |
va009039 | 0:1ed23ab1345f | 161 | IF_EQ_THEN_PRINTF(VS_INPUT_HEADER, subtype) |
va009039 | 0:1ed23ab1345f | 162 | VERBOSE("NumFormats: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 163 | VERBOSE("TotalLength: %d\n", LE16(buf+4)); |
va009039 | 0:1ed23ab1345f | 164 | VERBOSE("EndpointAddress: %02X\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 165 | VERBOSE("Info: %02X\n", buf[7]); |
va009039 | 0:1ed23ab1345f | 166 | VERBOSE("TerminalLink: %d\n", buf[8]); |
va009039 | 0:1ed23ab1345f | 167 | VERBOSE("StillCaptureMethod: %d\n", buf[9]); |
va009039 | 0:1ed23ab1345f | 168 | VERBOSE("TriggerSupport: %d\n", buf[10]); |
va009039 | 0:1ed23ab1345f | 169 | VERBOSE("TriggerUsage: %d\n", buf[11]); |
va009039 | 0:1ed23ab1345f | 170 | VERBOSE("ControlSize: %d\n", buf[12]); |
va009039 | 0:1ed23ab1345f | 171 | int pos = 13; |
va009039 | 0:1ed23ab1345f | 172 | for (int n = 1; n <= buf[12]; n++) { |
va009039 | 0:1ed23ab1345f | 173 | VERBOSE("Controls(%d): %02X\n", n, buf[pos]); |
va009039 | 0:1ed23ab1345f | 174 | pos++; |
va009039 | 0:1ed23ab1345f | 175 | } |
va009039 | 0:1ed23ab1345f | 176 | cfg->bEndpointAddress = buf[6]; |
va009039 | 0:1ed23ab1345f | 177 | ENDIF |
va009039 | 0:1ed23ab1345f | 178 | IF_EQ_THEN_PRINTF(VS_STILL_FRAME, subtype) |
va009039 | 0:1ed23ab1345f | 179 | VERBOSE("EndpointAdress: %02X\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 180 | VERBOSE("NumImageSizePatterns: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 181 | int ptn = buf[4]; |
va009039 | 0:1ed23ab1345f | 182 | int pos = 5; |
va009039 | 0:1ed23ab1345f | 183 | for (int n = 1; n <= ptn; n++) { |
va009039 | 0:1ed23ab1345f | 184 | VERBOSE("Width(%d): %d\n", n, LE16(buf+pos)); |
va009039 | 0:1ed23ab1345f | 185 | VERBOSE("Height(%d): %d\n", n, LE16(buf+pos+2)); |
va009039 | 0:1ed23ab1345f | 186 | pos += 4; |
va009039 | 0:1ed23ab1345f | 187 | } |
va009039 | 0:1ed23ab1345f | 188 | VERBOSE("NumCompressPtn: %d\n", buf[pos]); |
va009039 | 0:1ed23ab1345f | 189 | ptn = buf[pos++]; |
va009039 | 0:1ed23ab1345f | 190 | for (int n = 1; n <= ptn; n++) { |
va009039 | 0:1ed23ab1345f | 191 | VERBOSE("Compress(%d): %d\n", n, buf[pos]); |
va009039 | 0:1ed23ab1345f | 192 | pos++; |
va009039 | 0:1ed23ab1345f | 193 | } |
va009039 | 0:1ed23ab1345f | 194 | ENDIF |
va009039 | 0:1ed23ab1345f | 195 | IF_EQ_THEN_PRINTF(VS_FORMAT_UNCOMPRESSED, subtype) |
va009039 | 0:1ed23ab1345f | 196 | VERBOSE("FormatIndex: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 197 | VERBOSE("NumFrameDescriptors: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 198 | uint32_t guid = LE32(buf+5); |
va009039 | 0:1ed23ab1345f | 199 | if (guid == 0x32595559) { |
va009039 | 0:1ed23ab1345f | 200 | VERBOSE("GUID: YUY2\n"); |
va009039 | 0:1ed23ab1345f | 201 | } else if (guid == 0x3231564e) { |
va009039 | 0:1ed23ab1345f | 202 | VERBOSE("GUID: NV12\n"); |
va009039 | 0:1ed23ab1345f | 203 | } else { |
va009039 | 0:1ed23ab1345f | 204 | VERBOSE("GUID: %08x\n", guid); |
va009039 | 0:1ed23ab1345f | 205 | } |
va009039 | 0:1ed23ab1345f | 206 | VERBOSE("DefaultFrameIndex: %d\n", buf[22]); |
va009039 | 0:1ed23ab1345f | 207 | if (cfg->payload == PAYLOAD_YUY2) { |
va009039 | 0:1ed23ab1345f | 208 | cfg->FormatIndex = buf[3]; |
va009039 | 0:1ed23ab1345f | 209 | } |
va009039 | 0:1ed23ab1345f | 210 | ENDIF |
va009039 | 0:1ed23ab1345f | 211 | IF_EQ_THEN_PRINTF(VS_FRAME_UNCOMPRESSED, subtype) |
va009039 | 0:1ed23ab1345f | 212 | VERBOSE("FrameIndex: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 213 | VERBOSE("Capabilites: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 214 | VERBOSE("Width: %d\n", LE16(buf+5)); |
va009039 | 0:1ed23ab1345f | 215 | VERBOSE("Height: %d\n", LE16(buf+7)); |
va009039 | 0:1ed23ab1345f | 216 | VERBOSE("MinBitRate: %d\n", LE32(buf+9)); |
va009039 | 0:1ed23ab1345f | 217 | VERBOSE("MaxBitRate: %d\n", LE32(buf+13)); |
va009039 | 0:1ed23ab1345f | 218 | VERBOSE("MaxVideoFrameBufferSize: %d\n", LE32(buf+17)); |
va009039 | 0:1ed23ab1345f | 219 | VERBOSE("DefaultFrameInterval: %d\n", LE32(buf+21)); |
va009039 | 0:1ed23ab1345f | 220 | VERBOSE("FrameIntervalType: %d\n", buf[25]); |
va009039 | 0:1ed23ab1345f | 221 | int it = buf[25]; |
va009039 | 0:1ed23ab1345f | 222 | uint32_t max_fi = 333333; // 30.0fps |
va009039 | 0:1ed23ab1345f | 223 | if (it == 0) { |
va009039 | 0:1ed23ab1345f | 224 | VERBOSE("FrameMinInterval: %d\n", buf[26]); |
va009039 | 0:1ed23ab1345f | 225 | VERBOSE("FrameMaxInterval: %d\n", buf[30]); |
va009039 | 0:1ed23ab1345f | 226 | VERBOSE("FrameIntervalStep: %d\n", buf[34]); |
va009039 | 0:1ed23ab1345f | 227 | } else { |
va009039 | 0:1ed23ab1345f | 228 | int pos = 26; |
va009039 | 0:1ed23ab1345f | 229 | for (int n = 1; n <= it; n++) { |
va009039 | 0:1ed23ab1345f | 230 | uint32_t fi = LE32(buf+pos); |
va009039 | 0:1ed23ab1345f | 231 | if (fi >= max_fi) { |
va009039 | 0:1ed23ab1345f | 232 | max_fi = fi; |
va009039 | 0:1ed23ab1345f | 233 | } |
va009039 | 0:1ed23ab1345f | 234 | float fps = 1e+7 / fi; |
va009039 | 0:1ed23ab1345f | 235 | VERBOSE("FrameInterval(%u): %d (%.1f fps)\n", n, fi, fps); |
va009039 | 0:1ed23ab1345f | 236 | pos += 4; |
va009039 | 0:1ed23ab1345f | 237 | } |
va009039 | 0:1ed23ab1345f | 238 | } |
va009039 | 0:1ed23ab1345f | 239 | if (cfg->payload == PAYLOAD_YUY2) { |
va009039 | 0:1ed23ab1345f | 240 | if (cfg->width == LE16(buf+5) && cfg->height == LE16(buf+7)) { |
va009039 | 0:1ed23ab1345f | 241 | cfg->FrameIndex = buf[3]; |
va009039 | 0:1ed23ab1345f | 242 | } |
va009039 | 0:1ed23ab1345f | 243 | if (cfg->dwFrameInterval == 0) { |
va009039 | 0:1ed23ab1345f | 244 | cfg->dwFrameInterval = max_fi; |
va009039 | 0:1ed23ab1345f | 245 | } |
va009039 | 0:1ed23ab1345f | 246 | } |
va009039 | 0:1ed23ab1345f | 247 | ENDIF |
va009039 | 0:1ed23ab1345f | 248 | IF_EQ_THEN_PRINTF(VS_FORMAT_MJPEG, subtype) |
va009039 | 0:1ed23ab1345f | 249 | VERBOSE("FormatIndex: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 250 | VERBOSE("NumFrameDescriptors: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 251 | VERBOSE("Flags: %d\n", buf[5]); |
va009039 | 0:1ed23ab1345f | 252 | VERBOSE("DefaultFrameIndex: %d\n", buf[6]); |
va009039 | 0:1ed23ab1345f | 253 | if (cfg->payload == PAYLOAD_MJPEG) { |
va009039 | 0:1ed23ab1345f | 254 | cfg->FormatIndex = buf[3]; |
va009039 | 0:1ed23ab1345f | 255 | } |
va009039 | 0:1ed23ab1345f | 256 | ENDIF |
va009039 | 0:1ed23ab1345f | 257 | IF_EQ_THEN_PRINTF(VS_FRAME_MJPEG, subtype) |
va009039 | 0:1ed23ab1345f | 258 | VERBOSE("FrameIndex: %d\n", buf[3]); |
va009039 | 0:1ed23ab1345f | 259 | VERBOSE("Capabilites: %d\n", buf[4]); |
va009039 | 0:1ed23ab1345f | 260 | VERBOSE("Width: %d\n", LE16(buf+5)); |
va009039 | 0:1ed23ab1345f | 261 | VERBOSE("Height: %d\n", LE16(buf+7)); |
va009039 | 0:1ed23ab1345f | 262 | VERBOSE("MinBitRate: %d\n", LE32(buf+9)); |
va009039 | 0:1ed23ab1345f | 263 | VERBOSE("MaxBitRate: %d\n", LE32(buf+13)); |
va009039 | 0:1ed23ab1345f | 264 | VERBOSE("MaxVideoFrameBufferSize: %d\n", LE32(buf+17)); |
va009039 | 0:1ed23ab1345f | 265 | VERBOSE("DefaultFrameInterval: %d\n", LE32(buf+21)); |
va009039 | 0:1ed23ab1345f | 266 | VERBOSE("FrameIntervalType: %d\n", buf[25]); |
va009039 | 0:1ed23ab1345f | 267 | int it = buf[25]; |
va009039 | 0:1ed23ab1345f | 268 | uint32_t max_fi = 333333; // 30.0fps |
va009039 | 0:1ed23ab1345f | 269 | if (it == 0) { |
va009039 | 0:1ed23ab1345f | 270 | VERBOSE("FrameMinInterval: %d\n", buf[26]); |
va009039 | 0:1ed23ab1345f | 271 | VERBOSE("FrameMaxInterval: %d\n", buf[30]); |
va009039 | 0:1ed23ab1345f | 272 | VERBOSE("FrameIntervalStep: %d\n", buf[34]); |
va009039 | 0:1ed23ab1345f | 273 | } else { |
va009039 | 0:1ed23ab1345f | 274 | int pos = 26; |
va009039 | 0:1ed23ab1345f | 275 | for (int n = 1; n <= it; n++) { |
va009039 | 0:1ed23ab1345f | 276 | uint32_t fi = LE32(buf+pos); |
va009039 | 0:1ed23ab1345f | 277 | if (fi >= max_fi) { |
va009039 | 0:1ed23ab1345f | 278 | max_fi = fi; |
va009039 | 0:1ed23ab1345f | 279 | } |
va009039 | 0:1ed23ab1345f | 280 | float fps = 1e+7 / fi; |
va009039 | 0:1ed23ab1345f | 281 | VERBOSE("FrameInterval(%u): %d (%.1f fps)\n", n, fi, fps); |
va009039 | 0:1ed23ab1345f | 282 | pos += 4; |
va009039 | 0:1ed23ab1345f | 283 | } |
va009039 | 0:1ed23ab1345f | 284 | } |
va009039 | 0:1ed23ab1345f | 285 | if (cfg->payload == PAYLOAD_MJPEG) { |
va009039 | 0:1ed23ab1345f | 286 | if (cfg->width == LE16(buf+5) && cfg->height == LE16(buf+7)) { |
va009039 | 0:1ed23ab1345f | 287 | cfg->FrameIndex = buf[3]; |
va009039 | 0:1ed23ab1345f | 288 | } |
va009039 | 0:1ed23ab1345f | 289 | if (cfg->dwFrameInterval == 0) { |
va009039 | 0:1ed23ab1345f | 290 | cfg->dwFrameInterval = max_fi; |
va009039 | 0:1ed23ab1345f | 291 | } |
va009039 | 0:1ed23ab1345f | 292 | } |
va009039 | 0:1ed23ab1345f | 293 | ENDIF |
va009039 | 0:1ed23ab1345f | 294 | IF_EQ_THEN_PRINTF(VS_COLOR_FORMAT, subtype) |
va009039 | 0:1ed23ab1345f | 295 | ENDIF |
va009039 | 0:1ed23ab1345f | 296 | } |
va009039 | 0:1ed23ab1345f | 297 | |
va009039 | 0:1ed23ab1345f | 298 | void _parserConfigurationDescriptor(struct stcamcfg* cfg, uint8_t* buf, int len) { |
va009039 | 0:1ed23ab1345f | 299 | //DBG("buf=%p len=%d\n", buf, len); |
va009039 | 0:1ed23ab1345f | 300 | //DBG_HEX(buf, len); |
va009039 | 0:1ed23ab1345f | 301 | int pos = 0; |
va009039 | 0:1ed23ab1345f | 302 | cfg->_IfClass = 0; |
va009039 | 0:1ed23ab1345f | 303 | cfg->_IfSubClass = 0; |
va009039 | 0:1ed23ab1345f | 304 | while (pos < len) { |
va009039 | 0:1ed23ab1345f | 305 | int type = buf[pos+1]; |
va009039 | 0:1ed23ab1345f | 306 | //DBG_BYTES(TYPE_Str(type), buf+pos, buf[pos]); |
va009039 | 0:1ed23ab1345f | 307 | IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_CONFIGURATION, type) |
va009039 | 0:1ed23ab1345f | 308 | VERBOSE("NumInterfaces: %d\n", buf[pos+4]); |
va009039 | 0:1ed23ab1345f | 309 | ENDIF |
va009039 | 0:1ed23ab1345f | 310 | IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_INTERFACE_ASSOCIATION, type) |
va009039 | 0:1ed23ab1345f | 311 | VERBOSE("FirstInterface: %d\n", buf[pos+2]); |
va009039 | 0:1ed23ab1345f | 312 | VERBOSE("InterfaceCount: %d\n", buf[pos+3]); |
va009039 | 0:1ed23ab1345f | 313 | VERBOSE("FunctionClass: %02X\n", buf[pos+4]); |
va009039 | 0:1ed23ab1345f | 314 | VERBOSE("FunctionSubClass: %02X\n", buf[pos+5]); |
va009039 | 0:1ed23ab1345f | 315 | VERBOSE("FunctionProtocol: %02X\n", buf[pos+6]); |
va009039 | 0:1ed23ab1345f | 316 | VERBOSE("Function: %d\n", buf[pos+7]); |
va009039 | 0:1ed23ab1345f | 317 | ENDIF |
va009039 | 0:1ed23ab1345f | 318 | IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_INTERFACE,type) |
va009039 | 0:1ed23ab1345f | 319 | VERBOSE("InterfaceNumber: %d\n", buf[pos+2]); |
va009039 | 0:1ed23ab1345f | 320 | VERBOSE("AlternateSetting: %d\n", buf[pos+3]); |
va009039 | 0:1ed23ab1345f | 321 | VERBOSE("NumEndpoint: %d\n", buf[pos+4]); |
va009039 | 0:1ed23ab1345f | 322 | VERBOSE("InterfaceClass: %02X\n", buf[pos+5]); |
va009039 | 0:1ed23ab1345f | 323 | VERBOSE("InterfaceSubClass: %02X\n", buf[pos+6]); |
va009039 | 0:1ed23ab1345f | 324 | VERBOSE("InterfaceProtocol: %02X\n", buf[pos+7]); |
va009039 | 0:1ed23ab1345f | 325 | VERBOSE("Interface: %d\n", buf[pos+8]); |
va009039 | 0:1ed23ab1345f | 326 | cfg->_If = buf[pos+2]; |
va009039 | 0:1ed23ab1345f | 327 | cfg->_Ifalt = buf[pos+3]; |
va009039 | 0:1ed23ab1345f | 328 | cfg->_IfClass = buf[pos+5]; |
va009039 | 0:1ed23ab1345f | 329 | cfg->_IfSubClass = buf[pos+6]; |
va009039 | 0:1ed23ab1345f | 330 | ENDIF |
va009039 | 0:1ed23ab1345f | 331 | IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_ENDPOINT, type) |
va009039 | 0:1ed23ab1345f | 332 | VERBOSE("EndpointAddress: %02X\n", buf[pos+2]); |
va009039 | 0:1ed23ab1345f | 333 | VERBOSE("Attributes: %02X\n", buf[pos+3]); |
va009039 | 0:1ed23ab1345f | 334 | VERBOSE("MaxPacketSize: %d\n", LE16(buf+pos+4)); |
va009039 | 0:1ed23ab1345f | 335 | VERBOSE("Interval: %d\n", buf[pos+6]); |
va009039 | 0:1ed23ab1345f | 336 | if (cfg->_IfClass == CC_VIDEO && cfg->_IfSubClass == SC_VIDEOSTREAMING) { |
va009039 | 0:1ed23ab1345f | 337 | if (cfg->bEndpointAddress == buf[pos+2]) { |
va009039 | 0:1ed23ab1345f | 338 | if (cfg->wMaxPacketSize == 0) { |
va009039 | 0:1ed23ab1345f | 339 | cfg->wMaxPacketSize = LE16(buf+pos+4); |
va009039 | 0:1ed23ab1345f | 340 | } |
va009039 | 0:1ed23ab1345f | 341 | if (cfg->wMaxPacketSize == LE16(buf+pos+4)) { |
va009039 | 0:1ed23ab1345f | 342 | cfg->bInterface = cfg->_If; |
va009039 | 0:1ed23ab1345f | 343 | cfg->bAlternate = cfg->_Ifalt; |
va009039 | 0:1ed23ab1345f | 344 | } |
va009039 | 0:1ed23ab1345f | 345 | } |
va009039 | 0:1ed23ab1345f | 346 | } |
va009039 | 0:1ed23ab1345f | 347 | ENDIF |
va009039 | 0:1ed23ab1345f | 348 | IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_CS_INTERFACE, type) |
va009039 | 0:1ed23ab1345f | 349 | IF_EQ_THEN_PRINTF(CC_VIDEO, cfg->_IfClass) |
va009039 | 0:1ed23ab1345f | 350 | IF_EQ_THEN_PRINTF(SC_VIDEOCONTROL, cfg->_IfSubClass) |
va009039 | 0:1ed23ab1345f | 351 | _parserVideoControl(buf+pos, buf[pos]); |
va009039 | 0:1ed23ab1345f | 352 | ENDIF |
va009039 | 0:1ed23ab1345f | 353 | IF_EQ_THEN_PRINTF(SC_VIDEOSTREAMING, cfg->_IfSubClass) |
va009039 | 0:1ed23ab1345f | 354 | _parserVideoStream(cfg, buf+pos, buf[pos]); |
va009039 | 0:1ed23ab1345f | 355 | ENDIF |
va009039 | 0:1ed23ab1345f | 356 | ENDIF |
va009039 | 0:1ed23ab1345f | 357 | if (cfg->_IfClass == CLASS_AUDIO) { |
va009039 | 0:1ed23ab1345f | 358 | if (cfg->_IfSubClass == 0x01) { |
va009039 | 0:1ed23ab1345f | 359 | _parserAudioControl(buf+pos, buf[pos]); |
va009039 | 0:1ed23ab1345f | 360 | } else if (cfg->_IfSubClass == 0x02) { |
va009039 | 0:1ed23ab1345f | 361 | _parserAudioStream(buf+pos, buf[pos]); |
va009039 | 0:1ed23ab1345f | 362 | } |
va009039 | 0:1ed23ab1345f | 363 | } |
va009039 | 0:1ed23ab1345f | 364 | ENDIF |
va009039 | 0:1ed23ab1345f | 365 | IF_EQ_THEN_PRINTF(DESCRIPTOR_TYPE_HUB, type) |
va009039 | 0:1ed23ab1345f | 366 | ENDIF |
va009039 | 0:1ed23ab1345f | 367 | pos += buf[pos]; |
va009039 | 0:1ed23ab1345f | 368 | } |
va009039 | 0:1ed23ab1345f | 369 | } |
va009039 | 0:1ed23ab1345f | 370 | |
va009039 | 0:1ed23ab1345f | 371 | void uvc::_config(struct stcamcfg* cfg) |
va009039 | 0:1ed23ab1345f | 372 | { |
va009039 | 0:1ed23ab1345f | 373 | DBG_ASSERT(cfg); |
va009039 | 0:1ed23ab1345f | 374 | int index = 0; |
va009039 | 0:1ed23ab1345f | 375 | uint8_t temp[4]; |
va009039 | 0:1ed23ab1345f | 376 | int rc = m_pDev->GetDescriptor(USB_DESCRIPTOR_TYPE_CONFIGURATION, index, temp, sizeof(temp)); |
va009039 | 0:1ed23ab1345f | 377 | DBG_ASSERT(rc == USBERR_OK); |
va009039 | 0:1ed23ab1345f | 378 | DBG_BYTES("Config Descriptor 4bytes", temp, sizeof(temp)); |
va009039 | 0:1ed23ab1345f | 379 | DBG_ASSERT(temp[0] == 9); |
va009039 | 0:1ed23ab1345f | 380 | DBG_ASSERT(temp[1] == 0x02); |
va009039 | 0:1ed23ab1345f | 381 | int TotalLength = LE16(temp+2); |
va009039 | 0:1ed23ab1345f | 382 | DBG("TotalLength: %d\n", TotalLength); |
va009039 | 0:1ed23ab1345f | 383 | |
va009039 | 0:1ed23ab1345f | 384 | uint8_t* buf = new uint8_t[TotalLength]; |
va009039 | 0:1ed23ab1345f | 385 | DBG_ASSERT(buf); |
va009039 | 0:1ed23ab1345f | 386 | rc = m_pDev->GetDescriptor(USB_DESCRIPTOR_TYPE_CONFIGURATION, index, |
va009039 | 0:1ed23ab1345f | 387 | buf, TotalLength); |
va009039 | 0:1ed23ab1345f | 388 | DBG_ASSERT(rc == USBERR_OK); |
va009039 | 0:1ed23ab1345f | 389 | |
va009039 | 0:1ed23ab1345f | 390 | _parserConfigurationDescriptor(cfg, buf, TotalLength); |
va009039 | 0:1ed23ab1345f | 391 | |
va009039 | 0:1ed23ab1345f | 392 | DBG("cfg->FrameIndex=%d\n", cfg->FrameIndex); |
va009039 | 0:1ed23ab1345f | 393 | DBG("cfg->dwFrameInterval=%u\n", cfg->dwFrameInterval); |
va009039 | 0:1ed23ab1345f | 394 | |
va009039 | 0:1ed23ab1345f | 395 | //DBG_ASSERT(cfg->FormatIndex >= 1); |
va009039 | 0:1ed23ab1345f | 396 | //DBG_ASSERT(cfg->FormatIndex <= 2); |
va009039 | 0:1ed23ab1345f | 397 | //DBG_ASSERT(cfg->FrameIndex >= 1); |
va009039 | 0:1ed23ab1345f | 398 | //DBG_ASSERT(cfg->FrameIndex <= 6); |
va009039 | 0:1ed23ab1345f | 399 | |
va009039 | 0:1ed23ab1345f | 400 | delete[] buf; |
va009039 | 0:1ed23ab1345f | 401 | } |