开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 嵌入式/单片机编程 > 嵌入式Linux > 查看源码
sm_psk4800.c
资源名称：SBC-2410X_kernel.tar.gz　[点击查看]
上传用户：lgb322
上传日期：2013-02-24
资源大小：30529k
文件大小：12k
源码类别：

嵌入式Linux

开发平台：

Unix_Linux

sm_psk4800.c：源码内容
  1. /*****************************************************************************/
  3. /*
  4.  * sm_psk4800.c  -- soundcard radio modem driver, 4800 baud 8PSK modem
  5.  *
  6.  * Copyright (C) 1997  Thomas Sailer (sailer@ife.ee.ethz.ch)
  7.  *
  8.  * This program is free software; you can redistribute it and/or modify
  9.  * it under the terms of the GNU General Public License as published by
  10.  * the Free Software Foundation; either version 2 of the License, or
  11.  * (at your option) any later version.
  12.  *
  13.  * This program is distributed in the hope that it will be useful,
  14.  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16.  * GNU General Public License for more details.
  17.  *
  18.  * You should have received a copy of the GNU General Public License
  19.  * along with this program; if not, write to the Free Software
  20.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  21.  *
  22.  *  Please note that the GPL allows you to use the driver, NOT the radio.
  23.  *  In order to use the radio, you need a license from the communications
  24.  *  authority of your country.
  25.  *
  26.  */
  28. #include "sm.h"
  29. #include "sm_tbl_psk4800.h"
  31. /* --------------------------------------------------------------------- */
  33. #define DESCRAM_TAP1 0x20000
  34. #define DESCRAM_TAP2 0x01000
  35. #define DESCRAM_TAP3 0x00001
  37. #define DESCRAM_TAPSH1 17
  38. #define DESCRAM_TAPSH2 12
  39. #define DESCRAM_TAPSH3 0
  41. #define SCRAM_TAP1 0x20000 /* X^17 */
  42. #define SCRAM_TAPN 0x00021 /* X^0+X^5 */
  44. #define SCRAM_SHIFT 17
  46. /* --------------------------------------------------------------------- */
  48. struct demod_state_psk48 {
  49. /*
  50.  * input mixer and lowpass
  51.  */
  52. short infi[PSK48_RXF_LEN/2], infq[PSK48_RXF_LEN/2];
  53. unsigned int downmixer;
  54. int ovrphase;
  55. short magi, magq;
  56. /*
  57.  * sampling instant recovery
  58.  */
  59. int pwrhist[5];
  60. unsigned int s_phase;
  61. int cur_sync;
  62. /*
  63.  * phase recovery
  64.  */
  65. short cur_phase_dev;
  66. short last_ph_err;
  67. unsigned short pskph;
  68. unsigned int phase;
  69. unsigned short last_pskph;
  70. unsigned char cur_raw, last_raw, rawbits;
  71. /*
  72.  * decoding
  73.  */
  74. unsigned int shreg;
  75. unsigned long descram;
  76. unsigned int bit_pll;
  77. unsigned char last_sample;
  78. unsigned int dcd_shreg;
  79. int dcd_sum0, dcd_sum1, dcd_sum2;
  80. unsigned int dcd_time;
  81. };
  83. struct mod_state_psk48 {
  84. unsigned char txbits[PSK48_TXF_NUMSAMPLES];
  85. unsigned short txphase;
  86. unsigned int shreg;
  87. unsigned long scram;
  88. const short *tbl;
  89. unsigned int txseq;
  90. };
  92. /* --------------------------------------------------------------------- */
  94. static void modulator_4800_u8(struct sm_state *sm, unsigned char *buf, unsigned int buflen)
  95. {
  96. struct mod_state_psk48 *st = (struct mod_state_psk48 *)(&sm->m);
  97. int i, j;
  98. int si, sq;
  100. for (; buflen > 0; buflen--, buf++) {
  101. if (!st->txseq++) {
  102. memmove(st->txbits+1, st->txbits, 
  103. sizeof(st->txbits)-sizeof(st->txbits[0]));
  104. for (i = 0; i < 3; i++) {
  105. if (st->shreg <= 1)
  106. st->shreg = hdlcdrv_getbits(&sm->hdrv) | 0x10000;
  107. st->scram = (st->scram << 1) |
  108. (st->shreg & 1);
  109. st->shreg >>= 1;
  110. if (st->scram & SCRAM_TAP1)
  111. st->scram ^= SCRAM_TAPN;
  112. }
  113. j = (st->scram >> (SCRAM_SHIFT+3)) & 7;
  114. st->txbits[0] -= (j ^ (j >> 1));
  115. st->txbits[0] &= 7;
  116. st->tbl = psk48_tx_table;
  117. }
  118. if (st->txseq >= PSK48_TXF_OVERSAMPLING)
  119. st->txseq = 0;
  120. for (j = si = sq = 0; j < PSK48_TXF_NUMSAMPLES; j++, st->tbl += 16) {
  121. si += st->tbl[st->txbits[j]];
  122. sq += st->tbl[st->txbits[j]+8];
  123. }
  124. *buf = ((si*COS(st->txphase)+ sq*SIN(st->txphase)) >> 23) + 0x80;
  125. st->txphase = (st->txphase + PSK48_PHASEINC) & 0xffffu;
  126. }
  127. }
  129. /* --------------------------------------------------------------------- */
  131. static void modulator_4800_s16(struct sm_state *sm, short *buf, unsigned int buflen)
  132. {
  133. struct mod_state_psk48 *st = (struct mod_state_psk48 *)(&sm->m);
  134. int i, j;
  135. int si, sq;
  137. for (; buflen > 0; buflen--, buf++) {
  138. if (!st->txseq++) {
  139. memmove(st->txbits+1, st->txbits, 
  140. sizeof(st->txbits)-sizeof(st->txbits[0]));
  141. for (i = 0; i < 3; i++) {
  142. if (st->shreg <= 1)
  143. st->shreg = hdlcdrv_getbits(&sm->hdrv) | 0x10000;
  144. st->scram = (st->scram << 1) |
  145. (st->shreg & 1);
  146. st->shreg >>= 1;
  147. if (st->scram & SCRAM_TAP1)
  148. st->scram ^= SCRAM_TAPN;
  149. }
  150. j = (st->scram >> (SCRAM_SHIFT+3)) & 7;
  151. st->txbits[0] -= (j ^ (j >> 1));
  152. st->txbits[0] &= 7;
  153. st->tbl = psk48_tx_table;
  154. }
  155. if (st->txseq >= PSK48_TXF_OVERSAMPLING)
  156. st->txseq = 0;
  157. for (j = si = sq = 0; j < PSK48_TXF_NUMSAMPLES; j++, st->tbl += 16) {
  158. si += st->tbl[st->txbits[j]];
  159. sq += st->tbl[st->txbits[j]+8];
  160. }
  161. *buf = (si*COS(st->txphase)+ sq*SIN(st->txphase)) >> 15;
  162. st->txphase = (st->txphase + PSK48_PHASEINC) & 0xffffu;
  163. }
  164. }
  166. /* --------------------------------------------------------------------- */
  168. static __inline__ unsigned short tbl_atan(short q, short i)
  169. {
  170. short tmp;
  171. unsigned short argoffs = 0;
  173. if (i == 0 && q == 0)
  174. return 0;
  175. switch (((q < 0) << 1) | (i < 0)) {
  176. case 0:
  177. break;
  178. case 1:
  179. tmp = q;
  180. q = -i;
  181. i = tmp;
  182. argoffs = 0x4000;
  183. break;
  184. case 3:
  185. q = -q;
  186. i = -i;
  187. argoffs = 0x8000;
  188. break;
  189. case 2:
  190. tmp = -q;
  191. q = i;
  192. i = tmp;
  193. argoffs = 0xc000;
  194. break;
  195. }
  196. if (q > i) {
  197. tmp = i / q * ATAN_TABLEN;
  198. return (argoffs+0x4000-atan_tab[((i<<15)/q*ATAN_TABLEN>>15)])
  199. &0xffffu;
  200. }
  201. return (argoffs+atan_tab[((q<<15)/i*ATAN_TABLEN)>>15])&0xffffu;
  202. }
  204. #define ATAN(q,i) tbl_atan(q, i)
  206. /* --------------------------------------------------------------------- */
  208. static void demod_psk48_baseband(struct sm_state *sm, struct demod_state_psk48 *st,
  209.  short vali, short valq)
  210. {
  211. int i, j;
  213. st->magi = vali;
  214. st->magq = valq;
  215. memmove(st->pwrhist+1, st->pwrhist, 
  216. sizeof(st->pwrhist)-sizeof(st->pwrhist[0]));
  217. st->pwrhist[0] = st->magi * st->magi +
  218. st->magq * st->magq;
  219. st->cur_sync = ((st->pwrhist[4] >> 2) > st->pwrhist[2] && 
  220. (st->pwrhist[0] >> 2) > st->pwrhist[2] &&
  221. st-> pwrhist[3] > st->pwrhist[2] && 
  222. st->pwrhist[1] > st->pwrhist[2]);
  223. st->s_phase &= 0xffff;
  224. st->s_phase += PSK48_SPHASEINC;
  225. st->dcd_shreg <<= 1;
  226. if (st->cur_sync) {
  227. if (st->s_phase >= (0x8000 + 5*PSK48_SPHASEINC/2))
  228. st->s_phase -= PSK48_SPHASEINC/6;
  229. else
  230. st->s_phase += PSK48_SPHASEINC/6;
  231. st->dcd_sum0 = 4*hweight8(st->dcd_shreg & 0xf8)-
  232. hweight16(st->dcd_shreg & 0x1f00);
  233. }
  234. if ((--st->dcd_time) <= 0) {
  235. hdlcdrv_setdcd(&sm->hdrv, (st->dcd_sum0 + st->dcd_sum1 + 
  236.    st->dcd_sum2) < 0);
  237. st->dcd_sum2 = st->dcd_sum1;
  238. st->dcd_sum1 = st->dcd_sum0;
  239. st->dcd_sum0 = 2; /* slight bias */
  240. st->dcd_time = 240;
  241. }
  242. if (st->s_phase < 0x10000)
  243. return;
  244. /*
  245.  * sample one constellation
  246.  */
  247. st->last_pskph = st->pskph;
  248. st->pskph = (ATAN(st->magq, st->magi)-
  249.        st->phase) & 0xffffu;
  250. st->last_ph_err = (st->pskph & 0x1fffu) - 0x1000;
  251. st->phase += st->last_ph_err/16;
  252. st->last_raw = st->cur_raw;
  253. st->cur_raw = ((st->pskph >> 13) & 7);
  254. i = (st->cur_raw - st->last_raw) & 7;
  255. st->rawbits = i ^ (i >> 1) ^ (i >> 2);
  256. st->descram = (st->descram << 3) | (st->rawbits);
  257. hdlcdrv_channelbit(&sm->hdrv, st->descram & 4);
  258. hdlcdrv_channelbit(&sm->hdrv, st->descram & 2);
  259. hdlcdrv_channelbit(&sm->hdrv, st->descram & 1);
  260. i = (((st->descram >> DESCRAM_TAPSH1) & 7) ^
  261.      ((st->descram >> DESCRAM_TAPSH2) & 7) ^
  262.      ((st->descram >> DESCRAM_TAPSH3) & 7));
  263.   for (j = 4; j; j >>= 1) {
  264. st->shreg >>= 1;
  265. st->shreg |= (!!(i & j)) << 16;
  266. if (st->shreg & 1) {
  267. hdlcdrv_putbits(&sm->hdrv, st->shreg >> 1);
  268. st->shreg = 0x10000;
  269. }
  270. }
  272. #if 0
  273. st->dcd_shreg <<= 1;
  274. st->bit_pll += 0x4000;
  275. curbit = (*buf >= 0x80);
  276. if (st->last_sample ^ curbit) {
  277. st->dcd_shreg |= 1;
  278. st->bit_pll += pll_corr
  279. [st->bit_pll < 0xa000];
  280. st->dcd_sum0 += 8 * 
  281. hweight8(st->dcd_shreg & 0x0c) - 
  282. !!(st->dcd_shreg & 0x10);
  283. }
  284. st->last_sample = curbit;
  285. hdlcdrv_channelbit(&sm->hdrv, st->last_sample);
  286. if ((--st->dcd_time) <= 0) {
  287. hdlcdrv_setdcd(&sm->hdrv, (st->dcd_sum0 + 
  288.    st->dcd_sum1 + 
  289.    st->dcd_sum2) < 0);
  290. st->dcd_sum2 = st->dcd_sum1;
  291. st->dcd_sum1 = st->dcd_sum0;
  292. st->dcd_sum0 = 2; /* slight bias */
  293. st->dcd_time = 240;
  294. }
  295. if (st->bit_pll >= 0x10000) {
  296. st->bit_pll &= 0xffffu;
  297. st->descram = (st->descram << 1) | curbit;
  298. descx = st->descram ^ (st->descram >> 1);
  299. descx ^= ((descx >> DESCRAM_TAPSH1) ^
  300.   (descx >> DESCRAM_TAPSH2));
  301. st->shreg >>= 1;
  302. st->shreg |= (!(descx & 1)) << 16;
  303. if (st->shreg & 1) {
  304. hdlcdrv_putbits(&sm->hdrv, st->shreg >> 1);
  305. st->shreg = 0x10000;
  306. }
  307. diag_trigger(sm);
  308. }
  309. diag_add_one(sm, ((short)(*buf - 0x80)) << 8);
  310. #endif
  312. diag_trigger(sm);
  313. diag_add_constellation(sm, (vali*COS(st->phase)+ valq*SIN(st->phase)) >> 13, 
  314.        (valq*COS(st->phase) - vali*SIN(st->phase)) >> 13);
  315. }
  317. /* --------------------------------------------------------------------- */
  319. static void demodulator_4800_u8(struct sm_state *sm, const unsigned char *buf, unsigned int buflen)
  320. {
  321. struct demod_state_psk48 *st = (struct demod_state_psk48 *)(&sm->d);
  322. int i, si, sq;
  323. const short *coeff;
  325. for (; buflen > 0; buflen--, buf++) {
  326. memmove(st->infi+1, st->infi, 
  327. sizeof(st->infi)-sizeof(st->infi[0]));
  328. memmove(st->infq+1, st->infq, 
  329. sizeof(st->infq)-sizeof(st->infq[0]));
  330. si = *buf;
  331. si &= 0xff;
  332. si -= 128;
  333. diag_add_one(sm, si << 8);
  334. st->infi[0] = (si * COS(st->downmixer))>>7;
  335. st->infq[0] = (si * SIN(st->downmixer))>>7;
  336. st->downmixer = (st->downmixer-PSK48_PHASEINC)&0xffffu;
  337. for (i = si = sq = 0, coeff = psk48_rx_coeff; i < (PSK48_RXF_LEN/2); 
  338.      i++, coeff += 2) {
  339. si += st->infi[i] * (*coeff);
  340. sq += st->infq[i] * (*coeff);
  341. }
  342. demod_psk48_baseband(sm, st, si >> 15, sq >> 15);
  343. for (i = si = sq = 0, coeff = psk48_rx_coeff + 1; i < (PSK48_RXF_LEN/2); 
  344.      i++, coeff += 2) {
  345. si += st->infi[i] * (*coeff);
  346. sq += st->infq[i] * (*coeff);
  347. }
  348. demod_psk48_baseband(sm, st, si >> 15, sq >> 15);
  349. }
  350. }
  352. /* --------------------------------------------------------------------- */
  354. static void demodulator_4800_s16(struct sm_state *sm, const short *buf, unsigned int buflen)
  355. {
  356. struct demod_state_psk48 *st = (struct demod_state_psk48 *)(&sm->d);
  357. int i, si, sq;
  358. const short *coeff;
  360. for (; buflen > 0; buflen--, buf++) {
  361. memmove(st->infi+1, st->infi, 
  362. sizeof(st->infi)-sizeof(st->infi[0]));
  363. memmove(st->infq+1, st->infq, 
  364. sizeof(st->infq)-sizeof(st->infq[0]));
  365. si = *buf;
  366. diag_add_one(sm, si);
  367. st->infi[0] = (si * COS(st->downmixer))>>15;
  368. st->infq[0] = (si * SIN(st->downmixer))>>15;
  369. st->downmixer = (st->downmixer-PSK48_PHASEINC)&0xffffu;
  370. for (i = si = sq = 0, coeff = psk48_rx_coeff; i < (PSK48_RXF_LEN/2); 
  371.      i++, coeff += 2) {
  372. si += st->infi[i] * (*coeff);
  373. sq += st->infq[i] * (*coeff);
  374. }
  375. demod_psk48_baseband(sm, st, si >> 15, sq >> 15);
  376. for (i = si = sq = 0, coeff = psk48_rx_coeff + 1; i < (PSK48_RXF_LEN/2); 
  377.      i++, coeff += 2) {
  378. si += st->infi[i] * (*coeff);
  379. sq += st->infq[i] * (*coeff);
  380. }
  381. demod_psk48_baseband(sm, st, si >> 15, sq >> 15);
  382. }
  383. }
  385. /* --------------------------------------------------------------------- */
  387. static void mod_init_4800(struct sm_state *sm)
  388. {
  389. struct mod_state_psk48 *st = (struct mod_state_psk48 *)(&sm->m);
  391. st->scram = 1;
  392. }
  394. /* --------------------------------------------------------------------- */
  396. static void demod_init_4800(struct sm_state *sm)
  397. {
  398. struct demod_state_psk48 *st = (struct demod_state_psk48 *)(&sm->d);
  400. st->dcd_time = 120;
  401. st->dcd_sum0 = 2;
  402. }
  404. /* --------------------------------------------------------------------- */
  406. const struct modem_tx_info sm_psk4800_tx = {
  407. "psk4800", sizeof(struct mod_state_psk48), 
  408. PSK48_SAMPLERATE, 4800,
  409. modulator_4800_u8, modulator_4800_s16, mod_init_4800
  410. };
  412. const struct modem_rx_info sm_psk4800_rx = {
  413. "psk4800", sizeof(struct demod_state_psk48), 
  414. PSK48_SAMPLERATE, 4800, 1, PSK48_TXF_OVERSAMPLING, 
  415. demodulator_4800_u8, demodulator_4800_s16, demod_init_4800
  416. };
  418. /* --------------------------------------------------------------------- */