modem.c

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

		dev->l1.txslots[i].inv = txrq.inv ? 0xff : 0;
		dev->l1.txslots[i].id = txrq.id;
		dev->l1.txslots[i].phase_incs[0] = ((txrq.freq[0]*0x10000+(HFMODEM_SRATE/2))/HFMODEM_SRATE)
			& 0xffff;
		dev->l1.txslots[i].phase_incs[1] = ((txrq.freq[1]*0x10000+(HFMODEM_SRATE/2))/HFMODEM_SRATE)
			& 0xffff;
		restore_flags(flags);
		return 0;
		
	case HFMODEM_IOCTL_FSKRXREQUEST:
		if ((ret = copy_from_user(&rxrq, (void *)arg, sizeof(rxrq))))
			return ret;
		if (rxrq.nbits > HFMODEM_MAXBITS)
			return -EINVAL;
		if (rxrq.baud < HFMODEM_MINBAUD || rxrq.baud > HFMODEM_MAXBAUD)
			return -EINVAL;
		len = rxrq.nbits * sizeof(hfmodem_soft_t);
		if (verify_area(VERIFY_WRITE, rxrq.data, len))
			return -EFAULT;
		if (!(data = kmalloc(len, GFP_KERNEL)))
			return -ENOMEM;
		save_flags(flags);
		cli();
		for (i = 0; i < HFMODEM_NUMRXSLOTS && dev->l1.rxslots[i].state != ss_unused; i++);
		if (i >= HFMODEM_NUMRXSLOTS) {
			restore_flags(flags);
			kfree_s(data, len);
			return -EBUSY;
		}
		dev->l1.rxslots[i].state = ss_ready;
		dev->l1.rxslots[i].tstart = rxrq.tstart;
		dev->l1.rxslots[i].tinc = rxrq.tinc;
		dev->l1.rxslots[i].data = data;
		dev->l1.rxslots[i].userdata = rxrq.data;
		dev->l1.rxslots[i].nbits = rxrq.nbits;
		dev->l1.rxslots[i].cntbits = 0;
		dev->l1.rxslots[i].id = rxrq.id;
		dev->l1.rxslots[i].corrlen = HFMODEM_SRATE/rxrq.baud;
		cc_prepare(dev->l1.rxslots+i, 
			   ((rxrq.freq[0]*0x10000+(HFMODEM_SRATE/2))/HFMODEM_SRATE) & 0xffff,
			   ((rxrq.freq[1]*0x10000+(HFMODEM_SRATE/2))/HFMODEM_SRATE) & 0xffff);
		restore_flags(flags);
		return 0;
		
	case HFMODEM_IOCTL_CLEARRQ:
		hfmodem_clear_rq(dev);
		return 0;
		
	case HFMODEM_IOCTL_GETCURTIME:
		return put_user(dev->l1.last_time + 20000L, (hfmodem_time_t *)arg); /* heuristic */
	
	case HFMODEM_IOCTL_WAITRQ:
		save_flags(flags);
		cli();
		ret = 0;
		for (idx = -1, i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
			if (dev->l1.rxslots[i].state == ss_unused)
				continue;
			if (dev->l1.rxslots[i].state != ss_retired) {
				ret++;
				continue;
			}
			if (idx < 0 || (signed)(tm - dev->l1.rxslots[i].tstart) > 0) {
				idx = i;
				tm = dev->l1.rxslots[i].tstart;
			}
		}
		if (idx >= 0) {
			cc_unlock(dev->l1.rxslots[idx].corr_cache);
			id = dev->l1.rxslots[idx].id;
			data = dev->l1.rxslots[idx].data;
			userdata = dev->l1.rxslots[idx].userdata;
			len = dev->l1.rxslots[idx].nbits * sizeof(hfmodem_soft_t);
			dev->l1.rxslots[idx].state = ss_unused;
			restore_flags(flags);
			ret = copy_to_user(userdata, data, len);
			kfree_s(data, len);
			return (put_user(id, (hfmodem_id_t *)arg)) ? -EFAULT : ret;
		}
		for (idx = -1, i = 0; i < HFMODEM_NUMTXSLOTS; i++) {
			if (dev->l1.txslots[i].state == ss_unused)
				continue;
			if (dev->l1.txslots[i].state != ss_retired) {
				ret++;
				continue;
			}
			if (idx < 0 || (signed)(tm - dev->l1.txslots[i].tstart) > 0) {
				idx = i;
				tm = dev->l1.txslots[i].tstart;
			}
		}
		if (idx >= 0) {
			id = dev->l1.txslots[idx].id;
			data = dev->l1.txslots[idx].data;
			len = (dev->l1.txslots[idx].nbits + 7) >> 3;
			dev->l1.txslots[idx].state = ss_unused;
			restore_flags(flags);
			kfree_s(data, len);
			return put_user(id, (hfmodem_id_t *)arg);
		}
		restore_flags(flags);
		return ret ? -EAGAIN : -EPIPE;

	case HFMODEM_IOCTL_MIXERPARAMS:
		if ((ret = copy_from_user(&mix, (void *)arg, sizeof(mix))))
			return ret;
		dev->scops->mixer(dev, mix.src, mix.igain, mix.ogain);
		return 0;
		
	case HFMODEM_IOCTL_SAMPLESTART:
		save_flags(flags);
		cli();
		if (dev->sbuf.kbuf) 
			kfree_s(dev->sbuf.kbuf, dev->sbuf.size);
		dev->sbuf.kbuf = dev->sbuf.kptr = NULL;
		dev->sbuf.size = dev->sbuf.rem = 0;
		restore_flags(flags);
		if ((ret = copy_from_user(&spar, (void *)arg, sizeof(spar))))
			return ret;
		if (spar.len == 0)
			return 0;
		if (spar.len < 2 || spar.len > 8192)
			return -EINVAL;
		if (verify_area(VERIFY_WRITE, spar.data, spar.len * sizeof(__s16)))
			return -EFAULT;
		if (!(dev->sbuf.kbuf = kmalloc(spar.len * sizeof(__s16), GFP_KERNEL)))
			return -ENOMEM;
		save_flags(flags);
		cli();
		dev->sbuf.kptr = dev->sbuf.kbuf;
		dev->sbuf.size = spar.len * sizeof(__s16);
		dev->sbuf.rem = spar.len;
		dev->sbuf.ubuf = spar.data;
		restore_flags(flags);
		return 0;
		
	case HFMODEM_IOCTL_SAMPLEFINISHED:
		save_flags(flags);
		cli();
		if (dev->sbuf.rem > 0) {
			restore_flags(flags);
			return -EAGAIN;
		}
		if (!dev->sbuf.kbuf || !dev->sbuf.size) {
			restore_flags(flags);
			return -EPIPE;
		}
		restore_flags(flags);
		ret = copy_to_user(dev->sbuf.ubuf, dev->sbuf.kbuf, dev->sbuf.size);
		kfree_s(dev->sbuf.kbuf, dev->sbuf.size);
		dev->sbuf.kbuf = dev->sbuf.kptr = NULL;
		dev->sbuf.size = dev->sbuf.rem = 0;
		return ret;
	}
}

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

#if LINUX_VERSION_CODE >= 0x20100

unsigned int hfmodem_poll(struct file *file, poll_table *wait)
{
	struct hfmodem_state *dev = &hfmodem_state[0];
	unsigned long flags;
	int i, cnt1, cnt2;
	
	poll_wait(file, &dev->wait, wait);
	save_flags(flags);
	cli();
	for (i = cnt1 = cnt2 = 0; i < HFMODEM_NUMTXSLOTS; i++) {
		if (dev->l1.txslots[i].state == ss_retired)
			cnt1++;
		if (dev->l1.txslots[i].state != ss_unused)
			cnt2++;
	}
	for (i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
		if (dev->l1.rxslots[i].state == ss_retired)
			cnt1++;
		if (dev->l1.rxslots[i].state != ss_unused)
			cnt2++;
	}
	restore_flags(flags);
        if (cnt1 || !cnt2)
                return POLLIN | POLLRDNORM;
        return 0;
}

#else 

int hfmodem_select(struct inode *inode, struct file *file, int sel_type, select_table *wait)
{
	struct hfmodem_state *dev = &hfmodem_state[0];
	unsigned long flags;
	int i, cnt1, cnt2;
	
	if (sel_type == SEL_IN) {
		save_flags(flags);
		cli();
		for (i = cnt1 = cnt2 = 0; i < HFMODEM_NUMTXSLOTS; i++) {
			if (dev->l1.txslots[i].state == ss_retired)
				cnt1++;
			if (dev->l1.txslots[i].state != ss_unused)
				cnt2++;
		}
		for (i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
			if (dev->l1.rxslots[i].state == ss_retired)
				cnt1++;
			if (dev->l1.rxslots[i].state != ss_unused)
				cnt2++;
		}
		restore_flags(flags);
		if (cnt1 || !cnt2)
			return 1;
		select_wait(&dev->wait, wait);
    	}
	return 0;
}

#endif

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

extern __inline__ unsigned int l1fsk_phinc(struct hfmodem_l1_txslot *txs, unsigned int nbit)
{
	return txs->phase_incs[!!((txs->data[nbit >> 3] ^ txs->inv) & (1 << (nbit & 7)))];
}

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

void hfmodem_input_samples(struct hfmodem_state *dev, hfmodem_time_t tstart, 
			 hfmodem_time_t tinc, __s16 *samples)
{
	hfmodem_time_t tst, tend;
	__s16 *s;
	int i, j;
	hfmodem_soft_t sample;

	dev->l1.last_time = tstart + (HFMODEM_FRAGSAMPLES-1) * tinc;
	for (i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
		struct hfmodem_l1_rxslot *rxs = dev->l1.rxslots + i;

		if (rxs->state == ss_unused || rxs->state == ss_retired)
			continue;
		tst = tstart - (rxs->corrlen-1) * tinc;
		tend = tst + (HFMODEM_FRAGSAMPLES-1) * tinc;
		if (rxs->state == ss_ready) {
			if ((signed)(rxs->tstart - tend) > 0) 
				continue;
			rxs->state = ss_oper;
		}
		for (s = samples, j = 0; j < HFMODEM_FRAGSAMPLES; j++, s++, tst += tinc)
			if ((signed)(rxs->tstart - tst) <= 0) {
				sample = do_filter(rxs, s);
				while ((signed)(rxs->tstart - tst) <= 0 && 
				       rxs->cntbits < rxs->nbits) {
					rxs->data[rxs->cntbits] = sample;
					rxs->cntbits++;
					rxs->tstart += rxs->tinc;
				}
				if (rxs->cntbits >= rxs->nbits) {
					rxs->state = ss_retired;
					break;
				}
			}
	}
}

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

extern __inline__ unsigned int output_one_sample(struct hfmodem_state *dev, hfmodem_time_t tm)
{
	int i, j, k;
	struct hfmodem_l1_txslot *txs;
	/*
	 * first activate new output slots
	 */
	for (j = -1, i = 0; i < HFMODEM_NUMTXSLOTS; i++) {
		txs = dev->l1.txslots + i;
		if (txs->state == ss_ready && (signed)(txs->tstart - tm) <= 0) {
			for (k = 0; k < HFMODEM_NUMTXSLOTS; k++) {
				if (dev->l1.txslots[k].state != ss_oper)
					continue;
				dev->l1.txslots[k].state = ss_retired;
			}
			txs->state = ss_oper;
			txs->tstart += txs->tinc;
			txs->phinc = l1fsk_phinc(txs, 0);
			txs->cntbits = 1;
		};
		if (txs->state != ss_oper)
			continue;
		j = i;
	}
	if (j < 0 || j >= HFMODEM_NUMTXSLOTS)
		return 0;
	/*
	 * calculate the current slot
	 */
	txs = dev->l1.txslots + j;
	while ((signed)(txs->tstart - tm) <= 0) {
		if (txs->cntbits >= txs->nbits) {
			txs->state = ss_retired;
			return 0;
		}
		txs->tstart += txs->tinc;
		txs->phinc = l1fsk_phinc(txs, txs->cntbits);
		txs->cntbits++;
	}
	return txs->phinc;
}

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

int hfmodem_output_samples(struct hfmodem_state *dev, hfmodem_time_t tstart, 
			   hfmodem_time_t tinc, __s16 *samples)
{
	int i, j;
	hfmodem_time_t tend = tstart + (HFMODEM_FRAGSAMPLES-1) * tinc;

	for (i = 0; i < HFMODEM_NUMTXSLOTS; i++) {
		if (dev->l1.txslots[i].state == ss_oper)
			break;
		if (dev->l1.txslots[i].state == ss_ready && 
		    (signed)(dev->l1.txslots[i].tstart - tend) <= 0)
			break;
	}
	if (i >= HFMODEM_NUMTXSLOTS)
		return 0;
	for (j = 0; j < HFMODEM_FRAGSAMPLES; j++, tstart += tinc, samples++) {
		*samples = isimplecos(dev->l1.tx_phase);
		dev->l1.tx_phase += output_one_sample(dev, tstart);
	}
	return 1;
}

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

long hfmodem_next_tx_event(struct hfmodem_state *dev, hfmodem_time_t curr)
{
	long diff = LONG_MAX, t;
	int i;

	for (i = 0; i < HFMODEM_NUMTXSLOTS; i++) {
		if (dev->l1.txslots[i].state == ss_oper)
			if (diff > 0)
				diff = 0;
		if (dev->l1.txslots[i].state == ss_ready) {
			t = dev->l1.txslots[i].tstart - curr;
			if (t < diff)
				diff = t;
		}
	}
	return diff;
}

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

void hfmodem_finish_pending_rx_requests(struct hfmodem_state *dev)
{
	int i;

	for (i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
		if (dev->l1.rxslots[i].state != ss_oper)
			continue;
		while (dev->l1.rxslots[i].cntbits < dev->l1.rxslots[i].nbits) {
			dev->l1.rxslots[i].data[dev->l1.rxslots[i].cntbits] = 0;
			dev->l1.rxslots[i].cntbits++;
		}
		dev->l1.rxslots[i].state = ss_retired;
	}
}

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

void hfmodem_wakeup(struct hfmodem_state *dev)
{
	int i, cnt1, cnt2;
	
	for (i = cnt1 = cnt2 = 0; i < HFMODEM_NUMTXSLOTS; i++) {
		if (dev->l1.txslots[i].state == ss_retired)
			cnt1++;
		if (dev->l1.txslots[i].state != ss_unused)
			cnt2++;
	}
	for (i = 0; i < HFMODEM_NUMRXSLOTS; i++) {
		if (dev->l1.rxslots[i].state == ss_retired)
			cnt1++;
		if (dev->l1.rxslots[i].state != ss_unused)
			cnt2++;
	}
	if (cnt1 || !cnt2)
		wake_up_interruptible(&dev->wait);
}

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