libs/audiographer/private/gdither/gdither.cc

   1 /*
   2  *  Copyright (C) 2002 Steve Harris <steve@plugin.org.uk>
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License, or
   7  *  (at your option) any later version.
   8  *
   9  *  This program is distributed in the hope that it will be useful,
  10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  *  GNU General Public License for more details.
  13  *
  14  *  You should have received a copy of the GNU General Public License
  15  *  along with this program; if not, write to the Free Software
  16  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17  *
  18  */
  19
  20 #include "gdither_types_internal.h"
  21 #include "gdither.h"
  22 #include "noise.h"
  23
  24 /* this monstrosity is necessary to get access to lrintf() and random().
  25    whoever is writing the glibc headers <cmath> and <cstdlib> should be
  26    hauled off to a programmer re-education camp. for the rest of
  27    their natural lives. or longer. <paul@linuxaudiosystems.com>
  28 */
  29
  30 #define _ISOC9X_SOURCE  1
  31 #define _ISOC99_SOURCE  1
  32 #ifdef __cplusplus
  33 #include <cmath>
  34 #else
  35 #include <math.h>
  36 #endif
  37
  38 #undef  __USE_SVID
  39 #define __USE_SVID 1
  40 #ifdef __cplusplus
  41 #include <cstdlib>
  42 #else
  43 #include <stdlib.h>
  44 #endif
  45
  46 #include <assert.h>
  47 #include <sys/types.h>
  48
  49 /* Lipshitz's minimally audible FIR, only really works for 46kHz-ish signals */
  50 static const float shaped_bs[] = { 2.033f, -2.165f, 1.959f, -1.590f, 0.6149f };
  51
  52 /* Some useful constants */
  53 #define MAX_U8        255
  54 #define MIN_U8          0
  55 #define SCALE_U8      128.0f
  56
  57 #define MAX_S16     32767
  58 #define MIN_S16    -32768
  59 #define SCALE_S16   32768.0f
  60
  61 #define MAX_S24   8388607
  62 #define MIN_S24  -8388608
  63 #define SCALE_S24 8388608.0f
  64
  65 GDither gdither_new(GDitherType type, uint32_t channels,
  66
  67                     GDitherSize bit_depth, int dither_depth)
  68 {
  69     GDither s;
  70
  71     s = (GDither)calloc(1, sizeof(struct GDither_s));
  72     s->type = type;
  73     s->channels = channels;
  74     s->bit_depth = (int)bit_depth;
  75
  76     if (dither_depth <= 0 || dither_depth > (int)bit_depth) {
  77         dither_depth = (int)bit_depth;
  78     }
  79     s->dither_depth = dither_depth;
  80
  81     s->scale = (float)(1LL << (dither_depth - 1));
  82     if (bit_depth == GDitherFloat || bit_depth == GDitherDouble) {
  83         s->post_scale_fp = 1.0f / s->scale;
  84         s->post_scale = 0;
  85     } else {
  86         s->post_scale_fp = 0.0f;
  87         s->post_scale = 1 << ((int)bit_depth - dither_depth);
  88     }
  89
  90     switch (bit_depth) {
  91     case GDither8bit:
  92         /* Unsigned 8 bit */
  93         s->bias = 1.0f;
  94         s->clamp_u = 255;
  95         s->clamp_l = 0;
  96         break;
  97     case GDither16bit:
  98         /* Signed 16 bit */
  99         s->bias = 0.0f;
 100         s->clamp_u = 32767;
 101         s->clamp_l = -32768;
 102         break;
 103     case GDither32bit:
 104         /* Signed 24 bit, in upper 24 bits of 32 bit word */
 105         s->bias = 0.0f;
 106         s->clamp_u = 8388607;
 107         s->clamp_l = -8388608;
 108         break;
 109     case GDitherFloat:
 110         /* normalised float */
 111         s->bias = 0.0f;
 112         s->clamp_u = lrintf(s->scale);
 113         s->clamp_l = lrintf(-s->scale);
 114         break;
 115     case GDitherDouble:
 116         /* normalised float */
 117         s->bias = 0.0f;
 118         s->clamp_u = lrintf(s->scale);
 119         s->clamp_l = lrintf(-s->scale);
 120         break;
 121     case GDitherPerformanceTest:
 122         /* special performance test case */
 123         s->scale = SCALE_S24;
 124         s->post_scale = 256;
 125         s->bias = 0.0f;
 126         s->clamp_u = 8388607;
 127         s->clamp_l = -8388608;
 128         break;
 129     default:
 130         /* Not a bit depth we can handle */
 131         free(s);
 132
 133         return NULL;
 134         break;
 135     }
 136
 137     switch (type) {
 138     case GDitherNone:
 139     case GDitherRect:
 140         /* No state */
 141         break;
 142
 143     case GDitherTri:
 144         /* The last whitenoise sample */
 145         s->tri_state = (float *) calloc(channels, sizeof(float));
 146         break;
 147
 148     case GDitherShaped:
 149         /* The error from the last few samples encoded */
 150         s->shaped_state = (GDitherShapedState*)
 151                            calloc(channels, sizeof(GDitherShapedState));
 152         break;
 153     }
 154
 155     return s;
 156 }
 157
 158 void gdither_free(GDither s)
 159 {
 160     if (s) {
 161         free(s->tri_state);
 162         free(s->shaped_state);
 163         free(s);
 164     }
 165 }
 166
 167 inline static void gdither_innner_loop(const GDitherType dt,
 168     const uint32_t stride, const float bias, const float scale,
 169
 170     const uint32_t post_scale, const int bit_depth,
 171     const uint32_t channel, const uint32_t length, float *ts,
 172
 173     GDitherShapedState *ss, float const *x, void *y, const int clamp_u,
 174
 175     const int clamp_l)
 176 {
 177     uint32_t pos, i;
 178     uint8_t *o8 = (uint8_t*) y;
 179     int16_t *o16 = (int16_t*) y;
 180     int32_t *o32 = (int32_t*) y;
 181     float tmp, r, ideal;
 182     int64_t clamped;
 183
 184     i = channel;
 185     for (pos = 0; pos < length; pos++, i += stride) {
 186         tmp = x[i] * scale + bias;
 187
 188         switch (dt) {
 189         case GDitherNone:
 190             break;
 191         case GDitherRect:
 192             tmp -= GDITHER_NOISE;
 193             break;
 194         case GDitherTri:
 195             r = GDITHER_NOISE - 0.5f;
 196             tmp -= r - ts[channel];
 197             ts[channel] = r;
 198             break;
 199         case GDitherShaped:
 200             /* Save raw value for error calculations */
 201             assert (ss);
 202             ideal = tmp;
 203
 204             /* Run FIR and add white noise */
 205             ss->buffer[ss->phase] = GDITHER_NOISE * 0.5f;
 206             tmp += ss->buffer[ss->phase] * shaped_bs[0]
 207                    + ss->buffer[(ss->phase - 1) & GDITHER_SH_BUF_MASK]
 208                      * shaped_bs[1]
 209                    + ss->buffer[(ss->phase - 2) & GDITHER_SH_BUF_MASK]
 210                      * shaped_bs[2]
 211                    + ss->buffer[(ss->phase - 3) & GDITHER_SH_BUF_MASK]
 212                      * shaped_bs[3]
 213                    + ss->buffer[(ss->phase - 4) & GDITHER_SH_BUF_MASK]
 214                      * shaped_bs[4];
 215
 216             /* Roll buffer and store last error */
 217             ss->phase = (ss->phase + 1) & GDITHER_SH_BUF_MASK;
 218             ss->buffer[ss->phase] = (float)lrintf(tmp) - ideal;
 219             break;
 220         }
 221
 222         clamped = lrintf(tmp);
 223         if (clamped > clamp_u) {
 224                 clamped = clamp_u;
 225         } else if (clamped < clamp_l) {
 226                 clamped = clamp_l;
 227         }
 228
 229         switch (bit_depth) {
 230         case GDither8bit:
 231             o8[i] = (uint8_t) (clamped * post_scale);
 232             break;
 233         case GDither16bit:
 234             o16[i] = (int16_t) (clamped * post_scale);
 235             break;
 236         case GDither32bit:
 237             o32[i] = (int32_t) (clamped * post_scale);
 238             break;
 239         }
 240     }
 241 }
 242
 243 /* floating pint version of the inner loop function */
 244 inline static void gdither_innner_loop_fp(const GDitherType dt,
 245     const uint32_t stride, const float bias, const float scale,
 246
 247     const float post_scale, const int bit_depth,
 248     const uint32_t channel, const uint32_t length, float *ts,
 249
 250     GDitherShapedState *ss, float const *x, void *y, const int clamp_u,
 251
 252     const int clamp_l)
 253 {
 254     uint32_t pos, i;
 255     float *oflt = (float*) y;
 256     double *odbl = (double*) y;
 257     float tmp, r, ideal;
 258     double clamped;
 259
 260     i = channel;
 261     for (pos = 0; pos < length; pos++, i += stride) {
 262         tmp = x[i] * scale + bias;
 263
 264         switch (dt) {
 265         case GDitherNone:
 266             break;
 267         case GDitherRect:
 268             tmp -= GDITHER_NOISE;
 269             break;
 270         case GDitherTri:
 271             r = GDITHER_NOISE - 0.5f;
 272             tmp -= r - ts[channel];
 273             ts[channel] = r;
 274             break;
 275         case GDitherShaped:
 276             assert (ss);
 277             /* Save raw value for error calculations */
 278             ideal = tmp;
 279
 280             /* Run FIR and add white noise */
 281             ss->buffer[ss->phase] = GDITHER_NOISE * 0.5f;
 282             tmp += ss->buffer[ss->phase] * shaped_bs[0]
 283                    + ss->buffer[(ss->phase - 1) & GDITHER_SH_BUF_MASK]
 284                      * shaped_bs[1]
 285                    + ss->buffer[(ss->phase - 2) & GDITHER_SH_BUF_MASK]
 286                      * shaped_bs[2]
 287                    + ss->buffer[(ss->phase - 3) & GDITHER_SH_BUF_MASK]
 288                      * shaped_bs[3]
 289                    + ss->buffer[(ss->phase - 4) & GDITHER_SH_BUF_MASK]
 290                      * shaped_bs[4];
 291
 292             /* Roll buffer and store last error */
 293             ss->phase = (ss->phase + 1) & GDITHER_SH_BUF_MASK;
 294             ss->buffer[ss->phase] = (float)lrintf(tmp) - ideal;
 295             break;
 296         }
 297
 298         clamped = (double)lrintf(tmp);
 299         if (clamped > clamp_u) {
 300                 clamped = clamp_u;
 301         } else if (clamped < clamp_l) {
 302                 clamped = clamp_l;
 303         }
 304
 305         switch (bit_depth) {
 306         case GDitherFloat:
 307             oflt[i] = (float) (clamped * post_scale);
 308             break;
 309         case GDitherDouble:
 310             odbl[i] = (double) (clamped * post_scale);
 311             break;
 312         }
 313     }
 314 }
 315
 316 #define GDITHER_CONV_BLOCK 512
 317
 318 void gdither_run(GDither s, uint32_t channel, uint32_t length,
 319                  double const *x, void *y)
 320 {
 321     float conv[GDITHER_CONV_BLOCK];
 322     uint32_t i, pos;
 323     char *ycast = (char *)y;
 324
 325     int step;
 326
 327     switch (s->bit_depth) {
 328     case GDither8bit:
 329         step = 1;
 330         break;
 331     case GDither16bit:
 332         step = 2;
 333         break;
 334     case GDither32bit:
 335     case GDitherFloat:
 336         step = 4;
 337         break;
 338     case GDitherDouble:
 339         step = 8;
 340         break;
 341     default:
 342         step = 0;
 343         break;
 344     }
 345
 346     pos = 0;
 347     while (pos < length) {
 348         for (i=0; (i + pos) < length && i < GDITHER_CONV_BLOCK; i++) {
 349             conv[i] = x[pos + i];
 350         }
 351         gdither_runf(s, channel, i, conv, ycast + s->channels * step);
 352         pos += i;
 353     }
 354 }
 355
 356 void gdither_runf(GDither s, uint32_t channel, uint32_t length,
 357                  float const *x, void *y)
 358 {
 359     uint32_t pos, i;
 360     float tmp;
 361     int64_t clamped;
 362     GDitherShapedState *ss = NULL;
 363
 364     if (!s || channel >= s->channels) {
 365         return;
 366     }
 367
 368     if (s->shaped_state) {
 369         ss = s->shaped_state + channel;
 370     }
 371
 372     if (s->type == GDitherNone && s->bit_depth == 23) {
 373         int32_t *o32 = (int32_t*) y;
 374
 375         for (pos = 0; pos < length; pos++) {
 376             i = channel + (pos * s->channels);
 377             tmp = x[i] * 8388608.0f;
 378
 379             clamped = lrintf(tmp);
 380             if (clamped > 8388607) {
 381                     clamped = 8388607;
 382             } else if (clamped < -8388608) {
 383                     clamped = -8388608;
 384             }
 385
 386             o32[i] = (int32_t) (clamped * 256);
 387         }
 388
 389         return;
 390     }
 391
 392     /* some common case handling code - looks a bit wierd, but it allows
 393      * the compiler to optimise out the branches in the inner loop */
 394     if (s->bit_depth == 8 && s->dither_depth == 8) {
 395         switch (s->type) {
 396         case GDitherNone:
 397             gdither_innner_loop(GDitherNone, s->channels, 128.0f, SCALE_U8,
 398                                 1, 8, channel, length, NULL, NULL, x, y,
 399                                 MAX_U8, MIN_U8);
 400             break;
 401         case GDitherRect:
 402             gdither_innner_loop(GDitherRect, s->channels, 128.0f, SCALE_U8,
 403                                 1, 8, channel, length, NULL, NULL, x, y,
 404                                 MAX_U8, MIN_U8);
 405             break;
 406         case GDitherTri:
 407             gdither_innner_loop(GDitherTri, s->channels, 128.0f, SCALE_U8,
 408                                 1, 8, channel, length, s->tri_state,
 409                                 NULL, x, y, MAX_U8, MIN_U8);
 410             break;
 411         case GDitherShaped:
 412             gdither_innner_loop(GDitherShaped, s->channels, 128.0f, SCALE_U8,
 413                                 1, 8, channel, length, NULL,
 414                                 ss, x, y, MAX_U8, MIN_U8);
 415             break;
 416         }
 417     } else if (s->bit_depth == 16 && s->dither_depth == 16) {
 418         switch (s->type) {
 419         case GDitherNone:
 420             gdither_innner_loop(GDitherNone, s->channels, 0.0f, SCALE_S16,
 421                                 1, 16, channel, length, NULL, NULL, x, y,
 422                                 MAX_S16, MIN_S16);
 423             break;
 424         case GDitherRect:
 425             gdither_innner_loop(GDitherRect, s->channels, 0.0f, SCALE_S16,
 426                                 1, 16, channel, length, NULL, NULL, x, y,
 427                                 MAX_S16, MIN_S16);
 428             break;
 429         case GDitherTri:
 430             gdither_innner_loop(GDitherTri, s->channels, 0.0f, SCALE_S16,
 431                                 1, 16, channel, length, s->tri_state,
 432                                 NULL, x, y, MAX_S16, MIN_S16);
 433             break;
 434         case GDitherShaped:
 435             gdither_innner_loop(GDitherShaped, s->channels, 0.0f,
 436                                 SCALE_S16, 1, 16, channel, length, NULL,
 437                                 ss, x, y, MAX_S16, MIN_S16);
 438             break;
 439         }
 440     } else if (s->bit_depth == 32 && s->dither_depth == 24) {
 441         switch (s->type) {
 442         case GDitherNone:
 443             gdither_innner_loop(GDitherNone, s->channels, 0.0f, SCALE_S24,
 444                                 256, 32, channel, length, NULL, NULL, x,
 445                                 y, MAX_S24, MIN_S24);
 446             break;
 447         case GDitherRect:
 448             gdither_innner_loop(GDitherRect, s->channels, 0.0f, SCALE_S24,
 449                                 256, 32, channel, length, NULL, NULL, x,
 450                                 y, MAX_S24, MIN_S24);
 451             break;
 452         case GDitherTri:
 453             gdither_innner_loop(GDitherTri, s->channels, 0.0f, SCALE_S24,
 454                                 256, 32, channel, length, s->tri_state,
 455                                 NULL, x, y, MAX_S24, MIN_S24);
 456             break;
 457         case GDitherShaped:
 458             gdither_innner_loop(GDitherShaped, s->channels, 0.0f, SCALE_S24,
 459                                 256, 32, channel, length,
 460                                 NULL, ss, x, y, MAX_S24, MIN_S24);
 461             break;
 462         }
 463     } else if (s->bit_depth == GDitherFloat || s->bit_depth == GDitherDouble) {
 464         gdither_innner_loop_fp(s->type, s->channels, s->bias, s->scale,
 465                             s->post_scale_fp, s->bit_depth, channel, length,
 466                             s->tri_state, ss, x, y, s->clamp_u, s->clamp_l);
 467     } else {
 468         /* no special case handling, just process it from the struct */
 469
 470         gdither_innner_loop(s->type, s->channels, s->bias, s->scale,
 471                             s->post_scale, s->bit_depth, channel,
 472                             length, s->tri_state, ss, x, y, s->clamp_u,
 473                             s->clamp_l);
 474     }
 475 }
 476
 477 /* vi:set ts=8 sts=4 sw=4: */