File content as of revision 0:de03cbbcd0ff:

#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);
}