Junichi Katsu / Mbed 2 deprecated JBB_BTLE_Test

JBBoardに接続したモーター２つをRCBControllerでコントロールするテストです。

Dependencies:   FatFileSystem TB6612FNG2 mbed

Fork of JBB_BTLE_Test by Jksoft Blue mbed Board Developer

uvc/myjpeg.cpp@0:1ed23ab1345f, 2012-06-26 (annotated)

Committer:
va009039
Date:
Tue Jun 26 14:27:45 2012 +0000
Revision:
0:1ed23ab1345f
fix overflow spp_service_buffer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
va009039 0:1ed23ab1345f 1 #include "mbed.h"
va009039 0:1ed23ab1345f 2 //#define __DEBUG
va009039 0:1ed23ab1345f 3 #include "mydbg.h"
va009039 0:1ed23ab1345f 4 #include "myjpeg.h"
va009039 0:1ed23ab1345f 5
va009039 0:1ed23ab1345f 6 static const uint8_t dht[] = {
va009039 0:1ed23ab1345f 7 0xFF,0xC4,0x01,0xA2,0x00,0x00,0x01,0x05,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,
va009039 0:1ed23ab1345f 8 0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,
va009039 0:1ed23ab1345f 9 0x0B,0x01,0x00,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,
va009039 0:1ed23ab1345f 10 0x00,0x00,0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,0x10,0x00,
va009039 0:1ed23ab1345f 11 0x02,0x01,0x03,0x03,0x02,0x04,0x03,0x05,0x05,0x04,0x04,0x00,0x00,0x01,0x7D,0x01,
va009039 0:1ed23ab1345f 12 0x02,0x03,0x00,0x04,0x11,0x05,0x12,0x21,0x31,0x41,0x06,0x13,0x51,0x61,0x07,0x22,
va009039 0:1ed23ab1345f 13 0x71,0x14,0x32,0x81,0x91,0xA1,0x08,0x23,0x42,0xB1,0xC1,0x15,0x52,0xD1,0xF0,0x24,
va009039 0:1ed23ab1345f 14 0x33,0x62,0x72,0x82,0x09,0x0A,0x16,0x17,0x18,0x19,0x1A,0x25,0x26,0x27,0x28,0x29,
va009039 0:1ed23ab1345f 15 0x2A,0x34,0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,
va009039 0:1ed23ab1345f 16 0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,
va009039 0:1ed23ab1345f 17 0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,
va009039 0:1ed23ab1345f 18 0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,
va009039 0:1ed23ab1345f 19 0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,
va009039 0:1ed23ab1345f 20 0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,0xD9,0xDA,0xE1,0xE2,0xE3,
va009039 0:1ed23ab1345f 21 0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF1,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,0xF8,0xF9,
va009039 0:1ed23ab1345f 22 0xFA,0x11,0x00,0x02,0x01,0x02,0x04,0x04,0x03,0x04,0x07,0x05,0x04,0x04,0x00,0x01,
va009039 0:1ed23ab1345f 23 0x02,0x77,0x00,0x01,0x02,0x03,0x11,0x04,0x05,0x21,0x31,0x06,0x12,0x41,0x51,0x07,
va009039 0:1ed23ab1345f 24 0x61,0x71,0x13,0x22,0x32,0x81,0x08,0x14,0x42,0x91,0xA1,0xB1,0xC1,0x09,0x23,0x33,
va009039 0:1ed23ab1345f 25 0x52,0xF0,0x15,0x62,0x72,0xD1,0x0A,0x16,0x24,0x34,0xE1,0x25,0xF1,0x17,0x18,0x19,
va009039 0:1ed23ab1345f 26 0x1A,0x26,0x27,0x28,0x29,0x2A,0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,
va009039 0:1ed23ab1345f 27 0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56,0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,
va009039 0:1ed23ab1345f 28 0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78,0x79,0x7A,0x82,0x83,0x84,0x85,
va009039 0:1ed23ab1345f 29 0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,0x9A,0xA2,0xA3,
va009039 0:1ed23ab1345f 30 0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,0xBA,
va009039 0:1ed23ab1345f 31 0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,
va009039 0:1ed23ab1345f 32 0xD9,0xDA,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF2,0xF3,0xF4,0xF5,0xF6,
va009039 0:1ed23ab1345f 33 0xF7,0xF8,0xF9,0xFA,
va009039 0:1ed23ab1345f 34 };
va009039 0:1ed23ab1345f 35
va009039 0:1ed23ab1345f 36 myjpeg::myjpeg(uint8_t* buf, int len, int capacity)
va009039 0:1ed23ab1345f 37 {
va009039 0:1ed23ab1345f 38 DBG_ASSERT(buf);
va009039 0:1ed23ab1345f 39 DBG_ASSERT(len >= 0);
va009039 0:1ed23ab1345f 40 m_buf = buf;
va009039 0:1ed23ab1345f 41 m_len = len;
va009039 0:1ed23ab1345f 42 m_capacity = capacity;
va009039 0:1ed23ab1345f 43 }
va009039 0:1ed23ab1345f 44
va009039 0:1ed23ab1345f 45 int myjpeg::getc()
va009039 0:1ed23ab1345f 46 {
va009039 0:1ed23ab1345f 47 if (m_pos >= m_len) {
va009039 0:1ed23ab1345f 48 return -1;
va009039 0:1ed23ab1345f 49 }
va009039 0:1ed23ab1345f 50 return m_buf[m_pos++];
va009039 0:1ed23ab1345f 51 }
va009039 0:1ed23ab1345f 52
va009039 0:1ed23ab1345f 53 int myjpeg::getBE16()
va009039 0:1ed23ab1345f 54 {
va009039 0:1ed23ab1345f 55 int c1 = getc();
va009039 0:1ed23ab1345f 56 if (c1 == (-1)) {
va009039 0:1ed23ab1345f 57 return -1;
va009039 0:1ed23ab1345f 58 }
va009039 0:1ed23ab1345f 59 int c2 = getc();
va009039 0:1ed23ab1345f 60 if (c2 == (-1)) {
va009039 0:1ed23ab1345f 61 return -1;
va009039 0:1ed23ab1345f 62 }
va009039 0:1ed23ab1345f 63 return c1<<8|c2;
va009039 0:1ed23ab1345f 64 }
va009039 0:1ed23ab1345f 65
va009039 0:1ed23ab1345f 66 void myjpeg::analytics()
va009039 0:1ed23ab1345f 67 {
va009039 0:1ed23ab1345f 68 m_pos = 0;
va009039 0:1ed23ab1345f 69 SOS_pos = 0;
va009039 0:1ed23ab1345f 70 DHT_pos = 0;
va009039 0:1ed23ab1345f 71 int skip;
va009039 0:1ed23ab1345f 72 int lp;
va009039 0:1ed23ab1345f 73 while(1) {
va009039 0:1ed23ab1345f 74 int marker_pos = m_pos;
va009039 0:1ed23ab1345f 75 int c = getc();
va009039 0:1ed23ab1345f 76 if (c == (-1)) {
va009039 0:1ed23ab1345f 77 break;
va009039 0:1ed23ab1345f 78 }
va009039 0:1ed23ab1345f 79 if (c != 0xff) {
va009039 0:1ed23ab1345f 80 continue;
va009039 0:1ed23ab1345f 81 }
va009039 0:1ed23ab1345f 82 c = getc();
va009039 0:1ed23ab1345f 83 if (c == (-1)) {
va009039 0:1ed23ab1345f 84 break;
va009039 0:1ed23ab1345f 85 }
va009039 0:1ed23ab1345f 86 uint8_t marker = c;
va009039 0:1ed23ab1345f 87 switch(marker) {
va009039 0:1ed23ab1345f 88 case 0xd8:
va009039 0:1ed23ab1345f 89 DBG("%04X SOI\n", marker_pos);
va009039 0:1ed23ab1345f 90 skip = 0;
va009039 0:1ed23ab1345f 91 break;
va009039 0:1ed23ab1345f 92 case 0xd9:
va009039 0:1ed23ab1345f 93 DBG("%04X EOI\n", marker_pos);
va009039 0:1ed23ab1345f 94 skip = 0;
va009039 0:1ed23ab1345f 95 break;
va009039 0:1ed23ab1345f 96 case 0x00:
va009039 0:1ed23ab1345f 97 skip = 0;
va009039 0:1ed23ab1345f 98 break;
va009039 0:1ed23ab1345f 99 case 0xc0:
va009039 0:1ed23ab1345f 100 lp = getBE16();
va009039 0:1ed23ab1345f 101 DBG("%04X SOF0 %d\n", marker_pos, lp);
va009039 0:1ed23ab1345f 102 skip = lp - 2;
va009039 0:1ed23ab1345f 103 break;
va009039 0:1ed23ab1345f 104 case 0xc4:
va009039 0:1ed23ab1345f 105 lp = getBE16();
va009039 0:1ed23ab1345f 106 DBG("%04X DHT Lh: %d\n", marker_pos, lp);
va009039 0:1ed23ab1345f 107 //DBG("Tc: %d\n", buf[pos+2]>>4);
va009039 0:1ed23ab1345f 108 //DBG("Th: %d\n", buf[pos+2]&0xf);
va009039 0:1ed23ab1345f 109 DHT_pos = marker_pos;
va009039 0:1ed23ab1345f 110 skip = lp - 2;
va009039 0:1ed23ab1345f 111 break;
va009039 0:1ed23ab1345f 112 case 0xda:
va009039 0:1ed23ab1345f 113 lp = getBE16();
va009039 0:1ed23ab1345f 114 DBG("%04X SOS Ls: %d\n", marker_pos, lp);
va009039 0:1ed23ab1345f 115 //DBG("Ns: %d\n", buf[pos+2]);
va009039 0:1ed23ab1345f 116 //for(i = 1; i <= buf[pos+2]; i++) {
va009039 0:1ed23ab1345f 117 // DBG("Cs%d: %d\n", i, buf[pos+3+(i-1)*2]);
va009039 0:1ed23ab1345f 118 // DBG("Td%d: %d\n", i, buf[pos+4+(i-1)*2]>>4);
va009039 0:1ed23ab1345f 119 // DBG("Ta%d: %d\n", i, buf[pos+4+(i-1)*2]&0xf);
va009039 0:1ed23ab1345f 120 //}
va009039 0:1ed23ab1345f 121 SOS_pos = marker_pos;
va009039 0:1ed23ab1345f 122 skip = lp - 2;
va009039 0:1ed23ab1345f 123 break;
va009039 0:1ed23ab1345f 124 case 0xdb:
va009039 0:1ed23ab1345f 125 lp = getBE16();
va009039 0:1ed23ab1345f 126 DBG("%04X DQT %d\n", marker_pos, lp);
va009039 0:1ed23ab1345f 127 skip = lp - 2;
va009039 0:1ed23ab1345f 128 break;
va009039 0:1ed23ab1345f 129 case 0xe0:
va009039 0:1ed23ab1345f 130 lp = getBE16();
va009039 0:1ed23ab1345f 131 DBG("%04X APP0 %d\n", marker_pos, lp);
va009039 0:1ed23ab1345f 132 skip = lp - 2;
va009039 0:1ed23ab1345f 133 break;
va009039 0:1ed23ab1345f 134 default:
va009039 0:1ed23ab1345f 135 DBG("%04X ??? %02X\n", marker_pos, marker);
va009039 0:1ed23ab1345f 136 skip = 0;
va009039 0:1ed23ab1345f 137 break;
va009039 0:1ed23ab1345f 138 }
va009039 0:1ed23ab1345f 139 while(skip-- > 0) {
va009039 0:1ed23ab1345f 140 getc();
va009039 0:1ed23ab1345f 141 }
va009039 0:1ed23ab1345f 142 }
va009039 0:1ed23ab1345f 143 }
va009039 0:1ed23ab1345f 144
va009039 0:1ed23ab1345f 145 int myjpeg::insertDHT()
va009039 0:1ed23ab1345f 146 {
va009039 0:1ed23ab1345f 147 DBG("m_len=%d m_capacity=%d SOS=%d\n", m_len, m_capacity, SOS_pos);
va009039 0:1ed23ab1345f 148 DBG_ASSERT(SOS_pos > 0);
va009039 0:1ed23ab1345f 149 DBG_ASSERT(SOS_pos < m_len);
va009039 0:1ed23ab1345f 150 DBG_ASSERT(m_len <= m_capacity);
va009039 0:1ed23ab1345f 151 DBG_ASSERT(sizeof(dht) == 420);
va009039 0:1ed23ab1345f 152
va009039 0:1ed23ab1345f 153 int num1 = m_len - SOS_pos;
va009039 0:1ed23ab1345f 154 if (num1 > (m_capacity - SOS_pos - sizeof(dht))) {
va009039 0:1ed23ab1345f 155 num1 = m_capacity - SOS_pos - sizeof(dht);
va009039 0:1ed23ab1345f 156 }
va009039 0:1ed23ab1345f 157 DBG("num1=%d\n", num1);
va009039 0:1ed23ab1345f 158 DBG_ASSERT(SOS_pos+sizeof(dht)+num1 <= m_capacity);
va009039 0:1ed23ab1345f 159 memmove(m_buf+SOS_pos+sizeof(dht), m_buf+SOS_pos, num1);
va009039 0:1ed23ab1345f 160
va009039 0:1ed23ab1345f 161 int num2 = sizeof(dht);
va009039 0:1ed23ab1345f 162 if (num2 > m_capacity - SOS_pos) {
va009039 0:1ed23ab1345f 163 num2 = m_capacity - SOS_pos;
va009039 0:1ed23ab1345f 164 }
va009039 0:1ed23ab1345f 165 DBG("num2=%d\n", num2);
va009039 0:1ed23ab1345f 166 DBG_ASSERT(SOS_pos+num2 <= m_capacity);
va009039 0:1ed23ab1345f 167 memcpy(m_buf+SOS_pos, dht, num2);
va009039 0:1ed23ab1345f 168 m_len += sizeof(dht);
va009039 0:1ed23ab1345f 169 if (m_len > m_capacity) {
va009039 0:1ed23ab1345f 170 m_len = m_capacity;
va009039 0:1ed23ab1345f 171 }
va009039 0:1ed23ab1345f 172 return m_len;
va009039 0:1ed23ab1345f 173 }
va009039 0:1ed23ab1345f 174
va009039 0:1ed23ab1345f 175 int fgetBE16(FILE* fp)
va009039 0:1ed23ab1345f 176 {
va009039 0:1ed23ab1345f 177 int c1 = fgetc(fp);
va009039 0:1ed23ab1345f 178 if (c1 == EOF) {
va009039 0:1ed23ab1345f 179 return -1;
va009039 0:1ed23ab1345f 180 }
va009039 0:1ed23ab1345f 181 int c2 = fgetc(fp);
va009039 0:1ed23ab1345f 182 if (c2 == EOF) {
va009039 0:1ed23ab1345f 183 return -1;
va009039 0:1ed23ab1345f 184 }
va009039 0:1ed23ab1345f 185 return c1<<8|c2;
va009039 0:1ed23ab1345f 186 }
va009039 0:1ed23ab1345f 187
va009039 0:1ed23ab1345f 188 void QcamCopy(const char* destination, const char* source)
va009039 0:1ed23ab1345f 189 {
va009039 0:1ed23ab1345f 190 FILE* fp;
va009039 0:1ed23ab1345f 191 FILE* fp2;
va009039 0:1ed23ab1345f 192 fp = fopen(source, "rb");
va009039 0:1ed23ab1345f 193 if (fp == NULL) {
va009039 0:1ed23ab1345f 194 return;
va009039 0:1ed23ab1345f 195 }
va009039 0:1ed23ab1345f 196 fp2 = fopen(destination, "wb");
va009039 0:1ed23ab1345f 197 if (fp2 == NULL) {
va009039 0:1ed23ab1345f 198 return;
va009039 0:1ed23ab1345f 199 }
va009039 0:1ed23ab1345f 200 int i,c1,c2;
va009039 0:1ed23ab1345f 201 bool f_dht = false;
va009039 0:1ed23ab1345f 202 bool f_lp = false;
va009039 0:1ed23ab1345f 203 while(!feof(fp)){
va009039 0:1ed23ab1345f 204 c1 = fgetc(fp);
va009039 0:1ed23ab1345f 205 if (c1 != 0xff) {
va009039 0:1ed23ab1345f 206 fputc(c1, fp2);
va009039 0:1ed23ab1345f 207 continue;
va009039 0:1ed23ab1345f 208 }
va009039 0:1ed23ab1345f 209 c2 = fgetc(fp);
va009039 0:1ed23ab1345f 210 switch(c2) {
va009039 0:1ed23ab1345f 211 case 0xda: // SOS
va009039 0:1ed23ab1345f 212 if (!f_dht) {
va009039 0:1ed23ab1345f 213 for(i = 0; i < sizeof(dht); i++) {
va009039 0:1ed23ab1345f 214 fputc(dht[i], fp2);
va009039 0:1ed23ab1345f 215 }
va009039 0:1ed23ab1345f 216 f_dht = true;
va009039 0:1ed23ab1345f 217 }
va009039 0:1ed23ab1345f 218 f_lp = true;
va009039 0:1ed23ab1345f 219 break;
va009039 0:1ed23ab1345f 220 case 0xc4: // DHT
va009039 0:1ed23ab1345f 221 f_dht = true;
va009039 0:1ed23ab1345f 222 f_lp = true;
va009039 0:1ed23ab1345f 223 break;
va009039 0:1ed23ab1345f 224 case 0xc0:
va009039 0:1ed23ab1345f 225 case 0xdb:
va009039 0:1ed23ab1345f 226 case 0xe0:
va009039 0:1ed23ab1345f 227 f_lp = true;
va009039 0:1ed23ab1345f 228 break;
va009039 0:1ed23ab1345f 229 default:
va009039 0:1ed23ab1345f 230 f_lp = false;
va009039 0:1ed23ab1345f 231 break;
va009039 0:1ed23ab1345f 232 }
va009039 0:1ed23ab1345f 233 fputc(c1, fp2);
va009039 0:1ed23ab1345f 234 fputc(c2, fp2); // marker
va009039 0:1ed23ab1345f 235 if (f_lp) {
va009039 0:1ed23ab1345f 236 c1 = fgetc(fp); // length
va009039 0:1ed23ab1345f 237 c2 = fgetc(fp);
va009039 0:1ed23ab1345f 238 fputc(c1, fp2);
va009039 0:1ed23ab1345f 239 fputc(c2, fp2);
va009039 0:1ed23ab1345f 240 int skip = c1<<8|c2;
va009039 0:1ed23ab1345f 241 while(skip-- > 2) {
va009039 0:1ed23ab1345f 242 int c = fgetc(fp);
va009039 0:1ed23ab1345f 243 if (c == EOF) {
va009039 0:1ed23ab1345f 244 break;
va009039 0:1ed23ab1345f 245 }
va009039 0:1ed23ab1345f 246 fputc(c, fp2);
va009039 0:1ed23ab1345f 247 }
va009039 0:1ed23ab1345f 248 }
va009039 0:1ed23ab1345f 249 }
va009039 0:1ed23ab1345f 250 fclose(fp);
va009039 0:1ed23ab1345f 251 fclose(fp2);
va009039 0:1ed23ab1345f 252 }