git.sesse.net Git - pistorm/blob - emulator.c

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