apps/codecs/libgme/resampler.c

   1 // Game_Music_Emu 0.5.5. http://www.slack.net/~ant/
   2
   3 #include "resampler.h"
   4
   5 #include <stdlib.h>
   6 #include <string.h>
   7
   8 /* Copyright (C) 2003-2006 Shay Green. This module is free software; you
   9 can redistribute it and/or modify it under the terms of the GNU Lesser
  10 General Public License as published by the Free Software Foundation; either
  11 version 2.1 of the License, or (at your option) any later version. This
  12 module is distributed in the hope that it will be useful, but WITHOUT ANY
  13 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  14 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
  15 details. You should have received a copy of the GNU Lesser General Public
  16 License along with this module; if not, write to the Free Software Foundation,
  17 Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA */
  18
  19 #include "blargg_source.h"
  20
  21 // TODO: fix this. hack since resampler holds back some output.
  22 unsigned const resampler_extra = 34;
  23
  24 enum { shift = 14 };
  25 int const unit = 1 << shift;
  26
  27 blargg_err_t Resampler_setup( struct Resampler* this, double oversample, double rolloff, double gain )
  28 {
  29         (void) rolloff;
  30
  31         this->gain_ = (int)((1 << gain_bits) * gain);
  32         this->step = (int) ( oversample * unit + 0.5);
  33         this->rate_ = 1.0 / unit * this->step;
  34         return 0;
  35 }
  36
  37 blargg_err_t Resampler_reset( struct Resampler* this, int pairs )
  38 {
  39         // expand allocations a bit
  40         Resampler_resize( this, pairs );
  41         this->resampler_size = this->oversamples_per_frame + (this->oversamples_per_frame >> 2);
  42
  43         this->buffer_size = this->resampler_size;
  44         this->pos = 0;
  45         this->write_pos = 0;
  46         return 0;
  47 }
  48
  49 void Resampler_resize( struct Resampler* this, int pairs )
  50 {
  51         int new_sample_buf_size = pairs * 2;
  52         if ( this->sample_buf_size != new_sample_buf_size )
  53         {
  54                 this->sample_buf_size = new_sample_buf_size;
  55                 this->oversamples_per_frame = (int) (pairs * this->rate_) * 2 + 2;
  56                 Resampler_clear( this );
  57         }
  58 }
  59
  60 void mix_mono( struct Resampler* this, struct Stereo_Buffer* stereo_buf, dsample_t* out_ )
  61 {
  62         int const bass = BLIP_READER_BASS( stereo_buf->bufs [0] );
  63         BLIP_READER_BEGIN( sn, stereo_buf->bufs [0] );
  64
  65         int count = this->sample_buf_size >> 1;
  66         BLIP_READER_ADJ_( sn, count );
  67
  68         typedef dsample_t stereo_dsample_t [2];
  69         stereo_dsample_t* BLARGG_RESTRICT out = (stereo_dsample_t*) out_ + count;
  70         stereo_dsample_t const* BLARGG_RESTRICT in =
  71                         (stereo_dsample_t const*) this->sample_buf + count;
  72         int offset = -count;
  73         int const gain = this->gain_;
  74         do
  75         {
  76                 int s = BLIP_READER_READ_RAW( sn ) >> (blip_sample_bits - 16);
  77                 BLIP_READER_NEXT_IDX_( sn, bass, offset );
  78
  79                 int l = (in [offset] [0] * gain >> gain_bits) + s;
  80                 int r = (in [offset] [1] * gain >> gain_bits) + s;
  81
  82                 BLIP_CLAMP( l, l );
  83                 out [offset] [0] = (blip_sample_t) l;
  84
  85                 BLIP_CLAMP( r, r );
  86                 out [offset] [1] = (blip_sample_t) r;
  87         }
  88         while ( ++offset );
  89
  90         BLIP_READER_END( sn, stereo_buf->bufs [0] );
  91 }
  92
  93 void mix_stereo( struct Resampler* this, struct Stereo_Buffer* stereo_buf, dsample_t* out_ )
  94 {
  95         int const bass = BLIP_READER_BASS( stereo_buf->bufs [0] );
  96         BLIP_READER_BEGIN( snc, stereo_buf->bufs [0] );
  97         BLIP_READER_BEGIN( snl, stereo_buf->bufs [1] );
  98         BLIP_READER_BEGIN( snr, stereo_buf->bufs [2] );
  99
 100         int count = this->sample_buf_size >> 1;
 101         BLIP_READER_ADJ_( snc, count );
 102         BLIP_READER_ADJ_( snl, count );
 103         BLIP_READER_ADJ_( snr, count );
 104
 105         typedef dsample_t stereo_dsample_t [2];
 106         stereo_dsample_t* BLARGG_RESTRICT out = (stereo_dsample_t*) out_ + count;
 107         stereo_dsample_t const* BLARGG_RESTRICT in =
 108                         (stereo_dsample_t const*) this->sample_buf + count;
 109         int offset = -count;
 110         int const gain = this->gain_;
 111         do
 112         {
 113                 int sc = BLIP_READER_READ_RAW( snc ) >> (blip_sample_bits - 16);
 114                 int sl = BLIP_READER_READ_RAW( snl ) >> (blip_sample_bits - 16);
 115                 int sr = BLIP_READER_READ_RAW( snr ) >> (blip_sample_bits - 16);
 116                 BLIP_READER_NEXT_IDX_( snc, bass, offset );
 117                 BLIP_READER_NEXT_IDX_( snl, bass, offset );
 118                 BLIP_READER_NEXT_IDX_( snr, bass, offset );
 119
 120                 int l = (in [offset] [0] * gain >> gain_bits) + sl + sc;
 121                 int r = (in [offset] [1] * gain >> gain_bits) + sr + sc;
 122
 123                 BLIP_CLAMP( l, l );
 124                 out [offset] [0] = (blip_sample_t) l;
 125
 126                 BLIP_CLAMP( r, r );
 127                 out [offset] [1] = (blip_sample_t) r;
 128         }
 129         while ( ++offset );
 130
 131         BLIP_READER_END( snc, stereo_buf->bufs [0] );
 132         BLIP_READER_END( snl, stereo_buf->bufs [1] );
 133         BLIP_READER_END( snr, stereo_buf->bufs [2] );
 134 }
 135
 136 void mix_stereo_no_center( struct Resampler* this, struct Stereo_Buffer* stereo_buf, dsample_t* out_ )
 137 {
 138         int const bass = BLIP_READER_BASS( stereo_buf->bufs [0] );
 139         BLIP_READER_BEGIN( snl, stereo_buf->bufs [1] );
 140         BLIP_READER_BEGIN( snr, stereo_buf->bufs [2] );
 141
 142         int count = this->sample_buf_size >> 1;
 143         BLIP_READER_ADJ_( snl, count );
 144         BLIP_READER_ADJ_( snr, count );
 145
 146         typedef dsample_t stereo_dsample_t [2];
 147         stereo_dsample_t* BLARGG_RESTRICT out = (stereo_dsample_t*) out_ + count;
 148         stereo_dsample_t const* BLARGG_RESTRICT in =
 149                         (stereo_dsample_t const*) this->sample_buf + count;
 150         int offset = -count;
 151         int const gain = this->gain_;
 152         do
 153         {
 154                 int sl = BLIP_READER_READ_RAW( snl ) >> (blip_sample_bits - 16);
 155                 int sr = BLIP_READER_READ_RAW( snr ) >> (blip_sample_bits - 16);
 156                 BLIP_READER_NEXT_IDX_( snl, bass, offset );
 157                 BLIP_READER_NEXT_IDX_( snr, bass, offset );
 158
 159                 int l = (in [offset] [0] * gain >> gain_bits) + sl;
 160                 int r = (in [offset] [1] * gain >> gain_bits) + sr;
 161
 162                 BLIP_CLAMP( l, l );
 163                 out [offset] [0] = (blip_sample_t) l;
 164
 165                 BLIP_CLAMP( r, r );
 166                 out [offset] [1] = (blip_sample_t) r;
 167         }
 168         while ( ++offset );
 169
 170         BLIP_READER_END( snl, stereo_buf->bufs [1] );
 171         BLIP_READER_END( snr, stereo_buf->bufs [2] );
 172 }
 173
 174 dsample_t const* resample_( struct Resampler* this, dsample_t** out_,
 175                 dsample_t const* out_end, dsample_t const in [], int in_size )
 176 {
 177         in_size -= write_offset;
 178         if ( in_size > 0 )
 179         {
 180                 dsample_t* BLIP_RESTRICT out = *out_;
 181                 dsample_t const* const in_end = in + in_size;
 182
 183                 int const step = this->step;
 184                 int       pos  = this->pos;
 185
 186                 // TODO: IIR filter, then linear resample
 187                 // TODO: detect skipped sample, allowing merging of IIR and resample?
 188
 189                 do
 190                 {
 191                         #define INTERP( i, out )\
 192                                 out = (in [0 + i] * (unit - pos) + ((in [2 + i] + in [4 + i] + in [6 + i]) << shift) +\
 193                                 in [8 + i] * pos) >> (shift + 2);
 194
 195                         int out_0;
 196                         INTERP( 0,                  out_0 )
 197                         INTERP( 1, out [0] = out_0; out [1] )
 198                         out += stereo;
 199
 200                         pos += step;
 201                         in += ((unsigned) pos >> shift) * stereo;
 202                         pos &= unit - 1;
 203                 }
 204                 while ( in < in_end && out < out_end );
 205
 206                 this->pos = pos;
 207                 *out_ = out;
 208         }
 209         return in;
 210 }
 211
 212 inline int resample_wrapper( struct Resampler* this, dsample_t out [], int* out_size,
 213                 dsample_t const in [], int in_size )
 214 {
 215         assert( Resampler_rate( this ) );
 216
 217         dsample_t* out_ = out;
 218         int result = resample_( this, &out_, out + *out_size, in, in_size ) - in;
 219         assert( out_ <= out + *out_size );
 220         assert( result <= in_size );
 221
 222         *out_size = out_ - out;
 223         return result;
 224 }
 225
 226 int skip_input( struct Resampler* this, int count )
 227 {
 228         this->write_pos -= count;
 229         if ( this->write_pos < 0 ) // occurs when downsampling
 230         {
 231                 count += this->write_pos;
 232                 this->write_pos = 0;
 233         }
 234         memmove( this->buf, &this->buf [count], this->write_pos * sizeof this->buf [0] );
 235         return count;
 236 }
 237
 238 void play_frame_( struct Resampler* this, struct Stereo_Buffer* stereo_buf, dsample_t* out )
 239 {
 240         long pair_count = this->sample_buf_size >> 1;
 241         blip_time_t blip_time = Blip_count_clocks( &stereo_buf->bufs [0], pair_count );
 242         int sample_count = this->oversamples_per_frame - this->write_pos + resampler_extra;
 243
 244         int new_count = this->callback( this->callback_data, blip_time, sample_count, &this->buf [this->write_pos] );
 245         assert( new_count < resampler_size );
 246
 247         Buffer_end_frame( stereo_buf, blip_time );
 248         /* Blip_end_frame( &stereo_buf->bufs [0], blip_time ); */
 249         assert( Blip_samples_avail( &stereo_buf->bufs [0] ) == pair_count * 2 );
 250
 251         this->write_pos += new_count;
 252         assert( (unsigned) this->write_pos <= this->buffer_size );
 253
 254         new_count = this->sample_buf_size;
 255         if ( new_count )
 256                 skip_input( this, resample_wrapper( this, this->sample_buf, &new_count, this->buf, this->write_pos ) );
 257         assert( new_count == (long) this->sample_buf_size );
 258
 259         int bufs_used = stereo_buf->stereo_added | stereo_buf->was_stereo;
 260         if ( bufs_used <= 1 ) {
 261                 mix_mono( this, stereo_buf, out );
 262                 Blip_remove_samples( &stereo_buf->bufs [0], pair_count );
 263                 Blip_remove_silence( &stereo_buf->bufs [1], pair_count );
 264                 Blip_remove_silence( &stereo_buf->bufs [2], pair_count );
 265         }
 266         else if ( bufs_used & 1 ) {
 267                 mix_stereo( this, stereo_buf, out );
 268                 Blip_remove_samples( &stereo_buf->bufs [0], pair_count );
 269                 Blip_remove_samples( &stereo_buf->bufs [1], pair_count );
 270                 Blip_remove_samples( &stereo_buf->bufs [2], pair_count );
 271         }
 272         else {
 273                 mix_stereo_no_center( this, stereo_buf, out );
 274                 Blip_remove_silence( &stereo_buf->bufs [0], pair_count );
 275                 Blip_remove_samples( &stereo_buf->bufs [1], pair_count );
 276                 Blip_remove_samples( &stereo_buf->bufs [2], pair_count );
 277         }
 278
 279         // to do: this might miss opportunities for optimization
 280         if ( !Blip_samples_avail( &stereo_buf->bufs [0] ) )
 281         {
 282                 stereo_buf->was_stereo   = stereo_buf->stereo_added;
 283                 stereo_buf->stereo_added = 0;
 284         }
 285
 286         /* mix_mono( this, stereo_buf, out );
 287         Blip_remove_samples( &stereo_buf->bufs [0], pair_count ); */
 288 }
 289
 290 void Resampler_play( struct Resampler* this, long count, dsample_t* out, struct Stereo_Buffer* stereo_buf )
 291 {
 292         // empty extra buffer
 293         long remain = this->sample_buf_size - this->buf_pos;
 294         if ( remain )
 295         {
 296                 if ( remain > count )
 297                         remain = count;
 298                 count -= remain;
 299                 memcpy( out, &this->sample_buf [this->buf_pos], remain * sizeof *out );
 300                 out += remain;
 301                 this->buf_pos += remain;
 302         }
 303
 304         // entire frames
 305         while ( count >= (long) this->sample_buf_size )
 306         {
 307                 play_frame_( this, stereo_buf, out );
 308                 out += this->sample_buf_size;
 309                 count -= this->sample_buf_size;
 310         }
 311
 312         // extra
 313         if ( count )
 314         {
 315                 play_frame_( this, stereo_buf, this->sample_buf );
 316                 this->buf_pos = count;
 317                 memcpy( out, this->sample_buf, count * sizeof *out );
 318                 out += count;
 319         }
 320 }