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 }
 132
 133 sample_number_t packet_demux::nextsample()
 134 {
 135         for(std::map<uint32_t, resampler*>::iterator i = resamplers.begin(); i != resamplers.end(); ++i)
 136                 use_mixer.send_sample(i->first, i->second->nextsample());
 137         return use_mixer.recv_sample();
 138 }
 139
 140 void packet_demux::do_volume_change(struct packet& p)
 141 {
 142         filter_number_t::base_type lv, rv;
 143         uint32_t ln = 0, ld = 0, rn = 0, rd = 0;
 144         if(p.rp_payload.size() < 16)
 145                 return;         //Malformed.
 146
 147         ln |= (uint32_t)p.rp_payload[0] << 24;
 148         ln |= (uint32_t)p.rp_payload[1] << 16;
 149         ln |= (uint32_t)p.rp_payload[2] << 8;
 150         ln |= (uint32_t)p.rp_payload[3];
 151         ld |= (uint32_t)p.rp_payload[4] << 24;
 152         ld |= (uint32_t)p.rp_payload[5] << 16;
 153         ld |= (uint32_t)p.rp_payload[6] << 8;
 154         ld |= (uint32_t)p.rp_payload[7];
 155         rn |= (uint32_t)p.rp_payload[8] << 24;
 156         rn |= (uint32_t)p.rp_payload[9] << 16;
 157         rn |= (uint32_t)p.rp_payload[10] << 8;
 158         rn |= (uint32_t)p.rp_payload[11];
 159         rd |= (uint32_t)p.rp_payload[12] << 24;
 160         rd |= (uint32_t)p.rp_payload[13] << 16;
 161         rd |= (uint32_t)p.rp_payload[14] << 8;
 162         rd |= (uint32_t)p.rp_payload[15];
 163
 164         if(!ld || !rd)
 165                 return;         //Malformed.
 166
 167         lv = (filter_number_t::base_type)ln / ld;
 168         rv = (filter_number_t::base_type)rn / rd;
 169         use_mixer.set_channel_volume(p.rp_channel_perm, filter_number_t(lv, rv));
 170 }
 171
 172 void packet_demux::sendpacket(struct packet& p)
 173 {
 174         if(p.rp_major != 1 && p.rp_major != 2)
 175                 return;         //Not interested.
 176         if(p.rp_minor == 0) {
 177                 do_volume_change(p);
 178                 return;         //Volume change.
 179         }
 180         if(!resamplers.count(p.rp_channel_perm)) {
 181                 if(p.rp_major == 1)
 182                         resamplers[p.rp_channel_perm] = new resampler_pcm(used_rate);
 183                 else if(p.rp_major == 2)
 184                         resamplers[p.rp_channel_perm] = new resampler_fm(used_rate);
 185                 if(input_filters.count(p.rp_channel_name))
 186                         use_mixer.set_input_filter(p.rp_channel_perm, *input_filters[p.rp_channel_name]);
 187         }
 188         resamplers[p.rp_channel_perm]->sendpacket(p);
 189 }
 190
 191 std::vector<filter_number_t> process_filter(std::string spec)
 192 {
 193         std::vector<filter_number_t> f;
 194         while(1) {
 195                 if(spec == "")
 196                         throw std::runtime_error("Bad filter specification");
 197                 char* x;
 198                 const char* o = spec.c_str();
 199                 double val = strtod(o, &x);
 200                 if(*x != ',' && *x)
 201                         throw std::runtime_error("Bad filter specification");
 202                 f.push_back(filter_number_t(val, val));
 203                 if(!*x)
 204                         break;
 205                 spec = spec.substr(x + 1 - o);
 206         }
 207         return f;
 208 }
 209
 210 void packet_demux::sendoption(const std::string& option)
 211 {
 212         std::string _option = option;
 213         std::string optionname;
 214         bool on_output = false;
 215         std::string optionchan;
 216         std::string optionvalue;
 217         struct filter* f = NULL;
 218         if(option == "silence") {
 219                 on_output = true;
 220                 f = new silencer();
 221         } else if(isstringprefix(option, "silence=")) {
 222                 optionchan = settingvalue(option);
 223                 f = new silencer();
 224         } else {
 225                 optionvalue = settingvalue(option);
 226                 size_t spos = optionvalue.find_first_of(":");
 227                 if(spos > optionvalue.length())
 228                         on_output = true;
 229                 else {
 230                         optionchan = optionvalue.substr(0, spos);
 231                         optionvalue = optionvalue.substr(spos + 1);
 232                 }
 233                 if(isstringprefix(option, "gain=")) {
 234                         char* x;
 235                         double gdb = strtod(optionvalue.c_str(), &x);
 236                         if(*x)
 237                                 throw std::runtime_error("Bad value");
 238                         f = new amplifier(gdb, AMPLIFIER_GAIN_DB);
 239                 } else if(isstringprefix(option, "attenuate=")) {
 240                         char* x;
 241                         double gdb = strtod(optionvalue.c_str(), &x);
 242                         if(*x)
 243                                 throw std::runtime_error("Bad value");
 244                         f = new amplifier(gdb, AMPLIFIER_ATTENUATION_DB);
 245                 } else if(isstringprefix(option, "filter=")) {
 246                         size_t spos = optionvalue.find_first_of(";");
 247                         if(spos > optionvalue.length())
 248                                 f = new digital_filter(process_filter(optionvalue));
 249                         else
 250                                 f = new digital_filter(process_filter(optionvalue.substr(0, spos)),
 251                                         process_filter(optionvalue.substr(spos + 1)));
 252                 } else
 253                         throw std::runtime_error("Unknown audio processing option");
 254         }
 255
 256         if(on_output) {
 257                 output_filter->add(*f);
 258         } else {
 259                 if(!input_filters.count(optionchan))
 260                         input_filters[optionchan] = new composite_filter();
 261                 input_filters[optionchan]->add(*f);
 262         }
 263 }
 264
 265         composite_filter* output_filter;
 266         std::map<std::string, composite_filter*> input_filters;
 267
 268 /*
 269
 270 --audio-fir=[<channel>:]<a0>,<a1>,...
 271                 Perform FIR filitering (y0 = a0*x0 + a1*x1 + ... + an*xn).
 272 --audio-iir=[<channel>:]<a0>,<a1>,...;<b0>,<b1>,...
 273                 Perform FIR filitering (b0*y0 + b1*y1 + ... bm*ym = a0*x0 + a1*x1 + ... + an*xn).
 274 --audio-attenuate=[<channel>:]<amount>
 275                 Attenuate sound by <amount> decibels.
 276 --audio-gain=[<channel>:]<amount>
 277                 Amplify sound by <amount> decibels.
 278 */
 279
 280 void print_audio_resampler_help(const std::string& prefix)
 281 {
 282         std::cout << prefix << "filter=[<channel>:]<a0>,<a1>,..." << std::endl;
 283         std::cout << "\tPreform FIR filtering (y0 = a0*x0 + a1*x1 + ... + an*xn)." << std::endl;
 284         std::cout << prefix << "filter=[<channel>:]<a0>,<a1>,...;<b0>,<b1>,..." << std::endl;
 285         std::cout << "\tPreform IIR filtering (b0*y0 + b1*y1 + ... bm*ym = a0*x0 + a1*x1 + ... + an*xn)." << std::endl;
 286         std::cout << prefix << "gain=[<channel>:]<decibels>" << std::endl;
 287         std::cout << "\tAmplify signal by <decibels> dB." << std::endl;
 288         std::cout << prefix << "attenuate=[<channel>:]<decibels>" << std::endl;
 289         std::cout << "\tAttenuate signal by <decibels> dB." << std::endl;
 290         std::cout << prefix << "silence" << std::endl;
 291         std::cout << "\tCompletely silence audio output." << std::endl;
 292         std::cout << prefix << "silence=<channel>" << std::endl;
 293         std::cout << "\tCompletely silence audio output on channel." << std::endl;
 294 }
 295
 296 void process_audio_resampler_options(packet_demux& d, const std::string& prefix, int argc, char** argv)
 297 {
 298         for(int i = 1; i < argc; i++) {
 299                 std::string arg = argv[i];
 300                 if(arg == "--")
 301                         break;
 302                 if(!isstringprefix(arg, prefix))
 303                         continue;
 304                 try {
 305                         d.sendoption(arg.substr(prefix.length()));
 306                 } catch(std::exception& e) {
 307                         std::stringstream str;
 308                         str << "Error processing option '" << arg << "': " << e.what();
 309                         throw std::runtime_error(str.str());
 310                 }
 311         }
 312 }