1/*--------------------------------------------------------------------------
2Copyright (c) 2010 - 2013, The Linux Foundation. All rights reserved.
3
4Redistribution and use in source and binary forms, with or without
5modification, are permitted provided that the following conditions are met:
6    * Redistributions of source code must retain the above copyright
7      notice, this list of conditions and the following disclaimer.
8    * Redistributions in binary form must reproduce the above copyright
9      notice, this list of conditions and the following disclaimer in the
10      documentation and/or other materials provided with the distribution.
11    * Neither the name of The Linux Foundation nor
12      the names of its contributors may be used to endorse or promote
13      products derived from this software without specific prior written
14      permission.
15
16THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19NON-INFRINGEMENT ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
20CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
21EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
22PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
23OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
24WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
25OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
26ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27--------------------------------------------------------------------------*/
28#include <stdio.h>
29#include <stddef.h>
30#include <stdlib.h>
31#include <fcntl.h>
32#include <stdarg.h>
33#include <string.h>
34#include <errno.h>
35#include <unistd.h>
36#include <pthread.h>
37#include <ctype.h>
38#include <sys/stat.h>
39#include <sys/ioctl.h>
40#include <sys/mman.h>
41#include <sys/time.h>
42#include <sys/poll.h>
43#include <stdint.h>
44
45#include "frameparser.h"
46#include "vidc_debug.h"
47
48#ifdef _ANDROID_
49extern "C" {
50#include<utils/Log.h>
51}
52#endif//_ANDROID_
53
54static unsigned char H264_mask_code[4] = {0xFF,0xFF,0xFF,0xFF};
55static unsigned char H264_start_code[4] = {0x00,0x00,0x00,0x01};
56
57static unsigned char MPEG4_start_code[4] = {0x00,0x00,0x01,0xB6};
58static unsigned char MPEG4_mask_code[4] = {0xFF,0xFF,0xFF,0xFF};
59
60static unsigned char H263_start_code[4] = {0x00,0x00,0x80,0x00};
61static unsigned char H263_mask_code[4] = {0xFF,0xFF,0xFC,0x00};
62
63static unsigned char VC1_AP_start_code[4] = {0x00,0x00,0x01,0x0C};
64static unsigned char VC1_AP_mask_code[4] = {0xFF,0xFF,0xFF,0xFC};
65
66static unsigned char MPEG2_start_code[4] = {0x00, 0x00, 0x01, 0x00};
67static unsigned char MPEG2_mask_code[4] = {0xFF, 0xFF, 0xFF, 0xFF};
68
69frame_parse::frame_parse():mutils(NULL),
70    parse_state(A0),
71    start_code(NULL),
72    mask_code(NULL),
73    last_byte_h263(0),
74    last_byte(0),
75    header_found(false),
76    skip_frame_boundary(false),
77    state_nal(NAL_LENGTH_ACC),
78    nal_length(0),
79    accum_length(0),
80    bytes_tobeparsed(0)
81{
82}
83
84frame_parse::~frame_parse ()
85{
86    if (mutils)
87        delete mutils;
88
89    mutils = NULL;
90}
91
92int frame_parse::init_start_codes (codec_type codec_type_parse)
93{
94    /*Check if Codec Type is proper and we are in proper state*/
95    if (codec_type_parse > CODEC_TYPE_MAX || parse_state != A0) {
96        return -1;
97    }
98
99    switch (codec_type_parse) {
100        case CODEC_TYPE_MPEG4:
101            start_code = MPEG4_start_code;
102            mask_code = MPEG4_mask_code;
103            break;
104        case CODEC_TYPE_H263:
105            start_code = H263_start_code;
106            mask_code = H263_mask_code;
107            break;
108        case CODEC_TYPE_H264:
109        case CODEC_TYPE_HEVC:
110            start_code = H264_start_code;
111            mask_code = H264_mask_code;
112            break;
113        case CODEC_TYPE_VC1:
114            start_code = VC1_AP_start_code;
115            mask_code = VC1_AP_mask_code;
116            break;
117        case CODEC_TYPE_MPEG2:
118            start_code = MPEG2_start_code;
119            mask_code = MPEG2_mask_code;
120            break;
121#ifdef _MSM8974_
122        case CODEC_TYPE_VP8:
123            break;
124#endif
125        default:
126            return -1;
127    }
128
129    return 1;
130}
131
132
133int frame_parse::init_nal_length (unsigned int nal_len)
134{
135    if (nal_len == 0 || nal_len > 4 || state_nal != NAL_LENGTH_ACC) {
136        return -1;
137    }
138
139    nal_length = nal_len;
140
141    return 1;
142}
143
144int frame_parse::parse_sc_frame ( OMX_BUFFERHEADERTYPE *source,
145        OMX_BUFFERHEADERTYPE *dest ,
146        OMX_U32 *partialframe)
147{
148    OMX_U8 *pdest = NULL,*psource = NULL, match_found = FALSE, is_byte_match = 0;
149    OMX_U32 dest_len =0, source_len = 0, temp_len = 0;
150    OMX_U32 parsed_length = 0,i=0;
151    int residue_byte = 0;
152
153    if (source == NULL || dest == NULL || partialframe == NULL) {
154        return -1;
155    }
156
157    /*Calculate how many bytes are left in source and destination*/
158    dest_len = dest->nAllocLen - (dest->nFilledLen + dest->nOffset);
159    psource = source->pBuffer + source->nOffset;
160    pdest = dest->pBuffer + (dest->nFilledLen + dest->nOffset);
161    source_len = source->nFilledLen;
162
163    /*Need Minimum Start Code size for destination to copy atleast Start code*/
164    if ((start_code == H263_start_code && dest_len < 3) ||
165            (start_code != H263_start_code && dest_len < 4) || (source_len == 0)) {
166        DEBUG_PRINT_LOW("FrameParser: dest_len %u source_len %u",(unsigned int)dest_len, (unsigned int)source_len);
167
168        if (source_len == 0 && (source->nFlags & 0x01)) {
169            DEBUG_PRINT_LOW("FrameParser: EOS rxd!! Notify it as a complete frame");
170            *partialframe = 0;
171            return 1;
172        }
173
174        DEBUG_PRINT_LOW("FrameParser: Bitstream Parsing error");
175        return -1;
176    }
177
178    /*Check if State of the previous find is a Start code*/
179    if (parse_state == A4 || parse_state == A5) {
180        /*Check for minimun size should be 4*/
181        dest->nFlags = source->nFlags;
182        dest->nTimeStamp = source->nTimeStamp;
183
184        if (start_code == H263_start_code) {
185            memcpy (pdest,start_code,2);
186            pdest[2] = last_byte_h263;
187            dest->nFilledLen += 3;
188            pdest += 3;
189        } else {
190            memcpy (pdest,start_code,4);
191
192            if (start_code == VC1_AP_start_code
193                    || start_code == MPEG4_start_code
194                    || start_code == MPEG2_start_code) {
195                pdest[3] = last_byte;
196                update_skip_frame();
197            }
198
199            dest->nFilledLen += 4;
200            pdest += 4;
201        }
202
203        parse_state = A0;
204    }
205
206    /*Entry State Machine*/
207    while ( source->nFilledLen > 0 && parse_state != A0
208            && parse_state != A4 && parse_state != A5 && dest_len > 0
209          ) {
210        //printf ("In the Entry Loop");
211        switch (parse_state) {
212            case A3:
213                parse_additional_start_code(psource,&parsed_length);
214
215                if (parse_state == A4) {
216                    source->nFilledLen--;
217                    source->nOffset++;
218                    psource++;
219                    break;
220                }
221
222                /*If fourth Byte is matching then start code is found*/
223                if ((*psource & mask_code [3]) == start_code [3]) {
224                    parse_state = A4;
225                    last_byte =  *psource;
226                    source->nFilledLen--;
227                    source->nOffset++;
228                    psource++;
229                } else if ((start_code [1] == start_code [0]) && (start_code [2]  == start_code [1])) {
230                    parse_state = A2;
231                    memcpy (pdest,start_code,1);
232                    pdest++;
233                    dest->nFilledLen++;
234                    dest_len--;
235                } else if (start_code [2] == start_code [0]) {
236                    parse_state = A1;
237                    memcpy (pdest,start_code,2);
238                    pdest += 2;
239                    dest->nFilledLen += 2;
240                    dest_len -= 2;
241                } else {
242                    parse_state = A0;
243                    memcpy (pdest,start_code,3);
244                    pdest += 3;
245                    dest->nFilledLen +=3;
246                    dest_len -= 3;
247                }
248
249                break;
250
251            case A2:
252                is_byte_match = ((*psource & mask_code [2]) == start_code [2]);
253                match_found = FALSE;
254
255                if (start_code == H263_start_code) {
256                    if (is_byte_match) {
257                        last_byte_h263 = *psource;
258                        parse_state = A5;
259                        match_found = TRUE;
260                    }
261                } else if (start_code == H264_start_code &&
262                        (*psource & mask_code [3]) == start_code [3]) {
263                    parse_state = A5;
264                    match_found = TRUE;
265                } else {
266                    if (is_byte_match) {
267                        parse_state = A3;
268                        match_found = TRUE;
269                    }
270                }
271
272                if (match_found) {
273                    source->nFilledLen--;
274                    source->nOffset++;
275                    psource++;
276                } else if (start_code [1] == start_code [0]) {
277                    parse_state = A1;
278                    memcpy (pdest,start_code,1);
279                    dest->nFilledLen +=1;
280                    dest_len--;
281                    pdest++;
282                } else {
283                    parse_state = A0;
284                    memcpy (pdest,start_code,2);
285                    dest->nFilledLen +=2;
286                    dest_len -= 2;
287                    pdest += 2;
288                }
289
290                break;
291
292            case A1:
293
294                if ((*psource & mask_code [1]) == start_code [1]) {
295                    parse_state = A2;
296                    source->nFilledLen--;
297                    source->nOffset++;
298                    psource++;
299                } else {
300                    memcpy (pdest,start_code,1);
301                    dest->nFilledLen +=1;
302                    pdest++;
303                    dest_len--;
304                    parse_state = A0;
305                }
306
307                break;
308            case A4:
309            case A0:
310            case A5:
311                break;
312        }
313
314        dest_len = dest->nAllocLen - (dest->nFilledLen + dest->nOffset);
315    }
316
317    if (parse_state == A4 || parse_state == A5) {
318        *partialframe = 0;
319        check_skip_frame_boundary(partialframe);
320        DEBUG_PRINT_LOW("FrameParser: Parsed Len = %u", (unsigned int)dest->nFilledLen);
321        return 1;
322    }
323
324    /*Partial Frame is true*/
325    *partialframe = 1;
326
327    /*Calculate how many bytes are left in source and destination*/
328    dest_len = dest->nAllocLen - (dest->nFilledLen + dest->nOffset);
329    psource = source->pBuffer + source->nOffset;
330    pdest = dest->pBuffer + (dest->nFilledLen + dest->nOffset);
331    source_len = source->nFilledLen;
332
333    temp_len = (source_len < dest_len)?source_len:dest_len;
334
335    /*Check if entry state machine consumed source or destination*/
336    if (temp_len == 0) {
337        return 1;
338    }
339
340    /*Parsing State Machine*/
341    while  (parsed_length < temp_len) {
342        switch (parse_state) {
343            case A0:
344
345                if ((psource [parsed_length] & mask_code [0])  == start_code[0]) {
346                    parse_state = A1;
347                }
348
349                parsed_length++;
350                break;
351            case A1:
352
353                if ((psource [parsed_length] & mask_code [1]) == start_code [1]) {
354                    parsed_length++;
355                    parse_state = A2;
356                } else {
357                    parse_state = A0;
358                }
359
360                break;
361            case A2:
362                is_byte_match = ((psource[parsed_length] & mask_code [2]) == start_code [2]);
363                match_found = FALSE;
364
365                if (start_code == H263_start_code) {
366                    if (is_byte_match) {
367                        last_byte_h263 = psource[parsed_length];
368                        parse_state = A5;
369                        match_found = TRUE;
370                    }
371                } else if (start_code == H264_start_code &&
372                        (psource[parsed_length] & mask_code [3]) == start_code [3]) {
373                    parse_state = A5;
374                    match_found = TRUE;
375                } else {
376                    if (is_byte_match) {
377                        parse_state = A3;
378                        match_found = TRUE;
379                    }
380                }
381
382                if (match_found) {
383                    parsed_length++;
384                } else if (start_code [1] == start_code [0]) {
385                    parse_state = A1;
386                } else {
387                    parse_state = A0;
388                }
389
390                break;
391            case A3:
392                parse_additional_start_code(psource,&parsed_length);
393
394                if (parse_state == A4) break;
395
396                if ((psource [parsed_length] & mask_code [3]) == start_code [3]) {
397                    last_byte = psource [parsed_length];
398                    parsed_length++;
399                    parse_state = A4;
400                } else if ((start_code [1] == start_code [0]) && (start_code [2] == start_code [1])) {
401                    parse_state = A2;
402                } else if (start_code [2] == start_code [0]) {
403                    parse_state = A1;
404                } else {
405                    parse_state = A0;
406                }
407
408                break;
409            case A4:
410            case A5:
411                break;
412        }
413
414        /*Found the code break*/
415        if (parse_state == A4 || parse_state == A5) {
416            break;
417        }
418    }
419
420    /*Exit State Machine*/
421    psource = source->pBuffer + source->nOffset;
422    OMX_U32 bytes_to_skip = 0;
423    switch (parse_state) {
424        case A5:
425            *partialframe = 0;
426            check_skip_frame_boundary(partialframe);
427            bytes_to_skip = 3;
428            break;
429        case A4:
430            *partialframe = 0;
431            check_skip_frame_boundary(partialframe);
432            bytes_to_skip = 4;
433            break;
434        case A3:
435            if (source->nFlags & OMX_BUFFERFLAG_EOS) {
436                bytes_to_skip = 0;
437            } else {
438                bytes_to_skip = 3;
439            }
440            break;
441        case A2:
442            if (source->nFlags & OMX_BUFFERFLAG_EOS) {
443                bytes_to_skip = 0;
444            } else {
445                bytes_to_skip = 2;
446            }
447            break;
448        case A1:
449            if (source->nFlags & OMX_BUFFERFLAG_EOS) {
450                bytes_to_skip = 0;
451            } else {
452                bytes_to_skip = 1;
453            }
454            break;
455        case A0:
456            bytes_to_skip = 0;
457            break;
458    }
459
460    if (source->nFilledLen < parsed_length) {
461        DEBUG_PRINT_ERROR ("FATAL Error");
462        return -1;
463    }
464
465    if (parsed_length > bytes_to_skip) {
466        memcpy (pdest, psource, (parsed_length-bytes_to_skip));
467        dest->nFilledLen += (parsed_length-bytes_to_skip);
468    }
469
470    source->nFilledLen -= parsed_length;
471    source->nOffset += parsed_length;
472
473    return 1;
474}
475
476
477int frame_parse::parse_h264_nallength (OMX_BUFFERHEADERTYPE *source,
478        OMX_BUFFERHEADERTYPE *dest ,
479        OMX_U32 *partialframe)
480{
481    OMX_U8 *pdest = NULL,*psource = NULL;
482    OMX_U32 dest_len =0, source_len = 0, temp_len = 0,parsed_length = 0;
483
484    if (source == NULL || dest == NULL || partialframe == NULL) {
485        return -1;
486    }
487
488    /*Calculate the length's*/
489    dest_len = dest->nAllocLen - (dest->nFilledLen + dest->nOffset);
490    source_len = source->nFilledLen;
491
492    if (dest_len < 4 || nal_length == 0) {
493        DEBUG_PRINT_LOW("FrameParser: NAL Parsing Error! dest_len %u "
494                "nal_length %u", (unsigned int)dest_len, nal_length);
495        return -1;
496    }
497
498    if (source_len == 0 ) {
499        if (source->nFlags & OMX_BUFFERFLAG_EOS) {
500            DEBUG_PRINT_LOW("FrameParser: EOS rxd for nallength!!"
501                " Notify it as a complete frame");
502            *partialframe = 0;
503            return 1;
504        } else {
505            DEBUG_PRINT_ERROR("FrameParser: NAL Parsing Error!"
506                "Buffer recieved with source_len = %u and with"
507                "flags %u", (unsigned int)source_len, (unsigned int)source->nFlags);
508            return -1;
509        }
510    }
511
512    *partialframe = 1;
513    temp_len = (source_len < dest_len)?source_len:dest_len;
514    psource = source->pBuffer + source->nOffset;
515    pdest = dest->pBuffer + (dest->nFilledLen + dest->nOffset);
516
517    /* Find the Bytes to Accumalte*/
518    if (state_nal == NAL_LENGTH_ACC) {
519        while (parsed_length < temp_len ) {
520            bytes_tobeparsed |= (((OMX_U32)(*psource))) << (((nal_length-accum_length-1) << 3));
521
522            /*COPY THE DATA FOR C-SIM TO BE REOMVED ON TARGET*/
523            //*pdest = *psource;
524            accum_length++;
525            source->nFilledLen--;
526            source->nOffset++;
527            psource++;
528            //dest->nFilledLen++;
529            //pdest++;
530            parsed_length++;
531
532            if (accum_length == nal_length) {
533                accum_length = 0;
534                state_nal = NAL_PARSING;
535                memcpy (pdest,H264_start_code,4);
536                dest->nFilledLen += 4;
537                break;
538            }
539        }
540    }
541
542    dest_len = dest->nAllocLen - (dest->nFilledLen + dest->nOffset);
543    source_len = source->nFilledLen;
544    temp_len = (source_len < dest_len)?source_len:dest_len;
545
546    psource = source->pBuffer + source->nOffset;
547    pdest = dest->pBuffer + (dest->nFilledLen + dest->nOffset);
548
549    dest->nTimeStamp = source->nTimeStamp;
550    dest->nFlags = source->nFlags;
551
552    /*Already in Parsing state go ahead and copy*/
553    if (state_nal == NAL_PARSING && temp_len > 0) {
554        if (temp_len < bytes_tobeparsed) {
555            memcpy (pdest,psource,temp_len);
556            dest->nFilledLen += temp_len;
557            source->nOffset += temp_len;
558            source->nFilledLen -= temp_len;
559            bytes_tobeparsed -= temp_len;
560        } else {
561            memcpy (pdest,psource,bytes_tobeparsed);
562            temp_len -= bytes_tobeparsed;
563            dest->nFilledLen += bytes_tobeparsed;
564            source->nOffset += bytes_tobeparsed;
565            source->nFilledLen -= bytes_tobeparsed;
566            bytes_tobeparsed = 0;
567        }
568    }
569
570    if (bytes_tobeparsed == 0 && state_nal == NAL_PARSING) {
571        *partialframe = 0;
572        state_nal = NAL_LENGTH_ACC;
573    }
574
575    return 1;
576}
577
578void frame_parse::flush ()
579{
580    parse_state = A0;
581    state_nal = NAL_LENGTH_ACC;
582    accum_length = 0;
583    bytes_tobeparsed = 0;
584    header_found = false;
585    skip_frame_boundary = false;
586}
587
588void frame_parse::parse_additional_start_code(OMX_U8 *psource,
589        OMX_U32 *parsed_length)
590{
591
592    if (((start_code == MPEG4_start_code) ||
593                (start_code == MPEG2_start_code)) &&
594            psource &&
595            parsed_length) {
596        OMX_U32 index = *parsed_length;
597
598        if ((start_code == MPEG4_start_code &&
599                    (psource [index] & 0xF0) == 0x20) ||
600                (start_code == MPEG2_start_code &&
601                 psource [index] == 0xB3)) {
602            if (header_found) {
603                last_byte = psource [index];
604                index++;
605                parse_state = A4;
606            } else
607                header_found = true;
608        }
609
610        *parsed_length = index;
611    }
612}
613
614void frame_parse::check_skip_frame_boundary(OMX_U32 *partialframe)
615{
616    if ((start_code == MPEG4_start_code ||
617                start_code == MPEG2_start_code) &&
618            partialframe) {
619
620        *partialframe = 1;
621
622        if (!skip_frame_boundary)
623            *partialframe = 0;
624
625        skip_frame_boundary = false;
626    }
627}
628
629void frame_parse::update_skip_frame()
630{
631    if (((start_code == MPEG4_start_code) &&
632                ((last_byte & 0xF0) == 0x20)) ||
633            ((start_code == MPEG2_start_code) &&
634             (last_byte == 0xB3))) {
635
636        skip_frame_boundary = true;
637    }
638}
639