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