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