/*
   forward-hanc.h - SMPTE SDI HANC decoder

   Copyright (C) 2017 - 2024 SoftLab-NSK <forward@softlab.tv>

   This program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public License
   as published by the Free Software Foundation; either version 2
   of the License, or (at your option) any later version.

   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.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "forward-hanc.h"

int forward_hanc_open(struct forward_hanc_stream *s, const uint16_t *buffer, size_t size,
		      int streams)
{
	int i;

	if (!buffer || !size || (streams < 0) || (streams > FORWARD_HANC_MAX_SDI_STREAMS))
		return -EINVAL;

	s->buffer = buffer;
	s->size = size;
	s->streams = streams;

	for (i = 0; i < FORWARD_HANC_MAX_SDI_STREAMS; i++)
		s->p[i] = 0;

	return 0;
}

void forward_hanc_close(struct forward_hanc_stream *s)
{
	s->buffer = 0;
	s->size = 0;
	s->streams = 0;
}

static inline int forward_hanc_skip(struct forward_hanc_stream *s, int stream, size_t size)
{
	int i = 0;

	while (i < size) {
		uint16_t data;

		if (s->p[stream] >= s->size)
			break;

		data = s->buffer[s->p[stream]];
		s->p[stream]++;

		if ((data >> 10) != stream)
			continue;

		i++;
	}

	return i;
}

static inline int forward_hanc_read(struct forward_hanc_stream *s, int stream, uint16_t *buffer,
				    size_t size)
{
	int i = 0;
	uint16_t stream_mark = (stream << 10);

	while (i < size) {
		uint16_t data;

		if (s->p[stream] >= s->size)
			break;

		data = s->buffer[s->p[stream]];
		s->p[stream]++;

		if (unlikely((data & 0xFC00) != stream_mark))
			continue;

		buffer[i] = data & 0x3FF;
		i++;
	}

	return i;
}

bool forward_hanc_read_smpte_291_header(struct forward_hanc_stream *s, int stream,
					struct forward_hanc_smpte_291_header *h)
{
	uint16_t header[6];

	for (;;) {
		int oldpos = s->p[stream];
		int readed = forward_hanc_read(s, stream, &header[0], 3);

		if (unlikely(readed != 3))
			return false;

		// Skip TRS
		if ((header[0] == 0x3FF) && (header[1] == 0x000) && (header[2] == 0x000))
			continue;

		if (unlikely((header[0] != 0x000) || (header[1] != 0x3FF) ||
			     (header[2] != 0x3FF))) {
			s->p[stream] = oldpos;
			forward_hanc_skip(s, stream, 1);
			continue;
		}

		readed = forward_hanc_read(s, stream, &header[3], 3);

		if (unlikely(readed != 3))
			return false;

		h->did = header[3] & 0xFF;
		h->sdid = header[4] & 0xFF;
		h->data_count = header[5] & 0xFF;

		return true;
	}
}

bool forward_hanc_read_smpte_291_data(struct forward_hanc_stream *s, int stream,
				      struct forward_hanc_smpte_291_header *h, uint16_t *data,
				      size_t maxsize)
{
	size_t realsize;
	int readed;
	int readout = 1;

	if (data && maxsize) {
		realsize = h->data_count > maxsize ? maxsize : h->data_count;
		readed = forward_hanc_read(s, stream, data, realsize);
		if (!readed)
			return false;
	}

	if (!data || !maxsize)
		readout = h->data_count;
	else if (maxsize < h->data_count)
		readout += h->data_count - maxsize;

	forward_hanc_skip(s, stream, readout);

	return true;
}
