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