modem.c

powerpc内核mpc8241linux系统下char驱动程序

/*****************************************************************************/

/*
 *      modem.c  --  Linux soundcard HF FSK driver,
 *                   Modem code.
 *
 *      Copyright (C) 1997  Thomas Sailer ([e-mail])
 *        Swiss Federal Institute of Technology (ETH), Electronics Lab
 *
 *      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.
 *
 *
 */

/*****************************************************************************/
      

#include <linux/wait.h>
#include <linux/malloc.h>
#include <linux/hfmodem.h>

/* --------------------------------------------------------------------- */

/*
 * currently this module is supposed to support both module styles, i.e.
 * the old one present up to about 2.1.9, and the new one functioning
 * starting with 2.1.21. The reason is I have a kit allowing to compile
 * this module also under 2.0.x which was requested by several people.
 * This will go in 2.2
 */
#include <linux/version.h>

#if LINUX_VERSION_CODE >= 0x20100
#include <asm/uaccess.h>
#else
#include <asm/segment.h>
#include <linux/mm.h>

#undef put_user
#undef get_user

#define put_user(x,ptr) ({ __put_user((unsigned long)(x),(ptr),sizeof(*(ptr))); 0; })
#define get_user(x,ptr) ({ x = ((__typeof__(*(ptr)))__get_user((ptr),sizeof(*(ptr)))); 0; })

extern inline int copy_from_user(void *to, const void *from, unsigned long n)
{
        int i = verify_area(VERIFY_READ, from, n);
        if (i)
                return i;
        memcpy_fromfs(to, from, n);
        return 0;
}

extern inline int copy_to_user(void *to, const void *from, unsigned long n)
{
        int i = verify_area(VERIFY_WRITE, to, n);
        if (i)
                return i;
        memcpy_tofs(to, from, n);
        return 0;
}
#endif

#if LINUX_VERSION_CODE >= 0x20123
#include <linux/init.h>
#else
#define __init
#define __initdata
#define __initfunc(x) x
#endif

/* --------------------------------------------------------------------- */

struct hfmodem_correlator_cache hfmodem_correlator_cache[HFMODEM_CORRELATOR_CACHE];

/* --------------------------------------------------------------------- */

#include "tables.h"

#define M_PI        3.14159265358979323846      /* pi */

/* --------------------------------------------------------------------- */

extern __inline__ int isimplecos(unsigned int arg)
{
	return isintab[((arg+0x4000) >> (16-SINTABBITS)) & (SINTABSIZE-1)];
}

extern __inline__ int isimplesin(unsigned int arg)
{
	return isintab[(arg >> (16-SINTABBITS)) & (SINTABSIZE-1)];
}

/* --------------------------------------------------------------------- */

extern __inline__ int itblcos(unsigned int arg)
{
	unsigned int x;
	int dx;
	int s, c;

	x = (arg + (0x8000 >> SINTABBITS)) & (0xffffu & (0xffffu << (16-SINTABBITS)));
	dx = arg - x;
	x >>= (16-SINTABBITS);
	c = isintab[x+(0x4000 >> (16-SINTABBITS))];
	s = isintab[x];
	return c - ((s * dx * (int)(M_PI*64.0)) >> 21);
}

/* --------------------------------------------------------------------- */

extern __inline__ void itblcossin(unsigned int arg, int *cos, int *sin)
{
	unsigned int x;
	int dx;
	int s, c;

	x = (arg + (0x8000 >> SINTABBITS)) & (0xffffu & (0xffffu << (16-SINTABBITS)));
	dx = arg - x;
	x >>= (16-SINTABBITS);
	c = isintab[x+(0x4000 >> (16-SINTABBITS))];
	s = isintab[x];
	*cos = c - ((s * dx * (int)(M_PI*64.0)) >> 21);
	*sin = s + ((c * dx * (int)(M_PI*64.0)) >> 21);
}

/* --------------------------------------------------------------------- */

static unsigned short random_seed;

extern __inline__ unsigned short random_num(void)
{
        random_seed = 28629 * random_seed + 157;
        return random_seed;
}

/* --------------------------------------------------------------------- */
/*
 * correlator cache routines
 */

extern __inline__ void cc_lock(unsigned int u)
{
	if (u >= HFMODEM_CORRELATOR_CACHE)
		return;
	hfmodem_correlator_cache[u].refcnt++;
}

extern __inline__ void cc_unlock(unsigned int u)
{
	if (u >= HFMODEM_CORRELATOR_CACHE)
		return;
	if ((--hfmodem_correlator_cache[u].refcnt) <= 0) {
		unsigned int i;

		for (i = 0; i < HFMODEM_CORRELATOR_CACHE; i++) 
			if (hfmodem_correlator_cache[i].lru < 32767)
				hfmodem_correlator_cache[i].lru++;
		hfmodem_correlator_cache[u].lru = 0;
		hfmodem_correlator_cache[u].refcnt = 0;
	}
}


/* --------------------------------------------------------------------- */

extern __inline__ unsigned int cc_lookup(unsigned short phinc0, unsigned short phinc1)
{
	unsigned int j;

	/* find correlator cache entry */
	for (j = 0; j < HFMODEM_CORRELATOR_CACHE; j++) 
		if (hfmodem_correlator_cache[j].phase_incs[0] == phinc0 &&
		    hfmodem_correlator_cache[j].phase_incs[1] == phinc1)
			return j;
	return ~0;
}

/* --------------------------------------------------------------------- */

extern __inline__ unsigned int cc_replace(void)
{
	unsigned int j, k = HFMODEM_CORRELATOR_CACHE;
	int l = -1;

	for (j = 0; j < HFMODEM_CORRELATOR_CACHE; j++)
		if (hfmodem_correlator_cache[j].refcnt <= 0 && hfmodem_correlator_cache[j].lru > l) {
			k = j;
			l = hfmodem_correlator_cache[j].lru;
		}
	if (k < HFMODEM_CORRELATOR_CACHE)
		return k;
	printk(KERN_ERR "%s: modem: out of filter coefficient cache entries\n", hfmodem_drvname);
	return random_num() % HFMODEM_CORRELATOR_CACHE;
}

/* --------------------------------------------------------------------- */

#define SH1  8     /* min. ceil(log2(L1CORR_LEN)) - (31-(2*15-1)) */
#define SH2  (3*15-2*SH1)

#ifdef __i386__

extern __inline__ int icorr(int n, const int *coeff, const short *inp)
{
	int ret, rethi, tmp1 = 0, tmp2 = 0;

	__asm__("\n1:\n\t"
		"movswl (%0),%%eax\n\t"
		"imull (%1)\n\t"
		"subl $2,%0\n\t"
		"addl $4,%1\n\t"
		"addl %%eax,%3\n\t"
		"adcl %%edx,%4\n\t"
		"decl %2\n\t"
		"jne 1b\n\t"
		: "=&S" (inp), "=&D" (coeff), "=&c" (n), "=m" (tmp1), "=m" (tmp2)
		: "0" (inp), "1" (coeff), "2" (n)
		: "ax", "dx");
	__asm__("shrdl %2,%1,%0\n\t"
		"# sarl %2,%1\n\t"
		: "=&r" (ret), "=&r" (rethi)
		: "i" (SH1), "0" (tmp1), "1" (tmp2));


	return ret;
}

#else /* __i386__ */

extern __inline__ int icorr(int n, const int *coeff, const short *inp)
{
	long long sum = 0;
	int i;

	for (i = n; i > 0; i--, coeff++, inp--)
		sum += (*coeff) * (*inp);
	sum >>= SH1;
	return sum;
}

#endif /* __i386__ */

/* --------------------------------------------------------------------- */

extern __inline__ long long isqr(int x) __attribute__ ((const));

extern __inline__ long long isqr(int x)
{
	return ((long long)x) * ((long long)x);
}

/* --------------------------------------------------------------------- */

extern __inline__ hfmodem_soft_t do_filter(struct hfmodem_l1_rxslot *slot, short *s)
{
	unsigned int cc = slot->corr_cache;
	long long ll;

	if (cc >= HFMODEM_CORRELATOR_CACHE) {
		printk(KERN_ERR "do_filter: correlator cache index overrange\n");
		return 0;
	}
	ll = isqr(icorr(slot->corrlen, hfmodem_correlator_cache[cc].correlator[1][0], s)) +
		isqr(icorr(slot->corrlen, hfmodem_correlator_cache[cc].correlator[1][1], s)) -
		isqr(icorr(slot->corrlen, hfmodem_correlator_cache[cc].correlator[0][0], s)) -
		isqr(icorr(slot->corrlen, hfmodem_correlator_cache[cc].correlator[0][1], s));
	ll >>= SH2;
       	return (ll * slot->scale) >> 23;
}

/* --------------------------------------------------------------------- */

static void cc_prepare(struct hfmodem_l1_rxslot *slot, unsigned short phinc0, unsigned short phinc1)
{
	unsigned int j, k, l, ph, phinc;

	slot->scale = (1<<23) / (slot->corrlen*slot->corrlen);

	j = cc_lookup(phinc0, phinc1);
	if (j >= HFMODEM_CORRELATOR_CACHE) {
		j = cc_replace();
		/* calculate the correlator values */
		printk(KERN_DEBUG "%s: corr cache calc: %u  phases: 0x%04x 0x%04x\n", 
		       hfmodem_drvname, j, phinc0, phinc1);
		hfmodem_correlator_cache[j].phase_incs[0] = phinc0;
		hfmodem_correlator_cache[j].phase_incs[1] = phinc1;
		for (k = 0; k < 2; k++) {
			phinc = hfmodem_correlator_cache[j].phase_incs[k];
			for (ph = l = 0; l < HFMODEM_MAXCORRLEN; l++, ph = (ph + phinc) & 0xffff)
				itblcossin(ph, &hfmodem_correlator_cache[j].correlator[k][0][l],
					   &hfmodem_correlator_cache[j].correlator[k][1][l]);
		}
		hfmodem_correlator_cache[j].refcnt = 0;

#if 0
		printk(KERN_DEBUG "%s: corr: %u ph: 0x%04x 0x%04x\n", hfmodem_drvname, j, 
		       hfmodem_correlator_cache[j].phase_incs[0],
		       hfmodem_correlator_cache[j].phase_incs[1]);
		for (l = 0; l < HFMODEM_MAXCORRLEN; l++)
			printk(KERN_DEBUG "%s: corr: %6d %6d %6d %6d\n", hfmodem_drvname, 
			       hfmodem_correlator_cache[j].correlator[0][0][l],
			       hfmodem_correlator_cache[j].correlator[0][1][l],
			       hfmodem_correlator_cache[j].correlator[1][0][l],
			       hfmodem_correlator_cache[j].correlator[1][1][l]);
#endif
	}
	slot->corr_cache = j;
	cc_lock(j);
}

/* --------------------------------------------------------------------- */

void hfmodem_clear_rq(struct hfmodem_state *dev)
{
	unsigned long flags;
	unsigned int i;

	save_flags(flags);
	cli();
	for (i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
		if (dev->l1.rxslots[i].state == ss_unused)
			continue;
		dev->l1.rxslots[i].state = ss_unused;
		kfree_s(dev->l1.rxslots[i].data, dev->l1.rxslots[i].nbits * sizeof(hfmodem_soft_t));
	}
	for (i = 0; i < HFMODEM_NUMTXSLOTS; i++) {
		if (dev->l1.txslots[i].state == ss_unused)
			continue;
		dev->l1.txslots[i].state = ss_unused;
		kfree_s(dev->l1.txslots[i].data, (dev->l1.txslots[i].nbits + 7) >> 3);
	}
	for (i = 0; i < HFMODEM_CORRELATOR_CACHE; i++)
		hfmodem_correlator_cache[i].refcnt = 0;
	restore_flags(flags);
}

/* --------------------------------------------------------------------- */

int hfmodem_ioctl(struct inode *inode, struct file *file, unsigned int cmd, unsigned long arg)
{
	struct hfmodem_state *dev = &hfmodem_state[0];
	struct hfmodem_ioctl_fsk_tx_request txrq;
	struct hfmodem_ioctl_fsk_rx_request rxrq;
	struct hfmodem_ioctl_mixer_params mix;
	struct hfmodem_ioctl_sample_params spar;
	unsigned long flags;
	unsigned int len;
	int ret, i, idx;
	void *data, *userdata;
	hfmodem_id_t id;
	hfmodem_time_t tm = 0;
	
	if (!dev->active)
		return -EBUSY;
	switch(cmd) {
	default:
		return -EINVAL;
		
	case HFMODEM_IOCTL_FSKTXREQUEST:
		if ((ret = copy_from_user(&txrq, (void *)arg, sizeof(txrq))))
			return ret;
		if (txrq.nbits > HFMODEM_MAXBITS)
			return -EINVAL;
		len = (txrq.nbits + 7) >> 3;
		if (!(data = kmalloc(len, GFP_KERNEL)))
			return -ENOMEM;
		if (copy_from_user(data, txrq.data, len)) {
			kfree_s(data, len);
			return -EFAULT;
		}
		save_flags(flags);
		cli();
		for (i = 0; i < HFMODEM_NUMTXSLOTS && dev->l1.txslots[i].state != ss_unused; i++);
		if (i >= HFMODEM_NUMTXSLOTS) {
			restore_flags(flags);
			kfree_s(data, len);
			return -EBUSY;
		}
		dev->l1.txslots[i].state = ss_ready;
		dev->l1.txslots[i].tstart = txrq.tstart;
		dev->l1.txslots[i].tinc = txrq.tinc;
		dev->l1.txslots[i].data = data;
		dev->l1.txslots[i].nbits = txrq.nbits;
		dev->l1.txslots[i].cntbits = 0;