static void
init_ctrl_data(void)
{
dprintf("Initializing DMA ...\n");
struct ctl *ctl = (struct ctl *)mbox.virt_addr;
dma_cb_t *cbp = ctl->cb;
uint32_t phys_fifo_addr;
uint32_t phys_gpclr0 = GPIO_PHYS_BASE + 0x28;
uint32_t phys_gpset0 = GPIO_PHYS_BASE + 0x1c;
uint32_t mask;
int i;
if (delay_hw == DELAY_VIA_PWM)
phys_fifo_addr = PWM_PHYS_BASE + 0x18;
else
phys_fifo_addr = PCM_PHYS_BASE + 0x04;
memset(ctl->sample, 0, sizeof(ctl->sample));
// Calculate a mask to turn off all the servos
mask = 0;
for (i = 0; i < num_channels; i++){
mask |= 1 << known_pins[i];
}
for (i = 0; i < NUM_SAMPLES; i++)
ctl->sample[i] = mask;
/* Initialize all the DMA commands. They come in pairs.
* - 1st command copies a value from the sample memory to a destination
* address which can be either the gpclr0 register or the gpset0 register
* - 2nd command waits for a trigger from an external source (PWM or PCM)
*/
for (i = 0; i < NUM_SAMPLES; i++) {
/* First DMA command */
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP;
cbp->src = mem_virt_to_phys(ctl->sample + i);
if (invert_mode)
cbp->dst = phys_gpset0;
else
cbp->dst = phys_gpclr0;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
/* Second DMA command */
if (delay_hw == DELAY_VIA_PWM)
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5);
else
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(2);
cbp->src = mem_virt_to_phys(ctl); // Any data will do
cbp->dst = phys_fifo_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
}
cbp--;
cbp->next = mem_virt_to_phys(ctl->cb);
}
static void
init_hardware(void)
{
dprintf("Initializing PWM/PCM HW...\n");
struct ctl *ctl = (struct ctl *)mbox.virt_addr;
if (delay_hw == DELAY_VIA_PWM) {
// Initialise PWM
pwm_reg[PWM_CTL] = 0;
udelay(10);
clk_reg[PWMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz)
udelay(100);
clk_reg[PWMCLK_DIV] = 0x5A000000 | (500<<12); // set pwm div to 500, giving 1MHz
udelay(100);
clk_reg[PWMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable
udelay(100);
pwm_reg[PWM_RNG1] = SAMPLE_US;
udelay(10);
pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
udelay(10);
pwm_reg[PWM_CTL] = PWMCTL_CLRF;
udelay(10);
pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1;
udelay(10);
} else {
// Initialise PCM
pcm_reg[PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
udelay(100);
clk_reg[PCMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz)
udelay(100);
clk_reg[PCMCLK_DIV] = 0x5A000000 | (500<<12); // Set pcm div to 500, giving 1MHz
udelay(100);
clk_reg[PCMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable
udelay(100);
pcm_reg[PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits
udelay(100);
pcm_reg[PCM_MODE_A] = (SAMPLE_US - 1) << 10;
udelay(100);
pcm_reg[PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs
udelay(100);
pcm_reg[PCM_DREQ_A] = 64<<24 | 64<<8; // DMA Req when one slot is free?
udelay(100);
pcm_reg[PCM_CS_A] |= 1<<9; // Enable DMA
udelay(100);
}
// Initialise the DMA
dma_reg[DMA_CS] = DMA_RESET;
udelay(10);
dma_reg[DMA_CS] = DMA_INT | DMA_END;
dma_reg[DMA_CONBLK_AD] = mem_virt_to_phys(ctl->cb);
dma_reg[DMA_DEBUG] = 7; // clear debug error flags
dma_reg[DMA_CS] = 0x10880001; // go, mid priority, wait for outstanding writes
if (delay_hw == DELAY_VIA_PCM) {
pcm_reg[PCM_CS_A] |= 1<<2; // Enable Tx
}
}
static void
debug_dump_hw(void)
{
#ifdef DEBUG
printf("pwm_reg: %p\n", (void *) pwm_reg);
int i;
struct ctl *ctl = (struct ctl *)mbox.virt_addr;
dma_cb_t *cbp = ctl->cb;
i = 0;
for (i = 0; i < NUM_SAMPLES; i++) {
printf("DMA Control Block: #%d @0x%08x, \n", i, cbp);
printf("info: 0x%08x\n", cbp->info);
printf("src: 0x%08x\n", cbp->src);
printf("dst: 0x%08x\n", cbp->dst);
printf("length: 0x%08x\n", cbp->length);
printf("stride: 0x%08x\n", cbp->stride);
printf("next: 0x%08x\n", cbp->next);
cbp++; // next control block
}
printf("PWM_BASE: %p\n", (void *) PWM_BASE);
printf("pwm_reg: %p\n", (void *) pwm_reg);
for (i=0; i<PWM_LEN/4; i++) {
printf("%04x: 0x%08x 0x%08x\n", i, &pwm_reg[i], pwm_reg[i]);
}
printf("CLK_BASE: %p\n", (void *) CLK_BASE);
printf("PWMCLK_CNTL: %x\n", PWMCLK_CNTL);
printf("clk_reg[PWMCLK_CNTL]: %p\n", &clk_reg[PWMCLK_CNTL]);
printf("PWMCLK_DIV: %x\n", PWMCLK_DIV);
printf("clk_reg: %p\n", (void *) clk_reg);
printf("virt_to_phys(clk_reg): %x\n", mem_virt_to_phys(clk_reg));
for (i=0; i<CLK_LEN/4; i++) {
printf("%04x: 0x%08x 0x%08x\n", i, &clk_reg[i], clk_reg[i]);
}
printf("DMA_BASE: %p\n", (void *) DMA_BASE);
printf("dma_virt_base: %p\n", (void *) dma_virt_base);
printf("dma_reg: %p\n", (void *) dma_reg);
printf("virt_to_phys(dma_reg): %x\n", mem_virt_to_phys(dma_reg));
for (i=0; i<DMA_CHAN_SIZE/4; i++) {
printf("%04x: 0x%08x 0x%08x\n", i, &dma_reg[i], dma_reg[i]);
}
#endif
}
int SetupGpioClock(uint32_t SymbolRate,double TuningFrequency)
{
char MASH=1;
if(UsePCMClk==0) TuningFrequency=TuningFrequency*2;
if((TuningFrequency>=100e6)&&(TuningFrequency<=150e6))
{
MASH=2;
}
if(TuningFrequency<100e6)
{
MASH=3;
}
printf("MASH %d Freq PLL# %d\n",MASH,PllNumber);
Originfsel=gpio_reg[GPFSEL0]; // Warning carefull if FSEL is used after !!!!!!!!!!!!!!!!!!!!
if(UsePCMClk==1)
gpio_reg[GPFSEL0] = (Originfsel & ~(7 << 12)) | (4 << 12); //ENABLE CLOCK ON GPIO CLK
/*
gpio_reg[GPFSEL0] = (gpio_reg[GPFSEL0] & ~(7 << 12)) | (4 << 12); //ENABLE CLOCK ON GPIO CLK
usleep(1000);
clk_reg[GPCLK_CNTL] = 0x5A << 24 |0<<4 | PLL_1GHZ;
usleep(1000);
clk_reg[GPCLK_CNTL] = 0x5A << 24 | MASH << 9 | 1 << 4 | PLL_1GHZ; // MASH 1
*/
// ------------------- MAKE MAX OUTPUT CURRENT FOR GPIO -----------------------
char OutputPower=7;
pad_gpios_reg[PADS_GPIO_0] = 0x5a000000 + (OutputPower&0x7) + (1<<4) + (0<<3); // Set output power for I/Q GPIO18/GPIO19
//------------------- Init PCM ------------------
pcm_reg[PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
udelay(100);
clk_reg[PCMCLK_CNTL] = 0x5A000000|PLL_1GHZ; // Source=PLLC (1GHHz) STOP PLL
udelay(1000);
static uint32_t FreqDividerPCM;
static uint32_t FreqFractionnalPCM;
int NbStepPCM = 25; // Should not exceed 1000 :
FreqDividerPCM=(int) ((double)PllFreq1GHZ/(SymbolRate*NbStepPCM/**PwmNumberStep*/));
FreqFractionnalPCM=4096.0 * (((double)PllFreq1GHZ/(SymbolRate*NbStepPCM/**PwmNumberStep*/))-FreqDividerPCM);
//FreqDividerPCM=(int) ((double)PllFreq1GHZ/(1000000L*NbStepPCM/**PwmNumberStep*/)); //SET TO 10MHZ = 100ns
//FreqFractionnalPCM=4096.0 * (((double)PllFreq1GHZ/(1000000L*NbStepPCM/**PwmNumberStep*/))-FreqDividerPCM);
printf("SampleRate=%d\n",SymbolRate);
if((FreqDividerPCM>4096)||(FreqDividerPCM<2)) printf("Warning : SampleRate is not valid\n");
clk_reg[PCMCLK_DIV] = 0x5A000000 | ((FreqDividerPCM)<<12) | FreqFractionnalPCM;
udelay(1000);
//printf("Div PCM %d FracPCM %d\n",FreqDividerPCM,FreqFractionnalPCM);
DelayFromSampleRate=(1e9/(SymbolRate));
//printf("Delay PCM (min step)=%d\n",DelayFromSampleRate);
//uint32_t DelayFromSampleRate=(1000000.0-2080.0)/27.4425;
//uint32_t DelayFromSampleRate=1000000;
//uint32_t DelayFromSampleRate=round((1000000-(2300))/27.4425);
//TimeDelay=0 : 2.08us
//TimeDelay=10000 : 280us
//TimdeDelay=100000 :2774us
//TimdeDelay=1000000 : 27400 us
//uint32_t DelayFromSampleRate=5000;
//printf("DelaySample %d Delay %d\n",DelayFromSampleRate,(int)(27.4425*DelayFromSampleRate+2300));
pcm_reg[PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits
udelay(100);
//printf("Nb PCM STEP (<1000):%d\n",NbStepPCM);
pcm_reg[PCM_MODE_A] = (NbStepPCM-1)<<10; // SHOULD NOT EXCEED 1000 !!!
udelay(100);
pcm_reg[PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs
udelay(100);
pcm_reg[PCM_DREQ_A] = 64<<24 | /*64<<8 |*/ 64<<8 ; //TX Fifo PCM=64 DMA Req when one slot is free?
udelay(100);
pcm_reg[PCM_CS_A] |= 1<<9; // Enable DMA
udelay(1000);
clk_reg[PCMCLK_CNTL] = 0x5A000010 |(1 << 9)| PLL_1GHZ /*PLL_1GHZ*/; // Source=PLLC and enable
udelay(100);
pcm_reg[PCM_CS_A] |= 1<<2; //START TX PCM
// FIN PCM
//INIT PWM in Serial Mode
if(UsePCMClk==0)
{
gpioSetMode(18, 2); /* set to ALT5, PWM1 : RF On PIN */
pwm_reg[PWM_CTL] = 0;
clk_reg[PWMCLK_CNTL] = 0x5A000000 | (MASH << 9) |PllNumber/*PLL_1GHZ*/ ;
udelay(300);
clk_reg[PWMCLK_DIV] = 0x5A000000 | (2<<12); //WILL BE UPDATED BY DMA
udelay(300);
clk_reg[PWMCLK_CNTL] = 0x5A000010 | (MASH << 9) | PllNumber /*PLL_1GHZ*/; //MASH3 : A TESTER SI MIEUX en MASH1
//MASH 3 doesnt seem work above 80MHZ, back to MASH1
pwm_reg[PWM_RNG1] = 32;// 250 -> 8KHZ
udelay(100);
pwm_reg[PWM_RNG2] = 32;// 32 Mandatory for Serial Mode without gap
pwm_reg[PWM_FIFO]=0xAAAAAAAA;
pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
udelay(100);
pwm_reg[PWM_CTL] = PWMCTL_CLRF;
udelay(100);
pwm_reg[PWM_CTL] = PWMCTL_USEF1| PWMCTL_MODE1| PWMCTL_PWEN1|PWMCTL_RPTL1; //PWM0 in Repeat mode
}
// FIN INIT PWM
//******************* INIT CLK MODE
if(UsePCMClk==1)
{
clk_reg[GPCLK_CNTL] = 0x5A000000 | (MASH << 9) |PllNumber/*PLL_1GHZ*/ ;
udelay(300);
clk_reg[GPCLK_DIV] = 0x5A000000 | (2<<12); //WILL BE UPDATED BY DMA !! CAREFUL NOT DIVIDE BY 2 LIKE PWM
udelay(300);
clk_reg[GPCLK_CNTL] = 0x5A000010 | (MASH << 9) | PllNumber /*PLL_1GHZ*/; //MASH3 : A TESTER SI MIEUX en MASH1
}
ctl = (struct control_data_s *)virtbase; // Struct ctl is mapped to the memory allocated by RpiDMA (Mailbox)
dma_cb_t *cbp = ctl->cb;
uint32_t phys_pwm_fifo_addr = 0x7e20c000 + 0x18;//PWM Fifo
uint32_t phys_pwm_range_addr = 0x7e20c000 + 0x10;//PWM Range
uint32_t phys_clock_div_addr = 0x7e101074;//CLOCK Frequency Setting
uint32_t phys_pwm_clock_div_addr = 0x7e1010a4; //CLK PWM
uint32_t phys_gpio_set_addr = 0x7e20001c;
uint32_t phys_gpio_clear_addr = 0x7e200028;
uint32_t dummy_gpio = 0x7e20b000;
uint32_t phys_pcm_fifo_addr = 0x7e203004;
uint32_t phys_gpio_pads_addr =0x7e10002c;
uint32_t phys_pcm_clock = 0x7e10109c ;
uint32_t phys_pcm_clock_div_addr = 0x7e10109c;//CLOCK Frequency Setting
uint32_t phys_DmaPwmfRegCtrl = 0x7e007000+DMA_CHANNEL_PWMFREQUENCY*0x100+0x4;
uint32_t phys_gpfsel = 0x7E200000 ;
int samplecnt;
gpioSetMode(27,1); // OSCILO SUR PIN 27
ctl->GpioDebugSampleRate=1<<27; // GPIO 27/ PIN 13 HEADER IS DEBUG SIGNAL OF SAMPLERATE
ctl->DmaPwmfControlRegister=mem_virt_to_phys((void *)(ctl->cbdma2) );
for (samplecnt = 0; samplecnt < NUM_SAMPLES ; samplecnt++)
{
//ctl->sample[samplecnt].WaitForThisSample=DelayFromSampleRate;
ctl->sample[samplecnt].PCMRegister=0x5A000000 | ((FreqDividerPCM)<<12) | FreqFractionnalPCM;
//@0
cbp->info = 0;//BCM2708_DMA_WAIT_RESP|BCM2708_DMA_NO_WIDE_BURSTS;//BCM2708_DMA_NO_WIDE_BURSTS /*| BCM2708_DMA_WAIT_RESP | */;
cbp->src = mem_virt_to_phys(&ctl->GpioDebugSampleRate); //Clear
if(Instrumentation==1)
cbp->dst = phys_gpio_clear_addr;
else
cbp->dst = dummy_gpio;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@1
//Set Amplitude by writing to PWM_SERIAL via PADS
cbp->info = 0;//BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP ;
cbp->src = mem_virt_to_phys(&ctl->sample[samplecnt].Amplitude1);
cbp->dst = phys_gpio_pads_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@2
//Set Amplitude by writing to PWM_SERIAL via Patern
cbp->info = 0;//BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP ;
cbp->src = mem_virt_to_phys(&ctl->sample[samplecnt].Amplitude2);
if(UsePCMClk==0)
cbp->dst = phys_pwm_fifo_addr;
if(UsePCMClk==1)
cbp->dst = phys_gpfsel;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@3
//Set PWMFrequency
cbp->info =/*BCM2708_DMA_NO_WIDE_BURSTS |BCM2708_DMA_WAIT_RESP|*/BCM2708_DMA_SRC_INC;
cbp->src = mem_virt_to_phys(&ctl->sample[samplecnt].FrequencyTab[0]);
if(UsePCMClk==0)
cbp->dst = phys_pwm_clock_div_addr;
if(UsePCMClk==1)
cbp->dst = phys_clock_div_addr;
cbp->length = 4;//mem_virt_to_phys(&ctl->sample[samplecnt].PWMF1); //Be updated by main DMA
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@4
cbp->info =0;// BCM2708_DMA_WAIT_RESP|BCM2708_DMA_NO_WIDE_BURSTS ; //A
cbp->src = mem_virt_to_phys(&ctl->GpioDebugSampleRate); //Set
if(Instrumentation==1)
cbp->dst = phys_gpio_set_addr;
else
cbp->dst = dummy_gpio;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@5
//SET PCM_FIFO TO WAIT
/*cbp->info = 0;//BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP|BCM2708_DMA_D_DREQ |BCM2708_DMA_PER_MAP(2);
cbp->src = mem_virt_to_phys(virtbase); //Dummy DATA
cbp->dst = dummy_gpio;//phys_pcm_fifo_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;*/
}
cbp--;
cbp->next = mem_virt_to_phys((void*)virtbase);
// *************************************** DMA CHANNEL 2 : PWM FREQUENCY *********************************************
// !!!!!!!!!!!!!!!!!!!!!!!!!!!! NOT USED ANYMORE ************************************************************
cbp = ctl->cbdma2;
//printf("DMA2 %x\n",mem_virt_to_phys(&ctl->cb[NUM_CBS_MAIN]));
gpioSetMode(17,1); // OSCILO SUR PIN 17
ctl->GpioDebugPWMF=1<<17; // GPIO 17/ PIN 11 HEADER IS DEBUG SIGNAL OF PWMFREQUENCY
//@0 = CBS_MAIN+0 1280 ns for 2 instructions
cbp->info = BCM2708_DMA_NO_WIDE_BURSTS /*| BCM2708_DMA_WAIT_RESP | */;
cbp->src = mem_virt_to_phys(&ctl->GpioDebugPWMF); //Clear
cbp->dst = phys_gpio_set_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@1 = CBS_MAIN+1 Length*27ns
cbp->info =BCM2708_DMA_WAIT_RESP |/*BCM2708_DMA_NO_WIDE_BURSTS| BCM2708_DMA_WAIT_RESP|*//*(0x8<< 21) |*/ BCM2708_DMA_SRC_INC;
cbp->src = mem_virt_to_phys(&ctl->SharedFrequencyTab[0]);//mem_virt_to_phys(&ctl->SharedFrequencyTab[0]);//mem_virt_to_phys(&ctl->SharedFrequency1);
cbp->dst = phys_pwm_clock_div_addr;
cbp->length = ctl->sample[0].PWMF1; //Be updated by main DMA
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
//@2 = CBS_MAIN+2
//** PWM LOW
cbp->info = BCM2708_DMA_NO_WIDE_BURSTS /*| BCM2708_DMA_WAIT_RESP |*/ ; //A
cbp->src = mem_virt_to_phys(&ctl->GpioDebugPWMF); //Set
cbp->dst = phys_gpio_clear_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(ctl->cbdma2); // Loop to begin of DMA 2
// ------------------------------ END DMA INIT ---------------------------------
dma_reg[DMA_CS+DMA_CHANNEL*0x40] = BCM2708_DMA_RESET;
udelay(1000);
dma_reg[DMA_CS+DMA_CHANNEL*0x40] = BCM2708_DMA_INT | BCM2708_DMA_END;
udelay(100);
dma_reg[DMA_CONBLK_AD+DMA_CHANNEL*0x40]=mem_virt_to_phys((void *)virtbase );
udelay(100);
dma_reg[DMA_DEBUG+DMA_CHANNEL*0x40] = 7; // clear debug error flags
udelay(100);
// START DMA will be later when data coming in
//dma_reg[DMA_CS+DMA_CHANNEL*0x40] = 0x10FF0001; // go, mid priority, wait for outstanding writes :7 Seems Max Priorit
//WAIT FOR OUTSTADING_WRITE make 50ns de plus pour ecrire dans un registre
//printf("END INIT SSTV\n");
dma_reg[DMA_CS+DMA_CHANNEL_PWMFREQUENCY*0x40] = BCM2708_DMA_RESET;
udelay(1000);
dma_reg[DMA_CS+DMA_CHANNEL_PWMFREQUENCY*0x40] = BCM2708_DMA_INT | BCM2708_DMA_END;
udelay(100);
dma_reg[DMA_CONBLK_AD+DMA_CHANNEL_PWMFREQUENCY*0x40]=mem_virt_to_phys((void *)(ctl->cbdma2) );
udelay(100);
dma_reg[DMA_DEBUG+DMA_CHANNEL_PWMFREQUENCY*0x40] = 7; // clear debug error flags
return 1;
}
int pitx_run(
const char Mode,
int SampleRate,
const float SetFrequency,
const float ppmpll,
const char NoUsePwmFrequency,
ssize_t (*readWrapper)(void *buffer, size_t count),
void (*reset)(void),
int* skipSignals)
{
int i;
//char pagemap_fn[64];
int OffsetModulation=1000;//TBR
int MicGain=100;
//unsigned char *data;
//Specific to ModeIQ
static signed short *IQArray=NULL;
//Specific to ModeIQ_FLOAT
static float *IQFloatArray=NULL;
//Specific to Mode RF
typedef struct {
double Frequency;
uint32_t WaitForThisSample;
} samplerf_t;
samplerf_t *TabRfSample=NULL;
fprintf(stdout,"rpitx Version %s compiled %s (F5OEO Evariste) running on ",PROGRAM_VERSION,__DATE__);
// Init Plls Frequency using ppm (or default)
PllFreq500MHZ=PLL_FREQ_500MHZ;
PllFreq500MHZ+=PllFreq500MHZ * (ppmpll / 1000000.0);
PllFreq1GHZ=PLL_FREQ_1GHZ;
PllFreq1GHZ+=PllFreq1GHZ * (ppmpll / 1000000.0);
PllFreq19MHZ=PLLFREQ_192;
PllFreq19MHZ+=PllFreq19MHZ * (ppmpll / 1000000.0);
//End of Init Plls
if(Mode==MODE_IQ)
{
IQArray=malloc(DmaSampleBurstSize*2*sizeof(signed short)); // TODO A FREE AT THE END OF SOFTWARE
reset();
}
if(Mode==MODE_IQ_FLOAT)
{
IQFloatArray=malloc(DmaSampleBurstSize*2*sizeof(float)); // TODO A FREE AT THE END OF SOFTWARE
}
if((Mode==MODE_RF)||(Mode==MODE_RFA))
{
//TabRfSample=malloc(DmaSampleBurstSize*sizeof(samplerf_t));
SampleRate=100000L; //NOT USED BUT BY CALCULATING TIMETOSLEEP IN RF MODE
}
if(Mode==MODE_VFO)
SampleRate=50000L; //50000 BY EXPERIMENT
if(Mode==MODE_IQ)
{
printf(" Frequency=%f ",GlobalTuningFrequency);
printf(" SampleRate=%d ",SampleRate);
}
pitx_SetTuneFrequency(SetFrequency*1000.0);
pitx_init(SampleRate, GlobalTuningFrequency, skipSignals);
static volatile uint32_t cur_cb,last_cb;
int last_sample;
int this_sample;
int free_slots;
//int SumDelay=0;
long int start_time;
static long time_difference=0;
struct timespec gettime_now;
cur_cb = (uint32_t)virtbase+ (NUM_SAMPLES-DmaSampleBurstSize)* sizeof(dma_cb_t) *CBS_SIZE_BY_SAMPLE;
last_cb=(uint32_t)virtbase /*+ 965* sizeof(dma_cb_t) *CBS_SIZE_BY_SAMPLE*/ ;
dma_reg[DMA_CONBLK_AD+DMA_CHANNEL*0x40]=mem_virt_to_phys((void*)cur_cb);
unsigned char Init=1;
// -----------------------------------------------------------------
for (;;)
{
int TimeToSleep;
static int StatusCompteur=0;
cur_cb = mem_phys_to_virt((uint32_t)(dma_reg[DMA_CONBLK_AD+DMA_CHANNEL*0x40]));
this_sample = (cur_cb - (uint32_t)virtbase) / (sizeof(dma_cb_t) * CBS_SIZE_BY_SAMPLE);
last_sample = (last_cb - (uint32_t)virtbase) / (sizeof(dma_cb_t) * CBS_SIZE_BY_SAMPLE);
free_slots = this_sample - last_sample;
if (free_slots < 0) // WARNING : ORIGINAL CODE WAS < strictly
free_slots += NUM_SAMPLES;
//printf("last_sample %lx cur_cb %lx FreeSlots = %d Time to sleep=%d\n",last_sample,cur_cb,free_slots,TimeToSleep);
if(Init==0)
{
TimeToSleep=(1e6*(NUM_SAMPLES-free_slots*2))/SampleRate; // Time to sleep in us
//printf("TimeToSleep%d\n",TimeToSleep);
}
else
TimeToSleep=1000;
//printf("Buffer Available=%d\n",BufferAvailable());
clock_gettime(CLOCK_REALTIME, &gettime_now);
start_time = gettime_now.tv_nsec;
if(TimeToSleep>=(2200+KERNEL_GRANULARITY)) // 2ms : Time to process File/Canal Coding
{
udelay(TimeToSleep-(2200+KERNEL_GRANULARITY));
TimeToSleep=0;
}
else
{
//udelay(TimeToSleep);
sched_yield();
//TimeToSleep=0;
//if(free_slots>(NUM_SAMPLES*9/10))
//printf("Buffer nearly empty...%d/%d\n",free_slots,NUM_SAMPLES);
}
static int free_slots_now;
cur_cb = mem_phys_to_virt(dma_reg[DMA_CONBLK_AD+DMA_CHANNEL*0x40]);
this_sample = (cur_cb - (uint32_t)virtbase) / (sizeof(dma_cb_t) * CBS_SIZE_BY_SAMPLE);
last_sample = (last_cb - (uint32_t)virtbase) / (sizeof(dma_cb_t) * CBS_SIZE_BY_SAMPLE);
free_slots_now = this_sample - last_sample;
if (free_slots_now < 0) // WARNING : ORIGINAL CODE WAS < strictly
free_slots_now += NUM_SAMPLES;
clock_gettime(CLOCK_REALTIME, &gettime_now);
time_difference = gettime_now.tv_nsec - start_time;
if(time_difference<0) time_difference+=1E9;
if(StatusCompteur%10==0)
{
//printf(" DiffTime = %ld FreeSlot %d FreeSlotDiff=%d Bitrate : %f\n",time_difference,free_slots_now,free_slots_now-free_slots,(1e9*(free_slots_now-free_slots))/(float)time_difference);
}
//if((1e9*(free_slots_now-free_slots))/(float)time_difference<40100.0) printf("Drift BAD\n"); else printf("Drift GOOD\n");
StatusCompteur++;
free_slots=free_slots_now;
// FIX IT : Max(freeslot et Numsample/8)
if((Init==1)&&(free_slots < DmaSampleBurstSize /*NUM_SAMPLES/8*/))
{
printf("****** STARTING TRANSMIT ********\n");
dma_reg[DMA_CONBLK_AD+DMA_CHANNEL*0x40]=mem_virt_to_phys((void*)virtbase );
usleep(100);
//Start DMA PWMFrequency
//dma_reg[DMA_CS+DMA_CHANNEL_PWMFREQUENCY*0x40] = 0x10880001;
//Start Main DMA
dma_reg[DMA_CS+DMA_CHANNEL*0x40] = 0x10FF0001; // go, mid priority, wait for outstanding writes :7 Seems Max Priority
Init=0;
continue;
}
clock_gettime(CLOCK_REALTIME, &gettime_now);
start_time = gettime_now.tv_nsec;
int debug=0;
if ((free_slots>=DmaSampleBurstSize))
{
// *************************************** MODE IQ **************************************************
if(Mode==MODE_IQ)
{
int NbRead=0;
static int Max=0;
static int Min=32767;
static int CompteSample=0;
CompteSample++;
NbRead=readWrapper(IQArray,DmaSampleBurstSize*2*2/*SHORT I,SHORT Q*/);
if(NbRead!=DmaSampleBurstSize*2*2)
{
if(loop_mode_flag==1)
{
printf("Looping FileIn\n");
reset();
NbRead=readWrapper(IQArray,DmaSampleBurstSize*2*2);
}
else {
stop_dma();
return 0;
}
}
for(i=0;i<DmaSampleBurstSize;i++)
{
//static float samplerate=48000;
static int amp;
static double df;
int CorrectionRpiFrequency=-1000; //TODO PPM / Offset=1KHZ at 144MHZ
CompteSample++;
//printf("i%d q%d\n",IQArray[2*i],IQArray[2*i+1]);
IQToFreqAmp(IQArray[2*i+1],IQArray[2*i],&df,&,SampleRate);
// Compression have to be done in modulation (SSB not here)
double A = 87.7f; // compression parameter
double ampf=amp/32767.0;
ampf = (fabs(ampf) < 1.0f/A) ? A*fabs(ampf)/(1.0f+ln(A)) : (1.0f+ln(A*fabs(ampf)))/(1.0f+ln(A)); //compand
amp= (int)(round(ampf * 32767.0f)) ;
if(amp>Max) Max=amp;
if(amp<Min) Min=amp;
/*
if((CompteSample%4800)==0)
{
//printf("%d\n",((CompteSample/48000)*1024)%32767);
//printf("Amp %d Freq %f MinAmp %d MaxAmp %d\n",amp,df,Min,Max);
printf("%d;%d;%d;%f\n",IQArray[2*i+1],IQArray[2*i],amp,df);
// printf(".");
fflush(stdout);
}
*/
// TEST - WARNING, REMOVE FOR RELEASE
//amp=((CompteSample/48000)*1024)%32767;
//df=0;
//amp=amp*10;
//amp=32767;
//if(df<OffsetModulation+100) amp=0;
//if(amp<32767/8) df= OffsetModulation;
//
// FIXME : df/harmonicNumber could alterate maybe modulations
FrequencyAmplitudeToRegister((GlobalTuningFrequency-OffsetModulation+/*(CompteSample/480)*/+df/HarmonicNumber)/HarmonicNumber,amp,last_sample++,0,SampleRate,NoUsePwmFrequency,CompteSample%2);
// !!!!!!!!!!!!!!!!!!!! 680 is for 48KHZ , should be adpated !!!!!!!!!!!!!!!!!
free_slots--;
if (last_sample == NUM_SAMPLES) last_sample = 0;
}
}
// *************************************** MODE IQ FLOAT**************************************************
if(Mode==MODE_IQ_FLOAT)
{
int NbRead=0;
static int Max=0;
static int Min=32767;
static int CompteSample=0;
CompteSample++;
NbRead=readWrapper(IQFloatArray,DmaSampleBurstSize*2*sizeof(float));
if(NbRead!=DmaSampleBurstSize*2*sizeof(float))
{
if(loop_mode_flag==1)
{
printf("Looping FileIn\n");
reset();
}
else if (!useStdin) {
stop_dma();
return 0;
}
}
for(i=0;i<DmaSampleBurstSize;i++)
{
//static float samplerate=48000;
static int amp;
static double df;
int CorrectionRpiFrequency=-1000; //TODO PPM / Offset=1KHZ at 144MHZ
CompteSample++;
//printf("i%d q%d\n",IQArray[2*i],IQArray[2*i+1]);
IQToFreqAmp(IQFloatArray[2*i+1]*32767,IQFloatArray[2*i]*32767,&df,&,SampleRate);
if(amp>Max) Max=amp;
if(amp<Min) Min=amp;
/*
if((CompteSample%4800)==0)
{
//printf("%d\n",((CompteSample/48000)*1024)%32767);
printf("Amp %d Freq %f MinAmp %d MaxAmp %d\n",amp,df,Min,Max);
// printf(".");
fflush(stdout);
}
*/
// TEST - WARNING, REMOVE FOR RELEASE
//amp=((CompteSample/48000)*1024)%32767;
//df=0;
//amp=amp*10;
//amp=32767;
//if(df<OffsetModulation+100) amp=0;
//
//amp=32767;
//if(df>SampleRate/2) df=SampleRate/2-df;
FrequencyAmplitudeToRegister((GlobalTuningFrequency-OffsetModulation+df/HarmonicNumber)/HarmonicNumber,amp,last_sample++,0,SampleRate,NoUsePwmFrequency,CompteSample%2);
free_slots--;
if (last_sample == NUM_SAMPLES) last_sample = 0;
}
}
// *************************************** MODE RF **************************************************
if((Mode==MODE_RF)||(Mode==MODE_RFA))
{
// SHOULD NOT EXEED 200 STEP*500ns; SAMPLERATE SHOULD BE MAX TO HAVE PRECISION FOR PCM
// BUT FIFO OF PCM IS 16 : SAMPLERATE MAYBE NOT EXCESS 16*80000 ! CAREFULL BUGS HERE
#define MAX_DELAY_WAIT 25000
static int CompteSample=0;
static uint32_t TimeRemaining=0;
static samplerf_t SampleRf;
static int NbRead;
CompteSample++;
int i;
for(i=0;i<DmaSampleBurstSize;i++)
{
if(TimeRemaining==0)
{
NbRead=readWrapper(&SampleRf,sizeof(samplerf_t));
if(NbRead!=sizeof(samplerf_t))
{
if(loop_mode_flag==1)
{
//printf("Looping FileIn\n");
reset();
NbRead=readWrapper(&SampleRf,sizeof(samplerf_t));
}
else if (!useStdin)
{
stop_dma();
return 0;
}
}
TimeRemaining=SampleRf.WaitForThisSample;
//TimeRemaining=50000;//SampleRf.WaitForThisSample;
debug=1;
//printf("A=%f Time =%d \n",SampleRf.Frequency,SampleRf.WaitForThisSample);
}
else
debug=0;
static int amp=32767;
static int WaitSample=0;
if(TimeRemaining>MAX_DELAY_WAIT)
WaitSample=MAX_DELAY_WAIT;
else
WaitSample=TimeRemaining;
//printf("TimeRemaining %d WaitSample %d\n",TimeRemaining,WaitSample);
if(Mode==MODE_RF)
{
if(SampleRf.Frequency==0.0)
{
amp=0;
SampleRf.Frequency=00.0;// TODO change that ugly frequency
}
else
amp=32767;
FrequencyAmplitudeToRegister((SampleRf.Frequency/HarmonicNumber+GlobalTuningFrequency)/HarmonicNumber,amp,last_sample++,WaitSample,0,NoUsePwmFrequency,debug);
}
if(Mode==MODE_RFA)
FrequencyAmplitudeToRegister((GlobalTuningFrequency)/HarmonicNumber,SampleRf.Frequency,last_sample++,WaitSample,0,NoUsePwmFrequency,debug);
TimeRemaining-=WaitSample;
free_slots--;
if (last_sample == NUM_SAMPLES) last_sample = 0;
}
}
// *************************************** MODE VFO **************************************************
if(Mode==MODE_VFO)
{
static uint32_t CompteSample=0;
int i;
//printf("Begin free %d\n",free_slots);
for(i=0;i<DmaSampleBurstSize;i++)
{
//To be fine tuned !!!!
static int OutputPower=32767;
CompteSample++;
debug=1;//(debug+1)%2;
//OutputPower=(CompteSample/10)%32768;
FrequencyAmplitudeToRegister(GlobalTuningFrequency/HarmonicNumber/*+(CompteSample*0.1)*/,OutputPower,last_sample++,25000,0,NoUsePwmFrequency,debug);
free_slots--;
//printf("%f \n",GlobalTuningFrequency+(((CompteSample/10)*1)%50000));
if (last_sample == NUM_SAMPLES) last_sample = 0;
if(CompteSample%40000==0)
{
//OutputPower=(OutputPower+1)%8;
//pad_gpios_reg[PADS_GPIO_0] = 0x5a000000 + (OutputPower&0x7) + (1<<4) + (0<<3); // Set output power for I/Q GPIO18/GPIO19
//printf("Freq %d Outputpower=%d\n",CompteSample/20000,OutputPower);
}
//usleep(1);
}
//printf("End free %d\n",free_slots);
}
}
clock_gettime(CLOCK_REALTIME, &gettime_now);
time_difference = gettime_now.tv_nsec - start_time;
if(time_difference<0) time_difference+=1E9;
last_cb = (uint32_t)virtbase + last_sample * sizeof(dma_cb_t) * CBS_SIZE_BY_SAMPLE;
}
stop_dma();
return(0);
}
int tx(uint32_t carrier_freq, char *audio_file, uint16_t pi, char *ps, char *rt, float ppm, char *control_pipe) {
// Catch all signals possible - it is vital we kill the DMA engine
// on process exit!
for (int i = 0; i < 64; i++) {
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = terminate;
sigaction(i, &sa, NULL);
}
dma_reg = map_peripheral(DMA_VIRT_BASE, DMA_LEN);
pwm_reg = map_peripheral(PWM_VIRT_BASE, PWM_LEN);
clk_reg = map_peripheral(CLK_VIRT_BASE, CLK_LEN);
gpio_reg = map_peripheral(GPIO_VIRT_BASE, GPIO_LEN);
// Use the mailbox interface to the VC to ask for physical memory.
mbox.handle = mbox_open();
if (mbox.handle < 0)
fatal("Failed to open mailbox. Check kernel support for vcio / BCM2708 mailbox.\n");
printf("Allocating physical memory: size = %d ", NUM_PAGES * 4096);
if(! (mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES * 4096, 4096, MEM_FLAG))) {
fatal("Could not allocate memory.\n");
}
// TODO: How do we know that succeeded?
printf("mem_ref = %u ", mbox.mem_ref);
if(! (mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref))) {
fatal("Could not lock memory.\n");
}
printf("bus_addr = %x ", mbox.bus_addr);
if(! (mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES * 4096))) {
fatal("Could not map memory.\n");
}
printf("virt_addr = %p\n", mbox.virt_addr);
// GPIO4 needs to be ALT FUNC 0 to output the clock
gpio_reg[GPFSEL0] = (gpio_reg[GPFSEL0] & ~(7 << 12)) | (4 << 12);
// Program GPCLK to use MASH setting 1, so fractional dividers work
clk_reg[GPCLK_CNTL] = 0x5A << 24 | 6;
udelay(100);
clk_reg[GPCLK_CNTL] = 0x5A << 24 | 1 << 9 | 1 << 4 | 6;
ctl = (struct control_data_s *) mbox.virt_addr;
dma_cb_t *cbp = ctl->cb;
uint32_t phys_sample_dst = CM_GP0DIV;
uint32_t phys_pwm_fifo_addr = PWM_PHYS_BASE + 0x18;
// Calculate the frequency control word
// The fractional part is stored in the lower 12 bits
uint32_t freq_ctl = ((float)(PLLFREQ / carrier_freq)) * ( 1 << 12 );
for (int i = 0; i < NUM_SAMPLES; i++) {
ctl->sample[i] = 0x5a << 24 | freq_ctl; // Silence
// Write a frequency sample
cbp->info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
cbp->src = mem_virt_to_phys(ctl->sample + i);
cbp->dst = phys_sample_dst;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
// Delay
cbp->info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
cbp->src = mem_virt_to_phys(mbox.virt_addr);
cbp->dst = phys_pwm_fifo_addr;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
}
cbp--;
cbp->next = mem_virt_to_phys(mbox.virt_addr);
// Here we define the rate at which we want to update the GPCLK control
// register.
//
// Set the range to 2 bits. PLLD is at 500 MHz, therefore to get 228 kHz
// we need a divisor of 500000 / 2 / 228 = 1096.491228
//
// This is 1096 + 2012*2^-12 theoretically
//
// However the fractional part may have to be adjusted to take the actual
// frequency of your Pi's oscillator into account. For example on my Pi,
// the fractional part should be 1916 instead of 2012 to get exactly
// 228 kHz. However RDS decoding is still okay even at 2012.
//
// So we use the 'ppm' parameter to compensate for the oscillator error
float divider = (500000./(2*228*(1.+ppm/1.e6)));
uint32_t idivider = (uint32_t) divider;
uint32_t fdivider = (uint32_t) ((divider - idivider)*pow(2, 12));
printf("ppm corr is %.4f, divider is %.4f (%d + %d*2^-12) [nominal 1096.4912].\n",
ppm, divider, idivider, fdivider);
pwm_reg[PWM_CTL] = 0;
udelay(10);
clk_reg[PWMCLK_CNTL] = 0x5A000006; // Source=PLLD and disable
udelay(100);
// theorically : 1096 + 2012*2^-12
clk_reg[PWMCLK_DIV] = 0x5A000000 | (idivider<<12) | fdivider;
udelay(100);
clk_reg[PWMCLK_CNTL] = 0x5A000216; // Source=PLLD and enable + MASH filter 1
udelay(100);
pwm_reg[PWM_RNG1] = 2;
udelay(10);
pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
udelay(10);
pwm_reg[PWM_CTL] = PWMCTL_CLRF;
udelay(10);
pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1;
udelay(10);
// Initialise the DMA
dma_reg[DMA_CS] = BCM2708_DMA_RESET;
udelay(10);
dma_reg[DMA_CS] = BCM2708_DMA_INT | BCM2708_DMA_END;
dma_reg[DMA_CONBLK_AD] = mem_virt_to_phys(ctl->cb);
dma_reg[DMA_DEBUG] = 7; // clear debug error flags
dma_reg[DMA_CS] = 0x10880001; // go, mid priority, wait for outstanding writes
uint32_t last_cb = (uint32_t)ctl->cb;
// Data structures for baseband data
float data[DATA_SIZE];
int data_len = 0;
int data_index = 0;
// Initialize the baseband generator
if(fm_mpx_open(audio_file, DATA_SIZE) < 0) return 1;
// Initialize the RDS modulator
char myps[9] = {0};
set_rds_pi(pi);
set_rds_rt(rt);
uint16_t count = 0;
uint16_t count2 = 0;
int varying_ps = 0;
if(ps) {
set_rds_ps(ps);
printf("PI: %04X, PS: \"%s\".\n", pi, ps);
} else {
printf("PI: %04X, PS: <Varying>.\n", pi);
varying_ps = 1;
}
printf("RT: \"%s\"\n", rt);
// Initialize the control pipe reader
if(control_pipe) {
if(open_control_pipe(control_pipe) == 0) {
printf("Reading control commands on %s.\n", control_pipe);
} else {
printf("Failed to open control pipe: %s.\n", control_pipe);
control_pipe = NULL;
}
}
printf("Starting to transmit on %3.1f MHz.\n", carrier_freq/1e6);
for (;;) {
// Default (varying) PS
if(varying_ps) {
if(count == 512) {
snprintf(myps, 9, "%08d", count2);
set_rds_ps(myps);
count2++;
}
if(count == 1024) {
set_rds_ps("RPi-Live");
count = 0;
}
count++;
}
if(control_pipe && poll_control_pipe() == CONTROL_PIPE_PS_SET) {
varying_ps = 0;
}
usleep(5000);
uint32_t cur_cb = mem_phys_to_virt(dma_reg[DMA_CONBLK_AD]);
int last_sample = (last_cb - (uint32_t)mbox.virt_addr) / (sizeof(dma_cb_t) * 2);
int this_sample = (cur_cb - (uint32_t)mbox.virt_addr) / (sizeof(dma_cb_t) * 2);
int free_slots = this_sample - last_sample;
if (free_slots < 0)
free_slots += NUM_SAMPLES;
while (free_slots >= SUBSIZE) {
// get more baseband samples if necessary
if(data_len == 0) {
if( fm_mpx_get_samples(data) < 0 ) {
terminate(0);
}
data_len = DATA_SIZE;
data_index = 0;
}
float dval = data[data_index] * (DEVIATION / 10.);
data_index++;
data_len--;
int intval = (int)((floor)(dval));
//int frac = (int)((dval - (float)intval) * SUBSIZE);
ctl->sample[last_sample++] = (0x5A << 24 | freq_ctl) + intval; //(frac > j ? intval + 1 : intval);
if (last_sample == NUM_SAMPLES)
last_sample = 0;
free_slots -= SUBSIZE;
}
last_cb = (uint32_t)mbox.virt_addr + last_sample * sizeof(dma_cb_t) * 2;
}
return 0;
}
