myjpeg.cpp

00001 #include "mbed.h"
00002 //#define __DEBUG
00003 #include "mydbg.h"
00004 #include "myjpeg.h"
00005 
00006 static const uint8_t dht[] = {
00007 0xFF,0xC4,0x01,0xA2,0x00,0x00,0x01,0x05,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,
00008 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,
00009 0x0B,0x01,0x00,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,
00010 0x00,0x00,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,0x10,0x00,
00011 0x02,0x01,0x03,0x03,0x02,0x04,0x03,0x05,0x05,0x04,0x04,0x00,0x00,0x01,0x7D,0x01,
00012 0x02,0x03,0x00,0x04,0x11,0x05,0x12,0x21,0x31,0x41,0x06,0x13,0x51,0x61,0x07,0x22,
00013 0x71,0x14,0x32,0x81,0x91,0xA1,0x08,0x23,0x42,0xB1,0xC1,0x15,0x52,0xD1,0xF0,0x24,
00014 0x33,0x62,0x72,0x82,0x09,0x0A,0x16,0x17,0x18,0x19,0x1A,0x25,0x26,0x27,0x28,0x29,
00015 0x2A,0x34,0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,
00016 0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,
00017 0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,
00018 0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,
00019 0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,
00020 0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,0xD9,0xDA,0xE1,0xE2,0xE3,
00021 0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF1,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,0xF8,0xF9,
00022 0xFA,0x11,0x00,0x02,0x01,0x02,0x04,0x04,0x03,0x04,0x07,0x05,0x04,0x04,0x00,0x01,
00023 0x02,0x77,0x00,0x01,0x02,0x03,0x11,0x04,0x05,0x21,0x31,0x06,0x12,0x41,0x51,0x07,
00024 0x61,0x71,0x13,0x22,0x32,0x81,0x08,0x14,0x42,0x91,0xA1,0xB1,0xC1,0x09,0x23,0x33,
00025 0x52,0xF0,0x15,0x62,0x72,0xD1,0x0A,0x16,0x24,0x34,0xE1,0x25,0xF1,0x17,0x18,0x19,
00026 0x1A,0x26,0x27,0x28,0x29,0x2A,0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,
00027 0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,
00028 0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0x82,0x83,0x84,0x85,
00029 0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9A,0xA2,0xA3,
00030 0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,0xBA,
00031 0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,
00032 0xD9,0xDA,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF2,0xF3,0xF4,0xF5,0xF6,
00033 0xF7,0xF8,0xF9,0xFA,
00034 };
00035 
00036 myjpeg::myjpeg(uint8_t* buf, int len, int capacity)
00037 {
00038     DBG_ASSERT(buf);
00039     DBG_ASSERT(len >= 0);
00040     m_buf = buf;
00041     m_len = len;
00042     m_capacity = capacity;
00043 }
00044 
00045 int myjpeg::getc()
00046 {
00047     if (m_pos >= m_len) {
00048         return -1;
00049     }
00050     return m_buf[m_pos++];    
00051 }
00052 
00053 int myjpeg::getBE16()
00054 {
00055     int c1 = getc();
00056     if (c1 == (-1)) {
00057         return -1;
00058     }
00059     int c2 = getc();
00060     if (c2 == (-1)) {
00061         return -1;
00062     }
00063     return c1<<8|c2;
00064 }
00065 
00066 void myjpeg::analytics()
00067 {
00068     m_pos = 0;
00069     SOS_pos = 0;
00070     DHT_pos = 0;
00071     int skip;
00072     int lp;
00073     while(1) {
00074         int marker_pos = m_pos;
00075         int c = getc();
00076         if (c == (-1)) {
00077             break;
00078         }
00079         if (c != 0xff) {
00080             continue;
00081         }
00082         c = getc();
00083         if (c == (-1)) {
00084             break;
00085         }
00086         uint8_t marker = c;
00087         switch(marker) {
00088             case 0xd8: 
00089                 DBG("%04X SOI\n", marker_pos);
00090                 skip = 0;
00091                 break;
00092             case 0xd9: 
00093                 DBG("%04X EOI\n", marker_pos);
00094                 skip = 0;  
00095                 break;
00096             case 0x00:
00097                 skip = 0;
00098                 break;
00099             case 0xc0:
00100                 lp = getBE16();
00101                 DBG("%04X SOF0 %d\n", marker_pos, lp);
00102                 skip = lp - 2;
00103                 break;
00104             case 0xc4:
00105                 lp = getBE16();
00106                 DBG("%04X DHT Lh: %d\n", marker_pos, lp);
00107                 //DBG("Tc: %d\n", buf[pos+2]>>4);
00108                 //DBG("Th: %d\n", buf[pos+2]&0xf);
00109                 DHT_pos = marker_pos;
00110                 skip = lp - 2;
00111                 break;
00112             case 0xda:
00113                 lp = getBE16();
00114                 DBG("%04X SOS Ls: %d\n", marker_pos, lp);
00115                 //DBG("Ns: %d\n", buf[pos+2]);
00116                 //for(i = 1; i <= buf[pos+2]; i++) {
00117                 //    DBG("Cs%d: %d\n", i, buf[pos+3+(i-1)*2]);
00118                 //    DBG("Td%d: %d\n", i, buf[pos+4+(i-1)*2]>>4);
00119                 //    DBG("Ta%d: %d\n", i, buf[pos+4+(i-1)*2]&0xf);
00120                 //}
00121                 SOS_pos = marker_pos;
00122                 skip = lp - 2;
00123                 break;
00124             case 0xdb:
00125                 lp = getBE16();
00126                 DBG("%04X DQT %d\n", marker_pos, lp);
00127                 skip = lp - 2;
00128                 break;
00129             case 0xe0: 
00130                 lp = getBE16();
00131                 DBG("%04X APP0 %d\n", marker_pos, lp);
00132                 skip = lp - 2;
00133                 break;
00134             default:
00135                 DBG("%04X ??? %02X\n", marker_pos, marker);
00136                 skip = 0;
00137                 break;
00138         }
00139         while(skip-- > 0) {
00140             getc();
00141         }
00142     }
00143 }
00144 
00145 int myjpeg::insertDHT()
00146 {
00147     DBG("m_len=%d m_capacity=%d SOS=%d\n", m_len, m_capacity, SOS_pos);
00148     DBG_ASSERT(SOS_pos > 0);
00149     DBG_ASSERT(SOS_pos < m_len);
00150     DBG_ASSERT(m_len <= m_capacity);
00151     DBG_ASSERT(sizeof(dht) == 420);
00152     
00153     int num1 = m_len - SOS_pos;
00154     if (num1 > (m_capacity - SOS_pos - sizeof(dht))) {
00155         num1 = m_capacity - SOS_pos - sizeof(dht);
00156     }
00157     DBG("num1=%d\n", num1);
00158     DBG_ASSERT(SOS_pos+sizeof(dht)+num1 <= m_capacity);
00159     memmove(m_buf+SOS_pos+sizeof(dht), m_buf+SOS_pos, num1);
00160 
00161     int num2 = sizeof(dht);
00162     if (num2 > m_capacity - SOS_pos) {
00163         num2 = m_capacity - SOS_pos;
00164     } 
00165     DBG("num2=%d\n", num2);
00166     DBG_ASSERT(SOS_pos+num2 <= m_capacity);
00167     memcpy(m_buf+SOS_pos, dht, num2);
00168     m_len += sizeof(dht);
00169     if (m_len > m_capacity) {
00170         m_len = m_capacity;
00171     }
00172     return m_len;
00173 }
00174 
00175 int fgetBE16(FILE* fp)
00176 {
00177     int c1 = fgetc(fp);
00178     if (c1 == EOF) {
00179         return -1;
00180     }
00181     int c2 = fgetc(fp);
00182     if (c2 == EOF) {
00183         return -1;
00184     }
00185     return c1<<8|c2;
00186 }
00187 
00188 void QcamCopy(const char* destination, const char* source)
00189 {
00190     FILE* fp;
00191     FILE* fp2;
00192     fp = fopen(source, "rb");
00193     if (fp == NULL) {
00194         return;
00195     }
00196     fp2 = fopen(destination, "wb");
00197     if (fp2 == NULL) {
00198         return;
00199     }
00200     int i,c1,c2;
00201     bool f_dht = false;
00202     bool f_lp = false;
00203     while(!feof(fp)){
00204         c1 = fgetc(fp);
00205         if (c1 != 0xff) {
00206             fputc(c1, fp2);
00207             continue;
00208         }
00209         c2 = fgetc(fp);
00210         switch(c2) {
00211             case 0xda: // SOS
00212                 if (!f_dht) {
00213                     for(i = 0; i < sizeof(dht); i++) {
00214                         fputc(dht[i], fp2);
00215                     }
00216                     f_dht = true;
00217                 }
00218                 f_lp = true;
00219                 break;
00220             case 0xc4: // DHT
00221                 f_dht = true;
00222                 f_lp = true;
00223                 break;
00224             case 0xc0:
00225             case 0xdb:
00226             case 0xe0:
00227                 f_lp = true;
00228                 break;
00229             default:
00230                 f_lp = false;
00231                 break;
00232         }
00233         fputc(c1, fp2); 
00234         fputc(c2, fp2); // marker
00235         if (f_lp) {
00236             c1 = fgetc(fp); // length
00237             c2 = fgetc(fp);
00238             fputc(c1, fp2);
00239             fputc(c2, fp2);
00240             int skip = c1<<8|c2;
00241             while(skip-- > 2) {
00242                 int c = fgetc(fp);
00243                 if (c == EOF) {
00244                     break;
00245                 }
00246                 fputc(c, fp2);
00247             }
00248         }
00249     }
00250     fclose(fp);
00251     fclose(fp2);
00252 }