drivers/media/platform/sti/delta/delta-mjpeg-hdr.c

/*
 * Copyright (C) STMicroelectronics SA 2013
 * Author: Hugues Fruchet <hugues.fruchet@st.com> for STMicroelectronics.
 * License terms:  GNU General Public License (GPL), version 2
 */

#include "delta.h"
#include "delta-mjpeg.h"

#define MJPEG_SOF_0  0xc0
#define MJPEG_SOF_1  0xc1
#define MJPEG_SOI    0xd8
#define MJPEG_MARKER 0xff

static char *header_str(struct mjpeg_header *header,
			char *str,
			unsigned int len)
{
	char *cur = str;
	unsigned int left = len;

	if (!header)
		return "";

	snprintf(cur, left, "[MJPEG header]\n"
			"|- length     = %d\n"
			"|- precision  = %d\n"
			"|- width      = %d\n"
			"|- height     = %d\n"
			"|- components = %d\n",
			header->length,
			header->sample_precision,
			header->frame_width,
			header->frame_height,
			header->nb_of_components);

	return str;
}

static int delta_mjpeg_read_sof(struct delta_ctx *pctx,
				unsigned char *data, unsigned int size,
				struct mjpeg_header *header)
{
	struct delta_dev *delta = pctx->dev;
	unsigned int offset = 0;

	if (size < 64)
		goto err_no_more;

	memset(header, 0, sizeof(*header));
	header->length           = be16_to_cpu(*(__be16 *)(data + offset));
	offset += sizeof(u16);
	header->sample_precision = *(u8 *)(data + offset);
	offset += sizeof(u8);
	header->frame_height     = be16_to_cpu(*(__be16 *)(data + offset));
	offset += sizeof(u16);
	header->frame_width      = be16_to_cpu(*(__be16 *)(data + offset));
	offset += sizeof(u16);
	header->nb_of_components = *(u8 *)(data + offset);
	offset += sizeof(u8);

	if (header->nb_of_components >= MJPEG_MAX_COMPONENTS) {
		dev_err(delta->dev,
			"%s   unsupported number of components (%d > %d)\n",
			pctx->name, header->nb_of_components,
			MJPEG_MAX_COMPONENTS);
		return -EINVAL;
	}

	if ((offset + header->nb_of_components *
	     sizeof(header->components[0])) > size)
		goto err_no_more;

	return 0;

err_no_more:
	dev_err(delta->dev,
		"%s   sof: reached end of %d size input stream\n",
		pctx->name, size);
	return -ENODATA;
}

int delta_mjpeg_read_header(struct delta_ctx *pctx,
			    unsigned char *data, unsigned int size,
			    struct mjpeg_header *header,
			    unsigned int *data_offset)
{
	struct delta_dev *delta = pctx->dev;
	unsigned char str[200];

	unsigned int ret = 0;
	unsigned int offset = 0;
	unsigned int soi = 0;

	if (size < 2)
		goto err_no_more;

	offset = 0;
	while (1) {
		if (data[offset] == MJPEG_MARKER)
			switch (data[offset + 1]) {
			case MJPEG_SOI:
				soi = 1;
				*data_offset = offset;
				break;

			case MJPEG_SOF_0:
			case MJPEG_SOF_1:
				if (!soi) {
					dev_err(delta->dev,
						"%s   wrong sequence, got SOF while SOI not seen\n",
						pctx->name);
					return -EINVAL;
				}

				ret = delta_mjpeg_read_sof(pctx,
							   &data[offset + 2],
							   size - (offset + 2),
							   header);
				if (ret)
					goto err;

				goto done;

			default:
				break;
			}

		offset++;
		if ((offset + 2) >= size)
			goto err_no_more;
	}

done:
	dev_dbg(delta->dev,
		"%s   found header @ offset %d:\n%s", pctx->name,
		*data_offset,
		header_str(header, str, sizeof(str)));
	return 0;

err_no_more:
	dev_err(delta->dev,
		"%s   no header found within %d bytes input stream\n",
		pctx->name, size);
	return -ENODATA;

err:
	return ret;
}