TAIXIN/sdk/app/spook/rtp-jpeg.c

/*
 * Copyright (C) 2004 Nathan Lutchansky <lutchann@litech.org>
 *
 * 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.
 */


#include <stdio.h>
#include "lwip\sockets.h"
#include "lwip\netif.h"
#include "lwip\dns.h"

#include "lwip\api.h"
#include "lwip\tcp.h"

#include <event.h>
#include <log.h>
#include <frame.h>
#include <stream.h>
#include <rtp.h>
#include <rtp_media.h>
#include <spook_config.h>
#include "list.h"
#include "jpgdef.h"
#include "osal/sleep.h"
#include "session.h"
//#include "common.h"
#include "csi_kernel.h"
#include "osal/string.h"
#include "utlist.h"


#ifdef USB_EN
#include "dev/usb/uvc_host.h"
#endif

#define EXTHDR_LEN 6
#define EXTHDROFF_MAGIC 0
#define EXTHDROFF_VER   1
#define EXTHDROFF_DRI   2
#define EXTHDROFF_HUFF  4

struct rtp_jpeg {
	unsigned char *d;//链表第一帧数据
	int type;
	int q;
	int width;
	int height;
	int luma_table;
	int chroma_table;
	int quant[16];
	int huffoff;
	int hufflen;
	unsigned char exthdr[EXTHDR_LEN];
	unsigned char *scan_data;
	int scan_data_len;
	int offset;
	int init_done;
	int ts_incr;
	unsigned int timestamp;	
	int reset_interval;
	int dri_num;
	unsigned short int dri_len[100];
	//为了获取到frame,因为使用了链表形式
	struct frame *f;
};

static int parse_DQT( struct rtp_jpeg *out, unsigned char *d, int len )
{
	int i;

	for( i = 0; i < len; i += 65 )
	{
		if( ( d[i] & 0xF0 ) != 0 )
		{
			_os_printf( "Unsupported quant table precision 0x%X!\n",
					d[i] & 0xF0 );
			return -1;
		}
		//out->quant[d[i] & 0xF] = d + i + 1;
		out->quant[d[i] & 0xF] = d-out->d+i+1;
		//out->quant[d[i] & 0xF] = out->scan_data - (d + i + 1);
	}
	return 0;
}

static int parse_SOF( struct rtp_jpeg *out, unsigned char *d, int len )
{
	int c;

	out->chroma_table = -1;
	if( d[0] != 8 )
	{
		_os_printf( "Invalid precision %d in SOF0\n", d[0] );
		return -1;
	}
	out->height = GET_16( d + 1 );
	out->width = GET_16( d + 3 );

	
	if( ( out->height & 0x7 ) || ( out->width & 0x7 ) )
	{
		os_printf(KERN_ERR"Width/height not divisible by 8!\n" );
		os_printf(KERN_ERR"w:%d\th:%d\n",out->width,out->height);
		return -1;
	}
	out->width >>= 3;
	out->height >>= 3;
	if( d[5] != 3 )
	{
		_os_printf( "Number of components is %d, not 3!\n", d[5] );
		return -1;
	}
	/* Loop over the parameters for each component */
	for( c = 6; c < 6 + 3 * 3; c += 3 )
	{
//		if( d[c + 2] >= 16 || ! out->quant[d[c + 2]] )
//		{
//			_os_printf( "Component %d specified undefined quant table %d!\n", d[c], d[c + 2] );
//			return -1;
//		}
		switch( d[c] ) /* d[c] contains the component ID */
		{
		case 1: /* Y */
			/*
			if( d[c + 1] == 0x11 ) out->type = 0;
			else if( d[c + 1] == 0x22 ) out->type = 1;
			*/			
			if( d[c + 1] == 0x21 ) out->type = out->type & ~1;   //YUV422
			else if( d[c + 1] == 0x22 ) out->type = (out->type & ~1)|1;  //YUV420
			else
			{
				_os_printf( "Invalid sampling factor 0x%02X in Y component!\n", d[c + 1] );
				return -1;
			}
			out->luma_table = d[c + 2];
			break;
		case 2: /* Cb */
		case 3: /* Cr */
			if( d[c + 1] != 0x11 )
			{
				_os_printf( "Invalid sampling factor 0x%02X in %s component!\n", d[c + 1], d[c] == 2 ? "Cb" : "Cr" );
				return -1;
			}
			if( out->chroma_table < 0 )
				out->chroma_table = d[c + 2];
			else if( out->chroma_table != d[c + 2] )
			{
				_os_printf( "Cb and Cr components do not share a quantization table!\n" );
				return -1;
			}
			break;
		default:
			_os_printf( "Invalid component %d in SOF!\n", d[c] );
			return -1;
		}
	}
	return 0;
}

static int parse_DHT( struct rtp_jpeg *out, unsigned char *d, int len )
{
	/* We should verify that this coder uses the standard Huffman tables */
  /* Kaifan:
   * Some USB-Sensor do not use standard Huffman tables.
   * So I add Huffman following quant table.
   * It's compatible to the old version of App,
   * It just looks like some dummy quant tables exist.
   * But the dummy size can not be too long, because the old rtpdec-jpeg.c uses hdr[1024]
   */
  if(0 == out->hufflen)
  {
		//out->huffoff = out->scan_data - (d - 4);
  	out->huffoff = (int)( ( d - 4 ) - out->d );
  }
  out->hufflen += len + 4;
  PUT_16(out->exthdr+EXTHDROFF_HUFF, out->hufflen);
	return 0;
}

static void parse_DRI (struct rtp_jpeg *out, unsigned char *d)
{
	if (GET_16( d )) {
		out->type |= 0x40;
		out->reset_interval = GET_16( d );
		/* Kaifan:
		 * Some USB-Sensor's reset-interval is not width/16.
		 * But the origin room for reset-interval is used by res_len.
		 * So Save the reset-interval before the Huffman table
		 */
		PUT_16(out->exthdr+EXTHDROFF_DRI, out->reset_interval);
	}
}

extern volatile uint8_t framerate_c;

void clear_init_done(void *d)
{
	_os_printf("%s:%d\n",__FUNCTION__,__LINE__);
	struct rtp_jpeg *out = (struct rtp_jpeg *)d;
	out->init_done = 0;
}

static int jpeg_process_frame( struct frame *f, void *d )
{
	struct rtp_media *rtp = (struct rtp_media *)d;
	struct rtp_jpeg *out = (struct rtp_jpeg *)rtp->private;
	out->f = f;
	int i, blen;
	uint32_t per_ms_incr = (25 * out->ts_incr)/1000;
	//static uint32_t last_time = 0;
	//uint32_t sys_time = 0;
	out->timestamp = per_ms_incr*f->timestamp;

	//out->scan_data = j->scan_data;
	//out->scan_data_len = j->scan_data_len;
	//out->dri_num = j->dri_num;
	//memcpy(out->dri_len, j->dri_len, sizeof(j->dri_len));
		
	/* note by jornny 
	* just parse once in order to reduce time of processing frame
	*/
	//uvc 每一次都要扫描
	#if USB_EN == 1 
			out->init_done = 0;
	#endif	

    if(out->init_done == 0 ) {
		out->init_done =0; 
		for( i = 0; i < 16; ++i ) out->quant[i] = 0;	
		out->type = 0;
		out->reset_interval = 0;
		/* Kaifan:
		 * exthdr[0] = 0 let the App knows the extend protocol
		 * exthdr[1] = 0 extend protocal version 0
		 */
		out->hufflen = 0;
		out->exthdr[EXTHDROFF_MAGIC] = 0;
		out->exthdr[EXTHDROFF_VER]   = 0;

	} else {
		return out->init_done;
	}
	//out->dri_num = 0;
	
	out->d = f->d;

	//增加一个前置空数的地方
	for( i = 0; i < f->first_length; i += blen + 2 )
	{
		if( f->d[i] != 0xFF ) 
		{
			_os_printf( "Found %02X at %d, expecting FF\n", f->d[i], i );
			out->scan_data_len = 0;
			return 0;
		}
		while(f->d[i+1] == 0xFF) ++i;

		/* SOI (FF D8) is a bare marker with no length field */
		if( f->d[i + 1] == 0xD8 ) blen = 0;
		else blen = GET_16( f->d + i + 2 );

		switch( f->d[i + 1] )
		{
		case 0xDB: /* Quantization Table */
			if( out->init_done ) break;
			if( parse_DQT( out, f->d + i + 4, blen - 2 ) < 0 )
			{
				out->scan_data_len = 0;
                _os_printf( "jpeg_process_frame Quantization Table err!\n" );
				return 0;
			}
			break;
		case 0xC0: /* Start of Frame */
			if( out->init_done ) break;
			if( parse_SOF( out, f->d + i + 4, blen - 2 ) < 0 )
			{
				out->scan_data_len = 0;
                _os_printf( "jpeg_process_frame Start of Frame err!\n" );
				return 0;
			}
			break;
		case 0xC4: /* Huffman Table */
//			if( out->init_done ) break; /* only parse DHT once */ /* Kaifan: No! Maybe has 4 DHTs! */
			if( parse_DHT( out, f->d + i + 4, blen - 2 ) < 0 )
			{
				out->scan_data_len = 0;
                _os_printf( "jpeg_process_frame Huffman Table err!\n" );
				return 0;
			}
//			out->init_done = 1; /* Kaifan: Maybe has 4 DHTs! So can't done here */
			break;		
		case 0xDD:	/* DRI */
			if( out->init_done ) break;
			parse_DRI (out, f->d + i + 4);
			break;
		case 0xDA: /* Start of Scan */
			out->init_done = 1; /* Kaifan: Now can done here */
			out->scan_data = f->d + i + 14;
			out->scan_data_len = f->length - (out->scan_data - f->d);
			out->offset = out->scan_data-f->d;
			//_os_printf("scan_data_len:%d\t%d\n",f->length,(out->scan_data - f->d));
		
			return out->init_done;
			/*out->scan_data = f->d + i + 2 + blen;
			out->scan_data_len = f->length - i - 2 - blen;
			out->timestamp += out->ts_incr;
			if(scan_DRI(out))
				return 0;
			else 
				return out->init_done;*/
		}
	}

	_os_printf( "Found no scan data!\n" );
	uint32_t xi=0;
	for(xi=0;xi<0x290;xi++)
	{
		_os_printf(" %02x",f->d[xi]);	
	}
    _os_printf(" \n");
	out->scan_data_len = 0;
	return 0;
}

static int jpeg_get_sdp( char *dest, int len, int payload, int port, void *d )
{
	return snprintf( dest, len, "m=video %d RTP/AVP 26\r\n", port );
}

static int jpeg_get_payload( int payload, void *d )
{
	return 26;
}

static int cal_data_len(struct rtp_jpeg *out, int *dri_index, int *res_len, int max_size)
{
	/* not by jorrny
	* At the beginning, we try to send the packet aligned to dri.
	* But we found it inefficient when the dri length is almost a half of the max_size
	* So we decide to send the packet with the max_size always to improve efficiency.
	* In the same way, we try to send the packet within max_size in order to prevent fragment in ip layer.
	*/

	int len = 0;

	do
	{
		if(*res_len) {
			len = *res_len;
		} else {
			len += out->dri_len[*dri_index];
		}
		*res_len = 0;
		if(len > max_size) {
			*res_len = len - max_size;
			len = max_size;
			break;
		} else if (len == max_size) {
			(*dri_index)++;
			break;
		}
		if(++(*dri_index) > out->dri_num)
			break;
	} while(1);

	return len;
}

//版本已经移除
static int jpeg_send( struct rtp_endpoint *ep, void *d )
{
	return 0;
}

static void jpeg_send_frame_to_endpoint( struct rtp_endpoint *ep, struct rtp_jpeg *out, struct framebuff *fb )
{
	uint8_t *jpeg_buf_addr = (uint8_t *)fb->data;
	int i = 0, plen, vcnt, hdr_len;
	uint32_t node_offset = out->offset;
	struct iovec v[8];
	uint8_t vhdr[12], qhdr[4];
	int res_len = 1, max_data_size;
	uint32_t node_len = out->f->node_len;

	if( !ep )
	{
		return;
	}
	if( !ep->sendEnable )
	{
		ep->sendEnable = 1;
		return;
	}

	vhdr[0] = 0;
	vhdr[4] = out->type;
	vhdr[5] = 255;
	vhdr[6] = out->width;
	vhdr[7] = out->height;
	v[1].iov_base = vhdr;
	v[1].iov_len = ( out->type & 0x40 ) ? 12 : 8;

	qhdr[0] = 0;
	qhdr[1] = 0;
#ifdef USE_EXTHDR
	PUT_16( qhdr + 2, 2 * 64 + out->hufflen + EXTHDR_LEN );
#else
	PUT_16( qhdr + 2, 2 * 64 );
#endif
	v[2].iov_base = qhdr;
	v[2].iov_len = 4;
	v[3].iov_base = jpeg_buf_addr + out->quant[out->luma_table];
	v[3].iov_len = 64;
	v[4].iov_base = jpeg_buf_addr + out->quant[out->chroma_table];
	v[4].iov_len = 64;
	hdr_len = 132 + v[1].iov_len;
#ifdef USE_EXTHDR
	vcnt = 7;
	v[5].iov_base = out->exthdr;
	v[5].iov_len = EXTHDR_LEN;
	/* The DHT block stays inside the current JPEG frame, so it can be sent directly. */
	v[6].iov_base = jpeg_buf_addr + out->huffoff;
	v[6].iov_len = out->hufflen;
	hdr_len += v[5].iov_len + v[6].iov_len;
#else
	vcnt = 5;
#endif

	while( i < out->scan_data_len )
	{
		if( node_offset >= node_len ) 
		{
			node_offset = 0;
		}

		max_data_size = rtp_get_payload_size_limit( ep, RTP_HEADER_SIZE + hdr_len );
		if( max_data_size <= 0 )
		{
			ep->sendEnable = 0;
			break;
		}
		if( out->type & 0x40 )
		{
			if( ( out->scan_data_len - i ) > max_data_size )
			{
				plen = max_data_size;
			}
			else
			{
				plen = out->scan_data_len - i;
			}
			
			if( res_len )
			{
				vhdr[8] = out->exthdr[EXTHDROFF_DRI];
				vhdr[9] = out->exthdr[EXTHDROFF_DRI + 1];
			}
			else
			{
				PUT_16( vhdr + 8, 0x28 );
			}
			PUT_16( vhdr + 10, 0xffff );
		}
		else
		{
			plen = out->scan_data_len - i;
			if( plen > max_data_size )
			{
				plen = max_data_size;
			}
			
		}

		vhdr[1] = i >> 16;
		vhdr[2] = ( i >> 8 ) & 0xff;
		vhdr[3] = i & 0xff;
		v[vcnt].iov_base = jpeg_buf_addr + node_offset;

		if( node_offset + plen >= node_len )
		{
			plen = node_len - node_offset;
		}
		else
		{
			uint32_t align_32 = ( node_offset + plen ) & ( ~0x1F );
			plen = align_32 - node_offset;
		}

		if( i + plen >= out->scan_data_len ) plen = out->scan_data_len - i;
		node_offset += plen;
		v[vcnt].iov_len = plen;
		if( rtp_sendmsg( ep, v, vcnt + 1, out->timestamp, plen + i == out->scan_data_len, 30 ) < 0 )
		{
			ep->sendEnable = 0;
			break;
		}

		vcnt = 2;
		hdr_len = v[1].iov_len;
		i += plen;
	}

	ep->sendEnable = 1;
}

static int jpeg_send_more( rtp_loop_search_ep search,void *ls,void *track, void *d )
{
	struct rtp_jpeg *out = (struct rtp_jpeg *)d;
	struct framebuff *fb = (struct framebuff *)out->f->get_f;
	struct rtp_endpoint *ep;
	void *head;

	if( !fb )
	{
		return 0;
	}
	out->scan_data_len = out->f->length - out->offset;

	head = ls;
	while( head )
	{
		head = search( head, track, (void **)&ep );
		if( ep )
		{
			jpeg_send_frame_to_endpoint( ep, out, fb );
		}
	}

	return 0;
}

struct rtp_media *new_rtp_media_jpeg_stream( struct stream *stream )
{
	struct rtp_jpeg *out;
	int fincr, fbase;
	struct rtp_media *m;

	stream->get_framerate( stream, &fincr, &fbase );
	out = (struct rtp_jpeg *)malloc( sizeof( struct rtp_jpeg ) );
	out->init_done = 0;
	out->timestamp = 0;
	out->scan_data = NULL;
	out->scan_data_len = 0;
	out->ts_incr = 90000 * fincr / fbase;
	m = new_rtp_rtcp_media( jpeg_get_sdp, jpeg_get_payload, jpeg_process_frame, jpeg_send, new_rtcp_send, out );
	if(m)
	{
		m->type = 0;
		m->send_more = jpeg_send_more;
		m->per_ms_incr = (25 * out->ts_incr)/1000;
	}
	return m;

}