g722_encode.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401
  1. /*
  2. * SpanDSP - a series of DSP components for telephony
  3. *
  4. * g722_encode.c - The ITU G.722 codec, encode part.
  5. *
  6. * Written by Steve Underwood <steveu@coppice.org>
  7. *
  8. * Copyright (C) 2005 Steve Underwood
  9. *
  10. * All rights reserved.
  11. *
  12. * Despite my general liking of the GPL, I place my own contributions
  13. * to this code in the public domain for the benefit of all mankind -
  14. * even the slimy ones who might try to proprietize my work and use it
  15. * to my detriment.
  16. *
  17. * Based on a single channel 64kbps only G.722 codec which is:
  18. *
  19. ***** Copyright (c) CMU 1993 *****
  20. * Computer Science, Speech Group
  21. * Chengxiang Lu and Alex Hauptmann
  22. *
  23. * $Id$
  24. */
  25. /*! \file */
  26. #ifdef HAVE_CONFIG_H
  27. #include <config.h>
  28. #endif
  29. #include <stdio.h>
  30. #include <inttypes.h>
  31. #include <memory.h>
  32. #include <stdlib.h>
  33. #if 0
  34. #include <tgmath.h>
  35. #endif
  36. #include "g722.h"
  37. #if !defined(FALSE)
  38. #define FALSE 0
  39. #endif
  40. #if !defined(TRUE)
  41. #define TRUE (!FALSE)
  42. #endif
  43. static __inline__ int16_t saturate(int32_t amp)
  44. {
  45. int16_t amp16;
  46. /* Hopefully this is optimised for the common case - not clipping */
  47. amp16 = (int16_t) amp;
  48. if (amp == amp16)
  49. return amp16;
  50. if (amp > INT16_MAX)
  51. return INT16_MAX;
  52. return INT16_MIN;
  53. }
  54. /*- End of function --------------------------------------------------------*/
  55. static void block4(g722_encode_state_t *s, int band, int d)
  56. {
  57. int wd1;
  58. int wd2;
  59. int wd3;
  60. int i;
  61. /* Block 4, RECONS */
  62. s->band[band].d[0] = d;
  63. s->band[band].r[0] = saturate(s->band[band].s + d);
  64. /* Block 4, PARREC */
  65. s->band[band].p[0] = saturate(s->band[band].sz + d);
  66. /* Block 4, UPPOL2 */
  67. for (i = 0; i < 3; i++)
  68. s->band[band].sg[i] = s->band[band].p[i] >> 15;
  69. wd1 = saturate(s->band[band].a[1] << 2);
  70. wd2 = (s->band[band].sg[0] == s->band[band].sg[1]) ? -wd1 : wd1;
  71. if (wd2 > 32767)
  72. wd2 = 32767;
  73. wd3 = (wd2 >> 7) + ((s->band[band].sg[0] == s->band[band].sg[2]) ? 128 : -128);
  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_encode_state_t *g722_encode_init(g722_encode_state_t *s, int rate, int options)
  132. {
  133. if (s == NULL)
  134. {
  135. if ((s = (g722_encode_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_encode_release(g722_encode_state_t *s)
  157. {
  158. free(s);
  159. return 0;
  160. }
  161. /*- End of function --------------------------------------------------------*/
  162. int g722_encode(g722_encode_state_t *s, uint8_t g722_data[], const int16_t amp[], int len)
  163. {
  164. static const int q6[32] =
  165. {
  166. 0, 35, 72, 110, 150, 190, 233, 276,
  167. 323, 370, 422, 473, 530, 587, 650, 714,
  168. 786, 858, 940, 1023, 1121, 1219, 1339, 1458,
  169. 1612, 1765, 1980, 2195, 2557, 2919, 0, 0
  170. };
  171. static const int iln[32] =
  172. {
  173. 0, 63, 62, 31, 30, 29, 28, 27,
  174. 26, 25, 24, 23, 22, 21, 20, 19,
  175. 18, 17, 16, 15, 14, 13, 12, 11,
  176. 10, 9, 8, 7, 6, 5, 4, 0
  177. };
  178. static const int ilp[32] =
  179. {
  180. 0, 61, 60, 59, 58, 57, 56, 55,
  181. 54, 53, 52, 51, 50, 49, 48, 47,
  182. 46, 45, 44, 43, 42, 41, 40, 39,
  183. 38, 37, 36, 35, 34, 33, 32, 0
  184. };
  185. static const int wl[8] =
  186. {
  187. -60, -30, 58, 172, 334, 538, 1198, 3042
  188. };
  189. static const int rl42[16] =
  190. {
  191. 0, 7, 6, 5, 4, 3, 2, 1, 7, 6, 5, 4, 3, 2, 1, 0
  192. };
  193. static const int ilb[32] =
  194. {
  195. 2048, 2093, 2139, 2186, 2233, 2282, 2332,
  196. 2383, 2435, 2489, 2543, 2599, 2656, 2714,
  197. 2774, 2834, 2896, 2960, 3025, 3091, 3158,
  198. 3228, 3298, 3371, 3444, 3520, 3597, 3676,
  199. 3756, 3838, 3922, 4008
  200. };
  201. static const int qm4[16] =
  202. {
  203. 0, -20456, -12896, -8968,
  204. -6288, -4240, -2584, -1200,
  205. 20456, 12896, 8968, 6288,
  206. 4240, 2584, 1200, 0
  207. };
  208. static const int qm2[4] =
  209. {
  210. -7408, -1616, 7408, 1616
  211. };
  212. static const int qmf_coeffs[12] =
  213. {
  214. 3, -11, 12, 32, -210, 951, 3876, -805, 362, -156, 53, -11,
  215. };
  216. static const int ihn[3] = {0, 1, 0};
  217. static const int ihp[3] = {0, 3, 2};
  218. static const int wh[3] = {0, -214, 798};
  219. static const int rh2[4] = {2, 1, 2, 1};
  220. int dlow;
  221. int dhigh;
  222. int el;
  223. int wd;
  224. int wd1;
  225. int ril;
  226. int wd2;
  227. int il4;
  228. int ih2;
  229. int wd3;
  230. int eh;
  231. int mih;
  232. int i;
  233. int j;
  234. /* Low and high band PCM from the QMF */
  235. int xlow;
  236. int xhigh;
  237. int g722_bytes;
  238. /* Even and odd tap accumulators */
  239. int sumeven;
  240. int sumodd;
  241. int ihigh;
  242. int ilow;
  243. int code;
  244. g722_bytes = 0;
  245. xhigh = 0;
  246. for (j = 0; j < len; )
  247. {
  248. if (s->itu_test_mode)
  249. {
  250. xlow =
  251. xhigh = amp[j++] >> 1;
  252. }
  253. else
  254. {
  255. if (s->eight_k)
  256. {
  257. xlow = amp[j++] >> 1;
  258. }
  259. else
  260. {
  261. /* Apply the transmit QMF */
  262. /* Shuffle the buffer down */
  263. for (i = 0; i < 22; i++)
  264. s->x[i] = s->x[i + 2];
  265. s->x[22] = amp[j++];
  266. s->x[23] = amp[j++];
  267. /* Discard every other QMF output */
  268. sumeven = 0;
  269. sumodd = 0;
  270. for (i = 0; i < 12; i++)
  271. {
  272. sumodd += s->x[2*i]*qmf_coeffs[i];
  273. sumeven += s->x[2*i + 1]*qmf_coeffs[11 - i];
  274. }
  275. xlow = (sumeven + sumodd) >> 14;
  276. xhigh = (sumeven - sumodd) >> 14;
  277. }
  278. }
  279. /* Block 1L, SUBTRA */
  280. el = saturate(xlow - s->band[0].s);
  281. /* Block 1L, QUANTL */
  282. wd = (el >= 0) ? el : -(el + 1);
  283. for (i = 1; i < 30; i++)
  284. {
  285. wd1 = (q6[i]*s->band[0].det) >> 12;
  286. if (wd < wd1)
  287. break;
  288. }
  289. ilow = (el < 0) ? iln[i] : ilp[i];
  290. /* Block 2L, INVQAL */
  291. ril = ilow >> 2;
  292. wd2 = qm4[ril];
  293. dlow = (s->band[0].det*wd2) >> 15;
  294. /* Block 3L, LOGSCL */
  295. il4 = rl42[ril];
  296. wd = (s->band[0].nb*127) >> 7;
  297. s->band[0].nb = wd + wl[il4];
  298. if (s->band[0].nb < 0)
  299. s->band[0].nb = 0;
  300. else if (s->band[0].nb > 18432)
  301. s->band[0].nb = 18432;
  302. /* Block 3L, SCALEL */
  303. wd1 = (s->band[0].nb >> 6) & 31;
  304. wd2 = 8 - (s->band[0].nb >> 11);
  305. wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
  306. s->band[0].det = wd3 << 2;
  307. block4(s, 0, dlow);
  308. if (s->eight_k)
  309. {
  310. /* Just leave the high bits as zero */
  311. code = (0xC0 | ilow) >> (8 - s->bits_per_sample);
  312. }
  313. else
  314. {
  315. /* Block 1H, SUBTRA */
  316. eh = saturate(xhigh - s->band[1].s);
  317. /* Block 1H, QUANTH */
  318. wd = (eh >= 0) ? eh : -(eh + 1);
  319. wd1 = (564*s->band[1].det) >> 12;
  320. mih = (wd >= wd1) ? 2 : 1;
  321. ihigh = (eh < 0) ? ihn[mih] : ihp[mih];
  322. /* Block 2H, INVQAH */
  323. wd2 = qm2[ihigh];
  324. dhigh = (s->band[1].det*wd2) >> 15;
  325. /* Block 3H, LOGSCH */
  326. ih2 = rh2[ihigh];
  327. wd = (s->band[1].nb*127) >> 7;
  328. s->band[1].nb = wd + wh[ih2];
  329. if (s->band[1].nb < 0)
  330. s->band[1].nb = 0;
  331. else if (s->band[1].nb > 22528)
  332. s->band[1].nb = 22528;
  333. /* Block 3H, SCALEH */
  334. wd1 = (s->band[1].nb >> 6) & 31;
  335. wd2 = 10 - (s->band[1].nb >> 11);
  336. wd3 = (wd2 < 0) ? (ilb[wd1] << -wd2) : (ilb[wd1] >> wd2);
  337. s->band[1].det = wd3 << 2;
  338. block4(s, 1, dhigh);
  339. code = ((ihigh << 6) | ilow) >> (8 - s->bits_per_sample);
  340. }
  341. if (s->packed)
  342. {
  343. /* Pack the code bits */
  344. s->out_buffer |= (code << s->out_bits);
  345. s->out_bits += s->bits_per_sample;
  346. if (s->out_bits >= 8)
  347. {
  348. g722_data[g722_bytes++] = (uint8_t) (s->out_buffer & 0xFF);
  349. s->out_bits -= 8;
  350. s->out_buffer >>= 8;
  351. }
  352. }
  353. else
  354. {
  355. g722_data[g722_bytes++] = (uint8_t) code;
  356. }
  357. }
  358. return g722_bytes;
  359. }
  360. /*- End of function --------------------------------------------------------*/
  361. /*- End of file ------------------------------------------------------------*/