MPEG4 Frame Construction( Identify the Frame Size)

I have to construct the Frames from the Bytes.
I need to develop the class for this one. and I need to test it with MPEG4 Decoder SDK in MPeg4DecoderExe;
Algorithm For constructing frame from bytes :
-----------------------------------------------
1.Pass Bytes to the algorithm with size. Add these Bytes to the custom Buffer.
2.Check For the StartCode 00 00 01 B6 and count the number of Bytes and
check for the beginning of a new frame.
3.if we found a frame, copy and send it to the MPEG4FeedData() and removed the frame bytes from the custom Buffer.
4.Call the PullData() fn...