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 0xFFFFFF
  56 #define FASTSIZE 0xFFFFFFF
  57
  58
  59 #define GAYLEBASE 0xD80000 //D7FFFF
  60 #define GAYLESIZE 0x6FFFF
  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
 170
 171 const struct sched_param priority = {99};
 172
 173     sched_setscheduler(0, SCHED_RR , &priority);
 174     printf("YES locked in memory\n");
 175     mlockall(MCL_CURRENT); // lock in memory to keep us from paging out
 176
 177
 178  InitGayle();
 179
 180 /*
 181    int fd;
 182    ide0 = ide_allocate("cf");
 183    fd = open("hd0.img", O_RDWR);
 184    if (fd == -1){
 185         printf("HDD Image hd0.image failed open\n");
 186    }else{
 187         ide_attach(ide0, 0, fd);
 188         ide_reset_begin(ide0);
 189         printf("HDD Image hd0.image attached\n");
 190    }
 191 */
 192   signal(SIGINT, sigint_handler);
 193   setup_io();
 194
 195   //Enable 200MHz CLK output on GPIO4, adjust divider and pll source depending on pi model
 196   printf("Enable GPCLK0 on GPIO4\n");
 197
 198         *(gpclk+ (CLK_GP0_CTL/4)) =  CLK_PASSWD | (1 << 5);
 199         usleep(10);
 200         while ( (*(gpclk+(CLK_GP0_CTL/4))) & (1 << 7));
 201         usleep(100);
 202         *(gpclk+(CLK_GP0_DIV/4)) =  CLK_PASSWD | (6 << 12); //divider , 6=200MHz on pi3
 203         usleep(10);
 204         *(gpclk+(CLK_GP0_CTL/4)) =   CLK_PASSWD | 5 | (1 << 4); //pll? 6=plld, 5=pllc
 205         usleep(10);
 206         while (((*(gpclk+(CLK_GP0_CTL/4))) & (1 << 7))== 0);
 207         usleep(100);
 208
 209     SET_GPIO_ALT(4,0); //gpclk0
 210
 211    //set SA to output
 212     INP_GPIO(2);
 213     OUT_GPIO(2);
 214     INP_GPIO(3);
 215     OUT_GPIO(3);
 216     INP_GPIO(5);
 217     OUT_GPIO(5);
 218
 219   //set gpio0 (aux0) and gpio1 (aux1) to input
 220   INP_GPIO(0);
 221   INP_GPIO(1);
 222
 223   // Set GPIO pins 6,7 and 8-23 to output
 224   for (g=6; g<=23; g++)
 225   {
 226     INP_GPIO(g);
 227     OUT_GPIO(g);
 228   }
 229      printf ("Precalculate GPIO8-23 aus Output\n");
 230      gpfsel0_o =*(gpio); //store gpio ddr
 231      printf ("gpfsel0: %#x\n", gpfsel0_o);
 232      gpfsel1_o =*(gpio+1); //store gpio ddr
 233      printf ("gpfsel1: %#x\n", gpfsel1_o);
 234      gpfsel2_o =*(gpio+2); //store gpio ddr
 235      printf ("gpfsel2: %#x\n", gpfsel2_o);
 236
 237   // Set GPIO pins 8-23 to input
 238   for (g=8; g<=23; g++)
 239   {
 240     INP_GPIO(g);
 241   }
 242      printf ("Precalculate GPIO8-23 as Input\n");
 243      gpfsel0 =*(gpio); //store gpio ddr
 244      printf ("gpfsel0: %#x\n", gpfsel0);
 245      gpfsel1 =*(gpio+1); //store gpio ddr
 246      printf ("gpfsel1: %#x\n", gpfsel1);
 247      gpfsel2 =*(gpio+2); //store gpio ddr
 248      printf ("gpfsel2: %#x\n", gpfsel2);
 249
 250  GPIO_CLR = 1<<2;
 251  GPIO_CLR = 1<<3;
 252  GPIO_SET = 1<<5;
 253
 254  GPIO_SET = 1<<6;
 255  GPIO_SET = 1<<7;
 256
 257  //reset cpld statemachine first
 258
 259  write_reg(0x01);
 260  usleep(100);
 261  usleep(1500);
 262  write_reg(0x00);
 263  usleep(100);
 264
 265  maprom = 0;
 266  FILE * fp;
 267  fp = fopen("kick.rom", "rb");
 268  if (!fp)
 269  {
 270  printf("kick.rom cannot be opened\n");
 271  } else {
 272  printf("kick.rom found, using that instead of motherboard rom\n");
 273  while (1)
 274   {
 275   unsigned int reads = fread(&g_kick, sizeof(g_kick), 1, fp);
 276   if (reads == 0){
 277      printf("failed loading kick.rom\n");
 278      }else{
 279      printf("loaded kick.rom\n");
 280      maprom = 1;
 281      }
 282   break;
 283   }
 284  }
 285
 286  ovl=1;
 287  m68k_write_memory_8(0xbfe201,0x0001); //AMIGA OVL
 288  m68k_write_memory_8(0xbfe001,0x0001); //AMIGA OVL high (ROM@0x0)
 289
 290
 291  usleep(1500);
 292
 293         m68k_init();
 294         m68k_set_cpu_type(M68K_CPU_TYPE_68EC030);
 295         m68k_pulse_reset();
 296         srdata2_old = read_reg();
 297         printf("STATUS: %d\n", srdata2_old);
 298         toggle = 0;
 299
 300 /*
 301     pthread_t id;
 302     int err;
 303
 304         //err = pthread_create(&id, NULL, &iplThread, NULL);
 305         if (err != 0)
 306             printf("\ncan't create IPL thread :[%s]", strerror(err));
 307         else
 308             printf("\n IPL Thread created successfully\n");
 309 */
 310         m68k_pulse_reset();
 311         while(42) {
 312
 313                 m68k_execute(150);
 314                 //usleep(1);
 315
 316                 //printf("IRQ:0x%06x\n",CheckIrq());
 317 /*
 318                 if (CheckIrq() == 1)
 319                    m68k_set_irq(2);
 320                 else
 321                    m68k_set_irq(0);
 322 */
 323
 324                 if (GET_GPIO(1) == 0 || CheckIrq() == 1){
 325                   srdata = read_reg();
 326                 //  if (CheckIrq() == 1) srdata |= (1 << 14);
 327                   if (srdata != srdata2_old){
 328                         srdata2 = ((srdata >> 13)&0xff);
 329                         //printf("STATUS: %d\n", srdata2);
 330                         srdata2_old = srdata;
 331                         m68k_set_irq(srdata2);
 332                         toggle = 1;
 333                         }
 334                 } else {
 335
 336                         if (toggle != 0){
 337                         srdata = read_reg();
 338                         srdata2 = ((srdata >> 13)&0xff);
 339                         srdata2_old = srdata;
 340                         m68k_set_irq(srdata2);
 341                          //printf("STATUS: 0\n");
 342                         toggle = 0;
 343                         }
 344                 }
 345
 346         }
 347
 348         return 0;
 349 }
 350
 351
 352
 353 void cpu_pulse_reset(void){
 354
 355         usleep(10000);
 356 }
 357
 358
 359
 360
 361 int cpu_irq_ack(int level)
 362 {
 363     printf("cpu irq ack\n");
 364     return level;
 365 }
 366
 367
 368
 369 unsigned int  m68k_read_memory_8(unsigned int address){
 370
 371         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 372         return readGayleB(address);
 373         }
 374
 375         if(address>FASTBASE){
 376         return g_ram[address- FASTBASE];
 377         }
 378
 379         if (maprom == 1){
 380             if (ovl == 1 && address<0x07FFFF ){
 381                return g_kick[address];}
 382               if (ovl == 0 && (address>0xF80000-1 && address<0xFFFFFF)){
 383                return g_kick[address-0xF80000];}
 384         }
 385
 386         if (address < 0xffffff){
 387          return read8((uint32_t)address);
 388         }
 389
 390         return 0;
 391 }
 392
 393 unsigned int  m68k_read_memory_16(unsigned int address){
 394
 395         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 396         return readGayle(address);
 397         }
 398
 399         if(address>FASTBASE){
 400         uint16_t value = *(uint16_t*)&g_ram[address- FASTBASE];
 401         value = (value << 8) | (value >> 8);
 402         return value;
 403         }
 404
 405         if (maprom == 1){
 406             if (ovl == 1 && address<0x07FFFF ){
 407                uint16_t value = *(uint16_t*)&g_kick[address];
 408                return (value << 8) | (value >> 8);}
 409               if (ovl == 0 && (address>0xF80000-1 && address<0xFFFFFF)){
 410                //printf("kread16/n");
 411                uint16_t value = *(uint16_t*)&g_kick[address-0xF80000];
 412                return (value << 8) | (value >> 8);}
 413         }
 414
 415         if (address < 0xffffff){
 416         return (unsigned int)read16((uint32_t)address);
 417         }
 418
 419         return 0;
 420 }
 421
 422 unsigned int  m68k_read_memory_32(unsigned int address){
 423
 424         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 425          return readGayleL(address);
 426         }
 427
 428         if(address>FASTBASE){
 429         uint32_t value = *(uint32_t*)&g_ram[address- FASTBASE];
 430         value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 431         return value << 16 | value >> 16;
 432         }
 433
 434         if (maprom == 1){
 435             if (ovl == 1 && address<0x07FFFF){
 436               uint32_t value = *(uint32_t*)&g_kick[address];
 437               value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 438               return value << 16 | value >> 16;}
 439
 440               if (ovl == 0 && (address>0xF80000-1 && address<0xFFFFFF)){
 441                //printf("kread32/n");
 442                uint32_t value = *(uint32_t*)&g_kick[address-0xF80000];
 443                value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 444                return value << 16 | value >> 16;}
 445         }
 446
 447         if (address < 0xffffff){
 448             uint16_t a = read16(address);
 449             uint16_t b = read16(address+2);
 450             return (a << 16) | b;
 451            }
 452
 453         return 0;
 454 }
 455
 456 void m68k_write_memory_8(unsigned int address, unsigned int value){
 457
 458
 459         if (address == 0xbfe001){
 460         ovl = (value & (1<<0));
 461         //printf("OVL:%x\n", ovl );
 462         }
 463
 464
 465         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 466         writeGayleB(address, value);
 467         return;
 468         }
 469
 470
 471       if(address>FASTBASE){
 472         g_ram[address- FASTBASE] = value;
 473         return;
 474         }
 475
 476         if (address < 0xffffff){
 477          write8((uint32_t)address,value);
 478          return;
 479         }
 480
 481         return;
 482 }
 483
 484 void m68k_write_memory_16(unsigned int address, unsigned int value){
 485 //        if (address==0xdff030) printf("%c", value);
 486
 487         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 488         writeGayle(address,value);
 489         return;
 490         }
 491
 492         if (address == 0xbfe001)
 493         printf("16CIA Output:%x\n", value );
 494
 495
 496       if(address>FASTBASE){
 497         uint16_t* dest = (uint16_t*)&g_ram[address- FASTBASE];
 498         value = (value << 8) | (value >> 8);
 499         *dest = value;
 500         return;
 501         }
 502
 503         if (address < 0xffffff){
 504          write16((uint32_t)address,value);
 505          return;
 506         }
 507       return;
 508 }
 509
 510 void m68k_write_memory_32(unsigned int address, unsigned int value){
 511
 512
 513         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 514         writeGayleL(address, value);
 515         }
 516
 517         if(address>FASTBASE){
 518            uint32_t* dest = (uint32_t*)&g_ram[address- FASTBASE];
 519            value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 520            value = value << 16 | value >> 16;
 521            *dest = value;
 522         return;
 523         }
 524
 525        if (address < 0xffffff){
 526         write16(address , value >> 16);
 527         write16(address+2 , value );
 528         return;
 529        }
 530
 531       return;
 532 }
 533
 534 /*
 535 void write32(uint32_t address, uint32_t data){
 536         write16(address+2 , data);
 537         write16(address , data >>16 );
 538 }
 539
 540 uint32_t read32(uint32_t address){
 541         uint16_t a = read16(address+2);
 542         uint16_t b = read16(address);
 543         return (a>>16)|b;
 544 }
 545 */
 546
 547 void write16(uint32_t address, uint32_t data)
 548 {
 549  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 550  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 551  uint32_t addr_l_s = (address >> 16) << 8;
 552  uint32_t addr_l_r = (~address >> 16) << 8;
 553  uint32_t data_s = (data & 0x0000ffff) << 8;
 554  uint32_t data_r = (~data & 0x0000ffff) << 8;
 555
 556   //      asm volatile ("dmb" ::: "memory");
 557         W16
 558         *(gpio) = gpfsel0_o;
 559         *(gpio + 1) = gpfsel1_o;
 560         *(gpio + 2) = gpfsel2_o;
 561
 562         *(gpio + 7) = addr_h_s;
 563         *(gpio + 10) = addr_h_r;
 564         GPIO_CLR = 1 << 7;
 565         GPIO_SET = 1 << 7;
 566
 567         *(gpio + 7) = addr_l_s;
 568         *(gpio + 10) = addr_l_r;
 569         GPIO_CLR = 1 << 7;
 570         GPIO_SET = 1 << 7;
 571
 572         //write phase
 573         *(gpio + 7) = data_s;
 574         *(gpio + 10) = data_r;
 575         GPIO_CLR = 1 << 7;
 576         GPIO_SET = 1 << 7;
 577
 578         *(gpio) = gpfsel0;
 579         *(gpio + 1) = gpfsel1;
 580         *(gpio + 2) = gpfsel2;
 581         while ((GET_GPIO(0)));
 582    //     asm volatile ("dmb" ::: "memory");
 583 }
 584
 585
 586 void write8(uint32_t address, uint32_t data)
 587 {
 588
 589         if ((address & 1) == 0)
 590             data = data + (data << 8); //EVEN, A0=0,UDS
 591         else data = data & 0xff ; //ODD , A0=1,LDS
 592  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 593  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 594  uint32_t addr_l_s = (address >> 16) << 8;
 595  uint32_t addr_l_r = (~address >> 16) << 8;
 596  uint32_t data_s = (data & 0x0000ffff) << 8;
 597  uint32_t data_r = (~data & 0x0000ffff) << 8;
 598
 599
 600      //   asm volatile ("dmb" ::: "memory");
 601         W8
 602         *(gpio) = gpfsel0_o;
 603         *(gpio + 1) = gpfsel1_o;
 604         *(gpio + 2) = gpfsel2_o;
 605
 606         *(gpio + 7) = addr_h_s;
 607         *(gpio + 10) = addr_h_r;
 608         GPIO_CLR = 1 << 7;
 609         GPIO_SET = 1 << 7;
 610
 611         *(gpio + 7) = addr_l_s;
 612         *(gpio + 10) = addr_l_r;
 613         GPIO_CLR = 1 << 7;
 614         GPIO_SET = 1 << 7;
 615
 616         //write phase
 617         *(gpio + 7) = data_s;
 618         *(gpio + 10) = data_r;
 619         GPIO_CLR = 1 << 7;
 620         GPIO_SET = 1 << 7;
 621
 622         *(gpio) = gpfsel0;
 623         *(gpio + 1) = gpfsel1;
 624         *(gpio + 2) = gpfsel2;
 625         while ((GET_GPIO(0)));
 626      //   asm volatile ("dmb" ::: "memory");
 627         GPIO_SET = 1 << 7;
 628 }
 629
 630
 631 uint32_t read16(uint32_t address)
 632 {
 633         volatile int val;
 634  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 635  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 636  uint32_t addr_l_s = (address >> 16) << 8;
 637  uint32_t addr_l_r = (~address >> 16) << 8;
 638
 639      //   asm volatile ("dmb" ::: "memory");
 640         R16
 641
 642         *(gpio) = gpfsel0_o;
 643         *(gpio + 1) = gpfsel1_o;
 644         *(gpio + 2) = gpfsel2_o;
 645
 646         *(gpio + 7) = addr_h_s;
 647         *(gpio + 10) = addr_h_r;
 648         GPIO_CLR = 1 << 7;
 649         GPIO_SET = 1 << 7;
 650
 651         *(gpio + 7) = addr_l_s;
 652         *(gpio + 10) = addr_l_r;
 653         GPIO_CLR = 1 << 7;
 654         GPIO_SET = 1 << 7;
 655
 656
 657         //read phase
 658
 659         *(gpio) = gpfsel0;
 660         *(gpio + 1) = gpfsel1;
 661         *(gpio + 2) = gpfsel2;
 662
 663         GPIO_CLR = 1 << 6;
 664         while (!(GET_GPIO(0)));
 665         GPIO_CLR = 1 << 6;
 666         asm volatile ("nop" ::);
 667         asm volatile ("nop" ::);
 668         asm volatile ("nop" ::);
 669         val = *(gpio + 13);
 670         GPIO_SET = 1 << 6;
 671     //    asm volatile ("dmb" ::: "memory");
 672         return (val >>8)&0xffff;
 673 }
 674
 675
 676 uint32_t read8(uint32_t address)
 677 {
 678         int val;
 679  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 680  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 681  uint32_t addr_l_s = (address >> 16) << 8;
 682  uint32_t addr_l_r = (~address >> 16) << 8;
 683
 684     //    asm volatile ("dmb" ::: "memory");
 685         R8
 686         *(gpio) = gpfsel0_o;
 687         *(gpio + 1) = gpfsel1_o;
 688         *(gpio + 2) = gpfsel2_o;
 689
 690         *(gpio + 7) = addr_h_s;
 691         *(gpio + 10) = addr_h_r;
 692         GPIO_CLR = 1 << 7;
 693         GPIO_SET = 1 << 7;
 694
 695         *(gpio + 7) = addr_l_s;
 696         *(gpio + 10) = addr_l_r;
 697         GPIO_CLR = 1 << 7;
 698         GPIO_SET = 1 << 7;
 699
 700         //read phase
 701
 702         *(gpio) = gpfsel0;
 703         *(gpio + 1) = gpfsel1;
 704         *(gpio + 2) = gpfsel2;
 705
 706         GPIO_CLR = 1 << 6;
 707         while (!(GET_GPIO(0)));
 708         GPIO_CLR = 1 << 6;
 709         asm volatile ("nop" ::);
 710         asm volatile ("nop" ::);
 711         asm volatile ("nop" ::);
 712         val = *(gpio + 13);
 713         GPIO_SET = 1 << 6;
 714     //    asm volatile ("dmb" ::: "memory");
 715
 716         val = (val >>8)&0xffff;
 717         if ((address & 1) == 0)
 718             val = (val >> 8) & 0xff ; //EVEN, A0=0,UDS
 719         else
 720             val = val & 0xff ; //ODD , A0=1,LDS
 721         return val;
 722 }
 723
 724
 725
 726 /******************************************************/
 727
 728 void write_reg(unsigned int value)
 729 {
 730         asm volatile ("dmb" ::: "memory");
 731         STATUSREGADDR
 732         asm volatile ("nop" ::);
 733         asm volatile ("nop" ::);
 734         asm volatile ("nop" ::);
 735         //Write Status register
 736         GPIO_CLR = 1 << SA0;
 737         GPIO_CLR = 1 << SA1;
 738         GPIO_SET = 1 << SA2;
 739
 740         *(gpio) = gpfsel0_o;
 741         *(gpio + 1) = gpfsel1_o;
 742         *(gpio + 2) = gpfsel2_o;
 743         *(gpio + 7) = (value & 0xffff) << 8;
 744         *(gpio + 10) = (~value & 0xffff) << 8;
 745         GPIO_CLR = 1 << 7;
 746         GPIO_CLR = 1 << 7; //delay
 747         GPIO_SET = 1 << 7;
 748         GPIO_SET = 1 << 7;
 749         //Bus HIGH-Z
 750         *(gpio) = gpfsel0;
 751         *(gpio + 1) = gpfsel1;
 752         *(gpio + 2) = gpfsel2;
 753         asm volatile ("dmb" ::: "memory");
 754 }
 755
 756
 757 uint16_t read_reg(void)
 758 {
 759         uint32_t val;
 760
 761         asm volatile ("dmb" ::: "memory");
 762         STATUSREGADDR
 763         asm volatile ("nop" ::);
 764         asm volatile ("nop" ::);
 765         asm volatile ("nop" ::);
 766         //Bus HIGH-Z
 767         *(gpio) = gpfsel0;
 768         *(gpio + 1) = gpfsel1;
 769         *(gpio + 2) = gpfsel2;
 770
 771         GPIO_CLR = 1 << 6;
 772         GPIO_CLR = 1 << 6;      //delay
 773         GPIO_CLR = 1 << 6;
 774         GPIO_CLR = 1 << 6;
 775         val = *(gpio + 13);
 776         GPIO_SET = 1 << 6;
 777         asm volatile ("dmb" ::: "memory");
 778
 779         return (uint16_t)(val >> 8);
 780 }
 781
 782
 783 //
 784 // Set up a memory regions to access GPIO
 785 //
 786 void setup_io()
 787 {
 788    /* open /dev/mem */
 789    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
 790       printf("can't open /dev/mem \n");
 791       exit(-1);
 792    }
 793
 794    /* mmap GPIO */
 795    gpio_map = mmap(
 796       NULL,             //Any adddress in our space will do
 797       BCM2708_PERI_SIZE,       //Map length
 798       PROT_READ|PROT_WRITE,// Enable reading & writting to mapped memory
 799       MAP_SHARED,       //Shared with other processes
 800       mem_fd,           //File to map
 801       BCM2708_PERI_BASE //Offset to GPIO peripheral
 802    );
 803
 804    close(mem_fd); //No need to keep mem_fd open after mmap
 805
 806    if (gpio_map == MAP_FAILED) {
 807       printf("gpio mmap error %d\n", (int)gpio_map);//errno also set!
 808       exit(-1);
 809    }
 810
 811    gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR/4;
 812    gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR/4;
 813
 814
 815 } // setup_io
 816