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