iso1394dataflowsource.c

从 IEEE 1394总线接收传输流

/** retrieves a new iso packet, if necessary */
static int getIsoPacket() {
	fd_set rfds;
	struct timeval tv, endtime, current_time;
	long usec_temp;
	int fd=raw1394_get_fd(rawhandle_iso);
	int no_packet = 0;

	(void) gettimeofday(&endtime, NULL);
	usec_temp = endtime.tv_usec + 100000;
	endtime.tv_sec += usec_temp / 1000000;
	endtime.tv_usec = usec_temp % 1000000;

	while ((last==current) && !no_packet)
	{
		FD_ZERO(&rfds);
		FD_SET(fd, &rfds);
		tv.tv_sec = 0;
		tv.tv_usec = 50000;

		switch (select(fd+1, &rfds, NULL, NULL, &tv))
		{
		case 0: /* we got no packet */
			no_packet = 1;
			fprintf(stderr,"IsoDataFlowSource: No iso packet for over 50 ms.\n");
			if (pthread_mutex_lock(&current_cycle_time_mutex)) /* lock mutex */
			{
				fprintf(stderr,"IsoDataFlowSource: cannot lock current cycle time mutex\n");
				exit(1);
			}
			packets_since_last_update_ct = MAX_PACKETS_BETWEEN_UPDATES; /* next packet should update current_cycle_time */
			if (pthread_mutex_unlock(&current_cycle_time_mutex)) /* unlock mutex */
			{
				fprintf(stderr,"IsoDataFlowSource: cannot unlock current cycle time mutex\n");
				exit(1);
			}
			break;
		case 1: /* we probably got an iso packet -> process it */
			if (raw1394_loop_iterate(rawhandle_iso) < 0) {
				fprintf(stderr,"Iso1394DataFlowSource: An error occured while waiting for iso packet\n");
			}
			break;
		default:
			fprintf(stderr,"An error occured during select\n");;
			exit(1);
		}
		if ((last==current) && !no_packet) {
			(void) gettimeofday(&current_time, NULL);
			if (timercmp(&current_time, &endtime, >)) {
				no_packet = 1;
				fprintf(stderr,"IsoDataFlowSource: Got iso packets, but no complete MPEG packet for over 100 ms.\n");
			}
		}
	}
	return !no_packet;
}


static enum raw1394_iso_disposition iso_handler(raw1394handle_t handle, unsigned char *data,  unsigned int length, unsigned char channel, unsigned char tag, unsigned char sy, unsigned int cycle, unsigned int dropped) {
	int fn,fn_short;
	int dbs,seq_block,seq_sp, sp_modul;
	unsigned int pos;
	enum raw1394_iso_disposition retval = RAW1394_ISO_OK;

	if (dropped) {
		fprintf(stderr,"IsoDataFlowSource: some iso packets dropped\n");
	}

	if (pthread_mutex_lock(&current_cycle_time_mutex)) /* lock mutex */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot lock current cycle time mutex\n");
		exit(1);
	}
	if (packets_since_last_update_ct >= MAX_PACKETS_BETWEEN_UPDATES) {
		getCurrentCycleTime(&current_cycle_time);
		packets_since_last_update_ct = 0;
	} else {
		packets_since_last_update_ct++;
	}
	if (pthread_mutex_unlock(&current_cycle_time_mutex)) /* unlock mutex */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot unlock current cycle time mutex\n");
		exit(1);
	}

	if (channel!=ch_number) {
		fprintf(stderr,"IsoDataFlowSource: receiving packet from wrong channel\n");
	} else {
		if (tag != 1) {
			fprintf(stderr,"IsoDataFlowSource: invalid iso header; maybe no cip header\n");
		} else {
			if (data[0] >> 6) {
				fprintf(stderr,"IsoDataFlowSource: first cip-header byte does not start 00\n");
			} else {
				switch (data[4])
				{
				case 0xBF:
					fprintf(stderr,"IsoDataFlowSource: skipping empty iso packet\n");
					if (length !=12)
						fprintf(stderr,"IsoDataFlowSource: empty packet has illegal size\n");;
					break;
				case 0xA0:
					dbs = data[1];
					if (data_block_size)
						if (data_block_size != dbs)
							fprintf(stderr,"IsoDataFlowSource: data block size may not change\n");;
					data_block_size=dbs;
					fn_short = data[2] >> 6;
					fn = 1 << fn_short;
					if (fraction_number)
						if (fraction_number != fn)
							fprintf(stderr,"IsoDataFlowSource: fraction number may not change\n");;
					fraction_number=fn;
					if (dbs * fn != 192/4)
						fprintf(stderr,"IsoDataFlowSource: illegal source packet length\n");;
					if ((data[2] & 0x3F) != 0x04)
						fprintf(stderr,"IsoDataFlowSource: must have qpc=0, sph=1 and rsv=0\n");;
					seq_block=data[3] & (fn - 1);
					seq_sp=data[3] >> fn_short;
					if (seq_block !=next_block)
						fprintf(stderr,"IsoDataFlowSource: uncorrect block sequence number (expected: %d; gotten: %d); data lost\n", next_block, seq_block);
					if (seq_sp !=next_source_packet)
						fprintf(stderr,"IsoDataFlowSource: uncorrect source packet sequence number(expected: %d; gotten: %d); data lost\n", next_source_packet, seq_sp);
					sp_modul= 1 << (8-fn_short);
					for (pos=2; (pos << 2) < length; pos += dbs)
					{
						memcpy(buffer[last] + ((dbs << 2) * seq_block), data + (pos << 2), dbs << 2);
						seq_block++;
						seq_block %= fn;
						if (!seq_block) /* if we completed a new source packet */
						{
							int sph_cycle_count;
							int cycle_count_diff;
							int new_last;

							seq_sp++;
							seq_sp %= sp_modul; /* advance sequence counter */
							/* complete/reconstruct second-count-fiels of timestamp in CIP packet */
							buffer[last][0] &= 0x01; /* make sure that the first 7 bits are 0 (as they should be in a compliant stream */
							sph_cycle_count=(buffer[last][0] << 12) | (buffer[last][1] << 4) | (buffer[last][2] >> 4);

							if (pthread_mutex_lock(&current_cycle_time_mutex)) /* lock mutex */
							{
								fprintf(stderr,"IsoDataFlowSource: cannot lock current cycle time mutex\n");
								exit(1);
							}
							cycle_count_diff=sph_cycle_count - current_cycle_time.cycle_count;
							if (cycle_count_diff < -3999)
								cycle_count_diff += 8000;
							else if (cycle_count_diff > 4000)
								cycle_count_diff -= 8000;

							if (current_cycle_time.cycle_count + cycle_count_diff > 7999)
								buffer[last][0] |= ((current_cycle_time.second_count+1) << 1);
							else if (current_cycle_time.cycle_count + cycle_count_diff < 0)
								buffer[last][0] |= ((current_cycle_time.second_count+127) << 1);
							else
								buffer[last][0] |= (current_cycle_time.second_count << 1);

							if (pthread_mutex_unlock(&current_cycle_time_mutex)) /* unlock mutex */
							{
								fprintf(stderr,"IsoDataFlowSource: cannot unlock current cycle time mutex\n");
								exit(1);
							}
							new_last = last + 1;
							new_last %= TS_BUFFER_ELEMENTS;
							if (new_last == current) {
								fprintf(stderr,"IsoDataFlowSource: TS packet buffer full, discarding packet\n");
								retval = RAW1394_ISO_DEFER;
							} else {
								last = new_last; /* advance end counter of buffer */
							}
						}
					}
					if ((pos << 2) != length)
						fprintf(stderr,"IsoDataFlowSource: invalid iso packet length %d\n", length);
					next_block=seq_block;
					next_source_packet=seq_sp;
					isopackets++;
					break;
				default:
					fprintf(stderr,"IsoDataFlowSource: iso packet has unexpected type\n");;
					break;
				}
			}
		}
	}
	if (((last - current + TS_BUFFER_ELEMENTS) % TS_BUFFER_ELEMENTS) + TS_BUFFER_MARGIN > TS_BUFFER_ELEMENTS) {
//		fprintf(stderr,"IsoDataFlowSource: TS buffer almost full. Stop fetching data from kernel\n");
		retval = RAW1394_ISO_DEFER;
	}
	return retval;
}


/* These functions are needed for synchronisation. They are placed here, because only
   the dataflowsource has exact knowledge of the arrival time of the TS packets */

/* set system time clock to a given value (pcr for the current packet)*/
void setSTC(unsigned long long int pcr_base, unsigned int pcr_ext)
{
	pcr_base=(pcr_base+8589934592ull-PCR_DELAY) % 8589934592ull; /* generate constant delay */
	if (pthread_mutex_lock(&set_pcr_mutex)) /* lock mutex */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot lock pcr mutex\n");;
		exit(1);
	}
	pcr0=pcr1;pcr1=pcr2;pcr2=pcr3;
	cycle_time0=cycle_time1;cycle_time1=cycle_time2;cycle_time2=cycle_time3;

	pcr3=pcr_base*300+pcr_ext;   /* calculate new cycle time */
	cycle_time3.second_count=buffer[current][0] >> 1;
	cycle_time3.cycle_count=((buffer[current][0] & 0x01) << 12) | (buffer[current][1] << 4) | (buffer[current][2] >> 4);
	cycle_time3.cycle_offset=((buffer[current][2] & 0x0f) << 8) | buffer[current][3];

	if (valid_pcrs < 4) valid_pcrs++; /* eventually enable sync */
	if (pthread_mutex_unlock(&set_pcr_mutex)) /* lock mutex */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot unlock pcr mutex\n");;
		exit(1);
	}
}

/* get the current value of the STC */
unsigned long long int getSTC()
{
	struct cycle_time1394 ct_diff, delta_ct, cycle_time;
	long int ct_diff_int, delta_ct_int;
	long long int current_stc;

	if (pthread_mutex_lock(&set_pcr_mutex)) /* lock mutex to ensure that pcr-ct-pairs are not changed while we read them */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot lock pcr mutex\n");;
		exit(1);
	}

	/* calculate difference between the last two CIP-SPH-cycle times */
	ct_diff.cycle_offset=cycle_time3.cycle_offset-cycle_time0.cycle_offset;
	ct_diff.cycle_count=cycle_time3.cycle_count-cycle_time0.cycle_count;
	ct_diff.second_count=cycle_time3.second_count-cycle_time0.second_count;
	if (ct_diff.second_count < 0)
		ct_diff.second_count += 128; /* handle wrap_around of cycle time */


	/* convert to units of the 24,576-MHz-clock */
	ct_diff_int=((ct_diff.second_count * 8000l) + ct_diff.cycle_count) * 3072 +ct_diff.cycle_offset;

	getCurrentCycleTime(&cycle_time);

	if (pthread_mutex_lock(&current_cycle_time_mutex)) /* lock mutex to ensure validity of current_cycle_time */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot lock current cycle time mutex\n");;
		exit(1);
	}
	current_cycle_time = cycle_time;
	packets_since_last_update_ct = 0;
	if (pthread_mutex_unlock(&current_cycle_time_mutex)) /* unlock mutex */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot unlock current cycle time mutex\n");;
		exit(1);
	}

	/* calculate difference between current cycle time and that of the last CIP-SPH */
	delta_ct.cycle_offset=cycle_time.cycle_offset-cycle_time3.cycle_offset;
	delta_ct.cycle_count=cycle_time.cycle_count-cycle_time3.cycle_count;
	delta_ct.second_count=cycle_time.second_count-cycle_time3.second_count;

	/* bring delta_ct.second_count into the interval [-64;63] */
	if (delta_ct.second_count < -64) /* is in [-127;-65] */
		delta_ct.second_count +=128;
	else if (delta_ct.second_count > 63) /* is in [64;127] */
		delta_ct.second_count -= 128;

	/* convert to units of the 24,576-MHz-clock */
	delta_ct_int=((delta_ct.second_count * 8000l) + delta_ct.cycle_count) * 3072 + delta_ct.cycle_offset;

	/* calculate current STC */
	current_stc=pcr3 + ((long long int)(pcr3-pcr0) * delta_ct_int) / ct_diff_int;
	if (pthread_mutex_unlock(&set_pcr_mutex)) /* lock mutex */
	{
		fprintf(stderr,"IsoDataFlowSource: cannot unlock pcr mutex\n");;
		exit(1);
	}

	return (current_stc / 300) & 0x01ffffffffull; /* calculate pcr_base and only return lower 33 bits */
}

void disable_sync()
{
	valid_pcrs=0;
	if (debug) {
		fprintf(stdout,"IsoDataFlowSource: sync disabled\n");
	}
}

int is_sync_enabled()
{
	return (valid_pcrs >= 4);
}