random version of pifm, not merged stereo support yet?

pifm.c

//  wget -O - http://beta.etherpad.org/p/pihackfm/export/txt 2>/dev/null | gcc -lm -std=c99 -g -xc - && ./a.out beatles.wav
// Access from ARM Running Linux

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <dirent.h>
#include <math.h>
#include <fcntl.h>
#include <assert.h>
#include <malloc.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <signal.h>
#include <unistd.h>
#include <sched.h>

#define PAGE_SIZE (4*1024)
#define BLOCK_SIZE (4*1024)
#define SLEEP_CONST 1000

int  mem_fd;
char *gpio_mem, *gpio_map;
char *spi0_mem, *spi0_map;

// I/O access
volatile unsigned *gpio;
volatile unsigned *allof7e;

// GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or SET_GPIO_ALT(x,y)
#define INP_GPIO(g) *(gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
#define OUT_GPIO(g) *(gpio+((g)/10)) |=  (1<<(((g)%10)*3))
#define SET_GPIO_ALT(g,a) *(gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))

#define GPIO_SET *(gpio+7)  // sets   bits which are 1 ignores bits which are 0
#define GPIO_CLR *(gpio+10) // clears bits which are 1 ignores bits which are 0

#define ACCESS(base) *(volatile int*)((int)allof7e+base-0x7e000000)
#define SETBIT(base, bit) ACCESS(base) |= 1<<bit
#define CLRBIT(base, bit) ACCESS(base) &= ~(1<<bit)

#define CM_GP0CTL (0x7e101070)
#define GPFSEL0 (0x7E200000)
#define CM_GP0DIV (0x7e101074)
#define CLKBASE (0x7E101000)
#define DMABASE (0x7E007000)
#define PWMBASE  (0x7e20C000) /* PWM controller */

struct GPCTL {
  char SRC         : 4;
  char ENAB        : 1;
  char KILL        : 1;
  char             : 1;
  char BUSY        : 1;
  char FLIP        : 1;
  char MASH        : 2;
  unsigned int     : 13;
  char PASSWD      : 8;
};

void getRealMemPage(void** vAddr, void** pAddr) {
  void* a = valloc(PAGE_SIZE);
  
  ((int*)a)[0] = 1;  // use page to force allocation.
  
  mlock(a, PAGE_SIZE);  // lock into ram.
  
  *vAddr = a;  // yay - we know the virtual address
  
  unsigned long long frameinfo;
  
  int fp = open("/proc/self/pagemap", 'r');
  lseek(fp, ((int)a)/4096*8, SEEK_SET);
  read(fp, &frameinfo, sizeof(frameinfo));
  
  *pAddr = (void*)((int)(frameinfo*PAGE_SIZE));
}

void freeRealMemPage(void* vAddr) {
  munlock(vAddr, PAGE_SIZE);  // unlock ram.
  
  free(vAddr);
}

void setup_fm() {
  // open /dev/mem
  if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
    printf("can't open /dev/mem \n");
    exit (-1);
  }

  allof7e = (unsigned *)mmap(
                NULL,
                0x01000000,  //len
                PROT_READ|PROT_WRITE,
                MAP_SHARED,
                mem_fd,
                0x20000000  //base
            );

  close(mem_fd); //No need to keep mem_fd open after mmap

  if ((int)allof7e==-1) exit(-1);

  SETBIT(GPFSEL0 , 14);
  CLRBIT(GPFSEL0 , 13);
  CLRBIT(GPFSEL0 , 12);

  struct GPCTL setupword = {6/*SRC*/, 1, 0, 0, 0, 1,0x5a};

  ACCESS(CM_GP0CTL) = *((int*)&setupword);
}

struct CB {
  volatile unsigned int TI;
  volatile unsigned int SOURCE_AD;
  volatile unsigned int DEST_AD;
  volatile unsigned int TXFR_LEN;
  volatile unsigned int STRIDE;
  volatile unsigned int NEXTCONBK;
  volatile unsigned int RES1;
  volatile unsigned int RES2;
};

struct DMAregs {
  volatile unsigned int CS;
  volatile unsigned int CONBLK_AD;
  volatile unsigned int TI;
  volatile unsigned int SOURCE_AD;
  volatile unsigned int DEST_AD;
  volatile unsigned int TXFR_LEN;
  volatile unsigned int STRIDE;
  volatile unsigned int NEXTCONBK;
  volatile unsigned int DEBUG;
};

struct PageInfo {
  void* p;  // physical address
  void* v;  // virtual address
};

struct PageInfo constPage;
struct PageInfo instrPage;
struct PageInfo instrs[1024];

void playWav(char* filename, float samplerate) {
  int fp = STDIN_FILENO;

  if(filename[0]!='-') fp = open(filename, O_NONBLOCK | O_RDONLY | O_SYNC);

  short data;
  int datatemp;

  for (int i=0; i<11; i++) {
    if(i == 6 && samplerate == -1) {  // get samplerate
      read(fp, &datatemp, 4);
      samplerate = (float) datatemp;
      printf("no samplerate given, autodetected: %.2f\n", samplerate);
    } else {
      read(fp, &datatemp, 4);  // read past header
    }
  }

  const int clocksPerSample = (22500.0*1400.0)/samplerate;  // for timing
  const int clocksPerSample2 = clocksPerSample/2;
  printf("Clocks per sample %d\n", clocksPerSample);
  const int cPp1 = (int)constPage.p + 2048 + 4;
  const int cPp2 = (int)constPage.p + 2048 - 4;
  register struct PageInfo * pi = instrs;
  const struct PageInfo * pi_max = instrs + 1024;
  register volatile int* status = (volatile int*)((int)allof7e+DMABASE + 0x04-0x7e000000);

  while (read(fp, &data, 2)) {
    const int bigval = data << 5;
    const int intval4 = (bigval >> 15) << 2;
    const unsigned int fracval = (((bigval- (intval4 << 13))*clocksPerSample) >> 16) + clocksPerSample2;

    ++pi;
    while( *status ==  (int)(pi->p)) {
      usleep(SLEEP_CONST);
    }
    ((struct CB*)(pi->v))->SOURCE_AD = cPp2 + intval4;

    ++pi;
    while( *status ==  (int)(pi->p)) {
      usleep(SLEEP_CONST);
    }
    ((struct CB*)(pi->v))->TXFR_LEN = clocksPerSample-fracval;

    ++pi;
    while( *status ==  (int)(pi->p)) {
      usleep(SLEEP_CONST);
    }
    ((struct CB*)(pi->v))->SOURCE_AD = cPp1 + intval4;

    ++pi;
    if (pi >= pi_max) { 
      pi = instrs;
    }
    while( *status ==  (int)(pi->p)) {
      usleep(SLEEP_CONST);
    }
    ((struct CB*)(pi->v))->TXFR_LEN = fracval;
  }

  close(fp);
}

void unSetupDMA() {
  printf("exiting\n");
  struct DMAregs* DMA0 = (struct DMAregs*)&(ACCESS(DMABASE));
  DMA0->CS =1<<31;  // reset dma controller
  // reset GPIO4
  system("/usr/bin/gpio reset");
}

void handSig() {
  exit(0);
}

void setupDMA( float centerFreq ) {
  atexit(unSetupDMA);
  signal(SIGINT, handSig);
  signal(SIGTERM, handSig);
  signal(SIGHUP, handSig);
  signal(SIGQUIT, handSig);

  // allocate a few pages of ram
  getRealMemPage(&constPage.v, &constPage.p);

  int centerFreqDivider = (int)((500.0 / centerFreq) * (float)(1<<12) + 0.5);

  // make data page contents - it's essientially 1024 different commands for the
  // DMA controller to send to the clock module at the correct time.
  for (int i=0; i<1024; i++) {
   ((int*)(constPage.v))[i] = (0x5a << 24) + centerFreqDivider - 512 + i;
  }

  int instrCnt = 0;

  while (instrCnt<1024) {
    getRealMemPage(&instrPage.v, &instrPage.p);

    // make copy instructions
    struct CB* instr0= (struct CB*)instrPage.v;

    for (int i=0; i<4096/sizeof(struct CB); i++) {
      instrs[instrCnt].v = (void*)((int)instrPage.v + sizeof(struct CB)*i);
      instrs[instrCnt].p = (void*)((int)instrPage.p + sizeof(struct CB)*i);
      instr0->SOURCE_AD = (unsigned int)constPage.p+2048;
      instr0->DEST_AD = PWMBASE+0x18 /* FIF1 */;
      instr0->TXFR_LEN = 4;
      instr0->STRIDE = 0;
      //instr0->NEXTCONBK = (int)instrPage.p + sizeof(struct CB)*(i+1);
      instr0->TI = (1/* DREQ  */<<6) | (5 /* PWM */<<16) |  (1<<26/* no wide*/) ;
      instr0->RES1 = 0;
      instr0->RES2 = 0;

      if (i%2) {
        instr0->DEST_AD = CM_GP0DIV;
        instr0->STRIDE = 4;
        instr0->TI = (1<<26/* no wide*/) ;
      }

      if (instrCnt!=0) {
        ((struct CB*)(instrs[instrCnt-1].v))->NEXTCONBK = (int)instrs[instrCnt].p;
      }
      instr0++;
      instrCnt++;
    }
  }
  ((struct CB*)(instrs[1023].v))->NEXTCONBK = (int)instrs[0].p;

  // set up a clock for the PWM
  ACCESS(CLKBASE + 40*4 /*PWMCLK_CNTL*/) = 0x5A000026;
  usleep(SLEEP_CONST);
  ACCESS(CLKBASE + 41*4 /*PWMCLK_DIV*/)  = 0x5A002800;
  ACCESS(CLKBASE + 40*4 /*PWMCLK_CNTL*/) = 0x5A000016;
  usleep(SLEEP_CONST); 

  // set up pwm
  ACCESS(PWMBASE + 0x0 /* CTRL*/) = 0;
  usleep(SLEEP_CONST);
  ACCESS(PWMBASE + 0x4 /* status*/) = -1;  // clear errors
  usleep(SLEEP_CONST);
  ACCESS(PWMBASE + 0x0 /* CTRL*/) = -1; //(1<<13 /* Use fifo */) | (1<<10 /* repeat */) | (1<<9 /* serializer */) | (1<<8 /* enable ch */) ;
  usleep(SLEEP_CONST);
  ACCESS(PWMBASE + 0x8 /* DMAC*/) = (1<<31 /* DMA enable */) | 0x0707;

  //activate dma
  struct DMAregs* DMA0 = (struct DMAregs*)&(ACCESS(DMABASE));
  DMA0->CS =1<<31;  // reset
  DMA0->CONBLK_AD=0; 
  DMA0->TI=0; 
  DMA0->CONBLK_AD = (unsigned int)(instrPage.p);
  DMA0->CS =(1<<0)|(255 <<16);  // enable bit = 0, clear end flag = 1, prio=19-16
}

int main(int argc, char **argv) {
  if(getuid()!=0) {
    fprintf(stderr, "has to be run as root\n");
    return 1;
  }

  nice(-20);
  struct sched_param sp;
  sp.sched_priority = sched_get_priority_max(SCHED_RR);
  sched_setscheduler(0, SCHED_RR, &sp);

  if (argc>1) {
    setup_fm();
    if(argc>2) {
      setupDMA(atof(argv[2]));
    } else {
      printf("no transmission frequency given, defaulting to 103.3\n");
      setupDMA(103.3);
    }
    playWav(argv[1],argc>3?atof(argv[3]):-1);
  } else {
    fprintf(stderr, "Usage: %s wavfile.wav [freq] [sample rate]\n\nWhere wavfile is 16 bit 22.5kHz Mono.\nfreq is in Mhz (default 103.3)\nsample rate of wav file in Hz\n\nPlay an empty file to transmit silence\n", argv[0]);
  }
  return 0;
}