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