/*
* Copyright (C) 2002 Steve Harris <steve@plugin.org.uk>
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*
* $Id: dither.cc,v 1.3 2004/02/22 23:04:00 taybin Exp $
*/
#include "gdither_types_internal.h"
#include "gdither.h"
#include "noise.h"
/* this monstrosity is necessary to get access to lrintf() and random().
whoever is writing the glibc headers <cmath> and <cstdlib> should be
hauled off to a programmer re-education camp. for the rest of
their natural lives. or longer. <paul@linuxaudiosystems.com>
*/
#define _ISOC9X_SOURCE 1
#define _ISOC99_SOURCE 1
#ifdef __cplusplus
#include <cmath>
#else
#include <math.h>
#endif
#undef __USE_SVID
#define __USE_SVID 1
#ifdef __cplusplus
#include <cstdlib>
#else
#include <stdlib.h>
#endif
/* and even then sometimes it doesn't work... */
long lrintf(float);
#include <sys/types.h>
/* Lipshitz's minimally audible FIR, only really works for 46kHz-ish signals */
static const float shaped_bs[] = { 2.033f, -2.165f, 1.959f, -1.590f, 0.6149f };
// 2nd order: static const float shaped_bs[] = { 1.652f, -1.049, 0.1382 };
/* Some useful constants */
#define MAX_U8 255
#define MIN_U8 0
#define SCALE_U8 128.0f
#define MAX_S16 32767
#define MIN_S16 -32768
#define SCALE_S16 32768.0f
#define MAX_S24 8388607
#define MIN_S24 -8388608
#define SCALE_S24 8388608.0f
GDither gdither_new(GDitherType type, unsigned int channels,
GDitherSize bit_depth, int dither_depth)
{
GDither s;
s = (GDither)calloc(1, sizeof(struct GDither_s));
s->type = type;
s->channels = channels;
s->bit_depth = (int)bit_depth;
if (dither_depth <= 0 || dither_depth > (int)bit_depth) {
dither_depth = (int)bit_depth;
}
s->dither_depth = dither_depth;
s->scale = (float)(1LL << (dither_depth - 1));
if (bit_depth == GDitherFloat || bit_depth == GDitherDouble) {
s->post_scale_fp = 1.0f / s->scale;
s->post_scale = 0;
} else {
s->post_scale_fp = 0.0f;
s->post_scale = 1 << ((int)bit_depth - dither_depth);
}
switch (bit_depth) {
case GDither8bit:
/* Unsigned 8 bit */
s->bias = 1.0f;
s->clamp_u = 255;
s->clamp_l = 0;
break;
case GDither16bit:
/* Signed 16 bit */
s->bias = 0.0f;
s->clamp_u = 32767;
s->clamp_l = -32768;
break;
case GDither32bit:
/* Signed 24 bit, in upper 24 bits of 32 bit word */
s->bias = 0.0f;
s->clamp_u = 8388607;
s->clamp_l = -8388608;
break;
case GDitherFloat:
/* normalised float */
s->bias = 0.0f;
s->clamp_u = lrintf(s->scale);
s->clamp_l = lrintf(-s->scale);
break;
case GDitherDouble:
/* normalised float */
s->bias = 0.0f;
s->clamp_u = lrintf(s->scale);
s->clamp_l = lrintf(-s->scale);
break;
case 23:
/* special performance test case */
s->scale = SCALE_S24;
s->post_scale = 256;
s->bias = 0.0f;
s->clamp_u = 8388607;
s->clamp_l = -8388608;
break;
default:
/* Not a bit depth we can handle */
free(s);
return NULL;
break;
}
switch (type) {
case GDitherNone:
case GDitherRect:
/* No state */
break;
case GDitherTri:
/* The last whitenoise sample */
s->tri_state = (float *) calloc(channels, sizeof(float));
break;
case GDitherShaped:
/* The error from the last few samples encoded */
s->shaped_state = (GDitherShapedState*)
calloc(channels, sizeof(GDitherShapedState));
break;
}
return s;
}
void gdither_free(GDither s)
{
if (s) {
free(s->tri_state);
free(s->shaped_state);
free(s);
}
}
inline static void gdither_innner_loop(const GDitherType dt,
const unsigned int stride, const float bias, const float scale,
const unsigned int post_scale, const int bit_depth,
const unsigned int channel, const unsigned int length, float *ts,
GDitherShapedState *ss, float *x, void *y, const int clamp_u,
const int clamp_l)
{
unsigned int pos, i;
u_int8_t *o8 = (u_int8_t*) y;
int16_t *o16 = (int16_t*) y;
int32_t *o32 = (int32_t*) y;
float tmp, r, ideal;
int64_t clamped;
i = channel;
for (pos = 0; pos < length; pos++, i += stride) {
tmp = x[pos] * scale + bias;
switch (dt) {
case GDitherNone:
break;
case GDitherRect:
tmp -= GDITHER_NOISE;
break;
case GDitherTri:
r = GDITHER_NOISE - 0.5f;
tmp -= r - ts[channel];
ts[channel] = r;
break;
case GDitherShaped:
/* Save raw value for error calculations */
ideal = tmp;
/* Run FIR and add white noise */
ss->buffer[ss->phase] = GDITHER_NOISE * 0.5f;
tmp += ss->buffer[ss->phase] * shaped_bs[0]
+ ss->buffer[(ss->phase - 1) & GDITHER_SH_BUF_MASK]
* shaped_bs[1]
+ ss->buffer[(ss->phase - 2) & GDITHER_SH_BUF_MASK]
* shaped_bs[2]
+ ss->buffer[(ss->phase - 3) & GDITHER_SH_BUF_MASK]
* shaped_bs[3]
+ ss->buffer[(ss->phase - 4) & GDITHER_SH_BUF_MASK]
* shaped_bs[4];
/* Roll buffer and store last error */
ss->phase = (ss->phase + 1) & GDITHER_SH_BUF_MASK;
ss->buffer[ss->phase] = (float)lrintf(tmp) - ideal;
break;
}
clamped = lrintf(tmp);
if (clamped > clamp_u) {
clamped = clamp_u;
} else if (clamped < clamp_l) {
clamped = clamp_l;
}
switch (bit_depth) {
case GDither8bit:
o8[i] = (u_int8_t) (clamped * post_scale);
break;
case GDither16bit:
o16[i] = (int16_t) (clamped * post_scale);
break;
case GDither32bit:
o32[i] = (int32_t) (clamped * post_scale);
break;
}
}
}
/* floating pint version of the inner loop function */
inline static void gdither_innner_loop_fp(const GDitherType dt,
const unsigned int stride, const float bias, const float scale,
const float post_scale, const int bit_depth,
const unsigned int channel, const unsigned int length, float *ts,
GDitherShapedState *ss, float *x, void *y, const int clamp_u,
const int clamp_l)
{
unsigned int pos, i;
float *oflt = (float*) y;
double *odbl = (double*) y;
float tmp, r, ideal;
double clamped;
i = channel;
for (pos = 0; pos < length; pos++, i += stride) {
tmp = x[i] * scale + bias;
switch (dt) {
case GDitherNone:
break;
case GDitherRect:
tmp -= GDITHER_NOISE;
break;
case GDitherTri:
r = GDITHER_NOISE - 0.5f;
tmp -= r - ts[channel];
ts[channel] = r;
break;
case GDitherShaped:
/* Save raw value for error calculations */
ideal = tmp;
/* Run FIR and add white noise */
ss->buffer[ss->phase] = GDITHER_NOISE * 0.5f;
tmp += ss->buffer[ss->phase] * shaped_bs[0]
+ ss->buffer[(ss->phase - 1) & GDITHER_SH_BUF_MASK]
* shaped_bs[1]
+ ss->buffer[(ss->phase - 2) & GDITHER_SH_BUF_MASK]
* shaped_bs[2]
+ ss->buffer[(ss->phase - 3) & GDITHER_SH_BUF_MASK]
* shaped_bs[3]
+ ss->buffer[(ss->phase - 4) & GDITHER_SH_BUF_MASK]
* shaped_bs[4];
/* Roll buffer and store last error */
ss->phase = (ss->phase + 1) & GDITHER_SH_BUF_MASK;
ss->buffer[ss->phase] = (float)lrintf(tmp) - ideal;
break;
}
clamped = rintf(tmp);
if (clamped > clamp_u) {
clamped = clamp_u;
} else if (clamped < clamp_l) {
clamped = clamp_l;
}
switch (bit_depth) {
case GDitherFloat:
oflt[i] = (float) (clamped * post_scale);
break;
case GDitherDouble:
odbl[i] = (double) (clamped * post_scale);
break;
}
}
}
#define GDITHER_CONV_BLOCK 512
void gdither_run(GDither s, unsigned int channel, unsigned int length,
double *x, void *y)
{
float conv[GDITHER_CONV_BLOCK];
unsigned int i, pos;
char *ycast = (char *)y;
int step;
switch (s->bit_depth) {
case GDither8bit:
step = 1;
break;
case GDither16bit:
step = 2;
break;
case GDither32bit:
case GDitherFloat:
step = 4;
break;
case GDitherDouble:
step = 8;
break;
default:
step = 0;
break;
}
pos = 0;
while (pos < length) {
for (i=0; (i + pos) < length && i < GDITHER_CONV_BLOCK; i++) {
conv[i] = x[pos + i];
}
gdither_runf(s, channel, i, conv, ycast + pos * step);
pos += i;
}
}
void gdither_runf(GDither s, unsigned int channel, unsigned int length,
float *x, void *y)
{
unsigned int pos, i;
float tmp;
int64_t clamped;
GDitherShapedState *ss = NULL;
if (!s || channel >= s->channels) {
return;
}
if (s->shaped_state) {
ss = s->shaped_state + channel;
}
if (s->type == GDitherNone && s->bit_depth == 23) {
int32_t *o32 = (int32_t*) y;
for (pos = 0; pos < length; pos++) {
i = channel + (pos * s->channels);
tmp = x[i] * 8388608.0f;
clamped = lrintf(tmp);
if (clamped > 8388607) {
clamped = 8388607;
} else if (clamped < -8388608) {
clamped = -8388608;
}
o32[i] = (int32_t) (clamped * 256);
}
return;
}
/* some common case handling code - looks a bit wierd, but it allows
* the compiler to optiomise out the branches in the inner loop */
if (s->bit_depth == 8 && s->dither_depth == 8) {
switch (s->type) {
case GDitherNone:
gdither_innner_loop(GDitherNone, s->channels, 128.0f, SCALE_U8,
1, 8, channel, length, NULL, NULL, x, y,
MAX_U8, MIN_U8);
break;
case GDitherRect:
gdither_innner_loop(GDitherRect, s->channels, 128.0f, SCALE_U8,
1, 8, channel, length, NULL, NULL, x, y,
MAX_U8, MIN_U8);
break;
case GDitherTri:
gdither_innner_loop(GDitherTri, s->channels, 128.0f, SCALE_U8,
1, 8, channel, length, s->tri_state,
NULL, x, y, MAX_U8, MIN_U8);
break;
case GDitherShaped:
gdither_innner_loop(GDitherShaped, s->channels, 128.0f, SCALE_U8,
1, 8, channel, length, NULL,
ss, x, y, MAX_U8, MIN_U8);
break;
}
} else if (s->bit_depth == 16 && s->dither_depth == 16) {
switch (s->type) {
case GDitherNone:
gdither_innner_loop(GDitherNone, s->channels, 0.0f, SCALE_S16,
1, 16, channel, length, NULL, NULL, x, y,
MAX_S16, MIN_S16);
break;
case GDitherRect:
gdither_innner_loop(GDitherRect, s->channels, 0.0f, SCALE_S16,
1, 16, channel, length, NULL, NULL, x, y,
MAX_S16, MIN_S16);
break;
case GDitherTri:
gdither_innner_loop(GDitherTri, s->channels, 0.0f, SCALE_S16,
1, 16, channel, length, s->tri_state,
NULL, x, y, MAX_S16, MIN_S16);
break;
case GDitherShaped:
gdither_innner_loop(GDitherShaped, s->channels, 0.0f,
SCALE_S16, 1, 16, channel, length, NULL,
ss, x, y, MAX_S16, MIN_S16);
break;
}
} else if (s->bit_depth == 32 && s->dither_depth == 24) {
switch (s->type) {
case GDitherNone:
gdither_innner_loop(GDitherNone, s->channels, 0.0f, SCALE_S24,
256, 32, channel, length, NULL, NULL, x,
y, MAX_S24, MIN_S24);
break;
case GDitherRect:
gdither_innner_loop(GDitherRect, s->channels, 0.0f, SCALE_S24,
256, 32, channel, length, NULL, NULL, x,
y, MAX_S24, MIN_S24);
break;
case GDitherTri:
gdither_innner_loop(GDitherTri, s->channels, 0.0f, SCALE_S24,
256, 32, channel, length, s->tri_state,
NULL, x, y, MAX_S24, MIN_S24);
break;
case GDitherShaped:
gdither_innner_loop(GDitherShaped, s->channels, 0.0f, SCALE_S24,
256, 32, channel, length,
NULL, ss, x, y, MAX_S24, MIN_S24);
break;
}
} else if (s->bit_depth == GDitherFloat || s->bit_depth == GDitherDouble) {
gdither_innner_loop_fp(s->type, s->channels, s->bias, s->scale,
s->post_scale_fp, s->bit_depth, channel, length,
s->tri_state, ss, x, y, s->clamp_u, s->clamp_l);
} else {
/* no special case handling, just process it from the struct */
gdither_innner_loop(s->type, s->channels, s->bias, s->scale,
s->post_scale, s->bit_depth, channel,
length, s->tri_state, ss, x, y, s->clamp_u,
s->clamp_l);
}
}
/* vi:set ts=8 sts=4 sw=4: */