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     // FIXME: Rework this to use keyboard events instead.
 365     /*while (kbhit()) {
 366       char c = getchar();
 367       if (c == cfg->keyboard_toggle_key && !kb_hook_enabled) {
 368         kb_hook_enabled = 1;
 369         printf("Keyboard hook enabled.\n");
 370       }
 371       else if (c == 0x1B && kb_hook_enabled) {
 372         kb_hook_enabled = 0;
 373         printf("Keyboard hook disabled.\n");
 374       }
 375       if (c == cfg->mouse_toggle_key) {
 376         mouse_hook_enabled ^= 1;
 377         printf("Mouse hook %s.\n", mouse_hook_enabled ? "enabled" : "disabled");
 378         mouse_dx = mouse_dy = mouse_buttons = 0;
 379       }
 380       if (c == 'r') {
 381         cpu_emulation_running ^= 1;
 382         printf("CPU emulation is now %s\n", cpu_emulation_running ? "running" : "stopped");
 383       }
 384       if (c == 'R') {
 385         cpu_pulse_reset();
 386         m68k_pulse_reset();
 387         printf("CPU emulation reset.\n");
 388       }
 389       if (c == 'q') {
 390         printf("Quitting and exiting emulator.\n");
 391         goto stop_cpu_emulation;
 392       }
 393     }*/
 394 /*
 395     if (toggle == 1){
 396       srdata = read_reg();
 397       m68k_set_irq((srdata >> 13) & 0xff);
 398     } else {
 399          m68k_set_irq(0);
 400     };
 401     usleep(1);
 402 */
 403
 404
 405     if (GET_GPIO(1) == 0) {
 406       srdata = read_reg();
 407       m68k_set_irq((srdata >> 13) & 0xff);
 408     } else {
 409       if (CheckIrq() == 1) {
 410         write16(0xdff09c, 0x8008);
 411         m68k_set_irq(2);
 412       }
 413       else
 414          m68k_set_irq(0);
 415     };
 416
 417   }
 418
 419   stop_cpu_emulation:;
 420
 421   if (mouse_fd != -1)
 422     close(mouse_fd);
 423   if (mem_fd)
 424     close(mem_fd);
 425
 426   return 0;
 427 }
 428
 429 void cpu_pulse_reset(void) {
 430   write_reg(0x00);
 431   // printf("Status Reg%x\n",read_reg());
 432   usleep(100000);
 433   write_reg(0x02);
 434   // printf("Status Reg%x\n",read_reg());
 435 }
 436
 437 int cpu_irq_ack(int level) {
 438   printf("cpu irq ack\n");
 439   return level;
 440 }
 441
 442 static unsigned int target = 0;
 443
 444 unsigned int m68k_read_memory_8(unsigned int address) {
 445   if (cfg) {
 446     int ret = handle_mapped_read(cfg, address, &target, OP_TYPE_BYTE, ovl);
 447     if (ret != -1)
 448       return target;
 449   }
 450
 451     address &=0xFFFFFF;
 452 //  if (address < 0xffffff) {
 453     return read8((uint32_t)address);
 454 //  }
 455
 456 //  return 1;
 457 }
 458
 459 unsigned int m68k_read_memory_16(unsigned int address) {
 460   if (cfg) {
 461     int ret = handle_mapped_read(cfg, address, &target, OP_TYPE_WORD, ovl);
 462     if (ret != -1)
 463       return target;
 464   }
 465
 466   if (mouse_hook_enabled) {
 467     if (address == JOY0DAT) {
 468       // Forward mouse valueses to Amyga.
 469       unsigned short result = (mouse_dy << 8) | (mouse_dx);
 470       mouse_dx = mouse_dy = 0;
 471       return (unsigned int)result;
 472     }
 473     if (address == CIAAPRA) {
 474       unsigned short result = (unsigned int)read16((uint32_t)address);
 475       if (mouse_buttons & 0x01) {
 476         mouse_buttons -= 1;
 477         return (unsigned int)(result | 0x40);
 478       }
 479       else
 480           return (unsigned int)result;
 481     }
 482     if (address == POTGOR) {
 483       unsigned short result = (unsigned int)read16((uint32_t)address);
 484       if (mouse_buttons & 0x02) {
 485         mouse_buttons -= 2;
 486         return (unsigned int)(result | 0x2);
 487       }
 488       else
 489           return (unsigned int)result;
 490     }
 491   }
 492
 493 //  if (address < 0xffffff) {
 494     address &=0xFFFFFF;
 495     return (unsigned int)read16((uint32_t)address);
 496 //  }
 497
 498 //  return 1;
 499 }
 500
 501 unsigned int m68k_read_memory_32(unsigned int address) {
 502   if (cfg) {
 503     int ret = handle_mapped_read(cfg, address, &target, OP_TYPE_LONGWORD, ovl);
 504     if (ret != -1)
 505       return target;
 506   }
 507
 508 //  if (address < 0xffffff) {
 509     address &=0xFFFFFF;
 510     uint16_t a = read16(address);
 511     uint16_t b = read16(address + 2);
 512     return (a << 16) | b;
 513 //  }
 514
 515 //  return 1;
 516 }
 517
 518 void m68k_write_memory_8(unsigned int address, unsigned int value) {
 519   if (cfg) {
 520     int ret = handle_mapped_write(cfg, address, value, OP_TYPE_BYTE, ovl);
 521     if (ret != -1)
 522       return;
 523   }
 524
 525   if (address == 0xbfe001) {
 526     ovl = (value & (1 << 0));
 527     printf("OVL:%x\n", ovl);
 528   }
 529
 530 //  if (address < 0xffffff) {
 531     address &=0xFFFFFF;
 532     write8((uint32_t)address, value);
 533     return;
 534 //  }
 535
 536 //  return;
 537 }
 538
 539 void m68k_write_memory_16(unsigned int address, unsigned int value) {
 540   if (cfg) {
 541     int ret = handle_mapped_write(cfg, address, value, OP_TYPE_WORD, ovl);
 542     if (ret != -1)
 543       return;
 544   }
 545
 546 //  if (address < 0xffffff) {
 547     address &=0xFFFFFF;
 548     write16((uint32_t)address, value);
 549     return;
 550 //  }
 551 //  return;
 552 }
 553
 554 void m68k_write_memory_32(unsigned int address, unsigned int value) {
 555   if (cfg) {
 556     int ret = handle_mapped_write(cfg, address, value, OP_TYPE_LONGWORD, ovl);
 557     if (ret != -1)
 558       return;
 559   }
 560
 561 //  if (address < 0xffffff) {
 562     address &=0xFFFFFF;
 563     write16(address, value >> 16);
 564     write16(address + 2, value);
 565     return;
 566 //  }
 567
 568 //  return;
 569 }
 570
 571 void write16(uint32_t address, uint32_t data) {
 572   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 573   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 574   uint32_t addr_l_s = (address >> 16) << 8;
 575   uint32_t addr_l_r = (~address >> 16) << 8;
 576   uint32_t data_s = (data & 0x0000ffff) << 8;
 577   uint32_t data_r = (~data & 0x0000ffff) << 8;
 578
 579   //      asm volatile ("dmb" ::: "memory");
 580   W16
 581   *(gpio) = gpfsel0_o;
 582   *(gpio + 1) = gpfsel1_o;
 583   *(gpio + 2) = gpfsel2_o;
 584
 585   *(gpio + 7) = addr_h_s;
 586   *(gpio + 10) = addr_h_r;
 587   GPIO_CLR = 1 << 7;
 588   GPIO_SET = 1 << 7;
 589
 590   *(gpio + 7) = addr_l_s;
 591   *(gpio + 10) = addr_l_r;
 592   GPIO_CLR = 1 << 7;
 593   GPIO_SET = 1 << 7;
 594
 595   // write phase
 596   *(gpio + 7) = data_s;
 597   *(gpio + 10) = data_r;
 598   GPIO_CLR = 1 << 7;
 599   GPIO_SET = 1 << 7;
 600
 601   *(gpio) = gpfsel0;
 602   *(gpio + 1) = gpfsel1;
 603   *(gpio + 2) = gpfsel2;
 604   while ((GET_GPIO(0)))
 605     ;
 606   //     asm volatile ("dmb" ::: "memory");
 607 }
 608
 609 void write8(uint32_t address, uint32_t data) {
 610   if ((address & 1) == 0)
 611     data = data + (data << 8);  // EVEN, A0=0,UDS
 612   else
 613     data = data & 0xff;  // ODD , A0=1,LDS
 614   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 615   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 616   uint32_t addr_l_s = (address >> 16) << 8;
 617   uint32_t addr_l_r = (~address >> 16) << 8;
 618   uint32_t data_s = (data & 0x0000ffff) << 8;
 619   uint32_t data_r = (~data & 0x0000ffff) << 8;
 620
 621   //   asm volatile ("dmb" ::: "memory");
 622   W8
 623   *(gpio) = gpfsel0_o;
 624   *(gpio + 1) = gpfsel1_o;
 625   *(gpio + 2) = gpfsel2_o;
 626
 627   *(gpio + 7) = addr_h_s;
 628   *(gpio + 10) = addr_h_r;
 629   GPIO_CLR = 1 << 7;
 630   GPIO_SET = 1 << 7;
 631
 632   *(gpio + 7) = addr_l_s;
 633   *(gpio + 10) = addr_l_r;
 634   GPIO_CLR = 1 << 7;
 635   GPIO_SET = 1 << 7;
 636
 637   // write phase
 638   *(gpio + 7) = data_s;
 639   *(gpio + 10) = data_r;
 640   GPIO_CLR = 1 << 7;
 641   GPIO_SET = 1 << 7;
 642
 643   *(gpio) = gpfsel0;
 644   *(gpio + 1) = gpfsel1;
 645   *(gpio + 2) = gpfsel2;
 646   while ((GET_GPIO(0)))
 647     ;
 648   //   asm volatile ("dmb" ::: "memory");
 649 }
 650
 651 uint32_t read16(uint32_t address) {
 652   volatile int val;
 653   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 654   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 655   uint32_t addr_l_s = (address >> 16) << 8;
 656   uint32_t addr_l_r = (~address >> 16) << 8;
 657
 658   //   asm volatile ("dmb" ::: "memory");
 659   R16
 660   *(gpio) = gpfsel0_o;
 661   *(gpio + 1) = gpfsel1_o;
 662   *(gpio + 2) = gpfsel2_o;
 663
 664   *(gpio + 7) = addr_h_s;
 665   *(gpio + 10) = addr_h_r;
 666   GPIO_CLR = 1 << 7;
 667   GPIO_SET = 1 << 7;
 668
 669   *(gpio + 7) = addr_l_s;
 670   *(gpio + 10) = addr_l_r;
 671   GPIO_CLR = 1 << 7;
 672   GPIO_SET = 1 << 7;
 673
 674   // read phase
 675   *(gpio) = gpfsel0;
 676   *(gpio + 1) = gpfsel1;
 677   *(gpio + 2) = gpfsel2;
 678   GPIO_CLR = 1 << 6;
 679   while (!(GET_GPIO(0)))
 680     ;
 681   GPIO_CLR = 1 << 6;
 682   val = *(gpio + 13);
 683   GPIO_SET = 1 << 6;
 684   //    asm volatile ("dmb" ::: "memory");
 685   return (val >> 8) & 0xffff;
 686 }
 687
 688 uint32_t read8(uint32_t address) {
 689   int val;
 690   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 691   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 692   uint32_t addr_l_s = (address >> 16) << 8;
 693   uint32_t addr_l_r = (~address >> 16) << 8;
 694
 695   //    asm volatile ("dmb" ::: "memory");
 696   R8
 697   *(gpio) = gpfsel0_o;
 698   *(gpio + 1) = gpfsel1_o;
 699   *(gpio + 2) = gpfsel2_o;
 700
 701   *(gpio + 7) = addr_h_s;
 702   *(gpio + 10) = addr_h_r;
 703   GPIO_CLR = 1 << 7;
 704   GPIO_SET = 1 << 7;
 705
 706   *(gpio + 7) = addr_l_s;
 707   *(gpio + 10) = addr_l_r;
 708   GPIO_CLR = 1 << 7;
 709   GPIO_SET = 1 << 7;
 710
 711   // read phase
 712   *(gpio) = gpfsel0;
 713   *(gpio + 1) = gpfsel1;
 714   *(gpio + 2) = gpfsel2;
 715
 716   GPIO_CLR = 1 << 6;
 717   while (!(GET_GPIO(0)))
 718     ;
 719   GPIO_CLR = 1 << 6;
 720   val = *(gpio + 13);
 721   GPIO_SET = 1 << 6;
 722   //    asm volatile ("dmb" ::: "memory");
 723
 724   val = (val >> 8) & 0xffff;
 725   if ((address & 1) == 0)
 726     return (val >> 8) & 0xff;  // EVEN, A0=0,UDS
 727   else
 728     return val & 0xff;  // ODD , A0=1,LDS
 729 }
 730
 731 /******************************************************/
 732
 733 void write_reg(unsigned int value) {
 734   STATUSREGADDR
 735   *(gpio) = gpfsel0_o;
 736   *(gpio + 1) = gpfsel1_o;
 737   *(gpio + 2) = gpfsel2_o;
 738   *(gpio + 7) = (value & 0xffff) << 8;
 739   *(gpio + 10) = (~value & 0xffff) << 8;
 740   GPIO_CLR = 1 << 7;
 741   GPIO_CLR = 1 << 7;  // delay
 742   GPIO_SET = 1 << 7;
 743   GPIO_SET = 1 << 7;
 744   // Bus HIGH-Z
 745   *(gpio) = gpfsel0;
 746   *(gpio + 1) = gpfsel1;
 747   *(gpio + 2) = gpfsel2;
 748 }
 749
 750 uint16_t read_reg(void) {
 751   uint32_t val;
 752   STATUSREGADDR
 753   // Bus HIGH-Z
 754   *(gpio) = gpfsel0;
 755   *(gpio + 1) = gpfsel1;
 756   *(gpio + 2) = gpfsel2;
 757   GPIO_CLR = 1 << 6;
 758   GPIO_CLR = 1 << 6;  // delay
 759   GPIO_CLR = 1 << 6;
 760   GPIO_CLR = 1 << 6;
 761   val = *(gpio + 13);
 762   GPIO_SET = 1 << 6;
 763   return (uint16_t)(val >> 8);
 764 }
 765
 766 //
 767 // Set up a memory regions to access GPIO
 768 //
 769 void setup_io() {
 770   /* open /dev/mem */
 771   if ((mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
 772     printf("can't open /dev/mem \n");
 773     exit(-1);
 774   }
 775
 776   /* mmap GPIO */
 777   gpio_map = mmap(
 778       NULL,                    // Any adddress in our space will do
 779       BCM2708_PERI_SIZE,       // Map length
 780       PROT_READ | PROT_WRITE,  // Enable reading & writting to mapped memory
 781       MAP_SHARED,              // Shared with other processes
 782       mem_fd,                  // File to map
 783       BCM2708_PERI_BASE        // Offset to GPIO peripheral
 784   );
 785
 786   close(mem_fd);  // No need to keep mem_fd open after mmap
 787
 788   if (gpio_map == MAP_FAILED) {
 789     printf("gpio mmap error %d\n", (int)gpio_map);  // errno also set!
 790     exit(-1);
 791   }
 792
 793   gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
 794   gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
 795
 796 }  // setup_io