EMC2 running on Raspberry Pi?

#include <stdio.h> #include <stdlib.h> #include <string.h> #include <stdarg.h> #include <stdint.h> #include <signal.h> #include <time.h> #include <sys/types.h> #include <fcntl.h> #include <sys/mman.h> int setup(int pw_incr_us); void shutdown(void); int init_channel(int channel, int subcycle_time_us); int print_channel(int channel); #define SUBCYCLE_TIME_US_DEFAULT 4000 // Subcycle minimum. We kept seeing no signals and strange behavior of the RPi #define SUBCYCLE_TIME_US_MIN 2000 // Default pulse-width-increment-granularity #define PULSE_WIDTH_INCREMENT_GRANULARITY_US_DEFAULT 20 // 15 DMA channels are usable on the RPi (0..14) #define DMA_CHANNELS 15 // Standard page sizes #define PAGE_SIZE 4096 #define PAGE_SHIFT 12 // Memory Addresses #define DMA_BASE 0x20007000 #define DMA_CHANNEL_INC 0x100 #define DMA_LEN 0x24 #define PWM_BASE 0x2020C000 #define PWM_LEN 0x28 #define CLK_BASE 0x20101000 #define CLK_LEN 0xA8 #define GPIO_BASE 0x20200000 #define GPIO_LEN 0x100 // Datasheet p. 51: #define DMA_NO_WIDE_BURSTS (1<<26) #define DMA_WAIT_RESP (1<<3) #define DMA_D_DREQ (1<<6) #define DMA_PER_MAP(x) ((x)<<16) #define DMA_END (1<<1) #define DMA_RESET (1<<31) #define DMA_INT (1<<2) // Each DMA channel has 3 writeable registers: #define DMA_CS (0x00/4) #define DMA_CONBLK_AD (0x04/4) #define DMA_DEBUG (0x20/4) // GPIO Memory Addresses #define GPIO_FSEL0 (0x00/4) #define GPIO_SET0 (0x1c/4) #define GPIO_CLR0 (0x28/4) #define GPIO_LEV0 (0x34/4) #define GPIO_PULLEN (0x94/4) #define GPIO_PULLCLK (0x98/4) // GPIO Modes (IN=0, OUT=1) #define GPIO_MODE_IN 0 #define GPIO_MODE_OUT 1 // PWM Memory Addresses #define PWM_CTL (0x00/4) #define PWM_DMAC (0x08/4) #define PWM_RNG1 (0x10/4) #define PWM_FIFO (0x18/4) #define PWMCLK_CNTL 40 #define PWMCLK_DIV 41 #define PWMCTL_MODE1 (1<<1) #define PWMCTL_PWEN1 (1<<0) #define PWMCTL_CLRF (1<<6) #define PWMCTL_USEF1 (1<<5) #define PWMDMAC_ENAB (1<<31) #define PWMDMAC_THRSHLD ((15<<8) | (15<<0)) // DMA Control Block Data Structure (p40): 8 words (256 bits) typedef struct { uint32_t info; // TI: transfer information uint32_t src; // SOURCE_AD uint32_t dst; // DEST_AD uint32_t length; // TXFR_LEN: transfer length uint32_t stride; // 2D stride mode uint32_t next; // NEXTCONBK uint32_t pad[2]; // _reserved_ } dma_cb_t; // Memory mapping typedef struct { uint8_t *virtaddr; uint32_t physaddr; } page_map_t; // Main control structure per channel struct channel { uint8_t *virtbase; uint32_t *sample; dma_cb_t *cb; page_map_t *page_map; volatile uint32_t *dma_reg; // Set by user uint32_t subcycle_time_us; uint32_t num_flags; // Set by system uint32_t num_samples; uint32_t num_cbs; uint32_t num_pages; // Used only for control purposes uint32_t width_max; }; // One control structure per channel static struct channel channels[DMA_CHANNELS]; // Pulse width increment granularity static uint16_t pulse_width_incr_us = -1; static uint8_t _is_setup = 0; static int gpio_setup = 0; // bitfield for setup gpios (setup = out/low) // Common registers static volatile uint32_t *pwm_reg; static volatile uint32_t *pcm_reg; static volatile uint32_t *clk_reg; static volatile uint32_t *gpio_reg; #define log_debug printf // Sets a GPIO to either GPIO_MODE_IN(=0) or GPIO_MODE_OUT(=1) static void gpio_set_mode(uint32_t pin, uint32_t mode) { uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10]; fsel &= ~(7 << ((pin % 10) * 3)); fsel |= mode << ((pin % 10) * 3); gpio_reg[GPIO_FSEL0 + pin/10] = fsel; } static void udelay(int us) { struct timespec ts = { 0, us * 1000 }; nanosleep(&ts, NULL); } // Shutdown -- its important to reset the DMA before quitting void shutdown(void) { int i; for (i = 0; i < DMA_CHANNELS; i++) { if (channels[i].dma_reg && channels[i].virtbase) { log_debug("shutting down dma channel %d\n", i); udelay(channels[i].subcycle_time_us); channels[i].dma_reg[DMA_CS] = DMA_RESET; udelay(10); } } } // Terminate is triggered by signals static void terminate(void) { shutdown(); exit(0); } #define fatal(fmt...) { printf(fmt); shutdown(); exit(-1); } // Catch all signals possible - it is vital we kill the DMA engine on process exit! static void setup_sighandlers(void) { int i; for (i = 0; i < 64; i++) { struct sigaction sa; memset(&sa, 0, sizeof(sa)); sa.sa_handler = (void *) terminate; sigaction(i, &sa, NULL); } } // Memory mapping static uint32_t mem_virt_to_phys(int channel, void *virt) { uint32_t offset = (uint8_t *)virt - channels[channel].virtbase; return channels[channel].page_map[offset >> PAGE_SHIFT].physaddr + (offset % PAGE_SIZE); } // Peripherals memory mapping static void * map_peripheral(uint32_t base, uint32_t len) { int fd = open("/dev/mem", O_RDWR); void * vaddr; if (fd < 0) { fatal("rpio-pwm: Failed to open /dev/mem: %m\n"); return NULL; } vaddr = mmap(NULL, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base); if (vaddr == MAP_FAILED) { fatal("rpio-pwm: Failed to map peripheral at 0x%08x: %m\n", base); return NULL; } close(fd); return vaddr; } // Returns a pointer to the control block of this channel in DMA memory uint8_t* get_cb(int channel) { return channels[channel].virtbase + (sizeof(uint32_t)) * channels[channel].num_samples + (sizeof(uint32_t)) * channels[channel].num_flags ; } // Get a channel's pagemap static int make_pagemap(int channel) { int i, fd, memfd, pid; char pagemap_fn[64]; channels[channel].page_map = malloc(channels[channel].num_pages * sizeof(*channels[channel].page_map)); if (channels[channel].page_map == 0) fatal("rpio-pwm: Failed to malloc page_map: %m\n"); memfd = open("/dev/mem", O_RDWR); if (memfd < 0) fatal("rpio-pwm: Failed to open /dev/mem: %m\n"); pid = getpid(); sprintf(pagemap_fn, "/proc/%d/pagemap", pid); fd = open(pagemap_fn, O_RDONLY); if (fd < 0) fatal("rpio-pwm: Failed to open %s: %m\n", pagemap_fn); if (lseek(fd, (uint32_t)channels[channel].virtbase >> 9, SEEK_SET) != (uint32_t)channels[channel].virtbase >> 9) { fatal("rpio-pwm: Failed to seek on %s: %m\n", pagemap_fn); } for (i = 0; i < channels[channel].num_pages; i++) { uint64_t pfn; channels[channel].page_map[i].virtaddr = channels[channel].virtbase + i * PAGE_SIZE; // Following line forces page to be allocated channels[channel].page_map[i].virtaddr[0] = 0; if (read(fd, &pfn, sizeof(pfn)) != sizeof(pfn)) fatal("rpio-pwm: Failed to read %s: %m\n", pagemap_fn); if (((pfn >> 55) & 0x1bf) != 0x10c) fatal("rpio-pwm: Page %d not present (pfn 0x%016llx)\n", i, pfn); channels[channel].page_map[i].physaddr = (uint32_t)pfn << PAGE_SHIFT | 0x40000000; } close(fd); close(memfd); return 0; } static int init_virtbase(int channel) { channels[channel].virtbase = mmap(NULL, channels[channel].num_pages * PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED, -1, 0); if (channels[channel].virtbase == MAP_FAILED) fatal("rpio-pwm: Failed to mmap physical pages: %m\n"); if ((unsigned long)channels[channel].virtbase & (PAGE_SIZE-1)) fatal("rpio-pwm: Virtual address is not page aligned\n"); return 0; } // Initialize control block for this channel static int init_ctrl_data(int channel) { dma_cb_t *cbp = (dma_cb_t *) get_cb(channel); uint32_t *sample = (uint32_t *) channels[channel].virtbase; uint32_t phys_gpset0 = 0x7e200000 + 0x1c; uint32_t phys_fifo_addr; uint32_t phys_gpclr0 = 0x7e200000 + 0x28; int i; channels[channel].dma_reg = map_peripheral(DMA_BASE, DMA_LEN) + (DMA_CHANNEL_INC * channel); if (channels[channel].dma_reg == NULL) return -1; phys_fifo_addr = (PWM_BASE | 0x7e000000) + 0x18; // Reset complete per-sample gpio mask to 0 memset(sample, 0, sizeof(channels[channel].num_samples * sizeof(uint32_t))); #define Setcbbp() cbp->length = 4; cbp->stride = 0; cbp->next = mem_virt_to_phys(channel, cbp + 1); cbp++; #define FLAG_BUF 0 #if FLAG_BUF // add a control block at start to flag writing this half of buffer cbp->info = DMA_NO_WIDE_BURSTS; cbp->src = mem_virt_to_phys(channel, sample + channels[channel].num_samples+1); cbp->dst = mem_virt_to_phys(channel, sample + channels[channel].num_samples); Setcbbp(); cbp->info = DMA_NO_WIDE_BURSTS; cbp->src = mem_virt_to_phys(channel, sample + channels[channel].num_samples+1); cbp->dst = mem_virt_to_phys(channel, sample + channels[channel].num_samples); Setcbbp(); #endif for (i = 0; i < channels[channel].num_samples; i++) { cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP; cbp->src = mem_virt_to_phys(channel, sample + i); cbp->dst = phys_gpclr0; Setcbbp(); // Delay cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5); cbp->src = mem_virt_to_phys(channel, sample); // Any data will do cbp->dst = phys_fifo_addr; Setcbbp(); #if FLAG_BUF // add an extra flag control block to show writing top half of buffer if(i == channels[channel].num_samples/2 ) { cbp->info = DMA_NO_WIDE_BURSTS; cbp->src = mem_virt_to_phys(channel, sample + channels[channel].num_samples+2); cbp->dst = mem_virt_to_phys(channel, sample + channels[channel].num_samples); Setcbbp(); cbp->info = DMA_NO_WIDE_BURSTS; cbp->src = mem_virt_to_phys(channel, sample + channels[channel].num_samples+2); cbp->dst = mem_virt_to_phys(channel, sample + channels[channel].num_samples); Setcbbp(); } #endif cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP; cbp->src = mem_virt_to_phys(channel, sample + i); // src contains mask of which gpios need change at this sample cbp->dst = phys_gpset0; //phys_gpclr0; // set each sample to clear set gpios by default Setcbbp(); // Delay cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5); cbp->src = mem_virt_to_phys(channel, sample); // Any data will do cbp->dst = phys_fifo_addr; Setcbbp(); } // The last control block links back to the first (= endless loop) cbp--; cbp->next = mem_virt_to_phys(channel, get_cb(channel)); // Initialize the DMA channel 0 (p46, 47) channels[channel].dma_reg[DMA_CS] = DMA_RESET; // DMA channel reset udelay(10); channels[channel].dma_reg[DMA_CS] = DMA_INT | DMA_END; // Interrupt status & DMA end flag channels[channel].dma_reg[DMA_CONBLK_AD] = mem_virt_to_phys(channel, get_cb(channel)); // initial CB channels[channel].dma_reg[DMA_DEBUG] = 7; // clear debug error flags channels[channel].dma_reg[DMA_CS] = 0x10880001; // go, mid priority, wait for outstanding writes return 0; } // Initialize PWM or PCM hardware once for all channels (10MHz) static void init_hardware(void) { // Initialise PWM pwm_reg[PWM_CTL] = 0; udelay(10); clk_reg[PWMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz) udelay(100); clk_reg[PWMCLK_DIV] = 0x5A000000 | (50<<12); // set pwm div to 50, giving 10MHz udelay(100); clk_reg[PWMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable udelay(100); pwm_reg[PWM_RNG1] = pulse_width_incr_us * 10; 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); } int init_channel(int channel, int subcycle_time_us) { log_debug("Initializing channel %d...\n", channel); if (_is_setup == 0) fatal("Error: you need to call `setup(..)` before initializing channels\n"); if (channel > DMA_CHANNELS-1) fatal("Error: maximum channel is %d (requested channel %d)\n", DMA_CHANNELS-1, channel); // Setup Data channels[channel].num_flags = 8; channels[channel].subcycle_time_us = subcycle_time_us; channels[channel].num_samples = channels[channel].subcycle_time_us / pulse_width_incr_us; channels[channel].width_max = channels[channel].num_samples - 1; channels[channel].num_cbs = (channels[channel].num_samples * 4) + 4; channels[channel].num_pages = ((channels[channel].num_cbs * 32 + channels[channel].num_samples * 4 + channels[channel].num_flags * 4 + \ PAGE_SIZE - 1) >> PAGE_SHIFT); // Initialize channel if (init_virtbase(channel) == -1) return -1; if (make_pagemap(channel) == -1) return -1; if (init_ctrl_data(channel) == -1) return -1; return 0; } int print_channel(int channel) { if (channel > DMA_CHANNELS - 1) fatal("Error: you tried to print channel %d, but max channel is %d\n", channel, DMA_CHANNELS-1); log_debug("Subcycle time: %dus\n", channels[channel].subcycle_time_us); log_debug("PW Increments: %dus\n", pulse_width_incr_us); log_debug("Num samples: %d\n", channels[channel].num_samples); log_debug("Num CBS: %d\n", channels[channel].num_cbs); log_debug("Num pages: %d\n", channels[channel].num_pages); return 0; } int setup(int pw_incr_us) { pulse_width_incr_us = pw_incr_us; if (_is_setup == 1) fatal("Error: setup(..) has already been called before\n"); log_debug("PW increments: %dus\n", pulse_width_incr_us); setup_sighandlers(); pwm_reg = map_peripheral(PWM_BASE, PWM_LEN); clk_reg = map_peripheral(CLK_BASE, CLK_LEN); gpio_reg = map_peripheral(GPIO_BASE, GPIO_LEN); if (pwm_reg == NULL || clk_reg == NULL || gpio_reg == NULL) return -1; init_hardware(); _is_setup = 1; return 0; } int freq(int channel, int gpio, int width) { int i; uint32_t phys_gpclr0 = 0x7e200000 + 0x28; uint32_t phys_gpset0 = 0x7e200000 + 0x1c; uint32_t *dp = (uint32_t *) channels[channel].virtbase; // this is for the buffer sync position flags... *(dp + channels[channel].num_samples+1) = 1; *(dp + channels[channel].num_samples+2) = 0; log_debug("freq: channel=%d, gpio=%d, width=%d\n", channel, gpio, width); if ((gpio_setup & 1<<gpio) == 0){ log_debug("init_gpio %d\n", gpio); gpio_reg[GPIO_CLR0] = 1 << gpio; gpio_set_mode(gpio, GPIO_MODE_OUT); gpio_setup |= 1 << gpio; } { int j=0, oon=1; for (i = 0; i < channels[channel].num_samples ; i++) { if(j== width){ j=0; oon= !oon; } if(oon){ *(dp + i) &= ~(1 << gpio); } // set 0 else{ *(dp + i) |= 1 << gpio; } // set 1 j++; } } return 0; } // prints out bit patterns sent to gpio 22/23/24 int print_buf(int channel) { int i; uint32_t *dp = (uint32_t *) channels[channel].virtbase; log_debug("buffer: \n"); for (i = 0; i < channels[channel].num_samples ; i++) { log_debug("%d%d%d\n", *(dp + i) & (1 << 22)? 0:1 , *(dp + i) & (1 << 23)? 0:1, *(dp + i) & (1 << 24)? 0:1); } log_debug("\n"); return 0; } int main(int argc, char **argv) { setup(PULSE_WIDTH_INCREMENT_GRANULARITY_US_DEFAULT); int gpio[3]={22,23,24}, period[4]={50,10,5,2}; int channel = 0,i=0; int subcycle_time_us = SUBCYCLE_TIME_US_DEFAULT; init_channel(channel, subcycle_time_us); print_channel(channel); freq( channel, gpio[0], 50); freq( channel, gpio[1], 5); freq( channel, gpio[2], 10); print_buf(channel); sleep(30); freq( channel, gpio[0], 1); freq( channel, gpio[1], 20); freq( channel, gpio[2], 2); print_buf(channel); sleep(30); shutdown(); exit(0); }