drivers/media/platform/mtk-jpeg/mtk_jpeg_parse.c

/*
 * Copyright (c) 2016 MediaTek Inc.
 * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
 *         Rick Chang <rick.chang@mediatek.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program 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.
 */

#include <linux/kernel.h>
#include <linux/videodev2.h>

#include "mtk_jpeg_parse.h"

#define TEM	0x01
#define SOF0	0xc0
#define RST	0xd0
#define SOI	0xd8
#define EOI	0xd9

struct mtk_jpeg_stream {
	u8 *addr;
	u32 size;
	u32 curr;
};

static int read_byte(struct mtk_jpeg_stream *stream)
{
	if (stream->curr >= stream->size)
		return -1;
	return stream->addr[stream->curr++];
}

static int read_word_be(struct mtk_jpeg_stream *stream, u32 *word)
{
	u32 temp;
	int byte;

	byte = read_byte(stream);
	if (byte == -1)
		return -1;
	temp = byte << 8;
	byte = read_byte(stream);
	if (byte == -1)
		return -1;
	*word = (u32)byte | temp;

	return 0;
}

static void read_skip(struct mtk_jpeg_stream *stream, long len)
{
	if (len <= 0)
		return;
	while (len--)
		read_byte(stream);
}

static bool mtk_jpeg_do_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
			      u32 src_size)
{
	bool notfound = true;
	struct mtk_jpeg_stream stream;

	stream.addr = src_addr_va;
	stream.size = src_size;
	stream.curr = 0;

	while (notfound) {
		int i, length, byte;
		u32 word;

		byte = read_byte(&stream);
		if (byte == -1)
			return false;
		if (byte != 0xff)
			continue;
		do
			byte = read_byte(&stream);
		while (byte == 0xff);
		if (byte == -1)
			return false;
		if (byte == 0)
			continue;

		length = 0;
		switch (byte) {
		case SOF0:
			/* length */
			if (read_word_be(&stream, &word))
				break;

			/* precision */
			if (read_byte(&stream) == -1)
				break;

			if (read_word_be(&stream, &word))
				break;
			param->pic_h = word;

			if (read_word_be(&stream, &word))
				break;
			param->pic_w = word;

			param->comp_num = read_byte(&stream);
			if (param->comp_num != 1 && param->comp_num != 3)
				break;

			for (i = 0; i < param->comp_num; i++) {
				param->comp_id[i] = read_byte(&stream);
				if (param->comp_id[i] == -1)
					break;

				/* sampling */
				byte = read_byte(&stream);
				if (byte == -1)
					break;
				param->sampling_w[i] = (byte >> 4) & 0x0F;
				param->sampling_h[i] = byte & 0x0F;

				param->qtbl_num[i] = read_byte(&stream);
				if (param->qtbl_num[i] == -1)
					break;
			}

			notfound = !(i == param->comp_num);
			break;
		case RST ... RST + 7:
		case SOI:
		case EOI:
		case TEM:
			break;
		default:
			if (read_word_be(&stream, &word))
				break;
			length = (long)word - 2;
			read_skip(&stream, length);
			break;
		}
	}

	return !notfound;
}

bool mtk_jpeg_parse(struct mtk_jpeg_dec_param *param, u8 *src_addr_va,
		    u32 src_size)
{
	if (!mtk_jpeg_do_parse(param, src_addr_va, src_size))
		return false;
	if (mtk_jpeg_dec_fill_param(param))
		return false;

	return true;
}