/*
   forward-v4l2-asi.c - DVB-ASI utilities for SoftLab-NSK Forward video boards

   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-splicer-asi.h"

static const u16 t10to8[1024] = {
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0x00ef, 0x00e0, 0x00e7, 0xffff, 0x00f0, 0x00ff, 0x00eb, 0x00f8,
	0x00ed, 0x00ee, 0xffff, 0xffff, 0x00e1, 0x00e2, 0x00f3, 0x00e4, 0x00f5, 0x00f6, 0x00f7,
	0x00e8, 0x00f9, 0x00fa, 0x00fb, 0x00fc, 0x00fd, 0x00fe, 0xffff, 0xffff, 0x00fe, 0x00fd,
	0x00e3, 0x00fb, 0x00e5, 0x00e6, 0x00e8, 0x00f7, 0x00e9, 0x00ea, 0x00e4, 0x00ec, 0x00e2,
	0x00e1, 0xffff, 0xffff, 0x00f1, 0x00f2, 0x00f8, 0x00f4, 0x00ff, 0x00f0, 0xffff, 0x00e7,
	0x00e0, 0x00ef, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0x000f, 0x0000, 0x0007, 0xffff, 0x0010, 0x001f, 0x000b, 0x0018, 0x000d, 0x000e,
	0xffff, 0xffff, 0x0001, 0x0002, 0x0013, 0x0004, 0x0015, 0x0016, 0x0017, 0x0008, 0x0019,
	0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0xffff, 0xffff, 0x001e, 0x001d, 0x0003, 0x001b,
	0x0005, 0x0006, 0x0008, 0x0017, 0x0009, 0x000a, 0x0004, 0x000c, 0x0002, 0x0001, 0xffff,
	0xffff, 0x0011, 0x0012, 0x0018, 0x0014, 0x001f, 0x0010, 0xffff, 0x0007, 0x0000, 0x000f,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x006f,
	0x0060, 0x0067, 0xffff, 0x0070, 0x007f, 0x006b, 0x0078, 0x006d, 0x006e, 0xffff, 0xffff,
	0x0061, 0x0062, 0x0073, 0x0064, 0x0075, 0x0076, 0x0077, 0x0068, 0x0079, 0x007a, 0x007b,
	0x007c, 0x007d, 0x007e, 0xffff, 0xffff, 0x007e, 0x007d, 0x0063, 0x007b, 0x0065, 0x0066,
	0x0068, 0x0077, 0x0069, 0x006a, 0x0064, 0x006c, 0x0062, 0x0061, 0xffff, 0xffff, 0x0071,
	0x0072, 0x0078, 0x0074, 0x007f, 0x0070, 0xffff, 0x0067, 0x0060, 0x006f, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x008f, 0x0080, 0x0087,
	0xffff, 0x0090, 0x009f, 0x008b, 0x0098, 0x008d, 0x008e, 0xffff, 0xffff, 0x0081, 0x0082,
	0x0093, 0x0084, 0x0095, 0x0096, 0x0097, 0x0088, 0x0099, 0x009a, 0x009b, 0x009c, 0x009d,
	0x009e, 0xffff, 0xffff, 0x009e, 0x009d, 0x0083, 0x009b, 0x0085, 0x0086, 0x0088, 0x0097,
	0x0089, 0x008a, 0x0084, 0x008c, 0x0082, 0x0081, 0xffff, 0xffff, 0x0091, 0x0092, 0x0098,
	0x0094, 0x009f, 0x0090, 0xffff, 0x0087, 0x0080, 0x008f, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00af, 0x00a0, 0x00a7, 0xffff, 0x00b0,
	0x00bf, 0x00ab, 0x00b8, 0x00ad, 0x00ae, 0xffff, 0xffff, 0x00a1, 0x00a2, 0x00b3, 0x00a4,
	0x00b5, 0x00b6, 0x00b7, 0x00a8, 0x00b9, 0x00ba, 0x00bb, 0x00bc, 0x00bd, 0x00be, 0xffff,
	0xffff, 0x00be, 0x00bd, 0x00a3, 0x00bb, 0x00a5, 0x00a6, 0x00a8, 0x00b7, 0x00a9, 0x00aa,
	0x00a4, 0x00ac, 0x00a2, 0x00a1, 0xffff, 0xffff, 0x00b1, 0x00b2, 0x00b8, 0x00b4, 0x00bf,
	0x00b0, 0xffff, 0x00a7, 0x00a0, 0x00af, 0xffff, 0x01bc, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0x00cf, 0x00c0, 0x00c7, 0xffff, 0x00d0, 0x00df, 0x00cb,
	0x00d8, 0x00cd, 0x00ce, 0xffff, 0xffff, 0x00c1, 0x00c2, 0x00d3, 0x00c4, 0x00d5, 0x00d6,
	0x00d7, 0x00c8, 0x00d9, 0x00da, 0x00db, 0x00dc, 0x00dd, 0x00de, 0xffff, 0xffff, 0x00de,
	0x00dd, 0x00c3, 0x00db, 0x00c5, 0x00c6, 0x00c8, 0x00d7, 0x00c9, 0x00ca, 0x00c4, 0x00cc,
	0x00c2, 0x00c1, 0xffff, 0xffff, 0x00d1, 0x00d2, 0x00d8, 0x00d4, 0x00df, 0x00d0, 0xffff,
	0x00c7, 0x00c0, 0x00cf, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0x00ef, 0x00e0, 0x00e7, 0xffff, 0x00f0, 0x00ff, 0x00eb, 0x00f8, 0x00ed,
	0x00ee, 0xffff, 0xffff, 0x00e1, 0x00e2, 0x00f3, 0x00e4, 0x00f5, 0x00f6, 0x00f7, 0x00e8,
	0x00f9, 0x00fa, 0x00fb, 0x00fc, 0x00fd, 0x00fe, 0xffff, 0xffff, 0x00fe, 0x00fd, 0x00e3,
	0x00fb, 0x00e5, 0x00e6, 0x00e8, 0x00f7, 0x00e9, 0x00ea, 0x00e4, 0x00ec, 0x00e2, 0x00e1,
	0xffff, 0xffff, 0x00f1, 0x00f2, 0x00f8, 0x00f4, 0x00ff, 0x00f0, 0xffff, 0x00e7, 0x00e0,
	0x00ef, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0x00ef, 0x00e0, 0x00e7, 0xffff, 0x00f0, 0x00ff, 0x00eb, 0x00f8, 0x00ed, 0x00ee, 0xffff,
	0xffff, 0x00e1, 0x00e2, 0x00f3, 0x00e4, 0x00f5, 0x00f6, 0x00f7, 0x00e8, 0x00f9, 0x00fa,
	0x00fb, 0x00fc, 0x00fd, 0x00fe, 0xffff, 0xffff, 0x00fe, 0x00fd, 0x00e3, 0x00fb, 0x00e5,
	0x00e6, 0x00e8, 0x00f7, 0x00e9, 0x00ea, 0x00e4, 0x00ec, 0x00e2, 0x00e1, 0xffff, 0xffff,
	0x00f1, 0x00f2, 0x00f8, 0x00f4, 0x00ff, 0x00f0, 0xffff, 0x00e7, 0x00e0, 0x00ef, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x002f, 0x0020,
	0x0027, 0xffff, 0x0030, 0x003f, 0x002b, 0x0038, 0x002d, 0x002e, 0xffff, 0xffff, 0x0021,
	0x0022, 0x0033, 0x0024, 0x0035, 0x0036, 0x0037, 0x0028, 0x0039, 0x003a, 0x003b, 0x003c,
	0x003d, 0x003e, 0xffff, 0xffff, 0x003e, 0x003d, 0x0023, 0x003b, 0x0025, 0x0026, 0x0028,
	0x0037, 0x0029, 0x002a, 0x0024, 0x002c, 0x0022, 0x0021, 0xffff, 0xffff, 0x0031, 0x0032,
	0x0038, 0x0034, 0x003f, 0x0030, 0xffff, 0x0027, 0x0020, 0x002f, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x01bc, 0xffff, 0x004f, 0x0040, 0x0047, 0xffff,
	0x0050, 0x005f, 0x004b, 0x0058, 0x004d, 0x004e, 0xffff, 0xffff, 0x0041, 0x0042, 0x0053,
	0x0044, 0x0055, 0x0056, 0x0057, 0x0048, 0x0059, 0x005a, 0x005b, 0x005c, 0x005d, 0x005e,
	0xffff, 0xffff, 0x005e, 0x005d, 0x0043, 0x005b, 0x0045, 0x0046, 0x0048, 0x0057, 0x0049,
	0x004a, 0x0044, 0x004c, 0x0042, 0x0041, 0xffff, 0xffff, 0x0051, 0x0052, 0x0058, 0x0054,
	0x005f, 0x0050, 0xffff, 0x0047, 0x0040, 0x004f, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x008f, 0x0080, 0x0087, 0xffff, 0x0090, 0x009f,
	0x008b, 0x0098, 0x008d, 0x008e, 0xffff, 0xffff, 0x0081, 0x0082, 0x0093, 0x0084, 0x0095,
	0x0096, 0x0097, 0x0088, 0x0099, 0x009a, 0x009b, 0x009c, 0x009d, 0x009e, 0xffff, 0xffff,
	0x009e, 0x009d, 0x0083, 0x009b, 0x0085, 0x0086, 0x0088, 0x0097, 0x0089, 0x008a, 0x0084,
	0x008c, 0x0082, 0x0081, 0xffff, 0xffff, 0x0091, 0x0092, 0x0098, 0x0094, 0x009f, 0x0090,
	0xffff, 0x0087, 0x0080, 0x008f, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0x006f, 0x0060, 0x0067, 0xffff, 0x0070, 0x007f, 0x006b, 0x0078,
	0x006d, 0x006e, 0xffff, 0xffff, 0x0061, 0x0062, 0x0073, 0x0064, 0x0075, 0x0076, 0x0077,
	0x0068, 0x0079, 0x007a, 0x007b, 0x007c, 0x007d, 0x007e, 0xffff, 0xffff, 0x007e, 0x007d,
	0x0063, 0x007b, 0x0065, 0x0066, 0x0068, 0x0077, 0x0069, 0x006a, 0x0064, 0x006c, 0x0062,
	0x0061, 0xffff, 0xffff, 0x0071, 0x0072, 0x0078, 0x0074, 0x007f, 0x0070, 0xffff, 0x0067,
	0x0060, 0x006f, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0x000f, 0x0000, 0x0007, 0xffff, 0x0010, 0x001f, 0x000b, 0x0018, 0x000d, 0x000e,
	0xffff, 0xffff, 0x0001, 0x0002, 0x0013, 0x0004, 0x0015, 0x0016, 0x0017, 0x0008, 0x0019,
	0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0xffff, 0xffff, 0x001e, 0x001d, 0x0003, 0x001b,
	0x0005, 0x0006, 0x0008, 0x0017, 0x0009, 0x000a, 0x0004, 0x000c, 0x0002, 0x0001, 0xffff,
	0xffff, 0x0011, 0x0012, 0x0018, 0x0014, 0x001f, 0x0010, 0xffff, 0x0007, 0x0000, 0x000f,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0x00ef,
	0x00e0, 0x00e7, 0xffff, 0x00f0, 0x00ff, 0x00eb, 0x00f8, 0x00ed, 0x00ee, 0xffff, 0xffff,
	0x00e1, 0x00e2, 0x00f3, 0x00e4, 0x00f5, 0x00f6, 0x00f7, 0x00e8, 0x00f9, 0x00fa, 0x00fb,
	0x00fc, 0x00fd, 0x00fe, 0xffff, 0xffff, 0x00fe, 0x00fd, 0x00e3, 0x00fb, 0x00e5, 0x00e6,
	0x00e8, 0x00f7, 0x00e9, 0x00ea, 0x00e4, 0x00ec, 0x00e2, 0x00e1, 0xffff, 0xffff, 0x00f1,
	0x00f2, 0x00f8, 0x00f4, 0x00ff, 0x00f0, 0xffff, 0x00e7, 0x00e0, 0x00ef, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff, 0xffff,
	0xffff
};

#pragma GCC push_options
#pragma GCC optimize("O3")

size_t forward_splicer_process_asi(struct forward_splicer_io *io, const u8 *hw_in, u8 *hw_out,
				   size_t hw_size)
{
	size_t i, cnt = 0;
	int err_cnt = 0;
	const u8 *inp_hw = hw_in;
	u8 *outp_hw = hw_out;
	int ts_offset = -1;
	size_t rd_pos = io->stream_rd_pos, wr_pos = io->stream_wr_pos;
	size_t buf_size = io->stream_buffer_size;
	int sync_pos[5];
	int sync_count = 0;

	for (i = 0; i < hw_size; i += 5) {
		u64 word = *((u64 *)inp_hw);
		u64 word_out = 0;
		int j;

		for (j = 0; j < 4; j++) {
			u16 dec = t10to8[word & 0x3FF];
			u8 b;
			word >>= 10;

			if (unlikely((dec & 0xFF00) == 0xFF00))
				err_cnt++;

			if (dec & 0x100)
				continue;

			b = dec & 0xFF;

			if (unlikely((b == 0x47) && (sync_count < 5))) {
				sync_pos[sync_count] = cnt;

				if (sync_count == 0)
					sync_count++;
				else if ((sync_pos[sync_count - 1] + 188) == sync_pos[sync_count])
					sync_count++;
				else if ((sync_pos[sync_count - 1] + 204) == sync_pos[sync_count])
					sync_count++;
				else if (sync_count < 2)
					sync_count = 0;
			}

			if (likely(io->in_en)) {
				io->stream_buffer[wr_pos] = b;
				wr_pos++;
				if (unlikely(wr_pos == buf_size))
					wr_pos = 0;
			}

			if (likely(!io->bypass && io->out_en)) {
				b = io->stream_buffer[rd_pos];
				rd_pos++;
				if (unlikely(rd_pos == buf_size))
					rd_pos = 0;
			}

			if (likely(io->bypass || io->out_en))
				word_out |= (u64)(0x100ULL | b) << (j * 10);

			cnt++;
		}
		*((u64 *)outp_hw) = word_out;
		inp_hw = inp_hw + 5;
		outp_hw = outp_hw + 5;
	}

	if (err_cnt != 0)
		forward_warn(io->splicer->dev, "splicer%d: %d errors during decoding!",
			     io->in_index, err_cnt);

	if (!err_cnt && (sync_count >= 5)) {
		io->fec = ((sync_pos[1] - sync_pos[0]) == 204) ? true : false;
		ts_offset = sync_pos[0] % (io->fec ? 204 : 188);
	}

	if (ts_offset >= 0) {
		int ts_wr_delta = 0, ts_rd_delta = 0;
		int psize = io->fec ? 204 : 188;

		if (io->in_en) {
			ts_wr_delta = ((io->stream_wr_pos % psize) + ts_offset) % psize;
			if (ts_wr_delta != 0) {
				forward_warn(io->splicer->dev,
					     "splicer%d: input stream desynced, syncing...",
					     io->in_index);
				io->stream_wr_pos = (io->stream_wr_pos + psize - ts_wr_delta) %
						    io->stream_buffer_size;
			}
		}

		if (io->out_en) {
			ts_rd_delta = ((io->stream_rd_pos % psize) + ts_offset) % psize;
			if (ts_rd_delta != 0) {
				forward_warn(io->splicer->dev,
					     "splicer%d: output stream desynced, syncing...",
					     io->in_index);
				io->stream_rd_pos = (io->stream_rd_pos + psize - ts_rd_delta) %
						    io->stream_buffer_size;
			}
		}
	}

	if (io->in_en)
		io->stream_wr_pos = (io->stream_wr_pos + cnt) % io->stream_buffer_size;

	if (io->out_en)
		io->stream_rd_pos = (io->stream_rd_pos + cnt) % io->stream_buffer_size;

	return (size_t)cnt;
}

void forward_splicer_unpack10b16b(const u8 *in, u8 *out, int words)
{
	int i;
	const u8 *inp = in;
	u16 *outp = (u16 *)out;

	for (i = 0; i < words; i += 4) {
		u64 v = *((u64 *)inp);
		*((u64 *)outp) =
			((v << 0) & 0x00000000000003FFULL) | ((v << 6) & 0x0000000003FF0000ULL) |
			((v << 12) & 0x000003FF00000000ULL) | ((v << 18) & 0x03FF000000000000ULL);
		outp += 4;
		inp += 5;
	}
}

void forward_splicer_pack16b10b(const u8 *in, u8 *out, int words)
{
	int i;
	const u16 *inp = (u16 *)in;
	u8 *outp = out;

	for (i = 0; i < words; i += 4) {
		u64 v = *((u64 *)inp);
		*((u64 *)outp) = ((v >> 0) & 0x00000003FFULL) | ((v >> 6) & 0x00000FFC00ULL) |
				 ((v >> 12) & 0x003FF00000ULL) | ((v >> 18) & 0xFFC0000000ULL);
		outp += 5;
		inp += 4;
	}
}
