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

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