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 (maprom == 1){
 363          if(address>KICKBASE && address<KICKBASE + KICKSIZE){
 364           return g_kick[address-KICKBASE];
 365           }
 366         }
 367
 368         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 369         return readGayleB(address);
 370         }
 371
 372         if(address>FASTBASE && address<FASTBASE + FASTSIZE){
 373         return g_ram[address- FASTBASE];
 374         }
 375
 376         /*
 377             if (ovl == 1 && address<KICKSIZE){
 378                return g_kick[address];}
 379                if (ovl == 0 && (address>KICKBASE && address<KICKBASE + KICKSIZE)){
 380                return g_kick[address-KICKBASE];}
 381         }
 382         */
 383         if (address < 0xffffff){
 384          return read8((uint32_t)address);
 385         }
 386
 387         return 0;
 388 }
 389
 390 unsigned int  m68k_read_memory_16(unsigned int address){
 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>FASTBASE && address<FASTBASE + FASTSIZE){
 406         uint16_t value = *(uint16_t*)&g_ram[address- FASTBASE];
 407         value = (value << 8) | (value >> 8);
 408         return value;
 409         }
 410
 411 /*
 412         if (maprom == 1){
 413             if (ovl == 1 && address<KICKSIZE ){
 414                uint16_t value = *(uint16_t*)&g_kick[address];
 415                return (value << 8) | (value >> 8);}
 416               if (ovl == 0 && (address>KICKBASE && address<KICKBASE + KICKSIZE)){
 417                //printf("kread16 addr: %x\n",address);
 418                uint16_t value = *(uint16_t*)&g_kick[address-KICKBASE];
 419                return (value << 8) | (value >> 8);}
 420         }
 421 */
 422         if (address < 0xffffff){
 423         return (unsigned int)read16((uint32_t)address);
 424         }
 425
 426         return 0;
 427 }
 428
 429 unsigned int  m68k_read_memory_32(unsigned int address){
 430
 431         if (maprom == 1){
 432           if(address>KICKBASE && address<KICKBASE + KICKSIZE){
 433             uint32_t value = *(uint32_t*)&g_kick[address-KICKBASE];
 434             value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 435             return value << 16 | value >> 16;
 436             }
 437         }
 438
 439         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 440          return readGayleL(address);
 441         }
 442
 443         if(address>FASTBASE && address<FASTBASE + FASTSIZE){
 444         uint32_t value = *(uint32_t*)&g_ram[address- FASTBASE];
 445         value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 446         return value << 16 | value >> 16;
 447         }
 448
 449 /*
 450         if (maprom == 1){
 451             if (ovl == 1 && address<KICKSIZE){
 452               uint32_t value = *(uint32_t*)&g_kick[address];
 453               value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 454               return value << 16 | value >> 16;}
 455             if (ovl == 0 && (address>KICKBASE && address<KICKBASE + KICKSIZE)){
 456                //printf("kread32/n");
 457                uint32_t value = *(uint32_t*)&g_kick[address-KICKBASE];
 458                value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 459                return value << 16 | value >> 16;}
 460         }
 461 */
 462         if (address < 0xffffff){
 463             uint16_t a = read16(address);
 464             uint16_t b = read16(address+2);
 465             return (a << 16) | b;
 466            }
 467
 468         return 0;
 469 }
 470
 471 void m68k_write_memory_8(unsigned int address, unsigned int value){
 472
 473
 474         if (address == 0xbfe001){
 475         ovl = (value & (1<<0));
 476         printf("OVL:%x\n", ovl );
 477         }
 478
 479
 480         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 481         writeGayleB(address, value);
 482         return;
 483         }
 484
 485
 486       if(address>FASTBASE && address<FASTBASE + FASTSIZE){
 487         g_ram[address- FASTBASE] = value;
 488         return;
 489         }
 490
 491         if (address < 0xffffff){
 492          write8((uint32_t)address,value);
 493          return;
 494         }
 495
 496         return;
 497 }
 498
 499 void m68k_write_memory_16(unsigned int address, unsigned int value){
 500 //        if (address==0xdff030) printf("%c", value);
 501
 502         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 503         writeGayle(address,value);
 504         return;
 505         }
 506
 507         if (address == 0xbfe001)
 508         printf("16CIA Output:%x\n", value );
 509
 510
 511       if(address>FASTBASE && address<FASTBASE + FASTSIZE ){
 512         uint16_t* dest = (uint16_t*)&g_ram[address- FASTBASE];
 513         value = (value << 8) | (value >> 8);
 514         *dest = value;
 515         return;
 516         }
 517
 518         if (address < 0xffffff){
 519          write16((uint32_t)address,value);
 520          return;
 521         }
 522       return;
 523 }
 524
 525 void m68k_write_memory_32(unsigned int address, unsigned int value){
 526
 527
 528         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 529         writeGayleL(address, value);
 530         }
 531
 532         if(address>FASTBASE && address<FASTBASE + FASTSIZE ){
 533            uint32_t* dest = (uint32_t*)&g_ram[address- FASTBASE];
 534            value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 535            value = value << 16 | value >> 16;
 536            *dest = value;
 537         return;
 538         }
 539
 540        if (address < 0xffffff){
 541         write16(address , value >> 16);
 542         write16(address+2 , value );
 543         return;
 544        }
 545
 546       return;
 547 }
 548
 549 /*
 550 void write32(uint32_t address, uint32_t data){
 551         write16(address+2 , data);
 552         write16(address , data >>16 );
 553 }
 554
 555 uint32_t read32(uint32_t address){
 556         uint16_t a = read16(address+2);
 557         uint16_t b = read16(address);
 558         return (a>>16)|b;
 559 }
 560 */
 561
 562 void write16(uint32_t address, uint32_t data)
 563 {
 564  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 565  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 566  uint32_t addr_l_s = (address >> 16) << 8;
 567  uint32_t addr_l_r = (~address >> 16) << 8;
 568  uint32_t data_s = (data & 0x0000ffff) << 8;
 569  uint32_t data_r = (~data & 0x0000ffff) << 8;
 570
 571   //      asm volatile ("dmb" ::: "memory");
 572         W16
 573         *(gpio) = gpfsel0_o;
 574         *(gpio + 1) = gpfsel1_o;
 575         *(gpio + 2) = gpfsel2_o;
 576
 577         *(gpio + 7) = addr_h_s;
 578         *(gpio + 10) = addr_h_r;
 579         while ((GET_GPIO(0)));
 580         GPIO_CLR = 1 << 7;
 581         GPIO_SET = 1 << 7;
 582
 583         *(gpio + 7) = addr_l_s;
 584         *(gpio + 10) = addr_l_r;
 585         GPIO_CLR = 1 << 7;
 586         GPIO_SET = 1 << 7;
 587
 588         //write phase
 589         *(gpio + 7) = data_s;
 590         *(gpio + 10) = data_r;
 591         GPIO_CLR = 1 << 7;
 592         GPIO_SET = 1 << 7;
 593
 594         *(gpio) = gpfsel0;
 595         *(gpio + 1) = gpfsel1;
 596         *(gpio + 2) = gpfsel2;
 597    //     while ((GET_GPIO(0)));
 598    //     asm volatile ("dmb" ::: "memory");
 599 }
 600
 601
 602 void write8(uint32_t address, uint32_t data)
 603 {
 604
 605         if ((address & 1) == 0)
 606             data = data + (data << 8); //EVEN, A0=0,UDS
 607         else data = data & 0xff ; //ODD , A0=1,LDS
 608  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 609  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 610  uint32_t addr_l_s = (address >> 16) << 8;
 611  uint32_t addr_l_r = (~address >> 16) << 8;
 612  uint32_t data_s = (data & 0x0000ffff) << 8;
 613  uint32_t data_r = (~data & 0x0000ffff) << 8;
 614
 615
 616      //   asm volatile ("dmb" ::: "memory");
 617         W8
 618         *(gpio) = gpfsel0_o;
 619         *(gpio + 1) = gpfsel1_o;
 620         *(gpio + 2) = gpfsel2_o;
 621
 622         *(gpio + 7) = addr_h_s;
 623         *(gpio + 10) = addr_h_r;
 624         //while ((GET_GPIO(0)));
 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         //write phase
 634         *(gpio + 7) = data_s;
 635         *(gpio + 10) = data_r;
 636         GPIO_CLR = 1 << 7;
 637         GPIO_SET = 1 << 7;
 638
 639         *(gpio) = gpfsel0;
 640         *(gpio + 1) = gpfsel1;
 641         *(gpio + 2) = gpfsel2;
 642         while ((GET_GPIO(0)));
 643      //   asm volatile ("dmb" ::: "memory");
 644      // GPIO_SET = 1 << 7;
 645 }
 646
 647
 648 uint32_t read16(uint32_t address)
 649 {
 650         volatile int val;
 651  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 652  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 653  uint32_t addr_l_s = (address >> 16) << 8;
 654  uint32_t addr_l_r = (~address >> 16) << 8;
 655
 656      //   asm volatile ("dmb" ::: "memory");
 657         R16
 658
 659         *(gpio) = gpfsel0_o;
 660         *(gpio + 1) = gpfsel1_o;
 661         *(gpio + 2) = gpfsel2_o;
 662
 663         *(gpio + 7) = addr_h_s;
 664         *(gpio + 10) = addr_h_r;
 665         while ((GET_GPIO(0)));
 666         GPIO_CLR = 1 << 7;
 667         GPIO_SET = 1 << 7;
 668
 669         *(gpio + 7) = addr_l_s;
 670         *(gpio + 10) = addr_l_r;
 671         GPIO_CLR = 1 << 7;
 672         GPIO_SET = 1 << 7;
 673
 674
 675         //read phase
 676
 677         *(gpio) = gpfsel0;
 678         *(gpio + 1) = gpfsel1;
 679         *(gpio + 2) = gpfsel2;
 680
 681         GPIO_CLR = 1 << 6;
 682         while (!(GET_GPIO(0)));
 683         GPIO_CLR = 1 << 6;
 684 //        asm volatile ("nop" ::);
 685 //      asm volatile ("nop" ::);
 686 //      asm volatile ("nop" ::);
 687         val = *(gpio + 13);
 688         GPIO_SET = 1 << 6;
 689     //    asm volatile ("dmb" ::: "memory");
 690         return (val >>8)&0xffff;
 691 }
 692
 693
 694 uint32_t read8(uint32_t address)
 695 {
 696         int val;
 697  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 698  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 699  uint32_t addr_l_s = (address >> 16) << 8;
 700  uint32_t addr_l_r = (~address >> 16) << 8;
 701
 702     //    asm volatile ("dmb" ::: "memory");
 703         R8
 704         *(gpio) = gpfsel0_o;
 705         *(gpio + 1) = gpfsel1_o;
 706         *(gpio + 2) = gpfsel2_o;
 707
 708         *(gpio + 7) = addr_h_s;
 709         *(gpio + 10) = addr_h_r;
 710         while ((GET_GPIO(0)));
 711         GPIO_CLR = 1 << 7;
 712         GPIO_SET = 1 << 7;
 713
 714         *(gpio + 7) = addr_l_s;
 715         *(gpio + 10) = addr_l_r;
 716         GPIO_CLR = 1 << 7;
 717         GPIO_SET = 1 << 7;
 718
 719         //read phase
 720
 721         *(gpio) = gpfsel0;
 722         *(gpio + 1) = gpfsel1;
 723         *(gpio + 2) = gpfsel2;
 724
 725         GPIO_CLR = 1 << 6;
 726         while (!(GET_GPIO(0)));
 727         GPIO_CLR = 1 << 6;
 728 //      asm volatile ("nop" ::);
 729 //      asm volatile ("nop" ::);
 730 //      asm volatile ("nop" ::);
 731         val = *(gpio + 13);
 732         GPIO_SET = 1 << 6;
 733     //    asm volatile ("dmb" ::: "memory");
 734
 735         val = (val >>8)&0xffff;
 736         if ((address & 1) == 0)
 737             val = (val >> 8) & 0xff ; //EVEN, A0=0,UDS
 738         else
 739             val = val & 0xff ; //ODD , A0=1,LDS
 740         return val;
 741 }
 742
 743
 744
 745 /******************************************************/
 746
 747 void write_reg(unsigned int value)
 748 {
 749         asm volatile ("dmb" ::: "memory");
 750         STATUSREGADDR
 751         asm volatile ("nop" ::);
 752         asm volatile ("nop" ::);
 753         asm volatile ("nop" ::);
 754         //Write Status register
 755         GPIO_CLR = 1 << SA0;
 756         GPIO_CLR = 1 << SA1;
 757         GPIO_SET = 1 << SA2;
 758
 759         *(gpio) = gpfsel0_o;
 760         *(gpio + 1) = gpfsel1_o;
 761         *(gpio + 2) = gpfsel2_o;
 762         *(gpio + 7) = (value & 0xffff) << 8;
 763         *(gpio + 10) = (~value & 0xffff) << 8;
 764         GPIO_CLR = 1 << 7;
 765         GPIO_CLR = 1 << 7; //delay
 766         GPIO_SET = 1 << 7;
 767         GPIO_SET = 1 << 7;
 768         //Bus HIGH-Z
 769         *(gpio) = gpfsel0;
 770         *(gpio + 1) = gpfsel1;
 771         *(gpio + 2) = gpfsel2;
 772         asm volatile ("dmb" ::: "memory");
 773 }
 774
 775
 776 uint16_t read_reg(void)
 777 {
 778         uint32_t val;
 779
 780         asm volatile ("dmb" ::: "memory");
 781         STATUSREGADDR
 782         asm volatile ("nop" ::);
 783         asm volatile ("nop" ::);
 784         asm volatile ("nop" ::);
 785         //Bus HIGH-Z
 786         *(gpio) = gpfsel0;
 787         *(gpio + 1) = gpfsel1;
 788         *(gpio + 2) = gpfsel2;
 789
 790         GPIO_CLR = 1 << 6;
 791         GPIO_CLR = 1 << 6;      //delay
 792         GPIO_CLR = 1 << 6;
 793         GPIO_CLR = 1 << 6;
 794         asm volatile ("nop" ::);
 795         asm volatile ("nop" ::);
 796         asm volatile ("nop" ::);
 797         val = *(gpio + 13);
 798         GPIO_SET = 1 << 6;
 799         asm volatile ("dmb" ::: "memory");
 800
 801         return (uint16_t)(val >> 8);
 802 }
 803
 804
 805 //
 806 // Set up a memory regions to access GPIO
 807 //
 808 void setup_io()
 809 {
 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
 837 } // setup_io
 838