g722_decode.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399
  1. /*
  2. * SpanDSP - a series of DSP components for telephony
  3. *
  4. * g722_decode.c - The ITU G.722 codec, decode part.
  5. *
  6. * Written by Steve Underwood <steveu@coppice.org>
  7. *
  8. * Copyright (C) 2005 Steve Underwood
  9. *
  10. * Despite my general liking of the GPL, I place my own contributions
  11. * to this code in the public domain for the benefit of all mankind -
  12. * even the slimy ones who might try to proprietize my work and use it
  13. * to my detriment.
  14. *
  15. * Based in part on a single channel G.722 codec which is:
  16. *
  17. * Copyright (c) CMU 1993
  18. * Computer Science, Speech Group
  19. * Chengxiang Lu and Alex Hauptmann
  20. *
  21. * $Id$
  22. */
  23. /*! \file */
  24. #ifdef HAVE_CONFIG_H
  25. #include <config.h>
  26. #endif
  27. #include <stdio.h>
  28. #include <inttypes.h>
  29. #include <memory.h>
  30. #include <stdlib.h>
  31. #if 0
  32. #include <tgmath.h>
  33. #endif
  34. #include "g722.h"
  35. #if !defined(FALSE)
  36. #define FALSE 0
  37. #endif
  38. #if !defined(TRUE)
  39. #define TRUE (!FALSE)
  40. #endif
  41. static __inline__ int16_t saturate(int32_t amp)
  42. {
  43. int16_t amp16;
  44. /* Hopefully this is optimised for the common case - not clipping */
  45. amp16 = (int16_t) amp;
  46. if (amp == amp16)
  47. return amp16;
  48. if (amp > INT16_MAX)
  49. return INT16_MAX;
  50. return INT16_MIN;
  51. }
  52. /*- End of function --------------------------------------------------------*/
  53. static void block4(g722_decode_state_t *s, int band, int d);
  54. static void block4(g722_decode_state_t *s, int band, int d)
  55. {
  56. int wd1;
  57. int wd2;
  58. int wd3;
  59. int i;
  60. /* Block 4, RECONS */
  61. s->band[band].d[0] = d;
  62. s->band[band].r[0] = saturate(s->band[band].s + d);
  63. /* Block 4, PARREC */
  64. s->band[band].p[0] = saturate(s->band[band].sz + d);
  65. /* Block 4, UPPOL2 */
  66. for (i = 0; i < 3; i++)
  67. s->band[band].sg[i] = s->band[band].p[i] >> 15;
  68. wd1 = saturate(s->band[band].a[1] << 2);
  69. wd2 = (s->band[band].sg[0] == s->band[band].sg[1]) ? -wd1 : wd1;
  70. if (wd2 > 32767)
  71. wd2 = 32767;
  72. wd3 = (s->band[band].sg[0] == s->band[band].sg[2]) ? 128 : -128;
  73. wd3 += (wd2 >> 7);
  74. wd3 += (s->band[band].a[2]*32512) >> 15;
  75. if (wd3 > 12288)
  76. wd3 = 12288;
  77. else if (wd3 < -12288)
  78. wd3 = -12288;
  79. s->band[band].ap[2] = wd3;
  80. /* Block 4, UPPOL1 */
  81. s->band[band].sg[0] = s->band[band].p[0] >> 15;
  82. s->band[band].sg[1] = s->band[band].p[1] >> 15;
  83. wd1 = (s->band[band].sg[0] == s->band[band].sg[1]) ? 192 : -192;
  84. wd2 = (s->band[band].a[1]*32640) >> 15;
  85. s->band[band].ap[1] = saturate(wd1 + wd2);
  86. wd3 = saturate(15360 - s->band[band].ap[2]);
  87. if (s->band[band].ap[1] > wd3)
  88. s->band[band].ap[1] = wd3;
  89. else if (s->band[band].ap[1] < -wd3)
  90. s->band[band].ap[1] = -wd3;
  91. /* Block 4, UPZERO */
  92. wd1 = (d == 0) ? 0 : 128;
  93. s->band[band].sg[0] = d >> 15;
  94. for (i = 1; i < 7; i++)
  95. {
  96. s->band[band].sg[i] = s->band[band].d[i] >> 15;
  97. wd2 = (s->band[band].sg[i] == s->band[band].sg[0]) ? wd1 : -wd1;
  98. wd3 = (s->band[band].b[i]*32640) >> 15;
  99. s->band[band].bp[i] = saturate(wd2 + wd3);
  100. }
  101. /* Block 4, DELAYA */
  102. for (i = 6; i > 0; i--)
  103. {
  104. s->band[band].d[i] = s->band[band].d[i - 1];
  105. s->band[band].b[i] = s->band[band].bp[i];
  106. }
  107. for (i = 2; i > 0; i--)
  108. {
  109. s->band[band].r[i] = s->band[band].r[i - 1];
  110. s->band[band].p[i] = s->band[band].p[i - 1];
  111. s->band[band].a[i] = s->band[band].ap[i];
  112. }
  113. /* Block 4, FILTEP */
  114. wd1 = saturate(s->band[band].r[1] + s->band[band].r[1]);
  115. wd1 = (s->band[band].a[1]*wd1) >> 15;
  116. wd2 = saturate(s->band[band].r[2] + s->band[band].r[2]);
  117. wd2 = (s->band[band].a[2]*wd2) >> 15;
  118. s->band[band].sp = saturate(wd1 + wd2);
  119. /* Block 4, FILTEZ */
  120. s->band[band].sz = 0;
  121. for (i = 6; i > 0; i--)
  122. {
  123. wd1 = saturate(s->band[band].d[i] + s->band[band].d[i]);
  124. s->band[band].sz += (s->band[band].b[i]*wd1) >> 15;
  125. }
  126. s->band[band].sz = saturate(s->band[band].sz);
  127. /* Block 4, PREDIC */
  128. s->band[band].s = saturate(s->band[band].sp + s->band[band].sz);
  129. }
  130. /*- End of function --------------------------------------------------------*/
  131. g722_decode_state_t *g722_decode_init(g722_decode_state_t *s, int rate, int options)
  132. {
  133. if (s == NULL)
  134. {
  135. if ((s = (g722_decode_state_t *) malloc(sizeof(*s))) == NULL)
  136. return NULL;
  137. }
  138. memset(s, 0, sizeof(*s));
  139. if (rate == 48000)
  140. s->bits_per_sample = 6;
  141. else if (rate == 56000)
  142. s->bits_per_sample = 7;
  143. else
  144. s->bits_per_sample = 8;
  145. if ((options & G722_SAMPLE_RATE_8000))
  146. s->eight_k = TRUE;
  147. if ((options & G722_PACKED) && s->bits_per_sample != 8)
  148. s->packed = TRUE;
  149. else
  150. s->packed = FALSE;
  151. s->band[0].det = 32;
  152. s->band[1].det = 8;
  153. return s;
  154. }
  155. /*- End of function --------------------------------------------------------*/
  156. int g722_decode_release(g722_decode_state_t *s)
  157. {
  158. free(s);
  159. return 0;
  160. }
  161. /*- End of function --------------------------------------------------------*/
  162. int g722_decode(g722_decode_state_t *s, int16_t amp[], const uint8_t g722_data[], int len)
  163. {
  164. static const int wl[8] = {-60, -30, 58, 172, 334, 538, 1198, 3042 };
  165. static const int rl42[16] = {0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0 };
  166. static const int ilb[32] =
  167. {
  168. 2048, 2093, 2139, 2186, 2233, 2282, 2332,
  169. 2383, 2435, 2489, 2543, 2599, 2656, 2714,
  170. 2774, 2834, 2896, 2960, 3025, 3091, 3158,
  171. 3228, 3298, 3371, 3444, 3520, 3597, 3676,
  172. 3756, 3838, 3922, 4008
  173. };
  174. static const int wh[3] = {0, -214, 798};
  175. static const int rh2[4] = {2, 1, 2, 1};
  176. static const int qm2[4] = {-7408, -1616, 7408, 1616};
  177. static const int qm4[16] =
  178. {
  179. 0, -20456, -12896, -8968,
  180. -6288, -4240, -2584, -1200,
  181. 20456, 12896, 8968, 6288,
  182. 4240, 2584, 1200, 0
  183. };
  184. static const int qm5[32] =
  185. {
  186. -280, -280, -23352, -17560,
  187. -14120, -11664, -9752, -8184,
  188. -6864, -5712, -4696, -3784,
  189. -2960, -2208, -1520, -880,
  190. 23352, 17560, 14120, 11664,
  191. 9752, 8184, 6864, 5712,
  192. 4696, 3784, 2960, 2208,
  193. 1520, 880, 280, -280
  194. };
  195. static const int qm6[64] =
  196. {
  197. -136, -136, -136, -136,
  198. -24808, -21904, -19008, -16704,
  199. -14984, -13512, -12280, -11192,
  200. -10232, -9360, -8576, -7856,
  201. -7192, -6576, -6000, -5456,
  202. -4944, -4464, -4008, -3576,
  203. -3168, -2776, -2400, -2032,
  204. -1688, -1360, -1040, -728,
  205. 24808, 21904, 19008, 16704,
  206. 14984, 13512, 12280, 11192,
  207. 10232, 9360, 8576, 7856,
  208. 7192, 6576, 6000, 5456,
  209. 4944, 4464, 4008, 3576,
  210. 3168, 2776, 2400, 2032,
  211. 1688, 1360, 1040, 728,
  212. 432, 136, -432, -136
  213. };
  214. static const int qmf_coeffs[12] =
  215. {
  216. 3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11,
  217. };
  218. int dlowt;
  219. int rlow;
  220. int ihigh;
  221. int dhigh;
  222. int rhigh;
  223. int xout1;
  224. int xout2;
  225. int wd1;
  226. int wd2;
  227. int wd3;
  228. int code;
  229. int outlen;
  230. int i;
  231. int j;
  232. outlen = 0;
  233. rhigh = 0;
  234. for (j = 0; j < len; )
  235. {
  236. if (s->packed)
  237. {
  238. /* Unpack the code bits */
  239. if (s->in_bits < s->bits_per_sample)
  240. {
  241. s->in_buffer |= (g722_data[j++] << s->in_bits);
  242. s->in_bits += 8;
  243. }
  244. code = s->in_buffer & ((1 << s->bits_per_sample) - 1);
  245. s->in_buffer >>= s->bits_per_sample;
  246. s->in_bits -= s->bits_per_sample;
  247. }
  248. else
  249. {
  250. code = g722_data[j++];
  251. }
  252. switch (s->bits_per_sample)
  253. {
  254. default:
  255. case 8:
  256. wd1 = code & 0x3F;
  257. ihigh = (code >> 6) & 0x03;
  258. wd2 = qm6[wd1];
  259. wd1 >>= 2;
  260. break;
  261. case 7:
  262. wd1 = code & 0x1F;
  263. ihigh = (code >> 5) & 0x03;
  264. wd2 = qm5[wd1];
  265. wd1 >>= 1;
  266. break;
  267. case 6:
  268. wd1 = code & 0x0F;
  269. ihigh = (code >> 4) & 0x03;
  270. wd2 = qm4[wd1];
  271. break;
  272. }
  273. /* Block 5L, LOW BAND INVQBL */
  274. wd2 = (s->band[0].det*wd2) >> 15;
  275. /* Block 5L, RECONS */
  276. rlow = s->band[0].s + wd2;
  277. /* Block 6L, LIMIT */
  278. if (rlow > 16383)
  279. rlow = 16383;
  280. else if (rlow < -16384)
  281. rlow = -16384;
  282. /* Block 2L, INVQAL */
  283. wd2 = qm4[wd1];
  284. dlowt = (s->band[0].det*wd2) >> 15;
  285. /* Block 3L, LOGSCL */
  286. wd2 = rl42[wd1];
  287. wd1 = (s->band[0].nb*127) >> 7;
  288. wd1 += wl[wd2];
  289. if (wd1 < 0)
  290. wd1 = 0;
  291. else if (wd1 > 18432)
  292. wd1 = 18432;
  293. s->band[0].nb = wd1;
  294. /* Block 3L, SCALEL */
  295. wd1 = (s->band[0].nb >> 6) & 31;
  296. wd2 = 8 - (s->band[0].nb >> 11);
  297. wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
  298. s->band[0].det = wd3 << 2;
  299. block4(s, 0, dlowt);
  300. if (!s->eight_k)
  301. {
  302. /* Block 2H, INVQAH */
  303. wd2 = qm2[ihigh];
  304. dhigh = (s->band[1].det*wd2) >> 15;
  305. /* Block 5H, RECONS */
  306. rhigh = dhigh + s->band[1].s;
  307. /* Block 6H, LIMIT */
  308. if (rhigh > 16383)
  309. rhigh = 16383;
  310. else if (rhigh < -16384)
  311. rhigh = -16384;
  312. /* Block 2H, INVQAH */
  313. wd2 = rh2[ihigh];
  314. wd1 = (s->band[1].nb*127) >> 7;
  315. wd1 += wh[wd2];
  316. if (wd1 < 0)
  317. wd1 = 0;
  318. else if (wd1 > 22528)
  319. wd1 = 22528;
  320. s->band[1].nb = wd1;
  321. /* Block 3H, SCALEH */
  322. wd1 = (s->band[1].nb >> 6) & 31;
  323. wd2 = 10 - (s->band[1].nb >> 11);
  324. wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
  325. s->band[1].det = wd3 << 2;
  326. block4(s, 1, dhigh);
  327. }
  328. if (s->itu_test_mode)
  329. {
  330. amp[outlen++] = (int16_t) (rlow << 1);
  331. amp[outlen++] = (int16_t) (rhigh << 1);
  332. }
  333. else
  334. {
  335. if (s->eight_k)
  336. {
  337. amp[outlen++] = (int16_t) (rlow << 1);
  338. }
  339. else
  340. {
  341. /* Apply the receive QMF */
  342. for (i = 0; i < 22; i++)
  343. s->x[i] = s->x[i + 2];
  344. s->x[22] = rlow + rhigh;
  345. s->x[23] = rlow - rhigh;
  346. xout1 = 0;
  347. xout2 = 0;
  348. for (i = 0; i < 12; i++)
  349. {
  350. xout2 += s->x[2*i]*qmf_coeffs[i];
  351. xout1 += s->x[2*i + 1]*qmf_coeffs[11 - i];
  352. }
  353. amp[outlen++] = (int16_t) (xout1 >> 11);
  354. amp[outlen++] = (int16_t) (xout2 >> 11);
  355. }
  356. }
  357. }
  358. return outlen;
  359. }
  360. /*- End of function --------------------------------------------------------*/
  361. /*- End of file ------------------------------------------------------------*/