ctools.c

DVBstream is based on the ts-rtp package available at http://www.linuxtv.org. It broadcasts a (subs

/*
 *  mpegtools for the Siemens Fujitsu DVB PCI card
 *
 * Copyright (C) 2000, 2001 Marcus Metzler 
 *            for convergence integrated media GmbH
 * 
 * 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
 * 

 * The author can be reached at [e-mail], 

 * the project's page is at http://linuxtv.org/dvb/
 */

#include "ctools.h"

#define MAX_SEARCH 1024 * 1024


/*

      PES
  
*/


void init_pes(pes_packet *p){
	p->stream_id = 0;
	p->llength[0] = 0;
	p->llength[1] = 0;
	p->length = 0;
	p->flags1 = 0x80;
	p->flags2 = 0;
	p->pes_hlength = 0;
	p->trick = 0;
	p->add_cpy = 0;
	p->priv_flags = 0;
	p->pack_field_length = 0;
	p->pack_header = (u8 *) NULL;
	p->pck_sqnc_cntr = 0;
	p->org_stuff_length = 0;
	p->pes_ext_lngth = 0;
	p->pes_ext = (u8 *) NULL;
	p->pes_pckt_data = (u8 *) NULL;
	p->padding = 0;
	p->mpeg = 2; // DEFAULT MPEG2
	p->mpeg1_pad = 0;
	p->mpeg1_headr = NULL;
	p->stuffing = 0;
}

void kill_pes(pes_packet *p){
	if (p->pack_header)
		free(p->pack_header);
	if (p->pes_ext)
		free(p->pes_ext);
	if (p->pes_pckt_data)
		free(p->pes_pckt_data);
	if (p->mpeg1_headr)
		free(p->mpeg1_headr);
	init_pes(p);
}

void setlength_pes(pes_packet *p){
 	short *ll;
	ll = (short *) p->llength;
	p->length = ntohs(*ll);
}

static void setl_pes(pes_packet *p){
	setlength_pes(p);
	if (p->length)
		p->pes_pckt_data = (u8 *)malloc(p->length);
}

void nlength_pes(pes_packet *p){
	if (p->length <= 0xFFFF){
		short *ll = (short *) p->llength;
		short l = p->length;
		*ll = htons(l);
	} else {
		p->llength[0] =0x00;
		p->llength[1] =0x00;
	}
}

static void nl_pes(pes_packet *p)
{
	nlength_pes(p);
	p->pes_pckt_data = (u8 *) malloc(p->length);
}

void pts2pts(u8 *av_pts, u8 *pts)
{
  
	av_pts[0] = ((pts[0] & 0x06) << 5) | 
		((pts[1] & 0xFC) >> 2); 
	av_pts[1] = ((pts[1] & 0x03) << 6) |
		((pts[2] & 0xFC) >> 2); 
	av_pts[2] = ((pts[2] & 0x02) << 6) |
			((pts[3] & 0xFE) >> 1);
	av_pts[3] = ((pts[3] & 0x01) << 7) |
		((pts[4] & 0xFE) >> 1);
	
}


int cwrite_pes(u8 *buf, pes_packet *p, long length){
	int count,i;
	u8 dummy;
	int more = 0;
	u8 headr[3] = { 0x00, 0x00 , 0x01};

	if (length <  p->length+p->pes_hlength){
		fprintf(stderr,"Wrong buffer size in cwrite_pes\n");
		exit(1);
	}


	memcpy(buf,headr,3);
	count = 3;
	buf[count] = p->stream_id;
	count++;

	switch ( p->stream_id ) {
				
	case PROG_STREAM_MAP:
	case PRIVATE_STREAM2:
	case PROG_STREAM_DIR:
	case ECM_STREAM     :
	case EMM_STREAM     :
	case PADDING_STREAM :
		buf[count] = p->llength[0];
		count++;
		buf[count] = p->llength[1];
		count++;
		memcpy(buf+count,p->pes_pckt_data,p->length);
		count += p->length;
		break;
	case DSM_CC_STREAM  :
	case ISO13522_STREAM:
	case PRIVATE_STREAM1:
	case AUDIO_STREAM_S ... AUDIO_STREAM_E:
	case VIDEO_STREAM_S ... VIDEO_STREAM_E:
		buf[count] = p->llength[0];
		count++;
		buf[count] = p->llength[1];
		count++;
		more = 1;
		break;
	}	
	

	if ( more ) {
		if ( p->mpeg == 2 ){
			memcpy(buf+count,&p->flags1,1);
			count++;
			memcpy(buf+count,&p->flags2,1);
			count++;
			memcpy(buf+count,&p->pes_hlength,1);
			count++;
			
			if ((p->flags2 & PTS_DTS_FLAGS) == PTS_ONLY){
				memcpy(buf+count,p->pts,5);
				count += 5;
			} else 
				if ((p->flags2 & PTS_DTS_FLAGS) == PTS_DTS){
					memcpy(buf+count,p->pts,5);
					count += 5;
					memcpy(buf+count,p->dts,5);
					count += 5;
				}
			if (p->flags2 & ESCR_FLAG){
				memcpy(buf+count,p->escr,6);
				count += 6;
			}
			if (p->flags2 & ES_RATE_FLAG){
				memcpy(buf+count,p->es_rate,3);
				count += 3;
			}
			if (p->flags2 & DSM_TRICK_FLAG){
				memcpy(buf+count,&p->trick,1);
				count++;
			}
			if (p->flags2 & ADD_CPY_FLAG){
				memcpy(buf+count,&p->add_cpy,1);
				count++;
			}
			if (p->flags2 & PES_CRC_FLAG){
				memcpy(buf+count,p->prev_pes_crc,2);
				count += 2;
			}
			if (p->flags2 & PES_EXT_FLAG){
				memcpy(buf+count,&p->priv_flags,1);
				count++;

				if (p->priv_flags & PRIVATE_DATA){
					memcpy(buf+count,p->pes_priv_data,16);
					count += 16;
				}
				if (p->priv_flags & HEADER_FIELD){
					memcpy(buf+count,&p->pack_field_length,
					       1);
					count++;
					memcpy(buf+count,p->pack_header,
						     p->pack_field_length);
					count += p->pack_field_length;

				}
				
				if ( p->priv_flags & PACK_SEQ_CTR){
					memcpy(buf+count,&p->pck_sqnc_cntr,1);
					count++;
					memcpy(buf+count,&p->org_stuff_length,
					       1);
					count++;
				}
				
				if ( p->priv_flags & P_STD_BUFFER){
					memcpy(buf+count,p->p_std,2);
					count += 2;
				}
				if ( p->priv_flags & PES_EXT_FLAG2){
					memcpy(buf+count,&p->pes_ext_lngth,1);
					count++;
					memcpy(buf+count,p->pes_ext,
						     p->pes_ext_lngth);
					count += p->pes_ext_lngth;
				}
			}
			dummy = 0xFF;
			for (i=0;i<p->stuffing;i++) {
				memcpy(buf+count,&dummy,1);
				count++;
			}
		} else {
			if (p->mpeg1_pad){
				memcpy(buf+count,p->mpeg1_headr,p->mpeg1_pad);
				count += p->mpeg1_pad;
			}
			if ((p->flags2 & PTS_DTS_FLAGS) == PTS_ONLY){
				memcpy(buf+count,p->pts,5);
				count += 5;
			}
			else if ((p->flags2 & PTS_DTS_FLAGS) == 
				 PTS_DTS){
				memcpy(buf+count,p->pts,5);
					count += 5;
				memcpy(buf+count,p->dts,5);
					count += 5;
			}
		}			
		memcpy(buf+count,p->pes_pckt_data,p->length);
		count += p->length;
	}

	return count;

}

void write_pes(int fd, pes_packet *p){
	long length;
	u8 *buf;
	int l = p->length+p->pes_hlength;
	
	buf = (u8 *) malloc(l);
	length = cwrite_pes(buf,p,l);
	write(fd,buf,length);
	free(buf);
}

static unsigned int find_length(int f){
	uint64_t p = 0;
	uint64_t start = 0;
	uint64_t q = 0;
	int found = 0;
	u8 sync4[4];
	int neof = 1;

	start = lseek(f,0,SEEK_CUR);
	start -=2;
        lseek(f,start,SEEK_SET);
	while ( neof > 0 && !found ){
		p = lseek(f,0,SEEK_CUR);
		neof = read(f,&sync4,4);
		if (sync4[0] == 0x00 && sync4[1] == 0x00 && sync4[2] == 0x01) {
			switch ( sync4[3] ) {
				
			case PROG_STREAM_MAP:
			case PRIVATE_STREAM2:
			case PROG_STREAM_DIR:
			case ECM_STREAM     :
			case EMM_STREAM     :
			case PADDING_STREAM :
			case DSM_CC_STREAM  :
			case ISO13522_STREAM:
			case PRIVATE_STREAM1:
			case AUDIO_STREAM_S ... AUDIO_STREAM_E:
			case VIDEO_STREAM_S ... VIDEO_STREAM_E:
				found = 1;
				break;
			default:
				q = lseek(f,0,SEEK_CUR);
				break;
			}	
		} 
	}
	q = lseek(f,0,SEEK_CUR);
	lseek(f,start+2,SEEK_SET);
	if (found) return (unsigned int)(q-start)-4-2;
	else return (unsigned int)(q-start-2);
	
}


void cread_pes(char *buf, pes_packet *p){
	
	u8 count, dummy, check;
	int i;
	uint64_t po = 0;
	int c=0;

	switch ( p->stream_id ) {
		
	case PROG_STREAM_MAP:
	case PRIVATE_STREAM2:
	case PROG_STREAM_DIR:
	case ECM_STREAM     :
	case EMM_STREAM     :
		memcpy(p->pes_pckt_data,buf+c,p->length);
		return;
		break;
	case PADDING_STREAM :
		p->padding = p->length;
		memcpy(p->pes_pckt_data,buf+c,p->length);
		return;
		break;			
	case DSM_CC_STREAM  :
	case ISO13522_STREAM:
	case PRIVATE_STREAM1:
	case AUDIO_STREAM_S ... AUDIO_STREAM_E:
	case VIDEO_STREAM_S ... VIDEO_STREAM_E:
		break;
	default:
		return;
		break;
	}	
	
	po = c;
	memcpy(&p->flags1,buf+c,1);
	c++;
	if ( (p->flags1 & 0xC0) == 0x80 ) p->mpeg = 2;
	else p->mpeg = 1;
	
	if ( p->mpeg == 2 ){
		memcpy(&p->flags2,buf+c,1);
		c++;
		memcpy(&p->pes_hlength,buf+c,1);
		c++;
		
		p->length -=p->pes_hlength+3;
		count = p->pes_hlength;
		
		if ((p->flags2 & PTS_DTS_FLAGS) == PTS_ONLY){
			memcpy(p->pts,buf+c,5);
			c += 5;
			count -=5;
		} else 
			if ((p->flags2 & PTS_DTS_FLAGS) == PTS_DTS){
				memcpy(p->pts,buf+c,5);
				c += 5;
				memcpy(p->dts,buf+c,5);
				c += 5;
				count -= 10;
			}
		
		if (p->flags2 & ESCR_FLAG){
			memcpy(p->escr,buf+c,6);
			c += 6;
			count -= 6;
		}
		
		if (p->flags2 & ES_RATE_FLAG){
			memcpy(p->es_rate,buf+c,3);
			c += 6;
			count -= 6;
		}

		if (p->flags2 & DSM_TRICK_FLAG){
			memcpy(&p->trick,buf+c,1);
			c += 6;
			count -= 1;
		}
		
		if (p->flags2 & ADD_CPY_FLAG){
			memcpy(&p->add_cpy,buf+c,1);
			c++;
			count -= 1;
		}
		
		if (p->flags2 & PES_CRC_FLAG){
			memcpy(p->prev_pes_crc,buf+c,2);
			c += 2;
			count -= 2;
		}			
		
		if (p->flags2 & PES_EXT_FLAG){
			memcpy(&p->priv_flags,buf+c,1);
			c++;
			count -= 1;
			
			if (p->priv_flags & PRIVATE_DATA){
				memcpy(p->pes_priv_data,buf+c,16);
				c += 16;
				count -= 16;
			}
			
			if (p->priv_flags & HEADER_FIELD){
				memcpy(&p->pack_field_length,buf+c,1);
				c++;
				p->pack_header = (u8 *)
					malloc(p->pack_field_length);
				memcpy(p->pack_header,buf+c,
				       p->pack_field_length);
				c += p->pack_field_length;
				count -= 1+p->pack_field_length;
			}
			
			if ( p->priv_flags & PACK_SEQ_CTR){
				memcpy(&p->pck_sqnc_cntr,buf+c,1);
				c++;
				memcpy(&p->org_stuff_length,buf+c,1);
				c++;
				count -= 2;
			}
			
			if ( p->priv_flags & P_STD_BUFFER){
				memcpy(p->p_std,buf+c,2);
				c += 2;
				count -= 2;
			}

			if ( p->priv_flags & PES_EXT_FLAG2){
				memcpy(&p->pes_ext_lngth,buf+c,1);
				c++;
				p->pes_ext = (u8 *)
					malloc(p->pes_ext_lngth);
				memcpy(p->pes_ext,buf+c,
				       p->pes_ext_lngth);
				c += p->pes_ext_lngth;
				count -= 1+p->pes_ext_lngth;
			}
		}
		p->stuffing = count;
		for(i = 0; i< count ;i++){ 
			memcpy(&dummy,buf+c,1);
			c++;
		}
	} else {
		p->mpeg1_pad = 1;
 		check = p->flags1;
		while (check == 0xFF){
			memcpy(&check,buf+c,1);
			c++;
			p->mpeg1_pad++;
		}
		
		if ( (check & 0xC0) == 0x40){
			memcpy(&check,buf+c,1);
			c++;
			p->mpeg1_pad++;
			memcpy(&check,buf+c,1);
			c++;
			p->mpeg1_pad++;
		}
		p->flags2 = 0;
		p->length -= p->mpeg1_pad;
		
		c = po;
		if ( (check & 0x30)){
			p->length ++;
			p->mpeg1_pad --;
			
			if (check == p->flags1){
				p->pes_hlength = 0;
			} else {
				p->mpeg1_headr = (u8 *)
					malloc(p->mpeg1_pad);
				p->pes_hlength = p->mpeg1_pad;
				memcpy(p->mpeg1_headr,buf+c,
				       p->mpeg1_pad);
				c += p->mpeg1_pad;
			}
			
			p->flags2 = (check & 0xF0) << 2;
			if ((p->flags2 & PTS_DTS_FLAGS) == PTS_ONLY){
				memcpy(p->pts,buf+c,5);
				c += 5;
				p->length -= 5;
				p->pes_hlength += 5;
			}
			else if ((p->flags2 & PTS_DTS_FLAGS) == 
				 PTS_DTS){
				memcpy(p->pts,buf+c,5);
				c += 5;
				memcpy(p->dts,buf+c,5);
				c += 5;
				p->length -= 10;
				p->pes_hlength += 10;
			}
		} else {
			p->mpeg1_headr = (u8 *) malloc(p->mpeg1_pad);
			p->pes_hlength = p->mpeg1_pad;
			memcpy(p->mpeg1_headr,buf+c,
			       p->mpeg1_pad);
			c += p->mpeg1_pad;
		}
	}
	memcpy(p->pes_pckt_data,buf+c,p->length);
}


int read_pes(int f, pes_packet *p){
	
	u8 sync4[4];
	int found=0;
	uint64_t po = 0;
	int neof = 1;
	u8 *buf;

	while (neof > 0 && !found) {
	        po = lseek(f,0,SEEK_CUR);
		if (po < 0) return -1;
		if ((neof = read(f,&sync4,4)) < 4) return -1;
		if (sync4[0] == 0x00 && sync4[1] == 0x00 && sync4[2] == 0x01) {
			p->stream_id = sync4[3];
			switch ( sync4[3] ) {
				
			case PROG_STREAM_MAP:
			case PRIVATE_STREAM2:
			case PROG_STREAM_DIR:
			case ECM_STREAM     :
			case EMM_STREAM     :
			case PADDING_STREAM :
			case DSM_CC_STREAM  :
			case ISO13522_STREAM:
			case PRIVATE_STREAM1:
			case AUDIO_STREAM_S ... AUDIO_STREAM_E:
			case VIDEO_STREAM_S ... VIDEO_STREAM_E:
				if((neof = read(f,p->llength,2)) < 2)
					return -1;
				setl_pes(p);
				if (!p->length){ 
					p->length = find_length(f);
					nl_pes(p);
				}
				found = 1;
				break;
				
			default:
				if (lseek(f,po+1,SEEK_SET) < po+1) return -1;
			break;
			}	
		} else if(lseek(f,po+1,SEEK_SET) < po+1) return -1;
	}

	if (!found || !p->length) return 0;
	
	if (p->length >0){
		buf = (u8 *) malloc(p->length);
		if((neof = read(f,buf,p->length))< p->length) return -1;
		cread_pes((char *)buf,p);
		free(buf);
	} else return 0;