Ken Todotani / Mbed 2 deprecated BTstackLE

BLE demo for mbed Ported RunningElectronics's SBDBT firmware for BLE. It can communicate with iOS

Dependencies:   FatFileSystem mbed

Fork of BTstack by Norimasa Okamoto

uvc/uvcini.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?

UserRevisionLine numberNew 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
va009039 0:1ed23ab1345f 7 const struct stcamcfg stcamcfg_table[] = {
va009039 0:1ed23ab1345f 8 /*
va009039 0:1ed23ab1345f 9 {0x045e, 0x074a,
va009039 0:1ed23ab1345f 10 160,120,
va009039 0:1ed23ab1345f 11 PAYLOAD_MJPEG,
va009039 0:1ed23ab1345f 12 0x81, 128,
va009039 0:1ed23ab1345f 13 1, 5, 2000000, // 160x120 5.0fps
va009039 0:1ed23ab1345f 14 1, 1, "Microsoft LifeCam VX-700",
va009039 0:1ed23ab1345f 15 8, 3,
va009039 0:1ed23ab1345f 16 },*/
va009039 0:1ed23ab1345f 17 /*
va009039 0:1ed23ab1345f 18 {0x0c45, 0x62c0,
va009039 0:1ed23ab1345f 19 160,120,
va009039 0:1ed23ab1345f 20 PAYLOAD_MJPEG,
va009039 0:1ed23ab1345f 21 0x81, 128,
va009039 0:1ed23ab1345f 22 1, 5, 2000000, // 160x120 5.0fps
va009039 0:1ed23ab1345f 23 1, 1, "UVCA130AF",
va009039 0:1ed23ab1345f 24 8, 3,
va009039 0:1ed23ab1345f 25 },*/
va009039 0:1ed23ab1345f 26
va009039 0:1ed23ab1345f 27 {0x046d, 0x0994,
va009039 0:1ed23ab1345f 28 160,120,
va009039 0:1ed23ab1345f 29 PAYLOAD_MJPEG,
va009039 0:1ed23ab1345f 30 0, 0,
va009039 0:1ed23ab1345f 31 0, 0, 2000000, // 160x120 10.0fps
va009039 0:1ed23ab1345f 32 0, 0, "Logitech QuickCam Orbit AF",
va009039 0:1ed23ab1345f 33 0, 3,
va009039 0:1ed23ab1345f 34 },
va009039 0:1ed23ab1345f 35 {0x0000, 0x0000,
va009039 0:1ed23ab1345f 36 160,120,
va009039 0:1ed23ab1345f 37 PAYLOAD_MJPEG,
va009039 0:1ed23ab1345f 38 0x00, 0,
va009039 0:1ed23ab1345f 39 0, 0, 2000000,
va009039 0:1ed23ab1345f 40 0, 0, "default",
va009039 0:1ed23ab1345f 41 0, 3,
va009039 0:1ed23ab1345f 42 },
va009039 0:1ed23ab1345f 43 };
va009039 0:1ed23ab1345f 44
va009039 0:1ed23ab1345f 45 inline void LE32(uint32_t n, uint8_t* d)
va009039 0:1ed23ab1345f 46 {
va009039 0:1ed23ab1345f 47 d[0] = (uint8_t)n;
va009039 0:1ed23ab1345f 48 d[1] = (uint8_t)(n >> 8);
va009039 0:1ed23ab1345f 49 d[2] = (uint8_t)(n >> 16);
va009039 0:1ed23ab1345f 50 d[3] = (uint8_t)(n >> 24);
va009039 0:1ed23ab1345f 51 }
va009039 0:1ed23ab1345f 52
va009039 0:1ed23ab1345f 53 void uvc::SetFormatIndex(int index)
va009039 0:1ed23ab1345f 54 {
va009039 0:1ed23ab1345f 55 DBG_ASSERT(index >= 1);
va009039 0:1ed23ab1345f 56 DBG_ASSERT(index <= 2);
va009039 0:1ed23ab1345f 57 m_FormatIndex = index;
va009039 0:1ed23ab1345f 58 }
va009039 0:1ed23ab1345f 59
va009039 0:1ed23ab1345f 60 void uvc::SetFrameIndex(int index)
va009039 0:1ed23ab1345f 61 {
va009039 0:1ed23ab1345f 62 DBG_ASSERT(index >= 1);
va009039 0:1ed23ab1345f 63 DBG_ASSERT(index <= 8);
va009039 0:1ed23ab1345f 64 m_FrameIndex = index;
va009039 0:1ed23ab1345f 65 }
va009039 0:1ed23ab1345f 66
va009039 0:1ed23ab1345f 67 void uvc::SetFrameInterval(int val)
va009039 0:1ed23ab1345f 68 {
va009039 0:1ed23ab1345f 69 DBG_ASSERT(val >= 333333);
va009039 0:1ed23ab1345f 70 DBG_ASSERT(val <= 10000000);
va009039 0:1ed23ab1345f 71 m_FrameInterval = val;
va009039 0:1ed23ab1345f 72 }
va009039 0:1ed23ab1345f 73
va009039 0:1ed23ab1345f 74 void uvc::SetPacketSize(int size)
va009039 0:1ed23ab1345f 75 {
va009039 0:1ed23ab1345f 76 DBG_ASSERT(size >= 128);
va009039 0:1ed23ab1345f 77 DBG_ASSERT(size <= 956);
va009039 0:1ed23ab1345f 78 m_PacketSize = size;
va009039 0:1ed23ab1345f 79 }
va009039 0:1ed23ab1345f 80
va009039 0:1ed23ab1345f 81 void uvc::SetImageSize(int width, int height)
va009039 0:1ed23ab1345f 82 {
va009039 0:1ed23ab1345f 83 DBG_ASSERT(width >= 160);
va009039 0:1ed23ab1345f 84 DBG_ASSERT(width <= 800);
va009039 0:1ed23ab1345f 85 DBG_ASSERT(height >= 120);
va009039 0:1ed23ab1345f 86 DBG_ASSERT(height <= 600);
va009039 0:1ed23ab1345f 87 m_width = width;
va009039 0:1ed23ab1345f 88 m_height = height;
va009039 0:1ed23ab1345f 89 }
va009039 0:1ed23ab1345f 90
va009039 0:1ed23ab1345f 91 void uvc::SetPayload(int payload)
va009039 0:1ed23ab1345f 92 {
va009039 0:1ed23ab1345f 93 DBG_ASSERT(payload == PAYLOAD_MJPEG || payload == PAYLOAD_YUY2);
va009039 0:1ed23ab1345f 94 m_payload = payload;
va009039 0:1ed23ab1345f 95 }
va009039 0:1ed23ab1345f 96
va009039 0:1ed23ab1345f 97 void uvc::poll()
va009039 0:1ed23ab1345f 98 {
va009039 0:1ed23ab1345f 99 isochronous();
va009039 0:1ed23ab1345f 100 }
va009039 0:1ed23ab1345f 101
va009039 0:1ed23ab1345f 102 int uvc::_init()
va009039 0:1ed23ab1345f 103 {
va009039 0:1ed23ab1345f 104 m_init = true;
va009039 0:1ed23ab1345f 105 UsbErr rc;
va009039 0:1ed23ab1345f 106 for(int i = 0; i < 2; i++) {
va009039 0:1ed23ab1345f 107 m_pDev = m_pHost->getDeviceByClass(CLASS_VIDEO, m_cam); // UVC
va009039 0:1ed23ab1345f 108 if (m_pDev || i > 0) {
va009039 0:1ed23ab1345f 109 break;
va009039 0:1ed23ab1345f 110 }
va009039 0:1ed23ab1345f 111 rc = Usb_poll();
va009039 0:1ed23ab1345f 112 if (rc == USBERR_PROCESSING) {
va009039 0:1ed23ab1345f 113 VERBOSE("%p USBERR_PROCESSING\n", this);
va009039 0:1ed23ab1345f 114 return -1;
va009039 0:1ed23ab1345f 115 }
va009039 0:1ed23ab1345f 116 }
va009039 0:1ed23ab1345f 117 DBG("m_pDev=%p\n", m_pDev);
va009039 0:1ed23ab1345f 118 if (!m_pDev) {
va009039 0:1ed23ab1345f 119 VERBOSE("%p UVC CAMERA(%d) NOT FOUND\n", this, m_cam);
va009039 0:1ed23ab1345f 120 return -1;
va009039 0:1ed23ab1345f 121 }
va009039 0:1ed23ab1345f 122 DBG_ASSERT(m_pDev);
va009039 0:1ed23ab1345f 123
va009039 0:1ed23ab1345f 124 struct stcamcfg cfg;
va009039 0:1ed23ab1345f 125 for(int i = 0; ; i++) {
va009039 0:1ed23ab1345f 126 cfg = stcamcfg_table[i];
va009039 0:1ed23ab1345f 127 if (cfg.idVender == 0x0000) {
va009039 0:1ed23ab1345f 128 DBG("not cam config\n");
va009039 0:1ed23ab1345f 129 DBG("vid: %04X\n", m_pDev->getVid());
va009039 0:1ed23ab1345f 130 DBG("pid: %04X\n", m_pDev->getPid());
va009039 0:1ed23ab1345f 131 break;
va009039 0:1ed23ab1345f 132 }
va009039 0:1ed23ab1345f 133 if (cfg.idVender == m_pDev->getVid() && cfg.idProduct == m_pDev->getPid()) {
va009039 0:1ed23ab1345f 134 DBG_ASSERT(cfg.name);
va009039 0:1ed23ab1345f 135 DBG("found %s\n", cfg.name);
va009039 0:1ed23ab1345f 136 break;
va009039 0:1ed23ab1345f 137 }
va009039 0:1ed23ab1345f 138 }
va009039 0:1ed23ab1345f 139
va009039 0:1ed23ab1345f 140 if (m_width) {
va009039 0:1ed23ab1345f 141 cfg.width = m_width;
va009039 0:1ed23ab1345f 142 }
va009039 0:1ed23ab1345f 143 if (m_height) {
va009039 0:1ed23ab1345f 144 cfg.height = m_height;
va009039 0:1ed23ab1345f 145 }
va009039 0:1ed23ab1345f 146 if (m_payload != PAYLOAD_UNDEF) {
va009039 0:1ed23ab1345f 147 cfg.payload = m_payload;
va009039 0:1ed23ab1345f 148 }
va009039 0:1ed23ab1345f 149 if (m_FormatIndex) {
va009039 0:1ed23ab1345f 150 cfg.FormatIndex = m_FormatIndex;
va009039 0:1ed23ab1345f 151 }
va009039 0:1ed23ab1345f 152 if (m_FrameIndex) {
va009039 0:1ed23ab1345f 153 cfg.FrameIndex = m_FrameIndex;
va009039 0:1ed23ab1345f 154 }
va009039 0:1ed23ab1345f 155 if (m_FrameInterval) {
va009039 0:1ed23ab1345f 156 cfg.dwFrameInterval = m_FrameInterval;
va009039 0:1ed23ab1345f 157 }
va009039 0:1ed23ab1345f 158 if (m_PacketSize) {
va009039 0:1ed23ab1345f 159 cfg.wMaxPacketSize = m_PacketSize;
va009039 0:1ed23ab1345f 160 }
va009039 0:1ed23ab1345f 161
va009039 0:1ed23ab1345f 162 _config(&cfg);
va009039 0:1ed23ab1345f 163
va009039 0:1ed23ab1345f 164 if (cfg.payload == PAYLOAD_YUY2) {
va009039 0:1ed23ab1345f 165 if (cfg.FormatIndex == 0) {
va009039 0:1ed23ab1345f 166 VERBOSE("YUY2 FORMAT NOT FOUND\n");
va009039 0:1ed23ab1345f 167 return -1;
va009039 0:1ed23ab1345f 168 }
va009039 0:1ed23ab1345f 169 }
va009039 0:1ed23ab1345f 170
va009039 0:1ed23ab1345f 171 if (cfg.iso_FrameCount == 0) {
va009039 0:1ed23ab1345f 172 int c = usb_bp_size() / cfg.wMaxPacketSize;
va009039 0:1ed23ab1345f 173 if (c > 8) {
va009039 0:1ed23ab1345f 174 c = 8;
va009039 0:1ed23ab1345f 175 }
va009039 0:1ed23ab1345f 176 cfg.iso_FrameCount = c;
va009039 0:1ed23ab1345f 177 }
va009039 0:1ed23ab1345f 178 DBG_ASSERT(cfg.iso_FrameCount >= 1);
va009039 0:1ed23ab1345f 179 DBG_ASSERT(cfg.iso_FrameCount <= 8);
va009039 0:1ed23ab1345f 180 DBG_ASSERT((cfg.iso_FrameCount * cfg.wMaxPacketSize) <= usb_bp_size());
va009039 0:1ed23ab1345f 181 if (cfg.iso_itdCount == 0) {
va009039 0:1ed23ab1345f 182 cfg.iso_itdCount = 3;
va009039 0:1ed23ab1345f 183 }
va009039 0:1ed23ab1345f 184 DBG_ASSERT(cfg.iso_itdCount >= 1);
va009039 0:1ed23ab1345f 185 DBG("cfg.wMaxPacketSize=%d\n", cfg.wMaxPacketSize);
va009039 0:1ed23ab1345f 186 DBG("cfg.iso_FrameCount=%d\n", cfg.iso_FrameCount);
va009039 0:1ed23ab1345f 187 DBG("cfg.iso_itdCount=%d\n", cfg.iso_itdCount);
va009039 0:1ed23ab1345f 188 DBG_ASSERT(cfg.iso_FrameCount >= 1 && cfg.iso_FrameCount <= 8);
va009039 0:1ed23ab1345f 189 //m_pEpIntIn = new UsbEndpoint(m_pDev, 0x83, true, USB_INT, 16);
va009039 0:1ed23ab1345f 190 //DBG_ASSERT(m_pEpIntIn);
va009039 0:1ed23ab1345f 191
va009039 0:1ed23ab1345f 192 DBG_ASSERT(cfg.bEndpointAddress == 0x81);
va009039 0:1ed23ab1345f 193 DBG_ASSERT(cfg.wMaxPacketSize >= 128);
va009039 0:1ed23ab1345f 194 DBG_ASSERT(cfg.wMaxPacketSize <= 956);
va009039 0:1ed23ab1345f 195 m_PacketSize = cfg.wMaxPacketSize;
va009039 0:1ed23ab1345f 196 DBG_ASSERT(m_PacketSize);
va009039 0:1ed23ab1345f 197 m_pEpIsoIn = new UsbEndpoint(m_pDev, cfg.bEndpointAddress, true, USB_ISO, m_PacketSize);
va009039 0:1ed23ab1345f 198 DBG_ASSERT(m_pEpIsoIn);
va009039 0:1ed23ab1345f 199
va009039 0:1ed23ab1345f 200 DBG_ASSERT(cfg.FormatIndex >= 1);
va009039 0:1ed23ab1345f 201 DBG_ASSERT(cfg.FormatIndex <= 2);
va009039 0:1ed23ab1345f 202 DBG_ASSERT(cfg.FrameIndex >= 1);
va009039 0:1ed23ab1345f 203 DBG_ASSERT(cfg.FrameIndex <= 8);
va009039 0:1ed23ab1345f 204 DBG_ASSERT(cfg.dwFrameInterval <= 10000000);
va009039 0:1ed23ab1345f 205 DBG_ASSERT(cfg.dwFrameInterval >= 333333);
va009039 0:1ed23ab1345f 206
va009039 0:1ed23ab1345f 207 uint8_t temp1[1];
va009039 0:1ed23ab1345f 208 temp1[0] = 0x00;
va009039 0:1ed23ab1345f 209 rc = Control(GET_INFO, VS_PROBE_CONTROL, 1, temp1, sizeof(temp1));
va009039 0:1ed23ab1345f 210 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 211 DBG_BYTES("GET_INFO Probe ", temp1, sizeof(temp1));
va009039 0:1ed23ab1345f 212
va009039 0:1ed23ab1345f 213 uint8_t temp[34];
va009039 0:1ed23ab1345f 214 rc = Control(GET_CUR, VS_PROBE_CONTROL, 1, temp, sizeof(temp));
va009039 0:1ed23ab1345f 215 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 216 DBG_BYTES("GET_CUR Probe ", temp, sizeof(temp));
va009039 0:1ed23ab1345f 217
va009039 0:1ed23ab1345f 218 uint8_t param[34];
va009039 0:1ed23ab1345f 219 memset(param, 0x00, sizeof(param));
va009039 0:1ed23ab1345f 220 param[0] = 0x00;
va009039 0:1ed23ab1345f 221 param[2] = cfg.FormatIndex;
va009039 0:1ed23ab1345f 222 param[3] = cfg.FrameIndex; // 160x120
va009039 0:1ed23ab1345f 223 LE32(cfg.dwFrameInterval, param+4); // Frame Interval
va009039 0:1ed23ab1345f 224
va009039 0:1ed23ab1345f 225 DBG_BYTES("SET_CUR Probe ", param, sizeof(param));
va009039 0:1ed23ab1345f 226 rc = Control(SET_CUR, VS_PROBE_CONTROL, 1, param, sizeof(param));
va009039 0:1ed23ab1345f 227 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 228
va009039 0:1ed23ab1345f 229 rc = Control(GET_CUR, VS_PROBE_CONTROL, 1, temp, sizeof(temp));
va009039 0:1ed23ab1345f 230 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 231 DBG_BYTES("GET_CUR Probe ", temp, sizeof(temp));
va009039 0:1ed23ab1345f 232
va009039 0:1ed23ab1345f 233 rc = Control(GET_CUR, VS_COMMIT_CONTROL, 1, temp, sizeof(temp));
va009039 0:1ed23ab1345f 234 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 235 DBG_BYTES("GET_CUR Commit", temp, sizeof(temp));
va009039 0:1ed23ab1345f 236
va009039 0:1ed23ab1345f 237 DBG_BYTES("SET_CUR Commit", param, sizeof(param));
va009039 0:1ed23ab1345f 238 rc = Control(SET_CUR, VS_COMMIT_CONTROL, 1, param, sizeof(param));
va009039 0:1ed23ab1345f 239 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 240
va009039 0:1ed23ab1345f 241 //USBH_SET_INTERFACE(1, 1); // alt=1 size=128
va009039 0:1ed23ab1345f 242 DBG_ASSERT(cfg.bInterface >= 1);
va009039 0:1ed23ab1345f 243 DBG_ASSERT(cfg.bAlternate >= 1);
va009039 0:1ed23ab1345f 244 DBG_ASSERT(cfg.bAlternate <= 6);
va009039 0:1ed23ab1345f 245 //rc = m_pDev->controlSend(
va009039 0:1ed23ab1345f 246 // USB_HOST_TO_DEVICE | USB_RECIPIENT_INTERFACE,
va009039 0:1ed23ab1345f 247 // SET_INTERFACE, cfg.bAlternate, cfg.bInterface, NULL, 0);
va009039 0:1ed23ab1345f 248 rc = m_pDev->SetInterfaceAlternate(cfg.bInterface, cfg.bAlternate);
va009039 0:1ed23ab1345f 249 DBG_ASSERT(rc == USBERR_OK);
va009039 0:1ed23ab1345f 250
va009039 0:1ed23ab1345f 251 DBG_ASSERT(cfg.iso_FrameCount >= 1);
va009039 0:1ed23ab1345f 252 DBG_ASSERT(cfg.iso_FrameCount <= 8);
va009039 0:1ed23ab1345f 253 m_FrameCount = cfg.iso_FrameCount;
va009039 0:1ed23ab1345f 254 DBG_ASSERT(cfg.iso_itdCount >= 1);
va009039 0:1ed23ab1345f 255 DBG_ASSERT(cfg.iso_itdCount <= 8);
va009039 0:1ed23ab1345f 256 m_itdCount = cfg.iso_itdCount;
va009039 0:1ed23ab1345f 257
va009039 0:1ed23ab1345f 258 LPC_USB->HcControl |= OR_CONTROL_PLE; // PeriodicListEnable
va009039 0:1ed23ab1345f 259 LPC_USB->HcControl |= OR_CONTROL_IE; // IsochronousEnable
va009039 0:1ed23ab1345f 260
va009039 0:1ed23ab1345f 261 m_connect = true;
va009039 0:1ed23ab1345f 262 return 0;
va009039 0:1ed23ab1345f 263 }