static void schci_bcm2708_cb_write(struct sdhci_bcm2708_priv *host,
int ix,
dma_addr_t dma_addr, unsigned len,
int /*bool*/ is_last)
{
struct bcm2708_dma_cb *cb = &host->cb_base[ix];
unsigned char dmawaits = host->dma_waits;
/* We can make arbitrarily large writes as long as we specify DREQ to
pace the delivery of bytes to the Arasan hardware */
cb->info = BCM2708_DMA_PER_MAP(BCM2708_DMA_DREQ_EMMC) |
BCM2708_DMA_WAITS(dmawaits) |
BCM2708_DMA_D_DREQ |
BCM2708_DMA_S_WIDTH |
BCM2708_DMA_S_INC;
cb->src = dma_addr;
cb->dst = DMA_SDHCI_BUFFER; /* DATA register DMA address */
cb->length = len;
cb->stride = 0;
if (is_last) {
cb->info |= BCM2708_DMA_INT_EN |
BCM2708_DMA_WAIT_RESP;
cb->next = 0;
} else
cb->next = host->cb_handle +
(ix+1)*sizeof(struct bcm2708_dma_cb);
cb->pad[0] = 0;
cb->pad[1] = 0;
}
static void schci_bcm2708_cb_read(struct sdhci_bcm2708_priv *host,
int ix,
dma_addr_t dma_addr, unsigned len,
int /*bool*/ is_last)
{
struct bcm2708_dma_cb *cb = &host->cb_base[ix];
unsigned char dmawaits = host->dma_waits;
cb->info = BCM2708_DMA_PER_MAP(BCM2708_DMA_DREQ_EMMC) |
BCM2708_DMA_WAITS(dmawaits) |
BCM2708_DMA_S_DREQ |
BCM2708_DMA_D_WIDTH |
BCM2708_DMA_D_INC;
cb->src = DMA_SDHCI_BUFFER; /* DATA register DMA address */
cb->dst = dma_addr;
cb->length = len;
cb->stride = 0;
if (is_last) {
cb->info |= BCM2708_DMA_INT_EN |
BCM2708_DMA_WAIT_RESP;
cb->next = 0;
} else
cb->next = host->cb_handle +
(ix+1)*sizeof(struct bcm2708_dma_cb);
cb->pad[0] = 0;
cb->pad[1] = 0;
}
int32_t audio_init(void) {
buf = new_ringbuffer(256);
// Set up the pwm clock
// vals read from raspbian:
// PWMCLK_CNTL = 148 = 10010100 - 100 is allegedly 'plla' but I can't make that work
// PWMCLK_DIV = 16384
// PWM_CONTROL=9509 = 10010100100101
// PWM0_RANGE=1024
// PWM1_RANGE=1024
uint32_t range = 0x400;
uint32_t idiv = 12;
SET_GPIO_ALT(40, 0); // set pins 40/45 (aka phone jack) to pwm function
SET_GPIO_ALT(45, 0);
usleep(10); // I don't know if all these usleeps are really necessary
PUT32(CLOCK_BASE + 4*BCM2835_PWMCLK_CNTL, PM_PASSWORD | BCM2835_PWMCLK_CNTL_KILL);
PUT32(PWM_BASE + 4*BCM2835_PWM_CONTROL, 0);
// In theory this should be divided by 2 again for the two channels, but
// that seems to lead to the wrong rate. TODO: investigate
uint32_t samplerate = 500000000.0 / idiv / range;
PUT32(CLOCK_BASE + 4*BCM2835_PWMCLK_DIV, PM_PASSWORD | (idiv<<12));
PUT32(CLOCK_BASE + 4*BCM2835_PWMCLK_CNTL,
PM_PASSWORD |
BCM2835_PWMCLK_CNTL_ENABLE |
BCM2835_PWMCLK_CNTL_PLLD);
usleep(1);
PUT32(PWM_BASE + 4*BCM2835_PWM0_RANGE, range);
PUT32(PWM_BASE + 4*BCM2835_PWM1_RANGE, range);
usleep(1);
buf->dma_cb->info = BCM2708_DMA_S_INC | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
buf->dma_cb->src = (uint32_t)(buf->buffer);
buf->dma_cb->dst = ((PWM_BASE+4*BCM2835_PWM_FIFO) & 0x00ffffff) | 0x7e000000; // physical address of fifo
buf->dma_cb->length = sizeof(uint32_t) * buf->buffer_sz;
buf->dma_cb->next = (uint32_t)buf->dma_cb;
usleep(1);
PUT32(PWM_BASE + 4*BCM2835_PWM_DMAC, PWMDMAC_ENAB | PWMDMAC_THRSHLD);
usleep(1);
PUT32(PWM_BASE + 4*BCM2835_PWM_CONTROL, PWMCTL_CLRF);
usleep(1);
PUT32(PWM_BASE + 4*BCM2835_PWM_CONTROL,
BCM2835_PWM1_USEFIFO |
// BCM2835_PWM1_REPEATFF |
BCM2835_PWM1_ENABLE |
BCM2835_PWM0_USEFIFO |
// BCM2835_PWM0_REPEATFF |
BCM2835_PWM0_ENABLE | 1<<6);
PUT32(DMA5_CNTL_BASE + BCM2708_DMA_CS, BCM2708_DMA_RESET);
usleep(10);
PUT32(DMA5_CNTL_BASE + BCM2708_DMA_CS, BCM2708_DMA_INT | BCM2708_DMA_END);
PUT32(DMA5_CNTL_BASE + BCM2708_DMA_ADDR, (uint32_t)buf->dma_cb);
PUT32(DMA5_CNTL_BASE + BCM2708_DMA_DEBUG, 7); // clear debug error flags
usleep(10);
PUT32(DMA5_CNTL_BASE + BCM2708_DMA_CS, 0x10880001); // go, mid priority, wait for outstanding writes
// printf("audio init done\r\n");
usleep(1);
return samplerate;
}
int init_module(void)
{
int res, i, s;
res = alloc_chrdev_region(&devno, 0, 1, "servoblaster");
if (res < 0) {
printk(KERN_WARNING "ServoBlaster: Can't allocated device number\n");
return res;
}
my_major = MAJOR(devno);
cdev_init(&my_cdev, &fops);
my_cdev.owner = THIS_MODULE;
my_cdev.ops = &fops;
res = cdev_add(&my_cdev, MKDEV(my_major, 0), 1);
if (res) {
printk(KERN_WARNING "ServoBlaster: Error %d adding device\n", res);
unregister_chrdev_region(devno, 1);
return res;
}
for (i = 0; i < NUM_SERVOS; i++)
setup_timer(idle_timer + i, servo_timeout, i);
if (idle_timeout && idle_timeout < 20) {
printk(KERN_WARNING "ServoBlaster: Increased idle timeout to minimum of 20ms\n");
idle_timeout = 20;
}
ctldatabase = __get_free_pages(GFP_KERNEL, 0);
printk(KERN_INFO "ServoBlaster: Control page is at 0x%lx, cycle_ticks %d, tick_scale %d, idle_timeout %d\n",
ctldatabase, cycle_ticks, tick_scale, idle_timeout);
if (ctldatabase == 0) {
printk(KERN_WARNING "ServoBlaster: alloc_pages failed\n");
cdev_del(&my_cdev);
unregister_chrdev_region(devno, 1);
return -EFAULT;
}
ctl = (struct ctldata_s *)ctldatabase;
gpio_reg = (uint32_t *)ioremap(GPIO_BASE, GPIO_LEN);
dma_reg = (uint32_t *)ioremap(DMA_BASE, DMA_LEN);
clk_reg = (uint32_t *)ioremap(CLK_BASE, CLK_LEN);
pwm_reg = (uint32_t *)ioremap(PWM_BASE, PWM_LEN);
memset(ctl, 0, sizeof(*ctl));
// Set all servo control pins to be outputs
for (i = 0; i < NUM_SERVOS; i++) {
int gpio = servo2gpio[i];
int fnreg = gpio/10 + GPFSEL0;
int fnshft = (gpio %10) * 3;
gpio_reg[GPCLR0] = 1 << gpio;
gpio_reg[fnreg] = (gpio_reg[fnreg] & ~(7 << fnshft)) | (1 << fnshft);
}
#ifdef PWM0_ON_GPIO18
// Set pwm0 output on gpio18
gpio_reg[GPCLR0] = 1 << 18;
gpio_reg[GPFSEL1] = (gpio_reg[GPFSEL1] & ~(7 << 8*3)) | ( 2 << 8*3);
#endif
// Build the DMA CB chain
for (s = 0; s < NUM_SERVOS; s++) {
int i = s*4;
// Set gpio high
ctl->gpiodata[s] = 1 << servo2gpio[s];
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
ctl->cb[i].src = (uint32_t)(&ctl->gpiodata[s]) & 0x7fffffff;
// We clear the GPIO here initially, so outputs go to 0 on startup
// Once someone writes to /dev/servoblaster we'll patch it to a 'set'
ctl->cb[i].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
ctl->cb[i].length = sizeof(uint32_t);
ctl->cb[i].stride = 0;
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
// delay
i++;
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
ctl->cb[i].src = (uint32_t)(&ctl->pwmdata) & 0x7fffffff;
ctl->cb[i].dst = ((PWM_BASE + PWM_FIFO*4) & 0x00ffffff) | 0x7e000000;
ctl->cb[i].length = sizeof(uint32_t) * 1; // default 1 tick high
ctl->cb[i].stride = 0;
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
// Set gpio lo
i++;
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
ctl->cb[i].src = (uint32_t)(&ctl->gpiodata[s]) & 0x7fffffff;
ctl->cb[i].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
ctl->cb[i].length = sizeof(uint32_t);
ctl->cb[i].stride = 0;
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
// delay
i++;
ctl->cb[i].info = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
ctl->cb[i].src = (uint32_t)(&ctl->pwmdata) & 0x7fffffff;
ctl->cb[i].dst = ((PWM_BASE + PWM_FIFO*4) & 0x00ffffff) | 0x7e000000;
ctl->cb[i].length = sizeof(uint32_t) * (cycle_ticks / 8 - 1);
ctl->cb[i].stride = 0;
ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
}
// Point last cb back to first one so it loops continuously
ctl->cb[NUM_SERVOS*4-1].next = (uint32_t)(ctl->cb) & 0x7fffffff;
// Initialise PWM - these delays may not all be necessary, but at least
// I seem to be able to switch between PWM audio and servoblaster
// reliably with this code.
pwm_reg[PWM_CTL] = 0;
udelay(10);
pwm_reg[PWM_STA] = pwm_reg[PWM_STA];
udelay(10);
clk_reg[PWMCLK_CNTL] = 0x5A000000;
clk_reg[PWMCLK_DIV] = 0x5A000000;
clk_reg[PWMCLK_CNTL] = 0x5A000001; // Source=osc
clk_reg[PWMCLK_DIV] = 0x5A000000 | (32<<12); // set pwm div to 32 (19.2MHz/32 = 600KHz)
udelay(500); // Delay needed before enabling
clk_reg[PWMCLK_CNTL] = 0x5A000011; // Source=osc and enable
udelay(500);
pwm_reg[PWM_RNG1] = tick_scale; // 600KHz/6 = 10us per FIFO write
udelay(10);
ctl->pwmdata = 1; // Give a pulse of one clock width for each fifo write
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] = (uint32_t)(ctl->cb) & 0x7fffffff;
dma_reg[DMA_DEBUG] = 7; // clear debug error flags
udelay(10);
dma_reg[DMA_CS] = 0x10880001; // go, mid priority, wait for outstanding writes
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;
}
