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)
  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 && p.rp_major != 6)
 176                 return;         //Not interested.
 177         if(p.rp_minor == 0 && p.rp_major != 6) {
 178                 do_volume_change(p);
 179                 return;         //Volume change.
 180         }
 181         if(!resamplers.count(p.rp_channel_perm)) {
 182                 std::cerr << "Create channel of type " << p.rp_major << "." << std::endl;
 183                 if(p.rp_major == 1)
 184                         resamplers[p.rp_channel_perm] = new resampler_pcm(used_rate);
 185                 else if(p.rp_major == 2)
 186                         resamplers[p.rp_channel_perm] = new resampler_fm(used_rate);
 187                 if(input_filters.count(p.rp_channel_name))
 188                         use_mixer.set_input_filter(p.rp_channel_perm, *input_filters[p.rp_channel_name]);
 189         }
 190         resamplers[p.rp_channel_perm]->sendpacket(p);
 191 }
 192
 193 std::vector<filter_number_t> process_filter(std::string spec)
 194 {
 195         std::vector<filter_number_t> f;
 196         while(1) {
 197                 if(spec == "")
 198                         throw std::runtime_error("Bad filter specification");
 199                 char* x;
 200                 const char* o = spec.c_str();
 201                 double val = strtod(o, &x);
 202                 if(*x != ',' && *x)
 203                         throw std::runtime_error("Bad filter specification");
 204                 f.push_back(filter_number_t(val, val));
 205                 if(!*x)
 206                         break;
 207                 spec = spec.substr(x + 1 - o);
 208         }
 209         return f;
 210 }
 211
 212 void packet_demux::sendoption(const std::string& option)
 213 {
 214         std::string _option = option;
 215         std::string optionname;
 216         bool on_output = false;
 217         std::string optionchan;
 218         std::string optionvalue;
 219         struct filter* f = NULL;
 220         if(option == "silence") {
 221                 on_output = true;
 222                 f = new silencer();
 223         } else if(isstringprefix(option, "silence=")) {
 224                 optionchan = settingvalue(option);
 225                 f = new silencer();
 226         } else {
 227                 optionvalue = settingvalue(option);
 228                 size_t spos = optionvalue.find_first_of(":");
 229                 if(spos > optionvalue.length())
 230                         on_output = true;
 231                 else {
 232                         optionchan = optionvalue.substr(0, spos);
 233                         optionvalue = optionvalue.substr(spos + 1);
 234                 }
 235                 if(isstringprefix(option, "gain=")) {
 236                         char* x;
 237                         double gdb = strtod(optionvalue.c_str(), &x);
 238                         if(*x)
 239                                 throw std::runtime_error("Bad value");
 240                         f = new amplifier(gdb, AMPLIFIER_GAIN_DB);
 241                 } else if(isstringprefix(option, "attenuate=")) {
 242                         char* x;
 243                         double gdb = strtod(optionvalue.c_str(), &x);
 244                         if(*x)
 245                                 throw std::runtime_error("Bad value");
 246                         f = new amplifier(gdb, AMPLIFIER_ATTENUATION_DB);
 247                 } else if(isstringprefix(option, "filter=")) {
 248                         size_t spos = optionvalue.find_first_of(";");
 249                         if(spos > optionvalue.length())
 250                                 f = new digital_filter(process_filter(optionvalue));
 251                         else
 252                                 f = new digital_filter(process_filter(optionvalue.substr(0, spos)),
 253                                         process_filter(optionvalue.substr(spos + 1)));
 254                 } else
 255                         throw std::runtime_error("Unknown audio processing option");
 256         }
 257
 258         if(on_output) {
 259                 output_filter->add(*f);
 260         } else {
 261                 if(!input_filters.count(optionchan))
 262                         input_filters[optionchan] = new composite_filter();
 263                 input_filters[optionchan]->add(*f);
 264         }
 265 }
 266
 267         composite_filter* output_filter;
 268         std::map<std::string, composite_filter*> input_filters;
 269
 270 /*
 271
 272 --audio-fir=[<channel>:]<a0>,<a1>,...
 273                 Perform FIR filitering (y0 = a0*x0 + a1*x1 + ... + an*xn).
 274 --audio-iir=[<channel>:]<a0>,<a1>,...;<b0>,<b1>,...
 275                 Perform FIR filitering (b0*y0 + b1*y1 + ... bm*ym = a0*x0 + a1*x1 + ... + an*xn).
 276 --audio-attenuate=[<channel>:]<amount>
 277                 Attenuate sound by <amount> decibels.
 278 --audio-gain=[<channel>:]<amount>
 279                 Amplify sound by <amount> decibels.
 280 */
 281
 282 void print_audio_resampler_help(const std::string& prefix)
 283 {
 284         std::cout << prefix << "filter=[<channel>:]<a0>,<a1>,..." << std::endl;
 285         std::cout << "\tPreform FIR filtering (y0 = a0*x0 + a1*x1 + ... + an*xn)." << std::endl;
 286         std::cout << prefix << "filter=[<channel>:]<a0>,<a1>,...;<b0>,<b1>,..." << std::endl;
 287         std::cout << "\tPreform IIR filtering (b0*y0 + b1*y1 + ... bm*ym = a0*x0 + a1*x1 + ... + an*xn)." << std::endl;
 288         std::cout << prefix << "gain=[<channel>:]<decibels>" << std::endl;
 289         std::cout << "\tAmplify signal by <decibels> dB." << std::endl;
 290         std::cout << prefix << "attenuate=[<channel>:]<decibels>" << std::endl;
 291         std::cout << "\tAttenuate signal by <decibels> dB." << std::endl;
 292         std::cout << prefix << "silence" << std::endl;
 293         std::cout << "\tCompletely silence audio output." << std::endl;
 294         std::cout << prefix << "silence=<channel>" << std::endl;
 295         std::cout << "\tCompletely silence audio output on channel." << std::endl;
 296 }
 297
 298 void process_audio_resampler_options(packet_demux& d, const std::string& prefix, int argc, char** argv)
 299 {
 300         for(int i = 1; i < argc; i++) {
 301                 std::string arg = argv[i];
 302                 if(arg == "--")
 303                         break;
 304                 if(!isstringprefix(arg, prefix))
 305                         continue;
 306                 try {
 307                         d.sendoption(arg.substr(prefix.length()));
 308                 } catch(std::exception& e) {
 309                         std::stringstream str;
 310                         str << "Error processing option '" << arg << "': " << e.what();
 311                         throw std::runtime_error(str.str());
 312                 }
 313         }
 314 }