amdtp.c

IEE1394 火线接口驱动 for linux

	/* Here we check when (which cycle) the last packet was sent
	 * and compare it to what the iso packer was using at the
	 * time.  If there is a mismatch, we adjust the cycle count in
	 * the iso packer.  However, there are still up to
	 * MAX_PACKET_LISTS packet lists queued with bad time stamps,
	 * so we disable time stamp monitoring for the next
	 * MAX_PACKET_LISTS packet lists.
	 */
	diff = (last->db->payload_desc.status - pl->last_cycle_count) & 0xf;
	if (diff > 0 && s->stale_count == 0) {
		atomic_add(diff, &s->cycle_count);
		atomic_add(diff, &s->cycle_count2);
		s->stale_count = MAX_PACKET_LISTS;
	}

	if (s->stale_count > 0)
		s->stale_count--;

	/* Finally, we move the packet list that was just processed
	 * back to the free list, and notify any waiters.
	 */
	spin_lock(&s->packet_list_lock);
	list_del(&pl->link);
	list_add_tail(&pl->link, &s->free_packet_lists);
	spin_unlock(&s->packet_list_lock);

	wake_up_interruptible(&s->packet_list_wait);
}

static struct packet *stream_current_packet(struct stream *s)
{
	if (s->current_packet_list == NULL &&
	    (s->current_packet_list = stream_get_free_packet_list(s)) == NULL)
		return NULL;

	return &s->current_packet_list->packets[s->current_packet];
}
	
static void stream_queue_packet(struct stream *s)
{
	s->current_packet++;
	if (s->current_packet == PACKET_LIST_SIZE) {
		stream_put_dma_packet_list(s, s->current_packet_list);
		s->current_packet_list = NULL;
		s->current_packet = 0;
	}
}

/* Integer fractional math.  When we transmit a 44k1Hz signal we must
 * send 5 41/80 samples per isochronous cycle, as these occur 8000
 * times a second.  Of course, we must send an integral number of
 * samples in a packet, so we use the integer math to alternate
 * between sending 5 and 6 samples per packet.
 */

static void fraction_init(struct fraction *f, int numerator, int denominator)
{
	f->integer = numerator / denominator;
	f->numerator = numerator % denominator;
	f->denominator = denominator;
}

static __inline__ void fraction_add(struct fraction *dst,
				    struct fraction *src1,
				    struct fraction *src2)
{
	/* assert: src1->denominator == src2->denominator */

	int sum, denom;

	/* We use these two local variables to allow gcc to optimize
	 * the division and the modulo into only one division. */

	sum = src1->numerator + src2->numerator;
	denom = src1->denominator;
	dst->integer = src1->integer + src2->integer + sum / denom;
	dst->numerator = sum % denom;
	dst->denominator = denom;
}

static __inline__ void fraction_sub_int(struct fraction *dst,
					struct fraction *src, int integer)
{
	dst->integer = src->integer - integer;
	dst->numerator = src->numerator;
	dst->denominator = src->denominator;
}

static __inline__ int fraction_floor(struct fraction *frac)
{
	return frac->integer;
}

static __inline__ int fraction_ceil(struct fraction *frac)
{
	return frac->integer + (frac->numerator > 0 ? 1 : 0);
}

void packet_initialize(struct packet *p, struct packet *next)
{
	/* Here we initialize the dma descriptor block for
	 * transferring one iso packet.  We use two descriptors per
	 * packet: an OUTPUT_MORE_IMMMEDIATE descriptor for the
	 * IEEE1394 iso packet header and an OUTPUT_LAST descriptor
	 * for the payload.
	 */

	p->db->header_desc.control =
		DMA_CTL_OUTPUT_MORE | DMA_CTL_IMMEDIATE | 8;

	if (next) {
		p->db->payload_desc.control = 
			DMA_CTL_OUTPUT_LAST | DMA_CTL_BRANCH;
		p->db->payload_desc.branch = next->db_bus | 3;
		p->db->header_desc.skip = next->db_bus | 3;
	}
	else {
		p->db->payload_desc.control = 
			DMA_CTL_OUTPUT_LAST | DMA_CTL_BRANCH |
			DMA_CTL_UPDATE | DMA_CTL_IRQ;
		p->db->payload_desc.branch = 0;
		p->db->header_desc.skip = 0;
	}
	p->db->payload_desc.data_address = p->payload_bus;
	p->db->payload_desc.status = 0;
}

struct packet_list *packet_list_alloc(struct stream *s)
{
	int i;
	struct packet_list *pl;
	struct packet *next;

	pl = kmalloc(sizeof *pl, SLAB_KERNEL);
	if (pl == NULL)
		return NULL;

	for (i = 0; i < PACKET_LIST_SIZE; i++) {
		struct packet *p = &pl->packets[i];
		p->db = pci_pool_alloc(s->descriptor_pool, SLAB_KERNEL,
				       &p->db_bus);
		p->payload = pci_pool_alloc(s->packet_pool, SLAB_KERNEL,
					    &p->payload_bus);
	}

	for (i = 0; i < PACKET_LIST_SIZE; i++) {
		if (i < PACKET_LIST_SIZE - 1)
			next = &pl->packets[i + 1];
		else 
			next = NULL;
		packet_initialize(&pl->packets[i], next);
	}

	return pl;
}

void packet_list_free(struct packet_list *pl, struct stream *s)
{
	int i;

	for (i = 0; i < PACKET_LIST_SIZE; i++) {
		struct packet *p = &pl->packets[i];
		pci_pool_free(s->descriptor_pool, p->db, p->db_bus);
		pci_pool_free(s->packet_pool, p->payload, p->payload_bus);
	}
	kfree(pl);
}

static struct buffer *buffer_alloc(int size)
{
	struct buffer *b;

	b = kmalloc(sizeof *b + size, SLAB_KERNEL);
	if (b == NULL)
		return NULL;
	b->head = 0;
	b->tail = 0;
	b->length = 0;
	b->size = size;

	return b;
}

static unsigned char *buffer_get_bytes(struct buffer *buffer, int size)
{
	unsigned char *p;

	if (buffer->head + size > buffer->size)
		BUG();

	p = &buffer->data[buffer->head];
	buffer->head += size;
	if (buffer->head == buffer->size)
		buffer->head = 0;
	buffer->length -= size;

	return p;
}

static unsigned char *buffer_put_bytes(struct buffer *buffer,
				       size_t max, size_t *actual)
{
	size_t length;
	unsigned char *p;

	p = &buffer->data[buffer->tail];
	length = min(buffer->size - buffer->length, max);
	if (buffer->tail + length < buffer->size) {
		*actual = length;
		buffer->tail += length;
	}
	else {
		*actual = buffer->size - buffer->tail;
		 buffer->tail = 0;
	}

	buffer->length += *actual;
	return p;
}

static u32 get_iec958_header_bits(struct stream *s, int sub_frame, u32 sample)
{
	int csi, parity, shift;
	int block_start;
	u32 bits;

	switch (s->iec958_frame_count) {
	case 1:
		csi = s->format == AMDTP_FORMAT_IEC958_AC3;
		break;
	case 2:
	case 9:
		csi = 1;
		break;
	case 24 ... 27:
		csi = (s->iec958_rate_code >> (27 - s->iec958_frame_count)) & 0x01;
		break;
	default:
		csi = 0;
		break;
	}

	block_start = (s->iec958_frame_count == 0 && sub_frame == 0);

	/* The parity bit is the xor of the sample bits and the
	 * channel status info bit. */
	for (shift = 16, parity = sample ^ csi; shift > 0; shift >>= 1)
		parity ^= (parity >> shift);

	bits =  (block_start << 5) |		/* Block start bit */
		((sub_frame == 0) << 4) |	/* Subframe bit */
		((parity & 1) << 3) |		/* Parity bit */
		(csi << 2);			/* Channel status info bit */

	return bits;
}

static u32 get_header_bits(struct stream *s, int sub_frame, u32 sample)
{
	switch (s->format) {
	case AMDTP_FORMAT_IEC958_PCM:
	case AMDTP_FORMAT_IEC958_AC3:
		return get_iec958_header_bits(s, sub_frame, sample);
		
	case AMDTP_FORMAT_RAW:
		return 0x40;

	default:
		return 0;
	}
}

static void fill_payload_le16(struct stream *s, quadlet_t *data, int nevents)
{
	quadlet_t *event, sample, bits;
	unsigned char *p;
	int i, j;

	for (i = 0, event = data; i < nevents; i++) {

		for (j = 0; j < s->dimension; j++) {
			p = buffer_get_bytes(s->input, 2);
			sample = (p[1] << 16) | (p[0] << 8);
			bits = get_header_bits(s, j, sample);
			event[j] = cpu_to_be32((bits << 24) | sample);
		}

		event += s->dimension;
		if (++s->iec958_frame_count == 192)
			s->iec958_frame_count = 0;
	}
}

static void fill_packet(struct stream *s, struct packet *packet, int nevents)
{
	int syt_index, syt, size;
	u32 control;

	size = (nevents * s->dimension + 2) * sizeof(quadlet_t);

	/* Update DMA descriptors */
	packet->db->payload_desc.status = 0;
	control = packet->db->payload_desc.control & 0xffff0000;
	packet->db->payload_desc.control = control | size;

	/* Fill IEEE1394 headers */
	packet->db->header_desc.header[0] =
		(IEEE1394_SPEED_100 << 16) | (0x01 << 14) | 
		(s->iso_channel << 8) | (TCODE_ISO_DATA << 4);
	packet->db->header_desc.header[1] = size << 16;
	
	/* Calculate synchronization timestamp (syt). First we
	 * determine syt_index, that is, the index in the packet of
	 * the sample for which the timestamp is valid. */
	syt_index = (s->syt_interval - s->dbc) & (s->syt_interval - 1);
	if (syt_index < nevents) {
		syt = ((atomic_read(&s->cycle_count) << 12) | 
		       s->cycle_offset.integer) & 0xffff;
		fraction_add(&s->cycle_offset, 
			     &s->cycle_offset, &s->ticks_per_syt_offset);

		/* This next addition should be modulo 8000 (0x1f40),
		 * but we only use the lower 4 bits of cycle_count, so
		 * we don't need the modulo. */
		atomic_add(s->cycle_offset.integer / 3072, &s->cycle_count);
		s->cycle_offset.integer %= 3072;
	}
	else
		syt = 0xffff;

	atomic_inc(&s->cycle_count2);
	
	/* Fill cip header */
	packet->payload->eoh0 = 0;
	packet->payload->sid = s->host->host->node_id & 0x3f;
	packet->payload->dbs = s->dimension;
	packet->payload->fn = 0;
	packet->payload->qpc = 0;
	packet->payload->sph = 0;
	packet->payload->reserved = 0;
	packet->payload->dbc = s->dbc;
	packet->payload->eoh1 = 2;
	packet->payload->fmt = FMT_AMDTP;
	packet->payload->fdf = s->fdf;
	packet->payload->syt = cpu_to_be16(syt);

	switch (s->sample_format) {
	case AMDTP_INPUT_LE16:
		fill_payload_le16(s, packet->payload->data, nevents);
		break;
	}

	s->dbc += nevents;
}

static void stream_flush(struct stream *s)
{
	struct packet *p;
	int nevents;
	struct fraction next;

	/* The AMDTP specifies two transmission modes: blocking and
	 * non-blocking.  In blocking mode you always transfer
	 * syt_interval or zero samples, whereas in non-blocking mode
	 * you send as many samples as you have available at transfer
	 * time.
	 *
	 * The fraction samples_per_cycle specifies the number of
	 * samples that become available per cycle.  We add this to
	 * the fraction ready_samples, which specifies the number of
	 * leftover samples from the previous transmission.  The sum,
	 * stored in the fraction next, specifies the number of
	 * samples available for transmission, and from this we
	 * determine the number of samples to actually transmit.
	 */

	while (1) {
		fraction_add(&next, &s->ready_samples, &s->samples_per_cycle);
		if (s->mode == AMDTP_MODE_BLOCKING) {
			if (fraction_floor(&next) >= s->syt_interval)
				nevents = s->syt_interval;
			else
				nevents = 0;
		}
		else
			nevents = fraction_floor(&next);

		p = stream_current_packet(s);
		if (s->input->length < nevents * s->dimension * 2 || p == NULL)
			break;

		fill_packet(s, p, nevents);
		stream_queue_packet(s);

		/* Now that we have successfully queued the packet for
		 * transmission, we update the fraction ready_samples. */
		fraction_sub_int(&s->ready_samples, &next, nevents);
	}
}

static int stream_alloc_packet_lists(struct stream *s)
{
	int max_nevents, max_packet_size, i;

	if (s->mode == AMDTP_MODE_BLOCKING)
		max_nevents = s->syt_interval;
	else
		max_nevents = fraction_ceil(&s->samples_per_cycle);

	max_packet_size = max_nevents * s->dimension * 4 + 8;
	s->packet_pool = pci_pool_create("packet pool", s->host->ohci->dev,
					 max_packet_size, 0, 0 ,SLAB_KERNEL);

	if (s->packet_pool == NULL)
		return -1;

	INIT_LIST_HEAD(&s->free_packet_lists);
	INIT_LIST_HEAD(&s->dma_packet_lists);
	for (i = 0; i < MAX_PACKET_LISTS; i++) {
		struct packet_list *pl = packet_list_alloc(s);
		if (pl == NULL)
			break;
		list_add_tail(&pl->link, &s->free_packet_lists);
	}

	return i < MAX_PACKET_LISTS ? -1 : 0;
}

static void stream_free_packet_lists(struct stream *s)
{
	struct list_head *lh, *next;

	if (s->current_packet_list != NULL)