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