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