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