usb_mjpeg.cpp

00001 #include "mbed.h"
00002 #include "usb_mjpeg.h"
00003 //#define __DEBUG
00004 #define __DEBUG2
00005 #include "mydbg.h"
00006 #include "Utils.h"
00007 #include "myjpeg.h"
00008 
00009 #ifdef __DEBUG
00010 extern DigitalOut led4;
00011 #endif // __DEBUG
00012 
00013 #define MJPEG_FID 0x01
00014 #define MJPEG_EOF 0x02
00015 #define MJPEG_PTS 0x04
00016 #define MJPEG_SCR 0x08
00017 #define MJPEG_STI 0x20
00018 #define MJPEG_ERR 0x40
00019 #define MJPEG_EOH 0x80
00020 
00021 
00022 usb_mjpeg::usb_mjpeg(uint8_t* buf, int size)
00023 {
00024     DBG_ASSERT(size >= 1024 && size <= 16000);
00025     m_size = size;
00026     m_seq = 0;
00027     m_buf = buf;
00028     ReportErrorFID = 0;
00029     ReportErrorPTS = 0;
00030 }
00031 
00032 usb_mjpeg::~usb_mjpeg()
00033 {
00034 
00035 }
00036 
00037 void usb_mjpeg::input(uint16_t frame, uint8_t* buf, int len)
00038 {
00039     uint8_t* StreamHeader = buf;
00040     DBG_ASSERT(buf);
00041     if (len > 12) {
00042         //DBG_PRINTF("%d %02X %d\n", frame, buf[1], len);
00043         //DBG_HEX(buf, len);
00044         DBG_ASSERT(StreamHeader[0] == 0x0c);
00045         //DBG_ASSERT(buf[1] == 0x8c || buf[1] == 0x8d);
00046         //DBG("bfh:%02X\n", buf[1]);
00047     }
00048     DBG_ASSERT(len >= 2);
00049 
00050     int hle = StreamHeader[0];
00051     uint8_t bfh = StreamHeader[1];
00052     DBG_ASSERT(len >= hle);
00053     DBG_ASSERT(hle == 12);
00054     uint8_t* data = buf + hle;
00055     int data_len = len - hle;
00056     if (m_seq == 0) {
00057         if (check_SOI(buf, len)) {
00058             DBG_PRINTF("%04X SOI\n", frame);
00059             DBG_BYTES("SOI", buf, 16);
00060             _open();
00061             m_bfh = bfh; // save FID
00062             if (bfh & MJPEG_PTS) {
00063                 m_pts = LE32(StreamHeader+2); // save PTS
00064             }
00065             _wrtie(data, data_len);
00066             m_seq++;
00067         }
00068     } else if (m_seq == 1) {
00069         if (len > hle) {
00070             _wrtie(data, data_len);
00071         }
00072         if (check_EOI(buf, len)) {
00073             DBG_PRINTF("%04X EOI\n", frame);
00074             DBG_BYTES("EOI", buf, 12);
00075             _close();
00076             m_seq = 2; // done
00077         } else if ((m_bfh ^ bfh) & MJPEG_FID) {
00078             ReportErrorFID++;
00079             DBG("ReportErrorFID=%d\n", ReportErrorFID);
00080             m_seq = 0; // restart
00081         } else if ((bfh & MJPEG_PTS) && m_pts != LE32(StreamHeader+2)) {
00082             ReportErrorPTS++;
00083             DBG("ReportErrorPTS=%d\n", ReportErrorPTS);
00084             m_seq = 0; //  restart
00085         }
00086     } else { // done
00087     }
00088     DBG_LED4(buf[1] & MJPEG_FID); // FID
00089 }
00090 
00091 int usb_mjpeg::status()
00092 {
00093     if (m_seq <= 1) {
00094         return USBERR_PROCESSING;
00095     }    
00096     return m_pos;
00097 }
00098 
00099 bool usb_mjpeg::check_SOI(uint8_t* buf, int len)
00100 {
00101     if (len >= (12+2)) {
00102         if (buf[12] == 0xff && buf[13] == 0xd8) {
00103             return true;
00104         }
00105     }
00106     return false;
00107 }
00108 
00109 bool usb_mjpeg::check_EOI(uint8_t* buf, int len)
00110 {
00111     if (len >= 12) {
00112         if (buf[1] & MJPEG_EOF) {
00113             return true;
00114         }
00115     }
00116     return false;
00117 }
00118 
00119 void usb_mjpeg::_open()
00120 {
00121     m_pos = 0;
00122 }
00123 
00124 void usb_mjpeg::_wrtie(uint8_t* buf, int len)
00125 {
00126     if (m_buf == NULL) {
00127         return;
00128     }
00129     for(int i = 0; i < len && m_pos < m_size; i++) {
00130         m_buf[m_pos++] = buf[i];
00131     }
00132 }
00133 
00134 void usb_mjpeg::_close()
00135 {
00136     //DBG("m_pos=%d\n", m_pos);
00137     //DBG_HEX(m_buf, m_pos);
00138     myjpeg JPEG(m_buf, m_pos, m_size);
00139     JPEG.analytics();
00140     if (JPEG.DHT_pos == 0) {
00141         m_pos = JPEG.insertDHT();
00142     }
00143 }