zpu/sw/emulation/zpuemu.c

   1 /* ZPU emulation library
   2  *
   3  * (c) 2011, Martin Strubel <hackfin@section5.ch>
   4  *
   5  * Limited functionality: Only one core in chain supported.
   6  *
   7  */
   8
   9 // These headers must be implemented by the JTAG interface to your
  10 // HW debug adapter
  11 #include "jtag.h"
  12 #include "jtag_intern.h"
  13
  14 #include "zpuemu.h"
  15
  16 #include "zpu-opcodes.h"
  17 #include <stdio.h>
  18
  19
  20 #define IRSIZE 4
  21
  22 static unsigned char
  23 s_reg8[2];
  24
  25 static JtagRegister
  26 ir_r = {
  27         .data = &s_reg8[1],
  28         .nbits = IRSIZE,
  29         .flags = JTAGREG_LSB
  30 };
  31
  32 static JtagRegister
  33 opcode_r = {
  34         .data = &s_reg8[0],
  35         .nbits = 8,
  36         .flags = JTAGREG_MSB
  37 };
  38
  39 static unsigned char
  40 s_reg16[2];
  41
  42 static JtagRegister
  43 ctrl_r = {
  44         .data = &s_reg16[0],
  45         .nbits = 16,
  46         .flags = JTAGREG_MSB
  47 };
  48
  49 static unsigned char
  50 s_reg32[4];
  51
  52 static JtagRegister
  53 data_r = {
  54         .data = &s_reg32[0],
  55         .nbits = 32,
  56         .flags = JTAGREG_MSB
  57 };
  58
  59 void select_dr(CpuContext *c, uint8_t dr)
  60 {
  61         jtag_goto_state(c->jtag, s_jtag_shift_ir);
  62         reg_set(&ir_r, 0, IRSIZE, dr);
  63         shift_generic(c->jtag, &ir_r, NULL, ir_r.nbits, UPDATE);
  64 }
  65
  66 void shiftout32(CpuContext *c, REGISTER *r, int mode)
  67 {
  68         jtag_flush(c->jtag);
  69         jtag_goto_state(c->jtag, s_jtag_shift_dr);
  70         shift_generic(c->jtag, &data_r, &data_r, data_r.nbits, mode);
  71         *r = reg_get(&data_r, 0, 32);
  72 }
  73
  74 void shiftin16(CpuContext *c, REGISTER r, int mode)
  75 {
  76         jtag_goto_state(c->jtag, s_jtag_shift_dr);
  77         reg_set(&ctrl_r, 0, 16, r);
  78         shift_generic(c->jtag, &ctrl_r, NULL, ctrl_r.nbits, mode);
  79 }
  80
  81 void shiftout16(CpuContext *c, REGISTER *r, int mode)
  82 {
  83         jtag_goto_state(c->jtag, s_jtag_shift_dr);
  84         shift_generic(c->jtag, &ctrl_r, &ctrl_r, ctrl_r.nbits, mode);
  85         *r = reg_get(&ctrl_r, 0, 16);
  86 }
  87
  88 // Auxiliaries
  89
  90 static
  91 void push_opcode(CpuContext *c, uint8_t opcode, int mode)
  92 {
  93         opcode_r.data[0] = opcode;
  94         jtag_goto_state(c->jtag, s_jtag_shift_dr);
  95         shift_generic(c->jtag, &opcode_r, NULL, opcode_r.nbits, mode);
  96 }
  97
  98 #if 0
  99 static
 100 void push_val16(CpuContext *c, uint16_t val)
 101 {
 102         int i = 14;
 103         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC); i -= 7;
 104         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC); i -= 7;
 105         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC);
 106         push_opcode(c, OPCODE_NOP, EXEC);
 107 }
 108 #endif
 109
 110 static
 111 void push_val32(CpuContext *c, uint32_t val)
 112 {
 113         int i = 28;
 114         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC); i -= 7;
 115         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC); i -= 7;
 116         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC); i -= 7;
 117         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC); i -= 7;
 118         push_opcode(c, OPCODE_IM | ((val >> i) & 0x7f), EXEC);
 119         push_opcode(c, OPCODE_NOP, EXEC);
 120 }
 121
 122 static
 123 uint32_t mem_read32(CpuContext *c, uint32_t addr)
 124 {
 125         REGISTER r;
 126         int q = jtag_queue(c->jtag, 1);
 127         select_dr(c, TAP_EMUIR);
 128                 push_opcode(c, OPCODE_PUSHSP, EXEC);
 129                 push_val32(c, addr);
 130                 push_opcode(c, OPCODE_LOAD, EXEC);
 131                 push_opcode(c, OPCODE_NOP, EXEC);
 132         select_dr(c, TAP_EMUDATA);
 133                 shiftout32(c, &r, UPDATE); // Execute Stack fixup
 134         select_dr(c, TAP_EMUIR);
 135                 push_opcode(c, OPCODE_LOADSP | (LOADSP_INV ^ 0x01), EXEC);
 136                 push_opcode(c, OPCODE_POPSP, EXEC);
 137                 push_opcode(c, OPCODE_NOP, EXEC);
 138         jtag_queue(c->jtag, q);
 139         return r;
 140 }
 141
 142 // Little read cache:
 143 struct cache {
 144         ADDR addr;
 145         uint32_t val;
 146 } g_cache = { 0xffffffff, 0 };
 147
 148 static
 149 uint32_t mem_read32_cached(CpuContext *c, ADDR a)
 150 {
 151         if (a != g_cache.addr) {
 152                 g_cache.val = mem_read32(c, a);
 153                 g_cache.addr = a;
 154                 // printf("Read %08x\n", g_cache.val);
 155         }
 156         return g_cache.val;
 157 }
 158
 159 uint8_t mem_read8(CpuContext *c, ADDR a)
 160 {
 161         uint32_t v;
 162         int shift = (3 - (a & 0x3)) << 3;
 163         a &= ~0x3;
 164         // printf("shift: %d\n", shift);
 165         v = mem_read32_cached(c, a) >> shift;
 166         return v;
 167 }
 168
 169 uint16_t mem_read16(CpuContext *c, ADDR a)
 170 {
 171         uint32_t v;
 172         int shift = (2 - (a & 0x2)) << 3;
 173         a &= ~0x3;
 174         // printf("shift: %d\n", shift);
 175         v = mem_read32_cached(c, a) >> shift;
 176         return v;
 177 }
 178
 179 void mem_write32(CpuContext*c, uint32_t addr, uint32_t val)
 180 {
 181         // Invalidate cache, when we're writing to the same addr:
 182         if (g_cache.addr == (addr)) {
 183                 g_cache.addr = 0xffffffff;
 184         }
 185         select_dr(c, TAP_EMUIR);
 186                 push_opcode(c, OPCODE_PUSHSP, EXEC);
 187                 push_val32(c, val);
 188                 push_val32(c, addr); // Address
 189                 push_opcode(c, OPCODE_STORE, EXEC);
 190                 push_opcode(c, OPCODE_LOADSP | (LOADSP_INV ^ 0x00), EXEC);
 191                 push_opcode(c, OPCODE_POPSP, EXEC);
 192                 push_opcode(c, OPCODE_NOP, UPDATE);
 193 }
 194
 195
 196 void mem_write16(CpuContext*c, uint32_t addr, uint16_t val)
 197 {
 198         int shift = (2 - (addr & 0x2)) << 3;
 199         uint32_t v;
 200
 201         addr &= ~0x3;
 202         v = mem_read32_cached(c, addr);
 203
 204         v = (v & ~(0xffff << shift)) | (val << shift);
 205         mem_write32(c, addr, v);
 206 }
 207
 208 void mem_write8(CpuContext *c, uint32_t addr, uint8_t val)
 209 {
 210         int shift = (3 - (addr & 0x3)) << 3;
 211         uint32_t v;
 212
 213         addr &= ~0x3;
 214         v = mem_read32_cached(c, addr);
 215
 216         v = (v & ~(0xff << shift)) | (val << shift);
 217         mem_write32(c, addr, v);
 218 }
 219
 220 int enter_emulation(CpuContext *c)
 221 {
 222         REGISTER r;
 223
 224         g_cache.addr = 0xffffffff; // Invalidate cache
 225
 226         // puts(">>> Enter emulation");
 227         select_dr(c, TAP_EMUCTRL);
 228         r = EMUREQ;
 229         shiftin16(c, r, UPDATE);
 230
 231         return 0;
 232 }
 233
 234 int leave_emulation(CpuContext *c)
 235 {
 236         int error = 0;
 237
 238         // Turn off emulation bit
 239         select_dr(c, TAP_EMUCTRL);
 240         shiftin16(c, 0, UPDATE);
 241
 242         // Run some emulated opcodes:
 243         // Return from emulation:
 244         select_dr(c, TAP_EMUIR);
 245         push_opcode(c, OPCODE_EMULEAVE, EXEC);
 246         return error;
 247 }
 248
 249 ////////////////////////////////////////////////////////////////////////////
 250 // API calls
 251
 252 int zpu_emuinit(CpuContext *c, CONTROLLER jtag)
 253 {
 254         c->jtag = jtag;
 255         return 0;
 256 }
 257
 258 int zpu_getid(CpuContext *c, uint32_t *code)
 259 {
 260         select_dr(c, TAP_IDCODE);
 261         shiftout32(c, code, UPDATE);
 262         return 0;
 263 }
 264
 265 int zpu_resume(CpuContext *c, int step)
 266 {
 267         jtag_flush(c->jtag);
 268         if (step) {
 269                 select_dr(c, TAP_EMUCTRL);
 270                 shiftin16(c, EMUREQ, UPDATE);
 271                 select_dr(c, TAP_EMUIR);
 272                 push_opcode(c, OPCODE_EMULEAVE, EXEC);
 273         } else {
 274                 leave_emulation(c);
 275         }
 276         return 0;
 277 }
 278
 279 int zpu_emulation(CpuContext *c, int which)
 280 {
 281         // FIXME: for multicore, this needs to change.
 282         jtag_flush(c->jtag);
 283         if (which) {
 284                 enter_emulation(c);
 285         } else {
 286                 leave_emulation(c);
 287         }
 288         return 0;
 289 }
 290
 291 int zpu_state(CpuContext *c, uint16_t *state)
 292 {
 293         REGISTER r;
 294         int error = 0;
 295
 296         select_dr(c, TAP_EMUSTAT);
 297         shiftout16(c, &r, UPDATE);
 298         *state = r;
 299         return error;
 300 }
 301
 302 int zpu_reset(CpuContext *c, int mode)
 303 {
 304         // TODO: Implement system control register on zealot
 305         return 0;
 306 }
 307
 308 int zpu_setreg(CpuContext *c, int regno, REGISTER val)
 309 {
 310         switch (regno) {
 311                 case REG_PC:
 312                         select_dr(c, TAP_EMUIR);
 313                                 push_val32(c, val);
 314                                 push_opcode(c, OPCODE_POPPC, EXEC);
 315                         break;
 316                 case REG_SP:
 317                         select_dr(c, TAP_EMUIR);
 318                                 push_val32(c, val);
 319                                 push_opcode(c, OPCODE_POPSP, EXEC);
 320                         break;
 321                 default: return -1;
 322         }
 323         return 0;
 324 }
 325
 326 int zpu_getreg(CpuContext *c, int regno, REGISTER *val)
 327 {
 328         REGISTER r;
 329         int q = jtag_queue(c->jtag, 1);
 330         switch (regno) {
 331                 case REG_PC:
 332                         // XXX needed to update dbg_o.<signal>:
 333                         select_dr(c, TAP_EMUIR); // XXX
 334                                 push_opcode(c, OPCODE_NOP, EXEC); // XXX
 335                         select_dr(c, TAP_DBGPC);
 336                         shiftout32(c, &r, UPDATE);
 337                         break;
 338                 case REG_SP:
 339                         select_dr(c, TAP_EMUIR);
 340                                 push_opcode(c, OPCODE_PUSHSP, EXEC);
 341                                 // XXX needed to update dbg_o.<signal>:
 342                                 push_opcode(c, OPCODE_NOP, EXEC); // XXX
 343                                 push_opcode(c, OPCODE_POPSP, UPDATE); // queue, exec later
 344                         select_dr(c, TAP_EMUDATA);
 345                         shiftout32(c, &r, EXEC); // (here)
 346                         break;
 347                 default: return -1;
 348         }
 349         *val = r;
 350         jtag_queue(c->jtag, q);
 351         return 0;
 352 }
 353
 354 void zpu_dumpstat(CpuContext *c)
 355 {
 356         REGISTER r;
 357
 358         select_dr(c, TAP_EMUSTAT);
 359         shiftout16(c, &r, UPDATE);
 360         printf("EMUSTAT: %04x -", r & 0xffff);
 361         if (r) {
 362                 if (r & ZPU_IDIM)    printf(" [IDIM]");
 363                 if (r & ZPU_INRESET) printf(" [RESET]");
 364                 if (r & ZPU_BREAK)   printf(" [BREAK]");
 365                 if (r & EMUACK)      printf(" [EMUACK]");
 366                 if (r & EMURDY)      printf(" [EMURDY]");
 367                 if (r & ZPU_MEMBUSY) printf(" [MEM_BUSY]");
 368         }
 369         printf("\n");
 370         select_dr(c, TAP_COUNT1);
 371         shiftout32(c, &r, UPDATE);
 372         printf("COUNT1: %012d\n", r);
 373         select_dr(c, TAP_COUNT2);
 374         shiftout16(c, &r, UPDATE);
 375         printf("COUNT2: %08d\n", r);
 376 }
 377
 378 int guess_access(ADDR addr, unsigned int *count)
 379 {
 380         int sizecode;
 381         // I/O space wants to be addressed long word wise:
 382         if (addr >= 0x80080000 && *count == 4) {
 383                 *count = 1;
 384                 return LDST_32;
 385         }
 386         // if we have even addresses and even count, we can
 387         // use word size transfers instead of byte wise.
 388         switch (addr % 4) {
 389         case 0:
 390                 switch (*count % 4) {
 391                         case 0:
 392                                 sizecode = LDST_32;
 393                                 *count /= 4;
 394                                 break;
 395                         case 2:
 396                                 sizecode = LDST_16;
 397                                 *count /= 2;
 398                                 break;
 399                         default:
 400                                 sizecode = LDST_8;
 401                                 break;
 402                 }
 403                 break;
 404         case 2:
 405                 if (*count % 2 == 0) {
 406                                 sizecode = LDST_16;
 407                                 *count /= 2;
 408                 } else {
 409                                 sizecode = LDST_8;
 410                 }
 411                 break;
 412         default:
 413                 sizecode = LDST_8;
 414         }
 415         return sizecode;
 416 }
 417
 418
 419 int zpu_mem_read(CpuContext *c, ADDR addr, unsigned int count,
 420         unsigned char *buf)
 421 {
 422         int sz;
 423         uint32_t v;
 424         int q = jtag_queue(c->jtag, 1);
 425
 426         sz = guess_access(addr, &count);
 427
 428         switch (sz) {
 429                 case LDST_8:
 430                         while (count--) {
 431                                 *buf++ = mem_read8(c, addr++);
 432                         }
 433                         break;
 434                 case LDST_16:
 435                         while (count--) {
 436                                 v = mem_read16(c, addr); addr += 2;
 437                                 buf[1] = v; v >>= 8;
 438                                 buf[0] = v;
 439                                 buf += 2;
 440                         }
 441                         break;
 442                 case LDST_32:
 443                         while (count--) {
 444                                 v = mem_read32(c, addr); addr += 4;
 445                                 buf[3] = v; v >>= 8;
 446                                 buf[2] = v; v >>= 8;
 447                                 buf[1] = v; v >>= 8;
 448                                 buf[0] = v;
 449                                 buf += 4;
 450                         }
 451                         break;
 452         }
 453         jtag_flush(c->jtag);
 454         jtag_queue(c->jtag, q);
 455         return 0;
 456 }
 457
 458 int zpu_mem_write(CpuContext *c, ADDR addr, unsigned int count,
 459         const unsigned char *buf)
 460 {
 461         int sz;
 462         uint32_t v;
 463         int q = jtag_queue(c->jtag, 1);
 464
 465         sz = guess_access(addr, &count);
 466
 467         switch (sz) {
 468                 case LDST_8:
 469                         // XXX: Could be optimized further
 470                         while (count--) {
 471                                 v = *buf++;
 472                                 mem_write8(c, addr, v); addr++;
 473                         }
 474                         break;
 475                 case LDST_16:
 476                         while (count--) {
 477                                 v = (buf[0] << 8) | buf[1];
 478                                 mem_write16(c, addr, v);
 479                                 addr += 2; buf += 2;
 480                         }
 481                         break;
 482                 case LDST_32:
 483                         while (count--) {
 484                                 v = (buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3];
 485                                 mem_write32(c, addr, v);
 486                                 addr += 4; buf += 4;
 487                         }
 488                         break;
 489         }
 490         jtag_flush(c->jtag);
 491         jtag_queue(c->jtag, q);
 492         return 0;
 493 }