2018-03-16 14:03:06 +00:00
/*
Copyright ( C ) 2018 Evariste COURJAUD F5OEO
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 3 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 , see < http : //www.gnu.org/licenses/>.
*/
# include "stdio.h"
# include "iqdmasync.h"
2018-03-21 09:44:43 +00:00
# include <unistd.h>
# include <sched.h>
2019-01-08 14:05:15 +00:00
# include "util.h"
2018-03-16 14:03:06 +00:00
2018-11-01 00:36:40 +00:00
iqdmasync : : iqdmasync ( uint64_t TuneFrequency , uint32_t SR , int Channel , uint32_t FifoSize , int Mode ) : bufferdma ( Channel , FifoSize , 4 , 3 )
2018-03-16 14:03:06 +00:00
{
// Usermem :
// FRAC frequency
// PAD Amplitude
// FSEL for amplitude 0
2018-11-01 00:36:40 +00:00
ModeIQ = Mode ;
2018-03-21 09:44:43 +00:00
SampleRate = SR ;
2018-03-16 14:03:06 +00:00
tunefreq = TuneFrequency ;
clkgpio : : SetAdvancedPllMode ( true ) ;
clkgpio : : SetCenterFrequency ( TuneFrequency , SampleRate ) ; // Write Mult Int and Frac : FixMe carrier is already there
clkgpio : : SetFrequency ( 0 ) ;
clkgpio : : enableclk ( 4 ) ;
syncwithpwm = false ;
if ( syncwithpwm )
{
pwmgpio : : SetPllNumber ( clk_plld , 1 ) ;
pwmgpio : : SetFrequency ( SampleRate ) ;
}
else
{
pcmgpio : : SetPllNumber ( clk_plld , 1 ) ;
pcmgpio : : SetFrequency ( SampleRate ) ;
}
mydsp . samplerate = SampleRate ;
2019-01-08 14:05:15 +00:00
Originfsel = clkgpio : : gengpio . gpioreg [ GPFSEL0 ] ;
2018-03-16 14:03:06 +00:00
SetDmaAlgo ( ) ;
// Note : Spurious are at +/-(19.2MHZ/2^20)*Div*N : (N=1,2,3...) So we need to have a big div to spurious away BUT
// Spurious are ALSO at +/-(19.2MHZ/2^20)*(2^20-Div)*N
// Max spurious avoid is to be in the center ! Theory shoud be that spurious are set away at 19.2/2= 9.6Mhz ! But need to get account of div of PLLClock
}
iqdmasync : : ~ iqdmasync ( )
{
2019-01-08 14:05:15 +00:00
clkgpio : : gengpio . gpioreg [ GPFSEL0 ] = Originfsel ;
2018-03-16 14:03:06 +00:00
clkgpio : : disableclk ( 4 ) ;
}
void iqdmasync : : SetPhase ( bool inversed )
{
clkgpio : : SetPhase ( inversed ) ;
}
void iqdmasync : : SetDmaAlgo ( )
{
dma_cb_t * cbp = cbarray ;
for ( uint32_t samplecnt = 0 ; samplecnt < buffersize ; samplecnt + + )
{
2019-01-06 10:51:52 +00:00
SetEasyCB ( cbp , samplecnt * registerbysample + 1 , dma_pad , 1 ) ;
2018-03-16 14:03:06 +00:00
cbp + + ;
2018-08-06 09:28:40 +00:00
//@2 Write a frequency sample : Order of DMA CS influence maximum rate : here 0,2,1 is the best : why !!!!!!
2019-01-06 10:51:52 +00:00
SetEasyCB ( cbp , samplecnt * registerbysample , dma_pllc_frac , 1 ) ;
2018-08-06 09:28:40 +00:00
cbp + + ;
2018-03-16 14:03:06 +00:00
//@1
//Set Amplitude to FSEL for amplitude=0
2019-01-06 10:51:52 +00:00
SetEasyCB ( cbp , samplecnt * registerbysample + 2 , dma_fsel , 1 ) ;
2018-03-16 14:03:06 +00:00
cbp + + ;
2019-01-06 10:51:52 +00:00
2018-03-16 14:03:06 +00:00
//@3 Delay
2019-01-06 10:51:52 +00:00
SetEasyCB ( cbp , samplecnt * registerbysample , syncwithpwm ? dma_pwm : dma_pcm , 1 ) ;
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"cbp : sample %x src %x dest %x next %x\n",samplecnt,cbp->src,cbp->dst,cbp->next);
2018-03-16 14:03:06 +00:00
cbp + + ;
}
cbp - - ;
cbp - > next = mem_virt_to_phys ( cbarray ) ; // We loop to the first CB
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"Last cbp : src %x dest %x next %x\n",cbp->src,cbp->dst,cbp->next);
2018-03-16 14:03:06 +00:00
}
2018-03-22 22:48:49 +00:00
void iqdmasync : : SetIQSample ( uint32_t Index , std : : complex < float > sample , int Harmonic )
2018-03-16 14:03:06 +00:00
{
Index = Index % buffersize ;
mydsp . pushsample ( sample ) ;
/*if(mydsp.frequency>2250) mydsp.frequency=2250;
if ( mydsp . frequency < 1000 ) mydsp . frequency = 1000 ; */
2018-03-22 22:58:05 +00:00
sampletab [ Index * registerbysample ] = ( 0x5A < < 24 ) | GetMasterFrac ( mydsp . frequency / Harmonic ) ; //Frequency
2018-08-06 09:28:40 +00:00
int IntAmplitude = ( int ) ( mydsp . amplitude * 8.0 ) - 1 ; //Fixme 1e4 seems to work with SSB but should be an issue with classical IQ file
2018-03-16 14:03:06 +00:00
2018-11-01 14:25:23 +00:00
int IntAmplitudePAD = IntAmplitude ;
2018-03-16 14:03:06 +00:00
if ( IntAmplitude > 7 ) IntAmplitudePAD = 7 ;
2018-11-01 14:25:23 +00:00
if ( IntAmplitude < 0 ) { IntAmplitudePAD = 0 ; IntAmplitude = - 1 ; }
2018-03-16 14:03:06 +00:00
sampletab [ Index * registerbysample + 1 ] = ( 0x5A < < 24 ) + ( IntAmplitudePAD & 0x7 ) + ( 1 < < 4 ) + ( 0 < < 3 ) ; // Amplitude PAD
//sampletab[Index*registerbysample+2]=(Originfsel & ~(7 << 12)) | (4 << 12); //Alternate is CLK
if ( IntAmplitude = = - 1 )
{
sampletab [ Index * registerbysample + 2 ] = ( Originfsel & ~ ( 7 < < 12 ) ) | ( 0 < < 12 ) ; //Pin is in -> Amplitude 0
}
else
{
2018-09-03 14:01:17 +00:00
sampletab [ Index * registerbysample + 2 ] = ( Originfsel & ~ ( 7 < < 12 ) ) | ( 4 < < 12 ) ; //Alternate is CLK : Fixme : do not work with clk2
2018-03-16 14:03:06 +00:00
}
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"amp%f %d\n",mydsp.amplitude,IntAmplitudePAD);
2018-03-16 14:03:06 +00:00
PushSample ( Index ) ;
}
2018-11-01 00:36:40 +00:00
void iqdmasync : : SetFreqAmplitudeSample ( uint32_t Index , std : : complex < float > sample , int Harmonic )
{
Index = Index % buffersize ;
sampletab [ Index * registerbysample ] = ( 0x5A < < 24 ) | GetMasterFrac ( sample . real ( ) / Harmonic ) ; //Frequency
2018-11-01 14:25:23 +00:00
int IntAmplitude = ( int ) roundf ( sample . imag ( ) ) - 1 ; //0->8 become -1->7
2018-11-01 00:36:40 +00:00
2018-11-01 14:25:23 +00:00
int IntAmplitudePAD = IntAmplitude ;
2018-11-01 00:36:40 +00:00
if ( IntAmplitude > 7 ) IntAmplitudePAD = 7 ;
2018-11-01 14:25:23 +00:00
if ( IntAmplitude < 0 ) { IntAmplitudePAD = 0 ; IntAmplitude = - 1 ; }
2018-11-01 00:36:40 +00:00
sampletab [ Index * registerbysample + 1 ] = ( 0x5A < < 24 ) + ( IntAmplitudePAD & 0x7 ) + ( 1 < < 4 ) + ( 0 < < 3 ) ; // Amplitude PAD
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"amp%d PAD %d\n",IntAmplitude,IntAmplitudePAD);
2018-11-01 14:25:23 +00:00
2018-11-01 00:36:40 +00:00
//sampletab[Index*registerbysample+2]=(Originfsel & ~(7 << 12)) | (4 << 12); //Alternate is CLK
if ( IntAmplitude = = - 1 )
{
sampletab [ Index * registerbysample + 2 ] = ( Originfsel & ~ ( 7 < < 12 ) ) | ( 0 < < 12 ) ; //Pin is in -> Amplitude 0
}
else
{
sampletab [ Index * registerbysample + 2 ] = ( Originfsel & ~ ( 7 < < 12 ) ) | ( 4 < < 12 ) ; //Alternate is CLK : Fixme : do not work with clk2
}
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"amp%f %d\n",mydsp.amplitude,IntAmplitudePAD);
2018-11-01 00:36:40 +00:00
PushSample ( Index ) ;
}
2018-03-22 22:48:49 +00:00
void iqdmasync : : SetIQSamples ( std : : complex < float > * sample , size_t Size , int Harmonic = 1 )
2018-03-21 09:44:43 +00:00
{
size_t NbWritten = 0 ;
int OSGranularity = 100 ;
2018-08-06 09:28:40 +00:00
long int start_time ;
long time_difference = 0 ;
struct timespec gettime_now ;
int debug = 1 ;
2018-03-21 09:44:43 +00:00
while ( NbWritten < Size )
{
2018-08-06 09:28:40 +00:00
if ( debug > 0 )
{
clock_gettime ( CLOCK_REALTIME , & gettime_now ) ;
start_time = gettime_now . tv_nsec ;
}
2018-03-21 09:44:43 +00:00
int Available = GetBufferAvailable ( ) ;
2018-08-06 09:28:40 +00:00
//printf("Available before=%d\n",Available);
int TimeToSleep = 1e6 * ( ( int ) buffersize * 3 / 4 - Available ) / ( float ) SampleRate /*-OSGranularity*/ ; // Sleep for theorically fill 3/4 of Fifo
2018-03-21 09:44:43 +00:00
if ( TimeToSleep > 0 )
{
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"buffer size %d Available %d SampleRate %d Sleep %d\n",buffersize,Available,SampleRate,TimeToSleep);
2018-03-21 09:44:43 +00:00
usleep ( TimeToSleep ) ;
}
else
{
2019-01-06 11:51:02 +00:00
dbg_printf ( 1 , " No Sleep %d \n " , TimeToSleep ) ;
2018-08-06 09:28:40 +00:00
//sched_yield();
}
if ( debug > 0 )
{
clock_gettime ( CLOCK_REALTIME , & gettime_now ) ;
time_difference = gettime_now . tv_nsec - start_time ;
if ( time_difference < 0 ) time_difference + = 1E9 ;
2019-01-06 11:51:02 +00:00
//dbg_printf(1,"Available %d Measure samplerate=%d\n",GetBufferAvailable(),(int)((GetBufferAvailable()-Available)*1e9/time_difference));
2018-08-06 09:28:40 +00:00
debug - - ;
2018-03-21 09:44:43 +00:00
}
Available = GetBufferAvailable ( ) ;
2018-08-06 09:28:40 +00:00
2018-03-21 09:44:43 +00:00
int Index = GetUserMemIndex ( ) ;
int ToWrite = ( ( int ) Size - ( int ) NbWritten ) < Available ? Size - NbWritten : Available ;
2018-08-06 09:28:40 +00:00
//printf("Available after=%d Timetosleep %d To Write %d\n",Available,TimeToSleep,ToWrite);
2018-11-01 00:36:40 +00:00
if ( ModeIQ = = MODE_IQ )
2018-03-21 09:44:43 +00:00
{
2018-11-01 00:36:40 +00:00
for ( int i = 0 ; i < ToWrite ; i + + )
{
SetIQSample ( Index + i , sample [ NbWritten + + ] , Harmonic ) ;
}
}
if ( ModeIQ = = MODE_FREQ_A )
{ for ( int i = 0 ; i < ToWrite ; i + + )
{
SetFreqAmplitudeSample ( Index + i , sample [ NbWritten + + ] , Harmonic ) ;
}
2018-03-21 09:44:43 +00:00
}
}
}
2018-03-16 14:03:06 +00:00
2018-03-22 22:48:49 +00:00