src/sdm.c

   1 /* Sigma-Delta modulator
   2  * Copyright (c) 2015 Mans Rullgard <mans@mansr.com>
   3  *
   4  * This library is free software; you can redistribute it and/or modify it
   5  * 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 (at
   7  * your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser
  12  * 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 library; if not, write to the Free Software Foundation,
  16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  17  */
  18
  19 /*
  20  * References:
  21  *
  22  * Derk Reefman, Erwin Janssen. 2002.
  23  * "Signal processing for Direct Stream Digital: A tutorial for
  24  * digital Sigma Delta modulation and 1-bit digital audio processing"
  25  * http://www.emmlabs.com/pdf/papers/DerkSigmaDelta.pdf
  26  *
  27  * P.J.A. Harpe. 2003.
  28  * "Trellis-type Sigma Delta Modulators for Super Audio CD applications"
  29  * http://www.pieterharpe.nl/docs/report_trunc.pdf
  30  *
  31  * Richard Schreier. 2000-2011.
  32  * "Delta Sigma Toolbox"
  33  * http://www.mathworks.com/matlabcentral/fileexchange/19-delta-sigma-toolbox
  34  */
  35
  36 #define _ISOC11_SOURCE
  37
  38 #include "sox_i.h"
  39 #include "sdm.h"
  40
  41 #define MAX_FILTER_ORDER 8
  42 #define PATH_HASH_SIZE 128
  43 #define PATH_HASH_MASK (PATH_HASH_SIZE - 1)
  44
  45 typedef struct {
  46   const double  a[MAX_FILTER_ORDER];
  47   const double  g[MAX_FILTER_ORDER];
  48   int32_t       order;
  49   double        scale;
  50   const char   *name;
  51   int           trellis_order;
  52   int           trellis_num;
  53   int           trellis_lat;
  54 } LSX_ALIGN(32) sdm_filter_t;
  55
  56 typedef struct sdm_state {
  57   double        state[MAX_FILTER_ORDER];
  58   double        cost;
  59   uint32_t      path;
  60   uint8_t       next;
  61   uint8_t       hist;
  62   uint8_t       hist_used;
  63   struct sdm_state *parent;
  64   struct sdm_state *path_list;
  65 } LSX_ALIGN(32) sdm_state_t;
  66
  67 typedef struct {
  68   sdm_state_t   sdm[2 * SDM_TRELLIS_MAX_NUM];
  69   sdm_state_t  *act[SDM_TRELLIS_MAX_NUM];
  70 } sdm_trellis_t;
  71
  72 struct sdm {
  73   sdm_trellis_t trellis[2];
  74   sdm_state_t  *path_hash[PATH_HASH_SIZE];
  75   uint8_t       hist_free[2 * SDM_TRELLIS_MAX_NUM];
  76   unsigned      hist_fnum;
  77   uint32_t      trellis_mask;
  78   uint32_t      trellis_num;
  79   uint32_t      trellis_lat;
  80   unsigned      num_cands;
  81   unsigned      pos;
  82   unsigned      pending;
  83   unsigned      draining;
  84   unsigned      idx;
  85   const sdm_filter_t *filter;
  86   double        prev_y;
  87   uint64_t      conv_fail;
  88   uint8_t       hist[2 * SDM_TRELLIS_MAX_NUM][SDM_TRELLIS_MAX_LAT / 8];
  89 };
  90
  91 static sdm_filter_t sdm_filter_fast = {
  92   {
  93     8.11979821108649e-01,  3.21578526301959e-01,
  94     8.03842133084308e-02,  1.36652129069769e-02,
  95     1.62614939720868e-03,  1.18730980344801e-04,
  96     5.81753857463105e-06, -4.43443601283455e-08,
  97   },
  98   {
  99     8.10778762576884e-05, 0, 6.65340842513387e-04, 0,
 100     1.52852264942192e-03, 0, 2.22035724073886e-03, 0,
 101   },
 102   8,
 103   0.492,
 104   "fast",
 105   0, 0, 0,
 106 };
 107
 108 static sdm_filter_t sdm_filter_hq = {
 109   {
 110     1.05966158780858e+00, 5.47009636009057e-01,
 111     1.76263553121650e-01, 3.79953988065231e-02,
 112     5.31936695611806e-03, 4.64865473231071e-04,
 113     1.21930947998838e-05,
 114   },
 115   {
 116     0, 3.96825873999969e-04, 0, 1.32436089566069e-03,
 117     0, 2.16898568341885e-03, 0,
 118   },
 119   7,
 120   0.50,
 121   "hq",
 122   16,
 123   10,
 124   1280,
 125 };
 126
 127 static sdm_filter_t sdm_filter_audiophile = {
 128   {
 129     1.17270840974752e+00, 6.69435755948125e-01,
 130     2.38385844332401e-01, 5.67404687000751e-02,
 131     8.79926385368848e-03, 8.47664163271991e-04,
 132     2.69551713329985e-05,
 133   },
 134   {
 135     0, 3.96825873999969e-04, 0, 1.32436089566069e-03,
 136     0, 2.16898568341885e-03, 0,
 137   },
 138   7,
 139   0.50,
 140   "audiophile",
 141   24,
 142   16,
 143   1664,
 144 };
 145
 146 static sdm_filter_t sdm_filter_goldenear = {
 147   {
 148     1.33055162190254e+00, 8.60392723676436e-01,
 149     3.46524494169335e-01, 9.31146164773126e-02,
 150     1.63339758570028e-02, 1.76908163241072e-03,
 151     6.86294038857449e-05,
 152   },
 153   {
 154     0, 3.96825873999969e-04, 0, 1.32436089566069e-03,
 155     0, 2.16898568341885e-03, 0,
 156   },
 157   7,
 158   0.50,
 159   "goldenear",
 160   24,
 161   24,
 162   2048,
 163 };
 164
 165 static const sdm_filter_t *sdm_filters[] = {
 166   &sdm_filter_fast,
 167   &sdm_filter_hq,
 168   &sdm_filter_audiophile,
 169   &sdm_filter_goldenear,
 170   NULL,
 171 };
 172
 173 static const sdm_filter_t *sdm_find_filter(const char *name)
 174 {
 175   int i;
 176
 177   if (!name)
 178     return sdm_filters[0];
 179
 180   for (i = 0; sdm_filters[i]; i++)
 181     if (!strcmp(name, sdm_filters[i]->name))
 182       return sdm_filters[i];
 183
 184   return NULL;
 185 }
 186
 187 #include "sdm_x86.h"
 188
 189 #ifndef sdm_filter_calc
 190 static double sdm_filter_calc(const double *s, double *d,
 191                               const sdm_filter_t *f,
 192                               double x, double y)
 193 {
 194   const double *a = f->a;
 195   const double *g = f->g;
 196   double v;
 197   int i;
 198
 199   d[0] = s[0] - g[0] * s[1] + x - y;
 200   v = x + a[0] * d[0];
 201
 202   for (i = 1; i < f->order - 1; i++) {
 203     d[i] = s[i] + s[i - 1] - g[i] * s[i + 1];
 204     v += a[i] * d[i];
 205   }
 206
 207   d[i] = s[i] + s[i - 1];
 208   v += a[i] * d[i];
 209
 210   return v;
 211 }
 212 #endif
 213
 214 #ifndef sdm_filter_calc2
 215 static void sdm_filter_calc2(sdm_state_t *src, sdm_state_t *dst,
 216                              const sdm_filter_t *f, double x)
 217 {
 218   const double *a = f->a;
 219   double v;
 220   int i;
 221
 222   v = sdm_filter_calc(src->state, dst[0].state, f, x, 0.0);
 223
 224   for (i = 0; i < f->order; i++)
 225     dst[1].state[i] = dst[0].state[i];
 226
 227   dst[0].state[0] += 1.0;
 228   dst[1].state[0] -= 1.0;
 229
 230   dst[0].cost = src->cost + sqr(v + a[0]);
 231   dst[1].cost = src->cost + sqr(v - a[0]);
 232 }
 233 #endif
 234
 235 static inline unsigned sdm_histbuf_get(sdm_t *p)
 236 {
 237   return p->hist_free[--p->hist_fnum];
 238 }
 239
 240 static inline void sdm_histbuf_put(sdm_t *p, unsigned h)
 241 {
 242   p->hist_free[p->hist_fnum++] = h;
 243 }
 244
 245 static inline unsigned get_bit(uint8_t *p, unsigned i)
 246 {
 247   return (p[i >> 3] >> (i & 7)) & 1;
 248 }
 249
 250 static inline void put_bit(uint8_t *p, unsigned i, unsigned v)
 251 {
 252   int b = p[i >> 3];
 253   int s = i & 7;
 254   b &= ~(1 << s);
 255   b |= v << s;
 256   p[i >> 3] = b;
 257 }
 258
 259 static inline unsigned sdm_hist_get(sdm_t *p, unsigned h, unsigned i)
 260 {
 261   return get_bit(p->hist[h], i);
 262 }
 263
 264 static inline void sdm_hist_put(sdm_t *p, unsigned h, unsigned i, unsigned v)
 265 {
 266   put_bit(p->hist[h], i, v);
 267 }
 268
 269 static inline void sdm_hist_copy(sdm_t *p, unsigned d, unsigned s)
 270 {
 271   memcpy(p->hist[d], p->hist[s], (size_t)(p->trellis_lat + 7) / 8);
 272 }
 273
 274 static inline int64_t dbl2int64(double a)
 275 {
 276   union { double d; int64_t i; } v;
 277   v.d = a;
 278   return v.i;
 279 }
 280
 281 static inline int sdm_cmplt(sdm_state_t *a, sdm_state_t *b)
 282 {
 283   return dbl2int64(a->cost) < dbl2int64(b->cost);
 284 }
 285
 286 static inline int sdm_cmple(sdm_state_t *a, sdm_state_t *b)
 287 {
 288   return dbl2int64(a->cost) <= dbl2int64(b->cost);
 289 }
 290
 291 static sdm_state_t *sdm_check_path(sdm_t *p, sdm_state_t *s)
 292 {
 293   unsigned index = s->path & PATH_HASH_MASK;
 294   sdm_state_t **hash = p->path_hash;
 295   sdm_state_t *t = hash[index];
 296
 297   while (t) {
 298     if (t->path == s->path)
 299       return t;
 300     t = t->path_list;
 301   }
 302
 303   s->path_list = hash[index];
 304   hash[index] = s;
 305
 306   return NULL;
 307 }
 308
 309 static unsigned sdm_sort_cands(sdm_t *p, sdm_trellis_t *st)
 310 {
 311   sdm_state_t *r, *s, *t;
 312   sdm_state_t *min;
 313   unsigned i, j, n;
 314
 315   for (i = 0; i < 2 * p->num_cands; i++) {
 316     s = &st->sdm[i];
 317     p->path_hash[s->path & PATH_HASH_MASK] = NULL;
 318     if (!i || sdm_cmplt(s, min))
 319       min = s;
 320   }
 321
 322   for (i = 0, n = 0; i < 2 * p->num_cands; i++) {
 323     s = &st->sdm[i];
 324
 325     if (s->next != min->next)
 326       continue;
 327
 328     if (n == p->trellis_num && sdm_cmple(st->act[n - 1], s))
 329       continue;
 330
 331     t = sdm_check_path(p, s);
 332
 333     if (!t) {
 334       for (j = n; j > 0; j--) {
 335         t = st->act[j - 1];
 336         if (sdm_cmple(t, s))
 337           break;
 338         st->act[j] = t;
 339       }
 340       if (j < p->trellis_num)
 341         st->act[j] = s;
 342       if (n < p->trellis_num)
 343         n++;
 344       continue;
 345     }
 346
 347     if (sdm_cmple(t, s))
 348       continue;
 349
 350     for (j = 0; j < n; j++) {
 351       r = st->act[j];
 352       if (sdm_cmple(s, r))
 353         break;
 354     }
 355
 356     st->act[j++] = s;
 357
 358     while (r != t && j < n) {
 359       sdm_state_t *u = st->act[j];
 360       st->act[j] = r;
 361       r = u;
 362       j++;
 363     }
 364   }
 365
 366   return n;
 367 }
 368
 369 static inline void sdm_step(sdm_t *p, sdm_state_t *cur, sdm_state_t *next,
 370                             double x)
 371 {
 372   const sdm_filter_t *f = p->filter;
 373   int i;
 374
 375   sdm_filter_calc2(cur, next, f, x);
 376
 377   for (i = 0; i < 2; i++) {
 378     next[i].path = (cur->path << 1 | i) & p->trellis_mask;
 379     next[i].hist = cur->hist;
 380     next[i].next = cur->next;
 381     next[i].parent = cur;
 382   }
 383 }
 384
 385 static sox_sample_t sdm_sample_trellis(sdm_t *p, double x)
 386 {
 387   sdm_trellis_t *st_cur = &p->trellis[p->idx];
 388   sdm_trellis_t *st_next = &p->trellis[p->idx ^ 1];
 389   double min_cost;
 390   unsigned new_cands;
 391   unsigned next_pos;
 392   unsigned output;
 393   unsigned i;
 394
 395   next_pos = p->pos + 1;
 396   if (next_pos == p->trellis_lat)
 397     next_pos = 0;
 398
 399   for (i = 0; i < p->num_cands; i++) {
 400     sdm_state_t *cur = st_cur->act[i];
 401     sdm_state_t *next = &st_next->sdm[2 * i];
 402     sdm_step(p, cur, next, x);
 403     cur->next = sdm_hist_get(p, cur->hist, next_pos);
 404     cur->hist_used = 0;
 405   }
 406
 407   new_cands = sdm_sort_cands(p, st_next);
 408   min_cost = st_next->act[0]->cost;
 409   output = st_next->act[0]->next;
 410
 411   for (i = 0; i < new_cands; i++) {
 412     sdm_state_t *s = st_next->act[i];
 413     if (s->parent->hist_used) {
 414       unsigned h = sdm_histbuf_get(p);
 415       sdm_hist_copy(p, h, s->hist);
 416       s->hist = h;
 417     } else {
 418       s->parent->hist_used = 1;
 419     }
 420
 421     s->cost -= min_cost;
 422     s->next = s->parent->next;
 423     sdm_hist_put(p, s->hist, p->pos, s->path & 1);
 424   }
 425
 426   for (i = 0; i < p->num_cands; i++) {
 427     sdm_state_t *s = st_cur->act[i];
 428     if (!s->hist_used)
 429       sdm_histbuf_put(p, s->hist);
 430   }
 431
 432   if (new_cands < p->num_cands)
 433     p->conv_fail++;
 434
 435   p->num_cands = new_cands;
 436   p->pos = next_pos;
 437   p->idx ^= 1;
 438
 439   return output ? SOX_SAMPLE_MAX : -SOX_SAMPLE_MAX;
 440 }
 441
 442 static sox_sample_t sdm_sample(sdm_t *p, double x)
 443 {
 444   const sdm_filter_t *f = p->filter;
 445   double *s0 = p->trellis[0].sdm[p->idx].state;
 446   double *s1 = p->trellis[0].sdm[p->idx ^ 1].state;
 447   double y, v;
 448
 449   v = sdm_filter_calc(s0, s1, f, x, p->prev_y);
 450   y = sign(v);
 451
 452   p->idx ^= 1;
 453   p->prev_y = y;
 454
 455   return y * SOX_SAMPLE_MAX;
 456 }
 457
 458 int sdm_process(sdm_t *p, const sox_sample_t *ibuf, sox_sample_t *obuf,
 459                 size_t *ilen, size_t *olen)
 460 {
 461   sox_sample_t *out = obuf;
 462   size_t len = *ilen = min(*ilen, *olen);
 463   double scale = p->filter->scale;
 464   double x;
 465
 466   if (p->trellis_mask) {
 467     if (p->pending < p->trellis_lat) {
 468       size_t pre = min(p->trellis_lat - p->pending, len);
 469       p->pending += pre;
 470       len -= pre;
 471       while (pre--) {
 472         x = *ibuf++ * scale * (1.0 / SOX_SAMPLE_MAX);
 473         sdm_sample_trellis(p, x);
 474       }
 475     }
 476     while (len--) {
 477       x = *ibuf++ * scale * (1.0 / SOX_SAMPLE_MAX);
 478       *out++ = sdm_sample_trellis(p, x);
 479     }
 480   } else {
 481     while (len--) {
 482       x = *ibuf++ * scale * (1.0 / SOX_SAMPLE_MAX);
 483       *out++ = sdm_sample(p, x);
 484     }
 485   }
 486
 487   *olen = out - obuf;
 488
 489   return SOX_SUCCESS;
 490 }
 491
 492 int sdm_drain(sdm_t *p, sox_sample_t *obuf, size_t *olen)
 493 {
 494   if (p->trellis_mask) {
 495     size_t len = *olen = min(p->pending, *olen);
 496
 497     if (!p->draining && p->pending < p->trellis_lat) {
 498       unsigned flush = p->trellis_lat - p->pending;
 499       while (flush--)
 500         sdm_sample_trellis(p, 0.0);
 501     }
 502
 503     p->draining = 1;
 504     p->pending -= len;
 505
 506     while (len--)
 507       *obuf++ = sdm_sample_trellis(p, 0.0);
 508   } else {
 509     *olen = 0;
 510   }
 511
 512   return SOX_SUCCESS;
 513 }
 514
 515 sdm_t *sdm_init(const char *filter_name,
 516                 unsigned trellis_order,
 517                 unsigned trellis_num,
 518                 unsigned trellis_latency)
 519 {
 520   sdm_t *p;
 521   const sdm_filter_t *f;
 522   sdm_trellis_t *st;
 523   unsigned i;
 524
 525   if (trellis_order > SDM_TRELLIS_MAX_ORDER) {
 526     lsx_fail("trellis order too high (max %d)", SDM_TRELLIS_MAX_ORDER);
 527     return NULL;
 528   }
 529
 530   if (trellis_num > SDM_TRELLIS_MAX_NUM) {
 531     lsx_fail("trellis size too high (max %d)", SDM_TRELLIS_MAX_NUM);
 532     return NULL;
 533   }
 534
 535   if (trellis_latency > SDM_TRELLIS_MAX_LAT) {
 536     lsx_fail("trellis latency too high (max %d)", SDM_TRELLIS_MAX_LAT);
 537     return NULL;
 538   }
 539
 540   p = aligned_alloc((size_t)32, sizeof(*p));
 541   if (!p)
 542     return NULL;
 543
 544   memset(p, 0, sizeof(*p));
 545
 546   p->filter = sdm_find_filter(filter_name);
 547   if (!p->filter) {
 548     lsx_fail("invalid filter name `%s'", filter_name);
 549     return NULL;
 550   }
 551
 552   f = p->filter;
 553   st = &p->trellis[0];
 554
 555   if (trellis_order || f->trellis_order) {
 556     if (trellis_order < 1)
 557       trellis_order = f->trellis_order ? f->trellis_order : 13;
 558
 559     if (trellis_num)
 560       p->trellis_num = trellis_num;
 561     else
 562       p->trellis_num = f->trellis_num ? f->trellis_num : 8;
 563
 564     if (trellis_latency)
 565       p->trellis_lat = trellis_latency;
 566     else
 567       p->trellis_lat = f->trellis_lat ? f->trellis_lat : 1024;
 568
 569     p->trellis_mask = ((uint64_t)1 << trellis_order) - 1;
 570
 571     for (i = 0; i < 2 * p->trellis_num; i++)
 572       sdm_histbuf_put(p, i);
 573
 574     p->num_cands = 1;
 575
 576     st->sdm[0].hist = sdm_histbuf_get(p);
 577     st->sdm[0].path = 0;
 578     st->act[0] = &st->sdm[0];
 579   }
 580
 581   return p;
 582 }
 583
 584 void sdm_close(sdm_t *p)
 585 {
 586   if (p->conv_fail)
 587     lsx_warn("failed to converge %"PRId64" times", p->conv_fail);
 588
 589   aligned_free(p);
 590 }
 591
 592 typedef struct sdm_effect {
 593   sdm_t        *sdm;
 594   const char   *filter_name;
 595   uint32_t      trellis_order;
 596   uint32_t      trellis_num;
 597   uint32_t      trellis_lat;
 598 } sdm_effect_t;
 599
 600 static int getopts(sox_effect_t *effp, int argc, char **argv)
 601 {
 602   sdm_effect_t *p = effp->priv;
 603   lsx_getopt_t optstate;
 604   int c;
 605
 606   lsx_getopt_init(argc, argv, "+f:t:n:l:", NULL, lsx_getopt_flag_none,
 607                   1, &optstate);
 608
 609   while ((c = lsx_getopt(&optstate)) != -1) switch (c) {
 610     case 'f': p->filter_name = optstate.arg; break;
 611     GETOPT_NUMERIC(optstate, 't', trellis_num, 8, SDM_TRELLIS_MAX_ORDER)
 612     GETOPT_NUMERIC(optstate, 'n', trellis_num, 8, SDM_TRELLIS_MAX_NUM)
 613     GETOPT_NUMERIC(optstate, 'l', trellis_lat, 100, SDM_TRELLIS_MAX_LAT)
 614     default: lsx_fail("invalid option `-%c'", optstate.opt); return lsx_usage(effp);
 615   }
 616
 617   return argc != optstate.ind ? lsx_usage(effp) : SOX_SUCCESS;
 618 }
 619
 620 static int start(sox_effect_t *effp)
 621 {
 622   sdm_effect_t *p = effp->priv;
 623
 624   p->sdm = sdm_init(p->filter_name, p->trellis_order,
 625                     p->trellis_num, p->trellis_lat);
 626   if (!p->sdm)
 627     return SOX_EOF;
 628
 629   effp->out_signal.precision = 1;
 630
 631   return SOX_SUCCESS;
 632 }
 633
 634 static int flow(sox_effect_t *effp, const sox_sample_t *ibuf,
 635                 sox_sample_t *obuf, size_t *isamp, size_t *osamp)
 636 {
 637   sdm_effect_t *p = effp->priv;
 638   return sdm_process(p->sdm, ibuf, obuf, isamp, osamp);
 639 }
 640
 641 static int drain(sox_effect_t *effp, sox_sample_t *obuf, size_t *osamp)
 642 {
 643   sdm_effect_t *p = effp->priv;
 644   return sdm_drain(p->sdm, obuf, osamp);
 645 }
 646
 647 static int stop(sox_effect_t *effp)
 648 {
 649   sdm_effect_t *p = effp->priv;
 650   sdm_close(p->sdm);
 651   return SOX_SUCCESS;
 652 }
 653
 654 const sox_effect_handler_t *lsx_sdm_effect_fn(void)
 655 {
 656   static sox_effect_handler_t handler = {
 657     "sdm", "[-f filter] [-t order] [-n num] [-l latency]"
 658     "\n  -f       Set filter to one of: fast, hq, audiophile, goldenear"
 659     "\n           Advanced options:"
 660     "\n  -t       Override trellis order"
 661     "\n  -n       Override number of trellis paths"
 662     "\n  -l       Override trellis latency",
 663     SOX_EFF_PREC, getopts, start, flow, drain, stop, 0, sizeof(sdm_effect_t),
 664   };
 665   return &handler;
 666 }