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; }