From 50eeaf77e530255d6f5b3d85727f22ab8ca5b7b4 Mon Sep 17 00:00:00 2001 From: Philip Heron Date: Mon, 30 May 2011 23:37:33 +0100 Subject: [PATCH] Initial check-in --- Makefile | 14 + main.c | 154 ++++++++ rs8.c | 318 ++++++++++++++++ rs8.h | 23 ++ ssdv.c | 1055 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ ssdv.h | 116 ++++++ 6 files changed, 1680 insertions(+) create mode 100644 Makefile create mode 100644 main.c create mode 100644 rs8.c create mode 100644 rs8.h create mode 100644 ssdv.c create mode 100644 ssdv.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..7ea30b5 --- /dev/null +++ b/Makefile @@ -0,0 +1,14 @@ + +CC=gcc +CFLAGS=-g -Wall +LDFLAGS=-g + +ssdv: main.o ssdv.o rs8.o + $(CC) $(LDFLAGS) main.o ssdv.o rs8.o -o ssdv + +.c.o: + $(CC) $(CFLAGS) -c $< -o $@ + +clean: + rm *.o + diff --git a/main.c b/main.c new file mode 100644 index 0000000..f869ea7 --- /dev/null +++ b/main.c @@ -0,0 +1,154 @@ + +#include +#include +#include +#include +#include +#include "ssdv.h" + +void exit_usage() +{ + fprintf(stderr, "Usage: ssdv [-e|-d] [-t ] [-i ] [] []\n"); + exit(-1); +} + +int main(int argc, char *argv[]) +{ + int c, i; + FILE *fin = stdin; + FILE *fout = stdout; + char encode = -1; + int droptest = 0; + uint8_t image_id = 0; + ssdv_t ssdv; + + uint8_t pkt[SSDV_PKT_SIZE], b[128], *jpeg; + size_t jpeg_length; + + opterr = 0; + while((c = getopt(argc, argv, "edi:t:")) != -1) + { + switch(c) + { + case 'e': encode = 1; break; + case 'd': encode = 0; break; + case 'i': image_id = atoi(optarg); break; + case 't': droptest = atoi(optarg); break; + case '?': exit_usage(); + } + } + + c = argc - optind; + if(c > 2) exit_usage(); + + for(i = 0; i < c; i++) + { + if(!strcmp(argv[optind + i], "-")) continue; + + switch(i) + { + case 0: + fin = fopen(argv[optind + i], "rb"); + if(!fin) + { + fprintf(stderr, "Error opening '%s' for input:\n", argv[optind + i]); + perror("fopen"); + return(-1); + } + break; + + case 1: + fout = fopen(argv[optind + i], "wb"); + if(!fout) + { + fprintf(stderr, "Error opening '%s' for output:\n", argv[optind + i]); + perror("fopen"); + return(-1); + } + break; + } + } + + switch(encode) + { + case 0: /* Decode */ + if(droptest > 0) fprintf(stderr, "*** NOTE: Drop test enabled: %i ***\n", droptest); + + ssdv_dec_init(&ssdv); + + jpeg_length = 1024 * 1024 * 4; + jpeg = malloc(jpeg_length); + ssdv_dec_set_buffer(&ssdv, jpeg, jpeg_length); + + i = 0; + while(fread(pkt, 1, SSDV_PKT_SIZE, fin) > 0) + { + /* Drop % of packets */ + if(droptest && (rand() / (RAND_MAX / 100) < droptest)) continue; + + /* Test the packet is valid */ + if(ssdv_dec_is_packet(pkt, NULL) != 0) continue; + + /* Feed it to the decoder */ + ssdv_dec_feed(&ssdv, pkt); + i++; + } + + ssdv_dec_get_jpeg(&ssdv, &jpeg, &jpeg_length); + fwrite(jpeg, 1, jpeg_length, fout); + free(jpeg); + + fprintf(stderr, "Read %i packets\n", i); + + break; + + case 1: /* Encode */ + ssdv_enc_init(&ssdv, image_id); + ssdv_enc_set_buffer(&ssdv, pkt); + + i = 0; + + while(1) + { + while((c = ssdv_enc_get_packet(&ssdv)) == SSDV_FEED_ME) + { + size_t r = fread(b, 1, 128, fin); + + if(r <= 0) + { + fprintf(stderr, "Premature end of file\n"); + break; + } + ssdv_enc_feed(&ssdv, b, r); + } + + if(c == SSDV_EOI) + { + fprintf(stderr, "ssdv_enc_get_packet said EOI\n"); + break; + } + else if(c != SSDV_OK) + { + fprintf(stderr, "ssdv_enc_get_packet failed: %i\n", c); + return(-1); + } + + fwrite(pkt, 1, SSDV_PKT_SIZE, fout); + i++; + } + + fprintf(stderr, "Wrote %i packets\n", i); + + break; + + default: + fprintf(stderr, "No mode specified.\n"); + break; + } + + if(fin != stdin) fclose(fin); + if(fout != stdout) fclose(fout); + + return(0); +} + diff --git a/rs8.c b/rs8.c new file mode 100644 index 0000000..b014ed7 --- /dev/null +++ b/rs8.c @@ -0,0 +1,318 @@ +/* General purpose Reed-Solomon codec for 8-bit symbols or less + * Copyright 2003 Phil Karn, KA9Q + * May be used under the terms of the GNU Lesser General Public License (LGPL) + * + * This version tweaked by Philip Heron +*/ + +#include +#include "rs8.h" + +static const uint8_t ALPHA_TO[] = { +0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80,0x87,0x89,0x95,0xAD,0xDD,0x3D,0x7A,0xF4, +0x6F,0xDE,0x3B,0x76,0xEC,0x5F,0xBE,0xFB,0x71,0xE2,0x43,0x86,0x8B,0x91,0xA5,0xCD, +0x1D,0x3A,0x74,0xE8,0x57,0xAE,0xDB,0x31,0x62,0xC4,0x0F,0x1E,0x3C,0x78,0xF0,0x67, +0xCE,0x1B,0x36,0x6C,0xD8,0x37,0x6E,0xDC,0x3F,0x7E,0xFC,0x7F,0xFE,0x7B,0xF6,0x6B, +0xD6,0x2B,0x56,0xAC,0xDF,0x39,0x72,0xE4,0x4F,0x9E,0xBB,0xF1,0x65,0xCA,0x13,0x26, +0x4C,0x98,0xB7,0xE9,0x55,0xAA,0xD3,0x21,0x42,0x84,0x8F,0x99,0xB5,0xED,0x5D,0xBA, +0xF3,0x61,0xC2,0x03,0x06,0x0C,0x18,0x30,0x60,0xC0,0x07,0x0E,0x1C,0x38,0x70,0xE0, +0x47,0x8E,0x9B,0xB1,0xE5,0x4D,0x9A,0xB3,0xE1,0x45,0x8A,0x93,0xA1,0xC5,0x0D,0x1A, +0x34,0x68,0xD0,0x27,0x4E,0x9C,0xBF,0xF9,0x75,0xEA,0x53,0xA6,0xCB,0x11,0x22,0x44, +0x88,0x97,0xA9,0xD5,0x2D,0x5A,0xB4,0xEF,0x59,0xB2,0xE3,0x41,0x82,0x83,0x81,0x85, +0x8D,0x9D,0xBD,0xFD,0x7D,0xFA,0x73,0xE6,0x4B,0x96,0xAB,0xD1,0x25,0x4A,0x94,0xAF, +0xD9,0x35,0x6A,0xD4,0x2F,0x5E,0xBC,0xFF,0x79,0xF2,0x63,0xC6,0x0B,0x16,0x2C,0x58, +0xB0,0xE7,0x49,0x92,0xA3,0xC1,0x05,0x0A,0x14,0x28,0x50,0xA0,0xC7,0x09,0x12,0x24, +0x48,0x90,0xA7,0xC9,0x15,0x2A,0x54,0xA8,0xD7,0x29,0x52,0xA4,0xCF,0x19,0x32,0x64, +0xC8,0x17,0x2E,0x5C,0xB8,0xF7,0x69,0xD2,0x23,0x46,0x8C,0x9F,0xB9,0xF5,0x6D,0xDA, +0x33,0x66,0xCC,0x1F,0x3E,0x7C,0xF8,0x77,0xEE,0x5B,0xB6,0xEB,0x51,0xA2,0xC3,0x00, +}; + +static const uint8_t INDEX_OF[] = { +0xFF,0x00,0x01,0x63,0x02,0xC6,0x64,0x6A,0x03,0xCD,0xC7,0xBC,0x65,0x7E,0x6B,0x2A, +0x04,0x8D,0xCE,0x4E,0xC8,0xD4,0xBD,0xE1,0x66,0xDD,0x7F,0x31,0x6C,0x20,0x2B,0xF3, +0x05,0x57,0x8E,0xE8,0xCF,0xAC,0x4F,0x83,0xC9,0xD9,0xD5,0x41,0xBE,0x94,0xE2,0xB4, +0x67,0x27,0xDE,0xF0,0x80,0xB1,0x32,0x35,0x6D,0x45,0x21,0x12,0x2C,0x0D,0xF4,0x38, +0x06,0x9B,0x58,0x1A,0x8F,0x79,0xE9,0x70,0xD0,0xC2,0xAD,0xA8,0x50,0x75,0x84,0x48, +0xCA,0xFC,0xDA,0x8A,0xD6,0x54,0x42,0x24,0xBF,0x98,0x95,0xF9,0xE3,0x5E,0xB5,0x15, +0x68,0x61,0x28,0xBA,0xDF,0x4C,0xF1,0x2F,0x81,0xE6,0xB2,0x3F,0x33,0xEE,0x36,0x10, +0x6E,0x18,0x46,0xA6,0x22,0x88,0x13,0xF7,0x2D,0xB8,0x0E,0x3D,0xF5,0xA4,0x39,0x3B, +0x07,0x9E,0x9C,0x9D,0x59,0x9F,0x1B,0x08,0x90,0x09,0x7A,0x1C,0xEA,0xA0,0x71,0x5A, +0xD1,0x1D,0xC3,0x7B,0xAE,0x0A,0xA9,0x91,0x51,0x5B,0x76,0x72,0x85,0xA1,0x49,0xEB, +0xCB,0x7C,0xFD,0xC4,0xDB,0x1E,0x8B,0xD2,0xD7,0x92,0x55,0xAA,0x43,0x0B,0x25,0xAF, +0xC0,0x73,0x99,0x77,0x96,0x5C,0xFA,0x52,0xE4,0xEC,0x5F,0x4A,0xB6,0xA2,0x16,0x86, +0x69,0xC5,0x62,0xFE,0x29,0x7D,0xBB,0xCC,0xE0,0xD3,0x4D,0x8C,0xF2,0x1F,0x30,0xDC, +0x82,0xAB,0xE7,0x56,0xB3,0x93,0x40,0xD8,0x34,0xB0,0xEF,0x26,0x37,0x0C,0x11,0x44, +0x6F,0x78,0x19,0x9A,0x47,0x74,0xA7,0xC1,0x23,0x53,0x89,0xFB,0x14,0x5D,0xF8,0x97, +0x2E,0x4B,0xB9,0x60,0x0F,0xED,0x3E,0xE5,0xF6,0x87,0xA5,0x17,0x3A,0xA3,0x3C,0xB7, +}; + +static const uint8_t GENPOLY[] = { +0x00,0xF9,0x3B,0x42,0x04,0x2B,0x7E,0xFB,0x61,0x1E,0x03,0xD5,0x32,0x42,0xAA,0x05, +0x18,0x05,0xAA,0x42,0x32,0xD5,0x03,0x1E,0x61,0xFB,0x7E,0x2B,0x04,0x42,0x3B,0xF9, +0x00, +}; + +static inline int mod255(int x) +{ + while(x >= 255) + { + x -= 255; + x = (x >> 8) + (x & 255); + } + return(x); +} +#define MODNN(x) mod255(x) + +#define MM (8) +#define NN (255) +#define NROOTS (32) +#define FCR (112) +#define PRIM (11) +#define IPRIM (116) + +#define MIN(a,b) ((a) < (b) ? (a) : (b)) +#define A0 (NN) /* Special reserved value encoding zero in index form */ + +/* Portable C version */ +void encode_rs_8(uint8_t *data, uint8_t *parity, int pad) +{ + int i, j; + uint8_t feedback; + + memset(parity, 0, NROOTS * sizeof(uint8_t)); + + for(i = 0; i < NN - NROOTS - pad; i++) + { + feedback = INDEX_OF[data[i] ^ parity[0]]; + if(feedback != A0) /* feedback term is non-zero */ + { + for(j = 1; j < NROOTS; j++) + parity[j] ^= ALPHA_TO[mod255(feedback + GENPOLY[NROOTS - j])]; + } + + /* Shift */ + memmove(&parity[0], &parity[1], sizeof(uint8_t) * (NROOTS - 1)); + if(feedback != A0) + parity[NROOTS - 1] = ALPHA_TO[mod255(feedback + GENPOLY[0])]; + else + parity[NROOTS - 1] = 0; + } +} + +int decode_rs_8(uint8_t *data, int *eras_pos, int no_eras, int pad) +{ + int deg_lambda, el, deg_omega; + int i, j, r, k; + uint8_t u, q, tmp, num1, num2, den, discr_r; + uint8_t lambda[NROOTS + 1], s[NROOTS]; /* Err+Eras Locator poly + * and syndrome poly */ + uint8_t b[NROOTS + 1], t[NROOTS + 1], omega[NROOTS + 1]; + uint8_t root[NROOTS], reg[NROOTS + 1], loc[NROOTS]; + int syn_error, count; + + if(pad < 0 || pad > 222) return(-1); + + /* form the syndromes; i.e., evaluate data(x) at roots of g(x) */ + for(i = 0; i < NROOTS; i++) s[i] = data[0]; + + for(j = 1; j < NN - pad; j++) + { + for(i = 0; i < NROOTS; i++) + { + if(s[i] == 0) s[i] = data[j]; + else s[i] = data[j] ^ ALPHA_TO[MODNN(INDEX_OF[s[i]] + (FCR + i) * PRIM)]; + } + } + + /* Convert syndromes to index form, checking for nonzero condition */ + syn_error = 0; + for(i = 0; i < NROOTS; i++) + { + syn_error |= s[i]; + s[i] = INDEX_OF[s[i]]; + } + + if(!syn_error) + { + /* if syndrome is zero, data[] is a codeword and there are no + * errors to correct. So return data[] unmodified + */ + count = 0; + goto finish; + } + + memset(&lambda[1], 0, NROOTS * sizeof(lambda[0])); + lambda[0] = 1; + + if(no_eras > 0) + { + /* Init lambda to be the erasure locator polynomial */ + lambda[1] = ALPHA_TO[MODNN(PRIM * (NN - 1 - eras_pos[0]))]; + for(i = 1; i < no_eras; i++) + { + u = MODNN(PRIM * (NN - 1 - eras_pos[i])); + for(j = i + 1; j > 0; j--) + { + tmp = INDEX_OF[lambda[j - 1]]; + if(tmp != A0) lambda[j] ^= ALPHA_TO[MODNN(u + tmp)]; + } + } + + } + + for(i = 0; i < NROOTS + 1; i++) + b[i] = INDEX_OF[lambda[i]]; + + /* + * Begin Berlekamp-Massey algorithm to determine error+erasure + * locator polynomial + */ + r = no_eras; + el = no_eras; + while(++r <= NROOTS) /* r is the step number */ + { + /* Compute discrepancy at the r-th step in poly-form */ + discr_r = 0; + for(i = 0; i < r; i++) + { + if((lambda[i] != 0) && (s[r - i - 1] != A0)) + { + discr_r ^= ALPHA_TO[MODNN(INDEX_OF[lambda[i]] + s[r - i - 1])]; + } + } + discr_r = INDEX_OF[discr_r]; /* Index form */ + if(discr_r == A0) + { + /* 2 lines below: B(x) <-- x*B(x) */ + memmove(&b[1], b, NROOTS * sizeof(b[0])); + b[0] = A0; + } + else + { + /* 7 lines below: T(x) <-- lambda(x) - discr_r*x*b(x) */ + t[0] = lambda[0]; + for(i = 0; i < NROOTS; i++) + { + if(b[i] != A0) + t[i + 1] = lambda[i + 1] ^ ALPHA_TO[MODNN(discr_r + b[i])]; + else + t[i + 1] = lambda[i + 1]; + } + + if(2 * el <= r + no_eras - 1) + { + el = r + no_eras - el; + /* + * 2 lines below: B(x) <-- inv(discr_r) * + * lambda(x) + */ + for(i = 0; i <= NROOTS; i++) + b[i] = (lambda[i] == 0) ? A0 : MODNN(INDEX_OF[lambda[i]] - discr_r + NN); + } + else + { + /* 2 lines below: B(x) <-- x*B(x) */ + memmove(&b[1], b, NROOTS * sizeof(b[0])); + b[0] = A0; + } + + memcpy(lambda, t, (NROOTS + 1) * sizeof(t[0])); + } + } + + /* Convert lambda to index form and compute deg(lambda(x)) */ + deg_lambda = 0; + for(i = 0; i < NROOTS + 1; i++) + { + lambda[i] = INDEX_OF[lambda[i]]; + if(lambda[i] != A0) deg_lambda = i; + } + + /* Find roots of the error+erasure locator polynomial by Chien search */ + memcpy(®[1], &lambda[1], NROOTS * sizeof(reg[0])); + count = 0; /* Number of roots of lambda(x) */ + for(i = 1, k = IPRIM - 1; i <= NN; i++, k = MODNN(k + IPRIM)) + { + q = 1; /* lambda[0] is always 0 */ + for(j = deg_lambda; j > 0; j--) + { + if(reg[j] != A0) + { + reg[j] = MODNN(reg[j] + j); + q ^= ALPHA_TO[reg[j]]; + } + } + + if (q != 0) continue; /* Not a root */ + + /* store root (index-form) and error location number */ + root[count] = i; + loc[count] = k; + /* If we've already found max possible roots, + * abort the search to save time + */ + if(++count == deg_lambda) break; + } + + if(deg_lambda != count) + { + /* + * deg(lambda) unequal to number of roots => uncorrectable + * error detected + */ + count = -1; + goto finish; + } + + /* + * Compute err+eras evaluator poly omega(x) = s(x)*lambda(x) (modulo + * x**NROOTS). in index form. Also find deg(omega). + */ + deg_omega = deg_lambda - 1; + for(i = 0; i <= deg_omega; i++) + { + tmp = 0; + for(j = i; j >= 0; j--) + { + if((s[i - j] != A0) && (lambda[j] != A0)) + tmp ^= ALPHA_TO[MODNN(s[i - j] + lambda[j])]; + } + omega[i] = INDEX_OF[tmp]; + } + + /* + * Compute error values in poly-form. num1 = omega(inv(X(l))), num2 = + * inv(X(l))**(FCR-1) and den = lambda_pr(inv(X(l))) all in poly-form + */ + for(j = count - 1; j >= 0; j--) + { + num1 = 0; + for(i = deg_omega; i >= 0; i--) + { + if(omega[i] != A0) num1 ^= ALPHA_TO[MODNN(omega[i] + i * root[j])]; + } + num2 = ALPHA_TO[MODNN(root[j] * (FCR - 1) + NN)]; + den = 0; + + /* lambda[i+1] for i even is the formal derivative lambda_pr of lambda[i] */ + for(i = MIN(deg_lambda, NROOTS - 1) & ~1; i >= 0; i -= 2) + { + if(lambda[i + 1] != A0) den ^= ALPHA_TO[MODNN(lambda[i + 1] + i * root[j])]; + } + + /* Apply error to data */ + if(num1 != 0 && loc[j] >= pad) + { + data[loc[j] - pad] ^= ALPHA_TO[MODNN(INDEX_OF[num1] + INDEX_OF[num2] + NN - INDEX_OF[den])]; + } + } + +finish: + if(eras_pos != NULL) + { + for(i = 0; i < count; i++) eras_pos[i] = loc[i]; + } + + return(count); +} + diff --git a/rs8.h b/rs8.h new file mode 100644 index 0000000..d0513dc --- /dev/null +++ b/rs8.h @@ -0,0 +1,23 @@ +/* Reed-Solomon encoder + * Copyright 2004, Phil Karn, KA9Q + * May be used under the terms of the GNU Lesser General Public License (LGPL) + * + * This version tweaked by Philip Heron +*/ + +#ifndef _RS8_H +#define _RS8_H +#ifdef __cplusplus +extern "C" { +#endif + +#include + +extern void encode_rs_8(uint8_t *data, uint8_t *parity, int pad); +extern int decode_rs_8(uint8_t *data, int *eras_pos, int no_eras, int pad); + +#ifdef __cplusplus +} +#endif +#endif + diff --git a/ssdv.c b/ssdv.c new file mode 100644 index 0000000..15335b5 --- /dev/null +++ b/ssdv.c @@ -0,0 +1,1055 @@ + +#include +#include +#include +#include +#include "ssdv.h" +#include "rs8.h" + +/* Recognised JPEG markers */ +enum { + J_TEM = 0xFF01, + J_SOF0 = 0xFFC0, J_SOF1, J_SOF2, J_SOF3, J_DHT, J_SOF5, J_SOF6, J_SOF7, + J_JPG, J_SOF9, J_SOF10, J_SOF11, J_DAC, J_SOF13, J_SOF14, J_SOF15, + J_RST0, J_RST1, J_RST2, J_RST3, J_RST4, J_RST5, J_RST6, J_RST7, + J_SOI, J_EOI, J_SOS, J_DQT, J_DNL, J_DRI, J_DHP, J_EXP, + J_APP0, J_APP1, J_APP2, J_APP3, J_APP4, J_APP5, J_APP6, J_APP7, + J_APP8, J_APP9, J_APP10, J_APP11, J_APP12, J_APP13, J_APP14, J_APP15, + J_JPG0, J_JPG1, J_JPG2, J_JPG3, J_JPG4, J_JPG5, J_JPG6, J_SOF48, + J_LSE, J_JPG9, J_JPG10, J_JPG11, J_JPG12, J_JPG13, J_COM, +} jpeg_marker_t; + +/* APP0 header data */ +static uint8_t app0[14] = { +0x4A,0x46,0x49,0x46,0x00,0x01,0x01,0x01,0x00,0x48,0x00,0x48,0x00,0x00, +}; + +/* SOS header data */ +static uint8_t sos[10] = { +0x03,0x01,0x00,0x02,0x11,0x03,0x11,0x00,0x3F,0x00, +}; + +/* Quantisation tables */ +static uint8_t std_dqt0[65] = { +0x00,0x10,0x0C,0x0C,0x0E,0x0C,0x0A,0x10,0x0E,0x0E,0x0E,0x12,0x12,0x10,0x14,0x18, +0x28,0x1A,0x18,0x16,0x16,0x18,0x32,0x24,0x26,0x1E,0x28,0x3A,0x34,0x3E,0x3C,0x3A, +0x34,0x38,0x38,0x40,0x48,0x5C,0x4E,0x40,0x44,0x58,0x46,0x38,0x38,0x50,0x6E,0x52, +0x58,0x60,0x62,0x68,0x68,0x68,0x3E,0x4E,0x72,0x7A,0x70,0x64,0x78,0x5C,0x66,0x68, +0x64, +}; + +static uint8_t std_dqt1[65] = { +0x01,0x12,0x12,0x12,0x16,0x16,0x16,0x30,0x1A,0x1A,0x30,0x64,0x42,0x38,0x42,0x64, +0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64, +0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64, +0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64,0x64, +0x64, +}; + +/* Decoding DQT -- for input file */ +static uint8_t *sdqt[3] = { std_dqt0, std_dqt1, std_dqt1 }; + +/* Encoding DQT -- for output */ +static uint8_t *dqt[3] = { std_dqt0, std_dqt1, std_dqt1 }; + +/* Standard Huffman tables */ +static uint8_t std_dht00[29] = { +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, +}; + +static uint8_t std_dht01[29] = { +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, +}; + +static uint8_t std_dht10[179] = { +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, +}; + +static uint8_t std_dht11[179] = { +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, +}; + +/* Decoding DHT -- for input file */ +static uint8_t *sdht_dc[3] = { std_dht00, std_dht01, std_dht01 }; +static uint8_t *sdht_ac[3] = { std_dht10, std_dht11, std_dht11 }; + +/* Encoding DHT -- for output */ +static uint8_t *dht_dc[3] = { std_dht00, std_dht01, std_dht01 }; +static uint8_t *dht_ac[3] = { std_dht10, std_dht11, std_dht11 }; + +/* +static char *strbits(uint32_t value, uint8_t bits) +{ + static char s[33]; + char *ss = s; + while(bits--) *(ss++) = value & 1 << bits ? '1' : '0'; + *ss = '\0'; + return(s); +} +*/ + +#ifndef crc_xmodem_update +uint16_t crc_xmodem_update(uint16_t crc, uint8_t data) +{ + int i; + + crc = crc ^ ((uint16_t) data << 8); + for(i = 0; i < 8; i++) + { + if(crc & 0x8000) crc = (crc << 1) ^ 0x1021; + else crc <<= 1; + } + + return(crc); +} +#endif + +static inline char jpeg_dht_lookup(ssdv_t *s, uint8_t *symbol, uint8_t *width) +{ + uint16_t code = 0; + uint8_t cw, n; + uint8_t *dht, *ss; + + /* Select the appropriate huffman table */ + if(s->acpart == 0) dht = sdht_dc[s->component]; + else dht = sdht_ac[s->component]; + ss = &dht[17]; + + for(cw = 1; cw <= 16; cw++) + { + /* Got enough bits? */ + if(cw >= s->worklen) return(SSDV_FEED_ME); + + /* Compare against each code 'cw' bits wide */ + for(n = dht[cw]; n > 0; n--) + { + if(s->workbits >> (s->worklen - cw) == code) + { + /* Found a match */ + *symbol = *ss; + *width = cw; + return(SSDV_OK); + } + ss++; code++; + } + + code <<= 1; + } + + /* No match found - error */ + return(SSDV_ERROR); +} + +static inline char jpeg_dht_lookup_symbol(uint8_t *dht, uint8_t symbol, uint16_t *bits, uint8_t *width) +{ + uint16_t code = 0; + uint8_t cw, n, *ss = &dht[17]; + + for(cw = 1; cw <= 16; cw++) + { + for(n = dht[cw]; n > 0; n--) + { + if(*ss == symbol) + { + /* Found a match */ + *bits = code; + *width = cw; + return(SSDV_OK); + } + ss++; code++; + } + + code <<= 1; + } + + /* No match found - error */ + return(SSDV_ERROR); +} + +static inline int jpeg_int(int bits, int width) +{ + int b = (1 << width) - 1; + if(bits <= b >> 1) bits = -(bits ^ b); + return(bits); +} + +static inline void jpeg_encode_int(int value, int *bits, uint8_t *width) +{ + *bits = value; + + /* Calculate the number of bits */ + if(value < 0) value = -value; + for(*width = 0; value; value >>= 1) (*width)++; + + /* Fix negative values */ + if(*bits < 0) *bits = -*bits ^ ((1 << *width) - 1); +} + +/*****************************************************************************/ + +static char ssdv_outbits(ssdv_t *s, uint16_t bits, uint8_t length) +{ + uint8_t b; + + if(length) + { + s->outbits <<= length; + s->outbits |= bits & ((1 << length) - 1); + s->outlen += length; + } + + while(s->outlen >= 8 && s->out_len > 0) + { + b = s->outbits >> (s->outlen - 8); + + /* Put the byte into the output buffer */ + *(s->outp++) = b; + s->outlen -= 8; + s->out_len--; + + /* Insert stuffing byte if needed */ + if(s->out_stuff && b == 0xFF) + { + s->outbits &= (1 << s->outlen) - 1; + s->outlen += 8; + } + } + + return(s->out_len ? SSDV_OK : SSDV_BUFFER_FULL); +} + +static char ssdv_outbits_sync(ssdv_t *s) +{ + uint8_t b = s->outlen % 8; + if(b) return(ssdv_outbits(s, 0xFF, 8 - b)); + return(SSDV_OK); +} + +static char ssdv_out_jpeg_int(ssdv_t *s, uint8_t rle, int value) +{ + uint16_t huffbits = 0; + int intbits; + uint8_t hufflen = 0, intlen; + uint8_t *dht; + int r; + + /* Select the appropriate huffman table */ + if(s->acpart == 0) dht = dht_dc[s->component]; + else dht = dht_ac[s->component]; + + /* Adjust for output DQT */ + //value *= sdqt[s->component][1 + s->acpart]; + //value /= dqt[s->component][1 + s->acpart]; + + jpeg_encode_int(value, &intbits, &intlen); + r = jpeg_dht_lookup_symbol(dht, (rle << 4) | (intlen & 0x0F), &huffbits, &hufflen); + + if(r != SSDV_OK) fprintf(stderr, "jpeg_dht_lookup_symbol: %i (%i:%i)\n", r, value, rle); + + ssdv_outbits(s, huffbits, hufflen); + if(intlen) ssdv_outbits(s, intbits, intlen); + + return(SSDV_OK); +} + +static char ssdv_process(ssdv_t *s) +{ + if(s->state == S_HUFF) + { + uint8_t symbol, width; + int r; + + /* Lookup the code, return if error or not enough bits yet */ + if((r = jpeg_dht_lookup(s, &symbol, &width)) != SSDV_OK) + return(r); + + if(s->acpart == 0) /* DC */ + { + if(symbol == 0x00) + { + /* No change in DC from last block */ + if(s->mcupart == 0 || s->mcupart == 4 || s->mcupart == 5) + { + if(s->dcmode == 0) ssdv_out_jpeg_int(s, 0, s->dc[s->component]); + else + { + ssdv_out_jpeg_int(s, 0, 0 - s->dc[s->component]); + s->dc[s->component] = 0; + } + } + else ssdv_out_jpeg_int(s, 0, 0); + + /* skip to the next AC part immediately */ + s->acpart++; + } + else + { + /* DC value follows, 'symbol' bits wide */ + s->state = S_INT; + s->needbits = symbol; + } + } + else /* AC */ + { + s->acrle = 0; + if(symbol == 0x00) + { + /* EOB -- all remaining AC parts are zero */ + ssdv_out_jpeg_int(s, 0, 0); + s->acpart = 64; + } + else if(symbol == 0xF0) + { + /* The next 16 AC parts are zero */ + ssdv_out_jpeg_int(s, 15, 0); + s->acpart += 16; + } + else + { + /* Next bits are an integer value */ + s->state = S_INT; + s->acrle = symbol >> 4; + s->acpart += s->acrle; + s->needbits = symbol & 0x0F; + } + } + + /* Clear processed bits */ + s->worklen -= width; + s->workbits &= (1 << s->worklen) - 1; + } + else if(s->state == S_INT) + { + int i; + + /* Not enough bits yet? */ + if(s->worklen < s->needbits) return(SSDV_FEED_ME); + + /* Decode the integer */ + i = jpeg_int(s->workbits >> (s->worklen - s->needbits), s->needbits); + + if(s->acpart == 0) /* DC */ + { + if(s->mcupart == 0 || s->mcupart == 4 || s->mcupart == 5) + { + if(s->dcmode == 0) + { + /* Output absolute DC value */ + s->dc[s->component] += i; + ssdv_out_jpeg_int(s, 0, s->dc[s->component]); + } + else + { + /* Output relative DC value */ + ssdv_out_jpeg_int(s, 0, i - s->dc[s->component]); + s->dc[s->component] = i; + } + } + else + { + /* Output relative DC value */ + s->dc[s->component] += i; + ssdv_out_jpeg_int(s, 0, i); + } + } + else /* AC */ + { + /* Output AC codes directly */ + ssdv_out_jpeg_int(s, s->acrle, i); + } + + /* Next AC part to expect */ + s->acpart++; + + /* Next bits are a huffman code */ + s->state = S_HUFF; + + /* Clear processed bits */ + s->worklen -= s->needbits; + s->workbits &= (1 << s->worklen) - 1; + } + + if(s->acpart >= 64) + { + /* Reached the end of this MCU part */ + if(++s->mcupart == 6) + { + s->mcupart = 0; + s->mcu_id++; + + /* Test for the end of image */ + if(s->mcu_id >= s->mcu_count) + { + /* Flush any remaining bits */ + ssdv_outbits_sync(s); + return(SSDV_EOI); + } + + /* Set the packet MCU marker - encoder only */ + if(s->packet_mcu_id == 0xFFFF) + { + s->packet_mcu_id = s->mcu_id; + s->packet_mcu_offset = + (SSDV_PKT_SIZE_PAYLOAD - s->out_len) * 8 + s->outlen; + } + } + + if(s->mcupart < 4) s->component = 0; + else s->component = s->mcupart - 3; + + s->acpart = 0; + } + + if(s->out_len == 0) return(SSDV_BUFFER_FULL); + + return(SSDV_OK); +} + +/*****************************************************************************/ + +static void ssdv_memset_prng(uint8_t *s, size_t n) +{ + /* A very simple PRNG for noise whitening */ + uint8_t l = 0x00; + for(; n > 0; n--) *(s++) = (l = l * 245 + 45); +} + +static char ssdv_have_marker(ssdv_t *s) +{ + switch(s->marker) + { + case J_SOF0: + case J_SOS: + /* Copy the data before processing */ + if(s->marker_len > HBUFF_LEN) + { + /* Not enough memory ... shouldn't happen! */ + return(SSDV_ERROR); + } + + s->marker_data = s->hbuff; + s->marker_data_len = 0; + s->state = S_MARKER_DATA; + break; + + case J_DHT: + case J_DQT: + /* Copy the tables into memory */ + s->marker_data = malloc(s->marker_len); + s->marker_data_len = 0; + s->state = S_MARKER_DATA; + break; + + case J_EOI: + s->state = S_EOI; + break; + + default: + /* Ignore other marks, skipping any associated data */ + s->in_skip = s->marker_len; + s->state = S_MARKER; + break; + } + + return(SSDV_OK); +} + +static char ssdv_have_marker_data(ssdv_t *s) +{ + uint8_t *d = s->marker_data; + size_t l = s->marker_len; + int i; + + switch(s->marker) + { + case J_SOF0: + s->width = (d[3] << 8) | d[4]; + s->height = (d[1] << 8) | d[2]; + + /* Calculate number of MCU blocks in this image -- assumes 16x16 blocks */ + s->mcu_count = (s->width >> 4) * (s->height >> 4); + + /* Display information about the image... */ + fprintf(stderr, "Precision: %i\n", d[0]); + fprintf(stderr, "Resolution: %ix%i\n", s->width, s->height); + fprintf(stderr, "Components: %i\n", d[5]); + fprintf(stderr, "MCU blocks: %i\n", s->mcu_count); + + /* The image must have a precision of 8 */ + if(d[0] != 8) + { + fprintf(stderr, "Error: The image must have a precision of 8\n"); + return(SSDV_ERROR); + } + + /* The image must have 3 components (Y'Cb'Cr) */ + if(d[5] != 3) + { + fprintf(stderr, "Error: The image must have 3 components\n"); + return(SSDV_ERROR); + } + + /* The image dimensions must be a multiple of 16 */ + if((s->width & 0x0F) || (s->height & 0x0F)) + { + fprintf(stderr, "Error: The image dimensions must be a multiple of 16\n"); + return(SSDV_ERROR); + } + + /* TODO: Read in the quantisation table ID for each component */ + // 01 22 00 02 11 01 03 11 01 + for(i = 0; i < 3; i++) + { + uint8_t *dq = &d[i * 3 + 6]; + if(dq[0] != i + 1) + { + fprintf(stderr, "Error: Components are not in order in the SOF0 header\n"); + return(SSDV_ERROR); + } + + fprintf(stderr, "DQT table for component %i: %02X, Sampling factor: %ix%i\n", dq[0], dq[2], dq[1] & 0x0F, dq[1] >> 4); + } + + break; + + case J_SOS: + fprintf(stderr, "Components: %i\n", d[0]); + + /* The image must have 3 components (Y'Cb'Cr) */ + if(d[0] != 3) + { + fprintf(stderr, "Error: The image must have 3 components\n"); + return(SSDV_ERROR); + } + + for(i = 0; i < 3; i++) + { + uint8_t *dh = &d[i * 2 + 1]; + if(dh[0] != i + 1) + { + fprintf(stderr, "Error: Components are not in order in the SOF0 header\n"); + return(SSDV_ERROR); + } + + fprintf(stderr, "Component %i DHT: %02X\n", dh[0], dh[1]); + } + + /* Do I need to look at the last three bytes of the SOS data? */ + /* 00 3F 00 */ + + /* The SOS data is followed by the image data */ + s->state = S_HUFF; + + return(SSDV_OK); + + case J_DHT: + while(l > 0) + { + int i, s; + + switch(d[0]) + { + case 0x00: sdht_dc[0] = d; break; + case 0x01: sdht_dc[1] = sdht_dc[2] = d; break; + case 0x10: sdht_ac[0] = d; break; + case 0x11: sdht_ac[1] = sdht_ac[2] = d; break; + } + + /* Skip to the next DHT table */ + for(s = 17, i = 1; i <= 16; i++) + s += d[i]; + + l -= s; + d += s; + } + break; + case J_DQT: + while(l > 0) + { + switch(d[0]) + { + case 0x00: sdqt[0] = d; break; + case 0x01: sdqt[1] = sdqt[2] = d; break; + } + + /* Skip to the next one, if present */ + l -= 65; + d += 65; + } + break; + } + + s->state = S_MARKER; + return(SSDV_OK); +} + +char ssdv_enc_init(ssdv_t *s, uint8_t image_id) +{ + memset(s, 0, sizeof(ssdv_t)); + s->image_id = image_id; + return(SSDV_OK); +} + +char ssdv_enc_set_buffer(ssdv_t *s, uint8_t *buffer) +{ + s->out = buffer; + s->outp = buffer + SSDV_PKT_SIZE_HEADER; + s->out_len = SSDV_PKT_SIZE_PAYLOAD; + + /* Zero the payload memory */ + memset(s->out, 0, SSDV_PKT_SIZE); + + /* Flush the output bits */ + ssdv_outbits(s, 0, 0); + + return(SSDV_OK); +} + +char ssdv_enc_get_packet(ssdv_t *s) +{ + int r; + uint8_t b; + + /* Have we reached the end of the image? */ + if(s->state == S_EOI) return(SSDV_EOI); + + /* If the output buffer is empty, re-initialise */ + if(s->out_len == 0) ssdv_enc_set_buffer(s, s->out); + + while(s->in_len) + { + b = *(s->inp++); + s->in_len--; + + /* Skip bytes if necessary */ + if(s->in_skip) { s->in_skip--; continue; } + + switch(s->state) + { + case S_MARKER: + s->marker = (s->marker << 8) | b; + + if(s->marker == J_TEM || + (s->marker >= J_RST0 && s->marker <= J_EOI)) + { + /* Marker without data */ + s->marker_len = 0; + ssdv_have_marker(s); + } + else if(s->marker >= J_SOF0 && s->marker <= J_COM) + { + /* All other markers are followed by data */ + s->marker_len = 0; + s->state = S_MARKER_LEN; + s->needbits = 16; + } + break; + + case S_MARKER_LEN: + s->marker_len = (s->marker_len << 8) | b; + if((s->needbits -= 8) == 0) + { + s->marker_len -= 2; + ssdv_have_marker(s); + } + break; + + case S_MARKER_DATA: + s->marker_data[s->marker_data_len++] = b; + if(s->marker_data_len == s->marker_len) + { + ssdv_have_marker_data(s); + } + break; + + case S_HUFF: + case S_INT: + /* Is the next byte a stuffing byte? Skip it */ + /* TODO: Test the next byte is actually 0x00 */ + if(b == 0xFF) s->in_skip++; + + /* Add the new byte to the work area */ + s->workbits = (s->workbits << 8) | b; + s->worklen += 8; + + /* Process the new data until more needed, or an error occurs */ + while((r = ssdv_process(s)) == SSDV_OK); + + if(r == SSDV_BUFFER_FULL || r == SSDV_EOI) + { + uint16_t mcu_id = s->packet_mcu_id; + uint16_t mcu_offset = s->packet_mcu_offset; + uint16_t i, x; + + if(mcu_offset != 0xFFFF && mcu_offset >= SSDV_PKT_SIZE_PAYLOAD * 8) + { + /* The first MCU begins in the next packet, not this one */ + mcu_id = mcu_offset = 0xFFFF; + s->packet_mcu_offset -= SSDV_PKT_SIZE_PAYLOAD * 8; + } + else + { + /* Clear the MCU data for the next packet */ + s->packet_mcu_id = s->packet_mcu_offset = 0xFFFF; + } + + /* A packet is ready, create the headers */ + s->out[0] = 0x55; /* Sync */ + s->out[1] = 0x66; /* Type */ + s->out[2] = s->image_id; /* Image ID */ + s->out[3] = s->packet_id >> 8; /* Packet ID MSB */ + s->out[4] = s->packet_id & 0xFF; /* Packet ID LSB */ + s->out[5] = s->width >> 4; /* Width / 16 */ + s->out[6] = s->height >> 4; /* Height / 16 */ + s->out[7] = mcu_offset >> 8; /* Next MCU offset MSB */ + s->out[8] = mcu_offset & 0xFF; /* Next MCU offset LSB */ + s->out[9] = mcu_id >> 8; /* MCU ID MSB */ + s->out[10] = mcu_id & 0xFF; /* MCU ID LSB */ + + /* Fill any remaining bytes with noise */ + if(s->out_len > 0) ssdv_memset_prng(s->outp, s->out_len); + + /* Calculate the CRC codes */ + for(i = 1, x = 0xFFFF; i < SSDV_PKT_SIZE - SSDV_PKT_SIZE_RSCODES - SSDV_PKT_SIZE_CRC; i++) + x = crc_xmodem_update(x, s->out[i]); + + s->out[i++] = x >> 8; + s->out[i++] = x & 0xFF; + + /* Generate the RS codes */ + encode_rs_8(&s->out[1], &s->out[i], 0); + + s->packet_id++; + + /* Have we reached the end of the image data? */ + if(r == SSDV_EOI) s->state = S_EOI; + + return(SSDV_OK); + } + else if(r != SSDV_FEED_ME) + { + /* An error occured */ + fprintf(stderr, "ssdv_process() failed: %i\n", r); + return(SSDV_ERROR); + } + break; + + case S_EOI: + /* Shouldn't reach this point */ + break; + } + } + + /* Need more data */ + return(SSDV_FEED_ME); +} + +char ssdv_enc_feed(ssdv_t *s, uint8_t *buffer, size_t length) +{ + s->inp = buffer; + s->in_len = length; + return(SSDV_OK); +} + +/*****************************************************************************/ + +static void ssdv_write_marker(ssdv_t *s, uint16_t id, uint16_t length, uint8_t *data) +{ + ssdv_outbits(s, id, 16); + if(length > 0) + { + ssdv_outbits(s, length + 2, 16); + while(length--) ssdv_outbits(s, *(data++), 8); + } +} + +static void ssdv_out_headers(ssdv_t *s) +{ + uint8_t *b = s->hbuff; + + ssdv_write_marker(s, J_SOI, 0, 0); + ssdv_write_marker(s, J_APP0, 14, app0); + ssdv_write_marker(s, J_DQT, 65, std_dqt0); /* DQT Luminance */ + ssdv_write_marker(s, J_DQT, 65, std_dqt1); /* DQT Chrominance */ + + /* Build SOF0 header */ + b[0] = 8; /* Precision */ + b[1] = s->height >> 8; + b[2] = s->height & 0xFF; + b[3] = s->width >> 8; + b[4] = s->width & 0xFF; + b[5] = 3; /* Components (Y'Cb'Cr) */ + b[6] = 1; /* Y */ + b[7] = 0x22; + b[8] = 0x00; + b[9] = 2; /* Cb */ + b[10] = 0x11; + b[11] = 0x01; + b[12] = 3; /* Cr */ + b[13] = 0x11; + b[14] = 0x01; + ssdv_write_marker(s, J_SOF0, 15, b); /* SOF0 (Baseline DCT) */ + + ssdv_write_marker(s, J_DHT, 29, std_dht00); /* DHT (DC Luminance) */ + ssdv_write_marker(s, J_DHT, 179, std_dht10); /* DHT (AC Luminance) */ + ssdv_write_marker(s, J_DHT, 29, std_dht01); /* DHT (DC Chrominance */ + ssdv_write_marker(s, J_DHT, 179, std_dht11); /* DHT (AC Chrominance */ + ssdv_write_marker(s, J_SOS, 10, sos); +} + +static void ssdv_fill_gap(ssdv_t *s, uint16_t next_mcu) +{ + if(s->mcupart > 0 || s->acpart > 0) + { + /* Cleanly end the current MCU part */ + if(s->acpart > 0) + { + ssdv_out_jpeg_int(s, 0, 0); + s->mcupart++; + } + + /* End the current MCU block */ + for(; s->mcupart < 6; s->mcupart++) + { + if(s->mcupart < 4) s->component = 0; + else s->component = s->mcupart - 3; + s->acpart = 0; ssdv_out_jpeg_int(s, 0, 0); /* DC */ + s->acpart = 1; ssdv_out_jpeg_int(s, 0, 0); /* AC */ + } + + s->mcu_id++; + } + + /* Pad out missing MCUs */ + for(; s->mcu_id < next_mcu; s->mcu_id++) + { + /* End the current MCU block */ + for(s->mcupart = 0; s->mcupart < 6; s->mcupart++) + { + if(s->mcupart < 4) s->component = 0; + else s->component = s->mcupart - 3; + s->acpart = 0; ssdv_out_jpeg_int(s, 0, 0); /* DC */ + s->acpart = 1; ssdv_out_jpeg_int(s, 0, 0); /* AC */ + } + } +} + +char ssdv_dec_init(ssdv_t *s) +{ + memset(s, 0, sizeof(ssdv_t)); + + /* The packet data should contain only scan data, no headers */ + s->state = S_HUFF; + + /* Converting absolute values to relative */ + s->dcmode = 1; + + return(SSDV_OK); +} + +char ssdv_dec_set_buffer(ssdv_t *s, uint8_t *buffer, size_t length) +{ + size_t c = s->outp - s->out; + + s->outp = buffer + c; + s->out = buffer; + s->out_len = length - c; + + /* Flush the output bits */ + ssdv_outbits(s, 0, 0); + + return(SSDV_OK); +} + +char ssdv_dec_feed(ssdv_t *s, uint8_t *packet) +{ + int i = 0, r; + uint8_t b; + uint16_t packet_id; + + /* Read the packet header */ + packet_id = (packet[3] << 8) | packet[4]; + s->packet_mcu_offset = (packet[7] << 8) | packet[8]; + s->packet_mcu_id = (packet[9] << 8) | packet[10]; + + /* If this is the first packet, write the JPEG headers */ + if(s->packet_id == 0) + { + /* Read the fixed headers from the packet */ + s->image_id = packet[2]; + s->width = packet[5] << 4; + s->height = packet[6] << 4; + s->mcu_count = packet[5] * packet[6]; + + /* Output JPEG headers and enable byte stuffing */ + ssdv_out_headers(s); + s->out_stuff = 1; + } + + /* Is this not the packet we expected? */ + if(packet_id != s->packet_id) + { + /* One or more packets have been lost! */ + fprintf(stderr, "Gap detected between packets %i and %i\n", s->packet_id - 1, packet_id); + + /* If this packet has no new MCU, ignore */ + if(s->packet_mcu_offset == 0xFFFF) return(SSDV_FEED_ME); + + /* Fill the gap left by the missing packet */ + ssdv_fill_gap(s, s->packet_mcu_id); + + /* Clear the workbits */ + s->workbits = s->worklen = 0; + + /* Skip the bytes of the lost MCU */ + i = s->packet_mcu_offset >> 3; + + /* Skip any remaining bits too */ + if((r = s->packet_mcu_offset & 7)) + { + /* Add the first bits of the new MCU */ + r = 8 - r; + b = packet[SSDV_PKT_SIZE_HEADER + i++]; + b &= ((1 << r) - 1); + s->workbits = b; + s->worklen += r; + } + + /* Reset the JPEG decoder state */ + s->state = S_HUFF; + s->component = 0; + s->mcupart = 0; + s->acpart = 0; + + s->packet_id = packet_id; + } + + /* Feed the JPEG data into the processor */ + for(; i < SSDV_PKT_SIZE_PAYLOAD; i++) + { + b = packet[SSDV_PKT_SIZE_HEADER + i]; + + /* Add the new byte to the work area */ + s->workbits = (s->workbits << 8) | b; + s->worklen += 8; + + /* Process the new data until more needed, or an error occurs */ + while((r = ssdv_process(s)) == SSDV_OK); + + if(r == SSDV_BUFFER_FULL) + { + /* Realloc memory */ + } + else if(r == SSDV_EOI) + { + /* All done! */ + return(SSDV_OK); + } + else if(r != SSDV_FEED_ME) + { + /* An error occured */ + fprintf(stderr, "ssdv_process() failed: %i\n", r); + return(SSDV_ERROR); + } + } + + /* The next packet to expect... */ + s->packet_id++; + + return(SSDV_FEED_ME); +} + +char ssdv_dec_get_jpeg(ssdv_t *s, uint8_t **jpeg, size_t *length) +{ + /* Is the image complete? */ + if(s->mcu_id < s->mcu_count) ssdv_fill_gap(s, s->mcu_count); + + /* Sync, and final EOI header and return */ + ssdv_outbits_sync(s); + s->out_stuff = 0; + ssdv_write_marker(s, J_EOI, 0, 0); + + *jpeg = s->out; + *length = (size_t) (s->outp - s->out); + + return(SSDV_OK); +} + +char ssdv_dec_is_packet(uint8_t *packet, int *errors) +{ + uint8_t pkt[SSDV_PKT_SIZE]; + ssdv_packet_info_t p; + uint16_t x; + int i; + + /* Testing is destructive, work on a copy */ + memcpy(pkt, packet, SSDV_PKT_SIZE); + pkt[0] = 0x55; + pkt[1] = 0x66; + + /* Run the reed-solomon decoder */ + i = decode_rs_8(&pkt[1], 0, 0, 0); + if(i < 0) return(-1); /* Reed-solomon decoder failed */ + if(errors) *errors = i; + + /* Sanity checks */ + if(pkt[1] != 0x66) return(-1); + + ssdv_dec_header(&p, pkt); + if(p.width == 0 || p.height == 0) return(-1); + if(p.mcu_id != 0xFFFF) + { + if(p.mcu_id >= p.mcu_count) return(-1); + if(p.mcu_offset >= SSDV_PKT_SIZE_PAYLOAD * 8) return(-1); + } + + /* Test the checksum */ + for(i = 1, x = 0xFFFF; i < SSDV_PKT_SIZE - SSDV_PKT_SIZE_RSCODES - SSDV_PKT_SIZE_CRC; i++) + x = crc_xmodem_update(x, pkt[i]); + + if(pkt[i++] != (x >> 8)) return(-1); + if(pkt[i++] != (x & 0xFF)) return(-1); + + /* Appears to be a valid packet! Copy it back */ + memcpy(packet, pkt, SSDV_PKT_SIZE); + + return(0); +} + +void ssdv_dec_header(ssdv_packet_info_t *info, uint8_t *packet) +{ + info->image_id = packet[2]; + info->packet_id = (packet[3] << 8) | packet[4]; + info->width = packet[5] << 4; + info->height = packet[6] << 4; + info->mcu_offset = (packet[7] << 8) | packet[8]; + info->mcu_id = (packet[9] << 8) | packet[10]; + info->mcu_count = packet[5] * packet[6]; +} + +/*****************************************************************************/ + diff --git a/ssdv.h b/ssdv.h new file mode 100644 index 0000000..8e03e84 --- /dev/null +++ b/ssdv.h @@ -0,0 +1,116 @@ + +#include + +#ifndef INC_SSDV_H +#define INC_SSDV_H +#ifdef __cplusplus +extern "C" { +#endif + +#define SSDV_ERROR (-1) +#define SSDV_OK (0) +#define SSDV_FEED_ME (1) +#define SSDV_HAVE_PACKET (2) +#define SSDV_BUFFER_FULL (3) +#define SSDV_EOI (4) + +/* Packet details */ +#define SSDV_PKT_SIZE (0x100) +#define SSDV_PKT_SIZE_HEADER (0x0B) +#define SSDV_PKT_SIZE_CRC (0x02) +#define SSDV_PKT_SIZE_RSCODES (0x20) +#define SSDV_PKT_SIZE_PAYLOAD (SSDV_PKT_SIZE - SSDV_PKT_SIZE_HEADER - SSDV_PKT_SIZE_CRC - SSDV_PKT_SIZE_RSCODES) + +#define HBUFF_LEN (16) +#define COMPONENTS (3) + +typedef struct +{ + /* Image information */ + uint16_t width; + uint16_t height; + uint8_t image_id; + uint16_t packet_id; + uint16_t mcu_id; + uint16_t mcu_count; + uint16_t packet_mcu_id; + uint16_t packet_mcu_offset; + + /* Source buffer */ + uint8_t *inp; /* Pointer to next input byte */ + size_t in_len; /* Number of input bytes remaining */ + size_t in_skip; /* Number of input bytes to skip */ + + /* Source bits */ + uint32_t workbits; /* Input bits currently being worked on */ + uint8_t worklen; /* Number of bits in the input bit buffer */ + + /* JPEG / Packet output buffer */ + uint8_t *out; /* Pointer to the beginning of the output buffer */ + uint8_t *outp; /* Pointer to the next output byte */ + size_t out_len; /* Number of output bytes remaining */ + char out_stuff; /* Flag to add stuffing bytes to output */ + + /* Output bits */ + uint32_t outbits; /* Output bit buffer */ + uint8_t outlen; /* Number of bits in the output bit buffer */ + + /* JPEG decoder state */ + enum { + S_MARKER = 0, + S_MARKER_LEN, + S_MARKER_DATA, + S_HUFF, + S_INT, + S_EOI, + } state; + uint16_t marker; /* Current marker */ + uint16_t marker_len; /* Length of data following marker */ + uint8_t *marker_data; /* Where to copy marker data too */ + uint16_t marker_data_len; /* How much is there */ + uint8_t component; /* 0 = Y, 1 = Cb, 2 = Cr */ + uint8_t mcupart; /* 0-3 = Y, 4 = Cb, 5 = Cr */ + uint8_t acpart; /* 0 - 64; 0 = DC, 1 - 64 = AC */ + int dc[COMPONENTS]; /* DC value for each component */ + uint8_t acrle; /* RLE value for current AC value */ + char dcmode; /* 0 = Absolute, 1 = Relative (parts 0, 4 & 5) */ + char needbits; /* Number of bits needed to decode integer */ + + /* Small buffer for reading SOF0 and SOS header data into */ + uint8_t hbuff[HBUFF_LEN]; + + /* SSDV2 experiment */ + int block[64]; + +} ssdv_t; + +typedef struct { + uint8_t image_id; + uint16_t packet_id; + uint16_t width; + uint16_t height; + uint16_t mcu_offset; + uint16_t mcu_id; + uint16_t mcu_count; +} ssdv_packet_info_t; + +/* Encoding */ +extern char ssdv_enc_init(ssdv_t *s, uint8_t image_id); +extern char ssdv_enc_set_buffer(ssdv_t *s, uint8_t *buffer); +extern char ssdv_enc_get_packet(ssdv_t *s); +extern char ssdv_enc_feed(ssdv_t *s, uint8_t *buffer, size_t length); + +/* Decoding */ +extern char ssdv_dec_init(ssdv_t *s); +extern char ssdv_dec_set_buffer(ssdv_t *s, uint8_t *buffer, size_t length); +extern char ssdv_dec_feed(ssdv_t *s, uint8_t *packet); +extern char ssdv_dec_get_jpeg(ssdv_t *s, uint8_t **jpeg, size_t *length); + +extern char ssdv_dec_is_packet(uint8_t *packet, int *errors); +extern void ssdv_dec_header(ssdv_packet_info_t *info, uint8_t *packet); + +#ifdef __cplusplus +} +#endif +#endif +