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