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

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