z80/ym2612 reset handling improved
[picodrive.git] / Pico / Pico / xpcm.c
1 /*
2  * The following ADPCM algorithm was stolen from MAME aica driver.
3  * I'm quite sure it's not the right one, but it's the
4  * best sounding of the ones that I tried.
5  */
6
7 #include "../PicoInt.h"
8
9 #define ADPCMSHIFT      8
10 #define ADFIX(f)        (int) ((double)f * (double)(1<<ADPCMSHIFT))
11
12 /* limitter */
13 #define Limit(val, max, min) { \
14         if ( val > max )      val = max; \
15         else if ( val < min ) val = min; \
16 }
17
18 static const int TableQuant[8] =
19 {
20   ADFIX(0.8984375),
21   ADFIX(0.8984375),
22   ADFIX(0.8984375),
23   ADFIX(0.8984375),
24   ADFIX(1.19921875),
25   ADFIX(1.59765625),
26   ADFIX(2.0),
27   ADFIX(2.3984375)
28 };
29
30 // changed using trial and error..
31 //static const int quant_mul[16] = { 1, 3, 5, 7, 9, 11, 13, 15, -1, -3, -5, -7, -9, -11, -13, -15 };
32 static const int quant_mul[16]   = { 1, 3, 5, 7, 9, 11, 13, -1, -1, -3, -5, -7, -9, -11, -13, -15 };
33
34 static int sample = 0, quant = 0, sgn = 0;
35 static int stepsamples = (44100<<10)/16000;
36
37
38 PICO_INTERNAL void PicoPicoPCMReset(void)
39 {
40   sample = sgn = 0;
41   quant = 0x7f;
42   memset(PicoPicohw.xpcm_buffer, 0, sizeof(PicoPicohw.xpcm_buffer));
43 }
44
45 PICO_INTERNAL void PicoPicoPCMRerate(int xpcm_rate)
46 {
47   stepsamples = (PsndRate<<10)/xpcm_rate;
48 }
49
50 #define XSHIFT 6
51
52 #define do_sample() \
53 { \
54   int delta = quant * quant_mul[srcval] >> XSHIFT; \
55   sample += delta - (delta >> 2); /* 3/4 */ \
56   quant = (quant * TableQuant[srcval&7]) >> ADPCMSHIFT; \
57   Limit(quant, 0x6000, 0x7f); \
58   Limit(sample, 32767*3/4, -32768*3/4); \
59 }
60
61 PICO_INTERNAL void PicoPicoPCMUpdate(short *buffer, int length, int stereo)
62 {
63   unsigned char *src = PicoPicohw.xpcm_buffer;
64   unsigned char *lim = PicoPicohw.xpcm_ptr;
65   int srcval, needsamples = 0;
66
67   if (src == lim) goto end;
68
69   for (; length > 0 && src < lim; src++)
70   {
71     srcval = *src >> 4;
72     do_sample();
73
74     for (needsamples += stepsamples; needsamples > (1<<10) && length > 0; needsamples -= (1<<10), length--) {
75       *buffer++ += sample;
76       if (stereo) { buffer[0] = buffer[-1]; buffer++; }
77     }
78
79     srcval = *src & 0xf;
80     do_sample();
81
82     for (needsamples += stepsamples; needsamples > (1<<10) && length > 0; needsamples -= (1<<10), length--) {
83       *buffer++ += sample;
84       if (stereo) { buffer[0] = buffer[-1]; buffer++; }
85     }
86
87     // lame normalization stuff, needed due to wrong adpcm algo
88     sgn += (sample < 0) ? -1 : 1;
89     if (sgn < -16 || sgn > 16) sample -= sample >> 5;
90   }
91
92   if (src < lim) {
93     int di = lim - src;
94     memmove(PicoPicohw.xpcm_buffer, src, di);
95     PicoPicohw.xpcm_ptr = PicoPicohw.xpcm_buffer + di;
96     elprintf(EL_PICOHW, "xpcm update: over %i", di);
97     // adjust fifo
98     PicoPicohw.fifo_bytes = di;
99     return;
100   }
101
102   elprintf(EL_PICOHW, "xpcm update: under %i", length);
103   PicoPicohw.xpcm_ptr = PicoPicohw.xpcm_buffer;
104
105 end:
106   if (stereo)
107     // still must expand SN76496 to stereo
108     for (; length > 0; buffer+=2, length--)
109       buffer[1] = buffer[0];
110
111   sample = sgn = 0;
112   quant = 0x7f;
113 }
114