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