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