File content as of revision 0:1ed23ab1345f:

#include "mbed.h"
#include "usb_mjpeg.h"
//#define __DEBUG
#define __DEBUG2
#include "mydbg.h"
#include "Utils.h"
#include "myjpeg.h"

#ifdef __DEBUG
extern DigitalOut led4;
#endif // __DEBUG

#define MJPEG_FID 0x01
#define MJPEG_EOF 0x02
#define MJPEG_PTS 0x04
#define MJPEG_SCR 0x08
#define MJPEG_STI 0x20
#define MJPEG_ERR 0x40
#define MJPEG_EOH 0x80


usb_mjpeg::usb_mjpeg(uint8_t* buf, int size)
{
    DBG_ASSERT(size >= 1024 && size <= 16000);
    m_size = size;
    m_seq = 0;
    m_buf = buf;
    ReportErrorFID = 0;
    ReportErrorPTS = 0;
}

usb_mjpeg::~usb_mjpeg()
{

}

void usb_mjpeg::input(uint16_t frame, uint8_t* buf, int len)
{
    uint8_t* StreamHeader = buf;
    DBG_ASSERT(buf);
    if (len > 12) {
        //DBG_PRINTF("%d %02X %d\n", frame, buf[1], len);
        //DBG_HEX(buf, len);
        DBG_ASSERT(StreamHeader[0] == 0x0c);
        //DBG_ASSERT(buf[1] == 0x8c || buf[1] == 0x8d);
        //DBG("bfh:%02X\n", buf[1]);
    }
    DBG_ASSERT(len >= 2);

    int hle = StreamHeader[0];
    uint8_t bfh = StreamHeader[1];
    DBG_ASSERT(len >= hle);
    DBG_ASSERT(hle == 12);
    uint8_t* data = buf + hle;
    int data_len = len - hle;
    if (m_seq == 0) {
        if (check_SOI(buf, len)) {
            DBG_PRINTF("%04X SOI\n", frame);
            DBG_BYTES("SOI", buf, 16);
            _open();
            m_bfh = bfh; // save FID
            if (bfh & MJPEG_PTS) {
                m_pts = LE32(StreamHeader+2); // save PTS
            }
            _wrtie(data, data_len);
            m_seq++;
        }
    } else if (m_seq == 1) {
        if (len > hle) {
            _wrtie(data, data_len);
        }
        if (check_EOI(buf, len)) {
            DBG_PRINTF("%04X EOI\n", frame);
            DBG_BYTES("EOI", buf, 12);
            _close();
            m_seq = 2; // done
        } else if ((m_bfh ^ bfh) & MJPEG_FID) {
            ReportErrorFID++;
            DBG("ReportErrorFID=%d\n", ReportErrorFID);
            m_seq = 0; // restart
        } else if ((bfh & MJPEG_PTS) && m_pts != LE32(StreamHeader+2)) {
            ReportErrorPTS++;
            DBG("ReportErrorPTS=%d\n", ReportErrorPTS);
            m_seq = 0; //  restart
        }
    } else { // done
    }
    DBG_LED4(buf[1] & MJPEG_FID); // FID
}

int usb_mjpeg::status()
{
    if (m_seq <= 1) {
        return USBERR_PROCESSING;
    }    
    return m_pos;
}

bool usb_mjpeg::check_SOI(uint8_t* buf, int len)
{
    if (len >= (12+2)) {
        if (buf[12] == 0xff && buf[13] == 0xd8) {
            return true;
        }
    }
    return false;
}

bool usb_mjpeg::check_EOI(uint8_t* buf, int len)
{
    if (len >= 12) {
        if (buf[1] & MJPEG_EOF) {
            return true;
        }
    }
    return false;
}

void usb_mjpeg::_open()
{
    m_pos = 0;
}

void usb_mjpeg::_wrtie(uint8_t* buf, int len)
{
    if (m_buf == NULL) {
        return;
    }
    for(int i = 0; i < len && m_pos < m_size; i++) {
        m_buf[m_pos++] = buf[i];
    }
}

void usb_mjpeg::_close()
{
    //DBG("m_pos=%d\n", m_pos);
    //DBG_HEX(m_buf, m_pos);
    myjpeg JPEG(m_buf, m_pos, m_size);
    JPEG.analytics();
    if (JPEG.DHT_pos == 0) {
        m_pos = JPEG.insertDHT();
    }
}