pi-blaster.c

/*
 * pi-blaster.c Multiple PWM for the Raspberry Pi
 * Copyright (c) 2013 Thomas Sarlandie <thomas@sarlandie.net>
 *
 * Based on the most excellent servod.c by Richard Hirst
 * Copyright (c) 2013 Richard Hirst <richardghirst@gmail.com>
 *
 * This program provides very similar functionality to servoblaster, except
 * that rather than implementing it as a kernel module, servod implements
 * the functionality as a usr space daemon.
 *
 */
/*
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
*/

#define _POSIX_C_SOURCE 200809L
#define _XOPEN_SOURCE   700
#define _DEFAULT_SOURCE

#ifdef HAVE_CONFIG_H
#include "config.h"
#else
static char VERSION[] = "SNAPSHOT";
#endif

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <getopt.h>
#include <errno.h>
#include <stdarg.h>
#include <stdint.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/sysmacros.h>
#include <fcntl.h>
#include <sys/mman.h>
#include "mailbox.h"

// MAX_CHANNELS is both the highest gpio we can address
// and the maximum number of channels
#define MAX_CHANNELS	32

// Create default known_pins with raspberry pi list of pins
// to compare against the param received.
static uint8_t known_pins[MAX_CHANNELS] = {
    4,      // P1-7
    17,     // P1-11
    18,     // P1-12
    27,     // P1-13
    21,     // P1-40
    22,     // P1-15
    23,     // P1-16
    24,     // P1-18
    25,     // P1-22
};

// Create a list of reserved GPIO pins
// http://elinux.org/RPi_Low-level_peripherals
static uint8_t banned_pins[MAX_CHANNELS] = {
    6,      	// On Model B, it is in use for the Ethernet function
    28,		// board ID and are connected to resistors R3 to R10 (only on Rev1.0 boards).
    29,		// board ID and are connected to resistors R3 to R10 (only on Rev1.0 boards).
    30,		// board ID and are connected to resistors R3 to R10 (only on Rev1.0 boards).
    31,		// board ID and are connected to resistors R3 to R10 (only on Rev1.0 boards).
//		40,		// used by analogue audio
//		45,		// used by analogue audio
    46,		// HDMI hotplug detect
    47,		// 47 to 53 are used by the SD card interface.
    48,		// 47 to 53 are used by the SD card interface.
    49,		// 47 to 53 are used by the SD card interface.
    50,		// 47 to 53 are used by the SD card interface.
    51,		// 47 to 53 are used by the SD card interface.
    52,		// 47 to 53 are used by the SD card interface.
    53,		// 47 to 53 are used by the SD card interface.
};

// Set num of possible PWM channels based on the known pins size.
uint8_t num_channels = (sizeof(known_pins)/sizeof(known_pins[0]));

// pin2gpio array is not setup as empty to avoid locking all GPIO
// inputs as PWM, they are set on the fly by the pin param passed.
static uint8_t pin2gpio[MAX_CHANNELS];

#define DEVFILE       "/dev/pi-blaster"
#define DEVFILE_MBOX  "/dev/pi-blaster-mbox"
#define DEVFILE_VCIO  "/dev/vcio"

#define PAGE_SIZE     4096
#define PAGE_SHIFT    12

// CYCLE_TIME_US is the period of the PWM signal in us.
#define CYCLE_TIME_US	10000
// SAMPLE_US is the pulse width increment granularity, again in microseconds.
// Setting SAMPLE_US too low will likely cause problems as the DMA controller
// will use too much memory bandwidth.  10us is a good value, though you
// might be ok setting it as low as 2us.
#define SAMPLE_US   10
#define NUM_SAMPLES (CYCLE_TIME_US/SAMPLE_US)
#define NUM_CBS     (NUM_SAMPLES*2)

#define NUM_PAGES   ((NUM_CBS * sizeof(dma_cb_t) + NUM_SAMPLES * 4 + \
                      PAGE_SIZE - 1) >> PAGE_SHIFT)

#define DMA_BASE        (periph_virt_base + 0x00007000)
#define DMA_CHAN_SIZE	  0x100 /* size of register space for a single DMA channel */
#define DMA_CHAN_MAX    14  /* number of DMA Channels we have... actually, there are 15... but channel fifteen is mapped at a different DMA_BASE, so we leave that one alone */
#define DMA_CHAN_NUM    14  /* the DMA Channel we are using, NOTE: DMA Ch 0 seems to be used by X... better not use it ;) */
#define DMA_CHAN_NUM_PI4  7
#define PWM_BASE_OFFSET 0x0020C000
#define PWM_BASE        (periph_virt_base + PWM_BASE_OFFSET)
#define PWM_PHYS_BASE   (periph_phys_base + PWM_BASE_OFFSET)
#define PWM_LEN         0x28
#define CLK_BASE_OFFSET 0x00101000
#define CLK_BASE        (periph_virt_base + CLK_BASE_OFFSET)
#define CLK_LEN         0xA8
#define GPIO_BASE_OFFSET  0x00200000
#define GPIO_BASE       (periph_virt_base + GPIO_BASE_OFFSET)
#define GPIO_PHYS_BASE  (periph_phys_base + GPIO_BASE_OFFSET)
#define GPIO_LEN        0x100
#define PCM_BASE_OFFSET 0x00203000
#define PCM_BASE        (periph_virt_base + PCM_BASE_OFFSET)
#define PCM_PHYS_BASE   (periph_phys_base + PCM_BASE_OFFSET)
#define PCM_LEN         0x24

#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)

#define DMA_CS              (0x00/4)
#define DMA_CONBLK_AD       (0x04/4)
#define DMA_DEBUG           (0x20/4)

#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)

#define GPIO_MODE_IN        0
#define GPIO_MODE_OUT       1

#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))

#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

#define DELAY_VIA_PWM 0
#define DELAY_VIA_PCM 1

/* New Board Revision format:
SRRR MMMM PPPP TTTT TTTT VVVV

S scheme (0=old, 1=new)
R RAM (0=256, 1=512, 2=1024)
M manufacturer (0='SONY',1='EGOMAN',2='EMBEST',3='UNKNOWN',4='EMBEST')
P processor (0=2835, 1=2836)
T type (0='A', 1='B', 2='A+', 3='B+', 4='Pi 2 B', 5='Alpha', 6='Compute Module', 11='4b')
V revision (0-15)
*/
#define BOARD_REVISION_SCHEME_MASK (0x1 << 23)
#define BOARD_REVISION_SCHEME_OLD (0x0 << 23)
#define BOARD_REVISION_SCHEME_NEW (0x1 << 23)
#define BOARD_REVISION_RAM_MASK (0x7 << 20)
#define BOARD_REVISION_MANUFACTURER_MASK (0xF << 16)
#define BOARD_REVISION_MANUFACTURER_SONY (0 << 16)
#define BOARD_REVISION_MANUFACTURER_EGOMAN (1 << 16)
#define BOARD_REVISION_MANUFACTURER_EMBEST (2 << 16)
#define BOARD_REVISION_MANUFACTURER_UNKNOWN (3 << 16)
#define BOARD_REVISION_MANUFACTURER_EMBEST2 (4 << 16)
#define BOARD_REVISION_PROCESSOR_MASK (0xF << 12)
#define BOARD_REVISION_PROCESSOR_2835 (0 << 12)
#define BOARD_REVISION_PROCESSOR_2836 (1 << 12)
#define BOARD_REVISION_TYPE_MASK (0xFF << 4)
#define BOARD_REVISION_TYPE_PI1_A (0 << 4)
#define BOARD_REVISION_TYPE_PI1_B (1 << 4)
#define BOARD_REVISION_TYPE_PI1_A_PLUS (2 << 4)
#define BOARD_REVISION_TYPE_PI1_B_PLUS (3 << 4)
#define BOARD_REVISION_TYPE_PI2_B (4 << 4)
#define BOARD_REVISION_TYPE_ALPHA (5 << 4)
#define BOARD_REVISION_TYPE_PI3_B (8 << 4)
#define BOARD_REVISION_TYPE_PI3_BP (0xD << 4)
#define BOARD_REVISION_TYPE_PI3_AP (0x7 << 5)
#define BOARD_REVISION_TYPE_CM (6 << 4)
#define BOARD_REVISION_TYPE_CM3 (10 << 4)
#define BOARD_REVISION_TYPE_PI4_B (0x11 << 4)
#define BOARD_REVISION_REV_MASK (0xF)

#define BUS_TO_PHYS(x) ((x)&~0xC0000000)

#ifdef DEBUG
#define dprintf(...) printf(__VA_ARGS__)
#else
#define dprintf(...)
#endif

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;

typedef struct {
  uint32_t info, src, dst, length,
     stride, next, pad[2];
} dma_cb_t;

struct ctl {
  uint32_t sample[NUM_SAMPLES];
  dma_cb_t cb[NUM_CBS];
};

static uint32_t periph_virt_base;
static uint32_t periph_phys_base;
static uint32_t mem_flag;

static volatile uint32_t *pwm_reg;
static volatile uint32_t *pcm_reg;
static volatile uint32_t *clk_reg;
static volatile uint32_t *dma_virt_base; /* base address of all DMA Channels */
static volatile uint32_t *dma_reg; /* pointer to the DMA Channel registers we are using */
static volatile uint32_t *gpio_reg;

static int delay_hw = DELAY_VIA_PWM;
static int invert_mode = 0;
static int daemonize = 1;

static float channel_pwm[MAX_CHANNELS];

static uint32_t plldfreq_mhz;
static int dma_chan;

static void set_pwm(int channel, float value);
static void update_pwm();
static void fatal(char *fmt, ...);

// open a char device file used for communicating with kernel mbox driver
int mbox_open() {
  int file_desc;

  // try to use /dev/vcio first (kernel 4.1+)
  file_desc = open(DEVFILE_VCIO, 0);
  if (file_desc < 0) {
    /* initialize mbox */
    unlink(DEVFILE_MBOX);
    if (mknod(DEVFILE_MBOX, S_IFCHR|0600, makedev(MAJOR_NUM, 0)) < 0)
        fatal("Failed to create mailbox device\n");
    file_desc = open(DEVFILE_MBOX, 0);
    if (file_desc < 0) {
        printf("Can't open device file: %s\n", DEVFILE_MBOX);
        perror(NULL);
        exit(-1);
    }
  }
  return file_desc;
}

void mbox_close(int file_desc) {
  close(file_desc);
}

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 gpio_set(int pin, int level) {
  if (level)
    gpio_reg[GPIO_SET0] = 1 << pin;
  else
    gpio_reg[GPIO_CLR0] = 1 << pin;
}

static void udelay(int us) {
  struct timespec ts = { 0, us * 1000 };

  nanosleep(&ts, NULL);
}

static void terminate(int dummy) {
  int i;

  dprintf("Resetting DMA...\n");
  if (dma_reg && mbox.virt_addr) {
    for (i = 0; i < num_channels; i++)
      channel_pwm[i] = 0;
    update_pwm();
    udelay(CYCLE_TIME_US);
    dma_reg[DMA_CS] = DMA_RESET;
    udelay(10);
  }

  dprintf("Freeing mbox memory...\n");
  if (mbox.virt_addr != NULL) {
    unmapmem(mbox.virt_addr, NUM_PAGES * PAGE_SIZE);
    if (mbox.handle <= 2) {
      /* we need to reopen mbox file */
      mbox.handle = mbox_open();
    }
    mem_unlock(mbox.handle, mbox.mem_ref);
    mem_free(mbox.handle, mbox.mem_ref);
    mbox_close(mbox.handle);
  }
  dprintf("Unlink %s...\n", DEVFILE);
  unlink(DEVFILE);
  dprintf("Unlink %s...\n", DEVFILE_MBOX);
  unlink(DEVFILE_MBOX);
  printf("pi-blaster stopped.\n");

  exit(1);
}

static void fatal(char *fmt, ...) {
  va_list ap;

  va_start(ap, fmt);
  vfprintf(stderr, fmt, ap);
  va_end(ap);
  terminate(0);
}

/*
 * determine which pi model we are running on
 */
static void get_model(unsigned mbox_board_rev) {
  int board_model = 0;

  if ((mbox_board_rev & BOARD_REVISION_SCHEME_MASK) == BOARD_REVISION_SCHEME_NEW) {
    if ((mbox_board_rev & BOARD_REVISION_TYPE_MASK) == BOARD_REVISION_TYPE_PI2_B) {
      board_model = 2;
    } else if ((mbox_board_rev & BOARD_REVISION_TYPE_MASK) == BOARD_REVISION_TYPE_PI3_B) {
      board_model = 3;
    } else if ((mbox_board_rev & BOARD_REVISION_TYPE_MASK) == BOARD_REVISION_TYPE_PI3_BP) {
      board_model = 3;
    } else if ((mbox_board_rev & BOARD_REVISION_TYPE_MASK) == BOARD_REVISION_TYPE_PI3_AP) {
      board_model = 3;
    } else if ((mbox_board_rev & BOARD_REVISION_TYPE_MASK) == BOARD_REVISION_TYPE_CM3) {
      board_model = 3;
    } else if ((mbox_board_rev & BOARD_REVISION_TYPE_MASK) == BOARD_REVISION_TYPE_PI4_B) {
      board_model = 4;
    } else {
      // no Pi 2, we assume a Pi 1
      board_model = 1;
    }
  } else {
    // if revision scheme is old, we assume a Pi 1
    board_model = 1;
  }

  switch(board_model) {
    case 1:
      periph_virt_base = 0x20000000;
      periph_phys_base = 0x7e000000;
      plldfreq_mhz = 500;
      dma_chan = DMA_CHAN_NUM;
      mem_flag         = MEM_FLAG_L1_NONALLOCATING | MEM_FLAG_ZERO;
      break;
    case 2:
    case 3:
      periph_virt_base = 0x3f000000;
      periph_phys_base = 0x7e000000;
      plldfreq_mhz = 500;
      dma_chan = DMA_CHAN_NUM;
      mem_flag         = MEM_FLAG_L1_NONALLOCATING | MEM_FLAG_ZERO;
      break;
    case 4:
      periph_virt_base = 0xFE000000;
      periph_phys_base = 0x7e000000;
      plldfreq_mhz = 750;
      dma_chan = DMA_CHAN_NUM_PI4;
      mem_flag         = MEM_FLAG_L1_NONALLOCATING | MEM_FLAG_ZERO;
      break;
    default:
      fatal("Unable to detect Board Model from board revision: %#x", mbox_board_rev);
      break;
  }
}

static uint32_t mem_virt_to_phys(void *virt) {
  uint32_t offset = (uint8_t *)virt - mbox.virt_addr;

  return mbox.bus_addr + offset;
}

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("pi-blaster: Failed to open /dev/mem: %m\n");
  vaddr = mmap(NULL, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base);
  if (vaddr == MAP_FAILED)
    fatal("pi-blaster: Failed to map peripheral at 0x%08x: %m\n", base);
  close(fd);

  return vaddr;
}

// Check if the pin provided is found in the list of known pins.
static int is_known_pin(int pin) {
  int found = 0;

  for (int i = 0; i < num_channels; i++) {
    if (known_pins[i] == pin) {
      found = 1;
      break;
    }
  }
  return(found);
}

// Check if the pin provided is found in the list of BANNED pins.
static int is_banned_pin(int pin) {
  int found = 0;

  for (int i = 0; i < num_channels; i++) {
    if (banned_pins[i] == pin) {
      found = 1;
      break;
    }
  }
  return(found);
}

// Set the pin to a pin2gpio element so pi-blaster can write to it,
// and set the width of the PWM pulse to the element with the same index
// in channel_pwm array.
static int set_pin2gpio(int pin, float width) {
  int established = 0;

  for (int i = 0; i < num_channels; i++) {
    if (pin2gpio[i] == pin || pin2gpio[i] == 0) {
      if (pin2gpio[i] == 0) {
        pin2gpio[i] = pin;
        gpio_set(pin, invert_mode);
        gpio_set_mode(pin, GPIO_MODE_OUT);
      }
      channel_pwm[i] = width;
      established = 1;
      break;
    }
  }

  return(established);
}

// To avoid storing the same pin 2 times after one pin has been released
// we compact the pin2gpio array so all ON PWM pins are at the begining.
static void compact_pin2gpio() {
  int i, j = 0;
  uint8_t tmp_pin2gpio[MAX_CHANNELS];
  float tmp_channel_pwm[MAX_CHANNELS];

  for (i = 0; i < num_channels; i++) {
    if (pin2gpio[i] != 0){
      tmp_pin2gpio[j] = pin2gpio[i];
      tmp_channel_pwm[j] = channel_pwm[i];
      j++;
    }
  }
  // Set the remaining slots in the arrays to 0, to disable them
  for (i = j; i < num_channels; i++) {
    tmp_pin2gpio[i] = 0;
    tmp_channel_pwm[i] = 0;
  }
  for (i = 0; i < num_channels; i++){
    pin2gpio[i] = tmp_pin2gpio[i];
    channel_pwm[i] = tmp_channel_pwm[i];
  }
}

// Pins can be relesead after being setup as PWM pins by writing the release <pin>
// command to the /dev/pi-blaster file. We make sure to compact the pin2gpio array
// that contains currently working pwm pins.
static int release_pin2gpio(int pin) {
  int released = 0;

  for (int i = 0; i < num_channels; i++) {
    if (pin2gpio[i] == pin) {
      channel_pwm[i] = 0;
      pin2gpio[i] = 0;
      released = 1;
      break;
    }
  }

  compact_pin2gpio();
  return(released);
}

// Set each provided pin to one in pin2gpio
static int set_pin(int pin, float width)
{
  int established = 0;

  if (is_known_pin(pin)) {
    established = set_pin2gpio(pin, width);
  } else {
    fprintf(stderr, "GPIO %d is not enabled for pi-blaster\n", pin);
  }

  return (established);
}

// Function make sure the pin we want to release is a valid pin, if it is
// then calls release_pin2gpio to delete it from currently ON pins.
static int release_pin(int pin) {
  int released = 0;

  if (is_known_pin(pin)){
    released = release_pin2gpio(pin);
  } else {
    fprintf(stderr, "GPIO %d is not enabled for pi-blaster\n", pin);
  }

  return(released);
}

// Releases the PWM pin requested (if found and valid) and updates the calls
// update_pwm to apply the changes to the actual hardware pins if needed.
static void release_pwm(int pin) {
  if (release_pin(pin)){
    update_pwm();
  }
}


// Set pin2gpio pins, channel width and update the pwm send to pins being used.
static void set_pwm(int channel, float width) {
  if (set_pin(channel, width)){
    update_pwm();
  }
}

// Run set_pwm on every known pin, setting all of them to the provided width.
static void set_all_pwm(float width) {
  for (int i = 0; i < num_channels; i++) {
      set_pwm(known_pins[i], width);
  }
}

/*
 * What we need to do here is:
 *   First DMA command turns on the pins that are >0
 *   All the other packets turn off the pins that are not used
 *
 * For the cpb packets (The DMA control packet)
 *  -> cbp[0]->dst = gpset0: set   the pwms that are active
 *  -> cbp[*]->dst = gpclr0: clear when the sample has a value
 *
 * For the samples     (The value that is written by the DMA command to cbp[n]->dst)
 *  -> dp[0] = mask of the pwms that are active
 *  -> dp[n] = mask of the pwm to stop at time n
 *
 * We dont really need to reset the cb->dst each time but I believe it helps a lot
 * in code readability in case someone wants to generate more complex signals.
 */
static void update_pwm() {
  uint32_t phys_gpclr0 = GPIO_PHYS_BASE + 0x28;
  uint32_t phys_gpset0 = GPIO_PHYS_BASE + 0x1c;
  struct ctl *ctl = (struct ctl *)mbox.virt_addr;
  uint32_t mask;

  /* First we turn on the channels that need to be on */
  /*   Take the first DMA Packet and set it's target to start pulse */
  if (invert_mode)
    ctl->cb[0].dst = phys_gpclr0;
  else
    ctl->cb[0].dst = phys_gpset0;

  /*   Now create a mask of all the pins that should be on */
  mask = 0;
  for (int i = 0; i < num_channels; i++) {
  // Check the pin2gpio pin has been set to avoid locking all of them as PWM.
    if (channel_pwm[i] > 0 && pin2gpio[i]) {
      mask |= 1 << pin2gpio[i];
    }
  }
  /*   And give that to the DMA controller to write */
  ctl->sample[0] = mask;

  /* Now we go through all the samples and turn the pins off when needed */
  for (int j = 1; j < NUM_SAMPLES; j++) {
    if (invert_mode)
      ctl->cb[j*2].dst = phys_gpset0;
    else
      ctl->cb[j*2].dst = phys_gpclr0;
    mask = 0;
    for (int i = 0; i < num_channels; i++) {
      // Check the pin2gpio pin has been set to avoid locking all of them as PWM.
      if ((float)j/NUM_SAMPLES > channel_pwm[i] && pin2gpio[i])
        mask |= 1 << pin2gpio[i];
    }
    ctl->sample[j] = mask;
  }
}

static void setup_sighandlers(void) {
  // 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);
  }
  signal(SIGWINCH, SIG_IGN);
}

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;

  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 (int i = 0; i < num_channels; i++) {
    mask |= 1 << known_pins[i];
  }
  for (int 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 (int 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 | (plldfreq_mhz<<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 | (plldfreq_mhz<<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);

    struct ctl *ctl = (struct ctl *)mbox.virt_addr;
    dma_cb_t *cbp = ctl->cb;

    for (int 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 (int 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 (int 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 (int i=0; i<DMA_CHAN_SIZE/4; i++) {
      printf("%04x: 0x%08x 0x%08x\n", i, &dma_reg[i], dma_reg[i]);
    }
#endif
}

static void debug_dump_samples() {
#ifdef DEBUG
  struct ctl *ctl = (struct ctl *)mbox.virt_addr;

  for (int i = 0; i < NUM_SAMPLES; i++) {
    printf("#%d @0x%08x, \n", i, ctl->sample[i]);
  }
#endif
}

static void init_pin2gpio(void) {
  for (int i = 0; i < num_channels; i++)
  pin2gpio[i] = 0;
}

static void init_pwm(void) {
  update_pwm();
}

static void init_channel_pwm(void) {
  dprintf("Initializing Channels...\n");

  for (int i = 0; i < num_channels; i++)
    channel_pwm[i] = 0;
}

static void go_go_go(void)
{
  FILE *fp;

  if ((fp = fopen(DEVFILE, "r+")) == NULL)
    fatal("pi-blaster: Failed to open %s: %m\n", DEVFILE);

  char *lineptr = NULL, nl;
  size_t linelen;

  for (;;) {
    int bad_input = 1;
    int n, servo;
    int charcnt = 0;
    int argcnt = 0;
    float value;
    char *charptr = NULL;

    if ((n = getline(&lineptr, &linelen, fp)) < 0)
      continue;
    dprintf("[%d]%s", n, lineptr);

    charptr = lineptr;
    if (!strcmp(charptr, "debug_regs\n")) {
      debug_dump_hw();
      bad_input = 0;
    } else if (!strcmp(charptr, "debug_samples\n")) {
      debug_dump_samples();
      bad_input = 0;
    } else if (sscanf(charptr, "release %d%c", &servo, &nl) == 2 && nl == '\n') {
      release_pwm(servo);
      bad_input = 0;
    } else if (sscanf(charptr, "*=%f%c", &value, &nl) == 2 && nl == '\n') {
      if (value < 0 || value > 1) {
        fprintf(stderr, "Invalid value %f\n", value);
      } else {
        set_all_pwm(value);
      }
      bad_input = 0;
    } else do {
      argcnt = sscanf(charptr, "%d=%f%c%n", &servo, &value, &nl, &charcnt);
      if (argcnt == 3)
      {
        if (servo < 0) {
          fprintf(stderr, "Invalid channel number %d\n", servo);
        } else if (value < 0 || value > 1) {
          fprintf(stderr, "Invalid value %f\n", value);
        } else {
          set_pin(servo, value);
        }
        if (nl == '\n') {
          update_pwm();
        } else {
          charptr += charcnt;
        }
      }
      bad_input = argcnt < 3;
    } while (nl != '\n' && argcnt == 3);

    if (bad_input) {
      fprintf(stderr, "Bad input: %s", lineptr);
    }

    if (lineptr) {
      free(lineptr);
      lineptr = NULL;
      charptr = NULL;
      linelen = 0;
    }
  }
}

void parseargs(int argc, char **argv)
{
  int index;
  int c;
  static uint8_t temp_known_pins[MAX_CHANNELS];
  int i=0;

  static struct option longopts[] =
  {
    {"help", no_argument, 0, 'h'},
    {"foreground", no_argument, 0, 'f'},
    {"gpio", required_argument, 0, 'g'},
    {"invert", no_argument, 0, 'i'},
    {"pcm", no_argument, 0, 'p'},
    {"version", no_argument, 0, 'v'},
    {0, 0, 0, 0}
  };

  while (1)
  {
    index = 0;
    c = getopt_long(argc, argv, "Dfg:hipv", longopts, &index);

    if (c == -1)
      break;

    switch (c)
    {
    case 0:
      /* handle flag options (array's 3rd field non-0) */
      break;

    case 'h':
      fprintf(stderr, "%s version %s\n", argv[0], VERSION);
      fprintf(stderr, "Usage: %s [-hDgipv]\n"
        "-h (--help)       - this information\n"
        "-D                - Don't daemonize (deprecated: use --foreground instead)\n"
        "-f (--foreground) - run in the foreground\n"
        "-g (--gpio)       - comma separated list of GPIOs to use\n"
        "                    If omitted, default is \"4,17,18,27,21,22,23,24,25\"\n"
        "-i (--invert)     - invert pin output (pulse LOW)\n"
        "-p (--pcm)        - use pcm for dmascheduling\n"
        "-v (--version)    - version information\n"
        , argv[0]);
      exit(-1);

    case 'D':
    case 'f':
      daemonize = 0;
      break;

    case 'g':
      if (optarg) {
        int temp=0;

        const char s[2] = ",";
        char *token;

        /* get the first token */
        token = strtok(optarg, s);

        /* walk through other tokens */
        i=0;
        while( token != NULL )
        {
          if ( (temp = atoi(token)) && (temp>=0) && (temp < MAX_CHANNELS) ){
            if (i>=MAX_CHANNELS) {
              printf( "ERROR maximum number of channels (%d) exceeded.\n", MAX_CHANNELS);
              exit(-1);
            }
            if (!is_banned_pin(temp)) {
              temp_known_pins[i++] = temp;
            } else {
              printf( "ERROR '%s' is a banned gpio\n", token);
              exit(-1);
            }
          } else {
            printf( "ERROR '%s' is an invalid gpio\n", token);
            exit(-1);
          }
          token = strtok(NULL, s);
        }
      }
      num_channels = i;
      for (i=0; i<MAX_CHANNELS; i++) {
        if (i<num_channels) {
          known_pins[i] = temp_known_pins[i];
        } else {
          known_pins[i] = 0;
        }
      }
      break;

    case 'i':
      invert_mode = 1;
      break;

    case 'p':
      delay_hw = DELAY_VIA_PCM;
      break;

    case 'v':
      fprintf(stderr, "%s version %s\n", argv[0], VERSION);
      exit(-1);

    case '?':
      /* getopt_long already reported error? */
      exit(-1);

    default:
      exit(-1);
    }
  }
}

int main(int argc, char **argv) {
  parseargs(argc, argv);
  mbox.handle = mbox_open();
  if (mbox.handle < 0)
    fatal("Failed to open mailbox\n");
  unsigned mbox_board_rev = get_board_revision(mbox.handle);
  printf("MBox Board Revision: %#x\n", mbox_board_rev);
  get_model(mbox_board_rev);
  unsigned mbox_dma_channels = get_dma_channels(mbox.handle);
  printf("DMA Channels Info: %#x, using DMA Channel: %d\n", mbox_dma_channels, dma_chan);

  printf("Using hardware:                 %5s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
  printf("Number of channels:             %5d\n", (int)num_channels);
  printf("PWM frequency:               %5d Hz\n", 1000000/CYCLE_TIME_US);
  printf("PWM steps:                      %5d\n", NUM_SAMPLES);
  printf("Maximum period (100  %%):      %5dus\n", CYCLE_TIME_US);
  printf("Minimum period (%1.3f%%):      %5dus\n", 100.0*SAMPLE_US / CYCLE_TIME_US, SAMPLE_US);
  printf("DMA Base:                  %#010x\n", DMA_BASE);

  setup_sighandlers();

  /* map the registers for all DMA Channels */
  dma_virt_base = map_peripheral(DMA_BASE, (DMA_CHAN_SIZE * (DMA_CHAN_MAX + 1)));
  /* set dma_reg to point to the DMA Channel we are using */
  dma_reg = dma_virt_base + dma_chan * (DMA_CHAN_SIZE / sizeof(dma_reg));
  pwm_reg = map_peripheral(PWM_BASE, PWM_LEN);
  pcm_reg = map_peripheral(PCM_BASE, PCM_LEN);
  clk_reg = map_peripheral(CLK_BASE, CLK_LEN);
  gpio_reg = map_peripheral(GPIO_BASE, GPIO_LEN);

  /* Use the mailbox interface to the VC to ask for physical memory */
  mbox.mem_ref = mem_alloc(mbox.handle, NUM_PAGES * PAGE_SIZE, PAGE_SIZE, mem_flag);
  /* TODO: How do we know that succeeded? */
  dprintf("mem_ref %u\n", mbox.mem_ref);
  mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
  dprintf("bus_addr = %#x\n", mbox.bus_addr);
  mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), NUM_PAGES * PAGE_SIZE);
  dprintf("virt_addr %p\n", mbox.virt_addr);

  if ((unsigned long)mbox.virt_addr & (PAGE_SIZE-1))
    fatal("pi-blaster: Virtual address is not page aligned\n");

  /* we are done with the mbox */
  mbox_close(mbox.handle);
  mbox.handle = -1;

  //fatal("TempFatal\n");

  init_ctrl_data();
  init_hardware();
  init_channel_pwm();
  // Init pin2gpio array with 0/false values to avoid locking all of them as PWM.
  init_pin2gpio();
  // Only calls update_pwm after ctrl_data calculates the pin mask to unlock all pins on start.
  init_pwm();
  unlink(DEVFILE);
  if (mkfifo(DEVFILE, 0666) < 0)
    fatal("pi-blaster: Failed to create %s: %m\n", DEVFILE);
  if (chmod(DEVFILE, 0666) < 0)
    fatal("pi-blaster: Failed to set permissions on %s: %m\n", DEVFILE);

  printf("Initialised, ");
  if (daemonize) {
    if (daemon(0,1) < 0)
      fatal("pi-blaster: Failed to daemonize process: %m\n");
    else
      printf("Daemonized, ");
  }
  printf("Reading %s.\n", DEVFILE);

  go_go_go();

  return 0;
}