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 <sys/ioctl.h>
  17 #include "Gayle.h"
  18 #include "ide.h"
  19 #include "m68k.h"
  20 #include "main.h"
  21 #include "platforms/platforms.h"
  22 #include "input/input.h"
  23
  24 #include "platforms/amiga/amiga-registers.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 #define FASTBASE 0x07FFFFFF
  75 #define FASTSIZE 0xFFFFFFF
  76 #define GAYLEBASE 0xD80000  // D7FFFF
  77 #define GAYLESIZE 0x6FFFF
  78
  79 #define JOY0DAT 0xDFF00A
  80 #define JOY1DAT 0xDFF00C
  81 #define CIAAPRA 0xBFE001
  82 #define POTGOR  0xDFF016
  83
  84 int kb_hook_enabled = 0;
  85 int mouse_hook_enabled = 0;
  86 int cpu_emulation_running = 1;
  87
  88 char mouse_dx = 0, mouse_dy = 0;
  89 char mouse_buttons = 0;
  90
  91 #define KICKBASE 0xF80000
  92 #define KICKSIZE 0x7FFFF
  93
  94 int mem_fd, mouse_fd = -1, keyboard_fd = -1;
  95 int mem_fd_gpclk;
  96 int gayle_emulation_enabled = 1;
  97 void *gpio_map;
  98 void *gpclk_map;
  99
 100 // Configurable emulator options
 101 unsigned int cpu_type = M68K_CPU_TYPE_68000;
 102 unsigned int loop_cycles = 300;
 103 struct emulator_config *cfg = NULL;
 104 char keyboard_file[256] = "/dev/input/event0";
 105
 106 // I/O access
 107 volatile unsigned int *gpio;
 108 volatile unsigned int *gpclk;
 109 volatile unsigned int gpfsel0;
 110 volatile unsigned int gpfsel1;
 111 volatile unsigned int gpfsel2;
 112 volatile unsigned int gpfsel0_o;
 113 volatile unsigned int gpfsel1_o;
 114 volatile unsigned int gpfsel2_o;
 115
 116 // GPIO setup macros. Always use INP_GPIO(x) before using OUT_GPIO(x) or
 117 // SET_GPIO_ALT(x,y)
 118 #define INP_GPIO(g) *(gpio + ((g) / 10)) &= ~(7 << (((g) % 10) * 3))
 119 #define OUT_GPIO(g) *(gpio + ((g) / 10)) |= (1 << (((g) % 10) * 3))
 120 #define SET_GPIO_ALT(g, a)  \
 121   *(gpio + (((g) / 10))) |= \
 122       (((a) <= 3 ? (a) + 4 : (a) == 4 ? 3 : 2) << (((g) % 10) * 3))
 123
 124 #define GPIO_SET \
 125   *(gpio + 7)  // sets   bits which are 1 ignores bits which are 0
 126 #define GPIO_CLR \
 127   *(gpio + 10)  // clears bits which are 1 ignores bits which are 0
 128
 129 #define GET_GPIO(g) (*(gpio + 13) & (1 << g))  // 0 if LOW, (1<<g) if HIGH
 130
 131 #define GPIO_PULL *(gpio + 37)      // Pull up/pull down
 132 #define GPIO_PULLCLK0 *(gpio + 38)  // Pull up/pull down clock
 133
 134 void setup_io();
 135
 136 uint32_t read8(uint32_t address);
 137 void write8(uint32_t address, uint32_t data);
 138
 139 uint32_t read16(uint32_t address);
 140 void write16(uint32_t address, uint32_t data);
 141
 142 void write32(uint32_t address, uint32_t data);
 143 uint32_t read32(uint32_t address);
 144
 145 uint16_t read_reg(void);
 146 void write_reg(unsigned int value);
 147
 148 volatile uint16_t srdata;
 149 volatile uint32_t srdata2;
 150 volatile uint32_t srdata2_old;
 151
 152 //unsigned char g_kick[524288];
 153 //unsigned char g_ram[FASTSIZE + 1]; /* RAM */
 154 unsigned char toggle;
 155 int ovl;
 156 static volatile unsigned char maprom;
 157
 158 void sigint_handler(int sig_num) {
 159   //if (sig_num) { }
 160   //cpu_emulation_running = 0;
 161
 162   //return;
 163   printf("Received sigint %d, exiting.\n", sig_num);
 164   if (mouse_fd != -1)
 165     close(mouse_fd);
 166   if (mem_fd)
 167     close(mem_fd);
 168
 169   exit(0);
 170 }
 171
 172 void *iplThread(void *args) {
 173   printf("IPL thread running/n");
 174
 175   while (42) {
 176
 177     if (GET_GPIO(1) == 0) {
 178       toggle = 1;
 179       m68k_end_timeslice();
 180    //printf("thread!/n");
 181     } else {
 182       toggle = 0;
 183     };
 184     usleep(1);
 185   }
 186
 187 }
 188
 189 int main(int argc, char *argv[]) {
 190   int g;
 191   const struct sched_param priority = {99};
 192
 193   // Some command line switch stuffles
 194   for (g = 1; g < argc; g++) {
 195     if (strcmp(argv[g], "--disable-gayle") == 0) {
 196       gayle_emulation_enabled = 0;
 197     }
 198     else if (strcmp(argv[g], "--cpu_type") == 0 || strcmp(argv[g], "--cpu") == 0) {
 199       if (g + 1 >= argc) {
 200         printf("%s switch found, but no CPU type specified.\n", argv[g]);
 201       } else {
 202         g++;
 203         cpu_type = get_m68k_cpu_type(argv[g]);
 204       }
 205     }
 206     else if (strcmp(argv[g], "--config-file") == 0 || strcmp(argv[g], "--config") == 0) {
 207       if (g + 1 >= argc) {
 208         printf("%s switch found, but no config filename specified.\n", argv[g]);
 209       } else {
 210         g++;
 211         cfg = load_config_file(argv[g]);
 212       }
 213     }
 214     else if (strcmp(argv[g], "--keyboard-file") == 0 || strcmp(argv[g], "--kbfile") == 0) {
 215       if (g + 1 >= argc) {
 216         printf("%s switch found, but no keyboard device path specified.\n", argv[g]);
 217       } else {
 218         g++;
 219         strcpy(keyboard_file, argv[g]);
 220       }
 221     }
 222   }
 223
 224   if (!cfg) {
 225     printf("No config file specified. Trying to load default.cfg...\n");
 226     cfg = load_config_file("default.cfg");
 227     if (!cfg) {
 228       printf("Couldn't load default.cfg, empty emulator config will be used.\n");
 229       cfg = (struct emulator_config *)calloc(1, sizeof(struct emulator_config));
 230       if (!cfg) {
 231         printf("Failed to allocate memory for emulator config!\n");
 232         return 1;
 233       }
 234       memset(cfg, 0x00, sizeof(struct emulator_config));
 235     }
 236   }
 237
 238   if (cfg) {
 239     if (cfg->cpu_type) cpu_type = cfg->cpu_type;
 240     if (cfg->loop_cycles) loop_cycles = cfg->loop_cycles;
 241
 242     if (!cfg->platform)
 243       cfg->platform = make_platform_config("none", "generic");
 244     cfg->platform->platform_initial_setup(cfg);
 245   }
 246
 247   if (cfg->mouse_enabled) {
 248     mouse_fd = open(cfg->mouse_file, O_RDONLY | O_NONBLOCK);
 249     if (mouse_fd == -1) {
 250       printf("Failed to open %s, can't enable mouse hook.\n", cfg->mouse_file);
 251       cfg->mouse_enabled = 0;
 252     }
 253   }
 254
 255   keyboard_fd = open(keyboard_file, O_RDONLY | O_NONBLOCK);
 256   if (keyboard_fd == -1) {
 257     printf("Failed to open keyboard event source.\n");
 258   }
 259
 260   sched_setscheduler(0, SCHED_FIFO, &priority);
 261   mlockall(MCL_CURRENT);  // lock in memory to keep us from paging out
 262
 263   InitGayle();
 264
 265   signal(SIGINT, sigint_handler);
 266   setup_io();
 267
 268   //goto skip_everything;
 269
 270   // Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending
 271   // on pi model
 272   printf("Enable 200MHz GPCLK0 on GPIO4\n");
 273
 274   *(gpclk + (CLK_GP0_CTL / 4)) = CLK_PASSWD | (1 << 5);
 275   usleep(10);
 276   while ((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7))
 277     ;
 278   usleep(100);
 279   *(gpclk + (CLK_GP0_DIV / 4)) =
 280       CLK_PASSWD | (6 << 12);  // divider , 6=200MHz on pi3
 281   usleep(10);
 282   *(gpclk + (CLK_GP0_CTL / 4)) =
 283       CLK_PASSWD | 5 | (1 << 4);  // pll? 6=plld, 5=pllc
 284   usleep(10);
 285   while (((*(gpclk + (CLK_GP0_CTL / 4))) & (1 << 7)) == 0)
 286     ;
 287   usleep(100);
 288
 289   SET_GPIO_ALT(4, 0);  // gpclk0
 290
 291   // set SA to output
 292   INP_GPIO(2);
 293   OUT_GPIO(2);
 294   INP_GPIO(3);
 295   OUT_GPIO(3);
 296   INP_GPIO(5);
 297   OUT_GPIO(5);
 298
 299   // set gpio0 (aux0) and gpio1 (aux1) to input
 300   INP_GPIO(0);
 301   INP_GPIO(1);
 302
 303   // Set GPIO pins 6,7 and 8-23 to output
 304   for (g = 6; g <= 23; g++) {
 305     INP_GPIO(g);
 306     OUT_GPIO(g);
 307   }
 308   printf("Precalculate GPIO8-23 as Output\n");
 309   gpfsel0_o = *(gpio);  // store gpio ddr
 310   printf("gpfsel0: %#x\n", gpfsel0_o);
 311   gpfsel1_o = *(gpio + 1);  // store gpio ddr
 312   printf("gpfsel1: %#x\n", gpfsel1_o);
 313   gpfsel2_o = *(gpio + 2);  // store gpio ddr
 314   printf("gpfsel2: %#x\n", gpfsel2_o);
 315
 316   // Set GPIO pins 8-23 to input
 317   for (g = 8; g <= 23; g++) {
 318     INP_GPIO(g);
 319   }
 320   printf("Precalculate GPIO8-23 as Input\n");
 321   gpfsel0 = *(gpio);  // store gpio ddr
 322   printf("gpfsel0: %#x\n", gpfsel0);
 323   gpfsel1 = *(gpio + 1);  // store gpio ddr
 324   printf("gpfsel1: %#x\n", gpfsel1);
 325   gpfsel2 = *(gpio + 2);  // store gpio ddr
 326   printf("gpfsel2: %#x\n", gpfsel2);
 327
 328   GPIO_CLR = 1 << 2;
 329   GPIO_CLR = 1 << 3;
 330   GPIO_SET = 1 << 5;
 331
 332   GPIO_SET = 1 << 6;
 333   GPIO_SET = 1 << 7;
 334
 335   // reset cpld statemachine first
 336
 337   write_reg(0x01);
 338   usleep(100);
 339   usleep(1500);
 340   write_reg(0x00);
 341   usleep(100);
 342
 343   // reset amiga and statemachine
 344   skip_everything:;
 345   cpu_pulse_reset();
 346   ovl = 1;
 347   m68k_write_memory_8(0xbfe201, 0x0001);  // AMIGA OVL
 348   m68k_write_memory_8(0xbfe001, 0x0001);  // AMIGA OVL high (ROM@0x0)
 349
 350   usleep(1500);
 351
 352   m68k_init();
 353   printf("Setting CPU type to %d.\n", cpu_type);
 354   m68k_set_cpu_type(cpu_type);
 355   m68k_pulse_reset();
 356
 357   if (maprom == 1) {
 358     m68k_set_reg(M68K_REG_PC, 0xF80002);
 359   } else {
 360     m68k_set_reg(M68K_REG_PC, 0x0);
 361   }
 362
 363 /*
 364           pthread_t id;
 365           int err;
 366           err = pthread_create(&id, NULL, &iplThread, NULL);
 367           if (err != 0)
 368               printf("\ncan't create IPL thread :[%s]", strerror(err));
 369           else
 370               printf("\n IPL Thread created successfully\n");
 371 */
 372   char c = 0;
 373
 374   m68k_pulse_reset();
 375   while (42) {
 376     if (mouse_hook_enabled) {
 377       if (get_mouse_status(&mouse_dx, &mouse_dy, &mouse_buttons)) {
 378         //printf("Maus: %d (%.2X), %d (%.2X), B:%.2X\n", mouse_dx, mouse_dx, mouse_dy, mouse_dy, mouse_buttons);
 379       }
 380     }
 381
 382     if (cpu_emulation_running)
 383       m68k_execute(loop_cycles);
 384
 385     // FIXME: Rework this to use keyboard events instead.
 386     /*while (get_key_char(&c)) {
 387       if (c == cfg->keyboard_toggle_key && !kb_hook_enabled) {
 388         kb_hook_enabled = 1;
 389         printf("Keyboard hook enabled.\n");
 390       }
 391       else if (c == 0x1B && kb_hook_enabled) {
 392         kb_hook_enabled = 0;
 393         printf("Keyboard hook disabled.\n");
 394       }
 395       if (!kb_hook_enabled) {
 396         if (c == cfg->mouse_toggle_key) {
 397           mouse_hook_enabled ^= 1;
 398           printf("Mouse hook %s.\n", mouse_hook_enabled ? "enabled" : "disabled");
 399           mouse_dx = mouse_dy = mouse_buttons = 0;
 400         }
 401         if (c == 'r') {
 402           cpu_emulation_running ^= 1;
 403           printf("CPU emulation is now %s\n", cpu_emulation_running ? "running" : "stopped");
 404         }
 405         if (c == 'R') {
 406           cpu_pulse_reset();
 407           m68k_pulse_reset();
 408           printf("CPU emulation reset.\n");
 409         }
 410         if (c == 'q') {
 411           printf("Quitting and exiting emulator.\n");
 412           goto stop_cpu_emulation;
 413         }
 414       }
 415     }*/
 416 /*
 417     if (toggle == 1){
 418       srdata = read_reg();
 419       m68k_set_irq((srdata >> 13) & 0xff);
 420     } else {
 421          m68k_set_irq(0);
 422     };
 423     usleep(1);
 424 */
 425
 426
 427     if (GET_GPIO(1) == 0) {
 428       srdata = read_reg();
 429       m68k_set_irq((srdata >> 13) & 0xff);
 430     } else {
 431       if (CheckIrq() == 1) {
 432         write16(0xdff09c, 0x8008);
 433         m68k_set_irq(2);
 434       }
 435       else
 436          m68k_set_irq(0);
 437     };
 438
 439   }
 440
 441   stop_cpu_emulation:;
 442
 443   if (mouse_fd != -1)
 444     close(mouse_fd);
 445   if (mem_fd)
 446     close(mem_fd);
 447
 448   return 0;
 449 }
 450
 451 void cpu_pulse_reset(void) {
 452   write_reg(0x00);
 453   // printf("Status Reg%x\n",read_reg());
 454   usleep(100000);
 455   write_reg(0x02);
 456   // printf("Status Reg%x\n",read_reg());
 457 }
 458
 459 int cpu_irq_ack(int level) {
 460   printf("cpu irq ack\n");
 461   return level;
 462 }
 463
 464 static unsigned int target = 0;
 465
 466 #define PLATFORM_CHECK_READ(a) \
 467   if (address >= cfg->custom_low && address < cfg->custom_high) { \
 468     unsigned int target = 0; \
 469     switch(cfg->platform->id) { \
 470       case PLATFORM_AMIGA: { \
 471         if (custom_read_amiga(cfg, address, &target, a) != -1) { \
 472           return target; \
 473         } \
 474         break; \
 475       } \
 476       default: \
 477         break; \
 478     } \
 479   } \
 480   if (ovl || (address >= cfg->mapped_low && address < cfg->mapped_high)) { \
 481     if (handle_mapped_read(cfg, address, &target, a) != -1) \
 482       return target; \
 483   }
 484
 485 unsigned int m68k_read_memory_8(unsigned int address) {
 486   PLATFORM_CHECK_READ(OP_TYPE_BYTE);
 487
 488     address &=0xFFFFFF;
 489 //  if (address < 0xffffff) {
 490     return read8((uint32_t)address);
 491 //  }
 492
 493 //  return 1;
 494 }
 495
 496 unsigned int m68k_read_memory_16(unsigned int address) {
 497   PLATFORM_CHECK_READ(OP_TYPE_WORD);
 498
 499   if (mouse_hook_enabled) {
 500     if (address == JOY0DAT) {
 501       // Forward mouse valueses to Amyga.
 502       unsigned short result = (mouse_dy << 8) | (mouse_dx);
 503       mouse_dx = mouse_dy = 0;
 504       return (unsigned int)result;
 505     }
 506     if (address == CIAAPRA) {
 507       unsigned short result = (unsigned int)read16((uint32_t)address);
 508       if (mouse_buttons & 0x01) {
 509         mouse_buttons -= 1;
 510         return (unsigned int)(result | 0x40);
 511       }
 512       else
 513           return (unsigned int)result;
 514     }
 515     if (address == POTGOR) {
 516       unsigned short result = (unsigned int)read16((uint32_t)address);
 517       if (mouse_buttons & 0x02) {
 518         mouse_buttons -= 2;
 519         return (unsigned int)(result | 0x2);
 520       }
 521       else
 522           return (unsigned int)result;
 523     }
 524   }
 525
 526 //  if (address < 0xffffff) {
 527     address &=0xFFFFFF;
 528     return (unsigned int)read16((uint32_t)address);
 529 //  }
 530
 531 //  return 1;
 532 }
 533
 534 unsigned int m68k_read_memory_32(unsigned int address) {
 535   PLATFORM_CHECK_READ(OP_TYPE_LONGWORD);
 536
 537 //  if (address < 0xffffff) {
 538     address &=0xFFFFFF;
 539     uint16_t a = read16(address);
 540     uint16_t b = read16(address + 2);
 541     return (a << 16) | b;
 542 //  }
 543
 544 //  return 1;
 545 }
 546
 547 #define PLATFORM_CHECK_WRITE(a) \
 548   if (address >= cfg->custom_low && address < cfg->custom_high) { \
 549     switch(cfg->platform->id) { \
 550       case PLATFORM_AMIGA: { \
 551         if (custom_write_amiga(cfg, address, value, a) != -1) { \
 552           return; \
 553         } \
 554         break; \
 555       } \
 556       default: \
 557         break; \
 558     } \
 559   } \
 560   if (address >= cfg->mapped_low && address < cfg->mapped_high) { \
 561     if (handle_mapped_write(cfg, address, value, a) != -1) \
 562       return; \
 563   }
 564
 565 void m68k_write_memory_8(unsigned int address, unsigned int value) {
 566   PLATFORM_CHECK_WRITE(OP_TYPE_BYTE);
 567
 568   if (address == 0xbfe001) {
 569     ovl = (value & (1 << 0));
 570     printf("OVL:%x\n", ovl);
 571   }
 572
 573 //  if (address < 0xffffff) {
 574     address &=0xFFFFFF;
 575     write8((uint32_t)address, value);
 576     return;
 577 //  }
 578
 579 //  return;
 580 }
 581
 582 void m68k_write_memory_16(unsigned int address, unsigned int value) {
 583   PLATFORM_CHECK_WRITE(OP_TYPE_WORD);
 584
 585 //  if (address < 0xffffff) {
 586     address &=0xFFFFFF;
 587     write16((uint32_t)address, value);
 588     return;
 589 //  }
 590 //  return;
 591 }
 592
 593 void m68k_write_memory_32(unsigned int address, unsigned int value) {
 594   PLATFORM_CHECK_WRITE(OP_TYPE_LONGWORD);
 595
 596 //  if (address < 0xffffff) {
 597     address &=0xFFFFFF;
 598     write16(address, value >> 16);
 599     write16(address + 2, value);
 600     return;
 601 //  }
 602
 603 //  return;
 604 }
 605
 606 inline void write16(uint32_t address, uint32_t data) {
 607   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 608   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 609   uint32_t addr_l_s = (address >> 16) << 8;
 610   uint32_t addr_l_r = (~address >> 16) << 8;
 611   uint32_t data_s = (data & 0x0000ffff) << 8;
 612   uint32_t data_r = (~data & 0x0000ffff) << 8;
 613
 614   //      asm volatile ("dmb" ::: "memory");
 615   W16
 616   *(gpio) = gpfsel0_o;
 617   *(gpio + 1) = gpfsel1_o;
 618   *(gpio + 2) = gpfsel2_o;
 619
 620   *(gpio + 7) = addr_h_s;
 621   *(gpio + 10) = addr_h_r;
 622   GPIO_CLR = 1 << 7;
 623   GPIO_SET = 1 << 7;
 624
 625   *(gpio + 7) = addr_l_s;
 626   *(gpio + 10) = addr_l_r;
 627   GPIO_CLR = 1 << 7;
 628   GPIO_SET = 1 << 7;
 629
 630   // write phase
 631   *(gpio + 7) = data_s;
 632   *(gpio + 10) = data_r;
 633   GPIO_CLR = 1 << 7;
 634   GPIO_SET = 1 << 7;
 635
 636   *(gpio) = gpfsel0;
 637   *(gpio + 1) = gpfsel1;
 638   *(gpio + 2) = gpfsel2;
 639   while ((GET_GPIO(0)))
 640     ;
 641   //     asm volatile ("dmb" ::: "memory");
 642 }
 643
 644 inline void write8(uint32_t address, uint32_t data) {
 645   if ((address & 1) == 0)
 646     data = data + (data << 8);  // EVEN, A0=0,UDS
 647   else
 648     data = data & 0xff;  // ODD , A0=1,LDS
 649   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 650   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 651   uint32_t addr_l_s = (address >> 16) << 8;
 652   uint32_t addr_l_r = (~address >> 16) << 8;
 653   uint32_t data_s = (data & 0x0000ffff) << 8;
 654   uint32_t data_r = (~data & 0x0000ffff) << 8;
 655
 656   //   asm volatile ("dmb" ::: "memory");
 657   W8
 658   *(gpio) = gpfsel0_o;
 659   *(gpio + 1) = gpfsel1_o;
 660   *(gpio + 2) = gpfsel2_o;
 661
 662   *(gpio + 7) = addr_h_s;
 663   *(gpio + 10) = addr_h_r;
 664   GPIO_CLR = 1 << 7;
 665   GPIO_SET = 1 << 7;
 666
 667   *(gpio + 7) = addr_l_s;
 668   *(gpio + 10) = addr_l_r;
 669   GPIO_CLR = 1 << 7;
 670   GPIO_SET = 1 << 7;
 671
 672   // write phase
 673   *(gpio + 7) = data_s;
 674   *(gpio + 10) = data_r;
 675   GPIO_CLR = 1 << 7;
 676   GPIO_SET = 1 << 7;
 677
 678   *(gpio) = gpfsel0;
 679   *(gpio + 1) = gpfsel1;
 680   *(gpio + 2) = gpfsel2;
 681   while ((GET_GPIO(0)))
 682     ;
 683   //   asm volatile ("dmb" ::: "memory");
 684 }
 685
 686 inline uint32_t read16(uint32_t address) {
 687   volatile int val;
 688   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 689   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 690   uint32_t addr_l_s = (address >> 16) << 8;
 691   uint32_t addr_l_r = (~address >> 16) << 8;
 692
 693   //   asm volatile ("dmb" ::: "memory");
 694   R16
 695   *(gpio) = gpfsel0_o;
 696   *(gpio + 1) = gpfsel1_o;
 697   *(gpio + 2) = gpfsel2_o;
 698
 699   *(gpio + 7) = addr_h_s;
 700   *(gpio + 10) = addr_h_r;
 701   GPIO_CLR = 1 << 7;
 702   GPIO_SET = 1 << 7;
 703
 704   *(gpio + 7) = addr_l_s;
 705   *(gpio + 10) = addr_l_r;
 706   GPIO_CLR = 1 << 7;
 707   GPIO_SET = 1 << 7;
 708
 709   // read phase
 710   *(gpio) = gpfsel0;
 711   *(gpio + 1) = gpfsel1;
 712   *(gpio + 2) = gpfsel2;
 713   GPIO_CLR = 1 << 6;
 714   while (!(GET_GPIO(0)))
 715     ;
 716   GPIO_CLR = 1 << 6;
 717   val = *(gpio + 13);
 718   GPIO_SET = 1 << 6;
 719   //    asm volatile ("dmb" ::: "memory");
 720   return (val >> 8) & 0xffff;
 721 }
 722
 723 inline uint32_t read8(uint32_t address) {
 724   int val;
 725   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 726   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 727   uint32_t addr_l_s = (address >> 16) << 8;
 728   uint32_t addr_l_r = (~address >> 16) << 8;
 729
 730   //    asm volatile ("dmb" ::: "memory");
 731   R8
 732   *(gpio) = gpfsel0_o;
 733   *(gpio + 1) = gpfsel1_o;
 734   *(gpio + 2) = gpfsel2_o;
 735
 736   *(gpio + 7) = addr_h_s;
 737   *(gpio + 10) = addr_h_r;
 738   GPIO_CLR = 1 << 7;
 739   GPIO_SET = 1 << 7;
 740
 741   *(gpio + 7) = addr_l_s;
 742   *(gpio + 10) = addr_l_r;
 743   GPIO_CLR = 1 << 7;
 744   GPIO_SET = 1 << 7;
 745
 746   // read phase
 747   *(gpio) = gpfsel0;
 748   *(gpio + 1) = gpfsel1;
 749   *(gpio + 2) = gpfsel2;
 750
 751   GPIO_CLR = 1 << 6;
 752   while (!(GET_GPIO(0)))
 753     ;
 754   GPIO_CLR = 1 << 6;
 755   val = *(gpio + 13);
 756   GPIO_SET = 1 << 6;
 757   //    asm volatile ("dmb" ::: "memory");
 758
 759   val = (val >> 8) & 0xffff;
 760   if ((address & 1) == 0)
 761     return (val >> 8) & 0xff;  // EVEN, A0=0,UDS
 762   else
 763     return val & 0xff;  // ODD , A0=1,LDS
 764 }
 765
 766 /******************************************************/
 767
 768 void write_reg(unsigned int value) {
 769   STATUSREGADDR
 770   *(gpio) = gpfsel0_o;
 771   *(gpio + 1) = gpfsel1_o;
 772   *(gpio + 2) = gpfsel2_o;
 773   *(gpio + 7) = (value & 0xffff) << 8;
 774   *(gpio + 10) = (~value & 0xffff) << 8;
 775   GPIO_CLR = 1 << 7;
 776   GPIO_CLR = 1 << 7;  // delay
 777   GPIO_SET = 1 << 7;
 778   GPIO_SET = 1 << 7;
 779   // Bus HIGH-Z
 780   *(gpio) = gpfsel0;
 781   *(gpio + 1) = gpfsel1;
 782   *(gpio + 2) = gpfsel2;
 783 }
 784
 785 uint16_t read_reg(void) {
 786   uint32_t val;
 787   STATUSREGADDR
 788   // Bus HIGH-Z
 789   *(gpio) = gpfsel0;
 790   *(gpio + 1) = gpfsel1;
 791   *(gpio + 2) = gpfsel2;
 792   GPIO_CLR = 1 << 6;
 793   GPIO_CLR = 1 << 6;  // delay
 794   GPIO_CLR = 1 << 6;
 795   GPIO_CLR = 1 << 6;
 796   val = *(gpio + 13);
 797   GPIO_SET = 1 << 6;
 798   return (uint16_t)(val >> 8);
 799 }
 800
 801 //
 802 // Set up a memory regions to access GPIO
 803 //
 804 void setup_io() {
 805   /* open /dev/mem */
 806   if ((mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
 807     printf("can't open /dev/mem \n");
 808     exit(-1);
 809   }
 810
 811   /* mmap GPIO */
 812   gpio_map = mmap(
 813       NULL,                    // Any adddress in our space will do
 814       BCM2708_PERI_SIZE,       // Map length
 815       PROT_READ | PROT_WRITE,  // Enable reading & writting to mapped memory
 816       MAP_SHARED,              // Shared with other processes
 817       mem_fd,                  // File to map
 818       BCM2708_PERI_BASE        // Offset to GPIO peripheral
 819   );
 820
 821   close(mem_fd);  // No need to keep mem_fd open after mmap
 822
 823   if (gpio_map == MAP_FAILED) {
 824     printf("gpio mmap error %d\n", (int)gpio_map);  // errno also set!
 825     exit(-1);
 826   }
 827
 828   gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
 829   gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
 830
 831 }  // setup_io