mpeg2ps.c

完整的RTP RTSP代码库

/*
 * The contents of this file are subject to the Mozilla Public
 * License Version 1.1 (the "License"); you may not use this file
 * except in compliance with the License. You may obtain a copy of
 * the License at http://www.mozilla.org/MPL/
 * 
 * Software distributed under the License is distributed on an "AS
 * IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
 * implied. See the License for the specific language governing
 * rights and limitations under the License.
 * 
 * The Original Code is MPEG4IP.
 * 
 * The Initial Developer of the Original Code is Cisco Systems Inc.
 * Portions created by Cisco Systems Inc. are
 * Copyright (C) Cisco Systems Inc. 2004.  All Rights Reserved.
 * 
 * Contributor(s): 
 *		Bill May [e-mail]
 */

/*
 * mpeg2ps.c - parse program stream and vob files
 */
#include "mpeg2_ps.h"
#include "mpeg2ps_private.h"
#include <mp4av.h>
#include <mp4av_h264.h>

//#define DEBUG_LOC 1
//#define DEBUG_STATE 1
static const uint lpcm_freq_tab[4] = {48000, 96000, 44100, 32000};

/*************************************************************************
 * File access routines.  Could all be inlined
 *************************************************************************/
static FDTYPE file_open (const char *name)
{
  return open(name, OPEN_RDONLY);
}

static bool file_okay (FDTYPE fd)
{
  return fd >= 0;
}

static void file_close (FDTYPE fd)
{
  close(fd);
}

static bool file_read_bytes (FDTYPE fd,
			     uint8_t *buffer, 
			     uint32_t len)
{
  uint32_t readval = read(fd, buffer, len);
  return readval == len;
}

// note: len could be negative.
static void file_skip_bytes (FDTYPE fd, int32_t len)
{
  lseek(fd, len, SEEK_CUR);
}

static off_t file_location (FDTYPE fd)
{
  return lseek(fd, 0, SEEK_CUR);
}

static off_t file_seek_to (FDTYPE fd, off_t loc)
{
  return lseek(fd, loc, SEEK_SET);
}

static off_t file_size (FDTYPE fd)
{
  off_t ret = lseek(fd, 0, SEEK_END);
  file_seek_to(fd, 0);
  return ret;
}

static uint64_t read_pts (uint8_t *pak)
{
  uint64_t pts;
  uint16_t temp;
  
  pts = ((pak[0] >> 1) & 0x7);
  pts <<= 15;
  temp = convert16(&pak[1]) >> 1;
  pts |= temp;
  pts <<= 15;
  temp = convert16(&pak[3]) >> 1;
  pts |= temp;
  return pts;
}


static mpeg2ps_stream_t *mpeg2ps_stream_create (uint8_t stream_id,
						uint8_t substream)
{
  mpeg2ps_stream_t *ptr = MALLOC_STRUCTURE(mpeg2ps_stream_t);

  memset(ptr, 0, sizeof(*ptr));

  ptr->m_stream_id = stream_id;
  ptr->m_substream_id = substream;
  ptr->is_video = stream_id >= 0xe0;
  ptr->pes_buffer = (uint8_t *)malloc(4*4096);
  ptr->pes_buffer_size_max = 4 * 4096;
  return ptr;
}

static void mpeg2ps_stream_destroy (mpeg2ps_stream_t *sptr)
{
  mpeg2ps_record_pes_t *p;
  while (sptr->record_first != NULL) {
    p = sptr->record_first;
    sptr->record_first = p->next_rec;
    free(p);
  }
  if (sptr->m_fd != FDNULL) {
    file_close(sptr->m_fd);
    sptr->m_fd = FDNULL;
  }
  CHECK_AND_FREE(sptr->pes_buffer);
  free(sptr);
}


/*
 * adv_past_pack_hdr - read the pack header, advance past it
 * we don't do anything with the data
 */
static void adv_past_pack_hdr (FDTYPE fd, 
			       uint8_t *pak,
			       uint32_t read_from_start)
{
  uint8_t stuffed;
  uint8_t readbyte;
  uint8_t val;
  if (read_from_start < 5) {
    file_skip_bytes(fd, 5 - read_from_start);
    file_read_bytes(fd, &readbyte, 1);
    val = readbyte;
  } else {
    val = pak[4];
  }
    
  // we've read 6 bytes
  if ((val & 0xc0) != 0x40) {
    // mpeg1
    file_skip_bytes(fd, 12 - read_from_start); // skip 6 more bytes
    return;
  }
  file_skip_bytes(fd, 13 - read_from_start);
  file_read_bytes(fd, &readbyte, 1);
  stuffed = readbyte & 0x7;
  file_skip_bytes(fd, stuffed);
}

/*
 * find_pack_start
 * look for the pack start code in the file - read 512 bytes at a time, 
 * searching for that code.
 * Note: we may also be okay looking for >= 00 00 01 bb
 */
static bool find_pack_start (FDTYPE fd, 
			     uint8_t *saved,
			     uint32_t len)
{
  uint8_t buffer[512];
  uint32_t buffer_on = 0, new_offset, scode;
  memcpy(buffer, saved, len);
  if (file_read_bytes(fd, buffer + len, sizeof(buffer) - len) == FALSE) {
    return FALSE;
  }
  while (1) {
    if (MP4AV_Mpeg3FindNextStart(buffer + buffer_on,
				 sizeof(buffer) - buffer_on, 
				 &new_offset, 
				 &scode) >= 0) {
      buffer_on += new_offset;
      if (scode == MPEG2_PS_PACKSTART) {
	file_skip_bytes(fd, buffer_on - 512); // go back to header
	return TRUE;
      }
      buffer_on += 1;
    } else {
      len = 0;
      if (buffer[sizeof(buffer) - 3] == 0 &&
	  buffer[sizeof(buffer) - 2] == 0 &&
	  buffer[sizeof(buffer) - 1] == 1) {
	buffer[0] = 0;
	buffer[1] = 0;
	buffer[2] = 1;
	len = 3;
      } else if (*(uint16_t *)(buffer + sizeof(buffer) - 2) == 0) {
	buffer[0] = 0;
	buffer[1] = 0;
	len = 2;
      } else if (buffer[sizeof(buffer) - 1] == 0) {
	buffer[0] = 0;
	len = 1;
      }
      if (file_read_bytes(fd, buffer + len, sizeof(buffer) - len) == FALSE) {
	return FALSE;
      }
      buffer_on = 0;
    }
  }
  return FALSE;
}

/*
 * copy_bytes_to_pes_buffer - read pes_len bytes into the buffer, 
 * adjusting it if we need it
 */
static void copy_bytes_to_pes_buffer (mpeg2ps_stream_t *sptr, 
			       uint16_t pes_len)
{
  uint32_t to_move;

  if (sptr->pes_buffer_size + pes_len > sptr->pes_buffer_size_max) {
    // if no room in the buffer, we'll move it - otherwise, just fill
    // note - we might want a better strategy about moving the buffer - 
    // right now, we might be moving a number of bytes if we have a large
    // followed by large frame.
    to_move = sptr->pes_buffer_size - sptr->pes_buffer_on;
    memmove(sptr->pes_buffer,
	    sptr->pes_buffer + sptr->pes_buffer_on,
	    to_move);
    sptr->pes_buffer_size = to_move;
    sptr->pes_buffer_on = 0;
    //printf("moving %d bytes\n", to_move);
    if (to_move + pes_len > sptr->pes_buffer_size_max) {
      sptr->pes_buffer = (uint8_t *)realloc(sptr->pes_buffer, 
					    to_move + pes_len + 2048);
      sptr->pes_buffer_size_max = to_move + pes_len + 2048;
    }
  }
  file_read_bytes(sptr->m_fd, sptr->pes_buffer + sptr->pes_buffer_size, pes_len);
  sptr->pes_buffer_size += pes_len;
#if 0
  printf("copying %u bytes - on %u size %u\n",
	 pes_len, sptr->pes_buffer_on, sptr->pes_buffer_size);
#endif
}

/*
 * read_to_next_pes_header - read the file, look for the next valid
 * pes header.  We will skip over PACK headers, but not over any of the
 * headers listed in 13818-1, table 2-18 - basically, anything with the
 * 00 00 01 and the next byte > 0xbb.
 * We return the pes len to read, and the "next byte"
 */
static bool read_to_next_pes_header (FDTYPE fd, 
				     uint8_t *stream_id,
				     uint16_t *pes_len)
{
  uint32_t hdr;
  uint8_t local[6];

  while (1) {
    // read the pes header
    if (file_read_bytes(fd, local, 6) == FALSE) {
      return FALSE;
    }

    hdr = convert32(local);
    // if we're not a 00 00 01, read until we get the next pack start
    // we might want to also read until next PES - look into that.
    if (((hdr & MPEG2_PS_START_MASK) != MPEG2_PS_START) ||
	(hdr < MPEG2_PS_END)) {
      if (find_pack_start(fd, local, 6) == FALSE) {
	return FALSE;
      }
      continue;
    }
    if (hdr == MPEG2_PS_PACKSTART) {
      // pack start code - we can skip down
      adv_past_pack_hdr(fd, local, 6);
      continue;
    }
    if (hdr == MPEG2_PS_END) {
      file_skip_bytes(fd, -2);
      continue;
    }

    // we should have a valid stream and pes_len here...
    *stream_id = hdr & 0xff;
    *pes_len = convert16(local + 4);
#if 0
    printf("loc: "X64" %x %x len %u\n", file_location(fd) - 6,
	   local[3],
	   *stream_id,
	   *pes_len);
#endif
    return TRUE;
  }
  return FALSE;
}

/*
 * read_pes_header_data
 * this should read past the pes header for the audio and video streams
 * it will store the timestamps if it reads them
 */
static bool read_pes_header_data (FDTYPE fd, 
				  uint16_t orig_pes_len,
				  uint16_t *pes_left,
				  bool *have_ts,
				  mpeg2ps_ts_t *ts)
{
  uint16_t pes_len = orig_pes_len;
  uint8_t local[10];
  uint32_t hdr_len;

  ts->have_pts = FALSE;
  ts->have_dts = FALSE;
  *have_ts = false;
  if (file_read_bytes(fd, local, 1) == FALSE) {
    return FALSE;
  }
  pes_len--; // remove this first byte from length
  while (*local == 0xff) {
    if (file_read_bytes(fd, local, 1) == FALSE) {
      return FALSE;
    }
    pes_len--;
    if (pes_len == 0) {
      *pes_left = 0;
      return TRUE;
    }
  }
  if ((*local & 0xc0) == 0x40) { 
    // buffer scale & size
    file_skip_bytes(fd, 1);
    if (file_read_bytes(fd, local, 1) == FALSE) {
      return FALSE;
    }
    pes_len -= 2;
  }

  if ((*local & 0xf0) == 0x20) {
    // mpeg-1 with pts
    if (file_read_bytes(fd, local + 1, 4) == FALSE) {
      return FALSE;
    }
    ts->have_pts = TRUE;
    ts->pts = ts->dts = read_pts(local);
    //printf("mpeg1 pts "U64"\n", ts->pts);
    *have_ts = true;
    pes_len -= 4;
  } else if ((*local & 0xf0) == 0x30) {
    // have mpeg 1 pts and dts
    if (file_read_bytes(fd, local + 1, 9) == FALSE) {
      return FALSE;
    }
    ts->have_pts = TRUE;
    ts->have_dts = TRUE;
    *have_ts = true;
    ts->pts = read_pts(local);
    ts->dts = read_pts(local + 5);
    pes_len -= 9;
  } else if ((*local & 0xc0) == 0x80) {
    // mpeg2 pes header  - we're pointing at the flags field now
    if (file_read_bytes(fd, local + 1, 2) == FALSE) {
      return FALSE;
    }
    hdr_len = local[2];
    pes_len -= hdr_len + 2; // first byte removed already
    if ((local[1] & 0xc0) == 0x80) {
      // just pts
      ts->have_pts = TRUE;
      file_read_bytes(fd, local, 5);
      ts->pts = ts->dts = read_pts(local);
      //printf("pts "U64"\n", ts->pts);
      *have_ts = true;
      hdr_len -= 5;
    } else if ((local[1] & 0xc0) == 0xc0) {
      // pts and dts
      ts->have_pts = TRUE;
      ts->have_dts = TRUE;
      *have_ts = true;
      file_read_bytes(fd, local, 10);
      ts->pts = read_pts(local);
      ts->dts = read_pts(local  + 5);    
      hdr_len -= 10;
    }
    file_skip_bytes(fd, hdr_len);
  } else if (*local != 0xf) {
    file_skip_bytes(fd, pes_len);
    pes_len = 0;
  } 
  *pes_left = pes_len;
  return TRUE;
}

static bool search_for_next_pes_header (mpeg2ps_stream_t *sptr, 
					uint16_t *pes_len,
					bool *have_ts, 
					off_t *found_loc)
{
  uint8_t stream_id;
  uint8_t local;
  off_t loc;
  mpeg2ps_ts_t *ps_ts;
  while (1) {
    // this will read until we find the next pes.  We don't know if the
    // stream matches - this will read over pack headers
    if (read_to_next_pes_header(sptr->m_fd, &stream_id, pes_len) == FALSE) {
      return FALSE;
    }

    if (stream_id != sptr->m_stream_id) {
      file_skip_bytes(sptr->m_fd, *pes_len);
      continue;
    }
    loc = file_location(sptr->m_fd) - 6; 
    // advance past header, reading pts
    // if our existing next_ps has pts stuff, we use the next_next
    // this happens when a frame is small, and at the very end of 
    // a pes, with a frame starting at the next pes.
    ps_ts = (sptr->next_pes_ts.have_pts || sptr->next_pes_ts.have_dts) ?
      &sptr->next_next_pes_ts : &sptr->next_pes_ts;

    if (read_pes_header_data(sptr->m_fd, 
			     *pes_len, 
			     pes_len, 
			     have_ts, 
			     ps_ts) == FALSE) {
      return FALSE;
    }
#if 0
    printf("loc: "X64" have ts %u "U64" %d\n",
	   loc, *have_ts, sptr->next_pes_ts.dts, *pes_len);
#endif
    // If we're looking at a private stream, make sure that the sub-stream
    // matches.
    if (sptr->m_stream_id == 0xbd) {
      // ac3 or pcm
      file_read_bytes(sptr->m_fd, &local, 1);
      *pes_len -= 1;
      if (local != sptr->m_substream_id) {
	file_skip_bytes(sptr->m_fd, *pes_len);
	continue; // skip to the next one
      }
      if (sptr->m_substream_id >= 0xa0) {
	*pes_len -= 6;
	file_read_bytes(sptr->m_fd, sptr->audio_private_stream_info, 6);
#if 0
	printf("reading %x %x %x %x\n", 
	       sptr->audio_private_stream_info[0],
	       sptr->audio_private_stream_info[1],
	       sptr->audio_private_stream_info[2],
	       sptr->audio_private_stream_info[3]);
#endif
      } else {
	*pes_len -= 3;
	file_skip_bytes(sptr->m_fd, 3); // 4 bytes - we don't need now...
      }
      // we need more here...
    }
    if (*have_ts) {
      mpeg2ps_record_pts(sptr, loc, ps_ts);
    }
    if (found_loc != NULL) *found_loc = loc;
    return true;
  }
  return false;
}

/*
 * mpeg2ps_stream_read_next_pes_buffer - for the given stream, 
 * go forward in the file until the next PES for the stream is read.  Read
 * the header (pts, dts), and read the data into the pes_buffer pointer
 */
static bool mpeg2ps_stream_read_next_pes_buffer (mpeg2ps_stream_t *sptr)
{
  uint16_t pes_len;
  bool have_ts;

  if (search_for_next_pes_header(sptr, &pes_len, &have_ts, NULL) == false) {
    return false;
  }

  copy_bytes_to_pes_buffer(sptr, pes_len);

  return TRUE;
}


static void copy_next_pes_ts_to_frame_ts (mpeg2ps_stream_t *sptr)
{
  sptr->frame_ts = sptr->next_pes_ts;
#if 1
  sptr->next_pes_ts = sptr->next_next_pes_ts;
  sptr->next_next_pes_ts.have_pts = 
    sptr->next_next_pes_ts.have_dts = false;
#else
  sptr->next_pes_ts.have_pts = sptr->next_pes_ts.have_dts = false;
#endif
}

/***************************************************************************
 * Frame reading routine.  For each stream, the fd's should be different.
 * we will read from the pes stream, and save it in the stream's pes buffer.
 * This will give us raw data that we can search through for frame headers, 
 * and the like.  We shouldn't read more than we need - when we need to read, 
 * we'll put the whole next pes buffer in the buffer
 *
 * Audio routines are of the format:
 *   look for header
 *   determine length
 *   make sure length is in buffer
 *
 * Video routines
 *   look for start header (GOP, SEQ, Picture)
 *   look for pict header
 *   look for next start (END, GOP, SEQ, Picture)
 *   
 ***************************************************************************/
#define IS_MPEG_START(a) ((a) == 0xb3 || (a) == 0x00 || (a) == 0xb8)

static bool 
mpeg2ps_stream_find_mpeg_video_frame (mpeg2ps_stream_t *sptr)
{
  uint32_t offset, scode;
  bool have_pict;
  bool started_new_pes = false;
  uint32_t start;
  /*
   * First thing - determine if we have enough bytes to read the header.
   * if we do, we have the correct timestamp.  If not, we read the new
   * pes, so we'd want to use the timestamp we read.
   */
  sptr->frame_ts = sptr->next_pes_ts; 
#if 0
  printf("frame read %u "U64"\n", sptr->frame_ts.have_dts, sptr->frame_ts.dts);
#endif