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