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