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 2
  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 #define AC_MEM_SIZE_8MB 0
 365 #define AC_MEM_SIZE_64KB 1
 366 #define AC_MEM_SIZE_128KB 2
 367 #define AC_MEM_SIZE_256KB 3
 368 #define AC_MEM_SIZE_512KB 4
 369 #define AC_MEM_SIZE_1MB 5
 370 #define AC_MEM_SIZE_2MB 6
 371 #define AC_MEM_SIZE_4MB 7
 372
 373 static unsigned char ac_fast_ram_rom[] = {
 374     0xe, AC_MEM_SIZE_8MB,                   // 00/02, link into memory free list, 8 MB
 375     0x6, 0x9,                               // 04/06, product id
 376     0x8, 0x0,                               // 08/0a, preference to 8 MB space
 377     0x0, 0x0,                               // 0c/0e, reserved
 378     0x0, 0x7, 0xd, 0xb,                     // 10/12/14/16, mfg id
 379     0x0, 0x0, 0x0, 0x0, 0x0, 0x4, 0x2, 0x0  // 18/.../26, serial
 380 };
 381
 382 static unsigned char ac_a314_rom[] = {
 383     0xc, AC_MEM_SIZE_64KB,                  // 00/02, 64 kB
 384     0xa, 0x3,                               // 04/06, product id
 385     0x0, 0x0,                               // 08/0a, any space okay
 386     0x0, 0x0,                               // 0c/0e, reserved
 387     0x0, 0x7, 0xd, 0xb,                     // 10/12/14/16, mfg id
 388     0xa, 0x3, 0x1, 0x4, 0x0, 0x0, 0x0, 0x0  // 18/.../26, serial
 389 };
 390
 391 static unsigned int autoconfig_read_memory_8(unsigned int address) {
 392   unsigned char *rom = NULL;
 393
 394   if (ac_current_pic == 0)
 395     rom = ac_fast_ram_rom;
 396   else if (ac_current_pic == 1)
 397     rom = ac_a314_rom;
 398
 399   unsigned char val = 0;
 400   if ((address & 1) == 0 && (address / 2) < sizeof(ac_fast_ram_rom))
 401     val = rom[address / 2];
 402   val <<= 4;
 403   if (address != 0 && address != 2 && address != 40 && address != 42)
 404     val ^= 0xf0;
 405   return (unsigned int)val;
 406 }
 407
 408 static void autoconfig_write_memory_8(unsigned int address, unsigned int value) {
 409   int done = 0;
 410
 411   unsigned int *base = NULL;
 412   int *base_configured = NULL;
 413
 414   if (ac_current_pic == 0) {
 415     base = &fast_base;
 416     base_configured = &fast_base_configured;
 417   } else if (ac_current_pic == 1) {
 418     base = &a314_base;
 419     base_configured = &a314_base_configured;
 420   }
 421
 422   if (address == 0x4a) {  // base[19:16]
 423     *base = (value & 0xf0) << (16 - 4);
 424   } else if (address == 0x48) {  // base[23:20]
 425     *base &= 0xff0fffff;
 426     *base |= (value & 0xf0) << (20 - 4);
 427     *base_configured = 1;
 428
 429     if (ac_current_pic == 0)  // fast ram
 430       a314_set_mem_base_size(*base, FAST_SIZE);
 431
 432     done = 1;
 433   } else if (address == 0x4c) {  // shut up
 434     done = 1;
 435   }
 436
 437   if (done) {
 438     ac_current_pic++;
 439     if (ac_current_pic == AC_PIC_COUNT)
 440       ac_done = 1;
 441   }
 442 }
 443
 444 unsigned int m68k_read_memory_8(unsigned int address) {
 445   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 446     return fast_ram_array[address - fast_base];
 447   }
 448
 449   if (!ac_done && address >= AC_BASE && address < AC_BASE + AC_SIZE) {
 450     return autoconfig_read_memory_8(address - AC_BASE);
 451   }
 452
 453   if (maprom == 1) {
 454     if (address >= KICKBASE && address < KICKBASE + KICKSIZE) {
 455       return g_kick[address - KICKBASE];
 456     }
 457   }
 458
 459   if (gayle_emulation_enabled) {
 460     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 461       return readGayleB(address);
 462     }
 463   }
 464
 465 #if A314_ENABLED
 466   if (a314_base_configured && address >= a314_base && address < a314_base + A314_COM_AREA_SIZE) {
 467     return a314_read_memory_8(address - a314_base);
 468   }
 469 #endif
 470
 471   address &= 0xFFFFFF;
 472   //  if (address < 0xffffff) {
 473   return read8((uint32_t)address);
 474   //  }
 475
 476   //  return 1;
 477 }
 478
 479 unsigned int m68k_read_memory_16(unsigned int address) {
 480   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 481     return be16toh(*(uint16_t *)&fast_ram_array[address - fast_base]);
 482   }
 483
 484   if (maprom == 1) {
 485     if (address >= KICKBASE && address < KICKBASE + KICKSIZE) {
 486       return be16toh(*(uint16_t *)&g_kick[address - KICKBASE]);
 487     }
 488   }
 489
 490   if (gayle_emulation_enabled) {
 491     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 492       return readGayle(address);
 493     }
 494   }
 495
 496 #if A314_ENABLED
 497   if (a314_base_configured && address >= a314_base && address < a314_base + A314_COM_AREA_SIZE) {
 498     return a314_read_memory_16(address - a314_base);
 499   }
 500 #endif
 501
 502   //  if (address < 0xffffff) {
 503   address &= 0xFFFFFF;
 504   return (unsigned int)read16((uint32_t)address);
 505   //  }
 506
 507   //  return 1;
 508 }
 509
 510 unsigned int m68k_read_memory_32(unsigned int address) {
 511   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 512     return be32toh(*(uint32_t *)&fast_ram_array[address - fast_base]);
 513   }
 514
 515   if (maprom == 1) {
 516     if (address >= KICKBASE && address < KICKBASE + KICKSIZE) {
 517       return be32toh(*(uint32_t *)&g_kick[address - KICKBASE]);
 518     }
 519   }
 520
 521   if (gayle_emulation_enabled) {
 522     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 523       return readGayleL(address);
 524     }
 525   }
 526
 527 #if A314_ENABLED
 528   if (a314_base_configured && address >= a314_base && address < a314_base + A314_COM_AREA_SIZE) {
 529     return a314_read_memory_32(address - a314_base);
 530   }
 531 #endif
 532
 533   //  if (address < 0xffffff) {
 534   address &= 0xFFFFFF;
 535   uint16_t a = read16(address);
 536   uint16_t b = read16(address + 2);
 537   return (a << 16) | b;
 538   //  }
 539
 540   //  return 1;
 541 }
 542
 543 void m68k_write_memory_8(unsigned int address, unsigned int value) {
 544   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 545     fast_ram_array[address - fast_base] = value;
 546     return;
 547   }
 548
 549   if (!ac_done && address >= AC_BASE && address < AC_BASE + AC_SIZE) {
 550     autoconfig_write_memory_8(address - AC_BASE, value);
 551     return;
 552   }
 553
 554   if (gayle_emulation_enabled) {
 555     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 556       writeGayleB(address, value);
 557       return;
 558     }
 559   }
 560
 561 #if A314_ENABLED
 562   if (a314_base_configured && address >= a314_base && address < a314_base + A314_COM_AREA_SIZE) {
 563     a314_write_memory_8(address - a314_base, value);
 564     return;
 565   }
 566 #endif
 567
 568   /*
 569   if (address == 0xbfe001) {
 570     ovl = (value & (1 << 0));
 571     printf("OVL:%x\n", ovl);
 572   }
 573 */
 574   //  if (address < 0xffffff) {
 575   address &= 0xFFFFFF;
 576   write8((uint32_t)address, value);
 577   return;
 578   //  }
 579
 580   //  return;
 581 }
 582
 583 void m68k_write_memory_16(unsigned int address, unsigned int value) {
 584   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 585     *(uint16_t *)&fast_ram_array[address - fast_base] = htobe16(value);
 586     return;
 587   }
 588
 589   if (gayle_emulation_enabled) {
 590     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 591       writeGayle(address, value);
 592       return;
 593     }
 594   }
 595
 596 #if A314_ENABLED
 597   if (a314_base_configured && address >= a314_base && address < a314_base + A314_COM_AREA_SIZE) {
 598     a314_write_memory_16(address - a314_base, value);
 599     return;
 600   }
 601 #endif
 602
 603   //  if (address < 0xffffff) {
 604   address &= 0xFFFFFF;
 605   write16((uint32_t)address, value);
 606   return;
 607   //  }
 608   //  return;
 609 }
 610
 611 void m68k_write_memory_32(unsigned int address, unsigned int value) {
 612   if (fast_base_configured && address >= fast_base && address < fast_base + FAST_SIZE) {
 613     *(uint32_t *)&fast_ram_array[address - fast_base] = htobe32(value);
 614     return;
 615   }
 616
 617   if (gayle_emulation_enabled) {
 618     if (address >= GAYLEBASE && address < GAYLEBASE + GAYLESIZE) {
 619       writeGayleL(address, value);
 620     }
 621   }
 622
 623 #if A314_ENABLED
 624   if (a314_base_configured && address >= a314_base && address < a314_base + A314_COM_AREA_SIZE) {
 625     a314_write_memory_32(address - a314_base, value);
 626     return;
 627   }
 628 #endif
 629
 630   //  if (address < 0xffffff) {
 631   address &= 0xFFFFFF;
 632   write16(address, value >> 16);
 633   write16(address + 2, value);
 634   return;
 635   //  }
 636
 637   //  return;
 638 }
 639
 640 void write16(uint32_t address, uint32_t data) {
 641   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 642   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 643   uint32_t addr_l_s = (address >> 16) << 8;
 644   uint32_t addr_l_r = (~address >> 16) << 8;
 645   uint32_t data_s = (data & 0x0000ffff) << 8;
 646   uint32_t data_r = (~data & 0x0000ffff) << 8;
 647
 648   //      asm volatile ("dmb" ::: "memory");
 649   W16
 650       *(gpio) = gpfsel0_o;
 651   *(gpio + 1) = gpfsel1_o;
 652   *(gpio + 2) = gpfsel2_o;
 653
 654   *(gpio + 7) = addr_h_s;
 655   *(gpio + 10) = addr_h_r;
 656   GPIO_CLR = 1 << 7;
 657   GPIO_SET = 1 << 7;
 658
 659   *(gpio + 7) = addr_l_s;
 660   *(gpio + 10) = addr_l_r;
 661   GPIO_CLR = 1 << 7;
 662   GPIO_SET = 1 << 7;
 663
 664   // write phase
 665   *(gpio + 7) = data_s;
 666   *(gpio + 10) = data_r;
 667   GPIO_CLR = 1 << 7;
 668   GPIO_SET = 1 << 7;
 669
 670   *(gpio) = gpfsel0;
 671   *(gpio + 1) = gpfsel1;
 672   *(gpio + 2) = gpfsel2;
 673   while ((GET_GPIO(0)))
 674     ;
 675   //     asm volatile ("dmb" ::: "memory");
 676 }
 677
 678 void write8(uint32_t address, uint32_t data) {
 679   if ((address & 1) == 0)
 680     data = data + (data << 8);  // EVEN, A0=0,UDS
 681   else
 682     data = data & 0xff;  // ODD , A0=1,LDS
 683   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 684   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 685   uint32_t addr_l_s = (address >> 16) << 8;
 686   uint32_t addr_l_r = (~address >> 16) << 8;
 687   uint32_t data_s = (data & 0x0000ffff) << 8;
 688   uint32_t data_r = (~data & 0x0000ffff) << 8;
 689
 690   //   asm volatile ("dmb" ::: "memory");
 691   W8
 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   // write phase
 707   *(gpio + 7) = data_s;
 708   *(gpio + 10) = data_r;
 709   GPIO_CLR = 1 << 7;
 710   GPIO_SET = 1 << 7;
 711
 712   *(gpio) = gpfsel0;
 713   *(gpio + 1) = gpfsel1;
 714   *(gpio + 2) = gpfsel2;
 715   while ((GET_GPIO(0)))
 716     ;
 717   //   asm volatile ("dmb" ::: "memory");
 718 }
 719
 720 uint32_t read16(uint32_t address) {
 721   volatile 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   R16
 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   GPIO_CLR = 1 << 6;
 748   while (!(GET_GPIO(0)))
 749     ;
 750   GPIO_CLR = 1 << 6;
 751   val = *(gpio + 13);
 752   GPIO_SET = 1 << 6;
 753   //    asm volatile ("dmb" ::: "memory");
 754   return (val >> 8) & 0xffff;
 755 }
 756
 757 uint32_t read8(uint32_t address) {
 758   int val;
 759   uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 760   uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 761   uint32_t addr_l_s = (address >> 16) << 8;
 762   uint32_t addr_l_r = (~address >> 16) << 8;
 763
 764   //    asm volatile ("dmb" ::: "memory");
 765   R8
 766       *(gpio) = gpfsel0_o;
 767   *(gpio + 1) = gpfsel1_o;
 768   *(gpio + 2) = gpfsel2_o;
 769
 770   *(gpio + 7) = addr_h_s;
 771   *(gpio + 10) = addr_h_r;
 772   GPIO_CLR = 1 << 7;
 773   GPIO_SET = 1 << 7;
 774
 775   *(gpio + 7) = addr_l_s;
 776   *(gpio + 10) = addr_l_r;
 777   GPIO_CLR = 1 << 7;
 778   GPIO_SET = 1 << 7;
 779
 780   // read phase
 781   *(gpio) = gpfsel0;
 782   *(gpio + 1) = gpfsel1;
 783   *(gpio + 2) = gpfsel2;
 784
 785   GPIO_CLR = 1 << 6;
 786   while (!(GET_GPIO(0)))
 787     ;
 788   GPIO_CLR = 1 << 6;
 789   val = *(gpio + 13);
 790   GPIO_SET = 1 << 6;
 791   //    asm volatile ("dmb" ::: "memory");
 792
 793   val = (val >> 8) & 0xffff;
 794   if ((address & 1) == 0)
 795     return (val >> 8) & 0xff;  // EVEN, A0=0,UDS
 796   else
 797     return val & 0xff;  // ODD , A0=1,LDS
 798 }
 799
 800 /******************************************************/
 801
 802 void write_reg(unsigned int value) {
 803   STATUSREGADDR
 804   *(gpio) = gpfsel0_o;
 805   *(gpio + 1) = gpfsel1_o;
 806   *(gpio + 2) = gpfsel2_o;
 807   *(gpio + 7) = (value & 0xffff) << 8;
 808   *(gpio + 10) = (~value & 0xffff) << 8;
 809   GPIO_CLR = 1 << 7;
 810   GPIO_CLR = 1 << 7;  // delay
 811   GPIO_SET = 1 << 7;
 812   GPIO_SET = 1 << 7;
 813   // Bus HIGH-Z
 814   *(gpio) = gpfsel0;
 815   *(gpio + 1) = gpfsel1;
 816   *(gpio + 2) = gpfsel2;
 817 }
 818
 819 uint16_t read_reg(void) {
 820   uint32_t val;
 821   STATUSREGADDR
 822   // Bus HIGH-Z
 823   *(gpio) = gpfsel0;
 824   *(gpio + 1) = gpfsel1;
 825   *(gpio + 2) = gpfsel2;
 826   GPIO_CLR = 1 << 6;
 827   GPIO_CLR = 1 << 6;  // delay
 828   GPIO_CLR = 1 << 6;
 829   GPIO_CLR = 1 << 6;
 830   val = *(gpio + 13);
 831   GPIO_SET = 1 << 6;
 832   return (uint16_t)(val >> 8);
 833 }
 834
 835 //
 836 // Set up a memory regions to access GPIO
 837 //
 838 void setup_io() {
 839   /* open /dev/mem */
 840   if ((mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
 841     printf("can't open /dev/mem \n");
 842     exit(-1);
 843   }
 844
 845   /* mmap GPIO */
 846   gpio_map = mmap(
 847       NULL,                    // Any adddress in our space will do
 848       BCM2708_PERI_SIZE,       // Map length
 849       PROT_READ | PROT_WRITE,  // Enable reading & writting to mapped memory
 850       MAP_SHARED,              // Shared with other processes
 851       mem_fd,                  // File to map
 852       BCM2708_PERI_BASE        // Offset to GPIO peripheral
 853   );
 854
 855   close(mem_fd);  // No need to keep mem_fd open after mmap
 856
 857   if (gpio_map == MAP_FAILED) {
 858     printf("gpio mmap error %d\n", (int)gpio_map);  // errno also set!
 859     exit(-1);
 860   }
 861
 862   gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR / 4;
 863   gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR / 4;
 864
 865 }  // setup_io