wdsp/syncbuffs.c

/*	syncbuffs.c

This file is part of a program that implements a Software-Defined Radio.

Copyright (C) 2018 Warren Pratt, NR0V

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.

The author can be reached by email at

warren@pratt.one

*/

#include "comm.h"

void start_syncbthread (SYNCB a)
{
	HANDLE handle = (HANDLE) _beginthread(syncb_main, 0, (void *)a);
	SetThreadPriority (handle, THREAD_PRIORITY_HIGHEST);
}

SYNCB create_syncbuffs (int accept, int nstreams, int max_insize, int max_outsize, int outsize, double** out, void (*exf)(void))
{
	SYNCB a = (SYNCB) malloc0 (sizeof(syncb));
	int i;
	a->accept = accept;
	a->nstreams = nstreams;
	a->run = 1;
	a->max_in_size = max_insize;
	a->max_outsize = max_outsize;
	a->r1_outsize = outsize;
	a->out = out;
	a->exf = exf;
	if (a->max_outsize > a->max_in_size)
		a->r1_size = a->max_outsize;
	else
		a->r1_size = a->max_in_size;
	a->r1_active_buffsize = SYNCB_MULT * a->r1_size;
	a->r1_baseptr = (double **) malloc0 (a->nstreams * sizeof (double *));
	for (i = 0; i < a->nstreams; i++)
		a->r1_baseptr[i] = (double *) malloc0 (a->r1_active_buffsize * sizeof (complex));
	a->r1_inidx = 0;
	a->r1_outidx = 0;
	a->r1_unqueuedsamps = 0;
	a->Sem_BuffReady = CreateSemaphore(0, 0, 1000, 0);
	InitializeCriticalSectionAndSpinCount ( &a->csIN, 2500 );
	InitializeCriticalSectionAndSpinCount ( &a->csOUT,	2500 );
	start_syncbthread (a);
	return a;
}

void destroy_syncbuffs (SYNCB a)
{
	int i;
	InterlockedBitTestAndReset(&a->accept, 0);		// shut the Syncbound() gate to prevent new infusions
	EnterCriticalSection (&a->csIN);				// wait until the current Inbound() infusion is finished
	EnterCriticalSection (&a->csOUT);				// block the syncb thread before syncbdata()
	Sleep (25);										// wait for the thread to arrive at the top of the syncb_main() loop
	InterlockedBitTestAndReset(&a->run, 0);			// set a trap for the syncb thread
	ReleaseSemaphore(a->Sem_BuffReady, 1, 0);		// be sure the syncb thread can pass WaitForSingleObject in syncb_main() //
	LeaveCriticalSection (&a->csOUT);				// let the thread pass to the trap in syncbdata()
	LeaveCriticalSection (&a->csIN);				
	Sleep (2);										// wait for the syncb thread to die
	DeleteCriticalSection (&a->csOUT);
	DeleteCriticalSection (&a->csIN);
	CloseHandle (a->Sem_BuffReady);
	for (i = 0; i < a->nstreams; i++)
		_aligned_free (a->r1_baseptr[i]);
	_aligned_free (a->r1_baseptr);
	_aligned_free (a);
}

void flush_syncbuffs (SYNCB a)
{
	int i;
	for (i = 0; i < a->nstreams; i++)
		memset (a->r1_baseptr[i], 0, a->r1_active_buffsize * sizeof (complex));
	a->r1_inidx = 0;
	a->r1_outidx = 0;
	a->r1_unqueuedsamps = 0;
	while (!WaitForSingleObject (a->Sem_BuffReady, 1)) ;
}

void Syncbound (SYNCB a, int nsamples, double** in)
{
	int i, n;
	int first, second;

	if (_InterlockedAnd (&a->accept, 1))
	{
		EnterCriticalSection (&a->csIN);
		if (nsamples > (a->r1_active_buffsize - a->r1_inidx))
		{
			first = a->r1_active_buffsize - a->r1_inidx;
			second = nsamples - first;
		}
		else
		{
			first = nsamples;
			second = 0;
		}
		for (i = 0; i < a->nstreams; i++)
		{
			memcpy (a->r1_baseptr[i] + 2 * a->r1_inidx, in[i],			   first  * sizeof (complex));
			memcpy (a->r1_baseptr[i],					in[i] + 2 * first, second * sizeof (complex));
		}
		if ((a->r1_unqueuedsamps += nsamples) >= a->r1_outsize)
		{
			n = a->r1_unqueuedsamps / a->r1_outsize;
			ReleaseSemaphore (a->Sem_BuffReady, n, 0);
			a->r1_unqueuedsamps -= n * a->r1_outsize;
		}
		if ((a->r1_inidx += nsamples) >= a->r1_active_buffsize)
			a->r1_inidx -= a->r1_active_buffsize;
		LeaveCriticalSection (&a->csIN);
	}
}

void syncbdata (SYNCB a)
{
	int i;
	int first, second;
	EnterCriticalSection (&a->csOUT);
	if (!_InterlockedAnd (&a->run, 1))
	{
		LeaveCriticalSection (&a->csOUT);
		_endthread();
	}
	if (a->r1_outsize > (a->r1_active_buffsize - a->r1_outidx))
	{
		first = a->r1_active_buffsize - a->r1_outidx;
		second = a->r1_outsize - first;
	}
	else
	{
		first = a->r1_outsize;
		second = 0;
	}
	for (i = 0; i < a->nstreams; i++)
	{
		memcpy (a->out[i],			   a->r1_baseptr[i] + 2 * a->r1_outidx, first  * sizeof (complex));
		memcpy (a->out[i] + 2 * first, a->r1_baseptr[i],					second * sizeof (complex));
	}
	if ((a->r1_outidx += a->r1_outsize) >= a->r1_active_buffsize)
		a->r1_outidx -= a->r1_active_buffsize;
	LeaveCriticalSection (&a->csOUT);
}

void syncb_main (void *p)
{
	SYNCB a = (SYNCB)p;
	
	while (_InterlockedAnd (&a->run, 1))
	{
		WaitForSingleObject (a->Sem_BuffReady,INFINITE);
		syncbdata (a);
		a->exf();
	}
	_endthread();
}

void SetSYNCBRingOutsize (SYNCB a, int size)
{
	InterlockedBitTestAndReset(&a->accept, 0);		// shut the Syncbound() gate to prevent new infusions
	EnterCriticalSection (&a->csIN);				// wait until the current Syncbound() infusion is finished
	EnterCriticalSection (&a->csOUT);				// block the syncb thread before syncbdata()
	Sleep (25);										// wait for the thread to arrive at the top of the syncb_main() loop
	InterlockedBitTestAndReset(&a->run, 0);			// set a trap for the syncb thread
	ReleaseSemaphore(a->Sem_BuffReady, 1, 0);		// be sure the syncb thread can pass WaitForSingleObject in syncb_main() //
	LeaveCriticalSection (&a->csOUT);				// let the thread pass to the trap in syncbdata()
	Sleep (2);										// wait for the syncb thread to die
	flush_syncbuffs(a);								// restore ring to pristine condition
	a->r1_outsize = size;							// set its new outsize
	InterlockedBitTestAndSet(&a->run,0);			// remove the syncb thread trap
	start_syncbthread(a);							// start the syncb thread
	LeaveCriticalSection (&a->csIN);				// enable Syncbound() processing
	InterlockedBitTestAndSet(&a->accept, 0);		// open the Syncbound() gate
}

/********************************************************************************************************
*																										*
*											Dummy Filter												*
*																										*
********************************************************************************************************/

DUMFILT create_dumfilt (int run, int delay, int opsize, double* in, double* out)
{
	DUMFILT a = (DUMFILT) malloc0 (sizeof (dumfilt));
	a->run = run;
	a->delay = delay;
	a->opsize = opsize;
	a->rsize = a->opsize + a->delay;
	a->in = in;
	a->out = out;
	a->ring = (double *) malloc0 (a->rsize * sizeof (complex));
	a->outidx = 0;
	a->inidx = a->delay;
	return a;
}

void destroy_dumfilt (DUMFILT a)
{
	_aligned_free (a->ring);
	_aligned_free (a);
}

void flush_dumfilt (DUMFILT a)
{
	memset (a->ring, 0, a->rsize * sizeof (complex));
	a->outidx = 0;
	a->inidx = a->delay;
}

void xdumfilt (DUMFILT a)
{
	int first, second;
	if (a->run)
	{
		if (a->opsize > (a->rsize - a->inidx))
		{
			first = a->rsize - a->inidx;
			second = a->opsize - first;
		}
		else
		{
			first = a->opsize;
			second = 0;
		}
		memcpy (a->ring + 2 * a->inidx, a->in,			   first  * sizeof (complex));
		memcpy (a->ring,				a->in + 2 * first, second * sizeof (complex));
		if ((a->inidx += a->opsize) > a->rsize) a->inidx -= a->rsize;
		if (a->opsize > a->rsize - a->outidx)
		{
			first = a->rsize - a->outidx;
			second = a->opsize - first;
		}
		else
		{
			first = a->opsize;
			second = 0;
		}
		memcpy (a->out,				a->ring + 2 * a->outidx, first	* sizeof (complex));
		memcpy (a->out + 2 * first, a->ring,				 second * sizeof (complex));
		if ((a->outidx += a->opsize) > a->rsize) a->outidx -= a->rsize;
	}
}