1/* 2** Copyright (C) 1999-2014 Erik de Castro Lopo <erikd@mega-nerd.com> 3** 4** This program is free software; you can redistribute it and/or modify 5** it under the terms of the GNU Lesser General Public License as published by 6** the Free Software Foundation; either version 2.1 of the License, or 7** (at your option) any later version. 8** 9** This program is distributed in the hope that it will be useful, 10** but WITHOUT ANY WARRANTY; without even the implied warranty of 11** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12** GNU Lesser General Public License for more details. 13** 14** You should have received a copy of the GNU Lesser General Public License 15** along with this program; if not, write to the Free Software 16** Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 17*/ 18 19#include "sfconfig.h" 20 21#include <stdio.h> 22#include <stdlib.h> 23#include <string.h> 24#include <math.h> 25 26#include "sndfile.h" 27#include "sfendian.h" 28#include "common.h" 29#include "G72x/g72x.h" 30 31/* This struct is private to the G72x code. */ 32struct g72x_state ; 33typedef struct g72x_state G72x_STATE ; 34 35typedef struct 36{ /* Private data. Don't mess with it. */ 37 struct g72x_state * private ; 38 39 /* Public data. Read only. */ 40 int blocksize, samplesperblock, bytesperblock ; 41 42 /* Public data. Read and write. */ 43 int blocks_total, block_curr, sample_curr ; 44 unsigned char block [G72x_BLOCK_SIZE] ; 45 short samples [G72x_BLOCK_SIZE] ; 46} G72x_PRIVATE ; 47 48static int psf_g72x_decode_block (SF_PRIVATE *psf, G72x_PRIVATE *pg72x) ; 49static int psf_g72x_encode_block (SF_PRIVATE *psf, G72x_PRIVATE *pg72x) ; 50 51static sf_count_t g72x_read_s (SF_PRIVATE *psf, short *ptr, sf_count_t len) ; 52static sf_count_t g72x_read_i (SF_PRIVATE *psf, int *ptr, sf_count_t len) ; 53static sf_count_t g72x_read_f (SF_PRIVATE *psf, float *ptr, sf_count_t len) ; 54static sf_count_t g72x_read_d (SF_PRIVATE *psf, double *ptr, sf_count_t len) ; 55 56static sf_count_t g72x_write_s (SF_PRIVATE *psf, const short *ptr, sf_count_t len) ; 57static sf_count_t g72x_write_i (SF_PRIVATE *psf, const int *ptr, sf_count_t len) ; 58static sf_count_t g72x_write_f (SF_PRIVATE *psf, const float *ptr, sf_count_t len) ; 59static sf_count_t g72x_write_d (SF_PRIVATE *psf, const double *ptr, sf_count_t len) ; 60 61static sf_count_t g72x_seek (SF_PRIVATE *psf, int mode, sf_count_t offset) ; 62 63static int g72x_close (SF_PRIVATE *psf) ; 64 65 66/*============================================================================================ 67** WAV G721 Reader initialisation function. 68*/ 69 70int 71g72x_init (SF_PRIVATE * psf) 72{ G72x_PRIVATE *pg72x ; 73 int bitspersample, bytesperblock, codec ; 74 75 if (psf->codec_data != NULL) 76 { psf_log_printf (psf, "*** psf->codec_data is not NULL.\n") ; 77 return SFE_INTERNAL ; 78 } ; 79 80 psf->sf.seekable = SF_FALSE ; 81 82 if (psf->sf.channels != 1) 83 return SFE_G72X_NOT_MONO ; 84 85 if ((pg72x = calloc (1, sizeof (G72x_PRIVATE))) == NULL) 86 return SFE_MALLOC_FAILED ; 87 88 psf->codec_data = (void*) pg72x ; 89 90 pg72x->block_curr = 0 ; 91 pg72x->sample_curr = 0 ; 92 93 switch (SF_CODEC (psf->sf.format)) 94 { case SF_FORMAT_G721_32 : 95 codec = G721_32_BITS_PER_SAMPLE ; 96 bytesperblock = G721_32_BYTES_PER_BLOCK ; 97 bitspersample = G721_32_BITS_PER_SAMPLE ; 98 break ; 99 100 case SF_FORMAT_G723_24: 101 codec = G723_24_BITS_PER_SAMPLE ; 102 bytesperblock = G723_24_BYTES_PER_BLOCK ; 103 bitspersample = G723_24_BITS_PER_SAMPLE ; 104 break ; 105 106 case SF_FORMAT_G723_40: 107 codec = G723_40_BITS_PER_SAMPLE ; 108 bytesperblock = G723_40_BYTES_PER_BLOCK ; 109 bitspersample = G723_40_BITS_PER_SAMPLE ; 110 break ; 111 112 default : return SFE_UNIMPLEMENTED ; 113 } ; 114 115 psf->filelength = psf_get_filelen (psf) ; 116 if (psf->filelength < psf->dataoffset) 117 psf->filelength = psf->dataoffset ; 118 119 psf->datalength = psf->filelength - psf->dataoffset ; 120 if (psf->dataend > 0) 121 psf->datalength -= psf->filelength - psf->dataend ; 122 123 if (psf->file.mode == SFM_READ) 124 { pg72x->private = g72x_reader_init (codec, &(pg72x->blocksize), &(pg72x->samplesperblock)) ; 125 if (pg72x->private == NULL) 126 return SFE_MALLOC_FAILED ; 127 128 pg72x->bytesperblock = bytesperblock ; 129 130 psf->read_short = g72x_read_s ; 131 psf->read_int = g72x_read_i ; 132 psf->read_float = g72x_read_f ; 133 psf->read_double = g72x_read_d ; 134 135 psf->seek = g72x_seek ; 136 137 if (psf->datalength % pg72x->blocksize) 138 { psf_log_printf (psf, "*** Odd psf->datalength (%D) should be a multiple of %d\n", psf->datalength, pg72x->blocksize) ; 139 pg72x->blocks_total = (psf->datalength / pg72x->blocksize) + 1 ; 140 } 141 else 142 pg72x->blocks_total = psf->datalength / pg72x->blocksize ; 143 144 psf->sf.frames = (sf_count_t) pg72x->blocks_total * pg72x->samplesperblock ; 145 146 psf_g72x_decode_block (psf, pg72x) ; 147 } 148 else if (psf->file.mode == SFM_WRITE) 149 { pg72x->private = g72x_writer_init (codec, &(pg72x->blocksize), &(pg72x->samplesperblock)) ; 150 if (pg72x->private == NULL) 151 return SFE_MALLOC_FAILED ; 152 153 pg72x->bytesperblock = bytesperblock ; 154 155 psf->write_short = g72x_write_s ; 156 psf->write_int = g72x_write_i ; 157 psf->write_float = g72x_write_f ; 158 psf->write_double = g72x_write_d ; 159 160 if (psf->datalength % pg72x->blocksize) 161 pg72x->blocks_total = (psf->datalength / pg72x->blocksize) + 1 ; 162 else 163 pg72x->blocks_total = psf->datalength / pg72x->blocksize ; 164 165 if (psf->datalength > 0) 166 psf->sf.frames = (8 * psf->datalength) / bitspersample ; 167 168 if ((psf->sf.frames * bitspersample) / 8 != psf->datalength) 169 psf_log_printf (psf, "*** Warning : weird psf->datalength.\n") ; 170 } ; 171 172 psf->codec_close = g72x_close ; 173 174 return 0 ; 175} /* g72x_init */ 176 177/*============================================================================================ 178** G721 Read Functions. 179*/ 180 181static int 182psf_g72x_decode_block (SF_PRIVATE *psf, G72x_PRIVATE *pg72x) 183{ int k ; 184 185 pg72x->block_curr ++ ; 186 pg72x->sample_curr = 0 ; 187 188 if (pg72x->block_curr > pg72x->blocks_total) 189 { memset (pg72x->samples, 0, G72x_BLOCK_SIZE * sizeof (short)) ; 190 return 1 ; 191 } ; 192 193 if ((k = psf_fread (pg72x->block, 1, pg72x->bytesperblock, psf)) != pg72x->bytesperblock) 194 psf_log_printf (psf, "*** Warning : short read (%d != %d).\n", k, pg72x->bytesperblock) ; 195 196 pg72x->blocksize = k ; 197 g72x_decode_block (pg72x->private, pg72x->block, pg72x->samples) ; 198 199 return 1 ; 200} /* psf_g72x_decode_block */ 201 202static int 203g72x_read_block (SF_PRIVATE *psf, G72x_PRIVATE *pg72x, short *ptr, int len) 204{ int count, total = 0, indx = 0 ; 205 206 while (indx < len) 207 { if (pg72x->block_curr > pg72x->blocks_total) 208 { memset (&(ptr [indx]), 0, (len - indx) * sizeof (short)) ; 209 return total ; 210 } ; 211 212 if (pg72x->sample_curr >= pg72x->samplesperblock) 213 psf_g72x_decode_block (psf, pg72x) ; 214 215 count = pg72x->samplesperblock - pg72x->sample_curr ; 216 count = (len - indx > count) ? count : len - indx ; 217 218 memcpy (&(ptr [indx]), &(pg72x->samples [pg72x->sample_curr]), count * sizeof (short)) ; 219 indx += count ; 220 pg72x->sample_curr += count ; 221 total = indx ; 222 } ; 223 224 return total ; 225} /* g72x_read_block */ 226 227static sf_count_t 228g72x_read_s (SF_PRIVATE *psf, short *ptr, sf_count_t len) 229{ G72x_PRIVATE *pg72x ; 230 int readcount, count ; 231 sf_count_t total = 0 ; 232 233 if (psf->codec_data == NULL) 234 return 0 ; 235 pg72x = (G72x_PRIVATE*) psf->codec_data ; 236 237 while (len > 0) 238 { readcount = (len > 0x10000000) ? 0x10000000 : (int) len ; 239 240 count = g72x_read_block (psf, pg72x, ptr, readcount) ; 241 242 total += count ; 243 len -= count ; 244 245 if (count != readcount) 246 break ; 247 } ; 248 249 return total ; 250} /* g72x_read_s */ 251 252static sf_count_t 253g72x_read_i (SF_PRIVATE *psf, int *ptr, sf_count_t len) 254{ BUF_UNION ubuf ; 255 G72x_PRIVATE *pg72x ; 256 short *sptr ; 257 int k, bufferlen, readcount = 0, count ; 258 sf_count_t total = 0 ; 259 260 if (psf->codec_data == NULL) 261 return 0 ; 262 pg72x = (G72x_PRIVATE*) psf->codec_data ; 263 264 sptr = ubuf.sbuf ; 265 bufferlen = SF_BUFFER_LEN / sizeof (short) ; 266 while (len > 0) 267 { readcount = (len >= bufferlen) ? bufferlen : (int) len ; 268 count = g72x_read_block (psf, pg72x, sptr, readcount) ; 269 270 for (k = 0 ; k < readcount ; k++) 271 ptr [total + k] = arith_shift_left (sptr [k], 16) ; 272 273 total += count ; 274 len -= readcount ; 275 if (count != readcount) 276 break ; 277 } ; 278 279 return total ; 280} /* g72x_read_i */ 281 282static sf_count_t 283g72x_read_f (SF_PRIVATE *psf, float *ptr, sf_count_t len) 284{ BUF_UNION ubuf ; 285 G72x_PRIVATE *pg72x ; 286 short *sptr ; 287 int k, bufferlen, readcount = 0, count ; 288 sf_count_t total = 0 ; 289 float normfact ; 290 291 if (psf->codec_data == NULL) 292 return 0 ; 293 pg72x = (G72x_PRIVATE*) psf->codec_data ; 294 295 normfact = (psf->norm_float == SF_TRUE) ? 1.0 / ((float) 0x8000) : 1.0 ; 296 297 sptr = ubuf.sbuf ; 298 bufferlen = SF_BUFFER_LEN / sizeof (short) ; 299 while (len > 0) 300 { readcount = (len >= bufferlen) ? bufferlen : (int) len ; 301 count = g72x_read_block (psf, pg72x, sptr, readcount) ; 302 for (k = 0 ; k < readcount ; k++) 303 ptr [total + k] = normfact * sptr [k] ; 304 305 total += count ; 306 len -= readcount ; 307 if (count != readcount) 308 break ; 309 } ; 310 311 return total ; 312} /* g72x_read_f */ 313 314static sf_count_t 315g72x_read_d (SF_PRIVATE *psf, double *ptr, sf_count_t len) 316{ BUF_UNION ubuf ; 317 G72x_PRIVATE *pg72x ; 318 short *sptr ; 319 int k, bufferlen, readcount = 0, count ; 320 sf_count_t total = 0 ; 321 double normfact ; 322 323 if (psf->codec_data == NULL) 324 return 0 ; 325 pg72x = (G72x_PRIVATE*) psf->codec_data ; 326 327 normfact = (psf->norm_double == SF_TRUE) ? 1.0 / ((double) 0x8000) : 1.0 ; 328 329 sptr = ubuf.sbuf ; 330 bufferlen = SF_BUFFER_LEN / sizeof (short) ; 331 while (len > 0) 332 { readcount = (len >= bufferlen) ? bufferlen : (int) len ; 333 count = g72x_read_block (psf, pg72x, sptr, readcount) ; 334 for (k = 0 ; k < readcount ; k++) 335 ptr [total + k] = normfact * (double) (sptr [k]) ; 336 337 total += count ; 338 len -= readcount ; 339 if (count != readcount) 340 break ; 341 } ; 342 343 return total ; 344} /* g72x_read_d */ 345 346static sf_count_t 347g72x_seek (SF_PRIVATE *psf, int UNUSED (mode), sf_count_t UNUSED (offset)) 348{ 349 psf_log_printf (psf, "seek unsupported\n") ; 350 351 /* No simple solution. To do properly, would need to seek 352 ** to start of file and decode everything up to seek position. 353 ** Maybe implement SEEK_SET to 0 only? 354 */ 355 return 0 ; 356 357/* 358** G72x_PRIVATE *pg72x ; 359** int newblock, newsample, sample_curr ; 360** 361** if (psf->codec_data == NULL) 362** return 0 ; 363** pg72x = (G72x_PRIVATE*) psf->codec_data ; 364** 365** if (! (psf->datalength && psf->dataoffset)) 366** { psf->error = SFE_BAD_SEEK ; 367** return PSF_SEEK_ERROR ; 368** } ; 369** 370** sample_curr = (8 * psf->datalength) / G721_32_BITS_PER_SAMPLE ; 371** 372** switch (whence) 373** { case SEEK_SET : 374** if (offset < 0 || offset > sample_curr) 375** { psf->error = SFE_BAD_SEEK ; 376** return PSF_SEEK_ERROR ; 377** } ; 378** newblock = offset / pg72x->samplesperblock ; 379** newsample = offset % pg72x->samplesperblock ; 380** break ; 381** 382** case SEEK_CUR : 383** if (psf->current + offset < 0 || psf->current + offset > sample_curr) 384** { psf->error = SFE_BAD_SEEK ; 385** return PSF_SEEK_ERROR ; 386** } ; 387** newblock = (8 * (psf->current + offset)) / pg72x->samplesperblock ; 388** newsample = (8 * (psf->current + offset)) % pg72x->samplesperblock ; 389** break ; 390** 391** case SEEK_END : 392** if (offset > 0 || sample_curr + offset < 0) 393** { psf->error = SFE_BAD_SEEK ; 394** return PSF_SEEK_ERROR ; 395** } ; 396** newblock = (sample_curr + offset) / pg72x->samplesperblock ; 397** newsample = (sample_curr + offset) % pg72x->samplesperblock ; 398** break ; 399** 400** default : 401** psf->error = SFE_BAD_SEEK ; 402** return PSF_SEEK_ERROR ; 403** } ; 404** 405** if (psf->file.mode == SFM_READ) 406** { psf_fseek (psf, psf->dataoffset + newblock * pg72x->blocksize, SEEK_SET) ; 407** pg72x->block_curr = newblock ; 408** psf_g72x_decode_block (psf, pg72x) ; 409** pg72x->sample_curr = newsample ; 410** } 411** else 412** { /+* What to do about write??? *+/ 413** psf->error = SFE_BAD_SEEK ; 414** return PSF_SEEK_ERROR ; 415** } ; 416** 417** psf->current = newblock * pg72x->samplesperblock + newsample ; 418** return psf->current ; 419** 420*/ 421} /* g72x_seek */ 422 423/*========================================================================================== 424** G72x Write Functions. 425*/ 426 427static int 428psf_g72x_encode_block (SF_PRIVATE *psf, G72x_PRIVATE *pg72x) 429{ int k ; 430 431 /* Encode the samples. */ 432 g72x_encode_block (pg72x->private, pg72x->samples, pg72x->block) ; 433 434 /* Write the block to disk. */ 435 if ((k = psf_fwrite (pg72x->block, 1, pg72x->blocksize, psf)) != pg72x->blocksize) 436 psf_log_printf (psf, "*** Warning : short write (%d != %d).\n", k, pg72x->blocksize) ; 437 438 pg72x->sample_curr = 0 ; 439 pg72x->block_curr ++ ; 440 441 /* Set samples to zero for next block. */ 442 memset (pg72x->samples, 0, G72x_BLOCK_SIZE * sizeof (short)) ; 443 444 return 1 ; 445} /* psf_g72x_encode_block */ 446 447static int 448g72x_write_block (SF_PRIVATE *psf, G72x_PRIVATE *pg72x, const short *ptr, int len) 449{ int count, total = 0, indx = 0 ; 450 451 while (indx < len) 452 { count = pg72x->samplesperblock - pg72x->sample_curr ; 453 454 if (count > len - indx) 455 count = len - indx ; 456 457 memcpy (&(pg72x->samples [pg72x->sample_curr]), &(ptr [indx]), count * sizeof (short)) ; 458 indx += count ; 459 pg72x->sample_curr += count ; 460 total = indx ; 461 462 if (pg72x->sample_curr >= pg72x->samplesperblock) 463 psf_g72x_encode_block (psf, pg72x) ; 464 } ; 465 466 return total ; 467} /* g72x_write_block */ 468 469static sf_count_t 470g72x_write_s (SF_PRIVATE *psf, const short *ptr, sf_count_t len) 471{ G72x_PRIVATE *pg72x ; 472 int writecount, count ; 473 sf_count_t total = 0 ; 474 475 if (psf->codec_data == NULL) 476 return 0 ; 477 pg72x = (G72x_PRIVATE*) psf->codec_data ; 478 479 while (len > 0) 480 { writecount = (len > 0x10000000) ? 0x10000000 : (int) len ; 481 482 count = g72x_write_block (psf, pg72x, ptr, writecount) ; 483 484 total += count ; 485 len -= count ; 486 if (count != writecount) 487 break ; 488 } ; 489 490 return total ; 491} /* g72x_write_s */ 492 493static sf_count_t 494g72x_write_i (SF_PRIVATE *psf, const int *ptr, sf_count_t len) 495{ BUF_UNION ubuf ; 496 G72x_PRIVATE *pg72x ; 497 short *sptr ; 498 int k, bufferlen, writecount = 0, count ; 499 sf_count_t total = 0 ; 500 501 if (psf->codec_data == NULL) 502 return 0 ; 503 pg72x = (G72x_PRIVATE*) psf->codec_data ; 504 505 sptr = ubuf.sbuf ; 506 bufferlen = SF_BUFFER_LEN / sizeof (short) ; 507 while (len > 0) 508 { writecount = (len >= bufferlen) ? bufferlen : (int) len ; 509 for (k = 0 ; k < writecount ; k++) 510 sptr [k] = ptr [total + k] >> 16 ; 511 count = g72x_write_block (psf, pg72x, sptr, writecount) ; 512 513 total += count ; 514 len -= writecount ; 515 if (count != writecount) 516 break ; 517 } ; 518 return total ; 519} /* g72x_write_i */ 520 521static sf_count_t 522g72x_write_f (SF_PRIVATE *psf, const float *ptr, sf_count_t len) 523{ BUF_UNION ubuf ; 524 G72x_PRIVATE *pg72x ; 525 short *sptr ; 526 int k, bufferlen, writecount = 0, count ; 527 sf_count_t total = 0 ; 528 float normfact ; 529 530 if (psf->codec_data == NULL) 531 return 0 ; 532 pg72x = (G72x_PRIVATE*) psf->codec_data ; 533 534 normfact = (psf->norm_float == SF_TRUE) ? (1.0 * 0x8000) : 1.0 ; 535 536 sptr = ubuf.sbuf ; 537 bufferlen = SF_BUFFER_LEN / sizeof (short) ; 538 while (len > 0) 539 { writecount = (len >= bufferlen) ? bufferlen : (int) len ; 540 for (k = 0 ; k < writecount ; k++) 541 sptr [k] = psf_lrintf (normfact * ptr [total + k]) ; 542 count = g72x_write_block (psf, pg72x, sptr, writecount) ; 543 544 total += count ; 545 len -= writecount ; 546 if (count != writecount) 547 break ; 548 } ; 549 550 return total ; 551} /* g72x_write_f */ 552 553static sf_count_t 554g72x_write_d (SF_PRIVATE *psf, const double *ptr, sf_count_t len) 555{ BUF_UNION ubuf ; 556 G72x_PRIVATE *pg72x ; 557 short *sptr ; 558 int k, bufferlen, writecount = 0, count ; 559 sf_count_t total = 0 ; 560 double normfact ; 561 562 if (psf->codec_data == NULL) 563 return 0 ; 564 pg72x = (G72x_PRIVATE*) psf->codec_data ; 565 566 normfact = (psf->norm_double == SF_TRUE) ? (1.0 * 0x8000) : 1.0 ; 567 568 sptr = ubuf.sbuf ; 569 bufferlen = SF_BUFFER_LEN / sizeof (short) ; 570 while (len > 0) 571 { writecount = (len >= bufferlen) ? bufferlen : (int) len ; 572 for (k = 0 ; k < writecount ; k++) 573 sptr [k] = psf_lrint (normfact * ptr [total + k]) ; 574 count = g72x_write_block (psf, pg72x, sptr, writecount) ; 575 576 total += count ; 577 len -= writecount ; 578 if (count != writecount) 579 break ; 580 } ; 581 582 return total ; 583} /* g72x_write_d */ 584 585static int 586g72x_close (SF_PRIVATE *psf) 587{ G72x_PRIVATE *pg72x ; 588 589 pg72x = (G72x_PRIVATE*) psf->codec_data ; 590 591 if (psf->file.mode == SFM_WRITE) 592 { /* If a block has been partially assembled, write it out 593 ** as the final block. 594 */ 595 596 if (pg72x->sample_curr && pg72x->sample_curr < G72x_BLOCK_SIZE) 597 psf_g72x_encode_block (psf, pg72x) ; 598 599 if (psf->write_header) 600 psf->write_header (psf, SF_FALSE) ; 601 } ; 602 603 /* Only free the pointer allocated by g72x_(reader|writer)_init. */ 604 free (pg72x->private) ; 605 606 return 0 ; 607} /* g72x_close */ 608 609