// ****************** Copyright (c) 2013 by Gigaset Communications GmbH ******
// Filename : tdav_codec_fsl_vpu_h264.c
// Owner    : Souparna Lodh [STC]
//-------------------------------------------------------------------------
// Defintion : H.264 codec plugin using Freescale VPU HW codec Interface.
// RTP payloader/depayloader follows RFC 3984
//-------------------------------------------------------------------------
// 26-07-2013, Gigaset Communications GmbH
// Added FSL VPU H264  codec plugin to Doubango Framework
//*************************************************************************

/*
* This file is part of Open Source Doubango Framework.
*
* DOUBANGO 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 3 of the License, or
* (at your option) any later version.
*
* DOUBANGO 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 DOUBANGO.
*
*/

#include <errno.h>
#include <sys/stat.h>
#include <stdio.h>
#include <fcntl.h>

#include "tinydav/codecs/h264/tdav_codec_fsl_vpu_h264.h"

#include "tinydav/codecs/h264/tdav_codec_h264_rtp.h"
#include "tinydav/video/tdav_converter_video.h"

#include "tinyrtp/rtp/trtp_rtp_packet.h"

#include "tinymedia/tmedia_params.h"

#include "tsk_params.h"
#include "tsk_memory.h"
#include "tsk_debug.h"

#include "vpu_lib.h"
#include "vpu_io.h"

#define TDAV_H264_GOP_SIZE_IN_SECONDS		25
#define STREAM_BUF_SIZE		0x200000
#define PS_SAVE_SIZE		0x080000
#define STREAM_END_SIZE		0

#define FB_INDEX_MASK	(NUM_FRAME_BUFS - 1)
#define NUM_FRAME_BUFS	32

enum {
    MODE420 = 0,
    MODE422 = 1,
    MODE224 = 2,
    MODE444 = 3,
    MODE400 = 4
};

struct frame_buf {
	    int addrY;
	    int addrCb;
	    int addrCr;
	    int strideY;
	    int strideC;
	    int mvColBuf;
	    vpu_mem_desc desc;
    };

static int fb_index;
static struct frame_buf *fbarray[NUM_FRAME_BUFS];
static struct frame_buf fbpool[NUM_FRAME_BUFS];

static tsk_bool_t decoder_stream_parsed;

typedef struct tdav_codec_fsl_vpu_h264_s
{
	TDAV_DECLARE_CODEC_H264_COMMON;

	// Encoder
	struct{
		void* buffer;
        tsk_size_t buffer_size;
        void* header_out_data;
        tsk_size_t header_out_data_size;
		int64_t frame_count;
		tsk_bool_t force_idr;
		//int rotation;

        vpu_mem_desc* mem_desc;
        EncHandle handle;		/* Encoder handle */
	    PhysicalAddress phy_bsbuf_addr; /* Physical bitstream buffer */
	    uint32_t virt_bsbuf_addr;		/* Virtual bitstream buffer */
	    int enc_picwidth;	/* Encoded Picture width */
	    int enc_picheight;	/* Encoded Picture height */
	    int src_picwidth;        /* Source Picture width */
	    int src_picheight;       /* Source Picture height */
	    int totalfb;	/* Total number of framebuffers allocated */
	    int src_fbid;	/* Index of frame buffer that contains YUV image */
	    FrameBuffer *fb; /* frame buffer base given to encoder */
	    struct frame_buf **pfbpool; /* allocated fb pointers are stored here */
	    int minFrameBufferCount;
	} encoder;
	
	// decoder
	struct{
		void* accumulator;
		tsk_size_t accumulator_pos;
		tsk_size_t accumulator_size;
		uint16_t last_seq;

        vpu_mem_desc* mem_desc;
        vpu_mem_desc* ps_mem_desc;
        vpu_mem_desc* slice_mem_desc;
        DecHandle handle;
	    PhysicalAddress phy_bsbuf_addr;
        PhysicalAddress phy_ps_buf;
	    PhysicalAddress phy_slice_buf;
        int phy_slicebuf_size;
	    uint32_t virt_bsbuf_addr;
	    int picwidth;
	    int picheight;
	    int stride;
	    int regfbcount;
	    int minfbcount;
	    FrameBuffer *fb;
	    struct frame_buf **pfbpool;
	    Rect picCropRect;
	} decoder;
}
tdav_codec_fsl_vpu_h264_t;

static int	freadn(int fd, void **vptr, size_t n);
static void framebuf_init(void);
static int mem_copy(const void *inputBuff, void *destBuff, int n);
static int dec_fill_bsbuffer(const void* inputBuff, DecHandle handle, uint32_t bs_va_startaddr, uint32_t bs_va_endaddr,
		uint32_t bs_pa_startaddr, int defaultsize,
		int *eos, int *fill_end_bs);
static int dec_parse(tdav_codec_fsl_vpu_h264_t* self);
static struct frame_buf *get_framebuf(void);
static struct frame_buf *framebuf_alloc(int stdMode, int format, int strideY, int height, int mvCol);
static void put_framebuf(struct frame_buf *fb);
static void framebuf_free(struct frame_buf *fb);
static void saveCropYuvImageHelper(tdav_codec_fsl_vpu_h264_t* self, uint8_t *buf, Rect cropRect, void **out_data);
static void saveNV12ImageHelper(uint8_t *pYuv, tdav_codec_fsl_vpu_h264_t* self, uint8_t *buf);
static void write_to_output(tdav_codec_fsl_vpu_h264_t* self, Rect cropRect, int index, void **out_data, tsk_size_t* out_max_size);
static int encoder_allocate_framebuffer(tdav_codec_fsl_vpu_h264_t* self);
static int encoder_readbs_reset_buffer(tdav_codec_fsl_vpu_h264_t* self, PhysicalAddress paBsBufAddr, int bsBufsize, void **out_data, tsk_size_t* out_max_size, tsk_bool_t append, tsk_size_t appendAfter);
static int encode_one_frame(tdav_codec_fsl_vpu_h264_t* self, const void* in_data, tsk_size_t in_size, void **out_data, tsk_size_t* out_max_size);
static void encoder_free_framebuffer(tdav_codec_fsl_vpu_h264_t* self);
static int decoder_allocate_framebuffer(tdav_codec_fsl_vpu_h264_t* self);
static int decode_one_frame(tdav_codec_fsl_vpu_h264_t* self, void **out_data, tsk_size_t* out_max_size, int mode);
static void decoder_free_framebuffer(tdav_codec_fsl_vpu_h264_t* self);

static int tdav_codec_fsl_vpu_h264_init(tdav_codec_fsl_vpu_h264_t* self, profile_idc_t profile);
static int tdav_codec_fsl_vpu_h264_deinit(tdav_codec_fsl_vpu_h264_t* self);
static int tdav_codec_fsl_vpu_h264_open_encoder(tdav_codec_fsl_vpu_h264_t* self);
static int tdav_codec_fsl_vpu_h264_close_encoder(tdav_codec_fsl_vpu_h264_t* self);
static int tdav_codec_fsl_vpu_h264_open_decoder(tdav_codec_fsl_vpu_h264_t* self);
static int tdav_codec_fsl_vpu_h264_close_decoder(tdav_codec_fsl_vpu_h264_t* self);

static void tdav_codec_fsl_vpu_h264_encap(const tdav_codec_fsl_vpu_h264_t* h264, const uint8_t* pdata, tsk_size_t size);

/* ============ H.264 Base/Main Profile X.X Plugin interface functions ================= */

static int tdav_codec_fsl_vpu_h264_set(tmedia_codec_t* self, const tmedia_param_t* param)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_set in\n");
	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)self;
	if(!self->opened){
		TSK_DEBUG_ERROR("Codec not opened");
		return -1;
	}
	if(param->value_type == tmedia_pvt_int32){
		if(tsk_striequals(param->key, "action")){
			tmedia_codec_action_t action = (tmedia_codec_action_t)TSK_TO_INT32((uint8_t*)param->value);
			switch(action){
				case tmedia_codec_action_encode_idr:
					{
						h264->encoder.force_idr = tsk_true;
						break;
					}
				case tmedia_codec_action_bw_down:
					{
						break;
					}
				case tmedia_codec_action_bw_up:
					{
						break;
					}
			}
			return 0;
		}
		else if(tsk_striequals(param->key, "rotation")){
            //TODO: Complete Rotation for VPU
/*			int rotation = *((int32_t*)param->value);
			if(h264->encoder.rotation != rotation){
				if(self->opened){
					int ret;
					h264->encoder.rotation = rotation;
					if((ret = tdav_codec_fsl_vpu_h264_close_encoder(h264))){
						return ret;
					}
					if((ret = tdav_codec_fsl_vpu_h264_open_encoder(h264))){
						return ret;
					}
#if 0 // Not working
					if((ret = avcodec_close(h264->encoder.context))){
						TSK_DEBUG_ERROR("Failed to close [%s] codec", TMEDIA_CODEC(h264)->plugin->desc);
						return ret;
					}
					h264->encoder.context->width = (rotation == 90 || rotation == 270) ? TMEDIA_CODEC_VIDEO(h264)->out.height : TMEDIA_CODEC_VIDEO(h264)->out.width;
					h264->encoder.context->height = (rotation == 90 || rotation == 270) ? TMEDIA_CODEC_VIDEO(h264)->out.width : TMEDIA_CODEC_VIDEO(h264)->out.height;
					if((ret = avcodec_open(h264->encoder.context, h264->encoder.codec)) < 0){
						TSK_DEBUG_ERROR("Failed to open [%s] codec", TMEDIA_CODEC(h264)->plugin->desc);
						return ret;
					}
					h264->encoder.force_idr = tsk_true;
#endif
				}
			}*/
			return 0;//Doubango is fooled into thinking that rotation setting is done successfully TODO: FIX this
		}
	}
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_set out\n");
	return -1;
}

/* Read n bytes from a file descriptor */
int	freadn(int fd, void **vptr, size_t n)
{
	int nleft = 0;
	int nread = 0;
    char  *ptr;

	ptr = *vptr;
	nleft = n;

    if(nleft == -1) { //meaning read complete file
        struct stat fileStat;
        if(fstat(fd, &fileStat) < 0) {
            return -1;
        }
        nleft = fileStat.st_size;
        //__android_log_print("Teleca", "FSL VPU H264", "freadn nleft = %ld\n", nleft);

        if(ptr == NULL) {
            ptr = (char *) (tsk_calloc(nleft, sizeof(uint8_t)));
            if(ptr == NULL) {
                return -1;
            }
            *vptr = (void *) ptr;
        }
    }

    while (nleft > 0) {
        if ( (nread = read(fd, ptr, nleft)) <= 0) {
			if (nread == 0)
				return (n - nleft);

			perror("read");
			return (-1);			/* error EINTR XXX */
		}

		nleft -= nread;
		ptr   += nread;
	}

    return (nread);
}

void framebuf_init(void)
{
	int i;

	for (i = 0; i < NUM_FRAME_BUFS; i++) {
		fbarray[i] = &fbpool[i];
	}
}

int mem_copy(const void *inputBuff, void *destBuff, int n)
{
    memcpy(destBuff, inputBuff, n);
    return n; //TODO: Remove this return and its dependencies in the whole code
}

int dec_fill_bsbuffer(const void* inputBuff, DecHandle handle, uint32_t bs_va_startaddr, uint32_t bs_va_endaddr,
		uint32_t bs_pa_startaddr, int defaultsize,
		int *eos, int *fill_end_bs)
{
    //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer in\n");
	RetCode ret;
	PhysicalAddress pa_read_ptr, pa_write_ptr;
	uint32_t target_addr, space;
	int size;
	int nread, room;
	*eos = 0;

	ret = vpu_DecGetBitstreamBuffer(handle, &pa_read_ptr, &pa_write_ptr,
					&space);
	if (ret != RETCODE_SUCCESS) {
        //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer vpu_DecGetBitstreamBuffer failed\n");
		TSK_DEBUG_ERROR("vpu_DecGetBitstreamBuffer failed\n");
		return -1;
	}

	/* Decoder bitstream buffer is empty */
	if (space <= 0) {
        //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer no space!!!\n");
		return 0;
    }

	if (defaultsize > 0) {
		if (space < defaultsize) {
            //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer not enuf space!!!\n");
			return 0;
        }
		size = defaultsize;
	} else {
		size = ((space >> 9) << 9);
	}

	if (size == 0) {
        //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer space == 0!!!\n");
		return 0;
    }
	/* Fill the bitstream buffer */
	target_addr = bs_va_startaddr + (pa_write_ptr - bs_pa_startaddr);
	if ( (target_addr + size) > bs_va_endaddr) {
        //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - 1\n");
		room = bs_va_endaddr - target_addr;
		nread = mem_copy(inputBuff, (void *)target_addr, room);
		if (nread <= 0) {
			/* EOF or error */
			if (nread < 0) {
				if (nread == -EAGAIN)
					return 0;

				return -1;
			}

			*eos = 1;
		} else {
			/* unable to fill the requested size, so back off! */
			if (nread != room)
				goto update;

			/* read the remaining */
			space = nread;
            //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - 2\n");
			nread = mem_copy(inputBuff, (void *)bs_va_startaddr, (size - room));
			if (nread <= 0) {
				/* EOF or error */
				if (nread < 0) {
					if (nread == -EAGAIN)
						return 0;

					return -1;
				}

				*eos = 1;
			}

			nread += space;
		}
	} else {
        //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - 3\n");
		nread = mem_copy(inputBuff, (void *)target_addr, size);
		if (nread <= 0) {
            //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - 3 - error\n");
			/* EOF or error */
			if (nread < 0) {
				if (nread == -EAGAIN)
					return 0;

				return -1;
			}

			*eos = 1;
		}
	}

update:
	if (*eos == 0) {
        //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - attempted vpu_DecUpdateBitstreamBuffer\n");
		ret = vpu_DecUpdateBitstreamBuffer(handle, nread);
		if (ret != RETCODE_SUCCESS) {
            //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - vpu_DecUpdateBitstreamBuffer failed\n");
			TSK_DEBUG_ERROR("vpu_DecUpdateBitstreamBuffer failed\n");
			return -1;
		}
		*fill_end_bs = 0;
	} else {
		if (!*fill_end_bs) {
            //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - attempted vpu_DecUpdateBitstreamBuffer\n");
			ret = vpu_DecUpdateBitstreamBuffer(handle,
					STREAM_END_SIZE);
			if (ret != RETCODE_SUCCESS) {
                //__android_log_print("Teleca", "FSL VPU H264", "dec_fill_bsbuffer - vpu_DecUpdateBitstreamBuffer failed\n");
				TSK_DEBUG_ERROR("vpu_DecUpdateBitstreamBuffer failed"
								"\n");
				return -1;
			}
			*fill_end_bs = 1;
		}

	}

	return nread;
}

int dec_parse(tdav_codec_fsl_vpu_h264_t* self)
{
	DecInitialInfo initinfo = {0};
	DecHandle handle = self->decoder.handle;
	int align, profile, level, extended_fbcount;
	RetCode ret;
	char *count;

	/* Parse bitstream and get width/height/framerate etc */
	vpu_DecSetEscSeqInit(handle, 1);
	ret = vpu_DecGetInitialInfo(handle, &initinfo);
	vpu_DecSetEscSeqInit(handle, 0);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("vpu_DecGetInitialInfo failed, ret:%d, errorcode:%ld\n",
		         ret, initinfo.errorcode);
		return -1;
	}

	/*
	 * It is suggested to add two more buffers than minFrameBufferCount:
	 *
	 * vpu_DecClrDispFlag is used to control framebuffer whether can be
	 * used for decoder again. One framebuffer dequeue from IPU is delayed
	 * for performance improvement and one framebuffer is delayed for
	 * display flag clear.
	 *
	 * Performance is better when more buffers are used if IPU performance
	 * is bottleneck.
	 *
	 * Two more buffers may be needed for interlace stream from IPU DVI view
	 */
	self->decoder.minfbcount = initinfo.minFrameBufferCount;
	count = getenv("VPU_EXTENDED_BUFFER_COUNT");
	if (count)
		extended_fbcount = atoi(count);
	else
		extended_fbcount = 2;

	if (initinfo.interlace)
		self->decoder.regfbcount = self->decoder.minfbcount + extended_fbcount + 2;
	else
		self->decoder.regfbcount = self->decoder.minfbcount + extended_fbcount;

    //__android_log_print("Teleca", "FSL VPU H264", "minfbcount = %d\n", self->decoder.minfbcount);
    //__android_log_print("Teleca", "FSL VPU H264", "regfbcount = %d\n", self->decoder.regfbcount);

	self->decoder.picwidth = ((initinfo.picWidth + 15) & ~15);

    //__android_log_print("Teleca", "FSL VPU H264", "picwidth from vpu= %d\n", self->decoder.picwidth);

    if(initinfo.interlace == 1)
	    align = 32;
	
	self->decoder.picheight = ((initinfo.picHeight + align - 1) & ~(align - 1));

    //__android_log_print("Teleca", "FSL VPU H264", "picheight from vpu = %d\n", self->decoder.picheight);

	if ((self->decoder.picwidth == 0) || (self->decoder.picheight == 0))
		return -1;

    /*__android_log_print("Teleca", "FSL VPU H264", "CROP left/top/right/bottom %lu %lu %lu %lu\n",
					initinfo.picCropRect.left,
					initinfo.picCropRect.top,
					initinfo.picCropRect.right,
					initinfo.picCropRect.bottom);*/

	/* Add non-h264 crop support, assume left=top=0 */
	if ((self->decoder.picwidth > initinfo.picWidth ||
		self->decoder.picheight > initinfo.picHeight) &&
		(!initinfo.picCropRect.left &&
		!initinfo.picCropRect.top &&
		!initinfo.picCropRect.right &&
		!initinfo.picCropRect.bottom)) {
        //__android_log_print("Teleca", "FSL VPU H264", "Getting in Add non-h264 crop support\n");
		initinfo.picCropRect.left = 0;
		initinfo.picCropRect.top = 0;
		initinfo.picCropRect.right = initinfo.picWidth;
		initinfo.picCropRect.bottom = initinfo.picHeight;
	}

    /*__android_log_print("Teleca", "FSL VPU H264", "CROP left/top/right/bottom %lu %lu %lu %lu\n",
					initinfo.picCropRect.left,
					initinfo.picCropRect.top,
					initinfo.picCropRect.right,
					initinfo.picCropRect.bottom);*/

    memcpy(&(self->decoder.picCropRect), &(initinfo.picCropRect),
	    sizeof(initinfo.picCropRect));

	/* worstSliceSize is in kilo-byte unit */
	self->decoder.phy_slicebuf_size = initinfo.worstSliceSize * 1024;
	self->decoder.stride = self->decoder.picwidth;

    self->decoder.slice_mem_desc->size = self->decoder.phy_slicebuf_size;
	ret = IOGetPhyMem((self->decoder.slice_mem_desc));
	if (ret) {
		TSK_DEBUG_ERROR("Unable to obtain physical slice save mem\n");
		return ret;
	}
	self->decoder.phy_slice_buf = self->decoder.slice_mem_desc->phy_addr;

	return 0;
}

struct frame_buf *get_framebuf(void)
{
	struct frame_buf *fb;

	fb = fbarray[fb_index];
	fbarray[fb_index] = 0;

	++fb_index;
	fb_index &= FB_INDEX_MASK;

	return fb;
}

struct frame_buf *framebuf_alloc(int stdMode, int format, int strideY, int height, int mvCol)
{
	struct frame_buf *fb;
	int err;
	int divX, divY;

	fb = get_framebuf();
	if (fb == NULL) {
        TSK_DEBUG_ERROR("framebuf_alloc - get_framebuf == NULL\n");
		return NULL;
    }

    divX = (format == MODE420 || format == MODE422) ? 2 : 1;
	divY = (format == MODE420 || format == MODE224) ? 2 : 1;

	memset(&(fb->desc), 0, sizeof(vpu_mem_desc));
	fb->desc.size = (strideY * height  + strideY / divX * height / divY * 2);
	if (mvCol)
		fb->desc.size += strideY / divX * height / divY;

	err = IOGetPhyMem(&fb->desc);
	if (err) {
        TSK_DEBUG_ERROR("framebuf_alloc - IOGetPhyMem error!\n");
		memset(&(fb->desc), 0, sizeof(vpu_mem_desc));
		return NULL;
	}

	fb->addrY = fb->desc.phy_addr;
	fb->addrCb = fb->addrY + strideY * height;
	fb->addrCr = fb->addrCb + strideY / divX * height / divY;
	fb->strideY = strideY;
	fb->strideC =  strideY / divX;
	if (mvCol)
		fb->mvColBuf = fb->addrCr + strideY / divX * height / divY;

	fb->desc.virt_uaddr = IOGetVirtMem(&(fb->desc));
	if (fb->desc.virt_uaddr <= 0) {
        TSK_DEBUG_ERROR("framebuf_alloc - IOGetVirtMem error!\n");
		IOFreePhyMem(&fb->desc);
		memset(&(fb->desc), 0, sizeof(vpu_mem_desc));
		return NULL;
	}

	return fb;
}

void put_framebuf(struct frame_buf *fb)
{
	--fb_index;
	fb_index &= FB_INDEX_MASK;

	fbarray[fb_index] = fb;
}

void framebuf_free(struct frame_buf *fb)
{
	if (fb == NULL)
		return;

	if (fb->desc.virt_uaddr) {
		IOFreeVirtMem(&fb->desc);
	}

	if (fb->desc.phy_addr) {
		IOFreePhyMem(&fb->desc);
	}

	memset(&(fb->desc), 0, sizeof(vpu_mem_desc));
	put_framebuf(fb);
}

void saveCropYuvImageHelper(tdav_codec_fsl_vpu_h264_t* self, uint8_t *buf, Rect cropRect, void **out_data)
{
	uint8_t *pCropYuv;
	int cropWidth, cropHeight;
	int i;

	int width = self->decoder.picwidth;
	int height = self->decoder.picheight;
	
    void *out_data_ptr = *out_data;

	if (!buf) {
		TSK_DEBUG_ERROR("buffer point should not be NULL.\n");
		return;
	}

	pCropYuv = buf;
	cropWidth = cropRect.right - cropRect.left;
	cropHeight = cropRect.bottom - cropRect.top;

	pCropYuv += width * cropRect.top;
	pCropYuv += cropRect.left;

	for (i = 0; i < cropHeight; i++) {
        mem_copy(pCropYuv, out_data_ptr, cropWidth);
        out_data_ptr += cropWidth;
		pCropYuv += width;
	}
    
	cropWidth /= 2;
	cropHeight /= 2;
	pCropYuv = buf + (width * height);
	pCropYuv += (width / 2) * (cropRect.top / 2);
	pCropYuv += cropRect.left / 2;

	for (i = 0; i < cropHeight; i++) {
        mem_copy(pCropYuv, out_data_ptr, cropWidth);
        out_data_ptr += cropWidth;
		pCropYuv += width / 2;
	}
    
	pCropYuv = buf + (width * height) * 5 / 4;
	pCropYuv += (width / 2) * (cropRect.top / 2);
	pCropYuv += cropRect.left / 2;

	for (i = 0; i < cropHeight; i++) {
        mem_copy(pCropYuv, out_data_ptr, cropWidth);
        out_data_ptr += cropWidth;
		pCropYuv += width / 2;
	}
}

/*
 * This function is to convert framebuffer from interleaved Cb/Cr mode
 * to non-interleaved Cb/Cr mode.
 *
 * Note: This function does _NOT_ really store this framebuffer into file.
 */
void saveNV12ImageHelper(uint8_t *pYuv, tdav_codec_fsl_vpu_h264_t* self, uint8_t *buf)
{
	int Y, Cb;
	uint8_t *Y1, *Cb1, *Cr1;
	int img_size;
	int y, x;
	uint8_t *tmp;
	int height = self->decoder.picheight;
	int stride = self->decoder.stride;

	if (!pYuv || !buf) {
		TSK_DEBUG_ERROR("pYuv or buf should not be NULL.\n");
		return;
	}

	img_size = stride * height;

	Y = (int)buf;
	Cb = Y + img_size;

	Y1 = pYuv;
	Cb1 = Y1 + img_size;
	Cr1 = Cb1 + (img_size >> 2);

	memcpy(Y1, (uint8_t *)Y, img_size);

	for (y = 0; y < (self->decoder.picheight / 2); y++) {
		tmp = (uint8_t *)(Cb + self->decoder.picwidth * y);
		for (x = 0; x < self->decoder.picwidth; x += 2) {
			*Cb1++ = tmp[x];
			*Cr1++ = tmp[x + 1];
		}
	}
}

void write_to_output(tdav_codec_fsl_vpu_h264_t* self, Rect cropRect, int index, void **out_data, tsk_size_t* out_max_size)
{
    //__android_log_print("Teleca", "FSL VPU H264", "write_to_output - in\n");
	int height = (self->decoder.picheight + 15) & ~15 ;
	int stride = self->decoder.stride;
	int chromaInterleave = 1;
	int img_size;
	uint8_t *pYuv = NULL, *pYuv0 = NULL, *buf;
	struct frame_buf *pfb = NULL;
	int cropping;

	cropping = cropRect.left | cropRect.top | cropRect.bottom | cropRect.right;

	pfb = self->decoder.pfbpool[index];
	buf = (uint8_t *)(pfb->addrY + pfb->desc.virt_uaddr - pfb->desc.phy_addr);

	img_size = stride * height * 3 / 2;
	
    if(!(*out_data)) {
        if(!(*out_data = tsk_malloc(img_size))){
			TSK_DEBUG_ERROR("Failed to allocate new buffer");
		}
		*out_max_size = img_size;
    } else if(*out_max_size != img_size) {
        if(!(*out_data = tsk_realloc(*out_data, img_size))){
			TSK_DEBUG_ERROR("Failed to allocate new buffer");
		}
		*out_max_size = img_size;
    }

	/*
	 * There could be these three cases:
	 * interleave == 0, cropping == 1
	 * interleave == 1, cropping == 0
	 * interleave == 1, cropping == 1
	 */
	if (chromaInterleave) {
		if (!pYuv) {
			pYuv0 = pYuv = tsk_malloc(img_size);
			if (!pYuv)
				TSK_DEBUG_ERROR("malloc error\n");
		}

        //__android_log_print("Teleca", "FSL VPU H264", "write_to_output - saveNV12ImageHelper\n");
		saveNV12ImageHelper(pYuv, self, buf);
	}

	if (cropping) {
        //__android_log_print("Teleca", "FSL VPU H264", "write_to_output - cropping\n");
		if (!pYuv0) {
			pYuv0 = pYuv = tsk_malloc(img_size);
			if (!pYuv)
				TSK_DEBUG_ERROR("malloc error\n");

			saveCropYuvImageHelper(self, buf, cropRect, *out_data);
		} else {/* process the buf beginning at pYuv0 again for Crop. */
			pYuv = pYuv0;
			saveCropYuvImageHelper(self, pYuv, cropRect, *out_data);
		}

	} else {
        //__android_log_print("Teleca", "FSL VPU H264", "write_to_output - NO cropping\n");
        mem_copy((uint8_t *)pYuv0, *out_data, img_size);
	}

out:
	if (pYuv0) {
		TSK_FREE(pYuv0);
		pYuv0 = NULL;
		pYuv = NULL;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "write_to_output - out\n");
	return;
}

int encoder_allocate_framebuffer(tdav_codec_fsl_vpu_h264_t* self)
{
    EncHandle handle = self->encoder.handle;
	int i, enc_stride, src_stride, src_fbid;
	int totalfb, minfbcount, srcfbcount, extrafbcount;
	RetCode ret;
	FrameBuffer *fb;
	PhysicalAddress subSampBaseA = 0, subSampBaseB = 0;
	struct frame_buf **pfbpool;
	EncExtBufInfo extbufinfo = {0};
	int enc_fbwidth, enc_fbheight, src_fbwidth, src_fbheight;

	minfbcount = self->encoder.minFrameBufferCount;
	srcfbcount = 1;

	enc_fbwidth = (self->encoder.enc_picwidth + 15) & ~15;
	enc_fbheight = (self->encoder.enc_picheight + 15) & ~15;
	src_fbwidth = (self->encoder.src_picwidth + 15) & ~15;
	src_fbheight = (self->encoder.src_picheight + 15) & ~15;
    /*enc_fbwidth = self->encoder.enc_picwidth;
	enc_fbheight = self->encoder.enc_picheight;
	src_fbwidth = self->encoder.src_picwidth;
	src_fbheight = self->encoder.src_picheight;*/

	if (cpu_is_mx6x()) 
        extrafbcount = 2; /* Subsamp buffer [2] */
	else
		extrafbcount = 0;

	self->encoder.totalfb = totalfb = minfbcount + extrafbcount + srcfbcount;

	/* last framebuffer is used as src frame in the test */
	self->encoder.src_fbid = src_fbid = totalfb - 1;

	fb = self->encoder.fb = tsk_calloc(totalfb, sizeof(FrameBuffer));
	if (fb == NULL) {
		TSK_DEBUG_ERROR("Failed to allocate enc->fb\n");
		return -1;
	}

	pfbpool = self->encoder.pfbpool = tsk_calloc(totalfb,
					sizeof(struct frame_buf *));
	if (pfbpool == NULL) {
		TSK_DEBUG_ERROR("Failed to allocate enc->pfbpool\n");
		TSK_FREE(fb);
		return -1;
	}

	/* All buffers are linear */
	for (i = 0; i < minfbcount + extrafbcount; i++) {
		pfbpool[i] = framebuf_alloc(STD_AVC, /*enc->mjpg_fmt*/0,
					    enc_fbwidth, enc_fbheight, 0);
		if (pfbpool[i] == NULL) {
			goto err1;
		}
	}
	
	for (i = 0; i < minfbcount + extrafbcount; i++) {
		fb[i].myIndex = i;
		fb[i].bufY = pfbpool[i]->addrY;
		fb[i].bufCb = pfbpool[i]->addrCb;
		fb[i].bufCr = pfbpool[i]->addrCr;
		fb[i].strideY = pfbpool[i]->strideY;
		fb[i].strideC = pfbpool[i]->strideC;
	}

	if (cpu_is_mx6x() ) {
		subSampBaseA = fb[minfbcount].bufY;
		subSampBaseB = fb[minfbcount + 1].bufY;
	}

	enc_stride = (self->encoder.enc_picwidth + 15) & ~15;
	src_stride = (self->encoder.src_picwidth + 15 ) & ~15;
    /*enc_stride = self->encoder.enc_picwidth;
	src_stride = self->encoder.src_picwidth;*/

	//extbufinfo.scratchBuf = self->encoder.scratchBuf;
	ret = vpu_EncRegisterFrameBuffer(handle, fb, minfbcount, enc_stride, src_stride,
					    subSampBaseA, subSampBaseB, &extbufinfo);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("Register frame buffer failed\n");
		goto err1;
	}

	/* Allocate a single frame buffer for source frame */
	pfbpool[src_fbid] = framebuf_alloc(STD_AVC, /*enc->mjpg_fmt*/0,
					   src_fbwidth, src_fbheight, 0);
	if (pfbpool[src_fbid] == NULL) {
		TSK_DEBUG_ERROR("failed to allocate single framebuf\n");
		goto err1;
	}

	fb[src_fbid].myIndex = self->encoder.src_fbid;
	fb[src_fbid].bufY = pfbpool[src_fbid]->addrY;
	fb[src_fbid].bufCb = pfbpool[src_fbid]->addrCb;
	fb[src_fbid].bufCr = pfbpool[src_fbid]->addrCr;
	fb[src_fbid].strideY = pfbpool[src_fbid]->strideY;
	fb[src_fbid].strideC = pfbpool[src_fbid]->strideC;
	
	return 0;

err1:
	for (i = 0; i < totalfb; i++) {
		framebuf_free(pfbpool[i]);
	}

	TSK_FREE(fb);
	TSK_FREE(pfbpool);
	return -1;
}

int encoder_readbs_reset_buffer(tdav_codec_fsl_vpu_h264_t* self, PhysicalAddress paBsBufAddr, int bsBufsize, void **out_data, tsk_size_t* out_max_size, tsk_bool_t append, tsk_size_t appendAfter)
{
    //__android_log_print("Teleca", "FSL VPU H264", "encoder_readbs_reset_buffer in\n");
	uint32_t vbuf;

	vbuf = self->encoder.virt_bsbuf_addr + paBsBufAddr - self->encoder.phy_bsbuf_addr;

    if(!(*out_data)) {
        if(append) {
            TSK_DEBUG_ERROR("append mode with *out_data NULL");
            return -1;
        }
        if(!(*out_data = tsk_malloc(bsBufsize))){
			TSK_DEBUG_ERROR("Failed to allocate new buffer");
            return -1;
		}
		*out_max_size = bsBufsize;
    } else if(*out_max_size != bsBufsize) {
        if(!(*out_data = tsk_realloc(*out_data, bsBufsize))){
			TSK_DEBUG_ERROR("Failed to allocate new buffer");
			return -1;
		}
        //__android_log_print("Teleca", "FSL VPU H264", "encoder_readbs_reset_buffer *out_max_size != bsBufsize = %lu\n", bsBufsize);
		*out_max_size = bsBufsize;
    }

    //__android_log_print("Teleca", "FSL VPU H264", "encoder_readbs_reset_buffer finally bsBufsize = %lu\n", bsBufsize);
    //__android_log_print("Teleca", "FSL VPU H264", "encoder_readbs_reset_buffer finally *out_max_size = %lu\n", *out_max_size);

    void *out_data_copy = *out_data;
    if(append) {
        out_data_copy += appendAfter;
    }
    tsk_size_t copySize = bsBufsize;
    if(append) {
        copySize -= appendAfter;
    }

	if(copySize == mem_copy((void *)vbuf, out_data_copy, copySize)) {
        //__android_log_print("Teleca", "FSL VPU H264", "encoder_readbs_reset_buffer out Successful - size = %lu\n", copySize);
        return 0;
    } else {
        //__android_log_print("Teleca", "FSL VPU H264", "encoder_readbs_reset_buffer out NOT success\n");
        return -1;
    }
}

int encode_one_frame(tdav_codec_fsl_vpu_h264_t* self, const void* in_data, tsk_size_t in_size, void **out_data, tsk_size_t* out_max_size)
{
    EncHandle handle = self->encoder.handle;
	EncParam  enc_param = {0};
	EncOutputInfo outinfo = {0};
	RetCode ret = 0;
	int src_fbid = self->encoder.src_fbid, img_size;
	FrameBuffer *fb = self->encoder.fb;
	struct frame_buf **pfbpool = self->encoder.pfbpool;
	struct frame_buf *pfb;
	uint32_t yuv_addr;
	//struct timeval tenc_begin,tenc_end, total_start, total_end;
	int /*sec, usec, */loop_id;
	//float tenc_time = 0, total_time=0;
	
	enc_param.sourceFrame = &(self->encoder.fb[src_fbid]);
	enc_param.quantParam = 23;
	enc_param.forceIPicture = self->encoder.force_idr ? 1:0;
	enc_param.skipPicture = 0;
	enc_param.enableAutoSkip = 1;

	enc_param.encLeftOffset = 0;
	enc_param.encTopOffset = 0;
	
    /*if ((enc_param.encLeftOffset + enc->enc_picwidth) > enc->src_picwidth) {
		err_msg("Configure is failure for width and left offset\n");
		return -1;
	}
	if ((enc_param.encTopOffset + enc->enc_picheight) > enc->src_picheight) {
		err_msg("Configure is failure for height and top offset\n");
		return -1;
	}*/

	img_size = self->encoder.src_picwidth * self->encoder.src_picheight * 3 / 2;
    if(img_size != in_size) {
        //__android_log_print("Teleca", "FSL VPU H264", "encode_one_frame - input size and detected input size do not match!!!\n");
    }
		
	//gettimeofday(&total_start, NULL);

	pfb = pfbpool[src_fbid];
	yuv_addr = pfb->addrY + pfb->desc.virt_uaddr -
			pfb->desc.phy_addr;
	memcpy((void *)yuv_addr, in_data, img_size);

    //__android_log_print("Teleca", "FSL VPU H264", "encode_one_frame - in_data reading done\n");

	//gettimeofday(&tenc_begin, NULL);

    loop_id = 0;
    while (vpu_IsBusy()) {
		vpu_WaitForInt(1);
		if (loop_id == 5000) {
            //__android_log_print("Teleca", "FSL VPU H264", "encode_one_frame - decoder reset!\n");
			ret = vpu_SWReset(self->decoder.handle, 0);
			return -1;
		}
		loop_id ++;
	}
	ret = vpu_EncStartOneFrame(handle, &enc_param);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("vpu_EncStartOneFrame failed Err code:%d\n",ret);
		return ret;
	}

	loop_id = 0;
	while (vpu_IsBusy()) {
		vpu_WaitForInt(1);
		if (loop_id == 5000) {
            //__android_log_print("Teleca", "FSL VPU H264", "encode_one_frame - encoder reset!\n");
			ret = vpu_SWReset(handle, 0);
			return -1;
		}
		loop_id ++;
	}

	/*gettimeofday(&tenc_end, NULL);
	sec = tenc_end.tv_sec - tenc_begin.tv_sec;
	usec = tenc_end.tv_usec - tenc_begin.tv_usec;

	if (usec < 0) {
		sec--;
		usec = usec + 1000000;
	}

	tenc_time += (sec * 1000000) + usec;*/

	ret = vpu_EncGetOutputInfo(handle, &outinfo);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("vpu_EncGetOutputInfo failed Err code: %d\n",ret);
		return -1;
	}

    if (outinfo.skipEncoded) {
        //__android_log_print("Teleca", "FSL VPU H264", "encode_one_frame - Skip encoding one Frame!\n");
		TSK_DEBUG_WARN("Skip encoding one Frame!\n");
    }

	ret = encoder_readbs_reset_buffer(self, outinfo.bitstreamBuffer, outinfo.bitstreamSize, out_data, out_max_size, tsk_false, -1);
	if (ret) {
		TSK_DEBUG_ERROR("writing bitstream buffer failed\n");
		return -1;
	}
	
    return outinfo.bitstreamSize;

	/*gettimeofday(&total_end, NULL);
	sec = total_end.tv_sec - total_start.tv_sec;
	usec = total_end.tv_usec - total_start.tv_usec;
	if (usec < 0) {
		sec--;
		usec = usec + 1000000;
	}
	total_time = (sec * 1000000) + usec;

	info_msg("Finished encoding: %d frames\n", frame_id);
	info_msg("enc fps = %.2f\n", (frame_id / (tenc_time / 1000000)));
	info_msg("total fps= %.2f \n",(frame_id / (total_time / 1000000)));*/
}

void encoder_free_framebuffer(tdav_codec_fsl_vpu_h264_t* self)
{
	int i;

	for (i = 0; i < self->encoder.totalfb; i++) {
		framebuf_free(self->encoder.pfbpool[i]);
	}

	TSK_FREE(self->encoder.fb);
	TSK_FREE(self->encoder.pfbpool);
}

int decoder_allocate_framebuffer(tdav_codec_fsl_vpu_h264_t* self)
{
	DecBufInfo bufinfo;
	int i, regfbcount = self->decoder.regfbcount, totalfb, img_size, mvCol;
    RetCode ret;
	DecHandle handle = self->decoder.handle;
	FrameBuffer *fb;
	struct frame_buf **pfbpool;
	int stride, divX, divY;
	
	totalfb = regfbcount; // + dec->extrafb;

	fb = self->decoder.fb = tsk_calloc(totalfb, sizeof(FrameBuffer));
	if (fb == NULL) {
		TSK_DEBUG_ERROR("Failed to allocate fb\n");
		return -1;
	}

	pfbpool = self->decoder.pfbpool = tsk_calloc(totalfb, sizeof(struct frame_buf *));
	if (pfbpool == NULL) {
		TSK_DEBUG_ERROR("Failed to allocate pfbpool\n");
		TSK_FREE(self->decoder.fb);
		self->decoder.fb = NULL;
		return -1;
	}

	mvCol = 1;

	/* All buffers are linear */
	for (i = 0; i < totalfb; i++) {
        pfbpool[i] = framebuf_alloc(STD_AVC, /*self->decoder.mjpg_fmt*/0,
				    self->decoder.stride, self->decoder.picheight, mvCol);
		if (pfbpool[i] == NULL)
			goto err;
	}

	for (i = 0; i < totalfb; i++) {
		fb[i].myIndex = i;
		fb[i].bufY = pfbpool[i]->addrY;
		fb[i].bufCb = pfbpool[i]->addrCb;
		fb[i].bufCr = pfbpool[i]->addrCr;
		if (!cpu_is_mx27()) {
			fb[i].bufMvCol = pfbpool[i]->mvColBuf;
		}
	}

	stride = ((self->decoder.stride + 15) & ~15);

    bufinfo.avcSliceBufInfo.bufferBase = self->decoder.phy_slice_buf;
	bufinfo.avcSliceBufInfo.bufferSize = self->decoder.phy_slicebuf_size;

	/* User needs to fill max suported macro block value of frame as following*/
	bufinfo.maxDecFrmInfo.maxMbX = self->decoder.stride / 16;
	bufinfo.maxDecFrmInfo.maxMbY = self->decoder.picheight / 16;
	bufinfo.maxDecFrmInfo.maxMbNum = self->decoder.stride * self->decoder.picheight / 256;
	ret = vpu_DecRegisterFrameBuffer(handle, fb, self->decoder.regfbcount, stride, &bufinfo);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("Register frame buffer failed, ret=%d\n", ret);
		goto err1;
	}

	return 0;

err1:

err:
    for (i = 0; i < totalfb; i++) {
	    framebuf_free(pfbpool[i]);
    }

	TSK_FREE(self->decoder.fb);
	TSK_FREE(self->decoder.pfbpool);
	self->decoder.fb = NULL;
	self->decoder.pfbpool = NULL;
	return -1;
}

int decode_one_frame(tdav_codec_fsl_vpu_h264_t* self, void **out_data, tsk_size_t* out_max_size, int mode)
{
    //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - in\n");
    int ret = 0, actual_display_index = -1;

    DecHandle handle = self->decoder.handle;
	DecOutputInfo outinfo = {0};
	DecParam decparam = {0};

    decparam.dispReorderBuf = 0;

    //Not used in IMX6
	/*if (!cpu_is_mx6x()) {
		decparam.prescanEnable = 0;
		decparam.prescanMode = 0;
	}*/

	decparam.skipframeMode = 0;
	decparam.skipframeNum = 0;

	/*
	 * once iframeSearchEnable is enabled, prescanEnable, prescanMode
	 * and skipframeMode options are ignored.
	 */
	decparam.iframeSearchEnable = 0;

    int loop_id = 0;
    while(vpu_IsBusy()) {
        if(loop_id == 5000) {
            //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - encoder reset!\n");
            vpu_SWReset(self->encoder.handle, 0);
            TSK_DEBUG_ERROR("VPU Hanged, SW Reset Command sent\n");
            return -1;
        }
        vpu_WaitForInt(1);
        loop_id++;
    }
    ret = vpu_DecStartOneFrame(handle, &decparam);

    if (ret != RETCODE_SUCCESS) {
        TSK_DEBUG_ERROR("vpu_DecStartOneFrame failed, ret=%d\n", ret);
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - vpu_DecStartOneFrame errored - %d\n", ret);
        return ret;
	}

    loop_id = 0;
    while(vpu_IsBusy()) {
        //TSK_DEBUG_INFO("vpu_IsBusy - %d\n", loop_id);
        if(loop_id == 5000) {
            //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - decoder reset!\n");
            vpu_SWReset(handle, 0);
            TSK_DEBUG_ERROR("VPU Hanged, SW Reset Command sent\n");
            return -1;
        }
        vpu_WaitForInt(1);
        loop_id++;
    }

    ret = vpu_DecGetOutputInfo(handle, &outinfo);

    if (ret != RETCODE_SUCCESS) {
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - vpu_DecGetOutputInfo failed - %d\n", ret);
        TSK_DEBUG_ERROR("vpu_DecGetOutputInfo failed, ret=%d\n", ret);
        return ret;
	}

    if (outinfo.notSufficientPsBuffer) {
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - vpu_DecGetOutputInfo - outinfo.notSufficientPsBuffer\n");
		TSK_DEBUG_ERROR("vpu_DecGetOutputInfo failed PS Buffer overflow\n");
		return -1;
	}

	if (outinfo.notSufficientSliceBuffer) {
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - vpu_DecGetOutputInfo - outinfo.notSufficientSliceBuffer\n");
		TSK_DEBUG_ERROR("vpu_DecGetOutputInfo failed Slice Buffer overflow\n");
		return -1;
	}

    /* BIT don't have picture to be displayed */
	if ((outinfo.indexFrameDisplay == -3) ||
			(outinfo.indexFrameDisplay == -2)) {
        TSK_DEBUG_INFO("decode_one_frame - No Frame to be outputted!!!\n");
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - no Frame to be outputted!!!\n");

        TSK_DEBUG_ERROR("No frame to be outputted\n");

        return -31; //No more frames in the Bit stream buffer
	} else if(outinfo.indexFrameDisplay == -1) {
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - END of Stream!!!\n");
        return -32; //End of stream
    }

    actual_display_index = outinfo.indexFrameDisplay;
    
    //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - outinfo.decodingSuccess = %d, outinfo.decPicWidth = %d, outinfo.decPicHeight = %d\n", outinfo.decodingSuccess, outinfo.decPicWidth, outinfo.decPicHeight);
    //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - *out_data address = %ld, out_max_size = %d\n", *out_data, *out_max_size);
    if(mode != -1) {
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - attempting outputing - actual_display_index = %d\n", actual_display_index);
        write_to_output(self, self->decoder.picCropRect, actual_display_index, out_data, out_max_size);
        //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - display index outputted - %d\n", actual_display_index);
    }

    if (actual_display_index >= 0) {
		ret = vpu_DecClrDispFlag(handle, actual_display_index);
		if (ret) {
			TSK_DEBUG_ERROR("vpu_DecClrDispFlag failed Error code %d\n", ret);
            return ret;
        }
	}

    //__android_log_print("Teleca", "FSL VPU H264", "decode_one_frame - out\n");
    return 0;
}

void decoder_free_framebuffer(tdav_codec_fsl_vpu_h264_t* self)
{
	int i, totalfb;

	totalfb = self->decoder.regfbcount;// + self->decoder.extrafb;

	for (i = 0; i < totalfb; i++) {
		if (self->decoder.pfbpool)
			framebuf_free(self->decoder.pfbpool[i]);
	}

	if (self->decoder.fb) {
		TSK_FREE(self->decoder.fb);
		self->decoder.fb = NULL;
	}
	if (self->decoder.pfbpool) {
		TSK_FREE(self->decoder.pfbpool);
		self->decoder.pfbpool = NULL;
	}

	return;
}

static int tdav_codec_fsl_vpu_h264_open(tmedia_codec_t* self)
{
	int ret;
	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)self;

	if(!h264){
		TSK_DEBUG_ERROR("Invalid parameter");
		return -1;
	}
	
	/* the caller (base class) already checked that the codec is not opened */

	//	Encoder
	if((ret = tdav_codec_fsl_vpu_h264_open_encoder(h264))){
		return ret;
	}

	//	Decoder
	if((ret = tdav_codec_fsl_vpu_h264_open_decoder(h264))){
		return ret;
	}

	return 0;
}

static int tdav_codec_fsl_vpu_h264_close(tmedia_codec_t* self)
{
	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)self;

	if(!h264){
		TSK_DEBUG_ERROR("Invalid parameter");
		return -1;
	}

	/* the caller (base class) alreasy checked that the codec is opened */

	//	Encoder
	tdav_codec_fsl_vpu_h264_close_encoder(h264);

	//	Decoder
	tdav_codec_fsl_vpu_h264_close_decoder(h264);

	return 0;
}

static tsk_size_t tdav_codec_fsl_vpu_h264_encode(tmedia_codec_t* self, const void* in_data, tsk_size_t in_size, void** out_data, tsk_size_t* out_max_size)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode in\n");
	int32_t ret = 0;

	tsk_bool_t send_idr, send_hdr;

	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)self;

	if(!self || !in_data || !in_size){
		TSK_DEBUG_ERROR("Invalid parameter");
		return 0;
	}

	if(!self->opened){
		TSK_DEBUG_ERROR("Codec not opened");
		return 0;
	}


	// send IDR for:
	//	- the first frame
	//  - remote peer requested an IDR
	//	- every second within the first 4seconds

    //TODO: Fix Frame-rate to NOT 30
	send_idr = (
		h264->encoder.frame_count++ == 0
		|| h264 ->encoder.force_idr
		|| ( (h264->encoder.frame_count < /*(int)TMEDIA_CODEC_VIDEO(h264)->out.fps*/30 * 4) && ((h264->encoder.frame_count % /*TMEDIA_CODEC_VIDEO(h264)->out.fps*/30)==0) )
	   );

	// send SPS and PPS headers for:
	//  - IDR frames (not required but it's the easiest way to deal with pakt loss)
	//  - every 5 seconds after the first 4seconds
	send_hdr = (
		send_idr
		|| ( (h264->encoder.frame_count % (/*TMEDIA_CODEC_VIDEO(h264)->out.fps*/30 * 5))==0 )
		);
	if(send_hdr){
        int hdrRet = 0;

        void* header_out_data = NULL;
        tsk_size_t header_out_data_size = 0;

        EncHeaderParam enchdr_param = {0};
        enchdr_param.userProfileLevelEnable = 0;
        enchdr_param.headerType = SPS_RBSP;
	    hdrRet = vpu_EncGiveCommand(h264->encoder.handle, ENC_PUT_AVC_HEADER, &enchdr_param);
        if (hdrRet == RETCODE_SUCCESS) {
	        hdrRet = encoder_readbs_reset_buffer(h264, enchdr_param.buf, enchdr_param.size, &(h264->encoder.header_out_data), &(h264->encoder.header_out_data_size), tsk_false, -1);
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode - encoder_readbs_reset_buffer 1st call - header_out_data = %lu, header_out_data_size = %lu, enchdr_param.size = %lu\n", h264->encoder.header_out_data, h264->encoder.header_out_data_size, enchdr_param.size);
		    if (!hdrRet) {
		        enchdr_param.headerType = PPS_RBSP;
		        hdrRet = vpu_EncGiveCommand(h264->encoder.handle, ENC_PUT_AVC_HEADER, &enchdr_param);
                if (hdrRet == RETCODE_SUCCESS) {
		            hdrRet = encoder_readbs_reset_buffer(h264, enchdr_param.buf, (h264->encoder.header_out_data_size + enchdr_param.size), &(h264->encoder.header_out_data), &(h264->encoder.header_out_data_size), tsk_true, h264->encoder.header_out_data_size);//previous header_out_data_size is sent as appendAfter
                    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode - encoder_readbs_reset_buffer 2nd call - header_out_data = %lu, header_out_data_size = %lu, enchdr_param.size = %lu, header_out_data_size + enchdr_param.size = %lu\n", h264->encoder.header_out_data, h264->encoder.header_out_data_size, enchdr_param.size, (h264->encoder.header_out_data_size + enchdr_param.size));
		            if (!hdrRet) {
                        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode - sending sps/pps header - header_out_data = %lu, header_out_data_size = %lu\n", h264->encoder.header_out_data, h264->encoder.header_out_data_size);
		                tdav_codec_fsl_vpu_h264_encap(h264, h264->encoder.header_out_data, h264->encoder.header_out_data_size);
                        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode - done sending sps/pps header\n");
                    }
                }
            }
        }
	}
	
	// Encode data
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode before encode call - encoder.buffer = %lu\n", h264->encoder.buffer);
    ret = encode_one_frame(h264, in_data, in_size, &(h264->encoder.buffer), &(h264->encoder.buffer_size));
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode after encode call - encoder.buffer = %lu\n", h264->encoder.buffer);

	if(ret > 0){
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode encode_one_frame returned size=%ld, h264->encoder.buffer_size=%lu\n", ret, h264->encoder.buffer_size);
		tdav_codec_fsl_vpu_h264_encap(h264, h264->encoder.buffer, (tsk_size_t)ret);    
	}
    else {
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode encode_one_frame returned 0 size!!!\n");
    }
	h264 ->encoder.force_idr = tsk_false;

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encode out\n");
	return 0;
}

static tsk_size_t tdav_codec_fsl_vpu_h264_decode(tmedia_codec_t* self, const void* in_data, tsk_size_t in_size, void** out_data, tsk_size_t* out_max_size, const tsk_object_t* proto_hdr)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode in\n");
	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)self;
	const trtp_rtp_header_t* rtp_hdr = (const trtp_rtp_header_t*)proto_hdr;
	
	const uint8_t* pay_ptr = tsk_null;
	tsk_size_t pay_size = 0;
	int ret;
	tsk_bool_t append_scp;
	tsk_bool_t sps_or_pps;
	tsk_size_t retsize = 0, size_to_copy = 0;
	static tsk_size_t xmax_size = (1920 * 1080 * 3) >> 3;
	static tsk_size_t start_code_prefix_size = sizeof(H264_START_CODE_PREFIX);

    int eos = 0, fill_end_bs = 0;

	if(!h264 || !in_data || !in_size || !out_data)
	{
		TSK_DEBUG_ERROR("Invalid parameter");
		return 0;
	}
	
	//TSK_DEBUG_INFO("SeqNo=%hu", rtp_hdr->seq_num);

	/* Packet lost? */
	if((h264->decoder.last_seq + 1) != rtp_hdr->seq_num && h264->decoder.last_seq){
		TSK_DEBUG_INFO("Packet lost, seq_num=%d", (h264->decoder.last_seq + 1));
	}
	h264->decoder.last_seq = rtp_hdr->seq_num;


	/* 5.3. NAL Unit Octet Usage
	  +---------------+
      |0|1|2|3|4|5|6|7|
      +-+-+-+-+-+-+-+-+
      |F|NRI|  Type   |
      +---------------+
	*/
	if(*((uint8_t*)in_data) & 0x80){
		TSK_DEBUG_WARN("F=1");
		/* reset accumulator */
		h264->decoder.accumulator = 0;
		return 0;
	}

	/* get payload */
	if((ret = tdav_codec_h264_get_pay(in_data, in_size, (const void**)&pay_ptr, &pay_size, &append_scp)) || !pay_ptr || !pay_size){
		TSK_DEBUG_ERROR("Depayloader failed to get H.264 content");
		return 0;
	}
	//append_scp = tsk_true;
	size_to_copy = pay_size + (append_scp ? start_code_prefix_size : 0);
	// whether it's SPS or PPS (append_scp is false for subsequent FUA chuncks)
	sps_or_pps = append_scp && pay_ptr && ((pay_ptr[0] & 0x1F) == 7 || (pay_ptr[0] & 0x1F) == 8);
	
	// start-accumulator
	if(!h264->decoder.accumulator){
		if(size_to_copy > xmax_size){
			TSK_DEBUG_ERROR("%u too big to contain valid encoded data. xmax_size=%u", size_to_copy, xmax_size);
			return 0;
		}
		if(!(h264->decoder.accumulator = tsk_calloc(size_to_copy, sizeof(uint8_t)))){
			TSK_DEBUG_ERROR("Failed to allocated new buffer");
			return 0;
		}
		h264->decoder.accumulator_size = size_to_copy;
	}
	if((h264->decoder.accumulator_pos + size_to_copy) >= xmax_size){
		TSK_DEBUG_ERROR("BufferOverflow");
		h264->decoder.accumulator_pos = 0;
		return 0;
	}
	if((h264->decoder.accumulator_pos + size_to_copy) > h264->decoder.accumulator_size){
		if(!(h264->decoder.accumulator = tsk_realloc(h264->decoder.accumulator, (h264->decoder.accumulator_pos + size_to_copy)))){
			TSK_DEBUG_ERROR("Failed to reallocated new buffer");
			h264->decoder.accumulator_pos = 0;
			h264->decoder.accumulator_size = 0;
			return 0;
		}
		h264->decoder.accumulator_size = (h264->decoder.accumulator_pos + size_to_copy);
	}

	if(append_scp){
		memcpy(&((uint8_t*)h264->decoder.accumulator)[h264->decoder.accumulator_pos], H264_START_CODE_PREFIX, start_code_prefix_size);
		h264->decoder.accumulator_pos += start_code_prefix_size;
	}
	memcpy(&((uint8_t*)h264->decoder.accumulator)[h264->decoder.accumulator_pos], pay_ptr, pay_size);
	h264->decoder.accumulator_pos += pay_size;
	// end-accumulator

	if(sps_or_pps){
		// http://libav-users.943685.n4.nabble.com/Decode-H264-streams-how-to-fill-AVCodecContext-from-SPS-PPS-td2484472.html
		// SPS and PPS should be bundled with IDR
		TSK_DEBUG_INFO("Receiving SPS or PPS ...to be tied to an IDR");
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode sps/pps found\n");
	}
	else if(rtp_hdr->marker){
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode rtp_hdr marker found\n");

		/* decode the picture */
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode accumulator_pos = %ld\n", h264->decoder.accumulator_pos);
        ret = dec_fill_bsbuffer(h264->decoder.accumulator, h264->decoder.handle, h264->decoder.virt_bsbuf_addr,
		        (h264->decoder.virt_bsbuf_addr + STREAM_BUF_SIZE),
			    h264->decoder.phy_bsbuf_addr, h264->decoder.accumulator_pos, &eos, &fill_end_bs);

        if (ret < 0) {
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode decoder fill bsbuffer failed\n");
	        TSK_DEBUG_ERROR("dec_fill_bsbuffer failed\n");
            h264->decoder.accumulator_pos = 0;
	        return 0;
        }

        //stream not yet parsed
        if(decoder_stream_parsed == tsk_false) {
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode parsing attempt\n");
            ret = dec_parse(h264);
            if(ret) {
                //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode decoder parsing failed\n");
                TSK_DEBUG_ERROR("VPU_Decoder Parsing error\n");
                h264->decoder.accumulator_pos = 0;
                return 0;
            }

            /* allocate frame buffers */
	        ret = decoder_allocate_framebuffer(h264);
	        if (ret) {
                //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode decoder FB allocation failed\n");
                TSK_DEBUG_ERROR("VPU_Decoder Framebuffer Allocation error\n");
                h264->decoder.accumulator_pos = 0;
		        return 0;
            }

            decoder_stream_parsed = tsk_true;
            //frameCount = 1;
        }

        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode before decode call - out_data = %lu, *out_data = %lu\n", out_data, *out_data);
        ret = decode_one_frame(h264, out_data, out_max_size, 0);//out_max_size might be changed by decode_one_frame()
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode after decode call - out_data = %lu, *out_data = %lu\n", out_data, *out_data);

		if (ret /*&& ret != -31*/) { //-31 means no frame to be outputted
			TSK_DEBUG_INFO("Failed to decode the buffer with error code =%d, size=%u, append=%s", ret, h264->decoder.accumulator_pos, append_scp ? "yes" : "no");
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode error = %d, append_scp = %s\n", ret, append_scp ? "yes" : "no");
			if(TMEDIA_CODEC_VIDEO(self)->in.callback){
				TMEDIA_CODEC_VIDEO(self)->in.result.type = tmedia_video_decode_result_type_error;
				TMEDIA_CODEC_VIDEO(self)->in.result.proto_hdr = proto_hdr;
				TMEDIA_CODEC_VIDEO(self)->in.callback(&TMEDIA_CODEC_VIDEO(self)->in.result);
			}
		}
		else {
			//tsk_size_t xsize;
			
			/* IDR ? */
			if(((pay_ptr[0] & 0x1F) == 0x05) && TMEDIA_CODEC_VIDEO(self)->in.callback){
                //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode IDR decoded\n");
				TSK_DEBUG_INFO("Decoded H.264 IDR");
				TMEDIA_CODEC_VIDEO(self)->in.result.type = tmedia_video_decode_result_type_idr;
				TMEDIA_CODEC_VIDEO(self)->in.result.proto_hdr = proto_hdr;
				TMEDIA_CODEC_VIDEO(self)->in.callback(&TMEDIA_CODEC_VIDEO(self)->in.result);
			}
            retsize = *out_max_size;

			TMEDIA_CODEC_VIDEO(h264)->in.width = TMEDIA_CODEC_VIDEO(self)->out.width;
			TMEDIA_CODEC_VIDEO(h264)->in.height = TMEDIA_CODEC_VIDEO(self)->out.height;
		}
        /* in all cases: reset accumulator */
		h264->decoder.accumulator_pos = 0;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_decode out - return size = %lu\n", retsize);
	return retsize;
}

static tsk_bool_t tdav_codec_fsl_vpu_h264_sdp_att_match(const tmedia_codec_t* codec, const char* att_name, const char* att_value)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_sdp_att_match in\n");
	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)codec;
	tsk_bool_t ret = tsk_true;

	if(!h264){
		TSK_DEBUG_ERROR("Invalid parameter");
		return tsk_false;
	}

	TSK_DEBUG_INFO("Trying to match [%s:%s]", att_name, att_value);

	if(tsk_striequals(att_name, "fmtp")){
		int val_int;
		profile_idc_t profile;
		level_idc_t level;
		tsk_params_L_t* params;

		/* Check whether the profile match (If the profile is missing, then we consider that it's ok) */
		if(tdav_codec_h264_get_profile_and_level(att_value, &profile, &level) != 0){
			TSK_DEBUG_ERROR("Not valid profile-level: %s", att_value);
			return tsk_false;
		}
		if(TDAV_CODEC_H264_COMMON(codec)->profile != profile){
			return tsk_false;
		}
		else{
			if(TDAV_CODEC_H264_COMMON(codec)->level != level){
				unsigned width, height;
				TDAV_CODEC_H264_COMMON(codec)->level = TSK_MIN(TDAV_CODEC_H264_COMMON(codec)->level, level);
				if(tdav_codec_h264_common_size_from_level(TDAV_CODEC_H264_COMMON(codec)->level, &width, &height) != 0){
					return tsk_false;
				}
				TMEDIA_CODEC_VIDEO(codec)->in.width = TMEDIA_CODEC_VIDEO(codec)->out.width = width;
				TMEDIA_CODEC_VIDEO(codec)->in.height = TMEDIA_CODEC_VIDEO(codec)->out.height = height;
			}
		}

		/* e.g. profile-level-id=42e00a; packetization-mode=1; max-br=452; max-mbps=11880 */
		if((params = tsk_params_fromstring(att_value, ";", tsk_true))){
			
			/* === max-br ===*/
			if((val_int = tsk_params_get_param_value_as_int(params, "max-br")) != -1){
				// should compare "max-br"?
				TMEDIA_CODEC_VIDEO(h264)->out.max_br = val_int*1000;
			}

			/* === max-mbps ===*/
			if((val_int = tsk_params_get_param_value_as_int(params, "max-mbps")) != -1){
				// should compare "max-mbps"?
				TMEDIA_CODEC_VIDEO(h264)->out.max_mbps = val_int*1000;
			}

			/* === packetization-mode ===*/
			if((val_int = tsk_params_get_param_value_as_int(params, "packetization-mode")) != -1){
				if((packetization_mode_t)val_int == Single_NAL_Unit_Mode || (packetization_mode_t)val_int == Non_Interleaved_Mode){
					TDAV_CODEC_H264_COMMON(h264)->pack_mode = (packetization_mode_t)val_int;
				}
				else{
					TSK_DEBUG_INFO("packetization-mode not matching");
					ret = tsk_false;
					goto bail;
				}
			}
		}
bail:
		TSK_OBJECT_SAFE_FREE(params);
	}
	else if(tsk_striequals(att_name, "imageattr")){
		unsigned in_width, in_height, out_width, out_height;
		unsigned width, height;
		tsk_size_t s;
		if(tmedia_parse_video_imageattr(att_value, TMEDIA_CODEC_VIDEO(codec)->pref_size, &in_width, &in_height, &out_width, &out_height) != 0){
			return tsk_false;
		}
		// check that 'imageattr' is comform to H.264 'profile-level'
		if(tdav_codec_h264_common_size_from_level(TDAV_CODEC_H264_COMMON(codec)->level, &width, &height) != 0){
			return tsk_false;
		}
		if((s = ((width * height * 3) >> 1)) < ((in_width * in_height * 3) >> 1) || s < ((out_width * out_height * 3) >> 1)){
			return tsk_false;
		}

		TMEDIA_CODEC_VIDEO(codec)->in.width = in_width;
		TMEDIA_CODEC_VIDEO(codec)->in.height = in_height;
		TMEDIA_CODEC_VIDEO(codec)->out.width = out_width;
		TMEDIA_CODEC_VIDEO(codec)->out.height = out_height;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_sdp_att_match out\n");
	return ret;
}

static char* tdav_codec_fsl_vpu_h264_sdp_att_get(const tmedia_codec_t* self, const char* att_name)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_sdp_att_get in\n");
	tdav_codec_fsl_vpu_h264_t* h264 = (tdav_codec_fsl_vpu_h264_t*)self;
	
	if(!h264 || !att_name){
		TSK_DEBUG_ERROR("Invalid parameter");
		return tsk_null;
	}

	if(tsk_striequals(att_name, "fmtp")){
		char* fmtp = tsk_null;
#if 1
		tsk_sprintf(&fmtp, "profile-level-id=%x; packetization-mode=%d", ((TDAV_CODEC_H264_COMMON(h264)->profile << 16) | TDAV_CODEC_H264_COMMON(h264)->level), TDAV_CODEC_H264_COMMON(h264)->pack_mode);
#else
		tsk_strcat_2(&fmtp, "profile-level-id=%s; packetization-mode=%d; max-br=%d; max-mbps=%d",
			profile_level, TDAV_CODEC_H264_COMMON(h264)->pack_mode, TMEDIA_CODEC_VIDEO(h264)->in.max_br/1000, TMEDIA_CODEC_VIDEO(h264)->in.max_mbps/1000);
#endif
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_sdp_att_get out - return fmtp\n");
		return fmtp;
	}
	else if(tsk_striequals(att_name, "imageattr")){
		return tmedia_get_video_imageattr(TMEDIA_CODEC_VIDEO(self)->pref_size, 
			TMEDIA_CODEC_VIDEO(self)->in.width, TMEDIA_CODEC_VIDEO(self)->in.height, TMEDIA_CODEC_VIDEO(self)->out.width, TMEDIA_CODEC_VIDEO(self)->out.height);
	}
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_sdp_att_get out\n");
	return tsk_null;
}




/* ============ H.264 Base Profile Plugin interface ================= */

/* constructor */
static tsk_object_t* tdav_codec_fsl_vpu_h264_base_ctor(tsk_object_t * self, va_list * app)
{
	tdav_codec_fsl_vpu_h264_t *h264 = (tdav_codec_fsl_vpu_h264_t*)self;
	if(h264){
		/* init base: called by tmedia_codec_create() */
		/* init self */
		if(tdav_codec_fsl_vpu_h264_init(h264, profile_idc_baseline) != 0){
			return tsk_null;
		}
	}
	return self;
}
/* destructor */
static tsk_object_t* tdav_codec_fsl_vpu_h264_base_dtor(tsk_object_t * self)
{ 
	tdav_codec_fsl_vpu_h264_t *h264 = (tdav_codec_fsl_vpu_h264_t*)self;
	if(h264){
		/* deinit base */
		tdav_codec_h264_common_deinit(self);
		/* deinit self */
		tdav_codec_fsl_vpu_h264_deinit(h264);
		
	}

	return self;
}
/* object definition */
static const tsk_object_def_t tdav_codec_fsl_vpu_h264_base_def_s = 
{
	sizeof(tdav_codec_fsl_vpu_h264_t),
	tdav_codec_fsl_vpu_h264_base_ctor, 
	tdav_codec_fsl_vpu_h264_base_dtor,
	tmedia_codec_cmp, 
};
/* plugin definition*/
static const tmedia_codec_plugin_def_t tdav_codec_fsl_vpu_h264_base_plugin_def_s = 
{
	&tdav_codec_fsl_vpu_h264_base_def_s,

	tmedia_video,
	tmedia_codec_id_h264_bp,
	"H264",
	"H264 Base Profile",
	TMEDIA_CODEC_FORMAT_H264_BP,
	tsk_true,
	90000, // rate
	
	/* audio */
	{ 0 },

	/* video */
	{176, 144, 15},

	tdav_codec_fsl_vpu_h264_set,
	tdav_codec_fsl_vpu_h264_open,
	tdav_codec_fsl_vpu_h264_close,
	tdav_codec_fsl_vpu_h264_encode,
	tdav_codec_fsl_vpu_h264_decode,
	tdav_codec_fsl_vpu_h264_sdp_att_match,
	tdav_codec_fsl_vpu_h264_sdp_att_get
};
const tmedia_codec_plugin_def_t *tdav_codec_fsl_vpu_h264_base_plugin_def_t = &tdav_codec_fsl_vpu_h264_base_plugin_def_s;

/* ============ H.264 Main Profile Plugin interface ================= */

/* constructor */
static tsk_object_t* tdav_codec_fsl_vpu_h264_main_ctor(tsk_object_t * self, va_list * app)
{
	tdav_codec_fsl_vpu_h264_t *h264 = self;
	if(h264){
		/* init base: called by tmedia_codec_create() */
		/* init self */
		if(tdav_codec_fsl_vpu_h264_init(h264, profile_idc_main) != 0){
			return tsk_null;
		}
	}
	return self;
}
/* destructor */
static tsk_object_t* tdav_codec_fsl_vpu_h264_main_dtor(tsk_object_t * self)
{ 
	tdav_codec_fsl_vpu_h264_t *h264 = (tdav_codec_fsl_vpu_h264_t*)self;
	if(h264){
		/* deinit base */
		tdav_codec_h264_common_deinit(self);
		/* deinit self */
		tdav_codec_fsl_vpu_h264_deinit(h264);
		
	}

	return self;
}
/* object definition */
static const tsk_object_def_t tdav_codec_fsl_vpu_h264_main_def_s = 
{
	sizeof(tdav_codec_fsl_vpu_h264_t),
	tdav_codec_fsl_vpu_h264_main_ctor, 
	tdav_codec_fsl_vpu_h264_main_dtor,
	tmedia_codec_cmp, 
};
/* plugin definition*/
static const tmedia_codec_plugin_def_t tdav_codec_fsl_vpu_h264_main_plugin_def_s = 
{
	&tdav_codec_fsl_vpu_h264_main_def_s,

	tmedia_video,
	tmedia_codec_id_h264_mp,
	"H264",
	"H264 Main Profile",
	TMEDIA_CODEC_FORMAT_H264_MP,
	tsk_true,
	90000, // rate
	
	/* audio */
	{ 0 },

	/* video */
	{176, 144, 15},

	tdav_codec_fsl_vpu_h264_set,
	tdav_codec_fsl_vpu_h264_open,
	tdav_codec_fsl_vpu_h264_close,
	tdav_codec_fsl_vpu_h264_encode,
	tdav_codec_fsl_vpu_h264_decode,
	tdav_codec_fsl_vpu_h264_sdp_att_match,
	tdav_codec_fsl_vpu_h264_sdp_att_get
};
const tmedia_codec_plugin_def_t *tdav_codec_fsl_vpu_h264_main_plugin_def_t = &tdav_codec_fsl_vpu_h264_main_plugin_def_s;





































/* ============ Common To all H264 codecs ================= */

int tdav_codec_fsl_vpu_h264_open_encoder(tdav_codec_fsl_vpu_h264_t* self)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder in\n");
	int ret;

    //encoder buffer to be initailised when actual encoded data size is known
    self->encoder.buffer = NULL;
    self->encoder.buffer_size = 0;

    //encoder header buffer
    self->encoder.header_out_data = NULL;
    self->encoder.header_out_data_size = 0;

    //self->encoder.mem_desc = {0};
    self->encoder.mem_desc = (vpu_mem_desc *) tsk_malloc(sizeof(vpu_mem_desc));

    EncHandle handle = {0};
	EncOpenParam encop = {0};

    /* get physical contigous bit stream buffer */
	self->encoder.mem_desc->size = STREAM_BUF_SIZE;
	ret = IOGetPhyMem((self->encoder.mem_desc));
	if (ret) {
		TSK_DEBUG_ERROR("VPU - Unable to obtain physical memory\n");
		return -1;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - 1\n");

	/* mmap that physical buffer */
	self->encoder.virt_bsbuf_addr = IOGetVirtMem((self->encoder.mem_desc));
	if (self->encoder.virt_bsbuf_addr <= 0) {
		TSK_DEBUG_ERROR("VPU - Unable to map physical memory\n");
		IOFreePhyMem((self->encoder.mem_desc));
		return -1;
	}

    self->encoder.phy_bsbuf_addr = self->encoder.mem_desc->phy_addr;

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - 2\n");

    /* Fill up parameters for encoding */
	encop.bitstreamBuffer = self->encoder.phy_bsbuf_addr;
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - encop.bitstreamBuffer=%ld\n", encop.bitstreamBuffer);
	encop.bitstreamBufferSize = STREAM_BUF_SIZE;
	encop.bitstreamFormat = STD_AVC;
	encop.mapType = LINEAR_FRAME_MAP;
	encop.linear2TiledEnable = 0;
	
    self->encoder.src_picwidth = TMEDIA_CODEC_VIDEO(self)->out.width;
	self->encoder.src_picheight = TMEDIA_CODEC_VIDEO(self)->out.height;
	
	self->encoder.enc_picwidth = TMEDIA_CODEC_VIDEO(self)->out.width;
	self->encoder.enc_picheight = TMEDIA_CODEC_VIDEO(self)->out.height;
	
	encop.picWidth = TMEDIA_CODEC_VIDEO(self)->out.width;
	encop.picHeight = TMEDIA_CODEC_VIDEO(self)->out.height;
	//__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - encop.picWidth=%ld & encop.picHeight=%ld\n", encop.picWidth, encop.picHeight);

	/*uint32_t num = CLOCKTICKSPERSECOND;//Calculating based on the 1 second = 10,000 ticks presumption. 
    uint32_t den = (uint32_t)(((float)num / (float)(TMEDIA_CODEC_VIDEO(self)->out.fps)) - 1);
    uint32_t frameRateInfo = den << 16;
    frameRateInfo = frameRateInfo | num;
	encop.frameRateInfo = frameRateInfo;
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - encop.frameRateInfo=%ld\n", encop.frameRateInfo);
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - out.fps=%ld\n", TMEDIA_CODEC_VIDEO(self)->out.fps);
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - calculated fps=%ld\n", (frameRateInfo & 0xffff) /((frameRateInfo >> 16) + 1));*/

    encop.frameRateInfo = 30;

	encop.bitRate = 0;
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - encop.bitRate=%ld\n", encop.bitRate);
	encop.gopSize = 15;
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder - encop.gopSize=%ld\n", encop.gopSize);
	encop.slicemode.sliceMode = 0;	/* 0: 1 slice per picture; 1: Multiple slices per picture */
	encop.slicemode.sliceSizeMode = 0; /* 0: silceSize defined by bits; 1: sliceSize defined by MB number*/
	encop.slicemode.sliceSize = 4000;  /* Size of a slice in bits or MB numbers */

	encop.initialDelay = 0;
	encop.vbvBufferSize = 0;        /* 0 = ignore 8 */
	encop.intraRefresh = 0;
	encop.sliceReport = 0;
	encop.mbReport = 0;
	encop.mbQpReport = 0;
	encop.rcIntraQp = 5;
	encop.userQpMax = 2;
	encop.userQpMin = 10;
	encop.userQpMinEnable = 0;
	encop.userQpMaxEnable = 0;

	encop.IntraCostWeight = 0;
	encop.MEUseZeroPmv  = 0;
	/* (3: 16x16, 2:32x16, 1:64x32, 0:128x64, H.263(Short Header : always 3) */
	encop.MESearchRange = 3;

	encop.userGamma = (Uint32)(0.75*32768);         /*  (0*32768 <= gamma <= 1*32768) */
	encop.RcIntervalMode= 1;        /* 0:normal, 1:frame_level, 2:slice_level, 3: user defined Mb_level */
	encop.MbInterval = 0;
	encop.avcIntra16x16OnlyModeEnable = 0;

	encop.ringBufferEnable = 0;
	encop.dynamicAllocEnable = 0;
	encop.chromaInterleave = 0;

    encop.EncStdParam.avcParam.avc_constrainedIntraPredFlag = 0;
	encop.EncStdParam.avcParam.avc_disableDeblk = 1;
	encop.EncStdParam.avcParam.avc_deblkFilterOffsetAlpha = 6;
	encop.EncStdParam.avcParam.avc_deblkFilterOffsetBeta = 0;
	encop.EncStdParam.avcParam.avc_chromaQpOffset = 10;
	encop.EncStdParam.avcParam.avc_audEnable = 0;

    encop.EncStdParam.avcParam.interview_en = 0;
	encop.EncStdParam.avcParam.paraset_refresh_en = 0;
	encop.EncStdParam.avcParam.prefix_nal_en = 0;
	encop.EncStdParam.avcParam.mvc_extension = 0;
	encop.EncStdParam.avcParam.avc_frameCroppingFlag = 0;
	encop.EncStdParam.avcParam.avc_frameCropLeft = 0;
	encop.EncStdParam.avcParam.avc_frameCropRight = 0;
	encop.EncStdParam.avcParam.avc_frameCropTop = 0;
	encop.EncStdParam.avcParam.avc_frameCropBottom = 0;

    ret = vpu_EncOpen(&handle, &encop);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("Encoder open failed %d\n", ret);

        IOFreeVirtMem((self->encoder.mem_desc));
    	IOFreePhyMem((self->encoder.mem_desc));
        //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder vpu_EncOpen - %d\n", ret);
		return -1;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder  - 3\n");

	self->encoder.handle = handle;

    EncInitialInfo initinfo = {0};
	
	ret = vpu_EncGetInitialInfo(handle, &initinfo);
	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("Encoder GetInitialInfo failed: %d\n", ret);

        IOFreeVirtMem((self->encoder.mem_desc));
    	IOFreePhyMem((self->encoder.mem_desc));
		return -1;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder  - 4\n");

	self->encoder.minFrameBufferCount = initinfo.minFrameBufferCount;

    ret = encoder_allocate_framebuffer(self);
    if (ret) {
		TSK_DEBUG_ERROR("encoder_allocate_framebuffer failed\n");

        IOFreeVirtMem((self->encoder.mem_desc));
    	IOFreePhyMem((self->encoder.mem_desc));
		return -1;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_encoder out\n");

	return ret;
}

int tdav_codec_fsl_vpu_h264_close_encoder(tdav_codec_fsl_vpu_h264_t* self)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_close_encoder in\n");

    if(self->encoder.header_out_data) {
        TSK_FREE(self->encoder.header_out_data);
    }
	if(self->encoder.buffer) {
		TSK_FREE(self->encoder.buffer);
	}
	self->encoder.frame_count = 0;

    encoder_free_framebuffer(self);

    int ret = 0;

	ret = vpu_EncClose(self->encoder.handle);
	if (ret == RETCODE_FRAME_NOT_COMPLETE) {
		vpu_SWReset(self->encoder.handle, 0);
		ret = vpu_EncClose(self->encoder.handle);
        if (ret != RETCODE_SUCCESS)
			TSK_DEBUG_ERROR("vpu_EncClose failed\n");
	}

    IOFreeVirtMem((self->encoder.mem_desc));
	IOFreePhyMem((self->encoder.mem_desc));
    
    TSK_FREE((self->encoder.mem_desc));

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_close_encoder out\n");

	return ret;
}

int tdav_codec_fsl_vpu_h264_open_decoder(tdav_codec_fsl_vpu_h264_t* self)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_decoder in\n");
	int ret;

    self->decoder.mem_desc = (vpu_mem_desc *) tsk_malloc(sizeof(vpu_mem_desc));
    self->decoder.ps_mem_desc = (vpu_mem_desc *) tsk_malloc(sizeof(vpu_mem_desc));
    self->decoder.slice_mem_desc = (vpu_mem_desc *) tsk_malloc(sizeof(vpu_mem_desc));

    DecHandle handle = {0};
	DecOpenParam oparam = {0};

    self->decoder.mem_desc->size = STREAM_BUF_SIZE;
	ret = IOGetPhyMem((self->decoder.mem_desc));
	if (ret) {
		TSK_DEBUG_ERROR("VPU - Unable to obtain physical mem\n");
		return -1;
	}

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_decoder - 1\n");

	if (IOGetVirtMem((self->decoder.mem_desc)) <= 0) {
		TSK_DEBUG_ERROR("VPU - Unable to obtain virtual mem\n");
		IOFreePhyMem((self->decoder.mem_desc));
		return -1;
	}

    self->decoder.ps_mem_desc->size = PS_SAVE_SIZE;
	ret = IOGetPhyMem((self->decoder.ps_mem_desc));
	if (ret) {
		TSK_DEBUG_ERROR("Unable to obtain physical ps save mem\n");
        IOFreeVirtMem((self->decoder.mem_desc));
        IOFreePhyMem((self->decoder.mem_desc));
		return -1;
	}
	
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_decoder - 2\n");

    self->decoder.phy_bsbuf_addr = self->decoder.mem_desc->phy_addr;
	self->decoder.virt_bsbuf_addr = self->decoder.mem_desc->virt_uaddr;
    self->decoder.phy_ps_buf = self->decoder.ps_mem_desc->phy_addr;
    
	oparam.bitstreamFormat = STD_AVC;
	oparam.bitstreamBuffer = self->decoder.phy_bsbuf_addr;
	oparam.bitstreamBufferSize = STREAM_BUF_SIZE;
	oparam.pBitStream = self->decoder.virt_bsbuf_addr;
	oparam.reorderEnable = 1;
    oparam.chromaInterleave = 1;
    oparam.mp4DeblkEnable = 0;
	oparam.mjpg_thumbNailDecEnable = 0;
    oparam.avcExtension = 0;
    oparam.mp4Class = 0;
    oparam.mapType = LINEAR_FRAME_MAP;
	oparam.tiled2LinearEnable = 0;
    oparam.bitstreamMode = 1;
    oparam.psSaveBuffer = self->decoder.phy_ps_buf;
	oparam.psSaveBufferSize = PS_SAVE_SIZE;
    oparam.picWidth = TMEDIA_CODEC_VIDEO(self)->in.width;
	oparam.picHeight = TMEDIA_CODEC_VIDEO(self)->in.height;

	ret = vpu_DecOpen(&handle, &oparam);

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_decoder vpu_DecOpen return = %d\n", ret);

	if (ret != RETCODE_SUCCESS) {
		TSK_DEBUG_ERROR("VPU - Decoder Open Failure\n");
        IOFreePhyMem((self->decoder.ps_mem_desc));
        IOFreeVirtMem((self->decoder.mem_desc));
    	IOFreePhyMem((self->decoder.mem_desc));

        return -1;
	}
    else {
	    self->decoder.handle = handle;
    }

    decoder_stream_parsed = tsk_false;

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_open_decoder out\n");
	return ret;
}

int tdav_codec_fsl_vpu_h264_close_decoder(tdav_codec_fsl_vpu_h264_t* self)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_close_decoder in\n");

	TSK_FREE(self->decoder.accumulator);
	self->decoder.accumulator_pos = 0;

    int ret = 0;

    ret = vpu_DecClose(self->decoder.handle);
	if (ret == RETCODE_FRAME_NOT_COMPLETE) {
		vpu_SWReset(self->decoder.handle, 0);
		ret = vpu_DecClose(self->decoder.handle);
		if (ret != RETCODE_SUCCESS)
			TSK_DEBUG_ERROR("vpu_DecClose failed\n");
	}

    decoder_free_framebuffer(self);

    IOFreePhyMem((self->decoder.slice_mem_desc));
    IOFreePhyMem((self->decoder.ps_mem_desc));
    IOFreeVirtMem((self->decoder.mem_desc));
    IOFreePhyMem((self->decoder.mem_desc));

    TSK_FREE((self->decoder.slice_mem_desc));
    TSK_FREE((self->decoder.ps_mem_desc));
    TSK_FREE((self->decoder.mem_desc));

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_close_decoder out\n");
	return 0;
}

int tdav_codec_fsl_vpu_h264_init(tdav_codec_fsl_vpu_h264_t* self, profile_idc_t profile)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_init in\n");
	int ret = 0;
	level_idc_t level;

	if(!self){
		TSK_DEBUG_ERROR("Invalid parameter");
		return -1;
	}

	if((ret = tdav_codec_h264_common_init(TDAV_CODEC_H264_COMMON(self)))){
		TSK_DEBUG_ERROR("tdav_codec_h264_common_init() faile with error code=%d", ret);
		return ret;
	}

	if((ret = tdav_codec_h264_common_level_from_size(TMEDIA_CODEC_VIDEO(self)->out.width, TMEDIA_CODEC_VIDEO(self)->out.height, &level))){
		TSK_DEBUG_ERROR("Failed to find level for size=[%u, %u]", TMEDIA_CODEC_VIDEO(self)->out.width, TMEDIA_CODEC_VIDEO(self)->out.height);
		return ret;
	}
	
	TDAV_CODEC_H264_COMMON(self)->pack_mode = H264_PACKETIZATION_MODE;
	TDAV_CODEC_H264_COMMON(self)->profile = profile;
	TDAV_CODEC_H264_COMMON(self)->level = level;
	TMEDIA_CODEC_VIDEO(self)->in.max_mbps = TMEDIA_CODEC_VIDEO(self)->out.max_mbps = H264_MAX_MBPS*1000;
	TMEDIA_CODEC_VIDEO(self)->in.max_br = TMEDIA_CODEC_VIDEO(self)->out.max_br = H264_MAX_BR*1000;

    framebuf_init();

    //__android_log_print("Teleca", "FSL VPU H264", "Teleca tdav_codec_fsl_vpu_h264_init before vpu init\n");

    ret = vpu_Init(NULL);

    //__android_log_print("Teleca", "FSL VPU H264", "Teleca tdav_codec_fsl_vpu_h264_init after vpu init = %d\n", ret);

    if (ret) {
		TSK_DEBUG_ERROR("VPU Init Failure.\n");
	}

	/* allocations MUST be done by open() */

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_init out\n");
	return ret;
}

int tdav_codec_fsl_vpu_h264_deinit(tdav_codec_fsl_vpu_h264_t* self)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_deinit in\n");
	if(!self){
		TSK_DEBUG_ERROR("Invalid parameter");
		return -1;
	}

    vpu_UnInit();

    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_deinit out\n");
	return 0;
}

static void tdav_codec_fsl_vpu_h264_encap(const tdav_codec_fsl_vpu_h264_t* h264, const uint8_t* pdata, tsk_size_t size)
{
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encap in\n");
	static const tsk_size_t size_of_scp = sizeof(H264_START_CODE_PREFIX); /* we know it's equal to 4 .. */
	register tsk_size_t i;
	tsk_size_t last_scp, prev_scp;
	tsk_size_t _size;

	if(!pdata || size < size_of_scp){
        if(!pdata) {
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encap - !pdata - returning empty\n");
        } else if (size < size_of_scp) {
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encap - size < size_of_scp - returning empty\n");
        }
		return;
	}

	if(pdata[0] == 0 && pdata[1] == 0){
		if(pdata[2] == 1){
			pdata += 3, size -= 3;
		}
		else if(pdata[2] == 0 && pdata[3] == 1){
			pdata += 4, size -= 4;
		}
	}

	_size = (size - size_of_scp);
	last_scp = 0, prev_scp = 0;
	for(i = size_of_scp; i<_size; i++){
		if(pdata[i] == 0 && pdata[i+1] == 0 && (pdata[i+2] == 1 || (pdata[i+2] == 0 && pdata[i+3] == 1))){  /* Find Start Code Prefix */
			prev_scp = last_scp;
			if((i - last_scp) >= H264_RTP_PAYLOAD_SIZE || 1){
                //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encap attempted rtp send - 1\n");
				tdav_codec_h264_rtp_callback(TDAV_CODEC_H264_COMMON(h264), pdata + prev_scp,
					(i - prev_scp), (prev_scp == size));
			}
			last_scp = i;
			i += (pdata[i+2] == 1) ? 3 : 4;
		}
	}

	if(last_scp < (int32_t)size){
            //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encap attempted rtp send - 2\n");
			tdav_codec_h264_rtp_callback(TDAV_CODEC_H264_COMMON(h264), pdata + last_scp,
				(size - last_scp), tsk_true);
	}
    //__android_log_print("Teleca", "FSL VPU H264", "tdav_codec_fsl_vpu_h264_encap out\n");
}

