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

   1 /*
   2 Copyright 2020 Claude Schwartz
   3 */
   4
   5 #include <assert.h>
   6 #include <dirent.h>
   7 #include <endian.h>
   8 #include <fcntl.h>
   9 #include <pthread.h>
  10 #include <sched.h>
  11 #include <signal.h>
  12 #include <stdint.h>
  13 #include <stdio.h>
  14 #include <stdlib.h>
  15 #include <string.h>
  16 #include <sys/mman.h>
  17 #include <sys/stat.h>
  18 #include <sys/types.h>
  19 #include <unistd.h>
  20
  21 #include "Gayle.h"
  22 #include "ide.h"
  23 #include "m68k.h"
  24 #include "main.h"
  25
  26 //#define BCM2708_PERI_BASE        0x20000000  //pi0-1
  27 //#define BCM2708_PERI_BASE     0xFE000000     //pi4
  28 #define BCM2708_PERI_BASE 0x3F000000  // pi3
  29 #define BCM2708_PERI_SIZE 0x01000000
  30 #define GPIO_BASE (BCM2708_PERI_BASE + 0x200000) /* GPIO controller */
  31 #define GPCLK_BASE (BCM2708_PERI_BASE + 0x101000)
  32 #define GPIO_ADDR 0x200000 /* GPIO controller */
  33 #define GPCLK_ADDR 0x101000
  34 #define CLK_PASSWD 0x5a000000
  35 #define CLK_GP0_CTL 0x070
  36 #define CLK_GP0_DIV 0x074
  37
  38 #define SA0 5
  39 #define SA1 3
  40 #define SA2 2
  41
  42 #define STATUSREGADDR  \
  43   GPIO_CLR = 1 << SA0; \
  44   GPIO_CLR = 1 << SA1; \
  45   GPIO_SET = 1 << SA2;
  46 #define W16            \
  47   GPIO_CLR = 1 << SA0; \
  48   GPIO_CLR = 1 << SA1; \
  49   GPIO_CLR = 1 << SA2;
  50 #define R16            \
  51   GPIO_SET = 1 << SA0; \
  52   GPIO_CLR = 1 << SA1; \
  53   GPIO_CLR = 1 << SA2;
  54 #define W8             \
  55   GPIO_CLR = 1 << SA0; \
  56   GPIO_SET = 1 << SA1; \
  57   GPIO_CLR = 1 << SA2;
  58 #define R8             \
  59   GPIO_SET = 1 << SA0; \
  60   GPIO_SET = 1 << SA1; \
  61   GPIO_CLR = 1 << SA2;
  62
  63 #define PAGE_SIZE (4 * 1024)
  64 #define BLOCK_SIZE (4 * 1024)
  65
  66 #define GPIOSET(no, ishigh) \
  67   do {                      \
  68     if (ishigh)             \
  69       set |= (1 << (no));   \
  70     else                    \
  71       reset |= (1 << (no)); \
  72   } while (0)
  73
  74 int fast_base_configured;
  75 unsigned int fast_base;
  76 #define FAST_SIZE (8 * 1024 * 1024)
  77
  78 #define GAYLEBASE 0xD80000
  79 #define GAYLESIZE (448 * 1024)
  80
  81 #define KICKBASE 0xF80000
  82 #define KICKSIZE (512 * 1024)
  83
  84 #define AC_BASE 0xE80000
  85 #define AC_SIZE (64 * 1024)
  86
  87 #define AC_PIC_COUNT 1
  88 int ac_current_pic = 0;
  89 int ac_done = 0;
  90
  91 int mem_fd;
  92 int mem_fd_gpclk;
  93 int gayle_emulation_enabled = 1;
  94 void *gpio_map;
  95 void *gpclk_map;
  96
  97 // I/O access
  98 volatile unsigned int *gpio;
  99 volatile unsigned int *gpclk;
 100 volatile unsigned int gpfsel0;
 101 volatile unsigned int gpfsel1;
 102 volatile unsigned int gpfsel2;
 103 volatile unsigned int gpfsel0_o;
 104 volatile unsigned int gpfsel1_o;
 105 volatile unsigned int gpfsel2_o;
 106
 107 // GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or
 108 // SET_GPIO_ALT(x,y)
 109 #define INP_GPIO(g) *(gpio + ((g) / 10)) &= ~(7 << (((g) % 10) * 3))
 110 #define OUT_GPIO(g) *(gpio + ((g) / 10)) |= (1 << (((g) % 10) * 3))
 111 #define SET_GPIO_ALT(g, a)  \
 112   *(gpio + (((g) / 10))) |= \
 113       (((a) <= 3 ? (a) + 4 : (a) == 4 ? 3 : 2) << (((g) % 10) * 3))
 114
 115 #define GPIO_SET \
 116   *(gpio + 7)  // sets   bits which are 1 ignores bits which are 0
 117 #define GPIO_CLR \
 118   *(gpio + 10)  // clears bits which are 1 ignores bits which are 0
 119
 120 #define GET_GPIO(g) (*(gpio + 13) & (1 << g))  // 0 if LOW, (1<<g) if HIGH
 121
 122 #define GPIO_PULL *(gpio + 37)      // Pull up/pull down
 123 #define GPIO_PULLCLK0 *(gpio + 38)  // Pull up/pull down clock
 124
 125 void setup_io();
 126
 127 uint32_t read8(uint32_t address);
 128 void write8(uint32_t address, uint32_t data);
 129
 130 uint32_t read16(uint32_t address);
 131 void write16(uint32_t address, uint32_t data);
 132
 133 void write32(uint32_t address, uint32_t data);
 134 uint32_t read32(uint32_t address);
 135
 136 uint16_t read_reg(void);
 137 void write_reg(unsigned int value);
 138
 139 volatile uint16_t srdata;
 140 volatile uint32_t srdata2;
 141 volatile uint32_t srdata2_old;
 142
 143 unsigned char g_kick[KICKSIZE];
 144 unsigned char fast_ram_array[FAST_SIZE]; /* RAM */
 145 unsigned char toggle;
 146 static volatile unsigned char ovl;
 147 static volatile unsigned char maprom;
 148
 149 void sigint_handler(int sig_num) {
 150   printf("\n Exit Ctrl+C %d\n", sig_num);
 151   exit(0);
 152 }
 153
 154 void *iplThread(void *args) {
 155   printf("IPL thread running/n");
 156
 157   while (42) {
 158     if (GET_GPIO(1) == 0) {
 159       toggle = 1;
 160       m68k_end_timeslice();
 161       //printf("thread!/n");
 162     } else {
 163       toggle = 0;
 164     };
 165     usleep(1);
 166   }
 167 }
 168
 169 int main(int argc, char *argv[]) {
 170   int g;
 171   const struct sched_param priority = {99};
 172
 173   // Some command line switch stuffles
 174   for (g = 1; g < argc; g++) {
 175     if (strcmp(argv[g], "--disable-gayle") == 0) {
 176       gayle_emulation_enabled = 0;
 177     }
 178   }
 179
 180   sched_setscheduler(0, SCHED_FIFO, &priority);
 181   mlockall(MCL_CURRENT);  // lock in memory to keep us from paging out
 182
 183   InitGayle();
 184
 185   signal(SIGINT, sigint_handler);
 186   setup_io();
 187
 188   // Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending
 189   // on pi model
 190   printf("Enable 200MHz GPCLK0 on GPIO4\n");
 191
 192   *(gpclk + (CLK_GP0_CTL / 4)) = CLK_PASSWD | (1 << 5);
 193   usleep(10);
 194   while ((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7))
 195     ;
 196   usleep(100);
 197   *(gpclk + (CLK_GP0_DIV / 4)) =
 198       CLK_PASSWD | (6 << 12);  // divider , 6=200MHz on pi3
 199   usleep(10);
 200   *(gpclk + (CLK_GP0_CTL / 4)) =
 201       CLK_PASSWD | 5 | (1 << 4);  // pll? 6=plld, 5=pllc
 202   usleep(10);
 203   while (((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7)) == 0)
 204     ;
 205   usleep(100);
 206
 207   SET_GPIO_ALT(4, 0);  // gpclk0
 208
 209   // set SA to output
 210   INP_GPIO(2);
 211   OUT_GPIO(2);
 212   INP_GPIO(3);
 213   OUT_GPIO(3);
 214   INP_GPIO(5);
 215   OUT_GPIO(5);
 216
 217   // set gpio0 (aux0) and gpio1 (aux1) to input
 218   INP_GPIO(0);
 219   INP_GPIO(1);
 220
 221   // Set GPIO pins 6,7 and 8-23 to output
 222   for (g = 6; g <= 23; g++) {
 223     INP_GPIO(g);
 224     OUT_GPIO(g);
 225   }
 226   printf("Precalculate GPIO8-23 as Output\n");
 227   gpfsel0_o = *(gpio);  // store gpio ddr
 228   printf("gpfsel0: %#x\n", gpfsel0_o);
 229   gpfsel1_o = *(gpio + 1);  // store gpio ddr
 230   printf("gpfsel1: %#x\n", gpfsel1_o);
 231   gpfsel2_o = *(gpio + 2);  // store gpio ddr
 232   printf("gpfsel2: %#x\n", gpfsel2_o);
 233
 234   // Set GPIO pins 8-23 to input
 235   for (g = 8; g <= 23; g++) {
 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   // load kick.rom if present
 262   maprom = 1;
 263   int fd = 0;
 264   fd = open("kick.rom", O_RDONLY);
 265   if (fd < 1) {
 266     printf("Failed loading kick.rom, using motherboard kickstart\n");
 267     maprom = 0;
 268   } else {
 269     int size = (int)lseek(fd, 0, SEEK_END);
 270     if (size == 0x40000) {
 271       lseek(fd, 0, SEEK_SET);
 272       read(fd, &g_kick, size);
 273       lseek(fd, 0, SEEK_SET);
 274       read(fd, &g_kick[0x40000], size);
 275     } else {
 276       lseek(fd, 0, SEEK_SET);
 277       read(fd, &g_kick, size);
 278     }
 279     printf("Loaded kick.rom with size %d kib\n", size / 1024);
 280   }
 281
 282   // reset amiga and statemachine
 283   cpu_pulse_reset();
 284   ovl = 1;
 285   m68k_write_memory_8(0xbfe201, 0x0001);  // AMIGA OVL
 286   m68k_write_memory_8(0xbfe001, 0x0001);  // AMIGA OVL high (ROM@0x0)
 287
 288   usleep(1500);
 289
 290   m68k_init();
 291   m68k_set_cpu_type(M68K_CPU_TYPE_68020);
 292   m68k_pulse_reset();
 293
 294   if (maprom == 1) {
 295     m68k_set_reg(M68K_REG_PC, 0xF80002);
 296   } else {
 297     m68k_set_reg(M68K_REG_PC, 0x0);
 298   }
 299
 300   /*
 301           pthread_t id;
 302           int err;
 303           err = pthread_create(&id, NULL, &iplThread, NULL);
 304           if (err != 0)
 305               printf("\ncan't create IPL thread :[%s]", strerror(err));
 306           else
 307               printf("\n IPL Thread created successfully\n");
 308 */
 309
 310   m68k_pulse_reset();
 311   while (42) {
 312     m68k_execute(300);
 313     /*
 314     if (toggle == 1){
 315       srdata = read_reg();
 316       m68k_set_irq((srdata >> 13) & 0xff);
 317     } else {
 318          m68k_set_irq(0);
 319     };
 320     usleep(1);
 321 */
 322
 323     if (GET_GPIO(1) == 0) {
 324       srdata = read_reg();
 325       m68k_set_irq((srdata >> 13) & 0xff);
 326     } else {
 327       if (CheckIrq() == 1) {
 328         write16(0xdff09c, 0x8008);
 329         m68k_set_irq(2);
 330       } else
 331         m68k_set_irq(0);
 332     };
 333   }
 334
 335   return 0;
 336 }
 337
 338 void cpu_pulse_reset(void) {
 339   write_reg(0x00);
 340   // printf("Status Reg%x\n",read_reg());
 341   usleep(100000);
 342   write_reg(0x02);
 343   // printf("Status Reg%x\n",read_reg());
 344 }
 345
 346 int cpu_irq_ack(int level) {
 347   printf("cpu irq ack\n");
 348   return level;
 349 }
 350
 351 static unsigned char ac_fast_ram_rom[] = {
 352     0xe, 0x0,                               // 00/02, link into memory free list, 8 MB
 353     0x6, 0x9,                               // 04/06, product id
 354     0x8, 0x0,                               // 08/0a, preference to 8 MB space
 355     0x0, 0x0,                               // 0c/0e, reserved
 356     0x0, 0x7, 0xd, 0xb,                     // 10/12/14/16, mfg id
 357     0x0, 0x0, 0x0, 0x0, 0x0, 0x4, 0x2, 0x0  // 18/.../26, serial
 358 };
 359
 360 static unsigned int ac_fast_ram_read_memory_8(unsigned int address) {
 361   unsigned char val = 0;
 362   if ((address & 1) == 0 && (address / 2) < sizeof(ac_fast_ram_rom))
 363     val = ac_fast_ram_rom[address / 2];
 364   val <<= 4;
 365   if (address != 0 && address != 2 && address != 40 && address != 42)
 366     val ^= 0xf0;
 367   return (unsigned int)val;
 368 }
 369
 370 static void ac_fast_ram_write_memory_8(unsigned int address, unsigned int value) {
 371   int done = 0;
 372
 373   if (address == 0x4a) {  // base[19:16]
 374     fast_base = (value & 0xf0) << (16 - 4);
 375   } else if (address == 0x48) {  // base[23:20]
 376     fast_base &= 0xff0fffff;
 377     fast_base |= (value & 0xf0) << (20 - 4);
 378     fast_base_configured = 1;
 379     done = 1;
 380   } else if (address == 0x4c) {  // shut up
 381     done = 1;
 382   }
 383
 384   if (done) {
 385     ac_current_pic++;
 386     if (ac_current_pic == AC_PIC_COUNT)
 387       ac_done = 1;
 388   }
 389 }
 390
 391 static unsigned int autoconfig_read_memory_8(unsigned int address) {
 392   if (ac_current_pic == 0) {
 393     return ac_fast_ram_read_memory_8(address);
 394   }
 395 }
 396
 397 static void autoconfig_write_memory_8(unsigned int address, unsigned int value) {
 398   if (ac_current_pic == 0) {
 399     return ac_fast_ram_write_memory_8(address, value);
 400   }
 401 }
 402
 403 unsigned int m68k_read_memory_8(unsigned int address) {
 404   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 405     return fast_ram_array[address - fast_base];
 406   }
 407
 408   if (!ac_done && address >= AC_BASE && address < AC_BASE + AC_SIZE) {
 409     return autoconfig_read_memory_8(address - AC_BASE);
 410   }
 411
 412   if (maprom == 1) {
 413     if (address >= KICKBASE && address < KICKBASE + KICKSIZE) {
 414       return g_kick[address - KICKBASE];
 415     }
 416   }
 417
 418   if (gayle_emulation_enabled) {
 419     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 420       return readGayleB(address);
 421     }
 422   }
 423
 424   address &= 0xFFFFFF;
 425   //  if (address < 0xffffff) {
 426   return read8((uint32_t)address);
 427   //  }
 428
 429   //  return 1;
 430 }
 431
 432 unsigned int m68k_read_memory_16(unsigned int address) {
 433   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 434     return be16toh(*(uint16_t *)&fast_ram_array[address - fast_base]);
 435   }
 436
 437   if (maprom == 1) {
 438     if (address >= KICKBASE && address < KICKBASE + KICKSIZE) {
 439       return be16toh(*(uint16_t *)&g_kick[address - KICKBASE]);
 440     }
 441   }
 442
 443   if (gayle_emulation_enabled) {
 444     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 445       return readGayle(address);
 446     }
 447   }
 448
 449   //  if (address < 0xffffff) {
 450   address &= 0xFFFFFF;
 451   return (unsigned int)read16((uint32_t)address);
 452   //  }
 453
 454   //  return 1;
 455 }
 456
 457 unsigned int m68k_read_memory_32(unsigned int address) {
 458   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 459     return be32toh(*(uint32_t *)&fast_ram_array[address - fast_base]);
 460   }
 461
 462   if (maprom == 1) {
 463     if (address >= KICKBASE && address < KICKBASE + KICKSIZE) {
 464       return be32toh(*(uint32_t *)&g_kick[address - KICKBASE]);
 465     }
 466   }
 467
 468   if (gayle_emulation_enabled) {
 469     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 470       return readGayleL(address);
 471     }
 472   }
 473
 474   //  if (address < 0xffffff) {
 475   address &= 0xFFFFFF;
 476   uint16_t a = read16(address);
 477   uint16_t b = read16(address + 2);
 478   return (a << 16) | b;
 479   //  }
 480
 481   //  return 1;
 482 }
 483
 484 void m68k_write_memory_8(unsigned int address, unsigned int value) {
 485   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 486     fast_ram_array[address - fast_base] = value;
 487     return;
 488   }
 489
 490   if (!ac_done && address >= AC_BASE && address < AC_BASE + AC_SIZE) {
 491     return autoconfig_write_memory_8(address - AC_BASE, value);
 492   }
 493
 494   if (gayle_emulation_enabled) {
 495     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 496       writeGayleB(address, value);
 497       return;
 498     }
 499   }
 500   /*
 501   if (address == 0xbfe001) {
 502     ovl = (value & (1 << 0));
 503     printf("OVL:%x\n", ovl);
 504   }
 505 */
 506   //  if (address < 0xffffff) {
 507   address &= 0xFFFFFF;
 508   write8((uint32_t)address, value);
 509   return;
 510   //  }
 511
 512   //  return;
 513 }
 514
 515 void m68k_write_memory_16(unsigned int address, unsigned int value) {
 516   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 517     *(uint16_t *)&fast_ram_array[address - fast_base] = htobe16(value);
 518     return;
 519   }
 520
 521   if (gayle_emulation_enabled) {
 522     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 523       writeGayle(address, value);
 524       return;
 525     }
 526   }
 527
 528   //  if (address < 0xffffff) {
 529   address &= 0xFFFFFF;
 530   write16((uint32_t)address, value);
 531   return;
 532   //  }
 533   //  return;
 534 }
 535
 536 void m68k_write_memory_32(unsigned int address, unsigned int value) {
 537   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 538     *(uint32_t *)&fast_ram_array[address - fast_base] = htobe32(value);
 539     return;
 540   }
 541
 542   if (gayle_emulation_enabled) {
 543     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 544       writeGayleL(address, value);
 545     }
 546   }
 547
 548   //  if (address < 0xffffff) {
 549   address &= 0xFFFFFF;
 550   write16(address, value >> 16);
 551   write16(address + 2, value);
 552   return;
 553   //  }
 554
 555   //  return;
 556 }
 557
 558 void write16(uint32_t address, uint32_t data) {
 559   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 560   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 561   uint32_t addr_l_s = (address >> 16) << 8;
 562   uint32_t addr_l_r = (~address >> 16) << 8;
 563   uint32_t data_s = (data & 0x0000ffff) << 8;
 564   uint32_t data_r = (~data & 0x0000ffff) << 8;
 565
 566   //      asm volatile ("dmb" ::: "memory");
 567   W16
 568       *(gpio) = gpfsel0_o;
 569   *(gpio + 1) = gpfsel1_o;
 570   *(gpio + 2) = gpfsel2_o;
 571
 572   *(gpio + 7) = addr_h_s;
 573   *(gpio + 10) = addr_h_r;
 574   GPIO_CLR = 1 << 7;
 575   GPIO_SET = 1 << 7;
 576
 577   *(gpio + 7) = addr_l_s;
 578   *(gpio + 10) = addr_l_r;
 579   GPIO_CLR = 1 << 7;
 580   GPIO_SET = 1 << 7;
 581
 582   // write phase
 583   *(gpio + 7) = data_s;
 584   *(gpio + 10) = data_r;
 585   GPIO_CLR = 1 << 7;
 586   GPIO_SET = 1 << 7;
 587
 588   *(gpio) = gpfsel0;
 589   *(gpio + 1) = gpfsel1;
 590   *(gpio + 2) = gpfsel2;
 591   while ((GET_GPIO(0)))
 592     ;
 593   //     asm volatile ("dmb" ::: "memory");
 594 }
 595
 596 void write8(uint32_t address, uint32_t data) {
 597   if ((address & 1) == 0)
 598     data = data + (data << 8);  // EVEN, A0=0,UDS
 599   else
 600     data = data & 0xff;  // ODD , A0=1,LDS
 601   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 602   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 603   uint32_t addr_l_s = (address >> 16) << 8;
 604   uint32_t addr_l_r = (~address >> 16) << 8;
 605   uint32_t data_s = (data & 0x0000ffff) << 8;
 606   uint32_t data_r = (~data & 0x0000ffff) << 8;
 607
 608   //   asm volatile ("dmb" ::: "memory");
 609   W8
 610       *(gpio) = gpfsel0_o;
 611   *(gpio + 1) = gpfsel1_o;
 612   *(gpio + 2) = gpfsel2_o;
 613
 614   *(gpio + 7) = addr_h_s;
 615   *(gpio + 10) = addr_h_r;
 616   GPIO_CLR = 1 << 7;
 617   GPIO_SET = 1 << 7;
 618
 619   *(gpio + 7) = addr_l_s;
 620   *(gpio + 10) = addr_l_r;
 621   GPIO_CLR = 1 << 7;
 622   GPIO_SET = 1 << 7;
 623
 624   // write phase
 625   *(gpio + 7) = data_s;
 626   *(gpio + 10) = data_r;
 627   GPIO_CLR = 1 << 7;
 628   GPIO_SET = 1 << 7;
 629
 630   *(gpio) = gpfsel0;
 631   *(gpio + 1) = gpfsel1;
 632   *(gpio + 2) = gpfsel2;
 633   while ((GET_GPIO(0)))
 634     ;
 635   //   asm volatile ("dmb" ::: "memory");
 636 }
 637
 638 uint32_t read16(uint32_t address) {
 639   volatile int val;
 640   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 641   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 642   uint32_t addr_l_s = (address >> 16) << 8;
 643   uint32_t addr_l_r = (~address >> 16) << 8;
 644
 645   //   asm volatile ("dmb" ::: "memory");
 646   R16
 647       *(gpio) = gpfsel0_o;
 648   *(gpio + 1) = gpfsel1_o;
 649   *(gpio + 2) = gpfsel2_o;
 650
 651   *(gpio + 7) = addr_h_s;
 652   *(gpio + 10) = addr_h_r;
 653   GPIO_CLR = 1 << 7;
 654   GPIO_SET = 1 << 7;
 655
 656   *(gpio + 7) = addr_l_s;
 657   *(gpio + 10) = addr_l_r;
 658   GPIO_CLR = 1 << 7;
 659   GPIO_SET = 1 << 7;
 660
 661   // read phase
 662   *(gpio) = gpfsel0;
 663   *(gpio + 1) = gpfsel1;
 664   *(gpio + 2) = gpfsel2;
 665   GPIO_CLR = 1 << 6;
 666   while (!(GET_GPIO(0)))
 667     ;
 668   GPIO_CLR = 1 << 6;
 669   val = *(gpio + 13);
 670   GPIO_SET = 1 << 6;
 671   //    asm volatile ("dmb" ::: "memory");
 672   return (val >> 8) & 0xffff;
 673 }
 674
 675 uint32_t read8(uint32_t address) {
 676   int val;
 677   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 678   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 679   uint32_t addr_l_s = (address >> 16) << 8;
 680   uint32_t addr_l_r = (~address >> 16) << 8;
 681
 682   //    asm volatile ("dmb" ::: "memory");
 683   R8
 684       *(gpio) = gpfsel0_o;
 685   *(gpio + 1) = gpfsel1_o;
 686   *(gpio + 2) = gpfsel2_o;
 687
 688   *(gpio + 7) = addr_h_s;
 689   *(gpio + 10) = addr_h_r;
 690   GPIO_CLR = 1 << 7;
 691   GPIO_SET = 1 << 7;
 692
 693   *(gpio + 7) = addr_l_s;
 694   *(gpio + 10) = addr_l_r;
 695   GPIO_CLR = 1 << 7;
 696   GPIO_SET = 1 << 7;
 697
 698   // read phase
 699   *(gpio) = gpfsel0;
 700   *(gpio + 1) = gpfsel1;
 701   *(gpio + 2) = gpfsel2;
 702
 703   GPIO_CLR = 1 << 6;
 704   while (!(GET_GPIO(0)))
 705     ;
 706   GPIO_CLR = 1 << 6;
 707   val = *(gpio + 13);
 708   GPIO_SET = 1 << 6;
 709   //    asm volatile ("dmb" ::: "memory");
 710
 711   val = (val >> 8) & 0xffff;
 712   if ((address & 1) == 0)
 713     return (val >> 8) & 0xff;  // EVEN, A0=0,UDS
 714   else
 715     return val & 0xff;  // ODD , A0=1,LDS
 716 }
 717
 718 /******************************************************/
 719
 720 void write_reg(unsigned int value) {
 721   STATUSREGADDR
 722   *(gpio) = gpfsel0_o;
 723   *(gpio + 1) = gpfsel1_o;
 724   *(gpio + 2) = gpfsel2_o;
 725   *(gpio + 7) = (value & 0xffff) << 8;
 726   *(gpio + 10) = (~value & 0xffff) << 8;
 727   GPIO_CLR = 1 << 7;
 728   GPIO_CLR = 1 << 7;  // delay
 729   GPIO_SET = 1 << 7;
 730   GPIO_SET = 1 << 7;
 731   // Bus HIGH-Z
 732   *(gpio) = gpfsel0;
 733   *(gpio + 1) = gpfsel1;
 734   *(gpio + 2) = gpfsel2;
 735 }
 736
 737 uint16_t read_reg(void) {
 738   uint32_t val;
 739   STATUSREGADDR
 740   // Bus HIGH-Z
 741   *(gpio) = gpfsel0;
 742   *(gpio + 1) = gpfsel1;
 743   *(gpio + 2) = gpfsel2;
 744   GPIO_CLR = 1 << 6;
 745   GPIO_CLR = 1 << 6;  // delay
 746   GPIO_CLR = 1 << 6;
 747   GPIO_CLR = 1 << 6;
 748   val = *(gpio + 13);
 749   GPIO_SET = 1 << 6;
 750   return (uint16_t)(val >> 8);
 751 }
 752
 753 //
 754 // Set up a memory regions to access GPIO
 755 //
 756 void setup_io() {
 757   /* open /dev/mem */
 758   if ((mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
 759     printf("can't open /dev/mem \n");
 760     exit(-1);
 761   }
 762
 763   /* mmap GPIO */
 764   gpio_map = mmap(
 765       NULL,                    // Any adddress in our space will do
 766       BCM2708_PERI_SIZE,       // Map length
 767       PROT_READ | PROT_WRITE,  // Enable reading & writting to mapped memory
 768       MAP_SHARED,              // Shared with other processes
 769       mem_fd,                  // File to map
 770       BCM2708_PERI_BASE        // Offset to GPIO peripheral
 771   );
 772
 773   close(mem_fd);  // No need to keep mem_fd open after mmap
 774
 775   if (gpio_map == MAP_FAILED) {
 776     printf("gpio mmap error %d\n", (int)gpio_map);  // errno also set!
 777     exit(-1);
 778   }
 779
 780   gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
 781   gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
 782
 783 }  // setup_io