/*
   forward-alsa-utils.c - v4l2 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-alsa-utils.h"
#include "forward-hanc.h"

#include <linux/string.h>

inline u32 forward_alsa_smpte_272_extract_sample32(uint16_t *udw)
{
	u32 result = 0;

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

	return result;
}

snd_pcm_sframes_t forward_alsa_input_decode_smpte272(struct forward_alsa_io *io, const u16 *buffer,
						     size_t size)
{
	struct forward_hanc_stream s;
	struct snd_pcm_runtime *runtime;
	snd_pcm_sframes_t wr_pointers[FORWARD_ALSA_MAX_SDI_GROUPS * 4];
	struct forward_hanc_smpte_291_header h;
	int sample_counters[FORWARD_ALSA_MAX_SDI_GROUPS * 4];
	snd_pcm_sframes_t max_sample_counter;
	int i;

	if (!io->streaming || !io->stream || !size)
		return 0;

	runtime = io->stream->runtime;

	for (i = 0; i < FORWARD_ALSA_MAX_SDI_GROUPS * 4; i++) {
		wr_pointers[i] = io->position[i / 4] % runtime->buffer_size;
		sample_counters[i] = 0;
	}

	forward_hanc_open(&s, buffer, size, 2);

	for (;;) {
		int group;
		uint16_t data[256];

		if (!forward_hanc_read_smpte_291_header(&s, 0, &h))
			break;

		if ((h.did != 0xFF) && (h.did != 0xFD) && (h.did != 0xFB) && (h.did != 0xF9)) {
			if (!forward_hanc_read_smpte_291_data(&s, 0, &h, 0, 0))
				break;
			continue;
		}
		group = 3 - (h.did - 0xF9) / 2;

		if (!forward_hanc_read_smpte_291_data(&s, 0, &h, data, h.data_count))
			break;

		for (i = 0; i < h.data_count / 3; i++) {
			int ch;
			u32 *p;

			ch = group * 4 + ((data[i * 3] >> 1) & 0x3);

			p = (u32 *)(runtime->dma_area + frames_to_bytes(runtime, wr_pointers[ch])) +
			    ch;

			forward_alsa_ptr_inc(wr_pointers[ch], runtime);
			sample_counters[ch]++;

			if (ch >= (int)runtime->channels)
				continue;

			*p = forward_alsa_smpte_272_extract_sample32(&data[i * 3]);
		}
	}
	forward_hanc_close(&s);

	for (i = 0; i < FORWARD_ALSA_MAX_SDI_GROUPS; i++) {
		io->position[i] += sample_counters[i * 4];
	}

	max_sample_counter = -1;
	for (i = 0; i < FORWARD_ALSA_MAX_SDI_GROUPS * 4; i++)
		if (sample_counters[i] > max_sample_counter)
			max_sample_counter = sample_counters[i];

	return max_sample_counter;
}

static inline u32 forward_alsa_smpte_299_extract_sample32(uint16_t *udw, int ch)
{
	u32 result = 0;

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

	return result;
}

snd_pcm_sframes_t forward_alsa_input_decode_smpte299(struct forward_alsa_io *io, const u16 *buffer,
						     size_t size)
{
	struct forward_hanc_stream s;
	struct snd_pcm_runtime *runtime;
	snd_pcm_sframes_t wr_pointers[FORWARD_ALSA_MAX_SDI_GROUPS];
	struct forward_hanc_smpte_291_header h;
	int sample_counters[4] = { 0, 0, 0, 0 };
	snd_pcm_sframes_t max_sample_counter;
	int i;

	if (!io->streaming || !io->stream || !size)
		return 0;

	runtime = io->stream->runtime;

	for (i = 0; i < FORWARD_ALSA_MAX_SDI_GROUPS; i++)
		wr_pointers[i] = io->position[i] % runtime->buffer_size;

	forward_hanc_open(&s, buffer, size, 2);
	for (;;) {
		int group;
		uint16_t data[32];

		if (!forward_hanc_read_smpte_291_header(&s, 0, &h))
			break;

		if ((h.did > 0xE7) || (h.did < 0xE4)) {
			if (!forward_hanc_read_smpte_291_data(&s, 0, &h, 0, 0))
				break;
			continue;
		}
		group = 3 - (h.did - 0xE4);

		if (!forward_hanc_read_smpte_291_data(&s, 0, &h, data, 32))
			break;

		for (i = 0; i < 4; i++) {
			int ch = group * 4 + i;
			u32 *p = (u32 *)(runtime->dma_area +
					 frames_to_bytes(runtime, wr_pointers[group])) +
				 ch;

			if (ch >= runtime->channels)
				break;

			*p = forward_alsa_smpte_299_extract_sample32(data, i);
		}

		forward_alsa_ptr_inc(wr_pointers[group], runtime);

		sample_counters[group]++;
	}
	forward_hanc_close(&s);

	for (i = 0; i < FORWARD_ALSA_MAX_SDI_GROUPS; i++) {
		io->position[i] += sample_counters[i];
	}

	max_sample_counter = -1;
	for (i = 0; i < 4; i++)
		if (sample_counters[i] > max_sample_counter)
			max_sample_counter = sample_counters[i];

	return max_sample_counter;
}

snd_pcm_sframes_t forward_alsa_input_decode_hdmi(struct forward_alsa_io *io, const uint8_t *buf,
						 size_t size)
{
	struct snd_pcm_runtime *runtime;
	snd_pcm_uframes_t wr_pointer;
	snd_pcm_uframes_t sample_count = 0;
	int i, j;
	u32 header;
	bool layout;
	int subpkts;
	uint32_t max = 0;

	if (!io->streaming || !io->stream || !size)
		return 0;

	runtime = io->stream->runtime;
	wr_pointer = (io->position[0] % runtime->buffer_size);

	for (i = 0; i < size; i += 36) {
		if (buf[i + 32] != 0x2)
			continue;

		header = le32_to_cpu(*((u32 *)&buf[i + 32]));
		layout = header & (1 << 12);
		subpkts = ((header >> 8) & 0x1) + ((header >> 9) & 0x1) + ((header >> 10) & 0x1) +
			  ((header >> 11) & 0x1);

		for (j = 0; j < subpkts; j++) {
			const uint8_t *p;
			uint32_t *wp;
			uint32_t sample_l, sample_r;

			p = &buf[i + j];
			wp = (u32 *)(runtime->dma_area + frames_to_bytes(runtime, wr_pointer));

			if (header & (1 << (j + 16))) {
				sample_l = 0;
				sample_r = 0;
			} else {
				sample_l = ((uint32_t)p[0] << 8) | ((uint32_t)p[4] << 16) |
					   ((uint32_t)p[8] << 24);
				sample_r = ((uint32_t)p[12] << 8) | ((uint32_t)p[16] << 16) |
					   ((uint32_t)p[20] << 24);
			}
			if (sample_l > max)
				max = sample_l;
			if (sample_r > max)
				max = sample_r;

			if (layout) {
				if ((j * 2) < runtime->channels)
					wp[j * 2] = sample_l;
				if ((j * 2 + 1) < runtime->channels)
					wp[j * 2 + 1] = sample_r;
			} else {
				wp[0] = sample_l;
				if (runtime->channels > 1)
					wp[1] = sample_r;

				forward_alsa_ptr_inc(wr_pointer, runtime);
				sample_count++;
			}
		}

		if (layout) {
			forward_alsa_ptr_inc(wr_pointer, runtime);
			sample_count++;
		}
	}
	io->position[0] += sample_count;

	return sample_count;
}

static snd_pcm_sframes_t forward_alsa_s24be_to_s32(struct forward_alsa_io *io, const uint8_t *buf,
						   size_t size, unsigned int channels)
{
	struct snd_pcm_runtime *runtime = io->stream->runtime;
	snd_pcm_uframes_t wr_pointer = (io->position[0] % runtime->buffer_size);
	snd_pcm_sframes_t counter = 0;
	int i, ch;

	if (!channels)
		return 0;

	for (i = 0; i < size; i += 3 * channels) {
		s32 *op = (s32 *)(runtime->dma_area + frames_to_bytes(runtime, wr_pointer));
		int rch = min(runtime->channels, channels);

		for (ch = 0; ch < rch; ch++) {
			*op = ((s32)buf[i + 3 * ch + 0] << 24) | ((s32)buf[i + 3 * ch + 1] << 16) |
			      ((s32)buf[i + 3 * ch + 2] << 8);
			op++;
		}

		forward_alsa_ptr_inc(wr_pointer, runtime);
		counter++;
	}

	io->position[0] += counter;

	return counter;
}

snd_pcm_sframes_t forward_alsa_input_decode_rfc3190(struct forward_alsa_io *io, const uint8_t *buf,
						    size_t size, u8 pt, bool *last)
{
	struct snd_pcm_runtime *runtime;
	snd_pcm_uframes_t wr_pointer;
	snd_pcm_uframes_t sample_count = 0;
	int i = 0;
	int pkt_counter = 0;

	if (!io->streaming || !io->stream || !size)
		return 0;

	runtime = io->stream->runtime;
	wr_pointer = (io->position[0] % runtime->buffer_size);

	while ((i + 16) <= size) {
		u64 h1 = *((u64 *)&buf[i]);
		u64 h2 = *((u64 *)&buf[i + 8]);
		u16 psize = (h2 >> 32) & 0xFFFF;
		u8 rxtag = (h2 >> 48) & 0x1F;

		*last = (h2 >> 63) & 0x1;

		if (rxtag != FORWARD_ALSA_RFC3190_TAG) {
			i += 16;
			continue;
		}

		if ((h1 & 0x0000000000007FC0ULL) != (((u64)pt << 8) | 0x80ULL)) {
			i += (psize + 16 + 15) & (~15);
			continue;
		}

		psize = min((size_t)psize, (size_t)(size - (i + 16)));

		if (io->hw_format == SNDRV_PCM_FORMAT_S24) {
			sample_count +=
				forward_alsa_s24be_to_s32(io, &buf[i + 16], psize, io->hw_channels);
		}

		i += (psize + 16 + 15) & (~15);
		pkt_counter++;

		if (*last)
			break;
	}

	return sample_count;
}

snd_pcm_sframes_t forward_alsa_output_encode_raw(struct forward_alsa_io *io,
						 snd_pcm_sframes_t delta)
{
	struct snd_pcm_runtime *runtime = io->stream->runtime;
	int groups = (runtime->channels - 1) / 4 + 1;
	u8 *inp, *outp;
	int i, j;

	inp = (u8 *)runtime->dma_area +
	      frames_to_bytes(runtime, (io->sw_rd_position % runtime->buffer_size));
	outp = (u8 *)io->hw_buffer + io->hw_wr_position * groups * 4 * 4;

	for (i = 0; i < delta; i++) {
		for (j = 0; j < groups * 4; j++) {
			if (j >= runtime->channels)
				((u32 *)outp)[j] = 0;
			else
				((u32 *)outp)[j] = ((u32 *)inp)[j] >> 8;
		}

		outp += groups * 4 * 4;
		if ((outp - (u8 *)io->hw_buffer) >= io->hw_buffer_size)
			outp -= io->hw_buffer_size;
		inp += frames_to_bytes(runtime, 1);
		if (bytes_to_frames(runtime, inp - (u8 *)runtime->dma_area) >= runtime->buffer_size)
			inp -= frames_to_bytes(runtime, runtime->buffer_size);
	}

	return delta;
}

snd_pcm_sframes_t forward_alsa_output_encode_raw24be(struct forward_alsa_io *io,
						     snd_pcm_sframes_t delta)
{
	struct snd_pcm_runtime *runtime = io->stream->runtime;
	int out_ch = io->hw_channels;
	u8 *inp, *outp;
	int i, j;

	inp = (u8 *)runtime->dma_area +
	      frames_to_bytes(runtime, (io->sw_rd_position % runtime->buffer_size));
	outp = (u8 *)io->hw_buffer + io->hw_wr_position * out_ch * 3;

	for (i = 0; i < delta; i++) {
		for (j = 0; j < out_ch; j++) {
			if (j >= runtime->channels) {
				outp[j * 3 + 0] = 0;
				outp[j * 3 + 1] = 0;
				outp[j * 3 + 2] = 0;
			} else {
				u32 v = ((u32 *)inp)[j];
				outp[j * 3 + 0] = (v >> 24) & 0xFF;
				outp[j * 3 + 1] = (v >> 16) & 0xFF;
				outp[j * 3 + 2] = (v >> 8) & 0xFF;
			}
		}

		outp += out_ch * 3;
		if ((outp - (u8 *)io->hw_buffer) >= io->hw_buffer_size)
			outp -= io->hw_buffer_size;
		inp += frames_to_bytes(runtime, 1);
		if (bytes_to_frames(runtime, inp - (u8 *)runtime->dma_area) >= runtime->buffer_size)
			inp -= frames_to_bytes(runtime, runtime->buffer_size);
	}

	return delta;
}
