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