Diff: uvc/myjpeg.cpp

Revision:

0:de03cbbcd0ff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/uvc/myjpeg.cpp	Mon Nov 30 09:32:15 2015 +0000
@@ -0,0 +1,252 @@
+#include "mbed.h"
+//#define __DEBUG
+#include "mydbg.h"
+#include "myjpeg.h"
+
+static const uint8_t dht[] = {
+0xFF,0xC4,0x01,0xA2,0x00,0x00,0x01,0x05,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,
+0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,
+0x0B,0x01,0x00,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,
+0x00,0x00,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,0x10,0x00,
+0x02,0x01,0x03,0x03,0x02,0x04,0x03,0x05,0x05,0x04,0x04,0x00,0x00,0x01,0x7D,0x01,
+0x02,0x03,0x00,0x04,0x11,0x05,0x12,0x21,0x31,0x41,0x06,0x13,0x51,0x61,0x07,0x22,
+0x71,0x14,0x32,0x81,0x91,0xA1,0x08,0x23,0x42,0xB1,0xC1,0x15,0x52,0xD1,0xF0,0x24,
+0x33,0x62,0x72,0x82,0x09,0x0A,0x16,0x17,0x18,0x19,0x1A,0x25,0x26,0x27,0x28,0x29,
+0x2A,0x34,0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,
+0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,
+0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,
+0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,
+0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,
+0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,0xD9,0xDA,0xE1,0xE2,0xE3,
+0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF1,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,0xF8,0xF9,
+0xFA,0x11,0x00,0x02,0x01,0x02,0x04,0x04,0x03,0x04,0x07,0x05,0x04,0x04,0x00,0x01,
+0x02,0x77,0x00,0x01,0x02,0x03,0x11,0x04,0x05,0x21,0x31,0x06,0x12,0x41,0x51,0x07,
+0x61,0x71,0x13,0x22,0x32,0x81,0x08,0x14,0x42,0x91,0xA1,0xB1,0xC1,0x09,0x23,0x33,
+0x52,0xF0,0x15,0x62,0x72,0xD1,0x0A,0x16,0x24,0x34,0xE1,0x25,0xF1,0x17,0x18,0x19,
+0x1A,0x26,0x27,0x28,0x29,0x2A,0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,
+0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,
+0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0x82,0x83,0x84,0x85,
+0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9A,0xA2,0xA3,
+0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,0xBA,
+0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,
+0xD9,0xDA,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF2,0xF3,0xF4,0xF5,0xF6,
+0xF7,0xF8,0xF9,0xFA,
+};
+
+myjpeg::myjpeg(uint8_t* buf, int len, int capacity)
+{
+    DBG_ASSERT(buf);
+    DBG_ASSERT(len >= 0);
+    m_buf = buf;
+    m_len = len;
+    m_capacity = capacity;
+}
+
+int myjpeg::getc()
+{
+    if (m_pos >= m_len) {
+        return -1;
+    }
+    return m_buf[m_pos++];    
+}
+
+int myjpeg::getBE16()
+{
+    int c1 = getc();
+    if (c1 == (-1)) {
+        return -1;
+    }
+    int c2 = getc();
+    if (c2 == (-1)) {
+        return -1;
+    }
+    return c1<<8|c2;
+}
+
+void myjpeg::analytics()
+{
+    m_pos = 0;
+    SOS_pos = 0;
+    DHT_pos = 0;
+    int skip;
+    int lp;
+    while(1) {
+        int marker_pos = m_pos;
+        int c = getc();
+        if (c == (-1)) {
+            break;
+        }
+        if (c != 0xff) {
+            continue;
+        }
+        c = getc();
+        if (c == (-1)) {
+            break;
+        }
+        uint8_t marker = c;
+        switch(marker) {
+            case 0xd8: 
+                DBG("%04X SOI\n", marker_pos);
+                skip = 0;
+                break;
+            case 0xd9: 
+                DBG("%04X EOI\n", marker_pos);
+                skip = 0;  
+                break;
+            case 0x00:
+                skip = 0;
+                break;
+            case 0xc0:
+                lp = getBE16();
+                DBG("%04X SOF0 %d\n", marker_pos, lp);
+                skip = lp - 2;
+                break;
+            case 0xc4:
+                lp = getBE16();
+                DBG("%04X DHT Lh: %d\n", marker_pos, lp);
+                //DBG("Tc: %d\n", buf[pos+2]>>4);
+                //DBG("Th: %d\n", buf[pos+2]&0xf);
+                DHT_pos = marker_pos;
+                skip = lp - 2;
+                break;
+            case 0xda:
+                lp = getBE16();
+                DBG("%04X SOS Ls: %d\n", marker_pos, lp);
+                //DBG("Ns: %d\n", buf[pos+2]);
+                //for(i = 1; i <= buf[pos+2]; i++) {
+                //    DBG("Cs%d: %d\n", i, buf[pos+3+(i-1)*2]);
+                //    DBG("Td%d: %d\n", i, buf[pos+4+(i-1)*2]>>4);
+                //    DBG("Ta%d: %d\n", i, buf[pos+4+(i-1)*2]&0xf);
+                //}
+                SOS_pos = marker_pos;
+                skip = lp - 2;
+                break;
+            case 0xdb:
+                lp = getBE16();
+                DBG("%04X DQT %d\n", marker_pos, lp);
+                skip = lp - 2;
+                break;
+            case 0xe0: 
+                lp = getBE16();
+                DBG("%04X APP0 %d\n", marker_pos, lp);
+                skip = lp - 2;
+                break;
+            default:
+                DBG("%04X ??? %02X\n", marker_pos, marker);
+                skip = 0;
+                break;
+        }
+        while(skip-- > 0) {
+            getc();
+        }
+    }
+}
+
+int myjpeg::insertDHT()
+{
+    DBG("m_len=%d m_capacity=%d SOS=%d\n", m_len, m_capacity, SOS_pos);
+    DBG_ASSERT(SOS_pos > 0);
+    DBG_ASSERT(SOS_pos < m_len);
+    DBG_ASSERT(m_len <= m_capacity);
+    DBG_ASSERT(sizeof(dht) == 420);
+    
+    int num1 = m_len - SOS_pos;
+    if (num1 > (m_capacity - SOS_pos - sizeof(dht))) {
+        num1 = m_capacity - SOS_pos - sizeof(dht);
+    }
+    DBG("num1=%d\n", num1);
+    DBG_ASSERT(SOS_pos+sizeof(dht)+num1 <= m_capacity);
+    memmove(m_buf+SOS_pos+sizeof(dht), m_buf+SOS_pos, num1);
+
+    int num2 = sizeof(dht);
+    if (num2 > m_capacity - SOS_pos) {
+        num2 = m_capacity - SOS_pos;
+    } 
+    DBG("num2=%d\n", num2);
+    DBG_ASSERT(SOS_pos+num2 <= m_capacity);
+    memcpy(m_buf+SOS_pos, dht, num2);
+    m_len += sizeof(dht);
+    if (m_len > m_capacity) {
+        m_len = m_capacity;
+    }
+    return m_len;
+}
+
+int fgetBE16(FILE* fp)
+{
+    int c1 = fgetc(fp);
+    if (c1 == EOF) {
+        return -1;
+    }
+    int c2 = fgetc(fp);
+    if (c2 == EOF) {
+        return -1;
+    }
+    return c1<<8|c2;
+}
+
+void QcamCopy(const char* destination, const char* source)
+{
+    FILE* fp;
+    FILE* fp2;
+    fp = fopen(source, "rb");
+    if (fp == NULL) {
+        return;
+    }
+    fp2 = fopen(destination, "wb");
+    if (fp2 == NULL) {
+        return;
+    }
+    int i,c1,c2;
+    bool f_dht = false;
+    bool f_lp = false;
+    while(!feof(fp)){
+        c1 = fgetc(fp);
+        if (c1 != 0xff) {
+            fputc(c1, fp2);
+            continue;
+        }
+        c2 = fgetc(fp);
+        switch(c2) {
+            case 0xda: // SOS
+                if (!f_dht) {
+                    for(i = 0; i < sizeof(dht); i++) {
+                        fputc(dht[i], fp2);
+                    }
+                    f_dht = true;
+                }
+                f_lp = true;
+                break;
+            case 0xc4: // DHT
+                f_dht = true;
+                f_lp = true;
+                break;
+            case 0xc0:
+            case 0xdb:
+            case 0xe0:
+                f_lp = true;
+                break;
+            default:
+                f_lp = false;
+                break;
+        }
+        fputc(c1, fp2); 
+        fputc(c2, fp2); // marker
+        if (f_lp) {
+            c1 = fgetc(fp); // length
+            c2 = fgetc(fp);
+            fputc(c1, fp2);
+            fputc(c2, fp2);
+            int skip = c1<<8|c2;
+            while(skip-- > 2) {
+                int c = fgetc(fp);
+                if (c == EOF) {
+                    break;
+                }
+                fputc(c, fp2);
+            }
+        }
+    }
+    fclose(fp);
+    fclose(fp2);
+}