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

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