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