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