transform.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 "transform.h"
#include <stdlib.h>
#include <string.h>
#include "ctools.h"

static uint8_t tspid0[TS_SIZE] = { 
	0x47, 0x40, 0x00, 0x10, 0x00, 0x00, 0xb0, 0x11, 
	0x00, 0x00, 0xcb, 0x00, 0x00, 0x00, 0x00, 0xe0, 
	0x10, 0x00, 0x01, 0xe4, 0x00, 0x2a, 0xd6, 0x1a, 
	0xfb, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff
};

static	uint8_t tspid1[TS_SIZE] = { 
	0x47, 0x44, 0x00, 0x10, 0x00, 0x02, 0xb0, 0x1c,
	0x00, 0x01, 0xcb, 0x00, 0x00, 0xe0, 0xa0, 0xf0, 
	0x05, 0x48, 0x03, 0x01, 0x00, 0x00, 0x02, 0xe0,
	0xa0, 0xf0, 0x00, 0x03, 0xe0, 0x50, 0xf0, 0x00, 
	0xae, 0xea, 0x4e, 0x48, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
	0xff, 0xff, 0xff, 0xff
};

uint32_t trans_pts_dts(uint8_t *pts)
{
	uint32_t wts;
	
	wts = (((pts[0] & 0x06) << 4) | 
	       ((pts[1] & 0xFC) >> 2)) << 24; 
	wts |= (((pts[1] & 0x03) << 6) |
		((pts[2] & 0xFC) >> 2)) << 16; 
	wts |= (((pts[2] & 0x02) << 6) |
		((pts[3] & 0xFE) >> 1)) << 8;
	wts |= (((pts[3] & 0x01) << 7) |
		((pts[4] & 0xFE) >> 1));
	return wts;
}


void get_pespts(uint8_t *av_pts,uint8_t *pts)
{
	
	pts[0] = 0x21 | 
		((av_pts[0] & 0xC0) >>5);
	pts[1] = ((av_pts[0] & 0x3F) << 2) |
		((av_pts[1] & 0xC0) >> 6);
	pts[2] = 0x01 | ((av_pts[1] & 0x3F) << 2) |
		((av_pts[2] & 0x80) >> 6);
	pts[3] = ((av_pts[2] & 0x7F) << 1) |
		((av_pts[3] & 0x80) >> 7);
	pts[4] = 0x01 | ((av_pts[3] & 0x7F) << 1);
}

uint16_t get_pid(uint8_t *pid)
{
	uint16_t pp = 0;

	pp = (pid[0] & PID_MASK_HI)<<8;
	pp |= pid[1];

	return pp;
}

int write_ts_header(uint16_t pid, uint8_t *counter, int pes_start, 
		    uint8_t *buf, uint8_t length)
{
	int i;
	int c = 0;
	int fill;
	uint8_t tshead[4] = { 0x47, 0x00, 0x00, 0x10}; 
        

	fill = TS_SIZE-4-length;
        if (pes_start) tshead[1] = 0x40;
	if (fill) tshead[3] = 0x30;
        tshead[1] |= (uint8_t)((pid & 0x1F00) >> 8);
        tshead[2] |= (uint8_t)(pid & 0x00FF);
        tshead[3] |= ((*counter)++ & 0x0F) ;
        memcpy(buf,tshead,4);
	c+=4;


	if (fill){
		buf[4] = fill-1;
		c++;
		if (fill >1){
			buf[5] = 0x00;
			c++;
		}
		for ( i = 6; i < fill+4; i++){
			buf[i] = 0xFF;
			c++;
		}
	}

        return c;
}


int write_pes_header(uint8_t id,int length , long PTS, uint8_t *obuf, 
		     int stuffing)
{
	uint8_t le[2];
	uint8_t dummy[3];
	uint8_t *pts;
	uint8_t ppts[5];
	long lpts;
	int c;
	uint8_t headr[3] = {0x00, 0x00, 0x01};
	
	lpts = htonl(PTS);
	pts = (uint8_t *) &lpts;
	
	get_pespts(pts,ppts);

	c = 0;
	memcpy(obuf+c,headr,3);
	c += 3;
	memcpy(obuf+c,&id,1);
	c++;

	le[0] = 0;
	le[1] = 0;
	length -= 6+stuffing;

	le[0] |= ((uint8_t)(length >> 8) & 0xFF); 
	le[1] |= ((uint8_t)(length) & 0xFF); 
	memcpy(obuf+c,le,2);
	c += 2;

	if (id == PADDING_STREAM){
		memset(obuf+c,0xff,length);
		c+= length;
		return c;
	}

	dummy[0] = 0x80;
	dummy[1] = 0;
	dummy[2] = 0;
	if (PTS){
		dummy[1] |= PTS_ONLY;
		dummy[2] = 5+stuffing;
	}
	memcpy(obuf+c,dummy,3);
	c += 3;
	memset(obuf+c,0xFF,stuffing);

	if (PTS){
		memcpy(obuf+c,ppts,5);
		c += 5;
	}
	
	return c;
}


void init_p2p(p2p *p, void (*func)(uint8_t *buf, int count, p2p *p), 
	      int repack){
	p->found = 0;
	p->cid = 0;
	p->mpeg = 0;
	memset(p->buf,0,MMAX_PLENGTH);
	p->done = 0;
	p->fd1 = -1;
	p->func = func;
	p->bigend_repack = 0;
	p->repack = 0; 
	if ( repack < MAX_PLENGTH && repack > 265 ){
		p->repack = repack-6;
		p->bigend_repack = (uint16_t)htons((short)
						   ((repack-6) & 0xFFFF));
	} else {
		fprintf(stderr, "Repack size %d is out of range\n",repack);
		exit(1);
	}
}



void pes_repack(p2p *p)
{
	int count = 0;
	int repack = p->repack;
	int rest = p->plength;
	uint8_t buf[MAX_PLENGTH];
	int bfill = 0;
	int diff;
	uint16_t length;

	if (rest < 0) {
                fprintf(stderr,"Error in repack\n");
                return;
        }

        if (!repack){
                fprintf(stderr,"forgot to set repack size\n");
                return;
        }

	if (p->plength == repack){
		memcpy(p->buf+4,(char *)&p->bigend_repack,2);
		p->func(p->buf, repack+6, p);
		return;
	}

	buf[0] = 0x00;
	buf[1] = 0x00;
	buf[2] = 0x01;
	buf[3] = p->cid;
	memcpy(buf+4,(char *)&p->bigend_repack,2);
	memset(buf+6,0,MAX_PLENGTH-6);

	if (p->mpeg == 2){

		if ( rest > repack){
			memcpy(p->buf+4,(char *)&p->bigend_repack,2);
			p->func(p->buf, repack+6, p);
			count += repack+6;
			rest -= repack;
		} else {
			memcpy(buf,p->buf,9+p->hlength);
			bfill = p->hlength;
			count += 9+p->hlength;
			rest -= p->hlength+3;
		}

		while (rest >= repack-3){
			memset(buf+6,0,MAX_PLENGTH-6);
			buf[6] = 0x80;
			buf[7] = 0x00;
			buf[8] = 0x00;
			memcpy(buf+9,p->buf+count,repack-3);
			rest -= repack-3;
			count += repack-3;
			p->func(buf, repack+6, p);
		}
		
		if (rest){
			diff = repack - 3 - rest - bfill;
			if (!bfill){
				buf[6] = 0x80;
				buf[7] = 0x00;
				buf[8] = 0x00;
			}

			if ( diff < PES_MIN){
				length = rest+ diff + bfill+3; 
				buf[4] = (uint8_t)((length & 0xFF00) >> 8);
				buf[5] = (uint8_t)(length & 0x00FF);
				buf[8] = (uint8_t)(bfill+diff);
				memset(buf+9+bfill,0xFF,diff);
				memcpy(buf+9+bfill+diff,p->buf+count,rest);
			} else {
				length = rest+ bfill+3; 
				buf[4] = (uint8_t)((length & 0xFF00) >> 8);
				buf[5] = (uint8_t)(length & 0x00FF);
				memcpy(buf+9+bfill,p->buf+count,rest);
				bfill += rest+9;
				write_pes_header( PADDING_STREAM, diff, 0,
						  buf+bfill, 0);
			}
			p->func(buf, repack+6, p);
		}
	}	

	if (p->mpeg == 1){

		if ( rest > repack){
			memcpy(p->buf+4,(char *)&p->bigend_repack,2);
			p->func(p->buf, repack+6, p);
			count += repack+6;
			rest -= repack;
		} else {
			memcpy(buf,p->buf,6+p->hlength);
			bfill = p->hlength;
			count += 6;
			rest -= p->hlength;
		}

		while (rest >= repack-1){
			memset(buf+6,0,MAX_PLENGTH-6);
			buf[6] = 0x0F;
			memcpy(buf+7,p->buf+count,repack-1);
			rest -= repack-1;
			count += repack-1;
			p->func(buf, repack+6, p);
		}
		

		if (rest){
			diff = repack - 1 - rest - bfill;

			if ( diff < PES_MIN){
				length = rest+ diff + bfill+1; 
				buf[4] = (uint8_t)((length & 0xFF00) >> 8);
				buf[5] = (uint8_t)(length & 0x00FF);
				memset(buf+6,0xFF,diff);
				if (!bfill){
					buf[6+diff] = 0x0F;
				}
				memcpy(buf+7+diff,p->buf+count,rest+bfill);
			} else {
				length = rest+ bfill+1; 
				buf[4] = (uint8_t)((length & 0xFF00) >> 8);
				buf[5] = (uint8_t)(length & 0x00FF);
				if (!bfill){
					buf[6] = 0x0F;
					memcpy(buf+7,p->buf+count,rest);
					bfill = rest+7;
				} else {
					memcpy(buf+6,p->buf+count,rest+bfill);
					bfill += rest+6;
				}
				write_pes_header( PADDING_STREAM, diff, 0,
						  buf+bfill, 0);
			}
			p->func(buf, repack+6, p);
		}
	}	
}




void pes_filt(p2p *p)
{
	int factor = p->mpeg-1;

	if ( p->cid == p->filter) {
		if (p->es)
			write(p->fd1,p->buf+p->hlength+6+3*factor, 
			      p->plength-p->hlength-3*factor);
		else
			write(p->fd1,p->buf,p->plength+6);
	}
}

#define SIZE 4096
void extract_from_pes(int fdin, int fdout, uint8_t id, int es)
{
		p2p p;
		int count = 1;
		uint8_t buf[SIZE];

		init_p2p(&p, NULL, 2048);
		p.fd1 = fdout;
		p.filter = id;
		p.es = es;

		while (count > 0){
			count = read(fdin,buf,SIZE);
			get_pes(buf,count,&p,pes_filt);
		}
}


void pes_dfilt(p2p *p)
{
	int factor = p->mpeg-1;
	int fd =0;
	int type = NOPES;

	switch ( p->cid ) {
		case AUDIO_STREAM_S ... AUDIO_STREAM_E:			
			fd = p->fd1;
			type = AUDIO;
			break;
		case VIDEO_STREAM_S ... VIDEO_STREAM_E:
			fd = p->fd2;
			type = VIDEO;
			break;
	}
	
	if (p->es && !p->startv && type == VIDEO){
		int found = 0;
		int c = 6+p->hlength+3*factor;
		
		if  ( p->flag2 & PTS_DTS ) 
			p->vpts =  ntohl(trans_pts_dts(p->pts)); 
		while ( !found && c+3 < p->plength+6 ){
			if ( p->buf[c] == 0x00 && 
			     p->buf[c+1] == 0x00 && 
			     p->buf[c+2] == 0x01 &&
			     p->buf[c+3] == 0xb3) 
				found = 1;
			else c++;
		}
		if (found){
			p->startv = 1;
			write(fd, p->buf+c, p->plength+6-c);
		}
		fd = 0;
	} 

		
	if ( p->es && !p->starta && type == AUDIO){
		int found = 0;
		int c = 6+p->hlength+3*factor;
		if  ( p->flag2 & PTS_DTS ) 
			p->apts =  ntohl(trans_pts_dts(p->pts));  
		if (p->startv)
			while ( !found && c+1 < p->plength+6){
				if ( p->buf[c] == 0xFF && 
				     (p->buf[c+1] & 0xF8) == 0xF8)
					found = 1;
				else c++;
			}
		if (found){
			p->starta = 1;
			write(fd, p->buf+c, p->plength+6-c);
		}
		fd = 0;
	} 


	if (fd){
		if (p->es)
			write(fd,p->buf+p->hlength+6+3*factor, 
			      p->plength-p->hlength-3*factor);
		else
			write(fd,p->buf,p->plength+6);
	}
} 

int64_t pes_dmx( int fdin, int fdouta, int fdoutv, int es)
{
	p2p p;
	int count = 1;
	uint8_t buf[SIZE];
	uint64_t length = 0;
	uint64_t l = 0;
	int verb = 0;
	
	init_p2p(&p, NULL, 2048);
	p.fd1 = fdouta;
	p.fd2 = fdoutv;
	p.es = es;
	p.startv = 0;
	p.starta = 0;
	p.apts=-1;
	p.vpts=-1;
	
	if (fdin != STDIN_FILENO) verb = 1; 
	
	if (verb) {
		length = lseek(fdin, 0, SEEK_END);
		lseek(fdin,0,SEEK_SET);
	}
	
	while (count > 0){
		count = read(fdin,buf,SIZE);
		l += count;
		if (verb)
			fprintf(stderr,"Demuxing %2.2f %%\r",
				100.*l/length);
		
		get_pes(buf,count,&p,pes_dfilt);
	}
	
	return (int64_t)p.vpts - (int64_t)p.apts;
	
}



static void pes_in_ts(p2p *p)
{
	int l, pes_start;
	uint8_t obuf[TS_SIZE];
	long int c = 0;
	int length = p->plength+6;
	uint16_t pid;
	uint8_t *counter;
	pes_start = 1;
	switch ( p->cid ) {
	case AUDIO_STREAM_S ... AUDIO_STREAM_E:			
		pid = p->pida;
		counter = &p->acounter;
		break;
	case VIDEO_STREAM_S ... VIDEO_STREAM_E:
		pid = p->pidv;
		counter = &p->acounter;

		tspid0[3] |= (p->count0++) 
			& 0x0F ;
		tspid1[3] |= (p->count1++) 
			& 0x0F ;
	
		tspid1[24]  = p->pidv;
		tspid1[23] |= (p->pidv >> 8) & 0x3F;
		tspid1[29]  = p->pida;
		tspid1[28] |= (p->pida >> 8) & 0x3F;
		
		p->func(tspid0,188,p);
		p->func(tspid1,188,p);
		break;
	default:
		return;
	}

	while ( c < length ){
		memset(obuf,0,TS_SIZE);
		if (length - c >= TS_SIZE-4){
			l = write_ts_header(pid, counter, pes_start
					     , obuf, TS_SIZE-4);
			memcpy(obuf+l, p->buf+c, TS_SIZE-l);
			c += TS_SIZE-l;
		} else { 
			l = write_ts_header(pid, counter, pes_start
					     , obuf, length-c);
			memcpy(obuf+l, p->buf+c, TS_SIZE-l);
			c = length;
		}
		p->func(obuf,188,p);
		pes_start = 0;
	}
}



void pes_to_ts2( int fdin, int fdout, uint16_t pida, uint16_t pidv)
{
	p2p p;
	int count = 1;
	uint8_t buf[SIZE];
	uint64_t length = 0;
	uint64_t l = 0;
	int verb = 0;
	
	init_p2p(&p, NULL, 2048);
	p.fd1 = fdout;
	p.pida = pida;
	p.pidv = pidv;
	p.acounter = 0;
	p.vcounter = 0;
	p.count1 = 0;
	p.count0 = 0;
		
	if (fdin != STDIN_FILENO) verb = 1; 

	if (verb) {
		length = lseek(fdin, 0, SEEK_END);
		lseek(fdin,0,SEEK_SET);
	}

	while (count > 0){
		count = read(fdin,buf,SIZE);
		l += count;
		if (verb)
			fprintf(stderr,"Writing TS  %2.2f %%\r",
				100.*l/length);

		get_pes(buf,count,&p,pes_in_ts);
	}
		
}

void write_out(uint8_t *buf, int count,void  *p)
{
	write(STDOUT_FILENO, buf, count);
}


#define IN_SIZE TS_SIZE*10
#define IPACKS 2048
void find_avpids(int fd, uint16_t *vpid, uint16_t *apid)
{
        uint8_t buf[IN_SIZE];
        int count;
        int i;  
        int off =0;

        while ( *apid == 0 || *vpid == 0){
                count = read(fd, buf, IN_SIZE);
                for (i = 0; i < count-7; i++){
                        if (buf[i] == 0x47){
                                if (buf[i+1] & 0x40){
                                        off = 0;
                                        if ( buf[3+i] & 0x20)//adapt field?
                                                off = buf[4+i] + 1;
                                        switch(buf[i+7+off]){
                                        case VIDEO_STREAM_S ... VIDEO_STREAM_E:
                                                *vpid = get_pid(buf+i+1);
                                                break;
                                        case PRIVATE_STREAM1:
                                        case AUDIO_STREAM_S ... AUDIO_STREAM_E:
                                                *apid = get_pid(buf+i+1);
                                                break;
                                        }
                                }
                                i += 187;
                        }
                        if (*apid != 0 && *vpid != 0) break;
                }
        }
}

void find_bavpids(uint8_t *buf, int count, uint16_t *vpid, uint16_t *apid)
{
        int i;  
        int founda = 0;
        int foundb = 0;
        int off = 0;
        
        *vpid = 0;
        *apid = 0;
        for (i = 0; i < count-7; i++){
                if (buf[i] == 0x47){
                        if ((buf[i+1] & 0xF0) == 0x40){
                                off = 0;
                                if ( buf[3+i] & 0x20)  // adaptation field?
                                        off = buf[4+i] + 1;
                                
                                if (buf[off+i+4] == 0x00 && 
                                    buf[off+i+5] == 0x00 &&
                                    buf[off+i+6] == 0x01){
                                        switch(buf[off+i+7]){
                                        case VIDEO_STREAM_S ... VIDEO_STREAM_E:
                                                *vpid = get_pid(buf+i+1);
                                                foundb=1;
                                                break;
                                        case PRIVATE_STREAM1:
                                        case AUDIO_STREAM_S ... AUDIO_STREAM_E:
                                                *apid = get_pid(buf+i+1);
                                                founda=1;
                                                break;
                                        }
                                }
                        }
                        i += 187;
                }
                if (founda && foundb) break;
        }
}


void ts_to_pes( int fdin, uint16_t pida, uint16_t pidv, int ps)
{
	
	uint8_t buf[IN_SIZE];
	uint8_t mbuf[TS_SIZE];
	int i;
	int count = 1;
	uint16_t pid;
	uint16_t dummy;
	ipack pa, pv;
	ipack *p;

	if (fdin != STDIN_FILENO && (!pida || !pidv))
		find_avpids(fdin, &pidv, &pida);

	init_ipack(&pa, IPACKS,write_out, ps);
	init_ipack(&pv, IPACKS,write_out, ps);

 	if ((count = read(fdin,mbuf,TS_SIZE))<0)
	    perror("reading");

	for ( i = 0; i < 188 ; i++){
		if ( mbuf[i] == 0x47 ) break;
	}
	if ( i == 188){