/*
   forward-passthrough-hanc.c - HANC handling for SoftLab-NSK Forward boards

   Copyright (C) 2017 - 2023 Konstantin Oblaukhov <oblaukhov@sl.iae.nsk.su>

   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-passthrough.h"

inline bool forward_st291_seek_header(u16 *buffer, size_t *offset, size_t size)
{
	u16 *p = &buffer[*offset];

	while ((int)*offset < ((int)size - 3)) {
		// Skip TRS
		if ((p[0] == 0x3FF) && (p[1] == 0x000) && (p[2] == 0x000)) {
			(*offset) += 4;
			p += 4;
			continue;
		}

		if ((p[0] == 0x000) && (p[1] == 0x3FF) && (p[2] == 0x3FF))
			return true;

		p++;
		(*offset)++;
	}
	return false;
}

static size_t forward_st272_decode(u16 *in_buffer, size_t in_size, u32 *out_buffer, size_t out_size,
				   int groups)
{
	size_t in_pos = 0;
	size_t out_pos[16];
	int stride = groups * 4;
	int ch;
	size_t result = 0;

	for (ch = 0; ch < 16; ch++)
		out_pos[ch] = 0;

	while (forward_st291_seek_header(in_buffer, &in_pos, in_size)) {
		u16 *in_p = &in_buffer[in_pos];
		u8 did = in_p[3] & 0xFF;
		u8 dc = in_p[5] & 0xFF;
		u16 *udw;
		int group;
		int i;

		if ((did != 0xFF) && (did != 0xFD) && (did != 0xFB) && (did != 0xF9)) {
			in_pos += dc + 7;
			continue;
		}

		group = 3 - (did - 0xF9) / 2;
		udw = &in_p[6];

		if (group >= groups) {
			in_pos += dc + 7;
			continue;
		}

		for (i = 0; i < dc / 3; i++) {
			u32 *p;
			u32 w = 0;

			ch = group * 4 + ((udw[i * 3] >> 1) & 0x3);
			p = out_buffer + out_pos[ch] * stride + ch;
			out_pos[ch]++;

			w |= (u32)((udw[i * 3 + 0] >> 3) & 0x3F) << 12;
			w |= (u32)(udw[i * 3 + 1] & 0x1FF) << 18;
			w |= (u32)(udw[i * 3 + 2] & 0x1F) << 27;

			*p = w;

			if (out_pos[ch] * stride > result)
				result = out_pos[ch] * stride;
		}

		in_pos += dc + 7;
	}

	return result;
}

static size_t forward_st299_decode(u16 *in_buffer, size_t in_size, u32 *out_buffer, size_t out_size,
				   int groups)
{
	size_t in_pos = 0;
	size_t out_pos[4];
	int stride = groups * 4;
	int g;
	size_t result = 0;

	for (g = 0; g < 4; g++)
		out_pos[g] = 0;

	while (forward_st291_seek_header(in_buffer, &in_pos, in_size)) {
		u16 *in_p = &in_buffer[in_pos];
		u8 did = in_p[3] & 0xFF;
		u8 dc = in_p[5] & 0xFF;
		u16 *udw;
		int group;
		size_t pos;
		int i;

		if ((did > 0xE7) || (did < 0xE4)) {
			in_pos += dc + 7;
			continue;
		}

		group = 3 - (did - 0xE4);
		pos = out_pos[group];

		if ((dc < 24) || (group >= groups) || ((pos + 1) * stride > out_size)) {
			in_pos += dc + 7;
			continue;
		}

		udw = &in_p[6];

		for (i = 0; i < 4; i++) {
			u32 *p = &out_buffer[pos * stride + group * 4 + i];
			u32 w = 0;

			w |= (u32)((udw[2 + i * 4 + 0] >> 4) & 0xF) << 8;
			w |= (u32)(udw[2 + i * 4 + 1] & 0xFF) << 12;
			w |= (u32)(udw[2 + i * 4 + 2] & 0xFF) << 20;
			w |= (u32)(udw[2 + i * 4 + 3] & 0xF) << 28;

			*p = w;
		}
		pos++;
		if (pos * stride > result)
			result = pos * stride;
		out_pos[group] = pos;

		in_pos += dc + 7;
	}

	return result;
}

void forward_passthrough_decode_hanc(struct forward_passthrough_io *io, int field, u16 *buffer,
				     size_t size)
{
	size_t i = 0;
	u16 *inp = buffer;
	u16 *outp[2];
	size_t out_s[2];

	for (i = 0; i < 2; i++) {
		outp[i] = io->a_in_decode_buffers[i];
		out_s[i] = 0;
	}

	for (i = 0; i < size; i++) {
		u16 v = *inp & 0x3FF;
		u8 s = (*inp & 0xFC00) >> 10;
		inp++;

		if (unlikely(s > 1))
			continue;

		*outp[s] = v;
		outp[s]++;
		out_s[s]++;
	}

	if (io->format.pixelclock == 13500000) {
		io->a_in_decoded_size[field] = forward_st272_decode(
			io->a_in_decode_buffers[0], out_s[0], io->a_in_decoded_buffers[field],
			io->a_in_buffer_size / 4, io->a_out_groups);
	} else {
		io->a_in_decoded_size[field] = forward_st299_decode(
			io->a_in_decode_buffers[0], out_s[0], io->a_in_decoded_buffers[field],
			io->a_in_buffer_size / 4, io->a_out_groups);
	}
}
