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