From 08a70e5f0e61219f136fc0e7679389d106a7e54d Mon Sep 17 00:00:00 2001 From: Josef Miegl Date: Mon, 14 Jun 2021 22:45:03 +0200 Subject: [PATCH] Initial commit --- .gitignore | 1 + Makefile | 39 +++ fm_mpx.c | 268 +++++++++++++++++++ fm_mpx.h | 10 + mailbox.c | 262 +++++++++++++++++++ mailbox.h | 47 ++++ pitxft.c | 753 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 1380 insertions(+) create mode 100644 .gitignore create mode 100644 Makefile create mode 100644 fm_mpx.c create mode 100644 fm_mpx.h create mode 100644 mailbox.c create mode 100644 mailbox.h create mode 100644 pitxft.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..da2e4ad --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +garaz.sh diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..8080c52 --- /dev/null +++ b/Makefile @@ -0,0 +1,39 @@ +CC = gcc +STD_CFLAGS = -Wall -std=gnu99 -c -O2 + +# Enable ARM-specific options only on ARM, and compilation of the app only on ARM +UNAME := $(shell uname -m) +PCPUI := $(shell cat /proc/cpuinfo | grep Revision | cut -c16-) + +# Determine the hardware platform. Below, pi1 stands for the RaspberryPi 1 (the original one), +# and pi2 stands for both the RaspberryPi 2 and 3. +ifeq ($(UNAME), armv6l) + CFLAGS = $(STD_CFLAGS) -march=armv6 -mtune=arm1176jzf-s -mfloat-abi=hard -mfpu=vfp -ffast-math -DRASPI=1 + TARGET = pi1 +else ifeq ($(PCPUI), 11) + CFLAGS = $(STD_CFLAGS) -march=armv7-a -mtune=arm1176jzf-s -mfloat-abi=hard -mfpu=vfp -ffast-math -DRASPI=4 + TARGET = pi4 +else ifeq ($(UNAME), armv7l) + CFLAGS = $(STD_CFLAGS) -march=armv7-a -mtune=arm1176jzf-s -mfloat-abi=hard -mfpu=vfp -ffast-math -DRASPI=2 + TARGET = pi2 +else + CFLAGS = $(STD_CFLAGS) + TARGET = other +endif + +ifneq ($(TARGET), other) + +app: pitxft.o mailbox.o + $(CC) -o pitxft mailbox.o pitxft.o -lm + +endif + + +mailbox.o: mailbox.c mailbox.h + $(CC) $(CFLAGS) mailbox.c + +pitxft.o: pitxft.c mailbox.h + $(CC) $(CFLAGS) pitxft.c + +clean: + rm -f *.o diff --git a/fm_mpx.c b/fm_mpx.c new file mode 100644 index 0000000..03b93f8 --- /dev/null +++ b/fm_mpx.c @@ -0,0 +1,268 @@ +/* + PiFmAdv - Advanced FM transmitter for the Raspberry Pi + Copyright (C) 2017 Miegl + + See https://github.com/Miegl/PiFmAdv +*/ + +#include +#include +#include +#include + +#include "rds.h" + +#define PI 3.14159265359 + +#define FIR_HALF_SIZE 30 +#define FIR_SIZE (2*FIR_HALF_SIZE-1) + +size_t length; + +// coefficients of the low-pass FIR filter +double low_pass_fir[FIR_HALF_SIZE]; + +double carrier_38[] = {0.0, 0.8660254037844386, 0.8660254037844388, 1.2246467991473532e-16, -0.8660254037844384, -0.8660254037844386}; +double carrier_19[] = {0.0, 0.5, 0.8660254037844386, 1.0, 0.8660254037844388, 0.5, 1.2246467991473532e-16, -0.5, -0.8660254037844384, -1.0, -0.8660254037844386, -0.5}; + +int phase_38 = 0; +int phase_19 = 0; + +double downsample_factor; + +double *audio_buffer; +int audio_index = 0; +int audio_len = 0; +double audio_pos; + +double fir_buffer_mono[FIR_SIZE] = {0}; +double fir_buffer_stereo[FIR_SIZE] = {0}; +int fir_index = 0; +int channels; + +double *last_buffer_val; +double preemphasis_prewarp; +double preemphasis_coefficient; + +SNDFILE *inf; + +double *alloc_empty_buffer(size_t length) { + double *p = malloc(length * sizeof(double)); + if(p == NULL) return NULL; + + bzero(p, length * sizeof(double)); + + return p; +} + + + +int fm_mpx_open(char *filename, size_t len, int cutoff_freq, int preemphasis_corner_freq, int srate, int nochan) { + length = len; + + if(filename != NULL) { + // Open the input file + SF_INFO sfinfo; + + if(filename[0] == '-' && srate > 0 && nochan > 0) { + printf("Using stdin for raw audio input at %d Hz.\n",srate); + sfinfo.samplerate = srate; + sfinfo.format = SF_FORMAT_RAW | SF_FORMAT_PCM_16 | SF_ENDIAN_LITTLE; + sfinfo.channels = nochan; + sfinfo.frames = 0; + } + + // stdin or file on the filesystem? + if(filename[0] == '-') { + if(!(inf = sf_open_fd(fileno(stdin), SFM_READ, &sfinfo, 0))) { + fprintf(stderr, "Error: could not open stdin for audio input.\n"); + return -1; + } else { + printf("Using stdin for audio input.\n"); + } + } else { + if(!(inf = sf_open(filename, SFM_READ, &sfinfo))) { + fprintf(stderr, "Error: could not open input file %s.\n", filename); + return -1; + } else { + printf("Using audio file: %s\n", filename); + } + } + + int in_samplerate = sfinfo.samplerate; + downsample_factor = 228000. / in_samplerate; + + printf("Input: %d Hz, upsampling factor: %.2f\n", in_samplerate, downsample_factor); + + channels = sfinfo.channels; + if(channels > 1) { + printf("%d channels, generating stereo multiplex.\n", channels); + } else { + printf("1 channel, monophonic operation.\n"); + } + + // Create the preemphasis + last_buffer_val = (double*) malloc(sizeof(double)*channels); + for(int i=0; i= downsample_factor) { + audio_pos -= downsample_factor; + + if(audio_len <= channels) { + for(int j=0; j<2; j++) { // one retry + audio_len = sf_read_double(inf, audio_buffer, length); + if (audio_len < 0) { + fprintf(stderr, "Error reading audio\n"); + return -1; + } + if(audio_len == 0) { + if( sf_seek(inf, 0, SEEK_SET) < 0 ) { + if(wait) { + return 0; + } else { + fprintf(stderr, "Could not rewind in audio file, terminating\n"); + return -1; + } + } + } else { + //apply preemphasis + int k; + int l; + double tmp; + for(k=0; k= FIR_SIZE) fir_index = 0; + + // Now apply the FIR low-pass filter + + /* As the FIR filter is symmetric, we do not multiply all + the coefficients independently, but two-by-two, thus reducing + the total number of multiplications by a factor of two + */ + double out_mono = 0; + double out_stereo = 0; + int ifbi = fir_index; // ifbi = increasing FIR Buffer Index + int dfbi = fir_index; // dfbi = decreasing FIR Buffer Index + for(int fi=0; fi 1) { + out_stereo += + low_pass_fir[fi] * + (fir_buffer_stereo[ifbi] + fir_buffer_stereo[dfbi]); + } + ifbi++; + if(ifbi >= FIR_SIZE) ifbi = 0; + } + // End of FIR filter + + if (channels>1) { + mpx_buffer[i] = + ((mpx-2)/2) * out_mono + + ((mpx-2)/2) * carrier_38[phase_38] * out_stereo + + 1 * carrier_19[phase_19]; + + if (rds) { + mpx_buffer[i] += rds_buffer[i]; + } + + phase_19++; + phase_38++; + if(phase_19 >= 12) phase_19 = 0; + if(phase_38 >= 6) phase_38 = 0; + } + else { + mpx_buffer[i] = + (mpx-1) * out_mono; + + if (rds) { + mpx_buffer[i] += rds_buffer[i]; + } + } + audio_pos++; + } + + return 0; +} + + +int fm_mpx_close() { + if(sf_close(inf) ) { + fprintf(stderr, "Error closing audio file"); + } + + if(audio_buffer != NULL) free(audio_buffer); + + return 0; +} diff --git a/fm_mpx.h b/fm_mpx.h new file mode 100644 index 0000000..0afbe87 --- /dev/null +++ b/fm_mpx.h @@ -0,0 +1,10 @@ +/* + PiFmAdv - Advanced FM transmitter for the Raspberry Pi + Copyright (C) 2017 Miegl + + See https://github.com/Miegl/PiFmAdv +*/ + +extern int fm_mpx_open(char *filename, size_t len, int cutoff_freq, int preemphasis_corner_freq, int srate, int nochan); +extern int fm_mpx_get_samples(double *mpx_buffer, double *rds_buffer, float mpx, int rds, int wait); +extern int fm_mpx_close(); diff --git a/mailbox.c b/mailbox.c new file mode 100644 index 0000000..de44e81 --- /dev/null +++ b/mailbox.c @@ -0,0 +1,262 @@ +/* +Copyright (c) 2012, Broadcom Europe Ltd. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mailbox.h" + +#define PAGE_SIZE (4*1024) + +void *mapmem(unsigned base, unsigned size) +{ + int mem_fd; + unsigned offset = base % PAGE_SIZE; + base = base - offset; + size = size + offset; + /* open /dev/mem */ + if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { + printf("can't open /dev/mem\nThis program should be run as root. Try prefixing command with: sudo\n"); + exit (-1); + } + void *mem = mmap( + 0, + size, + PROT_READ|PROT_WRITE, + MAP_SHARED/*|MAP_FIXED*/, + mem_fd, + base); +#ifdef DEBUG + printf("base=0x%x, mem=%p\n", base, mem); +#endif + if (mem == MAP_FAILED) { + printf("mmap error %d\n", (int)mem); + exit (-1); + } + close(mem_fd); + return (char *)mem + offset; +} + +void unmapmem(void *addr, unsigned size) +{ + const intptr_t offset = (intptr_t)addr % PAGE_SIZE; + addr = (char *)addr - offset; + size = size + offset; + int s = munmap(addr, size); + if (s != 0) { + printf("munmap error %d\n", s); + exit (-1); + } +} + +/* + * use ioctl to send mbox property message + */ + +static int mbox_property(int file_desc, void *buf) +{ + int ret_val = ioctl(file_desc, IOCTL_MBOX_PROPERTY, buf); + + if (ret_val < 0) { + printf("ioctl_set_msg failed:%d\n", ret_val); + } + +#ifdef DEBUG + unsigned *p = buf; int i; unsigned size = *(unsigned *)buf; + for (i=0; i + +#define MAJOR_NUM 100 +#define IOCTL_MBOX_PROPERTY _IOWR(MAJOR_NUM, 0, char *) +#define DEVICE_FILE_NAME "/dev/vcio" + +int mbox_open(); +void mbox_close(int file_desc); + +unsigned get_version(int file_desc); +unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags); +unsigned mem_free(int file_desc, unsigned handle); +unsigned mem_lock(int file_desc, unsigned handle); +unsigned mem_unlock(int file_desc, unsigned handle); +void *mapmem(unsigned base, unsigned size); +void unmapmem(void *addr, unsigned size); + +unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5); +unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout); +unsigned qpu_enable(int file_desc, unsigned enable); diff --git a/pitxft.c b/pitxft.c new file mode 100644 index 0000000..6a4e2bb --- /dev/null +++ b/pitxft.c @@ -0,0 +1,753 @@ +/* + PiTxFt - Raspberry Pi ft transmitter + Copyright (C) 2021 Miegl + + See https://github.com/Miegl/PiTxFt +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mailbox.h" + +#define MBFILE DEVICE_FILE_NAME // From mailbox.h + +#if (RASPI) == 1 // Original Raspberry Pi 1 +#define PERIPH_VIRT_BASE 0x20000000 +#define PERIPH_PHYS_BASE 0x7e000000 +#define DRAM_PHYS_BASE 0x40000000 +#define MEM_FLAG 0x0c +#define CLOCK_BASE 19.2e6 +#define DMA_CHANNEL 14 +#elif (RASPI) == 2 // Raspberry Pi 2 & 3 +#define PERIPH_VIRT_BASE 0x3f000000 +#define PERIPH_PHYS_BASE 0x7e000000 +#define DRAM_PHYS_BASE 0xc0000000 +#define MEM_FLAG 0x04 +#define CLOCK_BASE 19.2e6 +#define DMA_CHANNEL 14 +#elif (RASPI) == 4 // Raspberry Pi 4 +#define PERIPH_VIRT_BASE 0xfe000000 +#define PERIPH_PHYS_BASE 0x7e000000 +#define DRAM_PHYS_BASE 0xc0000000 +#define MEM_FLAG 0x04 +#define CLOCK_BASE 54.0e6 +#define DMA_CHANNEL 6 +#else +#error Unknown Raspberry Pi version (variable RASPI) +#endif + +#define DMA_BASE_OFFSET 0x00007000 +#define PWM_BASE_OFFSET 0x0020C000 +#define PWM_LEN 0x28 +#define CLK_BASE_OFFSET 0x00101000 +#define CLK_LEN 0x1300 +#define GPIO_BASE_OFFSET 0x00200000 +#define GPIO_LEN 0x100 +#define PCM_BASE_OFFSET 0x00203000 +#define PCM_LEN 0x24 +#define PAD_BASE_OFFSET 0x00100000 +#define PAD_LEN (0x40/4) //0x64 + +#define DMA_VIRT_BASE (PERIPH_VIRT_BASE + DMA_BASE_OFFSET) +#define PWM_VIRT_BASE (PERIPH_VIRT_BASE + PWM_BASE_OFFSET) +#define CLK_VIRT_BASE (PERIPH_VIRT_BASE + CLK_BASE_OFFSET) +#define GPIO_VIRT_BASE (PERIPH_VIRT_BASE + GPIO_BASE_OFFSET) +#define PAD_VIRT_BASE (PERIPH_VIRT_BASE + PAD_BASE_OFFSET) +#define PCM_VIRT_BASE (PERIPH_VIRT_BASE + PCM_BASE_OFFSET) + +#define PWM_PHYS_BASE (PERIPH_PHYS_BASE + PWM_BASE_OFFSET) +#define PCM_PHYS_BASE (PERIPH_PHYS_BASE + PCM_BASE_OFFSET) +#define GPIO_PHYS_BASE (PERIPH_PHYS_BASE + GPIO_BASE_OFFSET) + +// GPIO +#define GPFSEL0 (0x00/4) +#define GPFSEL1 (0x04/4) +#define GPFSEL2 (0x08/4) +#define GPPUD (0x94/4) +#define GPPUDCLK0 (0x98/4) +#define GPPUDCLK1 (0x9C/4) + +#define CORECLK_CNTL (0x08/4) +#define CORECLK_DIV (0x0c/4) +#define GPCLK_CNTL (0x70/4) +#define GPCLK_DIV (0x74/4) +#define EMMCCLK_CNTL (0x1C0/4) +#define EMMCCLK_DIV (0x1C4/4) + +#define CM_LOCK (0x114/4) +#define CM_LOCK_FLOCKA (1<<8) +#define CM_LOCK_FLOCKB (1<<9) +#define CM_LOCK_FLOCKC (1<<10) +#define CM_LOCK_FLOCKD (1<<11) +#define CM_LOCK_FLOCKH (1<<12) + +#define CM_PLLA (0x104/4) +#define CM_PLLC (0x108/4) +#define CM_PLLD (0x10c/4) +#define CM_PLLH (0x110/4) +#define CM_PLLB (0x170/4) + +#define A2W_PLLA_ANA0 (0x1010/4) +#define A2W_PLLC_ANA0 (0x1030/4) +#define A2W_PLLD_ANA0 (0x1050/4) +#define A2W_PLLH_ANA0 (0x1070/4) +#define A2W_PLLB_ANA0 (0x10f0/4) +#define A2W_PLL_KA_SHIFT 7 +#define A2W_PLL_KI_SHIFT 19 +#define A2W_PLL_KP_SHIFT 15 + +#define PLLA_CTRL (0x1100/4) +#define PLLA_FRAC (0x1200/4) +#define PLLA_DSI0 (0x1300/4) +#define PLLA_CORE (0x1400/4) +#define PLLA_PER (0x1500/4) +#define PLLA_CCP2 (0x1600/4) + +#define PLLB_CTRL (0x11e0/4) +#define PLLB_FRAC (0x12e0/4) +#define PLLB_ARM (0x13e0/4) +#define PLLB_SP0 (0x14e0/4) +#define PLLB_SP1 (0x15e0/4) +#define PLLB_SP2 (0x16e0/4) + +#define PLLC_CTRL (0x1120/4) +#define PLLC_FRAC (0x1220/4) +#define PLLC_CORE2 (0x1320/4) +#define PLLC_CORE1 (0x1420/4) +#define PLLC_PER (0x1520/4) +#define PLLC_CORE0 (0x1620/4) + +#define PLLD_CTRL (0x1140/4) +#define PLLD_FRAC (0x1240/4) +#define PLLD_DSI0 (0x1340/4) +#define PLLD_CORE (0x1440/4) +#define PLLD_PER (0x1540/4) +#define PLLD_DSI1 (0x1640/4) + +#define PLLH_CTRL (0x1160/4) +#define PLLH_FRAC (0x1260/4) +#define PLLH_AUX (0x1360/4) +#define PLLH_RCAL (0x1460/4) +#define PLLH_PIX (0x1560/4) +#define PLLH_STS (0x1660/4) + +// PWM +#define PWM_CTL (0x00/4) +#define PWM_DMAC (0x08/4) +#define PWM_RNG1 (0x10/4) +#define PWM_RNG2 (0x20/4) +#define PWM_FIFO (0x18/4) + +#define PWMCLK_CNTL 40 +#define PWMCLK_DIV 41 + +#define PWMCTL_PWEN1 (1<<0) +#define PWMCTL_MODE1 (1<<1) +#define PWMCTL_RPTL1 (1<<2) +#define PWMCTL_POLA1 (1<<4) +#define PWMCTL_USEF1 (1<<5) +#define PWMCTL_CLRF (1<<6) +#define PWMCTL_MSEN1 (1<<7) +#define PWMCTL_PWEN2 (1<<8) +#define PWMCTL_MODE2 (1<<9) +#define PWMCTL_RPTL2 (1<<10) +#define PWMCTL_USEF2 (1<<13) +#define PWMCTL_MSEN2 (1<<15) + +#define PWMDMAC_ENAB (1<<31) +#define PWMDMAC_THRSHLD ((15<<8)|(15<<0)) + +// PCM +#define PCM_CS_A (0x00/4) +#define PCM_FIFO_A (0x04/4) +#define PCM_MODE_A (0x08/4) +#define PCM_RXC_A (0x0c/4) +#define PCM_TXC_A (0x10/4) +#define PCM_DREQ_A (0x14/4) +#define PCM_INTEN_A (0x18/4) +#define PCM_INT_STC_A (0x1c/4) +#define PCM_GRAY (0x20/4) + +#define PCMCLK_CNTL 38 +#define PCMCLK_DIV 39 + +// PAD +#define GPIO_PAD_0_27 (0x2C/4) +#define GPIO_PAD_28_45 (0x30/4) +#define GPIO_PAD_46_52 (0x34/4) + +// DMA +#define DMA_CHANNEL_MAX 14 +#define DMA_CHANNEL_SIZE 0x100 + +#define BCM2708_DMA_ACTIVE (1<<0) +#define BCM2708_DMA_END (1<<1) +#define BCM2708_DMA_INT (1<<2) +#define BCM2708_DMA_WAIT_RESP (1<<3) +#define BCM2708_DMA_D_DREQ (1<<6) +#define BCM2708_DMA_DST_IGNOR (1<<7) +#define BCM2708_DMA_SRC_INC (1<<8) +#define BCM2708_DMA_SRC_IGNOR (1<<11) +#define BCM2708_DMA_NO_WIDE_BURSTS (1<<26) +#define BCM2708_DMA_DISDEBUG (1<<28) +#define BCM2708_DMA_ABORT (1<<30) +#define BCM2708_DMA_RESET (1<<31) +#define BCM2708_DMA_PER_MAP(x) ((x)<<16) +#define BCM2708_DMA_PRIORITY(x) ((x)&0xf << 16) +#define BCM2708_DMA_PANIC_PRIORITY(x) ((x)&0xf << 20) + +#define DMA_CS (0x00/4) +#define DMA_CONBLK_AD (0x04/4) +#define DMA_DEBUG (0x20/4) + +#define DMA_CS_RESET (1<<31) +#define DMA_CS_ABORT (1<<30) +#define DMA_CS_DISDEBUG (1<<29) +#define DMA_CS_WAIT_FOR_OUTSTANDING_WRITES (1<<28) +#define DMA_CS_INT (1<<2) +#define DMA_CS_END (1<<1) +#define DMA_CS_ACTIVE (1<<0) +#define DMA_CS_PRIORITY(x) ((x)&0xf << 16) +#define DMA_CS_PANIC_PRIORITY(x) ((x)&0xf << 20) + +#define DREQ_PCM_TX 2 +#define DREQ_PCM_RX 3 +#define DREQ_SMI 4 +#define DREQ_PWM 5 +#define DREQ_SPI_TX 6 +#define DREQ_SPI_RX 7 +#define DREQ_SPI_SLAVE_TX 8 +#define DREQ_SPI_SLAVE_RX 9 + +#define MEM_FLAG_DISCARDABLE (1 << 0) /* can be resized to 0 at any time. Use for cached data */ +#define MEM_FLAG_NORMAL (0 << 2) /* normal allocating alias. Don't use from ARM */ +#define MEM_FLAG_DIRECT (1 << 2) /* 0xC alias uncached */ +#define MEM_FLAG_COHERENT (2 << 2) /* 0x8 alias. Non-allocating in L2 but coherent */ +#define MEM_FLAG_L1_NONALLOCATING (MEM_FLAG_DIRECT | MEM_FLAG_COHERENT) /* Allocating in L2 */ +#define MEM_FLAG_ZERO (1 << 4) /* initialise buffer to all zeros */ +#define MEM_FLAG_NO_INIT (1 << 5) /* don't initialise (default is initialise to all ones */ +#define MEM_FLAG_HINT_PERMALOCK (1 << 6) /* Likely to be locked for long periods of time. */ + +#define BUS_TO_PHYS(x) ((x)&~0xC0000000) + +#define PAGE_SIZE 4096 +#define PAGE_SHIFT 12 +#define NUM_PAGES ((sizeof(struct control_data_s) + PAGE_SIZE - 1) >> PAGE_SHIFT) + +#define NUM_SAMPLES 64000 +#define NUM_CBS (NUM_SAMPLES * 2) + + +struct __attribute__((__packed__)) ft { + double frequency; + uint32_t duration; + uint32_t padding; +}; + +typedef struct { + uint32_t info, src, dst, length, stride, next, pad[2]; +} dma_cb_t; + +static struct { + int handle; /* From mbox_open() */ + unsigned mem_ref; /* From mem_alloc() */ + unsigned bus_addr; /* From mem_lock() */ + uint8_t *virt_addr; /* From mapmem() */ +} mbox; + +struct control_data_s { + dma_cb_t cb[NUM_CBS]; + uint32_t sample[NUM_SAMPLES]; +}; + +static struct control_data_s *ctl; + +static volatile uint32_t *pwm_reg; +static volatile uint32_t *clk_reg; +static volatile uint32_t *dma_reg; +static volatile uint32_t *gpio_reg; +static volatile uint32_t *pcm_reg; +static volatile uint32_t *pad_reg; + +static void udelay(int us) +{ + struct timespec ts = { 0, us * 1000 }; + nanosleep(&ts, NULL); +} + +static void terminate(int num) +{ + // Stop outputting and generating the clock. + if (clk_reg && gpio_reg && mbox.virt_addr) { + // Set GPIOs to be an output (instead of ALT FUNC 0, which is the clock). + gpio_reg[0] = (gpio_reg[0] & ~(7 << 12)) | (1 << 12); //GPIO4 + udelay(10); + gpio_reg[2] = (gpio_reg[2] & ~(7 << 0)) | (1 << 0); //GPIO20 + udelay(10); + gpio_reg[3] = (gpio_reg[3] & ~(7 << 6)) | (1 << 6); //GPIO32 + udelay(10); + //gpio_reg[3] = (gpio_reg[3] & ~(7 << 12)) | (1 << 12); //GPIO34 - Doesn't work on Pi 3, 3B+, Zero W + //udelay(10); + + // Disable the clock generator. + clk_reg[GPCLK_CNTL] = 0x5A; + } + + if (dma_reg && mbox.virt_addr) { + dma_reg[DMA_CS] = BCM2708_DMA_RESET; + udelay(10); + } + + if (mbox.virt_addr != NULL) { + unmapmem(mbox.virt_addr, NUM_PAGES * PAGE_SIZE); + mem_unlock(mbox.handle, mbox.mem_ref); + mem_free(mbox.handle, mbox.mem_ref); + } + + printf("Terminating: cleanly deactivated the DMA engine and killed the carrier.\n"); + + exit(num); +} + +static void fatal(char *fmt, ...) +{ + va_list ap; + fprintf(stderr,"ERROR: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); + terminate(0); +} + +static void warn(char *fmt, ...) +{ + va_list ap; + fprintf(stderr,"WARNING: "); + va_start(ap, fmt); + vfprintf(stderr, fmt, ap); + va_end(ap); +} + +static uint32_t mem_virt_to_phys(void *virt) +{ + uint32_t offset = (uint8_t *)virt - mbox.virt_addr; + + return mbox.bus_addr + offset; +} + +static uint32_t mem_phys_to_virt(uint32_t phys) +{ + return phys - (uint32_t)mbox.bus_addr + (uint32_t)mbox.virt_addr; +} + +static void *map_peripheral(uint32_t base, uint32_t len) +{ + int fd = open("/dev/mem", O_RDWR | O_SYNC); + void * vaddr; + + if (fd < 0) + fatal("Failed to open /dev/mem: %m.\n"); + vaddr = mmap(NULL, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base); + if (vaddr == MAP_FAILED) + fatal("Failed to map peripheral at 0x%08x: %m.\n", base); + close(fd); + + return vaddr; +} + + +int tx(uint32_t carrier_freq, int divider, char *ft_file, float ppm, int power, int gpio, bool repeat) { + // Catch only important signals + for (int i = 0; i < 25; i++) { + struct sigaction sa; + + memset(&sa, 0, sizeof(sa)); + sa.sa_handler = terminate; + sigaction(i, &sa, NULL); + } + + FILE *fd = NULL; + + if(ft_file != NULL) { + if(ft_file[0] == '-') { + if(!(fd = freopen(NULL, "rb", stdin))) { + fatal("Error: could not reopen stdin.\n"); + } else { + printf("Using stdin for input.\n"); + } + } else { + if(!(fd = fopen(ft_file, "rb"))) { + fatal("Error: could not open input file %s.\n", ft_file); + } else { + printf("Using input file: %s\n", ft_file); + } + } + } else { + fatal("Error: no input file specified.\n"); + } + + dma_reg = map_peripheral(DMA_VIRT_BASE, (DMA_CHANNEL_SIZE * (DMA_CHANNEL_MAX + 1))); + dma_reg = dma_reg + ((DMA_CHANNEL_SIZE / sizeof(int)) * (DMA_CHANNEL)); + 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); + pcm_reg = map_peripheral(PCM_VIRT_BASE, PCM_LEN); + pad_reg = map_peripheral(PAD_VIRT_BASE, PAD_LEN); + uint32_t freq_ctl; + + // 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 * PAGE_SIZE); + if(!(mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES * PAGE_SIZE, PAGE_SIZE, MEM_FLAG))) { + fatal("Could not allocate memory.\n"); + } + 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 * PAGE_SIZE))) { + fatal("Could not map memory.\n"); + } + printf("virt_addr = %p\n", mbox.virt_addr); + + clk_reg[GPCLK_CNTL] = (0x5a<<24) | (1<<4) | (4); + udelay(100); + + clk_reg[CM_PLLA] = 0x5A00022A; // Enable PLLA_PER + udelay(100); + + int ana[4]; + for (int i = 3; i >= 0; i--) + { + ana[i] = clk_reg[(A2W_PLLA_ANA0) + i]; + } + + ana[1]&=~(1<<14); + for (int i = 3; i >= 0; i--) + { + clk_reg[(A2W_PLLA_ANA0) + i] = (0x5A << 24) | ana[i]; + } + udelay(100); + + clk_reg[PLLA_CORE] = (0x5a<<24) | (1<<8); // Disable + clk_reg[PLLA_PER] = 0x5A000001; // Div + udelay(100); + + + // Adjust PLLA frequency + freq_ctl = (unsigned int)(((carrier_freq*divider)/CLOCK_BASE*((double)(1<<20)))); + clk_reg[PLLA_CTRL] = (0x5a<<24) | (0x21<<12) | (freq_ctl>>20); // Integer part + freq_ctl&=0xFFFFF; + clk_reg[PLLA_FRAC] = (0x5a<<24) | (freq_ctl&0xFFFFC); // Fractional part + udelay(100); + + if ((clk_reg[CM_LOCK] & CM_LOCK_FLOCKA) > 0) + printf("Master PLLA Locked\n"); + else + printf("Warning: Master PLLA NOT Locked\n"); + + // Program GPCLK integer division + //int clktmp; + //clktmp = clk_reg[GPCLK_CNTL]; + //clk_reg[GPCLK_CNTL] = (0xF0F&clktmp) | (0x5a<<24); // Clear run + //udelay(100); + clk_reg[GPCLK_DIV] = (0x5a<<24) | (divider<<12); + udelay(100); + clk_reg[GPCLK_CNTL] = (0x5a<<24) | (4); // Source = PLLA (4) + udelay(100); + clk_reg[GPCLK_CNTL] = (0x5a<<24) | (1<<4) | (4); // Run, Source = PLLA (4) + udelay(100); + + // Drive Strength: 0 = 2mA, 7 = 16mA. Ref: https://www.scribd.com/doc/101830961/GPIO-Pads-Control2 + pad_reg[GPIO_PAD_0_27] = 0x5a000018 + power; + pad_reg[GPIO_PAD_28_45] = 0x5a000018 + power; + udelay(100); + + int reg = gpio / 10; + int shift = (gpio % 10) * 3; + int mode; + if(gpio == 20) { + mode = 2; + } else { + mode = 4; + } + + // GPIO needs to be ALT FUNC 0 to output the clock + gpio_reg[reg] = (gpio_reg[reg] & ~(7 << shift)) | (mode << shift); + udelay(100); + + ctl = (struct control_data_s *) mbox.virt_addr; + dma_cb_t *cbp = ctl->cb; + + 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 = PERIPH_PHYS_BASE + (PLLA_FRAC<<2) + CLK_BASE_OFFSET; + 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 = PERIPH_PHYS_BASE + (PWM_FIFO<<2) + PWM_BASE_OFFSET; + 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 + double srdivider = (((double)carrier_freq*divider/1e3)/(2*200*(1.+ppm/1.e6))); + uint32_t idivider = (uint32_t)srdivider; + uint32_t fdivider = (uint32_t)((srdivider - idivider)*pow(2, 12)); + + printf("PPM correction is %.4f, divider is %.4f (%d + %d*2^-12).\n", ppm, srdivider, idivider, fdivider); + + pwm_reg[PWM_CTL] = 0; + udelay(100); + clk_reg[PWMCLK_CNTL] = (0x5a<<24) | (4); // Source = PLLA & disable + udelay(100); + clk_reg[PWMCLK_DIV] = (0x5a<<24) | (idivider<<12) | fdivider; + udelay(100); + clk_reg[PWMCLK_CNTL] = (0x5a<<24) | (1<<9) | (1<<4) | (4); // Source = PLLA, enable, MASH setting 1 + udelay(100); + pwm_reg[PWM_RNG1] = 2; + udelay(100); + 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_MSEN1; + //pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1; + udelay(100); + + // Initialise the DMA + dma_reg[DMA_CS] = BCM2708_DMA_RESET; + udelay(100); + 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] = BCM2708_DMA_PRIORITY(15) | BCM2708_DMA_PANIC_PRIORITY(15) | BCM2708_DMA_DISDEBUG | BCM2708_DMA_ACTIVE; + + + uint32_t last_cb = (uint32_t)ctl->cb; + + struct ft* ft_data = NULL; + size_t ft_len = 500; + size_t ft_index = 0; + ft_data = calloc(ft_len, sizeof(struct ft)); + + //while(ft_index += fread(&ft_data[ft_index], sizeof(struct ft), ft_len - ft_index, fd) == (ft_len - ft_index)) { + // ft_len *= 2; + // ft_data = reallocarray(ft_data, ft_len, sizeof(struct ft)); + //} + + while(fread(&ft_data[ft_index++], sizeof(struct ft), 1, fd) > 0) { + if(ft_index == ft_len - 1) { + ft_len *= 2; + ft_data = reallocarray(ft_data, ft_len, sizeof(struct ft)); + } + } + + double *buffer; + size_t buffer_len = 0; + size_t buffer_index = 0; + for(size_t i = 0; i < ft_index; i++) { + buffer_len += (uint64_t)ft_data[i].duration*200000/1000000000; + } + buffer = calloc(buffer_len, sizeof(double)); + for(size_t i = 0; i < ft_index; i++) { + for(size_t j = 0; j < ((uint64_t)ft_data[i].duration*200000/1000000000); j++) { + buffer[buffer_index++] = ft_data[i].frequency; + } + } + buffer_index = 0; + + printf("Starting to transmit on %3.1f MHz.\n", carrier_freq/1e6); + + for (;;) { + uint32_t cur_cb = (int)mem_phys_to_virt(dma_reg[DMA_CONBLK_AD]); + int last_sample = (last_cb - (int)mbox.virt_addr) / (sizeof(dma_cb_t) * 2); + int this_sample = (cur_cb - (int)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 >= 1) { + // Get more baseband samples if necessary + + if(repeat && (buffer_index == buffer_len)) buffer_index = 0; + + uint32_t freqval = (uint32_t)(((buffer[buffer_index++]*divider)/CLOCK_BASE*((double)(1<<20)))); + freqval &= 0xFFFFF; + + ctl->sample[last_sample++] = (0x5A << 24 | freqval); + if (last_sample == NUM_SAMPLES) + last_sample = 0; + + free_slots -= 1; + } + last_cb = (uint32_t)mbox.virt_addr + last_sample * sizeof(dma_cb_t) * 2; + + usleep(5000); + } + + return 0; +} + +int main(int argc, char **argv) { + int opt; + + char *ft_file = NULL; + uint32_t carrier_freq = 87600000; + float ppm = 0; + int divc = 0; + int power = 7; + int gpio = 4; + bool repeat = false; + + const char *short_opt = "i:f:p:d:w:g:rh"; + struct option long_opt[] = + { + {"input", required_argument, NULL, 'i'}, + {"freq", required_argument, NULL, 'f'}, + {"ppm", required_argument, NULL, 'p'}, + {"div", required_argument, NULL, 'd'}, + {"power", required_argument, NULL, 'w'}, + {"gpio", required_argument, NULL, 'g'}, + {"repeat", no_argument, NULL, 'r'}, + + {"help", no_argument, NULL, 'h'}, + { 0, 0, 0, 0 } + }; + + while((opt = getopt_long(argc, argv, short_opt, long_opt, NULL)) != -1) + { + switch(opt) + { + case -1: + case 0: + break; + + case 'i': //input file + ft_file = optarg; + break; + + case 'f': //freq + carrier_freq = 1e6 * atof(optarg); + break; + + case 'p': //ppm + ppm = atof(optarg); + break; + + case 'd': //div + divc = atoi(optarg); + break; + + case 'w': //power + power = atoi(optarg); + if(power < 0 || power > 7) + fatal("Output power has to be set in range of 0 - 7\n"); + break; + + case 'g': //gpio + gpio = atoi(optarg); + if(gpio != 4 && gpio != 20 && gpio != 32) // && gpio != 34) + fatal("Available GPIO pins: 4,20,32\n"); + break; + + case 'r': //repeat + repeat = true; + break; + + case 'h': //help + fatal("Help:\n" + "Syntax: pitxft [--input (-i) file] [--freq (-f) frequency] [--ppm (-p) ppm-error] ...\n"); + + break; + + case ':': + fatal("%s: option '-%c' requires an argument\n", argv[0], optopt); + break; + + case '?': + default: + fatal("%s: option '-%c' is invalid. See -h (--help)\n", argv[0], optopt); + break; + } + } + + double xtal_freq_recip=1.0/CLOCK_BASE; + int divider, best_divider = 0; + int min_int_multiplier, max_int_multiplier; + int int_multiplier; + double frac_multiplier; + int fom, best_fom = 0; + int solution_count = 0; + for(divider = 2; divider < 50; divider += 1) + { + if(carrier_freq * divider > 1400e6) break; + + max_int_multiplier=((int)((double)(carrier_freq + 10 + (1 * 1000)) * divider * xtal_freq_recip)); + min_int_multiplier=((int)((double)(carrier_freq - 10 - (1 * 1000)) * divider * xtal_freq_recip)); + if(min_int_multiplier != max_int_multiplier) continue; + + solution_count++; + fom = 0; + + if(carrier_freq * divider > 900e6) fom++; // Prefer frequencies close to 1.0 Ghz + if(carrier_freq * divider < 1100e6) fom++; + + if(carrier_freq * divider > 800e6) fom++; + if(carrier_freq * divider < 1200e6) fom++; + + frac_multiplier = ((double)(carrier_freq) * divider * xtal_freq_recip); + int_multiplier = (int)frac_multiplier; + frac_multiplier = frac_multiplier - int_multiplier; + if((frac_multiplier > 0.2) && (frac_multiplier < 0.8)) fom++; // Prefer mulipliers away from integer boundaries + + if(fom > best_fom) // Best match so far + { + best_fom = fom; + best_divider = divider; + } + } + + if(divc) { + best_divider = divc; + } + else if(!solution_count & !best_divider) { + fatal("No tuning solution found. You can specify the divider manually by setting the -div parameter.\n"); + } + + printf("Carrier: %3.2f Mhz, VCO: %4.1f MHz, Multiplier: %f, Divider: %d\n", carrier_freq/1e6, (double)carrier_freq * best_divider / 1e6, carrier_freq * best_divider * xtal_freq_recip, best_divider); + + int errcode = tx(carrier_freq, best_divider, ft_file, ppm, power, gpio, repeat); + + terminate(errcode); +}