git.sesse.net Git - pistorm/blob - emulator.c

   1 #include <assert.h>
   2 #include <dirent.h>
   3 #include <endian.h>
   4 #include <fcntl.h>
   5 #include <pthread.h>
   6 #include <sched.h>
   7 #include <signal.h>
   8 #include <stdint.h>
   9 #include <stdio.h>
  10 #include <stdlib.h>
  11 #include <string.h>
  12 #include <sys/mman.h>
  13 #include <sys/stat.h>
  14 #include <sys/types.h>
  15 #include <unistd.h>
  16 #include "Gayle.h"
  17 #include "ide.h"
  18 #include "m68k.h"
  19 #include "main.h"
  20
  21 //#define BCM2708_PERI_BASE        0x20000000  //pi0-1
  22 //#define BCM2708_PERI_BASE     0xFE000000     //pi4
  23 #define BCM2708_PERI_BASE 0x3F000000  // pi3
  24 #define BCM2708_PERI_SIZE 0x01000000
  25 #define GPIO_BASE (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */
  26 #define GPCLK_BASE (BCM2708_PERI_BASE + 0x101000)
  27 #define GPIO_ADDR 0x200000 /* GPIO controller */
  28 #define GPCLK_ADDR 0x101000
  29 #define CLK_PASSWD 0x5a000000
  30 #define CLK_GP0_CTL 0x070
  31 #define CLK_GP0_DIV 0x074
  32
  33 #define SA0 5
  34 #define SA1 3
  35 #define SA2 2
  36
  37 #define STATUSREGADDR  \
  38   GPIO_CLR = 1 << SA0; \
  39   GPIO_CLR = 1 << SA1; \
  40   GPIO_SET = 1 << SA2;
  41 #define W16            \
  42   GPIO_CLR = 1 << SA0; \
  43   GPIO_CLR = 1 << SA1; \
  44   GPIO_CLR = 1 << SA2;
  45 #define R16            \
  46   GPIO_SET = 1 << SA0; \
  47   GPIO_CLR = 1 << SA1; \
  48   GPIO_CLR = 1 << SA2;
  49 #define W8             \
  50   GPIO_CLR = 1 << SA0; \
  51   GPIO_SET = 1 << SA1; \
  52   GPIO_CLR = 1 << SA2;
  53 #define R8             \
  54   GPIO_SET = 1 << SA0; \
  55   GPIO_SET = 1 << SA1; \
  56   GPIO_CLR = 1 << SA2;
  57
  58 #define PAGE_SIZE (4 * 1024)
  59 #define BLOCK_SIZE (4 * 1024)
  60
  61 #define GPIOSET(no, ishigh) \
  62   do {                      \
  63     if (ishigh)             \
  64       set |= (1 << (no));   \
  65     else                    \
  66       reset |= (1 << (no)); \
  67   } while (0)
  68
  69 #define FASTBASE 0x07FFFFFF
  70 #define FASTSIZE 0xFFFFFFF
  71 #define GAYLEBASE 0xD80000  // D7FFFF
  72 #define GAYLESIZE 0x6FFFF
  73
  74 #define KICKBASE 0xF80000
  75 #define KICKSIZE 0x7FFFF
  76
  77 int mem_fd;
  78 int mem_fd_gpclk;
  79 int gayle_emulation_enabled = 1;
  80 void *gpio_map;
  81 void *gpclk_map;
  82
  83 // I/O access
  84 volatile unsigned int *gpio;
  85 volatile unsigned int *gpclk;
  86 volatile unsigned int gpfsel0;
  87 volatile unsigned int gpfsel1;
  88 volatile unsigned int gpfsel2;
  89 volatile unsigned int gpfsel0_o;
  90 volatile unsigned int gpfsel1_o;
  91 volatile unsigned int gpfsel2_o;
  92
  93 // GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or
  94 // SET_GPIO_ALT(x,y)
  95 #define INP_GPIO(g) *(gpio + ((g) / 10)) &= ~(7 << (((g) % 10) * 3))
  96 #define OUT_GPIO(g) *(gpio + ((g) / 10)) |= (1 << (((g) % 10) * 3))
  97 #define SET_GPIO_ALT(g, a)  \
  98   *(gpio + (((g) / 10))) |= \
  99       (((a) <= 3 ? (a) + 4 : (a) == 4 ? 3 : 2) << (((g) % 10) * 3))
 100
 101 #define GPIO_SET \
 102   *(gpio + 7)  // sets   bits which are 1 ignores bits which are 0
 103 #define GPIO_CLR \
 104   *(gpio + 10)  // clears bits which are 1 ignores bits which are 0
 105
 106 #define GET_GPIO(g) (*(gpio + 13) & (1 << g))  // 0 if LOW, (1<<g) if HIGH
 107
 108 #define GPIO_PULL *(gpio + 37)      // Pull up/pull down
 109 #define GPIO_PULLCLK0 *(gpio + 38)  // Pull up/pull down clock
 110
 111 void setup_io();
 112
 113 uint32_t read8(uint32_t address);
 114 void write8(uint32_t address, uint32_t data);
 115
 116 uint32_t read16(uint32_t address);
 117 void write16(uint32_t address, uint32_t data);
 118
 119 void write32(uint32_t address, uint32_t data);
 120 uint32_t read32(uint32_t address);
 121
 122 uint16_t read_reg(void);
 123 void write_reg(unsigned int value);
 124
 125 volatile uint16_t srdata;
 126 volatile uint32_t srdata2;
 127 volatile uint32_t srdata2_old;
 128
 129 unsigned char g_kick[524288];
 130 unsigned char g_ram[FASTSIZE + 1]; /* RAM */
 131 unsigned char toggle;
 132 static volatile unsigned char ovl;
 133 static volatile unsigned char maprom;
 134
 135 void sigint_handler(int sig_num) {
 136   printf("\n Exit Ctrl+C %d\n", sig_num);
 137   exit(0);
 138 }
 139
 140 void *iplThread(void *args) {
 141   printf("thread!/n");
 142
 143   while (42) {
 144     usleep(1);
 145   }
 146 }
 147
 148 int main(int argc, char *argv[]) {
 149   int g;
 150   const struct sched_param priority = {99};
 151
 152   // Some command line switch stuffles
 153   for (g = 1; g < argc; g++) {
 154     if (strcmp(argv[g], "--disable-gayle") == 0) {
 155       gayle_emulation_enabled = 0;
 156     }
 157   }
 158
 159   sched_setscheduler(0, SCHED_FIFO, &priority);
 160   mlockall(MCL_CURRENT);  // lock in memory to keep us from paging out
 161
 162   InitGayle();
 163
 164   signal(SIGINT, sigint_handler);
 165   setup_io();
 166
 167   // Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending
 168   // on pi model
 169   printf("Enable 200MHz GPCLK0 on GPIO4\n");
 170
 171   *(gpclk + (CLK_GP0_CTL / 4)) = CLK_PASSWD | (1 << 5);
 172   usleep(10);
 173   while ((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7))
 174     ;
 175   usleep(100);
 176   *(gpclk + (CLK_GP0_DIV / 4)) =
 177       CLK_PASSWD | (6 << 12);  // divider , 6=200MHz on pi3
 178   usleep(10);
 179   *(gpclk + (CLK_GP0_CTL / 4)) =
 180       CLK_PASSWD | 5 | (1 << 4);  // pll? 6=plld, 5=pllc
 181   usleep(10);
 182   while (((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7)) == 0)
 183     ;
 184   usleep(100);
 185
 186   SET_GPIO_ALT(4, 0);  // gpclk0
 187
 188   // set SA to output
 189   INP_GPIO(2);
 190   OUT_GPIO(2);
 191   INP_GPIO(3);
 192   OUT_GPIO(3);
 193   INP_GPIO(5);
 194   OUT_GPIO(5);
 195
 196   // set gpio0 (aux0) and gpio1 (aux1) to input
 197   INP_GPIO(0);
 198   INP_GPIO(1);
 199
 200   // Set GPIO pins 6,7 and 8-23 to output
 201   for (g = 6; g <= 23; g++) {
 202     INP_GPIO(g);
 203     OUT_GPIO(g);
 204   }
 205   printf("Precalculate GPIO8-23 as Output\n");
 206   gpfsel0_o = *(gpio);  // store gpio ddr
 207   printf("gpfsel0: %#x\n", gpfsel0_o);
 208   gpfsel1_o = *(gpio + 1);  // store gpio ddr
 209   printf("gpfsel1: %#x\n", gpfsel1_o);
 210   gpfsel2_o = *(gpio + 2);  // store gpio ddr
 211   printf("gpfsel2: %#x\n", gpfsel2_o);
 212
 213   // Set GPIO pins 8-23 to input
 214   for (g = 8; g <= 23; g++) {
 215     INP_GPIO(g);
 216   }
 217   printf("Precalculate GPIO8-23 as Input\n");
 218   gpfsel0 = *(gpio);  // store gpio ddr
 219   printf("gpfsel0: %#x\n", gpfsel0);
 220   gpfsel1 = *(gpio + 1);  // store gpio ddr
 221   printf("gpfsel1: %#x\n", gpfsel1);
 222   gpfsel2 = *(gpio + 2);  // store gpio ddr
 223   printf("gpfsel2: %#x\n", gpfsel2);
 224
 225   GPIO_CLR = 1 << 2;
 226   GPIO_CLR = 1 << 3;
 227   GPIO_SET = 1 << 5;
 228
 229   GPIO_SET = 1 << 6;
 230   GPIO_SET = 1 << 7;
 231
 232   // reset cpld statemachine first
 233
 234   write_reg(0x01);
 235   usleep(100);
 236   usleep(1500);
 237   write_reg(0x00);
 238   usleep(100);
 239
 240   // load kick.rom if present
 241   maprom = 1;
 242   int fd = 0;
 243   fd = open("kick.rom", O_RDONLY);
 244   if (fd < 1) {
 245     printf("Failed loading kick.rom, using motherboard kickstart\n");
 246     maprom = 0;
 247   } else {
 248     int size = (int)lseek(fd, 0, SEEK_END);
 249     if (size == 0x40000) {
 250       lseek(fd, 0, SEEK_SET);
 251       read(fd, &g_kick, size);
 252       lseek(fd, 0, SEEK_SET);
 253       read(fd, &g_kick[0x40000], size);
 254     } else {
 255       lseek(fd, 0, SEEK_SET);
 256       read(fd, &g_kick, size);
 257     }
 258     printf("Loaded kick.rom with size %d kib\n", size / 1024);
 259   }
 260
 261   // reset amiga and statemachine
 262   cpu_pulse_reset();
 263   ovl = 1;
 264   m68k_write_memory_8(0xbfe201, 0x0001);  // AMIGA OVL
 265   m68k_write_memory_8(0xbfe001, 0x0001);  // AMIGA OVL high (ROM@0x0)
 266
 267   usleep(1500);
 268
 269   m68k_init();
 270   m68k_set_cpu_type(M68K_CPU_TYPE_68040);
 271   m68k_pulse_reset();
 272
 273   if (maprom == 1) {
 274     m68k_set_reg(M68K_REG_PC, 0xF80002);
 275   } else {
 276     m68k_set_reg(M68K_REG_PC, 0x0);
 277   }
 278
 279   /*
 280            pthread_t id;
 281            int err;
 282           //err = pthread_create(&id, NULL, &iplThread, NULL);
 283           if (err != 0)
 284               printf("\ncan't create IPL thread :[%s]", strerror(err));
 285           else
 286               printf("\n IPL Thread created successfully\n");
 287   */
 288
 289   m68k_pulse_reset();
 290   while (42) {
 291     m68k_execute(300);
 292
 293     if (GET_GPIO(1) == 0){
 294       srdata = read_reg();
 295       m68k_set_irq((srdata >> 13) & 0xff);
 296     } else {
 297 //      if (CheckIrq() == 1)
 298 //       m68k_set_irq(2);
 299 //      else
 300          m68k_set_irq(0);
 301     };
 302
 303   }
 304
 305   return 0;
 306 }
 307
 308 void cpu_pulse_reset(void) {
 309   write_reg(0x00);
 310   // printf("Status Reg%x\n",read_reg());
 311   usleep(100000);
 312   write_reg(0x02);
 313   // printf("Status Reg%x\n",read_reg());
 314 }
 315
 316 int cpu_irq_ack(int level) {
 317   printf("cpu irq ack\n");
 318   return level;
 319 }
 320
 321 unsigned int m68k_read_memory_8(unsigned int address) {
 322   if (address > FASTBASE && address < FASTBASE + FASTSIZE) {
 323     return g_ram[address - FASTBASE];
 324   }
 325
 326   if (maprom == 1) {
 327     if (address > KICKBASE && address < KICKBASE + KICKSIZE) {
 328       return g_kick[address - KICKBASE];
 329     }
 330   }
 331
 332   if (gayle_emulation_enabled) {
 333     if (address > GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 334       return readGayleB(address);
 335     }
 336   }
 337
 338   if (address < 0xffffff) {
 339     return read8((uint32_t)address);
 340   }
 341
 342   return 1;
 343 }
 344
 345 unsigned int m68k_read_memory_16(unsigned int address) {
 346   if (address > FASTBASE && address < FASTBASE + FASTSIZE) {
 347     return be16toh(*(uint16_t *)&g_ram[address - FASTBASE]);
 348   }
 349
 350   if (maprom == 1) {
 351     if (address > KICKBASE && address < KICKBASE + KICKSIZE) {
 352       return be16toh(*(uint16_t *)&g_kick[address - KICKBASE]);
 353     }
 354   }
 355
 356   if (gayle_emulation_enabled) {
 357     if (address > GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 358       return readGayle(address);
 359     }
 360   }
 361
 362   if (address < 0xffffff) {
 363     return (unsigned int)read16((uint32_t)address);
 364   }
 365
 366   return 1;
 367 }
 368
 369 unsigned int m68k_read_memory_32(unsigned int address) {
 370   if (address > FASTBASE && address < FASTBASE + FASTSIZE) {
 371     return be32toh(*(uint32_t *)&g_ram[address - FASTBASE]);
 372   }
 373
 374   if (maprom == 1) {
 375     if (address > KICKBASE && address < KICKBASE + KICKSIZE) {
 376       return be32toh(*(uint32_t *)&g_kick[address - KICKBASE]);
 377     }
 378   }
 379
 380   if (gayle_emulation_enabled) {
 381     if (address > GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 382       return readGayleL(address);
 383     }
 384   }
 385
 386   if (address < 0xffffff) {
 387     uint16_t a = read16(address);
 388     uint16_t b = read16(address + 2);
 389     return (a << 16) | b;
 390   }
 391
 392   return 1;
 393 }
 394
 395 void m68k_write_memory_8(unsigned int address, unsigned int value) {
 396   if (address > FASTBASE && address < FASTBASE + FASTSIZE) {
 397     g_ram[address - FASTBASE] = value;
 398     return;
 399   }
 400
 401   if (gayle_emulation_enabled) {
 402     if (address > GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 403       writeGayleB(address, value);
 404       return;
 405     }
 406   }
 407
 408   if (address == 0xbfe001) {
 409     ovl = (value & (1 << 0));
 410     printf("OVL:%x\n", ovl);
 411   }
 412
 413   if (address < 0xffffff) {
 414     write8((uint32_t)address, value);
 415     return;
 416   }
 417
 418   return;
 419 }
 420
 421 void m68k_write_memory_16(unsigned int address, unsigned int value) {
 422   if (address > FASTBASE && address < FASTBASE + FASTSIZE) {
 423     *(uint16_t *)&g_ram[address - FASTBASE] = htobe16(value);
 424     return;
 425   }
 426
 427   if (gayle_emulation_enabled) {
 428     if (address > GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 429       writeGayle(address, value);
 430       return;
 431     }
 432   }
 433
 434   if (address < 0xffffff) {
 435     write16((uint32_t)address, value);
 436     return;
 437   }
 438   return;
 439 }
 440
 441 void m68k_write_memory_32(unsigned int address, unsigned int value) {
 442   if (address > FASTBASE && address < FASTBASE + FASTSIZE) {
 443     *(uint32_t *)&g_ram[address - FASTBASE] = htobe32(value);
 444     return;
 445   }
 446
 447   if (gayle_emulation_enabled) {
 448     if (address > GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 449       writeGayleL(address, value);
 450     }
 451   }
 452
 453   if (address < 0xffffff) {
 454     write16(address, value >> 16);
 455     write16(address + 2, value);
 456     return;
 457   }
 458
 459   return;
 460 }
 461
 462 void write16(uint32_t address, uint32_t data) {
 463   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 464   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 465   uint32_t addr_l_s = (address >> 16) << 8;
 466   uint32_t addr_l_r = (~address >> 16) << 8;
 467   uint32_t data_s = (data & 0x0000ffff) << 8;
 468   uint32_t data_r = (~data & 0x0000ffff) << 8;
 469
 470   //      asm volatile ("dmb" ::: "memory");
 471   W16
 472   *(gpio) = gpfsel0_o;
 473   *(gpio + 1) = gpfsel1_o;
 474   *(gpio + 2) = gpfsel2_o;
 475
 476   *(gpio + 7) = addr_h_s;
 477   *(gpio + 10) = addr_h_r;
 478   GPIO_CLR = 1 << 7;
 479   GPIO_SET = 1 << 7;
 480
 481   *(gpio + 7) = addr_l_s;
 482   *(gpio + 10) = addr_l_r;
 483   GPIO_CLR = 1 << 7;
 484   GPIO_SET = 1 << 7;
 485
 486   // write phase
 487   *(gpio + 7) = data_s;
 488   *(gpio + 10) = data_r;
 489   GPIO_CLR = 1 << 7;
 490   GPIO_SET = 1 << 7;
 491
 492   *(gpio) = gpfsel0;
 493   *(gpio + 1) = gpfsel1;
 494   *(gpio + 2) = gpfsel2;
 495   while ((GET_GPIO(0)))
 496     ;
 497   //     asm volatile ("dmb" ::: "memory");
 498 }
 499
 500 void write8(uint32_t address, uint32_t data) {
 501   if ((address & 1) == 0)
 502     data = data + (data << 8);  // EVEN, A0=0,UDS
 503   else
 504     data = data & 0xff;  // ODD , A0=1,LDS
 505   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 506   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 507   uint32_t addr_l_s = (address >> 16) << 8;
 508   uint32_t addr_l_r = (~address >> 16) << 8;
 509   uint32_t data_s = (data & 0x0000ffff) << 8;
 510   uint32_t data_r = (~data & 0x0000ffff) << 8;
 511
 512   //   asm volatile ("dmb" ::: "memory");
 513   W8
 514   *(gpio) = gpfsel0_o;
 515   *(gpio + 1) = gpfsel1_o;
 516   *(gpio + 2) = gpfsel2_o;
 517
 518   *(gpio + 7) = addr_h_s;
 519   *(gpio + 10) = addr_h_r;
 520   GPIO_CLR = 1 << 7;
 521   GPIO_SET = 1 << 7;
 522
 523   *(gpio + 7) = addr_l_s;
 524   *(gpio + 10) = addr_l_r;
 525   GPIO_CLR = 1 << 7;
 526   GPIO_SET = 1 << 7;
 527
 528   // write phase
 529   *(gpio + 7) = data_s;
 530   *(gpio + 10) = data_r;
 531   GPIO_CLR = 1 << 7;
 532   GPIO_SET = 1 << 7;
 533
 534   *(gpio) = gpfsel0;
 535   *(gpio + 1) = gpfsel1;
 536   *(gpio + 2) = gpfsel2;
 537   while ((GET_GPIO(0)))
 538     ;
 539   //   asm volatile ("dmb" ::: "memory");
 540 }
 541
 542 uint32_t read16(uint32_t address) {
 543   volatile int val;
 544   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 545   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 546   uint32_t addr_l_s = (address >> 16) << 8;
 547   uint32_t addr_l_r = (~address >> 16) << 8;
 548
 549   //   asm volatile ("dmb" ::: "memory");
 550   R16
 551   *(gpio) = gpfsel0_o;
 552   *(gpio + 1) = gpfsel1_o;
 553   *(gpio + 2) = gpfsel2_o;
 554
 555   *(gpio + 7) = addr_h_s;
 556   *(gpio + 10) = addr_h_r;
 557   GPIO_CLR = 1 << 7;
 558   GPIO_SET = 1 << 7;
 559
 560   *(gpio + 7) = addr_l_s;
 561   *(gpio + 10) = addr_l_r;
 562   GPIO_CLR = 1 << 7;
 563   GPIO_SET = 1 << 7;
 564
 565   // read phase
 566   *(gpio) = gpfsel0;
 567   *(gpio + 1) = gpfsel1;
 568   *(gpio + 2) = gpfsel2;
 569   GPIO_CLR = 1 << 6;
 570   while (!(GET_GPIO(0)))
 571     ;
 572   GPIO_CLR = 1 << 6;
 573   val = *(gpio + 13);
 574   GPIO_SET = 1 << 6;
 575   //    asm volatile ("dmb" ::: "memory");
 576   return (val >> 8) & 0xffff;
 577 }
 578
 579 uint32_t read8(uint32_t address) {
 580   int val;
 581   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 582   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 583   uint32_t addr_l_s = (address >> 16) << 8;
 584   uint32_t addr_l_r = (~address >> 16) << 8;
 585
 586   //    asm volatile ("dmb" ::: "memory");
 587   R8
 588   *(gpio) = gpfsel0_o;
 589   *(gpio + 1) = gpfsel1_o;
 590   *(gpio + 2) = gpfsel2_o;
 591
 592   *(gpio + 7) = addr_h_s;
 593   *(gpio + 10) = addr_h_r;
 594   GPIO_CLR = 1 << 7;
 595   GPIO_SET = 1 << 7;
 596
 597   *(gpio + 7) = addr_l_s;
 598   *(gpio + 10) = addr_l_r;
 599   GPIO_CLR = 1 << 7;
 600   GPIO_SET = 1 << 7;
 601
 602   // read phase
 603   *(gpio) = gpfsel0;
 604   *(gpio + 1) = gpfsel1;
 605   *(gpio + 2) = gpfsel2;
 606
 607   GPIO_CLR = 1 << 6;
 608   while (!(GET_GPIO(0)))
 609     ;
 610   GPIO_CLR = 1 << 6;
 611   val = *(gpio + 13);
 612   GPIO_SET = 1 << 6;
 613   //    asm volatile ("dmb" ::: "memory");
 614
 615   val = (val >> 8) & 0xffff;
 616   if ((address & 1) == 0)
 617     return (val >> 8) & 0xff;  // EVEN, A0=0,UDS
 618   else
 619     return val & 0xff;  // ODD , A0=1,LDS
 620 }
 621
 622 /******************************************************/
 623
 624 void write_reg(unsigned int value) {
 625   STATUSREGADDR
 626   *(gpio) = gpfsel0_o;
 627   *(gpio + 1) = gpfsel1_o;
 628   *(gpio + 2) = gpfsel2_o;
 629   *(gpio + 7) = (value & 0xffff) << 8;
 630   *(gpio + 10) = (~value & 0xffff) << 8;
 631   GPIO_CLR = 1 << 7;
 632   GPIO_CLR = 1 << 7;  // delay
 633   GPIO_SET = 1 << 7;
 634   GPIO_SET = 1 << 7;
 635   // Bus HIGH-Z
 636   *(gpio) = gpfsel0;
 637   *(gpio + 1) = gpfsel1;
 638   *(gpio + 2) = gpfsel2;
 639 }
 640
 641 uint16_t read_reg(void) {
 642   uint32_t val;
 643   STATUSREGADDR
 644   // Bus HIGH-Z
 645   *(gpio) = gpfsel0;
 646   *(gpio + 1) = gpfsel1;
 647   *(gpio + 2) = gpfsel2;
 648   GPIO_CLR = 1 << 6;
 649   GPIO_CLR = 1 << 6;  // delay
 650   GPIO_CLR = 1 << 6;
 651   GPIO_CLR = 1 << 6;
 652   val = *(gpio + 13);
 653   GPIO_SET = 1 << 6;
 654   return (uint16_t)(val >> 8);
 655 }
 656
 657 //
 658 // Set up a memory regions to access GPIO
 659 //
 660 void setup_io() {
 661   /* open /dev/mem */
 662   if ((mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
 663     printf("can't open /dev/mem \n");
 664     exit(-1);
 665   }
 666
 667   /* mmap GPIO */
 668   gpio_map = mmap(
 669       NULL,                    // Any adddress in our space will do
 670       BCM2708_PERI_SIZE,       // Map length
 671       PROT_READ | PROT_WRITE,  // Enable reading & writting to mapped memory
 672       MAP_SHARED,              // Shared with other processes
 673       mem_fd,                  // File to map
 674       BCM2708_PERI_BASE        // Offset to GPIO peripheral
 675   );
 676
 677   close(mem_fd);  // No need to keep mem_fd open after mmap
 678
 679   if (gpio_map == MAP_FAILED) {
 680     printf("gpio mmap error %d\n", (int)gpio_map);  // errno also set!
 681     exit(-1);
 682   }
 683
 684   gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
 685   gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
 686
 687 }  // setup_io