UTS6710/drivers/hdmi/6710/spdif_parser.c

/* --------------------------------------------------------
 SPDIF parser for dolby digital plus data
 Date:      2014-10-30
 Author:    jimmy.lai@s2-tek.com
 ---------------------------------------------------------- */

#include "spdif_parser.h"
#include <linux/kernel.h>   // abs(), max()
#include <linux/string.h>

/* ----- Local defines for debug --------------------------------- */

#define SPDIF_DD_PARSER_DEBUG   0

#if SPDIF_DD_PARSER_DEBUG
#define dbg(fmt,args...) printk(fmt, ## args)
#else
#define dbg(fmt,args...)
#endif

static void dump(const unsigned char *ptr, int size)
{
#if SPDIF_DD_PARSER_DEBUG
    int i = 0, n = 0;
    char str[3 * 0x10 + 8];

    for (i = 0; i < size ; i++)
    {
        if (n >= 0)
        {
            n += sprintf(&str[n], "%02x ", ptr[i]);
        }

        if (n >= 3 * 0x10 || i + 1 == size)
        {
            n = 0;
            dbg("%s\n", str);
        }
    }

#endif
}

/* ----- Local structures and defines ---------------------------- */

typedef struct __SPDIF_PARSER
{
    /* source buffer info */
#define IEC61937_DDP_BUFSIZE    (6144*2*2)
    BOOL    bFound;
    UINT32  parsed_sz;
    UINT32  spdif_header_sz;
    UINT8   spdif_header[8];

    /* destination buffer info */
#define MAX_CMDBUF_BUFSIZE      (4*1024)
#define CMDBUF_NUMBER           (16)
    UINT32  idx;
    UINT32  offset;
    UINT32  cmdbufsz;
    UINT32  pts;
    UINT8   *cmdbuf[CMDBUF_NUMBER];
    UINT8   *curbuf;

    /* fire call back function */
    FireCallBack    fire;
    void            *fire_param;
} SPDIF_PARSER;


/* ----- static functions ---------------------------------------- */

static SPDIF_PARSER gSpdifParser;

static SPDIF_PARSER *get_spdif_parser(void)
{
    return &gSpdifParser;
}

static UINT32 spdif_copy(UINT8 *dst, UINT8 *src, UINT32 srcsz)
{
    UINT32 *s = (UINT32*)src;
    UINT32 i, d;

    for (i = 0, d = 0; i < srcsz / 4; i++)
    {
        dst[d++] = (s[i] >> 24) & 0xff;
        dst[d++] = (s[i] >> 16) & 0xff;
    }

    return d;
}

static UINT8 *find_spdif_header(UINT8 *inbuf, UINT32 inbufsz)
{
    UINT32 i;
    UINT32 *in = (UINT32*)inbuf;

	if(inbufsz/4 == 0)
	{
		printk("inbufsz %d,skip find spdif header\n", inbufsz);
	    return NULL;
	}

    for (i = 0; i < inbufsz / 4 - 1; i += 2)
    {
        if ((in[i] >> 16) == 0xf872 && (in[i + 1] >> 16) == 0x4e1f)
        {
            return inbuf + i * 4;
        }
    }

    return NULL;
}

/* ----- global functions ---------------------------------------- */

void hdmi_spdif_parser_handler(UINT8 *inbuf, UINT32 inbufsz, UINT32 timestamp)
{
    SPDIF_PARSER *p = get_spdif_parser();
    UINT8 *src;
    UINT32 leftsz;
    src = inbuf;
    leftsz = inbufsz;
parse_again:

    if (p->bFound == FALSE)
    {
        src = find_spdif_header(src, leftsz);

        if (src)
        {
            p->bFound = TRUE;
            p->offset = 0;
            p->spdif_header_sz = 0;
            p->cmdbufsz = 0;
            p->parsed_sz = 0;
            leftsz = inbufsz - (UINT32)(src - inbuf);
            dbg("head(%d): spdif header found\n", src - inbuf);
        }
        else
        {
            dbg("head(%d): spdif header not found\n", inbufsz);
            return;
        }
    }

	//printk("inbufsz=%d, p->parse_sz=%d p->offset =%d\n", inbufsz, p->parsed_sz, p->offset);
    /* spdif header is found */
    if (p->parsed_sz < 8)
    {
        /* parse spdif header : pa pb pc pd */
        if (p->spdif_header_sz < 8)
        {
            UINT32 needsz, copysz;
            needsz = 16 - p->spdif_header_sz * 2;
            copysz = (leftsz < needsz) ? leftsz : needsz;
            p->spdif_header_sz += spdif_copy(p->spdif_header + p->spdif_header_sz, src, copysz);
            p->parsed_sz = p->spdif_header_sz;
            dbg("head(%d): %d (needsz %d leftsz %d) parsed %d\n", src - inbuf, copysz, needsz, leftsz, p->parsed_sz);
            src += copysz;
            leftsz -= copysz;

            if (p->spdif_header_sz == 8)
            {
                /* check header and data type is dd */
                if ((*(UINT32*)p->spdif_header) != 0x1f4e72f8 ||
                        p->spdif_header[5] != 21)
                {
                    dbg("head(%d): invalid spdif header\n", src - inbuf);
                    dump(p->spdif_header, 8);
                    p->bFound = FALSE;
					if (leftsz > 0)
					{
                    goto parse_again;
                }
					else
					{
						printk("leftsz=%d, copysz=%d, needsz= %d, skip \n" ,leftsz,copysz,needsz);
						return;
					}
						
                }

                /* Note: ddp frame size is indicated in bytes */
                p->cmdbufsz = ((p->spdif_header[6] << 8) | p->spdif_header[7]);
                p->pts = timestamp;

                /* Reserve continuous buffers for big data */
                if (p->cmdbufsz + 8 > MAX_CMDBUF_BUFSIZE)
                {
                    UINT32 cnt = (p->cmdbufsz + 8) / MAX_CMDBUF_BUFSIZE;

                    if (p->idx + cnt >= CMDBUF_NUMBER)
                    {
                        p->idx = 0;
                        p->curbuf = p->cmdbuf[p->idx];
                    }
                }

#if DDPFRMAE_CONTAINS_SPDIF_HEADER
                memcpy(p->curbuf, p->spdif_header, 8);
#endif
                dbg("head(%d): valid spdif header\n", src - inbuf);
                dump(p->spdif_header, 8);
            }
        }
    }

    /* parse spdif payload (nonlinear and zero stuffings) */
    if (p->parsed_sz >= 8)
    {
        /* parse nonlinear part */
        if (p->offset < p->cmdbufsz)
        {
            UINT32 needsz, copysz;
            needsz = 2 * (p->cmdbufsz - p->offset);
            copysz = (leftsz < needsz) ? leftsz : needsz;
#if DDPFRMAE_CONTAINS_SPDIF_HEADER
            spdif_copy(p->curbuf + 8 + p->offset, src, copysz);
#else
            spdif_copy(p->curbuf + p->offset, src, copysz);
#endif
            p->parsed_sz += (copysz / 2);
            dbg("copy(%d): %d (needsz %d leftsz %d) parsed %d\n", src - inbuf, copysz, needsz, leftsz, p->parsed_sz);
            src += copysz;
            leftsz -= copysz;
            p->offset += (copysz / 2);
        }

        /* skip zero stuffing */
        if (p->offset == p->cmdbufsz && p->parsed_sz < IEC61937_DDP_BUFSIZE)
        {
            UINT32 needsz, skipsz;
            needsz = 2 * (IEC61937_DDP_BUFSIZE - p->parsed_sz);
            skipsz = (leftsz < needsz) ? leftsz : needsz;
            p->parsed_sz += (skipsz / 2);
            dbg("skip(%d): %d (needsz %d leftsz %d) parsed %d\n", src - inbuf, skipsz, needsz, leftsz, p->parsed_sz);
            src += skipsz;
            leftsz -= skipsz;
        }
    }

    /* parse done */
    if (p->parsed_sz >= IEC61937_DDP_BUFSIZE)
    {
        /* fire command queue */
        dbg("done(%d): %d fire dd frame\n", src - inbuf, p->cmdbufsz);
        dump(p->curbuf, 0x10);
#if DDPFRMAE_CONTAINS_SPDIF_HEADER
        p->fire(p->fire_param, p->curbuf, p->cmdbufsz + 8, p->pts);
#else
        p->fire(p->fire_param, p->curbuf, p->cmdbufsz, p->pts);
#endif
        /* update target buffer */
        p->idx = (p->idx + 1) % CMDBUF_NUMBER;

        if (p->cmdbufsz + 8 > MAX_CMDBUF_BUFSIZE)
        {
            UINT32 cnt = (p->cmdbufsz + 8) / MAX_CMDBUF_BUFSIZE;
            p->idx = (p->idx + cnt) % CMDBUF_NUMBER;
        }

        p->curbuf = p->cmdbuf[p->idx];
        p->offset = 0;
        p->cmdbufsz = 0;
        p->spdif_header_sz = 0;
        p->parsed_sz = 0;
    }

    if (leftsz > 0)
    {
        goto parse_again;
    }
}

void hdmi_spdif_parser_reset(void)
{
    SPDIF_PARSER *p = get_spdif_parser();
    p->bFound = FALSE;
    p->parsed_sz = 0;
    p->spdif_header_sz = 0;
    p->idx = 0;
    p->offset = 0;
    p->cmdbufsz = 0;
    p->curbuf = p->cmdbuf[p->idx];
}

void hdmi_spdif_parser_init(UINT8 *cmdbuf, FireCallBack callback, void *cb_param)
{
    UINT32 i;
    SPDIF_PARSER *p = get_spdif_parser();
    memset(p, 0, sizeof(SPDIF_PARSER));

    for (i = 0; i < CMDBUF_NUMBER; i++)
    {
        p->cmdbuf[i] = cmdbuf + (i * MAX_CMDBUF_BUFSIZE);
    }

    p->fire = callback;
    p->fire_param = cb_param;
    hdmi_spdif_parser_reset();
}