/alps/ufodecode : revision 45

12

13

#define CHECKS

14

15

#ifdef DEBUG

16

# define SLOW_CHECKS

17

# define ALL_CHECKS

18

# define VERBOSE

19

#endif /* DEBUG */

20

21

#ifdef ALL_CHECKS

22

# undef HAVE_SSE

23

#endif /* ALL_CHECKS */

24

25

15

26

#define IPECAMERA_NUM_ROWS 1088

16

27

#define IPECAMERA_NUM_CHANNELS 16 /**< Number of channels per row */

17

28

#define IPECAMERA_PIXELS_PER_CHANNEL 128 /**< Number of pixels per channel */

35

46

/**

36

47

* Check if value matches expected input.

37

48

*/

38

#ifdef DEBUG

49

#ifdef VERBOSE

39

50

# define CHECK_VALUE(value, expected) \

40

51

if (value != expected) { \

41

52

fprintf(stderr, "<%s:%i> 0x%x != 0x%x\n", __FILE__, __LINE__, value, expected); \

51

62

/**

52

63

* Check that flag evaluates to non-zero.

53

64

*/

54

#ifdef DEBUG

65

#ifdef VERBOSE

55

66

# define CHECK_FLAG(flag, check, ...) \

56

67

if (!(check)) { \

57

68

fprintf(stderr, "<%s:%i> Unexpected value 0x%x of " flag "\n", __FILE__, __LINE__, __VA_ARGS__); \

136

147

int pos = 0;

137

148

uint32_t data;

138

149

139

#if defined(HAVE_SSE) && !defined(DEBUG)

150

#ifdef HAVE_SSE

140

151

__m128i mask = _mm_set_epi32(0x3FF, 0x3FF, 0x3FF, 0x3FF);

141

152

__m128i packed;

142

153

__m128i tmp1, tmp2;

144

155

#endif

145

156

146

157

if (cpi * channel_size > num_words) {

147

#ifdef DEBUG

158

#ifdef VERBOSE

148

159

fprintf(stderr, "Not enough data to decode frame, expected %lu bytes, but received %lu\n", cpi * channel_size * sizeof(uint32_t), num_words * sizeof(uint32_t));

149

160

#endif

150

161

return EILSEQ;

156

167

int channel = info & 0x0F;

157

168

int pixels = (info >> 20) & 0xFF;

158

169

159

#ifdef CHECKS

170

#ifdef SLOW_CHECKS

160

171

int header = (info >> 30) & 0x03;

161

172

const int bpp = (info >> 16) & 0x0F;

162

173

int err;

177

188

pixel_buffer[base] = 0;

178

189

/* base++; */

179

190

}

180

#ifdef CHECKS

191

#ifdef SLOW_CHECKS

181

192

else

182

193

CHECK_FLAG("number of pixels, %i is expected", pixels == IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL);

183

194

#endif

184

195

185

#if defined(HAVE_SSE) && !defined(DEBUG)

196

#ifdef HAVE_SSE

186

197

for (int i = 1 ; i < bytes-4; i += 4, base += 12) {

187

198

packed = _mm_set_epi32(raw[i], raw[i+1], raw[i+2], raw[i+3]);

188

199

224

235

#else

225

236

for (int i = 1 ; i < bytes; i++) {

226

237

data = raw[i];

227

#ifdef DEBUG

238

# ifdef SLOW_CHECKS

228

239

header = (data >> 30) & 0x03;

229

240

CHECK_FLAG("raw data magic", header == 3, header);

230

241

if (err)

231

242

return err;

232

#endif

243

# endif

233

244

pixel_buffer[base++] = (data >> 20) & 0x3FF;

234

245

pixel_buffer[base++] = (data >> 10) & 0x3FF;

235

246

pixel_buffer[base++] = data & 0x3FF;

237

248

#endif

238

249

239

250

data = raw[bytes];

240

#ifdef DEBUG

251

#ifdef SLOW_CHECKS

241

252

header = (data >> 30) & 0x03;

242

253

CHECK_FLAG("raw data magic", header == 3, header);

243

254

CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF));

253

264

raw += channel_size;

254

265

}

255

266

256

*offset = pos;

267

*offset += pos;

257

268

return 0;

258

269

}

259

270

274

285

int pos = 0;

275

286

uint32_t data;

276

287

277

#if defined(HAVE_SSE) && !defined(DEBUG)

288

#ifdef HAVE_SSE

278

289

__m128i mask = _mm_set_epi32(0x3FF, 0x3FF, 0x3FF, 0x3FF);

279

290

__m128i packed;

280

291

__m128i tmp1, tmp2;

282

293

#endif

283

294

284

295

if (cpi * channel_size > num_words) {

285

#ifdef DEBUG

296

#ifdef VERBOSE

286

297

fprintf(stderr, "Not enough data to decode frame, expected %lu bytes, but received %lu\n", cpi * channel_size * sizeof(uint32_t), num_words * sizeof(uint32_t));

287

298

#endif

288

299

return EILSEQ;

289

300

}

290

301

291

302

for (size_t c = 0; c < cpi; c++) {

292

const int info = raw[0];

293

int row = (info >> 4) & 0x7FF;

294

int channel = info & 0x0F;

295

int pixels = (info >> 20) & 0xFF;

296

297

#ifdef DEBUG

298

303

int err = 0;

304

const int info = raw[0];

305

int row = (info >> 4) & 0x7FF;

306

int channel = info & 0x0F;

307

int pixels = (info >> 20) & 0xFF;

308

309

#ifdef SLOW_CHECKS

299

310

int header = (info >> 30) & 0x03;

300

311

const int bpp = (info >> 16) & 0x0F;

301

312

CHECK_FLAG("raw header magick", header == 2, header);

302

313

CHECK_FLAG("pixel size, only 10 bits are supported", bpp == 10, bpp);

303

CHECK_FLAG("channel, limited by %zu output channels", channel < channels_per_row, channel, channels_per_row);

304

314

#endif

305

315

306

if ((channel > channels_per_row) || (pixels > IPECAMERA_PIXELS_PER_CHANNEL))

316

CHECK_FLAG("pixels, only %u pixels per channel (offset: %zx)", pixels <= IPECAMERA_PIXELS_PER_CHANNEL, pixels, IPECAMERA_PIXELS_PER_CHANNEL, (*offset + pos));

317

CHECK_FLAG("channel, limited by %zu output channels (offset: %zx)", channel <= channels_per_row, channel, channels_per_row, (*offset + pos));

318

if (err)

307

319

return EILSEQ;

308

320

309

321

channel = channel_order[channel];

315

327

/* base++; */

316

328

}

317

329

318

#if defined(HAVE_SSE) && !defined(DEBUG)

330

#ifdef HAVE_SSE

319

331

for (int i = 1 ; i < bytes-4; i += 4, base += 12) {

320

332

packed = _mm_set_epi32(raw[i], raw[i+1], raw[i+2], raw[i+3]);

321

333

357

369

#else

358

370

for (int i = 1 ; i < bytes; i++) {

359

371

data = raw[i];

360

#ifdef DEBUG

372

# ifdef SLOW_CHECKS

361

373

header = (data >> 30) & 0x03;

362

CHECK_FLAG("raw data magic", header == 3, header);

374

CHECK_FLAG("raw data magic (offset: 0x%zx, expected: 0x02)", header == 3, header, (*offset + pos + i)*sizeof(uint32_t));

363

375

if (err)

364

376

return err;

365

#endif

377

# endif

366

378

pixel_buffer[base++] = (data >> 20) & 0x3FF;

367

379

pixel_buffer[base++] = (data >> 10) & 0x3FF;

368

380

pixel_buffer[base++] = data & 0x3FF;

370

382

#endif

371

383

372

384

data = raw[bytes];

373

#ifdef DEBUG

385

#ifdef SLOW_CHECKS

374

386

header = (data >> 30) & 0x03;

375

CHECK_FLAG("raw data magic", header == 3, header);

376

CHECK_FLAG("raw footer magic", (data & 0x3FF) == 0x55, (data & 0x3FF));

387

CHECK_FLAG("raw data magic (offset: 0x%zx, expected: 0x03)", header == 3, header, (*offset + pos + bytes)*sizeof(uint32_t));

388

CHECK_FLAG("raw footer magic (offset: 0x%zx, expected: 0x55)", (data & 0x3FF) == 0x55, (data & 0x3FF), (*offset + pos + bytes)*sizeof(uint32_t));

377

389

if (err)

378

390

return err;

379

391

#endif

386

398

raw += channel_size;

387

399

}

388

400

389

*offset = pos;

401

*offset += pos;

390

402

return 0;

391

403

}

392

404

460

472

}

461

473

}

462

474

463

*offset = base;

475

*offset += base;

464

476

return 0;

465

477

}

466

478

535

547

{

536

548

int err = 0;

537

549

size_t pos = 0;

538

size_t advance = 0;

539

550

const size_t num_words = num_bytes / 4;

540

551

541

if ((pixels == NULL) || (num_words < 16))

552

if ((pixels == NULL) || (num_words < 16)) {

553

#ifdef VERBOSE

554

if (num_words < 16)

555

fprintf(stderr, "Not enough frame data, only %zu bytes supplied \n", num_bytes);

556

#endif

542

557

return 0;

558

}

543

559

544

560

size_t rows_per_frame = decoder->height;

545

561

const int version = (raw[pos+6] >> 24) & 0xF;

546

562

547

#ifdef DEBUG

563

#ifdef CHECKS

548

564

CHECK_VALUE(raw[pos++], 0x51111111);

549

565

CHECK_VALUE(raw[pos++], 0x52222222);

550

566

CHECK_VALUE(raw[pos++], 0x53333333);

576

592

pos++;

577

593

578

594

if ((meta->output_mode != IPECAMERA_MODE_4_CHAN_IO) && (meta->output_mode != IPECAMERA_MODE_16_CHAN_IO)) {

579

#ifdef DEBUG

595

# ifdef VERBOSE

580

596

fprintf(stderr, "Output mode 0x%x is not supported\n", meta->output_mode);

581

#endif

582

return EILSEQ;

597

# endif

598

return 0;

583

599

}

584

600

break;

585

601

586

602

default:

603

# ifdef VERBOSE

587

604

fprintf(stderr, "Unsupported data format version %i detected\n", version);

605

# endif

588

606

return 0;

589

607

}

590

608

597

615

meta->time_stamp = raw[pos + 7] & 0xFFFFFFF;

598

616

break;

599

617

case 4:

618

meta->n_rows = rows_per_frame = raw[pos + 5] & 0x7FF;

600

619

case 5:

601

620

meta->frame_number = raw[pos + 6] & 0x1FFFFFF;

602

621

meta->time_stamp = raw[pos + 7] & 0xFFFFFF;

605

624

606

625

break;

607

626

default:

627

# ifdef VERBOSE

608

628

fprintf(stderr, "Unsupported data format detected\n");

629

# endif

609

630

return 0;

610

631

}

611

632

614

635

615

636

switch (version) {

616

637

case 0:

617

err = ufo_decode_frame_channels_v0(decoder, pixels, raw + pos, num_words - pos - 8, &advance);

638

err = ufo_decode_frame_channels_v0(decoder, pixels, raw + pos, num_words - pos - 8, &pos);

618

639

break;

619

640

case 4:

620

err = ufo_decode_frame_channels_v4(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &advance);

641

err = ufo_decode_frame_channels_v4(decoder, pixels, raw + pos, num_words - pos - 8, rows_per_frame, &pos);

621

642

break;

622

643

case 5:

623

err = ufo_decode_frame_channels_v5(decoder, pixels, raw + pos, rows_per_frame, &advance, meta->output_mode);

644

err = ufo_decode_frame_channels_v5(decoder, pixels, raw + pos, rows_per_frame, &pos, meta->output_mode);

624

645

break;

625

646

default:

626

647

break;

629

650

if (err)

630

651

return 0;

631

652

632

pos += advance;

633

634

653

#ifdef CHECKS

635

654

CHECK_VALUE(raw[pos++], 0x0AAAAAAA);

636

655