JPEGカメラを使ってみよう

写真を表示する

サイレントシステムで扱っているC1098-SSを用いて写真を撮り、それをカラーLCDに表示してみます。

他の項目も同じですが、このまま利用しても正しく動くという保証はありません。たまたま動いたかもしれないプログラムです。


main.cpp
/**
  C1098 JPEGカメラ画像をRGBに変換し、LCDに直接表示させる。
  p13: 緑色   p14:黄色
  Vdd=3.3V
  画像の大きさは80×60(1/4scale)
  RGB

LCDはどうも個々に制御LSIが異なる場合があるのでこのままで動かない可能性がある。
 */
#include "mbed.h"
#include "CameraC1098.h"
#include "tjpgd.h"
#include "externs.h"
#include "NokiaLCD.h"

#define S11520
#define SCALE 2 /* Output scaling 0:1/1, 1:1/2, 2:1/4 or 3:1/8 */

JRESULT rc;
JDEC jd;    // Decompression object (70 bytes)

/*
 * Modules.
 */
/*
C1098(J1)     mbed
3.3V（赤）
Tx  （黄）→ Rx(14)
Rx  （緑）← Tx(13)
GND （黒）

チェックのためにmbed上のLEDおよびLCD上のLEDを利用している
*/

#ifdef S115200
//                 tx   rx
CameraC1098 camera(p13, p14, CameraC1098::baud115200);
#else
CameraC1098 camera(p13, p14, CameraC1098::baud57600);
#endif
//          mosi, sclk, cs, rst
NokiaLCD lcd(p5,   p7,  p8, p9);
/**
 * A entry point.
 */
int main() {
  int counter=0;
  unsigned int pix;
  const size_t sz_work = 4096;
  UINT xs, ys;
  work = malloc(sz_work);
  led4=1;
  lcd.background(0x000080);   // 濃い青
  lcd.cls();
  sync();

  while(true)
  {
    led1=1;
    jpeg_snapshot_picture();
    led2=1;
    jd.width=0; jd.height=0;
    rc = jd_prepare(&jd, tjd_input, work, sz_work, 0);
    led3=1;
    xs = jd.width >> SCALE;     // Output size
    ys = jd.height >> SCALE;
    rc = jd_decomp(&jd, output_func, SCALE);
    led1=0;
    led2=0;
    led3=0;
    led4=0;
    camera.sendReset(true);
    for(int i=0; i<3; i++) {
      lcd.locate(i,i+12);
      lcd.printf("Hello %d", counter++);
    }
  }
  return 0;
}

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15 
16 
17 80×60とする
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69 
70 
71 
72 
73 
74 
75 
76 
77 
78 
79


メインプログラムを見ると「写真を撮る」とあまり変わりませんが、パソコンを利用した方法は、メモリも十分にあり、さらにWindowsのライブラリが頑張って処理してくれているから簡単にできるだけです。
本当の技術者は、そういうことは当然できるとして、mbedのような組み込み用のCPUでも同様に出来るようにするような人だと思います。このような技術者を目指しましょう。
どこが技術的に難しいかと簡単に示すと
  1. SPIのカラーLCDを用いている
  2. JPEGフォーマットのカメラを用いている
カラーLCDは別項目で説明するとして、２はJPEGフォーマットをRGBに変換しないとLCDに表示できないことが必要であるということを理解する必要があります。これをmbed上で実現しないといけません。これはそう簡単ではありません。JPEGの解凍が必要になります。ここではTJpgDecを利用して実現してみます。従って、勝手に使いやすいようにいじっています。

TJpgDecのヘッダ

tjpgd.h
/*----------------------------------------------------------------------------/
/ TJpgDec - Tiny JPEG Decompressor include file               (C)ChaN, 2011
/----------------------------------------------------------------------------*/

/* System Configurations */

#define	JD_SZBUF		512	/* Size of stream input buffer (should be multiple of 512) */
#define JD_FORMAT		0	/* Output RGB format 0:RGB888 (3 BYTE/pix), 1:RGB565 (1 WORD/pix) */
#define	JD_USE_SCALE	1	/* Use descaling feature for output */


/*---------------------------------------------------------------------------*/

#include "integer.h"


/* Error code */
typedef enum {
	JDR_OK = 0,	/* 0: Succeeded */
	JDR_INTR,	/* 1: Interrupted by output function */	
	JDR_INP,	/* 2: Device error or wrong termination of input stream */
	JDR_MEM1,	/* 3: Insufficient memory pool for the image */
	JDR_MEM2,	/* 4: Insufficient stream input buffer */
	JDR_PAR,	/* 5: Parameter error */
	JDR_FMT1,	/* 6: Data format error (may be damaged data) */
	JDR_FMT2,	/* 7: Right format but not supported */
	JDR_FMT3	/* 8: Not supported JPEG standard */
} JRESULT;


typedef struct {
	WORD left, right, top, bottom;
} JRECT;


/* Decompressor object structure */
typedef struct JDEC JDEC;
struct JDEC {
	UINT dctr;				/* Number of bytes available in the input buffer */
	BYTE* dptr;				/* Current data read ptr */
	BYTE* inbuf;			/* Bit stream input buffer */
	BYTE dmsk;				/* Current bit in the current read byte */
	BYTE scale;				/* Output scaling ratio */
	BYTE msx, msy;			/* MCU size in unit of block (width, height) */
	BYTE qtid[3];			/* Quantization table ID of each component */
	SHORT dcv[3];			/* Previous DC element of each component */
	WORD nrst;				/* Restart inverval */
	UINT width, height;		/* Size of the input image (pixel) */
	BYTE* huffbits[2][2];	/* Huffman bit distribution tables [id][dcac] */
	WORD* huffcode[2][2];	/* Huffman code word tables [id][dcac] */
	BYTE* huffdata[2][2];	/* Huffman decoded data tables [id][dcac] */
	LONG* qttbl[4];			/* Dequaitizer tables [id] */
	void* workbuf;			/* Working buffer for IDCT and RGB output */
	BYTE* mcubuf;			/* Working buffer for the MCU */
	void* pool;				/* Pointer to available memory pool */
	UINT sz_pool;			/* Size of momory pool (bytes available) */
	UINT (*infunc)(JDEC*, BYTE*, UINT);/* Pointer to jpeg stream input function */
	UINT (*outfunc)(JDEC*, void*, JRECT*);	/* Pointer to RGB output function */
	void* device;			/* Pointer to I/O device identifiler for the session */
};


#define	LDB_WORD(ptr)		(WORD)(((WORD)*((BYTE*)(ptr))<<8)|(WORD)*(BYTE*)((ptr)+1))



/* TJpgDec API functions */
JRESULT jd_prepare (JDEC*, UINT(*)(JDEC*,BYTE*,UINT), void*, UINT, void*);
JRESULT jd_decomp (JDEC*, UINT(*)(JDEC*,void*,JRECT*), BYTE);

 1 
 2 
 3 
 4 
 5 
 6 
 7 
 8 
 9 
10 
11 
12 
13 
14 
15 
16 
17 
18 
19 
20 
21 
22 
23 
24 
25 
26 
27 
28 
29 
30 
31 
32 
33 
34 
35 
36 
37 
38 
39 
40 
41 
42 
43 
44 
45 
46 
47 
48 
49 
50 
51 
52 
53 
54 
55 
56 
57 
58 
59 
60 
61 
62 
63 
64 
65 
66 
67 
68 
69


何処を修正したか忘れたが・・・取りあえず

tjpgd.c
/*----------------------------------------------------------------------------/
/ TJpgDec - Tiny JPEG Decompressor R0.01                      (C)ChaN, 2011
/-----------------------------------------------------------------------------/
/ The TJpgDec is a generic JPEG decompressor module for tiny embedded systems.
/ This is a free software that opened for education, research and commercial
/  developments under license policy of following terms.
/
/  Copyright (C) 2011, ChaN, all right reserved.
/
/ * The TJpgDec module is a free software and there is NO WARRANTY.
/ * No restriction on use. You can use, modify and redistribute it for
/   personal, non-profit or commercial products UNDER YOUR RESPONSIBILITY.
/ * Redistributions of source code must retain the above copyright notice.
/
/-----------------------------------------------------------------------------/
/ Oct 04,'11 R0.01  First release.
/----------------------------------------------------------------------------*/
#include <stdio.h>
#include "tjpgd.h"



/*-----------------------------------------------*/
/* Zigzag-order to raster-order conversion table */
/*-----------------------------------------------*/

#define ZIG(n)  Zig[n]

static
const BYTE Zig[64] = {  /* Zigzag-order to raster-order conversion table */
   0,  1,  8, 16,  9,  2,  3, 10, 17, 24, 32, 25, 18, 11,  4,  5,
  12, 19, 26, 33, 40, 48, 41, 34, 27, 20, 13,  6,  7, 14, 21, 28,
  35, 42, 49, 56, 57, 50, 43, 36, 29, 22, 15, 23, 30, 37, 44, 51,
  58, 59, 52, 45, 38, 31, 39, 46, 53, 60, 61, 54, 47, 55, 62, 63
};



/*-------------------------------------------------*/
/* Input scale factor of Arai algorithm            */
/* (scaled up 13 bits for fixed point operations)  */
/*-------------------------------------------------*/

#define IPSF(n) Ipsf[n]

static
const WORD Ipsf[64] = {
  (WORD)(1.00000*8192), (WORD)(1.38704*8192), (WORD)(1.30656*8192), (WORD)(1.17588*8192), (WORD)(1.00000*8192), (WORD)(0.78570*8192), (WORD)(0.54120*8192), (WORD)(0.27590*8192),
  (WORD)(1.38704*8192), (WORD)(1.92388*8192), (WORD)(1.81226*8192), (WORD)(1.63099*8192), (WORD)(1.38704*8192), (WORD)(1.08979*8192), (WORD)(0.75066*8192), (WORD)(0.38268*8192),
  (WORD)(1.30656*8192), (WORD)(1.81226*8192), (WORD)(1.70711*8192), (WORD)(1.53636*8192), (WORD)(1.30656*8192), (WORD)(1.02656*8192), (WORD)(0.70711*8192), (WORD)(0.36048*8192),
  (WORD)(1.17588*8192), (WORD)(1.63099*8192), (WORD)(1.53636*8192), (WORD)(1.38268*8192), (WORD)(1.17588*8192), (WORD)(0.92388*8192), (WORD)(0.63638*8192), (WORD)(0.32442*8192),
  (WORD)(1.00000*8192), (WORD)(1.38704*8192), (WORD)(1.30656*8192), (WORD)(1.17588*8192), (WORD)(1.00000*8192), (WORD)(0.78570*8192), (WORD)(0.54120*8192), (WORD)(0.27590*8192),
  (WORD)(0.78570*8192), (WORD)(1.08979*8192), (WORD)(1.02656*8192), (WORD)(0.92388*8192), (WORD)(0.78570*8192), (WORD)(0.61732*8192), (WORD)(0.42522*8192), (WORD)(0.21677*8192),
  (WORD)(0.54120*8192), (WORD)(0.75066*8192), (WORD)(0.70711*8192), (WORD)(0.63638*8192), (WORD)(0.54120*8192), (WORD)(0.42522*8192), (WORD)(0.29290*8192), (WORD)(0.14932*8192),
  (WORD)(0.27590*8192), (WORD)(0.38268*8192), (WORD)(0.36048*8192), (WORD)(0.32442*8192), (WORD)(0.27590*8192), (WORD)(0.21678*8192), (WORD)(0.14932*8192), (WORD)(0.07612*8192)
};



/*---------------------------------------------*/
/* Conversion table for fast clipping process  */
/*---------------------------------------------*/

#define BYTECLIP(v) Clip8[(UINT)(v) & 0x3FF]

static
const BYTE Clip8[1024] = {
  /* 0..255 */
  0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
  32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63,
  64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95,
  96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127,
  128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159,
  160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191,
  192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223,
  224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255,
  /* 256..511 */
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
  /* -512..-257 */
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  /* -256..-1 */
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};




/*-----------------------------------------------------------------------*/
/* Allocate a memory block from memory pool                              */
/*-----------------------------------------------------------------------*/

static
void* alloc_pool (  /* Pointer to allocated memory block (NULL:no memory available) */
  JDEC* jd,   /* Pointer to the decompressor object */
  UINT nd     /* Number of bytes to allocate */
)
{
  char *rp = 0;


  nd = (nd + 3) & ~3;     /* Align block size to the word boundary */

  if (jd->sz_pool >= nd) {
    jd->sz_pool -= nd;
    rp = (char*)jd->pool;     /* Get start of available memory pool */
    jd->pool = (void*)(rp + nd);  /* Allocate requierd bytes */
  }

  return (void*)rp; /* Return allocated memory block (NULL:no memory to allocate) */
}




/*-----------------------------------------------------------------------*/
/* Create de-quantization and prescaling tables with a DQT segment       */
/*-----------------------------------------------------------------------*/

static
UINT create_qt_tbl (  /* 0:OK, !0:Failed */
  JDEC* jd,     /* Pointer to the decompressor object */
  const BYTE* data, /* Pointer to the quantizer tables */
  UINT ndata      /* Size of input data */
)
{
  UINT i;
  BYTE d, z;
  LONG *pb;


  while (ndata) { /* Process all tables in the segment */
    if (ndata < 65) return JDR_FMT1;  /* Err: table size is unaligned */
    ndata -= 65;
    d = *data++;              /* Get table property */
    if (d & 0xF0) return JDR_FMT1;      /* Err: not 8-bit resolution */
    i = d & 3;                /* Get table ID */
    pb = (LONG*)alloc_pool(jd, 64 * sizeof(LONG)); /* Allocate a memory block for the table */
    if (!pb) return JDR_MEM1;       /* Err: not enough memory */
    jd->qttbl[i] = pb;            /* Register the table */
    for (i = 0; i < 64; i++) {        /* Load the table */
      z = ZIG(i);             /* Zigzag-order to raster-order conversion */
      pb[z] = (LONG)((DWORD)*data++ * IPSF(z)); /* Apply scale factor of Arai algorithm to the de-quantizers */
    }
  }

  return JDR_OK;
}




/*-----------------------------------------------------------------------*/
/* Create huffman code tables with a DHT segment                         */
/*-----------------------------------------------------------------------*/

static
UINT create_huffman_tbl ( /* 0:OK, !0:Failed */
  JDEC* jd,       /* Pointer to the decompressor object */
  const BYTE* data,   /* Pointer to the packed huffman tables */
  UINT ndata        /* Size of input data */
)
{
  UINT i, j, b, np, cls, num;
  BYTE d, *pb, *pd;
  WORD hc, *ph;


  while (ndata) { /* Process all tables in the segment */
    if (ndata < 17) return JDR_FMT1;  /* Err: wrong data size */
    ndata -= 17;
    d = *data++;            /* Get table number and class */
    cls = (d >> 4); num = d & 0x0F;   /* class = dc(0)/ac(1), table number = 0/1 */
    if (d & 0xEE) return JDR_FMT1;    /* Err: invalid class/number */
    pb = (BYTE*)alloc_pool(jd, 16);      /* Allocate a memory block for the bit distribution table */
    if (!pb) return JDR_MEM1;     /* Err: not enough memory */
    jd->huffbits[num][cls] = pb;
    for (np = i = 0; i < 16; i++) {   /* Load number of patterns for 1 to 16-bit code */
      pb[i] = b = *data++;
      np += b;  /* Get sum of code words for each code */
    }

    ph = (WORD*)alloc_pool(jd, np * sizeof(WORD)); /* Allocate a memory block for the code word table */
    if (!ph) return JDR_MEM1;     /* Err: not enough memory */
    jd->huffcode[num][cls] = ph;
    hc = 0;
    for (j = i = 0; i < 16; i++) {    /* Re-build huffman code word table */
      b = pb[i];
      while (b--) ph[j++] = hc++;
      hc <<= 1;
    }

    if (ndata < np) return JDR_FMT1;  /* Err: wrong data size */
    ndata -= np;
    pd = (BYTE*)alloc_pool(jd, np);      /* Allocate a memory block for the decoded data */
    if (!pd) return JDR_MEM1;     /* Err: not enough memory */
    jd->huffdata[num][cls] = pd;
    for (i = 0; i < np; i++) {      /* Load decoded data corresponds to each code ward */
      d = *data++;
      if (!cls && d > 11) return JDR_FMT1;
      *pd++ = d;
    }
  }

  return JDR_OK;
}




/*-----------------------------------------------------------------------*/
/* Extract N bits from input stream                                      */
/*-----------------------------------------------------------------------*/

static
INT bitext (  /* >=0: extracted data, <0: error code */
  JDEC* jd, /* Pointer to the decompressor object */
  UINT nbit /* Number of bits to extract (1 to 11) */
)
{
  BYTE msk, s, *dp;
  UINT dc, v, f;


  msk = jd->dmsk; dc = jd->dctr; dp = jd->dptr; /* Bit mask, number of data available, read ptr */
  s = *dp; v = f = 0;
  do {
    if (!msk) {       /* Next byte? */
      if (!dc) {      /* No input data is available, re-fill input buffer */
        dp = jd->inbuf; /* Top of input buffer */
        dc = jd->infunc(jd, dp, JD_SZBUF);
        if (!dc) return 0 - JDR_INP;  /* Err: read error or wrong stream termination */
      } else {
        dp++;     /* Next data ptr */
      }
      dc--;       /* Decrement number of available bytes */
      if (f) {      /* In flag sequence? */
        f = 0;      /* Exit flag sequence */
        if (*dp != 0) return 0 - JDR_FMT1;  /* Err: unexpected flag is detected (may be collapted data) */
        *dp = s = 0xFF;     /* The flag is a data 0xFF */
      } else {
        s = *dp;        /* Get next data byte */
        if (s == 0xFF) {    /* Is start of flag sequence? */
          f = 1; continue;  /* Enter flag sequence */
        }
      }
      msk = 0x80;   /* Read from MSB */
    }
    v <<= 1;  /* Get a bit */
    if (s & msk) v++;
    msk >>= 1;
    nbit--;
  } while (nbit);
  jd->dmsk = msk; jd->dctr = dc; jd->dptr = dp;

  return (INT)v;
}




/*-----------------------------------------------------------------------*/
/* Extract a huffman decoded data from input stream                      */
/*-----------------------------------------------------------------------*/

static
INT huffext (     /* >=0: decoded data, <0: error code */
  JDEC* jd,     /* Pointer to the decompressor object */
  const BYTE* hbits,  /* Pointer to the bit distribution table */
  const WORD* hcode,  /* Pointer to the code word table */
  const BYTE* hdata /* Pointer to the data table */
)
{
  BYTE msk, s, *dp;
  UINT dc, v, f, bl, nd;


  msk = jd->dmsk; dc = jd->dctr; dp = jd->dptr; /* Bit mask, number of data available, read ptr */
  s = *dp; v = f = 0;
  bl = 16;  /* Max code length */
  do {
    if (!msk) {   /* Next byte? */
      if (!dc) {  /* No input data is available, re-fill input buffer */
        dp = jd->inbuf; /* Top of input buffer */
        dc = jd->infunc(jd, dp, JD_SZBUF);
        if (!dc) return 0 - JDR_INP;  /* Err: read error or wrong stream termination */
      } else {
        dp++; /* Next data ptr */
      }
      dc--;   /* Decrement number of available bytes */
      if (f) {    /* In flag sequence? */
        f = 0;    /* Exit flag sequence */
        if (*dp != 0)
          return 0 - JDR_FMT1;/* Err: unexpected flag is detected (may be collapted data) */
        *dp = s = 0xFF;     /* The flag is a data 0xFF */
      } else {
        s = *dp;        /* Get next data byte */
        if (s == 0xFF) {    /* Is start of flag sequence? */
          f = 1; continue;  /* Enter flag sequence, get trailing byte */
        }
      }
      msk = 0x80;   /* Read from MSB */
    }
    v <<= 1;  /* Get a bit */
    if (s & msk) v++;
    msk >>= 1;

    for (nd = *hbits++; nd; nd--) { /* Search the code word in this bit length */
      if (v == *hcode++) {    /* Matched? */
        jd->dmsk = msk; jd->dctr = dc; jd->dptr = dp;
        return *hdata;      /* Return the decoded data */
      }
      hdata++;
    }
    bl--;
  } while (bl);

  return 0 - JDR_FMT1;  /* Err: code not found (may be collapted data) */
}




/*-----------------------------------------------------------------------*/
/* Apply Inverse-DCT in Arai Algorithm (refer to aa_idct.png)            */
/*-----------------------------------------------------------------------*/

static
void block_idct (
  LONG* src,  /* Input block data (de-quantized and pre-scaled for Arai Algorithm) */
  BYTE* dst /* Pointer to the destination to store the block as byte array */
)
{
  const LONG M13 = (LONG)(1.41421*(1<<12)), M2 = (LONG)(1.08239*(1<<12)), M4 = (LONG)(2.61313*(1<<12)), M5 = (LONG)(1.84776*(1<<12));
  LONG v0, v1, v2, v3, v4, v5, v6, v7;
  LONG t10, t11, t12, t13;
  UINT i;

  /* Process columns */
  for (i = 0; i < 8; i++) {
    v0 = src[8 * 0];  /* Get even elements */
    v1 = src[8 * 2];
    v2 = src[8 * 4];
    v3 = src[8 * 6];

    t10 = v0 + v2;    /* Process the even elements */
    t12 = v0 - v2;
    t11 = (v1 - v3) * M13 >> 12;
    v3 += v1;
    t11 -= v3;
    v0 = t10 + v3;
    v3 = t10 - v3;
    v1 = t11 + t12;
    v2 = t12 - t11;

    v4 = src[8 * 7];  /* Get odd elements */
    v5 = src[8 * 1];
    v6 = src[8 * 5];
    v7 = src[8 * 3];

    t10 = v5 - v4;    /* Process the odd elements */
    t11 = v5 + v4;
    t12 = v6 - v7;
    v7 += v6;
    v5 = (t11 - v7) * M13 >> 12;
    v7 += t11;
    t13 = (t10 + t12) * M5 >> 12;
    v4 = t13 - (t10 * M2 >> 12);
    v6 = t13 - (t12 * M4 >> 12) - v7;
    v5 -= v6;
    v4 -= v5;

    src[8 * 0] = v0 + v7; /* Write-back transformed values */
    src[8 * 7] = v0 - v7;
    src[8 * 1] = v1 + v6;
    src[8 * 6] = v1 - v6;
    src[8 * 2] = v2 + v5;
    src[8 * 5] = v2 - v5;
    src[8 * 3] = v3 + v4;
    src[8 * 4] = v3 - v4;

    src++;  /* Next column */
  }

  /* Process rows */
  src -= 8;
  for (i = 0; i < 8; i++) {
    v0 = src[0] + ((LONG)128 << 8); /* Get even elements (remove DC offset (-128) here) */
    v1 = src[2];
    v2 = src[4];
    v3 = src[6];

    t10 = v0 + v2;        /* Process the even elements */
    t12 = v0 - v2;
    t11 = (v1 - v3) * M13 >> 12;
    v3 += v1;
    t11 -= v3;
    v0 = t10 + v3;
    v3 = t10 - v3;
    v1 = t11 + t12;
    v2 = t12 - t11;

    v4 = src[7];        /* Get odd elements */
    v5 = src[1];
    v6 = src[5];
    v7 = src[3];

    t10 = v5 - v4;        /* Process the odd elements */
    t11 = v5 + v4;
    t12 = v6 - v7;
    v7 += v6;
    v5 = (t11 - v7) * M13 >> 12;
    v7 += t11;
    t13 = (t10 + t12) * M5 >> 12;
    v4 = t13 - (t10 * M2 >> 12);
    v6 = t13 - (t12 * M4 >> 12) - v7;
    v5 -= v6;
    v4 -= v5;

    dst[0] = BYTECLIP((v0 + v7) >> 8);  /* Output transformed values */
    dst[7] = BYTECLIP((v0 - v7) >> 8);
    dst[1] = BYTECLIP((v1 + v6) >> 8);
    dst[6] = BYTECLIP((v1 - v6) >> 8);
    dst[2] = BYTECLIP((v2 + v5) >> 8);
    dst[5] = BYTECLIP((v2 - v5) >> 8);
    dst[3] = BYTECLIP((v3 + v4) >> 8);
    dst[4] = BYTECLIP((v3 - v4) >> 8);
    dst += 8;

    src += 8; /* Next row */
  }
}




/*-----------------------------------------------------------------------*/
/* Load all blocks in the MCU into working buffer                        */
/*-----------------------------------------------------------------------*/

static
JRESULT mcu_load (
  JDEC* jd    /* Pointer to the decompressor object */
)
{
  LONG *tmp = (LONG*)jd->workbuf; /* Block working buffer for de-quantize and IDCT */
  UINT blk, nby, nbc, i, z, id, cmp;
  INT b, d, e;
  BYTE *bp;
  const BYTE *hb, *hd;
  const WORD *hc;
  const LONG *dqf;


  nby = jd->msx * jd->msy;  /* Number of Y blocks (1, 2 or 4) */
  nbc = 2;          /* Number of C blocks (2) */
  bp = jd->mcubuf;      /* Pointer to the first block */

  for (blk = 0; blk < nby + nbc; blk++) {
    cmp = (blk < nby) ? 0 : blk - nby + 1;  /* Component number 0:Y, 1:Cb, 2:Cr */
    id = cmp ? 1 : 0;           /* Huffman table ID of the component */

    /* Extract a DC element from input stream */
    hb = jd->huffbits[id][0];       /* Huffman table for the DC element */
    hc = jd->huffcode[id][0];
    hd = jd->huffdata[id][0];
    b = huffext(jd, hb, hc, hd);      /* Extract a huffman coded data (bit length) */
    if (b < 0) return (JRESULT)(0 - b);        /* Err: invalid code or input */
    d = jd->dcv[cmp];           /* DC value of previous block */
    if (b) {                /* If there is any difference from previous block */
      e = bitext(jd, b);          /* Extract data bits */
      if (e < 0) return (JRESULT)(0 - e);      /* Err: input */
      b = 1 << (b - 1);         /* MSB position */
      if (!(e & b)) e -= (b << 1) - 1;  /* Restore sign if needed */
      d += e;               /* Get current value */
      jd->dcv[cmp] = (SHORT)d;      /* Save current DC value for next block */
    }
    dqf = jd->qttbl[jd->qtid[cmp]];     /* De-quantizer table ID for this component */
    tmp[0] = d * dqf[0] >> 8;       /* De-quantize and apply scale factor of Arai algorithm */

    /* Extract following 63 AC elements from input stream */
    for (i = 1; i < 64; i++) tmp[i] = 0;  /* Clear rest of elements */
    hb = jd->huffbits[id][1];       /* Huffman table for the AC elements */
    hc = jd->huffcode[id][1];
    hd = jd->huffdata[id][1];
    i = 1;          /* Top of the AC elements */
    do {
      b = huffext(jd, hb, hc, hd);    /* Extract a huffman coded value (zero runs and bit length) */
      if (b == 0) break;          /* EOB? */
      if (b < 0) return (JRESULT)(0 - b);      /* Err: invalid code or input error */
      z = (UINT)b >> 4;         /* Number of leading zero elements */
      if (z) {
        i += z;             /* Skip zero elements */
        if (i >= 64) return JDR_FMT1; /* Too long zero run */
      }
      if (b &= 0x0F) {          /* Bit length */
        d = bitext(jd, b);        /* Extract data bits */
        if (d < 0) return (JRESULT)(0 - d);    /* Err: input device */
        b = 1 << (b - 1);       /* MSB position */
        if (!(d & b)) d -= (b << 1) - 1;/* Restore negative value if needed */
        z = ZIG(i);           /* Zigzag-order to raster-order converted index */
        tmp[z] = d * dqf[z] >> 8;   /* De-quantize and apply scale factor of Arai algorithm */
      }
    } while (++i < 64);   /* Next AC element */

    if (JD_USE_SCALE && jd->scale == 3) {
      *bp = (*tmp / 256) + 128; /* If scale ratio is 1/8, IDCT can be ommited and only DC element is used */
    } else {
      block_idct(tmp, bp);    /* Apply IDCT and store the block to the MCU buffer */
    }

    bp += 64;       /* Next block */
  }

  return JDR_OK;  /* All blocks have been loaded successfully */
}




/*-----------------------------------------------------------------------*/
/* Output an MCU: Convert YCrCb to RGB and output it in RGB form         */
/*-----------------------------------------------------------------------*/

static
JRESULT mcu_output (
  JDEC* jd, /* Pointer to the decompressor object */
  UINT x,   /* MCU position in the image (left of the MCU) */
  UINT y    /* MCU position in the image (top of the MCU) */
)
{
  const INT CVACC = (sizeof(INT) > 2) ? 1024 : 128;
  UINT ix, iy, mx, my, rx, ry;
  INT yy, cb, cr;
  BYTE *py, *pc, *rgb24;
  JRECT rect;


  mx = jd->msx * 8; my = jd->msy * 8;         /* MCU size (pixel) */
  rx = (x + mx <= jd->width) ? mx : jd->width - x;  /* Output rectangular size (it may be clipped at right/bottom end) */
  ry = (y + my <= jd->height) ? my : jd->height - y;
  if (JD_USE_SCALE) {
    rx >>= jd->scale; ry >>= jd->scale;
    if (!rx || !ry) return JDR_OK;          /* Skip this MCU if all pixel is to be rounded off */
    x >>= jd->scale; y >>= jd->scale;
  }
  rect.left = x; rect.right = x + rx - 1;       /* Rectangular area in the frame buffer */
  rect.top = y; rect.bottom = y + ry - 1;


  if (!JD_USE_SCALE || jd->scale != 3) {  /* Not for 1/8 scaling */

    /* Build an RGB MCU from discrete comopnents */
    rgb24 = (BYTE*)jd->workbuf;
    for (iy = 0; iy < my; iy++) {
      pc = jd->mcubuf;
      py = pc + iy * 8;
      if (my == 16) {   /* Double block height? */
        pc += 64 * 4 + (iy >> 1) * 8;
        if (iy >= 8) py += 64;
      } else {      /* Single block height */
        pc += mx * 8 + iy * 8;
      }
      for (ix = 0; ix < mx; ix++) {
        cb = pc[0] - 128;   /* Get Cb/Cr component and restore right level */
        cr = pc[64] - 128;
        if (mx == 16) {         /* Double block width? */
          if (ix == 8) py += 64 - 8;  /* Jump to next block if double block heigt */
          pc += ix & 1;       /* Increase chroma pointer every two pixels */
        } else {            /* Single block width */
          pc++;           /* Increase chroma pointer every pixel */
        }
        yy = *py++;     /* Get Y component */

        /* Convert YCbCr to RGB */
        *rgb24++ = /* R */ BYTECLIP(yy + ((INT)(1.402 * CVACC) * cr) / CVACC);
        *rgb24++ = /* G */ BYTECLIP(yy - ((INT)(0.344 * CVACC) * cb + (INT)(0.714 * CVACC) * cr) / CVACC);
        *rgb24++ = /* B */ BYTECLIP(yy + ((INT)(1.772 * CVACC) * cb) / CVACC);
      }
    }

    /* Descale the MCU rectangular if needed */
    if (JD_USE_SCALE && jd->scale) {
      UINT x, y, r, g, b, s, w, a;
      BYTE *op;

      /* Get averaged RGB value of each square correcponds to a pixel */
      s = jd->scale * 2;  /* Bumber of shifts for averaging */
      w = 1 << jd->scale; /* Width of square */
      a = (mx - w) * 3; /* Bytes to skip for next line in the square */
      op = (BYTE*)jd->workbuf;
      for (iy = 0; iy < my; iy += w) {
        for (ix = 0; ix < mx; ix += w) {
          rgb24 = (BYTE*)jd->workbuf + (iy * mx + ix) * 3;
          r = g = b = 0;
          for (y = 0; y < w; y++) { /* Accumulate RGB value in the square */
            for (x = 0; x < w; x++) {
              r += *rgb24++;
              g += *rgb24++;
              b += *rgb24++;
            }
            rgb24 += a;
          }             /* Put the averaged RGB value as a pixel */
          *op++ = (BYTE)(r >> s);
          *op++ = (BYTE)(g >> s);
          *op++ = (BYTE)(b >> s);
        }
      }
    }

  } else {  /* For only 1/8 scaling (left-top pixel in each block are the DC value of the block) */

    /* Build a 1/8 descaled RGB MCU from discrete comopnents */
    rgb24 = (BYTE*)jd->workbuf;
    pc = jd->mcubuf + mx * my;
    cb = pc[0] - 128;   /* Get Cb/Cr component and restore right level */
    cr = pc[64] - 128;
    for (iy = 0; iy < my; iy += 8) {
      py = jd->mcubuf;
      if (iy == 8) py += 64 * 2;
      for (ix = 0; ix < mx; ix += 8) {
        yy = *py; /* Get Y component */
        py += 64;

        /* Convert YCbCr to RGB */
        *rgb24++ = /* R */ BYTECLIP(yy + ((INT)(1.402 * CVACC) * cr / CVACC));
        *rgb24++ = /* G */ BYTECLIP(yy - ((INT)(0.344 * CVACC) * cb + (INT)(0.714 * CVACC) * cr) / CVACC);
        *rgb24++ = /* B */ BYTECLIP(yy + ((INT)(1.772 * CVACC) * cb / CVACC));
      }
    }
  }

  /* Squeeze up pixel table if a part of MCU is to be truncated */
  mx >>= jd->scale;
  if (rx < mx) {
    BYTE *s, *d;
    UINT x, y;

    s = d = (BYTE*)jd->workbuf;
    for (y = 0; y < ry; y++) {
      for (x = 0; x < rx; x++) {  /* Copy effective pixels */
        *d++ = *s++;
        *d++ = *s++;
        *d++ = *s++;
      }
      s += (mx - rx) * 3; /* Skip truncated pixels */
    }
  }

  /* Convert RGB888 to RGB565 if needed */
  if (JD_FORMAT == 1) {
    BYTE *s = (BYTE*)jd->workbuf;
    WORD w, *d = (WORD*)s;
    UINT n = rx * ry;

    do {
      w = (*s++ & 0xF8) << 8;   /* RRRRR----------- */
      w |= (*s++ & 0xFC) << 3;  /* -----GGGGGG----- */
      w |= *s++ >> 3;       /* -----------BBBBB */
      *d++ = w;
    } while (--n);
  }

  /* Output the RGB rectangular */

  return jd->outfunc(jd, jd->workbuf, &rect) ? JDR_OK : JDR_INTR;
}




/*-----------------------------------------------------------------------*/
/* Process restart interval                                              */
/*-----------------------------------------------------------------------*/

static
JRESULT restart (
  JDEC* jd, /* Pointer to the decompressor object */
  WORD rstn /* Expected restert sequense number */
)
{
  UINT i, dc;
  WORD d;
  BYTE *dp;


  /* Discard padding bits and get two bytes from the input stream */
  dp = jd->dptr; dc = jd->dctr;
  d = 0;
  for (i = 0; i < 2; i++) {
    if (!dc) {  /* No input data is available, re-fill input buffer */
      dp = jd->inbuf;
      dc = jd->infunc(jd, dp, JD_SZBUF);
      if (!dc) return JDR_INP;
    } else {
      dp++;
    }
    dc--;
    d = (d << 8) | *dp; /* Get a byte */
  }
  jd->dptr = dp; jd->dctr = dc; jd->dmsk = 0;

  /* Check the marker */
  if ((d & 0xFFD8) != 0xFFD0 || (d & 7) != (rstn & 7))
    return JDR_FMT1;  /* Err: expected RSTn marker is not detected (may be collapted data) */

  /* Reset DC offset */
  jd->dcv[2] = jd->dcv[1] = jd->dcv[0] = 0;

  return JDR_OK;
}




/*-----------------------------------------------------------------------*/
/* Analyze the JPEG image and Initialize decompressor object             */
/*-----------------------------------------------------------------------*/

#define LDB_WORD(ptr)   (WORD)(((WORD)*((BYTE*)(ptr))<<8)|(WORD)*(BYTE*)((ptr)+1))


JRESULT jd_prepare (
  JDEC* jd,     /* Blank decompressor object */
  UINT (*infunc)(JDEC*, BYTE*, UINT), /* JPEG strem input function */
  void* pool,     /* Working buffer for the decompression session */
  UINT sz_pool,   /* Size of working buffer */
  void* dev     /* I/O device identifier for the session */
)
{
  BYTE *seg, b;
  WORD marker;
  DWORD ofs;
  UINT n, i, j, len;
  JRESULT rc;


  if (!pool) return JDR_PAR;

  jd->pool = pool;    /* Work memroy */
  jd->sz_pool = sz_pool;  /* Size of given work memory */
  jd->infunc = infunc;  /* Stream input function */
  jd->device = dev;   /* I/O device identifier */
  jd->nrst = 0;     /* No restart interval (default) */

  for (i = 0; i < 2; i++) { /* Nulls pointers */
    for (j = 0; j < 2; j++) {
      jd->huffbits[i][j] = 0;
      jd->huffcode[i][j] = 0;
      jd->huffdata[i][j] = 0;
    }
  }
  for (i = 0; i < 4; i++) jd->qttbl[i] = 0;

  jd->inbuf = seg = (BYTE*)alloc_pool(jd, JD_SZBUF);   /* Allocate stream input buffer */
  if (!seg) return JDR_MEM1;
  if (jd->infunc(jd, seg, 2) != 2) return JDR_INP;/* Check SOI marker */
  if (LDB_WORD(seg) != 0xFFD8) return JDR_FMT1; /* Err: SOI is not detected */
  ofs = 2;
//printf("for (;;) {\n");

  for (;;) {
    /* Get a JPEG marker */
    if (jd->infunc(jd, seg, 4) != 4) return JDR_INP;
    marker = LDB_WORD(seg);   /* Marker */
    len = LDB_WORD(seg + 2);  /* Length field */
//    printf("Len=%02x\n", len);
    if (len <= 2 || (marker >> 8) != 0xFF) return JDR_FMT1;
    len -= 2;   /* Content size excluding length field */
    ofs += 4 + len; /* Number of bytes loaded */
//printf("ofs += 4 + len; %d marker=%02x len=%02x\n", ofs, marker, len);

    switch (marker & 0xFF) {
    case 0xC0:  /* SOF0 (baseline JPEG) */
      /* Load segment data */
      if (len > JD_SZBUF) return JDR_MEM2;
      if (jd->infunc(jd, seg, len) != len) return JDR_INP;

      jd->width = LDB_WORD(seg+3);    /* Image width in unit of pixel */
      jd->height = LDB_WORD(seg+1);   /* Image height in unit of pixel */
      if (seg[5] != 3) return JDR_FMT3; /* Err: Supports only Y/Cb/Cr format */

      /* Check three image components */
      for (i = 0; i < 3; i++) {
        b = seg[7 + 3 * i];             /* Get sampling factor */
        if (!i) { /* Y component */
          if (b != 0x11 && b != 0x22 && b != 0x21)/* Check sampling factor */
            return JDR_FMT3;          /* Err: Supports only 4:4:4, 4:2:0 or 4:2:2 */
          jd->msx = b >> 4; jd->msy = b & 15;   /* Size of MCU [blocks] */
        } else {  /* Cb/Cr component */
          if (b != 0x11) return JDR_FMT3;     /* Err: Sampling factor of Cr/Cb must be 1 */
        }
        b = seg[8 + 3 * i];             /* Get dequantizer table ID for this component */
        if (b > 3) return JDR_FMT3;         /* Err: Invalid ID */
        jd->qtid[i] = b;
      }
      break;

    case 0xDD:  /* DRI */
      /* Load segment data */
      if (len > JD_SZBUF) return JDR_MEM2;
      if (jd->infunc(jd, seg, len) != len) return JDR_INP;

      /* Get restart interval (MCUs) */
      jd->nrst = LDB_WORD(seg);
      break;

    case 0xC4:  /* DHT */
      /* Load segment data */
      if (len > JD_SZBUF) return JDR_MEM2;
      if (jd->infunc(jd, seg, len) != len) return JDR_INP;

      /* Create huffman tables */
      rc = (JRESULT)create_huffman_tbl(jd, seg, len);
      if (rc) return rc;
      break;

    case 0xDB:  /* DQT */
      /* Load segment data */
      if (len > JD_SZBUF) return JDR_MEM2;
      if (jd->infunc(jd, seg, len) != len) return JDR_INP;

      /* Create de-quantizer tables */
      rc = (JRESULT)create_qt_tbl(jd, seg, len);
      if (rc) return rc;
      break;

    case 0xDA:  /* SOS */
      /* Load segment data */
      if (len > JD_SZBUF) return JDR_MEM2;
      if (jd->infunc(jd, seg, len) != len) return JDR_INP;

      if (!jd->width || !jd->height) return JDR_FMT1; /* Err: Invalid image size */

      if (seg[0] != 3) return JDR_FMT3;       /* Err: Supports only three color components format */

      /* Check if all tables corresponding to each components have been loaded */
      for (i = 0; i < 3; i++) {
        b = seg[2 + 2 * i]; /* Get huffman table ID */
        if (b != 0x00 && b != 0x11) return JDR_FMT3;  /* Err: Different table number for DC/AC element */
        b = i ? 1 : 0;
        if (!jd->huffbits[b][0] || !jd->huffbits[b][1]) /* Check huffman table for this component */
          return JDR_FMT1;              /* Err: Huffman table not loaded */
        if (!jd->qttbl[jd->qtid[i]]) return JDR_FMT1; /* Err: Dequantizer table not loaded */
      }

      /* Allocate working buffer for MCU and RGB */
      n = jd->msy * jd->msx;            /* Number of Y blocks in the MCU */
      if (!n) return JDR_FMT1;          /* Err: SOF0 has not been loaded */
      len = n * 64 * 2 + 64;            /* Allocate buffer for IDCT and RGB output */
      if (len < 256) len = 256;         /* but at least 256 byte is required for IDCT */
      jd->workbuf = alloc_pool(jd, len);      /* and it may occupy a part of following MCU working buffer for RGB output */
      if (!jd->workbuf) return JDR_MEM1;      /* Err: not enough memory */
      jd->mcubuf = (BYTE*)alloc_pool(jd, (n + 2) * 64);  /* Allocate MCU working buffer */
      if (!jd->mcubuf) return JDR_MEM1;     /* Err: not enough memory */

      /* Pre-load the JPEG data to extract it from the bit stream */
      len = jd->infunc(jd, seg, JD_SZBUF - (UINT)ofs % JD_SZBUF); /* Align read offset to JD_SZBUF */
      if (!len) return JDR_INP;               /* Err: wrong stream termination */
      jd->dctr = len - 1; jd->dptr = seg; jd->dmsk = 0x80;  /* Prepare to read bit stream */

      return JDR_OK;    /* Initialization succeeded. Ready to decompress the JPEG image. */

    case 0xC1:  /* SOF1 */
    case 0xC2:  /* SOF2 */
    case 0xC3:  /* SOF3 */
    case 0xC5:  /* SOF5 */
    case 0xC6:  /* SOF6 */
    case 0xC7:  /* SOF7 */
    case 0xC9:  /* SOF9 */
    case 0xCA:  /* SOF10 */
    case 0xCB:  /* SOF11 */
    case 0xCD:  /* SOF13 */
    case 0xCE:  /* SOF14 */
    case 0xCF:  /* SOF15 */
    case 0xD9:  /* EOI */
      return JDR_FMT3;  /* Unsuppoted JPEG standard (may be progressive JPEG) */

    default:  /* Unknown segment (comment, exif or etc..) */
      /* Skip segment data */
//      printf("Unknown segment\n");
      if (jd->infunc(jd, 0, len) != len)  /* Null pointer specifies to skip bytes of stream */
        return JDR_INP;
    }
  }
}




/*-----------------------------------------------------------------------*/
/* Start to decompress the JPEG picture                                  */
/*-----------------------------------------------------------------------*/

JRESULT jd_decomp (
  JDEC* jd,               /* Initialized decompression object */
  UINT (*outfunc)(JDEC*, void*, JRECT*),  /* RGB output function */
  BYTE scale                /* Output de-scaling factor (0 to 3) */
)
{
  UINT x, y, mx, my;
  WORD rst, rsc;
  JRESULT rc;


  jd->outfunc = outfunc;
  if (scale > (JD_USE_SCALE ? 3 : 0)) return JDR_PAR;
  jd->scale = scale;

  mx = jd->msx * 8; my = jd->msy * 8;     /* Size of the MCU (pixel) */

  jd->dcv[2] = jd->dcv[1] = jd->dcv[0] = 0; /* Initialize DC values */
  rst = rsc = 0;

  rc = JDR_OK;
  for (y = 0; y < jd->height; y += my) {    /* Vertical loop of MCUs */
    for (x = 0; x < jd->width; x += mx) { /* Horizontal loop of MCUs */
      if (jd->nrst && rst++ == jd->nrst) {  /* Process restart interval if enabled */
        rc = restart(jd, rsc++);
        if (rc != JDR_OK) return rc;
        rst = 1;
      }
      rc = mcu_load(jd);          /* Load an MCU (decompress huffman coded stream and IDCT) */
      if (rc != JDR_OK) return rc;
      rc = mcu_output(jd, x, y);      /* Output the MCU (color space conversion, scaling and output) */
      if (rc != JDR_OK) return rc;
    }
  }

  return rc;
}


externs.h
extern void *work;
extern void sync();
extern void jpeg_snapshot_picture();
extern DigitalOut led1;
extern DigitalOut led2;
extern DigitalOut led3;
extern DigitalOut led4;
extern UINT tjd_input (
  JDEC* jd,   // Decompressor object
  BYTE* buff, // Pointer to the read buffer (NULL:skip)
  UINT nd     // Number of bytes to read/skip from input stream  4
);

extern UINT output_func (
  JDEC* jd,     // Decompression object
  void* bitmap, // Bitmap data to be output
  JRECT* rect   // Rectangular region to output
);
extern unsigned char picture[60][80*3];

実際に撮影するメソッド類

cam.cpp
/**
  パソコンからgあるいはGで撮影開始。データをパソコン側に送る。
  p13: 緑色   p14:黄色
  Vdd=3.3V
  画像の大きさは80×60(1/4scale)

 */

/*
 * Include files.
 */

#include "mbed.h"
#include "CameraC1098.h"
#include "tjpgd.h"
#include "NokiaLCD.h"

#define FR_OK 0
#define SCALE 2 /* Output scaling 0:1/1, 1:1/2, 2:1/4 or 3:1/8 */

#define NBUF 1536
//#define NBUF 2500

unsigned char picture[60][80*3];
//unsigned char **picture;
//JRESULT rc;
//JDEC jd;    // Decompression object (70 bytes)
void *work;   // Pointer to the working buffer (must be 4-byte aligned)
extern CameraC1098 camera;
/*
 * Definitions.
 */
//#define LARGE
#define S115200

//Serial pc(USBTX, USBRX); // tx, rx
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
DigitalOut led5(p10);
DigitalOut led6(p11);
DigitalOut led7(p12);
static bool pict=false;
int num=0;

/*
 * Modules.
 */
/*
#ifdef S115200
CameraC1098 camera(p13, p14, CameraC1098::baud115200);
#else
CameraC1098 camera(p13, p14, CameraC1098::baud57600);
#endif
*/

/**
 * Synchronizing.
 */
void sync(void)
{
  led5=1;
  led6=1;
  led7=1;
  CameraC1098::ErrorNumber err = CameraC1098::NoError;
  if(!(err = camera.sync())) return;
  int count=0;
  while(err) {
    switch(count)
    {
      case 0: camera.setBaud(CameraC1098::baud14400); break;
      case 1: camera.setBaud(CameraC1098::baud115200); break;
      case 2: camera.setBaud(CameraC1098::baud57600); break;
      case 3: camera.setBaud(CameraC1098::baud28800); break;
      default: count=0;
    }
    count++;
    err = camera.sync();
    if(!err) {
#ifdef S115200
        camera.init(CameraC1098::Baud115200, CameraC1098::JpegResolution320x240);
        camera.setBaud(CameraC1098::baud115200);
        err = camera.sync();
        err = camera.init(CameraC1098::Baud115200, CameraC1098::JpegResolution320x240);
#else
        camera.init(CameraC1098::Baud57600, CameraC1098::JpegResolution320x240);
        camera.setBaud(CameraC1098::baud57600);
        err = camera.sync();
        err = camera.init(CameraC1098::Baud57600, CameraC1098::JpegResolution320x240);
#endif
    }
  }
}


uint16_t pkg;
int pkg_n;
int getData(int);
char buf[NBUF];
int wp, rp;
bool startFlag;
void jpeg_snapshot_picture(void) {
  CameraC1098::ErrorNumber err = CameraC1098::NoError;
  wp=rp=0;
  startFlag=false;
  err=camera.getJpegSnapshotPicture(pkg);
  for(pkg_n=0; pkg_n<3; pkg_n++) getData(pkg_n);
  free(camera.databuf);
}

CameraC1098::ErrorNumber en;

int getData(int i)
{
  if(i>pkg) return 0;
  if(startFlag) {
    camera.sendAck(0x00, i);// 一つ前のAck
    startFlag=false;
  }
  uint16_t checksum = 0;
        // ID.
  char idbuf[2];
  camera.waitRecv();
  if (!camera.recvBytes(idbuf, sizeof(idbuf))) {
//            return (ErrorNumber)UnexpectedReply;
  }
  checksum += idbuf[0];
  checksum += idbuf[1];
  uint16_t id = (idbuf[1] << 8) | (idbuf[0] << 0);
  if (id != i) {
   //         return (ErrorNumber)UnexpectedReply;
  }

        // Size of the data.
  char dsbuf[2];
  camera.waitRecv();
  if (!camera.recvBytes(dsbuf, sizeof(dsbuf))) {
      //   return (ErrorNumber)UnexpectedReply;
  }

        // Received the data.
  checksum += dsbuf[0];
  checksum += dsbuf[1];
  uint16_t ds = (dsbuf[1] << 8) | (dsbuf[0] << 0);
  camera.waitRecv();
  if (!camera.recvBytes(&camera.databuf[0], ds)) {
      //return (ErrorNumber)UnexpectedReply;
  }
  for (int j = 0; j < ds; j++) {
      checksum += camera.databuf[j];
  }
        // Verify code.
  char vcbuf[2];
  camera.waitRecv();
  if (!camera.recvBytes(vcbuf, sizeof(vcbuf))) {
          //  return (ErrorNumber)UnexpectedReply;
  }
  uint16_t vc = (vcbuf[1] << 8) | (vcbuf[0] << 0);
  if (vc != (checksum & 0xff)) {
      //  return (ErrorNumber)UnexpectedReply;
  }
         //func(databuf, ds);
  for(int i=0; i<ds; i++){
    buf[(i+wp)%NBUF] = camera.databuf[i];
  }
  wp += ds;
  startFlag=true;
  if (CameraC1098::NoError != en) {
            //return en;
  }
  return ds;
}


// User defined call-back function to input JPEG data
//static
UINT tjd_input (
  JDEC* jd,   // Decompressor object
  BYTE* buff, // Pointer to the read buffer (NULL:skip)
  UINT nd     // Number of bytes to read/skip from input stream  4
)
{
  UINT rb;
  int nrb=wp-rp-nd;
  while(nrb<=512){
    getData(pkg_n++);
    nrb=wp-rp-nd;
    if(pkg_n>pkg) break;
  }
  led5=0;
  int i;
  if (buff) { // Read nd bytes from the input stream
    for(i=0; i<nd; i++) buff[i]=buf[(rp+i)%NBUF];
    rp += nd;
    led5=1;
    return (UINT)nd;  // Returns number of bytes could be read
  } else {  // Skip nd bytes on the input stream
    if(nrb>0){
      rp += nd;
      i = FR_OK;
    }
    led5=1;
    return (i == FR_OK) ? nd : 0;
  }
}

extern NokiaLCD lcd;

UINT output_func (
  JDEC* jd,     /* Decompression object */
  void* bitmap, /* Bitmap data to be output */
  JRECT* rect   /* Rectangular region to output */
)
{
  BYTE* bp=(BYTE*)bitmap;
  BYTE* pix;
  UINT colour;

  for(volatile int i=rect->top ; i<=rect->bottom; i++)
  {
    pix = &picture[i][rect->left*3];
    for(volatile int j=rect->left; j<=rect->right; j++) {
      // 画像処理する場合
/*
      *pix++ = *bp++;
      *pix++ = *bp++;
      *pix++ = *bp++;
*/
// lcdに表示するだけ
      colour = *bp++<<16;   // red
      colour |= *bp++<<8;   // green
      colour |= *bp++;      // blue
      lcd.pixel(j+27,i+64,colour);
    }
  }

}


CameraC1098.h
/**
 * C1098 device driver class (Version 0.0.0)
 * 多分いじり壊していると思う
 */

#include "mbed.h"
#include "SerialBuffered.h"

#ifndef _CAMERA_C1098_H_
#define _CAMERA_C1098_H_

/*
 * Class: CameraC1098
 */
class CameraC1098 {
public:

    /**
     * Color type.
     */
    enum ColorType {
        GrayScale2bit = 0x01,   // 2bit for Y only
        GrayScale4bit = 0x02,   // 4bit for Y only
        GrayScale8bit = 0x03,   // 8bit for Y only
        Color12bit = 0x05,      // 444 (RGB)
        Color16bit = 0x06,      // 565 (RGB)
        Jpeg = 0x07
    };

    /**
     * Raw resolution.
     */
/*
    enum RawResolution {
        RawResolution80x60 = 0x01,
        RawResolution160x120 = 0x03
    };
*/

    /**
     * JPEG resolution.
     */
    enum JpegResolution {
        JpegResolution320x240 = 0x05,
        JpegResolution640x480 = 0x07
    };

    /**
     * Error number.
     */
    enum ErrorNumber {
        NoError = 0x00,
        PictureTypeError = 0x01,
        PictureUpScale = 0x02,
        PictureScaleError = 0x03,
        UnexpectedReply = 0x04,
        SendPictureTimeout = 0x05,
        UnexpectedCommand = 0x06,
        SramJpegTypeError = 0x07,
        SramJpegSizeError = 0x08,
        PictureFormatError = 0x09,
        PictureSizeError = 0x0a,
        ParameterError = 0x0b,
        SendRegisterTimeout = 0x0c,
        CommandIdError = 0x0d,
        PictureNotReady = 0x0f,
        TransferPackageNumberError = 0x10,
        SetTransferPackageSizeWrong = 0x11,
        CommandHeaderError = 0xf0,
        CommandLengthError = 0xf1,
        SendPictureError = 0xf5,
        SendCommandError = 0xff
    };

    /**
     * Picture type.
     */
    enum PictureType {
        SnapshotPicture = 0x01,
        PreviewPicture = 0x02,
        JpegPreviewPicture = 0x05
    };

    /**
     * Snapshot type.
     */
    enum SnapshotType {
        CompressedPicture = 0x00,
        UncompressedPicture = 0x01
    };

    /**
     * Baud rate.
     */
    enum InterfaceSpeed {
        Baud14400 = 7,  // Default.
        Baud28800 = 6,
        Baud57600 = 5,
        Baud115200 = 4,
        Baud230400 = 3,
        Baud460800 = 2
    };
    /**
     * Baud rate.
     */
    enum Baud {
        baud7200 = 7200,
        baud9600 = 9600,
        baud14400 = 14400,
        baud19200 = 19200,  // Default.
        baud28800 = 28800,
        baud38400 = 38400,
        baud57600 = 57600,
        baud115200 = 115200
    };

    /**
     * Reset type.
     */
    enum ResetType {
        ResetWholeSystem = 0x00,
        ResetStateMachines = 0x01
    };

    /**
     * Data type.
     */
    enum DataType {
        DataTypeSnapshotPicture = 0x01,
        DataTypePreviewPicture = 0x02,
        DataTypeJpegPreviewPicture = 0x05
    };

    /**
     * Constructor.
     *
     * @param tx A pin for transmit.
     * @param rx A pin for receive.
     * @param baud Baud rate. (Default is Baud14400.)
     */
    CameraC1098(PinName tx, PinName rx, Baud baud = baud14400);
    /**
     * Destructor.
     */
    ~CameraC1098();

    /**
     * Make a sync. for baud rate.
     */
    ErrorNumber sync();

    /**
     * Initialize.
     *
     * @param ct Color type.
     * @param rr Raw resolution.
     * @param jr JPEG resolution.
     */
    ErrorNumber init(InterfaceSpeed baud, JpegResolution jr);

    /**
     * Get uncompressed snapshot picture.
     *
     * @param func A pointer to a callback function.
     *             Please do NOT block this callback function.
     *             Because the camera module transmit image datas continuously.
     * @return Status of the error.
     */
    ErrorNumber getUncompressedSnapshotPicture(void(*func)(size_t done, size_t total, char c));

    /**
     * Get uncompressed preview picture.
     *
     * @param func A pointer to a callback function.
     *             Please do NOT block this callback function.
     *             Because the camera module transmit image datas continuously.
     * @return Status of the error.
     */
    ErrorNumber getUncompressedPreviewPicture(void(*func)(size_t done, size_t total, char c));

    /**
     * Get JPEG snapshot picture.
     *
     * @param func A pointer to a callback function.
     *             You can block this function until saving the image datas.
     * @return Status of the error.
     */
    ErrorNumber getJpegSnapshotPicture(void(*func)(char *buf, size_t siz));
    ErrorNumber getJpegSnapshotPicture(uint16_t& pkg_total);

    /**
     * Get JPEG preview picture.
     *
     * @param func A pointer to a callback function.
     *             You can block this function until saving the image datas.
     * @return Status of the error.
     */
    ErrorNumber getJpegPreviewPicture(void(*func)(char *buf, size_t siz));

/* ***********************************/
    ErrorNumber sendInitial(InterfaceSpeed baud, JpegResolution jr);
    SerialBuffered serial;
    void setBaud(Baud baud);
    ErrorNumber sendReset(bool specialReset);
    char *databuf;

    bool sendBytes(char *buf, size_t len, int timeout_us = 20000);
    bool recvBytes(char *buf, size_t len, int timeout_us = 20000);
    bool waitRecv();
    bool waitIdle();
    ErrorNumber sendAck(uint8_t commandId, uint16_t packageId);

private:
    static const int COMMAND_LENGTH = 6;
    static const int SYNCMAX = 60;
    static const int packageSize = 512;

    ErrorNumber sendGetPicture();
    ErrorNumber sendSnapshot();
    ErrorNumber sendSetPackageSize(uint16_t packageSize);
    ErrorNumber sendPowerOff();
    ErrorNumber recvData(DataType *dt, uint32_t *length);
    char recv[COMMAND_LENGTH];
    ErrorNumber sendSync();
    ErrorNumber recvSync();
    ErrorNumber recvAckOrNck();

};

#endif


CameraC1098.cpp
/**
 * C1098 device driver class　多分いじり壊していると思う
 */

#include "CameraC1098.h"

#define WAITIDLE    waitIdle
#define SENDFUNC    sendBytes
#define RECVFUNC    recvBytes
#define WAITFUNC    waitRecv

/**
 * Constructor.
 *
 * @param tx A pin for transmit.
 * @param rx A pin for receive.
 * @param baud Baud rate. (Default is Baud14400.)
 */
CameraC1098::CameraC1098(PinName tx, PinName rx, Baud baud) : serial(tx, rx)
{
    serial.baud((int)baud);
}

/**
 * Destructor.
 */
CameraC1098::~CameraC1098() {
}

void CameraC1098::setBaud(Baud baud)
{
  serial.baud((int)baud);
}
/**
 * Make a sync. for baud rate.
 */
CameraC1098::ErrorNumber CameraC1098::sync() {
    WAITIDLE();

    for (int i = 0; i < SYNCMAX; i++) {
        if (NoError == sendSync()) {
            if (NoError == recvAckOrNck()) {
                if (NoError == recvSync()) {
                    if (NoError == sendAck(0x0D, 0x00)) {
                        /*
                         * After synchronization, the camera needs a little time for AEC and AGC to be stable.
                         * Users should wait for 1-2 seconds before capturing the first picture.
                         */
                        wait_ms(100);
                        return NoError;
                    }
                }
            }
        }
        wait_ms(50);
    }
    return UnexpectedReply;
}

/**
 * Initialize.
 *
 * @param ct Color type.
 * @param rr Raw resolution.
 * @param jr JPEG resolution.
 */
CameraC1098::ErrorNumber CameraC1098::init(InterfaceSpeed baud, JpegResolution jr) {
    WAITIDLE();
    ErrorNumber en;

    en = sendInitial(baud, jr);
    if (NoError != en) {
        return en;
    }
    WAITFUNC();
    en = recvAckOrNck();
    if (NoError != en) {
        return en;
    }

    static bool alreadySetupPackageSize = false;
    if (!alreadySetupPackageSize) {
        en = sendSetPackageSize(packageSize);
        if (NoError != en) {
            return en;
        }
        WAITFUNC();
        en = recvAckOrNck();
        if (NoError != en) {
            return en;
        }
        alreadySetupPackageSize = true;
    }

    return (ErrorNumber)NoError;
}

extern DigitalOut led2; //(LED2);
extern DigitalOut led3; //(LED3);
extern DigitalOut led4; //(LED4);
extern DigitalOut led5; //(LED4);
extern DigitalOut led6; //(LED4);
extern DigitalOut led7; //(LED4);
extern Serial pc;

/**
 * Get JPEG snapshot picture.
 *
 * @param func A pointer to a callback function.
 *             You can block this function until saving the image datas.
 * @return Status of the error.
 //////////////////////////////////////////////////////////////////////////
 //////////////////////////////////////////////////////////////////////////
 */

CameraC1098::ErrorNumber CameraC1098::getJpegSnapshotPicture(uint16_t& pkg_total)
{
    WAITIDLE();
    ErrorNumber en;
led6=0;
    en = sendSnapshot();
    if (NoError != en) {
        return en;
    }
    WAITFUNC();
    en = recvAckOrNck();
    if (NoError != en) {
        return en;
    }

    en = sendGetPicture();
    if (NoError != en) {
        return en;
    }
    WAITFUNC();
    en = recvAckOrNck();
    if (NoError != en) {
        return en;
    }

    /*
     * Data : snapshot picture
     */
    DataType dt;
    uint32_t length = 0;
    WAITFUNC();
    en = recvData(&dt, &length);
    if (NoError != en) {
        return en;
    }
    en = sendAck(0x00, 0);
    if (NoError != en) {
        return en;
    }

    databuf = (char*) malloc(packageSize - 6);
    pkg_total = length / (packageSize - 6);
    led6=1;
    return (ErrorNumber)NoError;
}



CameraC1098::ErrorNumber CameraC1098::sendInitial(InterfaceSpeed baud, JpegResolution jr) {
    char send[COMMAND_LENGTH];

    send[0] = 0xAA;
    send[1] = 0x01;
    send[2] = (char)baud;
    send[3] = 7;
    send[4] = 0;
    send[5] = (char)jr;

    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }
    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::sendGetPicture() {
    char send[COMMAND_LENGTH];

    send[0] = 0xAA;
    send[1] = 0x04;
    send[2] = 0x01;     //(char)pt;
    send[3] = 0x00;
    send[4] = 0x00;
    send[5] = 0x00;

    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }
    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::sendSnapshot() {
    char send[COMMAND_LENGTH];
    send[0] = 0xAA;
    send[1] = 0x05;
    send[2] = 0x00;        //(char)st;
    send[3] = 0x00;        //(skipFrames >> 0) & 0xff;
    send[4] = 0x00;        //(skipFrames >> 8) & 0xff;
    send[5] = 0x00;

    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }
    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::sendSetPackageSize(uint16_t packageSize) {
    char send[COMMAND_LENGTH];
    send[0] = 0xAA;
    send[1] = 0x06;
    send[2] = 0x08;
    send[3] = (packageSize >> 0) & 0xff;
    send[4] = (packageSize >> 8) & 0xff;
    send[5] = 0x00;

    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }
    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::sendReset(bool specialReset) {
    char send[COMMAND_LENGTH];
    send[0] = 0xAA;
    send[1] = 0x08;
    send[2] = 0x00;
    send[3] = 0x00;
    send[4] = 0x00;
    send[5] = specialReset ? 0xff : 0x00;
    /*
     * Special reset : If the parameter is 0xFF, the command is a special Reset command and the firmware responds to it immediately.
     */

    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }

    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::sendPowerOff() {
    char send[COMMAND_LENGTH];
    send[0] = 0xAA;
    send[1] = 0x09;
    send[2] = 0x00;
    send[3] = 0x00;
    send[4] = 0x00;
    send[5] = 0x00;

    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }

    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::recvData(DataType *dt, uint32_t *length) {
    char recv[COMMAND_LENGTH];
    if (!RECVFUNC(recv, sizeof(recv))) {
        return (ErrorNumber)UnexpectedReply;
    }
    if ((0xAA != recv[0]) || (0x0A != recv[1])) {
        return (ErrorNumber)UnexpectedReply;
    }
    *dt = (DataType)recv[2];
    *length = (recv[5] << 16) | (recv[4] << 8) | (recv[3] << 0);
    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::sendSync() {
    char send[COMMAND_LENGTH];
    send[0] = 0xAA;
    send[1] = 0x0D;
    send[2] = 0x00;
    send[3] = 0x00;
    send[4] = 0x00;
    send[5] = 0x00;
    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }
    return (ErrorNumber)NoError;
}

CameraC1098::ErrorNumber CameraC1098::recvSync() {
    char recv[COMMAND_LENGTH];
    if (!RECVFUNC(recv, sizeof(recv))) {
        return (ErrorNumber)UnexpectedReply;
    }
    if ((0xAA != recv[0]) || (0x0D != recv[1])) {
        return (ErrorNumber)UnexpectedReply;
    }
    return (ErrorNumber)NoError;
}

/**
 * Send ACK.
 *
 * @param commandId The command with that ID is acknowledged by this command.
 * @param packageId For acknowledging Data command, these two bytes represent the requested package ID. While for acknowledging other commands, these two bytes are set to 00h.
 */
CameraC1098::ErrorNumber CameraC1098::sendAck(uint8_t commandId, uint16_t packageId) {
    char send[COMMAND_LENGTH];
    send[0] = 0xAA;
    send[1] = 0x0E;
    send[2] = commandId;
    send[3] = 0x00;    // ACK counter is not used.
    send[4] = (packageId >> 0) & 0xff;
    send[5] = (packageId >> 8) & 0xff;
    if (!SENDFUNC(send, sizeof(send))) {
        return (ErrorNumber)SendRegisterTimeout;
    }
    return (ErrorNumber)NoError;
}

/**
 * Receive ACK or NCK.
 *
 * @return Error number.
 */
CameraC1098::ErrorNumber CameraC1098::recvAckOrNck() {
    char recv[COMMAND_LENGTH];
    if (!RECVFUNC(recv, sizeof(recv))) {
        return (ErrorNumber)UnexpectedReply;
    }
    if ((0xAA == recv[0]) && (0x0E == recv[1])) {
        return (ErrorNumber)NoError;
    }
    if ((0xAA == recv[0]) && (0x0F == recv[1])) {
        return (ErrorNumber)recv[4];
    }
    return (ErrorNumber)UnexpectedReply;
}

/**
 * Send bytes to camera module.
 *
 * @param buf Pointer to the data buffer.
 * @param len Length of the data buffer.
 *
 * @return True if the data sended.
 */
bool CameraC1098::sendBytes(char *buf, size_t len, int timeout_us) {
    for (uint32_t i = 0; i < (uint32_t)len; i++) {
        int cnt = 0;
        while (!serial.writeable()) {
            wait_us(1);
            cnt++;
            if (timeout_us < cnt) {
                return false;
            }
        }
        serial.putc(buf[i]);
    }
    return true;
}

/**
 * Receive bytes from camera module.
 *
 * @param buf Pointer to the data buffer.
 * @param len Length of the data buffer.
 *
 * @return True if the data received.
 */
bool CameraC1098::recvBytes(char *buf, size_t len, int timeout_us) {
    for (uint32_t i = 0; i < (uint32_t)len; i++) {
        int cnt = 0;
        while (!serial.readable()) {
            wait_us(1);
            cnt++;
            if (timeout_us < cnt) {
                return false;
            }
        }
        buf[i] = serial.getc();
    }
    return true;
}

/**
 * Wait received.
 *
 * @return True if the data received.
 */
bool CameraC1098::waitRecv() {
    while (!serial.readable()) {
    }
    return true;
}

/**
 * Wait idle state.
 */
bool CameraC1098::waitIdle() {
    while (serial.readable()) {
        serial.getc();
    }
    return true;
}
どこかいじっているのでオリジナルと異なる場合があります。

NokiaLCD.h
/* mbed NokiaLCD Library, for a 130x130 Nokia colour LCD
 * Copyright (c) 2007-2010, sford
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.

 多分いじり壊していると思う
 */

#ifndef MBED_NOKIALCD_H
#define MBED_NOKIALCD_H

#include "mbed.h"

/** An interface for the 130x130 Nokia Mobile phone screens
 *
 * @code
 * #include "mbed.h"
 * #include "NokiaLCD.h"
 *
 * NokiaLCD lcd(p5, p7, p8, p9, NokiaLCD::6610); // mosi, sclk, cs, rst, type
 *
 * int main() {
 *     lcd.printf("Hello World!");
 * }
 * @endcode
 */
class NokiaLCD : public Stream {

public:
    /** LCD panel format */
    enum LCDType {
        LCD6100     /**  Nokia 6100, as found on sparkfun board (default) */
        , LCD6610   /**  Nokia 6610, as found on olimex board */
        , PCF8833
    };

    /** Create and Nokia LCD interface, using a SPI and two DigitalOut interfaces
     *
     * @param mosi SPI data out
     * @param sclk SPI clock
     * @param cs Chip Select (DigitalOut)
     * @param rst Reset (DigitalOut)
     * @param type The LCDType to select driver chip variants
     */
    NokiaLCD(PinName mosi, PinName sclk, PinName cs, PinName rst, LCDType type = PCF8833);

#if DOXYGEN_ONLY
    /** Write a character to the LCD
     *
     * @param c The character to write to the display
     */
    int putc(int c);

    /** Write a formated string to the LCD
     *
     * @param format A printf-style format string, followed by the
     *               variables to use in formating the string.
     */
    int printf(const char* format, ...);
#endif

    /** Locate to a screen column and row
     *
     * @param column  The horizontal position from the left, indexed from 0
     * @param row     The vertical position from the top, indexed from 0
     */
    void locate(int column, int row);

    /** Clear the screen and locate to 0,0 */
    void cls();

    /** Set a pixel on te screen
     *
     * @param x horizontal position from left
     * @param y vertical position from top
     * @param colour 24-bit colour in format 0x00RRGGBB
     */
    void pixel(int x, int y, int colour);

    /** Fill an area of the screen
     *
     * @param x horizontal position from left
     * @param y vertical position from top
     * @param width width in pixels
     * @param height height in pixels
     * @param colour 24-bit colour in format 0x00RRGGBB
     */
    void fill(int x, int y, int width, int height, int colour);

    void blit(int x, int y, int width, int height, const int* colour);
    void bitblit(int x, int y, int width, int height, const char* bitstream);

    int width();
    int height();
    int columns();
    int rows();

    void reset();

    /** Set the foreground colour
     *
     * @param c 24-bit colour
     */
    void foreground(int c);

    /** Set the background colour
     *
     * @param c 24-bit colour
     */
    void background(int c);

protected:
    virtual void _window(int x, int y, int width, int height);
    virtual void _putp(int colour);

    void command(int value);
    void data(int value);

    void newline();
    virtual int _putc(int c);
    virtual int _getc() {
        return 0;
    }
    void putp(int v);
    void window(int x, int y, int width, int height);

    SPI _spi;
    DigitalOut _rst;
    DigitalOut _cs;

    LCDType _type;
    int _row, _column, _rows, _columns, _foreground, _background, _width, _height;
};

#endif


取りあえず動いただけです。多分LCDは青の印があるものしか正常に動作しないと思われます。

NokiaLCD.cpp
/* mbed Nokia LCD Library
 * Copyright (c) 2007-2010, sford
 */

#include "NokiaLCD.h"

#include "mbed.h"

#define NOKIALCD_ROWS 16
#define NOKIALCD_COLS 16
#define NOKIALCD_WIDTH 131
#define NOKIALCD_HEIGHT 131
#define NOKIALCD_FREQUENCY 5000000

NokiaLCD::NokiaLCD(PinName mosi, PinName sclk, PinName cs, PinName rst, LCDType type)
        : _spi(mosi, NC, sclk)
        , _rst(rst)
        , _cs(cs) {

    _type = type;

    _row = 0;
    _column = 0;
    _foreground = 0x00FFFFFF;
    _background = 0x00000000;

    reset();
}


void NokiaLCD::reset() {

    // setup the SPI interface and bring display out of reset
    _cs = 1;
    _rst = 0;
    _spi.format(9);
    _spi.frequency(NOKIALCD_FREQUENCY);
    wait_ms(1);
    _rst = 1;
    wait_ms(1);

    _cs = 0;

    command(0x11);  // sleep out
    command(0x3A);  // column mode
    data(0x05);     // 16 bits par pixel
    command(0x36);  // madctl
    data(0x60);     // vertical RAM, flip x
    command(0x25);  // setcon
    data(0x48);// contrast 0x30
    wait_ms(2);
    command(0x29);//DISPON
    command(0x03);//BSTRON

    _cs = 1;

    cls();
}

void NokiaLCD::command(int value) {
    _spi.write(value & 0xFF);
}

void NokiaLCD::data(int value) {
    _spi.write(value | 0x100);
}

void NokiaLCD::_window(int x, int y, int width, int height) {
    int x1 = x;
    int y1 = y + 0;
    int x2 = x1 + width-1;
    int y2 = y1 + height-1;
    command(0x2A);  // column
    data(x1);
    data(x2);
    command(0x2B); // row
    data(y1);
    data(y2);
    command(0x2C); // start write to ram
}

void NokiaLCD::_putp(int colour) {
    int rg = ((colour >> 16) & 0xF8)
             | ((colour >> 13 ) & 0x07);
    int gb = ((colour >> 5 ) & 0xE0)
             | ((colour >> 3 ) & 0x1f);
    data(rg);
    data(gb);
}

const unsigned char FONT8x8[97][8] = {
    0x08,0x08,0x08,0x00,0x00,0x00,0x00,0x00, // columns, rows, num_bytes_per_char
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // space 0x20
    0x30,0x78,0x78,0x30,0x30,0x00,0x30,0x00, // !
    0x6C,0x6C,0x6C,0x00,0x00,0x00,0x00,0x00, // "
    0x6C,0x6C,0xFE,0x6C,0xFE,0x6C,0x6C,0x00, // #
    0x18,0x3E,0x60,0x3C,0x06,0x7C,0x18,0x00, // $
    0x00,0x63,0x66,0x0C,0x18,0x33,0x63,0x00, // %
    0x1C,0x36,0x1C,0x3B,0x6E,0x66,0x3B,0x00, // &
    0x30,0x30,0x60,0x00,0x00,0x00,0x00,0x00, // '
    0x0C,0x18,0x30,0x30,0x30,0x18,0x0C,0x00, // (
    0x30,0x18,0x0C,0x0C,0x0C,0x18,0x30,0x00, // )
    0x00,0x66,0x3C,0xFF,0x3C,0x66,0x00,0x00, // *
    0x00,0x30,0x30,0xFC,0x30,0x30,0x00,0x00, // +
    0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x30, // ,
    0x00,0x00,0x00,0x7E,0x00,0x00,0x00,0x00, // -
    0x00,0x00,0x00,0x00,0x00,0x18,0x18,0x00, // .
    0x03,0x06,0x0C,0x18,0x30,0x60,0x40,0x00, // / (forward slash)
    0x3E,0x63,0x63,0x6B,0x63,0x63,0x3E,0x00, // 0 0x30
    0x18,0x38,0x58,0x18,0x18,0x18,0x7E,0x00, // 1
    0x3C,0x66,0x06,0x1C,0x30,0x66,0x7E,0x00, // 2
    0x3C,0x66,0x06,0x1C,0x06,0x66,0x3C,0x00, // 3
    0x0E,0x1E,0x36,0x66,0x7F,0x06,0x0F,0x00, // 4
    0x7E,0x60,0x7C,0x06,0x06,0x66,0x3C,0x00, // 5
    0x1C,0x30,0x60,0x7C,0x66,0x66,0x3C,0x00, // 6
    0x7E,0x66,0x06,0x0C,0x18,0x18,0x18,0x00, // 7
    0x3C,0x66,0x66,0x3C,0x66,0x66,0x3C,0x00, // 8
    0x3C,0x66,0x66,0x3E,0x06,0x0C,0x38,0x00, // 9
    0x00,0x18,0x18,0x00,0x00,0x18,0x18,0x00, // :
    0x00,0x18,0x18,0x00,0x00,0x18,0x18,0x30, // ;
    0x0C,0x18,0x30,0x60,0x30,0x18,0x0C,0x00, // <
    0x00,0x00,0x7E,0x00,0x00,0x7E,0x00,0x00, // =
    0x30,0x18,0x0C,0x06,0x0C,0x18,0x30,0x00, // >
    0x3C,0x66,0x06,0x0C,0x18,0x00,0x18,0x00, // ?
    0x3E,0x63,0x6F,0x69,0x6F,0x60,0x3E,0x00, // @ 0x40
    0x18,0x3C,0x66,0x66,0x7E,0x66,0x66,0x00, // A
    0x7E,0x33,0x33,0x3E,0x33,0x33,0x7E,0x00, // B
    0x1E,0x33,0x60,0x60,0x60,0x33,0x1E,0x00, // C
    0x7C,0x36,0x33,0x33,0x33,0x36,0x7C,0x00, // D
    0x7F,0x31,0x34,0x3C,0x34,0x31,0x7F,0x00, // E
    0x7F,0x31,0x34,0x3C,0x34,0x30,0x78,0x00, // F
    0x1E,0x33,0x60,0x60,0x67,0x33,0x1F,0x00, // G
    0x66,0x66,0x66,0x7E,0x66,0x66,0x66,0x00, // H
    0x3C,0x18,0x18,0x18,0x18,0x18,0x3C,0x00, // I
    0x0F,0x06,0x06,0x06,0x66,0x66,0x3C,0x00, // J
    0x73,0x33,0x36,0x3C,0x36,0x33,0x73,0x00, // K
    0x78,0x30,0x30,0x30,0x31,0x33,0x7F,0x00, // L
    0x63,0x77,0x7F,0x7F,0x6B,0x63,0x63,0x00, // M
    0x63,0x73,0x7B,0x6F,0x67,0x63,0x63,0x00, // N
    0x3E,0x63,0x63,0x63,0x63,0x63,0x3E,0x00, // O
    0x7E,0x33,0x33,0x3E,0x30,0x30,0x78,0x00, // P 0x50
    0x3C,0x66,0x66,0x66,0x6E,0x3C,0x0E,0x00, // Q
    0x7E,0x33,0x33,0x3E,0x36,0x33,0x73,0x00, // R
    0x3C,0x66,0x30,0x18,0x0C,0x66,0x3C,0x00, // S
    0x7E,0x5A,0x18,0x18,0x18,0x18,0x3C,0x00, // T
    0x66,0x66,0x66,0x66,0x66,0x66,0x7E,0x00, // U
    0x66,0x66,0x66,0x66,0x66,0x3C,0x18,0x00, // V
    0x63,0x63,0x63,0x6B,0x7F,0x77,0x63,0x00, // W
    0x63,0x63,0x36,0x1C,0x1C,0x36,0x63,0x00, // X
    0x66,0x66,0x66,0x3C,0x18,0x18,0x3C,0x00, // Y
    0x7F,0x63,0x46,0x0C,0x19,0x33,0x7F,0x00, // Z
    0x3C,0x30,0x30,0x30,0x30,0x30,0x3C,0x00, // [
    0x60,0x30,0x18,0x0C,0x06,0x03,0x01,0x00, // \ (back slash)
    0x3C,0x0C,0x0C,0x0C,0x0C,0x0C,0x3C,0x00, // ]
    0x08,0x1C,0x36,0x63,0x00,0x00,0x00,0x00, // ^
    0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF, // _
    0x18,0x18,0x0C,0x00,0x00,0x00,0x00,0x00, // ` 0x60
    0x00,0x00,0x3C,0x06,0x3E,0x66,0x3B,0x00, // a
    0x70,0x30,0x3E,0x33,0x33,0x33,0x6E,0x00, // b
    0x00,0x00,0x3C,0x66,0x60,0x66,0x3C,0x00, // c
    0x0E,0x06,0x3E,0x66,0x66,0x66,0x3B,0x00, // d
    0x00,0x00,0x3C,0x66,0x7E,0x60,0x3C,0x00, // e
    0x1C,0x36,0x30,0x78,0x30,0x30,0x78,0x00, // f
    0x00,0x00,0x3B,0x66,0x66,0x3E,0x06,0x7C, // g
    0x70,0x30,0x36,0x3B,0x33,0x33,0x73,0x00, // h
    0x18,0x00,0x38,0x18,0x18,0x18,0x3C,0x00, // i
    0x06,0x00,0x06,0x06,0x06,0x66,0x66,0x3C, // j
    0x70,0x30,0x33,0x36,0x3C,0x36,0x73,0x00, // k
    0x38,0x18,0x18,0x18,0x18,0x18,0x3C,0x00, // l
    0x00,0x00,0x66,0x7F,0x7F,0x6B,0x63,0x00, // m
    0x00,0x00,0x7C,0x66,0x66,0x66,0x66,0x00, // n
    0x00,0x00,0x3C,0x66,0x66,0x66,0x3C,0x00, // o
    0x00,0x00,0x6E,0x33,0x33,0x3E,0x30,0x78, // p
    0x00,0x00,0x3B,0x66,0x66,0x3E,0x06,0x0F, // q
    0x00,0x00,0x6E,0x3B,0x33,0x30,0x78,0x00, // r
    0x00,0x00,0x3E,0x60,0x3C,0x06,0x7C,0x00, // s
    0x08,0x18,0x3E,0x18,0x18,0x1A,0x0C,0x00, // t
    0x00,0x00,0x66,0x66,0x66,0x66,0x3B,0x00, // u
    0x00,0x00,0x66,0x66,0x66,0x3C,0x18,0x00, // v
    0x00,0x00,0x63,0x6B,0x7F,0x7F,0x36,0x00, // w
    0x00,0x00,0x63,0x36,0x1C,0x36,0x63,0x00, // x
    0x00,0x00,0x66,0x66,0x66,0x3E,0x06,0x7C, // y
    0x00,0x00,0x7E,0x4C,0x18,0x32,0x7E,0x00, // z
    0x0E,0x18,0x18,0x70,0x18,0x18,0x0E,0x00, // {
    0x0C,0x0C,0x0C,0x00,0x0C,0x0C,0x0C,0x00, // |
    0x70,0x18,0x18,0x0E,0x18,0x18,0x70,0x00, // }
    0x3B,0x6E,0x00,0x00,0x00,0x00,0x00,0x00, // ~
    0x1C,0x36,0x36,0x1C,0x00,0x00,0x00,0x00
}; // DEL

void NokiaLCD::locate(int column, int row) {
    _column = column;
    _row = row;
}

void NokiaLCD::newline() {
    _column = 0;
    _row++;
    if (_row >= _rows) {
        _row = 0;
    }
}

// 青用
int NokiaLCD::_putc(int value) {
    int x = _column * 8;  // FIXME: Char sizes
    int y = _row * 8-94;
    bitblit(y , x, 8, 8, (char*)&(FONT8x8[value - 0x1F][0]));

    _column++;

    if (_column >= NOKIALCD_COLS) {
        _row++;
        _column = 0;
    }

    if (_row >= NOKIALCD_ROWS) {
        _row = 0;
    }

    return value;
}

void NokiaLCD::cls() {
    fill(0, 0, NOKIALCD_WIDTH, NOKIALCD_HEIGHT, _background);
    _row = 0;
    _column = 0;
}


void NokiaLCD::window(int x, int y, int width, int height) {
    _cs = 0;
    _window(x, y, width, height);
    _cs = 1;
}

void NokiaLCD::putp(int colour) {
    _cs = 0;
    _putp(colour);
    _cs = 1;
}

void NokiaLCD::pixel(int x, int y, int colour) {
    _cs = 0;
//    _window(x, y, 1, 1);
    _window(y, x, 1, 1);
    _putp(colour);
    _cs = 1;
}

void NokiaLCD::fill(int x, int y, int width, int height, int colour) {
    _cs = 0;
    _window(y+1, x+1, height, width);
    for (int i=0; i<width*height; i++) {
        _putp(colour);
    }

    _window(0, 0, NOKIALCD_WIDTH, NOKIALCD_HEIGHT);
    _cs = 1;
}

void NokiaLCD::blit(int x, int y, int width, int height, const int* colour) {
    _cs = 0;

    _window(x, y, width, height);
    for (int i=0; i<width*height; i++) {
         _putp(colour[i]);
    }
    _window(0, 0, NOKIALCD_WIDTH, NOKIALCD_HEIGHT);
    _cs = 1;
}

void NokiaLCD::bitblit(int x, int y, int width, int height, const char* bitstream) {
    _cs = 0;
    _window(x, y, width, height);
    for (int i=0; i<height*width; i++) {
        int bit = i / 8;
        int byte = 7-(i % 8);
        int colour = ((bitstream[byte] << bit) & 0x80) ? _foreground : _background;
        _putp(colour);
    }
    _window(0, 0, _width, _height);
    _cs = 1;
}

void NokiaLCD::foreground(int c) {
    _foreground = c;
}

void NokiaLCD::background(int c) {
    _background = c;
}

int NokiaLCD::width() {
    return NOKIALCD_WIDTH;
}

int NokiaLCD::height() {
    return NOKIALCD_HEIGHT;
}

int NokiaLCD::columns() {
    return NOKIALCD_COLS;
}

int NokiaLCD::rows() {
    return NOKIALCD_ROWS;
}


SerialBuffered.h
#ifndef _SERIAL_BUFFERED_H_
#define _SERIAL_BUFFERED_H_

/**
 * Buffered serial class.
 */
class SerialBuffered : public Serial {
public:
    /**
     * Create a buffered serial class.
     *
     * @param tx A pin for transmit.
     * @param rx A pin for receive.
     */
    SerialBuffered(PinName tx, PinName rx);

    /**
     * Destroy.
     */
    virtual ~SerialBuffered();

    /**
     * Get a character.
     *
     * @return A character. (-1:timeout)
     */
    int getc();

    /**
     * Returns 1 if there is a character available to read, 0 otherwise.
     */
    int readable();

    /**
     * Set timeout for getc().
     *
     * @param ms milliseconds. (-1:Disable timeout)
     */
    void setTimeout(int ms);

    /**
     * Read requested bytes.
     *
     * @param bytes A pointer to a buffer.
     * @param requested Length.
     *
     * @return Readed byte length.
     */
    size_t readBytes(uint8_t *bytes, size_t requested);

private:
    void handleInterrupt();
    static const int BUFFERSIZE = 4096;
    uint8_t buffer[BUFFERSIZE];   // points at a circular buffer, 
                                  // containing data from m_contentStart, 
                                  // for m_contentSize bytes, wrapping when 
                                  // you get to the end
    uint16_t indexContentStart;   // index of first bytes of content
    uint16_t indexContentEnd;     // index of bytes after last byte of content
    int timeout;
    Timer timer;
};

#endif


SerialBuffered.cpp
#include "mbed.h"
#include "SerialBuffered.h"

/**
 * Create a buffered serial class.
 *
 * @param tx A pin for transmit.
 * @param rx A pin for receive.
 */
SerialBuffered::SerialBuffered(PinName tx, PinName rx) : Serial(tx, rx) {
    indexContentStart = 0;
    indexContentEnd = 0;
    timeout = 1;
    attach(this, &SerialBuffered::handleInterrupt);
}

/**
 * Destroy.
 */
SerialBuffered::~SerialBuffered() {
}

/**
 * Set timeout for getc().
 *
 * @param ms milliseconds. (-1:Disable timeout)
 */
void SerialBuffered::setTimeout(int ms) {
    timeout = ms;
}

/**
 * Read requested bytes.
 *
 * @param bytes A pointer to a buffer.
 * @param requested Length.
 *
 * @return Readed byte length.
 */
size_t SerialBuffered::readBytes(uint8_t *bytes, size_t requested) {
    int i = 0;
    while (i < requested) {
        int c = getc();
        if (c < 0) {
            break;
        }
        bytes[i] = c;
        i++;
    }
    return i;
}

/**
 * Get a character.
 *
 * @return A character. (-1:timeout)
 */
int SerialBuffered::getc() {
    timer.reset();
    timer.start();
    while (indexContentStart == indexContentEnd) {
        wait_ms(1);
        if ((timeout > 0) && (timer.read_ms() > timeout)) {
            /*
             * Timeout occured.
             */
            // printf("Timeout occured.\n");
            return EOF;
        }
    }
    timer.stop();

    uint8_t result = buffer[indexContentStart++];
    indexContentStart =  indexContentStart % BUFFERSIZE;

    return result;
}

/**
 * Returns 1 if there is a character available to read, 0 otherwise.
 */
int SerialBuffered::readable() {
    return indexContentStart != indexContentEnd;
}

void SerialBuffered::handleInterrupt() {
    while (Serial::readable()) {
        if (indexContentStart == ((indexContentEnd + 1) % BUFFERSIZE)) {
            /*
             * Buffer overrun occured.
             */
            // printf("Buffer overrun occured.\n");
            Serial::getc();
        } else {
            buffer[indexContentEnd++] = Serial::getc();
            indexContentEnd = indexContentEnd % BUFFERSIZE;
        }
    }
}