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  ovl=1;
 293  m68k_write_memory_8(0xbfe201,0x0001); //AMIGA OVL
 294  m68k_write_memory_8(0xbfe001,0x0001); //AMIGA OVL high (ROM@0x0)
 295
 296
 297  usleep(1500);
 298
 299         m68k_init();
 300         m68k_set_cpu_type(M68K_CPU_TYPE_68030);
 301         m68k_pulse_reset();
 302
 303         if (maprom == 1){
 304         m68k_set_reg(M68K_REG_PC, 0xF80002);
 305         }else{
 306         m68k_set_reg(M68K_REG_PC, 0x0);
 307         }
 308
 309 /*
 310          pthread_t id;
 311          int err;
 312         //err = pthread_create(&id, NULL, &iplThread, NULL);
 313         if (err != 0)
 314             printf("\ncan't create IPL thread :[%s]", strerror(err));
 315         else
 316             printf("\n IPL Thread created successfully\n");
 317 */
 318
 319         m68k_pulse_reset();
 320         while(42) {
 321
 322                 m68k_execute(30000);
 323                 if (GET_GPIO(1) == 0){
 324                  srdata = read_reg();
 325                  m68k_set_irq((srdata >> 13)&0xff);
 326                 } else {
 327                  m68k_set_irq(0);
 328                 };
 329         }
 330
 331         return 0;
 332 }
 333
 334
 335
 336 void cpu_pulse_reset(void){
 337
 338         write_reg(0x02);
 339         usleep(10000);
 340         write_reg(0x00);
 341 }
 342
 343
 344
 345
 346 int cpu_irq_ack(int level)
 347 {
 348     printf("cpu irq ack\n");
 349     return level;
 350 }
 351
 352
 353
 354 unsigned int  m68k_read_memory_8(unsigned int address){
 355
 356
 357         if (maprom == 1){
 358          if(address>KICKBASE && address<KICKBASE + KICKSIZE){
 359           return g_kick[address-KICKBASE];
 360           }
 361         }
 362
 363         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 364         return readGayleB(address);
 365         }
 366
 367         if(address>FASTBASE){
 368         return g_ram[address- FASTBASE];
 369         }
 370
 371         /*
 372             if (ovl == 1 && address<KICKSIZE){
 373                return g_kick[address];}
 374                if (ovl == 0 && (address>KICKBASE && address<KICKBASE + KICKSIZE)){
 375                return g_kick[address-KICKBASE];}
 376         }
 377         */
 378         if (address < 0xffffff){
 379          return read8((uint32_t)address);
 380         }
 381
 382         return 0;
 383 }
 384
 385 unsigned int  m68k_read_memory_16(unsigned int address){
 386
 387
 388         if (maprom == 1){
 389           if(address>KICKBASE && address<KICKBASE + KICKSIZE){
 390            uint16_t value = *(uint16_t*)&g_kick[address-KICKBASE];
 391            value = (value << 8) | (value >> 8);
 392            return value;
 393           }
 394         }
 395
 396         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 397         return readGayle(address);
 398         }
 399
 400         if(address>FASTBASE){
 401         uint16_t value = *(uint16_t*)&g_ram[address- FASTBASE];
 402         value = (value << 8) | (value >> 8);
 403         return value;
 404         }
 405
 406 /*
 407         if (maprom == 1){
 408             if (ovl == 1 && address<KICKSIZE ){
 409                uint16_t value = *(uint16_t*)&g_kick[address];
 410                return (value << 8) | (value >> 8);}
 411               if (ovl == 0 && (address>KICKBASE && address<KICKBASE + KICKSIZE)){
 412                //printf("kread16 addr: %x\n",address);
 413                uint16_t value = *(uint16_t*)&g_kick[address-KICKBASE];
 414                return (value << 8) | (value >> 8);}
 415         }
 416 */
 417         if (address < 0xffffff){
 418         return (unsigned int)read16((uint32_t)address);
 419         }
 420
 421         return 0;
 422 }
 423
 424 unsigned int  m68k_read_memory_32(unsigned int address){
 425
 426         if (maprom == 1){
 427           if(address>KICKBASE && address<KICKBASE + KICKSIZE){
 428             uint32_t value = *(uint32_t*)&g_kick[address-KICKBASE];
 429             value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 430             return value << 16 | value >> 16;
 431             }
 432         }
 433
 434         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 435          return readGayleL(address);
 436         }
 437
 438         if(address>FASTBASE){
 439         uint32_t value = *(uint32_t*)&g_ram[address- FASTBASE];
 440         value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 441         return value << 16 | value >> 16;
 442         }
 443
 444 /*
 445         if (maprom == 1){
 446             if (ovl == 1 && address<KICKSIZE){
 447               uint32_t value = *(uint32_t*)&g_kick[address];
 448               value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 449               return value << 16 | value >> 16;}
 450             if (ovl == 0 && (address>KICKBASE && address<KICKBASE + KICKSIZE)){
 451                //printf("kread32/n");
 452                uint32_t value = *(uint32_t*)&g_kick[address-KICKBASE];
 453                value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 454                return value << 16 | value >> 16;}
 455         }
 456 */
 457         if (address < 0xffffff){
 458             uint16_t a = read16(address);
 459             uint16_t b = read16(address+2);
 460             return (a << 16) | b;
 461            }
 462
 463         return 0;
 464 }
 465
 466 void m68k_write_memory_8(unsigned int address, unsigned int value){
 467
 468
 469         if (address == 0xbfe001){
 470         ovl = (value & (1<<0));
 471         printf("OVL:%x\n", ovl );
 472         }
 473
 474
 475         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 476         writeGayleB(address, value);
 477         return;
 478         }
 479
 480
 481       if(address>FASTBASE){
 482         g_ram[address- FASTBASE] = value;
 483         return;
 484         }
 485
 486         if (address < 0xffffff){
 487          write8((uint32_t)address,value);
 488          return;
 489         }
 490
 491         return;
 492 }
 493
 494 void m68k_write_memory_16(unsigned int address, unsigned int value){
 495 //        if (address==0xdff030) printf("%c", value);
 496
 497         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 498         writeGayle(address,value);
 499         return;
 500         }
 501
 502         if (address == 0xbfe001)
 503         printf("16CIA Output:%x\n", value );
 504
 505
 506       if(address>FASTBASE){
 507         uint16_t* dest = (uint16_t*)&g_ram[address- FASTBASE];
 508         value = (value << 8) | (value >> 8);
 509         *dest = value;
 510         return;
 511         }
 512
 513         if (address < 0xffffff){
 514          write16((uint32_t)address,value);
 515          return;
 516         }
 517       return;
 518 }
 519
 520 void m68k_write_memory_32(unsigned int address, unsigned int value){
 521
 522
 523         if(address>GAYLEBASE && address<GAYLEBASE + GAYLESIZE){
 524         writeGayleL(address, value);
 525         }
 526
 527         if(address>FASTBASE){
 528            uint32_t* dest = (uint32_t*)&g_ram[address- FASTBASE];
 529            value = ((value << 8) & 0xFF00FF00 ) | ((value >> 8) & 0xFF00FF );
 530            value = value << 16 | value >> 16;
 531            *dest = value;
 532         return;
 533         }
 534
 535        if (address < 0xffffff){
 536         write16(address , value >> 16);
 537         write16(address+2 , value );
 538         return;
 539        }
 540
 541       return;
 542 }
 543
 544 /*
 545 void write32(uint32_t address, uint32_t data){
 546         write16(address+2 , data);
 547         write16(address , data >>16 );
 548 }
 549
 550 uint32_t read32(uint32_t address){
 551         uint16_t a = read16(address+2);
 552         uint16_t b = read16(address);
 553         return (a>>16)|b;
 554 }
 555 */
 556
 557 void write16(uint32_t address, uint32_t data)
 558 {
 559  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 560  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 561  uint32_t addr_l_s = (address >> 16) << 8;
 562  uint32_t addr_l_r = (~address >> 16) << 8;
 563  uint32_t data_s = (data & 0x0000ffff) << 8;
 564  uint32_t data_r = (~data & 0x0000ffff) << 8;
 565
 566   //      asm volatile ("dmb" ::: "memory");
 567         W16
 568         *(gpio) = gpfsel0_o;
 569         *(gpio + 1) = gpfsel1_o;
 570         *(gpio + 2) = gpfsel2_o;
 571
 572         *(gpio + 7) = addr_h_s;
 573         *(gpio + 10) = addr_h_r;
 574         GPIO_CLR = 1 << 7;
 575         GPIO_SET = 1 << 7;
 576
 577         *(gpio + 7) = addr_l_s;
 578         *(gpio + 10) = addr_l_r;
 579         GPIO_CLR = 1 << 7;
 580         GPIO_SET = 1 << 7;
 581
 582         //write phase
 583         *(gpio + 7) = data_s;
 584         *(gpio + 10) = data_r;
 585         GPIO_CLR = 1 << 7;
 586         GPIO_SET = 1 << 7;
 587
 588         *(gpio) = gpfsel0;
 589         *(gpio + 1) = gpfsel1;
 590         *(gpio + 2) = gpfsel2;
 591         while ((GET_GPIO(0)));
 592    //     asm volatile ("dmb" ::: "memory");
 593 }
 594
 595
 596 void write8(uint32_t address, uint32_t data)
 597 {
 598
 599         if ((address & 1) == 0)
 600             data = data + (data << 8); //EVEN, A0=0,UDS
 601         else data = data & 0xff ; //ODD , A0=1,LDS
 602  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 603  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 604  uint32_t addr_l_s = (address >> 16) << 8;
 605  uint32_t addr_l_r = (~address >> 16) << 8;
 606  uint32_t data_s = (data & 0x0000ffff) << 8;
 607  uint32_t data_r = (~data & 0x0000ffff) << 8;
 608
 609
 610      //   asm volatile ("dmb" ::: "memory");
 611         W8
 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         GPIO_CLR = 1 << 7;
 619         GPIO_SET = 1 << 7;
 620
 621         *(gpio + 7) = addr_l_s;
 622         *(gpio + 10) = addr_l_r;
 623         GPIO_CLR = 1 << 7;
 624         GPIO_SET = 1 << 7;
 625
 626         //write phase
 627         *(gpio + 7) = data_s;
 628         *(gpio + 10) = data_r;
 629         GPIO_CLR = 1 << 7;
 630         GPIO_SET = 1 << 7;
 631
 632         *(gpio) = gpfsel0;
 633         *(gpio + 1) = gpfsel1;
 634         *(gpio + 2) = gpfsel2;
 635         while ((GET_GPIO(0)));
 636      //   asm volatile ("dmb" ::: "memory");
 637         GPIO_SET = 1 << 7;
 638 }
 639
 640
 641 uint32_t read16(uint32_t address)
 642 {
 643         volatile int val;
 644  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 645  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 646  uint32_t addr_l_s = (address >> 16) << 8;
 647  uint32_t addr_l_r = (~address >> 16) << 8;
 648
 649      //   asm volatile ("dmb" ::: "memory");
 650         R16
 651
 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         GPIO_CLR = 1 << 7;
 659         GPIO_SET = 1 << 7;
 660
 661         *(gpio + 7) = addr_l_s;
 662         *(gpio + 10) = addr_l_r;
 663         GPIO_CLR = 1 << 7;
 664         GPIO_SET = 1 << 7;
 665
 666
 667         //read phase
 668
 669         *(gpio) = gpfsel0;
 670         *(gpio + 1) = gpfsel1;
 671         *(gpio + 2) = gpfsel2;
 672
 673         GPIO_CLR = 1 << 6;
 674         while (!(GET_GPIO(0)));
 675         GPIO_CLR = 1 << 6;
 676         asm volatile ("nop" ::);
 677         asm volatile ("nop" ::);
 678         asm volatile ("nop" ::);
 679         val = *(gpio + 13);
 680         GPIO_SET = 1 << 6;
 681     //    asm volatile ("dmb" ::: "memory");
 682         return (val >>8)&0xffff;
 683 }
 684
 685
 686 uint32_t read8(uint32_t address)
 687 {
 688         int val;
 689  uint32_t addr_h_s = (address & 0x0000ffff) << 8;
 690  uint32_t addr_h_r = (~address & 0x0000ffff) << 8;
 691  uint32_t addr_l_s = (address >> 16) << 8;
 692  uint32_t addr_l_r = (~address >> 16) << 8;
 693
 694     //    asm volatile ("dmb" ::: "memory");
 695         R8
 696         *(gpio) = gpfsel0_o;
 697         *(gpio + 1) = gpfsel1_o;
 698         *(gpio + 2) = gpfsel2_o;
 699
 700         *(gpio + 7) = addr_h_s;
 701         *(gpio + 10) = addr_h_r;
 702         GPIO_CLR = 1 << 7;
 703         GPIO_SET = 1 << 7;
 704
 705         *(gpio + 7) = addr_l_s;
 706         *(gpio + 10) = addr_l_r;
 707         GPIO_CLR = 1 << 7;
 708         GPIO_SET = 1 << 7;
 709
 710         //read phase
 711
 712         *(gpio) = gpfsel0;
 713         *(gpio + 1) = gpfsel1;
 714         *(gpio + 2) = gpfsel2;
 715
 716         GPIO_CLR = 1 << 6;
 717         while (!(GET_GPIO(0)));
 718         GPIO_CLR = 1 << 6;
 719         asm volatile ("nop" ::);
 720         asm volatile ("nop" ::);
 721         asm volatile ("nop" ::);
 722         val = *(gpio + 13);
 723         GPIO_SET = 1 << 6;
 724     //    asm volatile ("dmb" ::: "memory");
 725
 726         val = (val >>8)&0xffff;
 727         if ((address & 1) == 0)
 728             val = (val >> 8) & 0xff ; //EVEN, A0=0,UDS
 729         else
 730             val = val & 0xff ; //ODD , A0=1,LDS
 731         return val;
 732 }
 733
 734
 735
 736 /******************************************************/
 737
 738 void write_reg(unsigned int value)
 739 {
 740         asm volatile ("dmb" ::: "memory");
 741         STATUSREGADDR
 742         asm volatile ("nop" ::);
 743         asm volatile ("nop" ::);
 744         asm volatile ("nop" ::);
 745         //Write Status register
 746         GPIO_CLR = 1 << SA0;
 747         GPIO_CLR = 1 << SA1;
 748         GPIO_SET = 1 << SA2;
 749
 750         *(gpio) = gpfsel0_o;
 751         *(gpio + 1) = gpfsel1_o;
 752         *(gpio + 2) = gpfsel2_o;
 753         *(gpio + 7) = (value & 0xffff) << 8;
 754         *(gpio + 10) = (~value & 0xffff) << 8;
 755         GPIO_CLR = 1 << 7;
 756         GPIO_CLR = 1 << 7; //delay
 757         GPIO_SET = 1 << 7;
 758         GPIO_SET = 1 << 7;
 759         //Bus HIGH-Z
 760         *(gpio) = gpfsel0;
 761         *(gpio + 1) = gpfsel1;
 762         *(gpio + 2) = gpfsel2;
 763         asm volatile ("dmb" ::: "memory");
 764 }
 765
 766
 767 uint16_t read_reg(void)
 768 {
 769         uint32_t val;
 770
 771         asm volatile ("dmb" ::: "memory");
 772         STATUSREGADDR
 773         asm volatile ("nop" ::);
 774         asm volatile ("nop" ::);
 775         asm volatile ("nop" ::);
 776         //Bus HIGH-Z
 777         *(gpio) = gpfsel0;
 778         *(gpio + 1) = gpfsel1;
 779         *(gpio + 2) = gpfsel2;
 780
 781         GPIO_CLR = 1 << 6;
 782         GPIO_CLR = 1 << 6;      //delay
 783         GPIO_CLR = 1 << 6;
 784         GPIO_CLR = 1 << 6;
 785         asm volatile ("nop" ::);
 786         asm volatile ("nop" ::);
 787         asm volatile ("nop" ::);
 788         val = *(gpio + 13);
 789         GPIO_SET = 1 << 6;
 790         asm volatile ("dmb" ::: "memory");
 791
 792         return (uint16_t)(val >> 8);
 793 }
 794
 795
 796 //
 797 // Set up a memory regions to access GPIO
 798 //
 799 void setup_io()
 800 {
 801    /* open /dev/mem */
 802    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
 803       printf("can't open /dev/mem \n");
 804       exit(-1);
 805    }
 806
 807    /* mmap GPIO */
 808    gpio_map = mmap(
 809       NULL,             //Any adddress in our space will do
 810       BCM2708_PERI_SIZE,       //Map length
 811       PROT_READ|PROT_WRITE,// Enable reading & writting to mapped memory
 812       MAP_SHARED,       //Shared with other processes
 813       mem_fd,           //File to map
 814       BCM2708_PERI_BASE //Offset to GPIO peripheral
 815    );
 816
 817    close(mem_fd); //No need to keep mem_fd open after mmap
 818
 819    if (gpio_map == MAP_FAILED) {
 820       printf("gpio mmap error %d\n", (int)gpio_map);//errno also set!
 821       exit(-1);
 822    }
 823
 824    gpio = ((volatile unsigned *)gpio_map) + GPIO_ADDR/4;
 825    gpclk = ((volatile unsigned *)gpio_map) + GPCLK_ADDR/4;
 826
 827
 828 } // setup_io
 829