uvcini.cpp

00001 #include "mbed.h"
00002 #include "uvc.h"
00003 #define __DEBUG
00004 #include "mydbg.h"
00005 #include "stcamcfg.h"
00006 
00007 #define UNDEF_VAL  (0)
00008 const struct stcamcfg stcamcfg_table[] = {
00009 /*
00010 {0x045e, 0x074a, "Microsoft LifeCam VX-700",
00011 160,120, PAYLOAD_MJPEG,
00012 0x81, 128,
00013 1, 5, 2000000, // 160x120 5.0fps
00014 1, 1, 
00015 8, 3,
00016 },*/
00017 /*
00018 {0x0c45, 0x62c0, "UVCA130AF",
00019 160,120, PAYLOAD_MJPEG,
00020 0x81, 128,
00021 1, 5, 2000000, // 160x120 5.0fps
00022 1, 1, 
00023 8, 3,
00024 },*/
00025 
00026 {0x046d, 0x0994, "Logitech QuickCam Orbit AF",
00027 160,120, PAYLOAD_MJPEG,
00028 UNDEF_VAL, UNDEF_VAL,
00029 UNDEF_VAL, UNDEF_VAL, 2000000, // 160x120 10.0fps
00030 UNDEF_VAL, UNDEF_VAL, 
00031 UNDEF_VAL, 3,
00032 },
00033 {0x0000, 0x0000, "default",
00034 160,120, PAYLOAD_MJPEG,
00035 UNDEF_VAL, UNDEF_VAL,
00036 UNDEF_VAL, UNDEF_VAL, 2000000,
00037 UNDEF_VAL, UNDEF_VAL, 
00038 UNDEF_VAL, 3,
00039 },
00040 };
00041 
00042 inline void LE32(uint32_t n, uint8_t* d)
00043 {
00044     d[0] = (uint8_t)n;
00045     d[1] = (uint8_t)(n >> 8);
00046     d[2] = (uint8_t)(n >> 16);
00047     d[3] = (uint8_t)(n >> 24);
00048 }
00049 
00050 void uvc::SetFormatIndex(int index)
00051 {
00052     DBG_ASSERT(index >= 1);
00053     DBG_ASSERT(index <= 2);
00054     m_FormatIndex = index;
00055 }
00056 
00057 void uvc::SetFrameIndex(int index)
00058 {
00059     DBG_ASSERT(index >= 1);
00060     DBG_ASSERT(index <= 8);
00061     m_FrameIndex = index;
00062 }
00063 
00064 void uvc::SetFrameInterval(int val)
00065 {
00066     DBG_ASSERT(val >= 333333);
00067     DBG_ASSERT(val <= 10000000);
00068     m_FrameInterval = val;
00069 }
00070 
00071 void uvc::SetPacketSize(int size)
00072 {
00073     DBG_ASSERT(size >= 128);
00074     DBG_ASSERT(size <= 1024);
00075     m_PacketSize = size;
00076 }
00077 
00078 void uvc::SetImageSize(int width, int height)
00079 {
00080     DBG_ASSERT(width >= 160);
00081     DBG_ASSERT(width <= 800);
00082     DBG_ASSERT(height >= 120);
00083     DBG_ASSERT(height <= 600);
00084     m_width = width;
00085     m_height = height;
00086 }
00087 
00088 void uvc::SetPayload(int payload)
00089 {
00090     DBG_ASSERT(payload == PAYLOAD_MJPEG || payload == PAYLOAD_YUY2);
00091     m_payload = payload;
00092 }
00093 
00094 void uvc::poll()
00095 {
00096     isochronous();
00097 }
00098 
00099 int uvc::_init()
00100 {
00101     m_init = true;
00102     UsbErr rc;
00103     for(int i = 0; i < 2; i++) {
00104         m_pDev = m_pHost->getDeviceByClass(CLASS_VIDEO, m_cam); // UVC
00105         if (m_pDev || i > 0) {
00106             break;
00107         }
00108         rc = Usb_poll();
00109         if (rc == USBERR_PROCESSING) {
00110             VERBOSE("%p USBERR_PROCESSING\n", this);
00111             return -1;
00112         }
00113     }
00114     DBG("m_pDev=%p\n", m_pDev);
00115     if (!m_pDev) {
00116         VERBOSE("%p UVC CAMERA(%d) NOT FOUND\n", this, m_cam);
00117         return -1;
00118     }
00119     DBG_ASSERT(m_pDev);
00120     
00121     struct stcamcfg cfg;
00122     for(int i = 0; ; i++) {
00123         cfg = stcamcfg_table[i];
00124         if (cfg.idVender == 0x0000) {
00125             DBG("not cam config\n");
00126             DBG("vid: %04X\n", m_pDev->getVid());
00127             DBG("pid: %04X\n", m_pDev->getPid());
00128             break;
00129         }
00130         if (cfg.idVender == m_pDev->getVid() && cfg.idProduct ==  m_pDev->getPid()) {
00131             DBG_ASSERT(cfg.name);
00132             DBG("found %s\n", cfg.name);
00133             break;
00134         }
00135     }
00136 
00137     if (m_width) {
00138         cfg.width = m_width;
00139     }
00140     if (m_height) {
00141         cfg.height = m_height;
00142     }
00143     if (m_payload != PAYLOAD_UNDEF) {
00144         cfg.payload = m_payload;
00145     }
00146     if (m_FormatIndex) {
00147         cfg.FormatIndex = m_FormatIndex;
00148     }
00149     if (m_FrameIndex) {
00150         cfg.FrameIndex = m_FrameIndex;
00151     }
00152     if (m_FrameInterval) {
00153         cfg.dwFrameInterval = m_FrameInterval;
00154     }
00155     if (m_PacketSize) {
00156         cfg.wMaxPacketSize = m_PacketSize;
00157     }
00158 
00159     _config(&cfg);
00160 
00161     if (cfg.FormatIndex == UNDEF_VAL) {
00162         if (cfg.payload == PAYLOAD_MJPEG) {
00163             VERBOSE("MJPG(Motion JPEG) FORMAT NOT FOUND\n");
00164         } else if (cfg.payload == PAYLOAD_YUY2) {
00165             VERBOSE("YUY2 FORMAT NOT FOUND\n");
00166         } else {
00167             VERBOSE("??? FORMAT NOT FOUND\n");
00168         }
00169         return -1;
00170     }
00171 
00172     if (cfg.bInterface == UNDEF_VAL) {
00173         VERBOSE("PacketSize(%d) CONFIG ERROR\n", cfg.wMaxPacketSize);
00174         return -1;
00175     }
00176 
00177     if (cfg.iso_FrameCount == 0) {
00178         int c = usb_bp_size() / cfg.wMaxPacketSize;
00179         if (c > 8) {
00180             c = 8;
00181         }
00182         cfg.iso_FrameCount = c; 
00183     }
00184     DBG_ASSERT(cfg.iso_FrameCount >= 1);
00185     DBG_ASSERT(cfg.iso_FrameCount <= 8);
00186     DBG_ASSERT((cfg.iso_FrameCount * cfg.wMaxPacketSize) <= usb_bp_size());
00187     if (cfg.iso_itdCount == 0) {
00188         cfg.iso_itdCount = 3;
00189     }
00190     DBG_ASSERT(cfg.iso_itdCount >= 1);
00191     DBG("cfg.wMaxPacketSize=%d\n", cfg.wMaxPacketSize);
00192     DBG("cfg.iso_FrameCount=%d\n", cfg.iso_FrameCount);
00193     DBG("cfg.iso_itdCount=%d\n", cfg.iso_itdCount);
00194     DBG_ASSERT(cfg.iso_FrameCount >= 1 && cfg.iso_FrameCount <= 8);
00195     //m_pEpIntIn = new UsbEndpoint(m_pDev, 0x83, true, USB_INT, 16);
00196     //DBG_ASSERT(m_pEpIntIn);
00197     
00198     DBG_ASSERT(cfg.bEndpointAddress & 0x80);
00199     DBG_ASSERT(cfg.wMaxPacketSize >= 128);
00200     DBG_ASSERT(cfg.wMaxPacketSize <= 1020);
00201     m_PacketSize = cfg.wMaxPacketSize;
00202     
00203     DBG_ASSERT(m_PacketSize); 
00204     m_pEpIsoIn = new UsbEndpoint(m_pDev, cfg.bEndpointAddress, true, USB_ISO, m_PacketSize);
00205     DBG_ASSERT(m_pEpIsoIn);
00206 
00207     DBG_ASSERT(cfg.FormatIndex >= 1);
00208     DBG_ASSERT(cfg.FormatIndex <= 2);
00209     DBG_ASSERT(cfg.FrameIndex >= 1);
00210     DBG_ASSERT(cfg.FrameIndex <= 8);
00211     DBG_ASSERT(cfg.dwFrameInterval <= 10000000);
00212     DBG_ASSERT(cfg.dwFrameInterval >= 333333);
00213 
00214     probe_commit_control(&cfg);
00215 
00216     DBG("cfg.bInterface = %d, cfg.bAlternate = %d\n", cfg.bInterface, cfg.bAlternate);
00217     //USBH_SET_INTERFACE(1, 1); // alt=1 size=128
00218     DBG_ASSERT(cfg.bInterface >= 1); 
00219     DBG_ASSERT(cfg.bAlternate >= 1);
00220     DBG_ASSERT(cfg.bAlternate <= 7);
00221     rc = m_pDev->SetInterfaceAlternate(cfg.bInterface, cfg.bAlternate);
00222     DBG_ASSERT(rc == USBERR_OK);
00223 
00224     DBG_ASSERT(cfg.iso_FrameCount >= 1);
00225     DBG_ASSERT(cfg.iso_FrameCount <= 8);
00226     m_FrameCount = cfg.iso_FrameCount;
00227     
00228     DBG_ASSERT(cfg.iso_itdCount >= 1);
00229     DBG_ASSERT(cfg.iso_itdCount <= 8);
00230     m_itdCount = cfg.iso_itdCount;
00231     
00232     LPC_USB->HcControl |= OR_CONTROL_PLE; // PeriodicListEnable
00233     LPC_USB->HcControl |= OR_CONTROL_IE;  // IsochronousEnable
00234 
00235     m_connect = true;
00236     return 0;
00237 }
00238 
00239 static void dump_param(uint8_t* buf)
00240 {
00241     VERBOSE("Hint: %04x\n", LE16(buf+0));
00242     VERBOSE("FormatIndex: %d\n", buf[2]);
00243     VERBOSE("FrameIndex: %d\n", buf[3]);
00244     VERBOSE("FrameInterval: %d\n", LE32(buf+4));
00245     VERBOSE("KeyFrameRate: %d\n", LE16(buf+8));
00246     VERBOSE("PFrameRate: %d\n", LE16(buf+10));
00247     VERBOSE("CompQuality: %d\n", LE16(buf+12));
00248     VERBOSE("CompWindowSize: %d\n", LE16(buf+14));
00249     VERBOSE("Delay: %d\n", LE16(buf+16));
00250     VERBOSE("MaxVideoFrameSize: %d\n", LE32(buf+18));
00251     VERBOSE("MaxPayloadTransferSize: %d\n", LE32(buf+22));
00252 }
00253 
00254 void uvc::probe_commit_control(struct stcamcfg* cfg)
00255 {
00256     DBG_ASSERT(cfg);
00257     uint8_t param[34];
00258     uint8_t temp[34];
00259 
00260     int param_len = 34;
00261     DBG_ASSERT(cfg->bcdUVC >= 0x0100);
00262     if (cfg->bcdUVC == 0x0100) { // UVC ver. 1.0
00263         param_len = 26;
00264     }
00265 
00266     UsbErr rc;
00267     rc = Control(GET_INFO, VS_PROBE_CONTROL, 1, param, 1);
00268     DBG_ASSERT(rc == USBERR_OK);
00269     DBG_BYTES("GET_INFO Probe ", param, 1);
00270 
00271     rc = Control(GET_DEF, VS_PROBE_CONTROL, 1, temp, param_len);
00272     DBG_ASSERT(rc == USBERR_OK);
00273     DBG_BYTES("GET_DEF Probe ", temp, param_len);
00274     dump_param(temp);
00275     
00276     rc = Control(GET_MIN, VS_PROBE_CONTROL, 1, temp, param_len);
00277     DBG_ASSERT(rc == USBERR_OK);
00278     DBG_BYTES("GET_MIN Probe ", temp, param_len);
00279     dump_param(temp);
00280     
00281     rc = Control(GET_MAX, VS_PROBE_CONTROL, 1, temp, param_len);
00282     DBG_ASSERT(rc == USBERR_OK);
00283     DBG_BYTES("GET_MAX Probe ", temp, param_len);
00284     dump_param(temp);
00285     
00286     rc = Control(GET_CUR, VS_PROBE_CONTROL, 1, temp, param_len);
00287     DBG_ASSERT(rc == USBERR_OK);
00288     DBG_BYTES("GET_CUR Probe ", temp, param_len);
00289     dump_param(temp);
00290     
00291     memset(param, 0x00, param_len);
00292     param[2] = cfg->FormatIndex;
00293     param[3] = cfg->FrameIndex; // 160x120
00294     LE32(cfg->dwFrameInterval, param+4); // Frame Interval
00295 
00296     DBG_BYTES("SET_CUR Probe ", param, param_len);
00297     rc = Control(SET_CUR, VS_PROBE_CONTROL, 1, param, param_len);
00298     DBG_ASSERT(rc == USBERR_OK);
00299 
00300     rc = Control(GET_CUR, VS_PROBE_CONTROL, 1, temp, param_len);
00301     DBG_ASSERT(rc == USBERR_OK);
00302     DBG_BYTES("GET_CUR Probe ", temp, param_len);
00303 
00304     rc = Control(GET_INFO, VS_COMMIT_CONTROL, 1, temp, param_len);
00305     DBG_ASSERT(rc == USBERR_OK);
00306     DBG_BYTES("GET_INFO Commit", temp, 1);
00307 
00308     rc = Control(GET_CUR, VS_COMMIT_CONTROL, 1, temp, param_len);
00309     DBG_ASSERT(rc == USBERR_OK);
00310     DBG_BYTES("GET_CUR Commit", temp, param_len);
00311     dump_param(temp);
00312 
00313     DBG_BYTES("SET_CUR Commit", param, param_len);
00314     dump_param(param);
00315     rc = Control(SET_CUR, VS_COMMIT_CONTROL, 1, param, param_len);
00316     if (rc != USBERR_OK) {
00317         VERBOSE("Error SET_CUR VS_COMMIT_CONTROL\n");
00318     }
00319     DBG_ASSERT(rc == USBERR_OK);
00320 
00321     rc = Control(GET_CUR, VS_COMMIT_CONTROL, 1, temp, param_len);
00322     DBG_ASSERT(rc == USBERR_OK);
00323     DBG_BYTES("GET_CUR Commit", temp, param_len);
00324     dump_param(temp);
00325 }