streamtools/resampler.cpp

   1 #include "resampler.hpp"
   2 #include <iostream>
   3 #include <sstream>
   4 #include <stdexcept>
   5 #include "misc.hpp"
   6 #include "opl.h"
   7
   8 uint64_t next_sample_time(uint64_t time, uint32_t rate)
   9 {
  10         uint64_t seconds = time / 1000000000;
  11         uint64_t subseconds = time % 1000000000;
  12         uint64_t subsamples = (rate * subseconds + 999999999) / 1000000000;
  13         subseconds = 1000000000 * subsamples / rate;
  14         return seconds * 1000000000 + subseconds;
  15 }
  16
  17 npair<int64_t> widen(npair<short> x)
  18 {
  19         return npair<int64_t>(x.get_x(), x.get_y());
  20 }
  21
  22 sample_number_t narrow(npair<int64_t> x)
  23 {
  24         return sample_number_t((sample_number_t::base_type)(x.get_x()), (sample_number_t::base_type)(x.get_y()));
  25 }
  26
  27 resampler::~resampler()
  28 {
  29 }
  30
  31 resampler_pcm::resampler_pcm(uint32_t rate)
  32 {
  33         written = false;
  34         output_rate = rate;
  35 }
  36
  37 sample_number_t resampler_pcm::nextsample()
  38 {
  39         if(!written)
  40                 return sample_number_t(0, 0);
  41
  42         uint64_t now = next_sample_time(last_read_time + 1, output_rate);
  43         if(last_write_time <= last_read_time) {
  44                 last_read_time = now;
  45                 last_write_time = now;
  46                 accumulator = npair<int64_t>::zero();
  47                 return current_levels;
  48         }
  49
  50         int64_t cover = now - last_read_time;
  51         last_read_time = now;
  52
  53         if(!cover) {
  54                 accumulator = npair<int64_t>::zero();
  55                 return current_levels;
  56         }
  57
  58         uint64_t wcover = now - last_write_time;
  59         accumulator = accumulator + widen(current_levels) * wcover;
  60         last_write_time = now;
  61         npair<int64_t> avg = accumulator / cover;
  62         accumulator = npair<int64_t>::zero();
  63         return narrow(avg);
  64 }
  65
  66 void resampler_pcm::sendpacket(struct packet& p)
  67 {
  68         if(p.rp_major != 1 || p.rp_minor != 1 || p.rp_payload.size() < 4)
  69                 return;         //Wrong type.
  70         int64_t cover = 0;
  71         if(written)
  72                 cover = p.rp_timestamp - last_write_time;
  73         else
  74                 last_read_time = p.rp_timestamp;
  75         written = true;
  76         accumulator = accumulator + cover * widen(current_levels);
  77         unsigned char* pl = &p.rp_payload[0];
  78         current_levels = npair<short>((unsigned short)pl[0] * 256 + (unsigned short)pl[1],
  79                 (unsigned short)pl[2] * 256 + (unsigned short)pl[3]);
  80         last_write_time = p.rp_timestamp;
  81 }
  82
  83 resampler_fm::resampler_fm(uint32_t rate)
  84 {
  85         adlib_init(rate);
  86 }
  87
  88 sample_number_t resampler_fm::nextsample()
  89 {
  90         short samples[2];
  91         adlib_getsample(samples, 1);
  92         return sample_number_t(samples[0], samples[1]);
  93 }
  94
  95 void resampler_fm::sendpacket(struct packet& p)
  96 {
  97         if(p.rp_major != 2 || p.rp_minor < 0)
  98                 return;         //Wrong type.
  99         if(p.rp_minor == 0 || p.rp_minor > 3)
 100                 return;         //Wrong type.
 101         if((p.rp_minor == 1 || p.rp_minor == 2) && p.rp_payload.size() < 2)
 102                 return;         //Malformed.
 103
 104         if(p.rp_minor == 3) {
 105                 //RESET.
 106                 for(int i = 0; i < 512; i++)
 107                         adlib_write(i, 0);
 108                 return;
 109         }
 110
 111         unsigned short reg = p.rp_payload[0];
 112         unsigned char val = p.rp_payload[1];
 113         if(p.rp_minor == 2)
 114                 reg += 256;     //Second set.
 115         adlib_write(reg, val);
 116 }
 117
 118
 119 packet_demux::packet_demux(mixer& mix, uint32_t rate)
 120         : use_mixer(mix)
 121 {
 122         used_rate = rate;
 123         output_filter = new composite_filter();
 124         use_mixer.set_output_filter(*output_filter);
 125 }
 126
 127 packet_demux::~packet_demux()
 128 {
 129         for(std::map<uint32_t, resampler*>::iterator i = resamplers.begin(); i != resamplers.end(); ++i)
 130                 delete i->second;
 131         delete &use_mixer;
 132 }
 133
 134 sample_number_t packet_demux::nextsample()
 135 {
 136         for(std::map<uint32_t, resampler*>::iterator i = resamplers.begin(); i != resamplers.end(); ++i)
 137                 use_mixer.send_sample(i->first, i->second->nextsample());
 138         return use_mixer.recv_sample();
 139 }
 140
 141 void packet_demux::do_volume_change(struct packet& p)
 142 {
 143         filter_number_t::base_type lv, rv;
 144         uint32_t ln = 0, ld = 0, rn = 0, rd = 0;
 145         if(p.rp_payload.size() < 16)
 146                 return;         //Malformed.
 147
 148         ln |= (uint32_t)p.rp_payload[0] << 24;
 149         ln |= (uint32_t)p.rp_payload[1] << 16;
 150         ln |= (uint32_t)p.rp_payload[2] << 8;
 151         ln |= (uint32_t)p.rp_payload[3];
 152         ld |= (uint32_t)p.rp_payload[4] << 24;
 153         ld |= (uint32_t)p.rp_payload[5] << 16;
 154         ld |= (uint32_t)p.rp_payload[6] << 8;
 155         ld |= (uint32_t)p.rp_payload[7];
 156         rn |= (uint32_t)p.rp_payload[8] << 24;
 157         rn |= (uint32_t)p.rp_payload[9] << 16;
 158         rn |= (uint32_t)p.rp_payload[10] << 8;
 159         rn |= (uint32_t)p.rp_payload[11];
 160         rd |= (uint32_t)p.rp_payload[12] << 24;
 161         rd |= (uint32_t)p.rp_payload[13] << 16;
 162         rd |= (uint32_t)p.rp_payload[14] << 8;
 163         rd |= (uint32_t)p.rp_payload[15];
 164
 165         if(!ld || !rd)
 166                 return;         //Malformed.
 167
 168         lv = (filter_number_t::base_type)ln / ld;
 169         rv = (filter_number_t::base_type)rn / rd;
 170         use_mixer.set_channel_volume(p.rp_channel_perm, filter_number_t(lv, rv));
 171 }
 172
 173 void packet_demux::sendpacket(struct packet& p)
 174 {
 175         if(p.rp_major != 1 && p.rp_major != 2)
 176                 return;         //Not interested.
 177         if(p.rp_minor == 0) {
 178                 do_volume_change(p);
 179                 return;         //Volume change.
 180         }
 181         if(!resamplers.count(p.rp_channel_perm)) {
 182                 if(p.rp_major == 1)
 183                         resamplers[p.rp_channel_perm] = new resampler_pcm(used_rate);
 184                 else if(p.rp_major == 2)
 185                         resamplers[p.rp_channel_perm] = new resampler_fm(used_rate);
 186                 if(input_filters.count(p.rp_channel_name))
 187                         use_mixer.set_input_filter(p.rp_channel_perm, *input_filters[p.rp_channel_name]);
 188         }
 189         resamplers[p.rp_channel_perm]->sendpacket(p);
 190 }
 191
 192 std::vector<filter_number_t> process_filter(std::string spec)
 193 {
 194         std::vector<filter_number_t> f;
 195         while(1) {
 196                 if(spec == "")
 197                         throw std::runtime_error("Bad filter specification");
 198                 char* x;
 199                 const char* o = spec.c_str();
 200                 double val = strtod(o, &x);
 201                 if(*x != ',' && *x)
 202                         throw std::runtime_error("Bad filter specification");
 203                 f.push_back(filter_number_t(val, val));
 204                 if(!*x)
 205                         break;
 206                 spec = spec.substr(x + 1 - o);
 207         }
 208         return f;
 209 }
 210
 211 void packet_demux::sendoption(const std::string& option)
 212 {
 213         std::string _option = option;
 214         std::string optionname;
 215         bool on_output = false;
 216         std::string optionchan;
 217         std::string optionvalue;
 218         struct filter* f = NULL;
 219         if(option == "silence") {
 220                 on_output = true;
 221                 f = new silencer();
 222         } else if(isstringprefix(option, "silence=")) {
 223                 optionchan = settingvalue(option);
 224                 f = new silencer();
 225         } else {
 226                 optionvalue = settingvalue(option);
 227                 size_t spos = optionvalue.find_first_of(":");
 228                 if(spos > optionvalue.length())
 229                         on_output = true;
 230                 else {
 231                         optionchan = optionvalue.substr(0, spos);
 232                         optionvalue = optionvalue.substr(spos + 1);
 233                 }
 234                 if(isstringprefix(option, "gain=")) {
 235                         char* x;
 236                         double gdb = strtod(optionvalue.c_str(), &x);
 237                         if(*x)
 238                                 throw std::runtime_error("Bad value");
 239                         f = new amplifier(gdb, AMPLIFIER_GAIN_DB);
 240                 } else if(isstringprefix(option, "attenuate=")) {
 241                         char* x;
 242                         double gdb = strtod(optionvalue.c_str(), &x);
 243                         if(*x)
 244                                 throw std::runtime_error("Bad value");
 245                         f = new amplifier(gdb, AMPLIFIER_ATTENUATION_DB);
 246                 } else if(isstringprefix(option, "filter=")) {
 247                         size_t spos = optionvalue.find_first_of(";");
 248                         if(spos > optionvalue.length())
 249                                 f = new digital_filter(process_filter(optionvalue));
 250                         else
 251                                 f = new digital_filter(process_filter(optionvalue.substr(0, spos)),
 252                                         process_filter(optionvalue.substr(spos + 1)));
 253                 } else
 254                         throw std::runtime_error("Unknown audio processing option");
 255         }
 256
 257         if(on_output) {
 258                 output_filter->add(*f);
 259         } else {
 260                 if(!input_filters.count(optionchan))
 261                         input_filters[optionchan] = new composite_filter();
 262                 input_filters[optionchan]->add(*f);
 263         }
 264 }
 265
 266         composite_filter* output_filter;
 267         std::map<std::string, composite_filter*> input_filters;
 268
 269 /*
 270
 271 --audio-fir=[<channel>:]<a0>,<a1>,...
 272                 Perform FIR filitering (y0 = a0*x0 + a1*x1 + ... + an*xn).
 273 --audio-iir=[<channel>:]<a0>,<a1>,...;<b0>,<b1>,...
 274                 Perform FIR filitering (b0*y0 + b1*y1 + ... bm*ym = a0*x0 + a1*x1 + ... + an*xn).
 275 --audio-attenuate=[<channel>:]<amount>
 276                 Attenuate sound by <amount> decibels.
 277 --audio-gain=[<channel>:]<amount>
 278                 Amplify sound by <amount> decibels.
 279 */
 280
 281 void print_audio_resampler_help(const std::string& prefix)
 282 {
 283         std::cout << prefix << "filter=[<channel>:]<a0>,<a1>,..." << std::endl;
 284         std::cout << "\tPreform FIR filtering (y0 = a0*x0 + a1*x1 + ... + an*xn)." << std::endl;
 285         std::cout << prefix << "filter=[<channel>:]<a0>,<a1>,...;<b0>,<b1>,..." << std::endl;
 286         std::cout << "\tPreform IIR filtering (b0*y0 + b1*y1 + ... bm*ym = a0*x0 + a1*x1 + ... + an*xn)." << std::endl;
 287         std::cout << prefix << "gain=[<channel>:]<decibels>" << std::endl;
 288         std::cout << "\tAmplify signal by <decibels> dB." << std::endl;
 289         std::cout << prefix << "attenuate=[<channel>:]<decibels>" << std::endl;
 290         std::cout << "\tAttenuate signal by <decibels> dB." << std::endl;
 291         std::cout << prefix << "silence" << std::endl;
 292         std::cout << "\tCompletely silence audio output." << std::endl;
 293         std::cout << prefix << "silence=<channel>" << std::endl;
 294         std::cout << "\tCompletely silence audio output on channel." << std::endl;
 295 }
 296
 297 void process_audio_resampler_options(packet_demux& d, const std::string& prefix, int argc, char** argv)
 298 {
 299         for(int i = 1; i < argc; i++) {
 300                 std::string arg = argv[i];
 301                 if(arg == "--")
 302                         break;
 303                 if(!isstringprefix(arg, prefix))
 304                         continue;
 305                 try {
 306                         d.sendoption(arg.substr(prefix.length()));
 307                 } catch(std::exception& e) {
 308                         std::stringstream str;
 309                         str << "Error processing option '" << arg << "': " << e.what();
 310                         throw std::runtime_error(str.str());
 311                 }
 312         }
 313 }