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