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