rom_funcs.c

   1 /*
   2  * TODO TODO TODO TODO TODO TODO TODO TODO TODO
   3  *
   4  * some ROM functions are not completely implemented nor documented
   5  * emulate more of smb access with fifo, rom functions emulate
   6  * argument pointer evaluate location
   7  *
   8  */
   9
  10
  11 #include <stdio.h>
  12 #include "log.h"
  13 #include "disasm.h"
  14 #include "access.h"
  15 #include "isa.h"
  16
  17 //for SMB input from user HACK
  18 extern u8 * test_buf(void);
  19
  20 #define r_r3 read_reg8(MAIN_REG_R3, 1)
  21 #define r_r2 read_reg8(MAIN_REG_R2, 1)
  22 #define r_r1 read_reg8(MAIN_REG_R1, 1)
  23 #define r_r0 read_reg8(MAIN_REG_R0, 1)
  24
  25 #define w_r3(x) write_reg8(MAIN_REG_R3, x, 1)
  26 #define w_r2(x) write_reg8(MAIN_REG_R2, x, 1)
  27 #define w_r1(x) write_reg8(MAIN_REG_R1, x, 1)
  28 #define w_r0(x) write_reg8(MAIN_REG_R0, x, 1)
  29
  30 #define r_st(x) read_mem(stack_ptr + (x), 1)
  31 #define w_st(x, y) write_mem(stack_ptr + (x), y, 1)
  32
  33 /** ********* rom routines ************/
  34
  35 static u16 rom_execute(u16 stack_ptr)
  36 {
  37         //TODO code analysis
  38         log_comment_add("       void? rom_execute(void?)        //untested\n");
  39
  40         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
  41
  42         return read_ip(1);
  43 }
  44
  45 static u16 smbSlaveCmd(u16 stack_ptr)
  46 {
  47         u8 cmd = r_r3;
  48         u8 size = r_r2;
  49         u16 table_ptr = (r_r1 << 8) | r_r0;
  50         u16 ret;
  51
  52         log_comment_add(
  53                 "       int smbSlaveCmd(\n"
  54                 "               u8 cmd  = %02hhx,\n"
  55                 "               u8 size = %hhu,\n"
  56                 "               unsigned *table() = %04hx)\n",
  57                 cmd,
  58                 size,
  59                 table_ptr
  60         );
  61
  62         if (cmd < size) {
  63                 write_ip(read_pc()+1, 1);
  64
  65 //TODO ?need modify rom code, perma hook?
  66                 ret = table_ptr + cmd;
  67
  68 //cannot be set registers will be changed in jumped functions (maybe use footer detection :-/ )
  69 //              reg[3] = 0;     //ret success
  70 //              reg[2] = 1;     //ret success
  71         } else {
  72                 w_r3(0);        //ret failed
  73                 w_r2(0);        //ret failed
  74
  75                 ret = read_ip(1);
  76         }
  77
  78         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
  79
  80         return ret;
  81 }
  82
  83 static u16 smb_NACK(u16 stack_ptr)
  84 {
  85         log_comment_add("       void smb_NACK(void)\n");
  86
  87         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
  88
  89         return read_ip(1);
  90 }
  91
  92 static u16 smb_ACK(u16 stack_ptr)
  93 {
  94         log_comment_add("       void smb_ACK(void)\n");
  95
  96         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
  97
  98         return read_ip(1);
  99 }
 100
 101
 102 // u8 smb_data_val=0x71;
 103
 104 static u16 smbSlaveRcvWord(u16 stack_ptr)
 105 {
 106         u16 data = (r_r3 << 8) | r_r2;
 107
 108         log_comment_add(
 109                 "       int smbSlaveRcvWord(int *data = [%04hx])\n",
 110                 data
 111         );
 112
 113         //testing NOTICE
 114         u16 val;
 115 //      val = 0x1402;
 116 //      val = 0xfd34;
 117         val = test_buf()[0] << 8;
 118         val |= test_buf()[0];
 119
 120         log_comment_add("       VVV %04hx\n", val);
 121
 122 //      smb_data_val = 0x73;
 123
 124         write_mem(data, val & 0xff, 1);
 125         write_mem(data+1, (val >> 8) & 0xff, 1);
 126
 127
 128         log_comment_add("       XXX %02hhx %02hhx\n",
 129                                         read_mem(data, 0),
 130                                         read_mem(data+1, 0)
 131
 132                                 );
 133
 134         w_r3(0);        //ret success
 135         w_r2(1);        //ret success
 136
 137         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 138
 139         return read_ip(1);
 140 }
 141
 142 static u16 smbSlaveSndWord(u16 stack_ptr)
 143 {
 144         u16 data = (r_r3 << 8) | r_r2;
 145
 146         log_comment_add(
 147                 "       int smbSlaveSndWord(int data = %04hx)\n",
 148                 data
 149         );
 150
 151         w_r3(0);        //ret success
 152         w_r2(1);        //ret success
 153
 154         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 155
 156         return read_ip(1);
 157 }
 158
 159
 160 static u16 smbSlaveSndBlock(u16 stack_ptr)
 161 {
 162         u8 cnt = r_r3;
 163         u16 dptr = (r_r2 << 8) | r_r1;
 164
 165         log_comment_add(
 166                 "       void smbSlaveSndBlock(\n"
 167                 "               u8 byte_cnt = %hhu,\n"
 168                 "               u8 *block   = [%04hx])\n",
 169                 cnt,
 170                 dptr
 171         );
 172
 173         log_comment_add("\n");
 174         log_comment_add("       Data: {");
 175         u8 data;
 176         for (unsigned a=0;a<cnt;a++) {
 177                 data = read_mem(dptr + a, 1);
 178                 log_comment_add("%02hhx, ", data);
 179         }
 180         log_comment_add("}\n");
 181
 182         w_r3(0);        //ret success
 183         w_r2(1);        //ret success
 184
 185         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 186
 187         return read_ip(1);
 188 }
 189
 190
 191 static u16 smbSlaveRcvBlock(u16 stack_ptr)
 192 {
 193         u16 cnt_ptr = (r_r3 << 8) | r_r2;
 194         u16 dptr = (r_r1 << 8) | r_r0;
 195
 196         log_comment_add(
 197                 "       void smbSlaveRcvBlock(\n"
 198                 "               u8 *byte_cnt = [%04hx],\n"
 199                 "               u8 *block    = [%04hx])\n",
 200                 cnt_ptr,
 201                 dptr
 202         );
 203
 204         u8 cnt = test_buf()[0];
 205         u8 data;
 206
 207         write_mem(cnt_ptr, cnt, 1);
 208
 209         log_comment_add("\n");
 210         log_comment_add("       Cnt=%hhu\n", cnt);
 211         log_comment_add("       Data: {");
 212         for (unsigned a=0;a<cnt;a++) {
 213                 data = test_buf()[0];
 214                 log_comment_add("%02hhx, ", data);
 215                 write_mem(dptr + a, data, 1);
 216         }
 217         log_comment_add("}\n");
 218
 219         w_r3(0);        //ret success
 220         w_r2(1);        //ret success
 221
 222         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 223
 224         return read_ip(1);
 225 }
 226
 227
 228
 229 static u16 smbSlaveWord(u16 stack_ptr)
 230 {
 231         u16 dir_ptr = (r_r3 << 8) | r_r2;
 232         u16 data_ptr = (r_r1 << 8) | r_r0;
 233
 234         log_comment_add(
 235                 "       int smbSlaveWord(\n"
 236                 "               u8 *dir   = [%04hx],\n"
 237                 "               int *data = [%04hx])\n",
 238                 dir_ptr,
 239                 data_ptr
 240         );
 241
 242         log_comment_add("\n");
 243         log_comment_add("       read pre %02hhx %02hhx\n",
 244                 read_mem(data_ptr, 0),
 245                 read_mem(data_ptr+1, 0)
 246         );
 247
 248         //this are changeable values
 249         u8 flag_written = test_buf()[0];
 250
 251         write_mem(dir_ptr, flag_written, 1);
 252
 253         if (flag_written) {
 254                 u8 lo = test_buf()[0];
 255                 u8 hi = test_buf()[0];
 256
 257                 log_comment_add("       write %02hhx %02hhx\n",
 258                         lo, hi
 259                 );
 260
 261                 write_mem(data_ptr, lo, 1);
 262                 write_mem(data_ptr+1, hi, 1);
 263         }
 264
 265         w_r3(0);        //ret success
 266         w_r2(1);        //ret success
 267
 268         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 269
 270         return read_ip(1);
 271 }
 272
 273
 274 static u16 smbSlaveBlock(u16 stack_ptr)
 275 {
 276         u16 drdy_ptr = (r_r3 << 8) | r_r2;
 277         u16 bytecnt_ptr = (r_r1 << 8) | r_r0;
 278         u8 maxcnt = r_st(0);
 279         u16 block_ptr = (r_st(1) << 8) | r_st(2);
 280
 281         log_comment_add(
 282                 "       int smbSlaveBlock(\n"
 283                 "               u8 *drdy    = [%04hx],\n"
 284                 "               u8 *bytecnt = [%04hx],\n"
 285                 "               u8 maxcnt   = %hhu,\n"
 286                 "               u8 *blocks  = [%04hx])\n",
 287                 drdy_ptr,
 288                 bytecnt_ptr,
 289                 maxcnt,
 290                 block_ptr
 291         );
 292         log_comment_add("\n");
 293
 294         //TODO maxcnt?
 295
 296         //this are changeable values
 297         u8 flag_written = test_buf()[0];
 298         write_mem(drdy_ptr, flag_written, 1);
 299
 300         u8 bytecnt = read_mem(bytecnt_ptr, 0);
 301         log_comment_add("       Cnt=%hhu\n", bytecnt);
 302         log_comment_add("       Data (slave->master): {");
 303         for (unsigned a=0;a<bytecnt;a++) {
 304                 log_comment_add("%02hhx, ", read_mem(block_ptr + a, 0));
 305         }
 306         log_comment_add("}\n");
 307
 308         if (flag_written) {
 309                 u8 data;
 310                 bytecnt = test_buf()[0];
 311                 log_comment_add("       Cnt=%hhu\n", bytecnt);
 312                 log_comment_add("       Data (master->slave): {");
 313                 for (unsigned a=0;a<bytecnt;a++) {
 314                         data = test_buf()[0];
 315                         log_comment_add("%02hhx, ", data);
 316                         write_mem(block_ptr + a, data, 1);
 317                 }
 318                 log_comment_add("}\n");
 319         }
 320
 321
 322         w_r3(0);        //ret success
 323         w_r2(1);        //ret success
 324
 325         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 326
 327         return read_ip(1);
 328 }
 329
 330
 331 #define BQ29330_REG_NUM 9
 332 u8 bq29330[BQ29330_REG_NUM];
 333
 334 void write_bq29330_reg(u8 reg, u8 val)
 335 {
 336         if (reg >= BQ29330_REG_NUM)
 337                 return;
 338
 339         bq29330[reg] = val;
 340 }
 341
 342 // #define BQ29330_REG_NAMES_MAX 16
 343
 344 const char *bq29330_reg_names[BQ29330_REG_NUM] = {
 345         "status",
 346         "output_ctrl",
 347         "state_ctrl",
 348         "fcn_ctrl",
 349         "cell_sel",
 350         "olv",
 351         "old",
 352         "scc",
 353         "scd",
 354 };
 355
 356 const char *bq29330_bits_names[BQ29330_REG_NUM][8] = {
 357         {
 358                 "scd", "scc", "ol", "wdf", "zv", "rsvd5", "rsvd6", "rsvd7"
 359         },
 360         {
 361                 "ltclr", "dsg", "chg", "xzv", "gpod", "pms_chg", "rsvd6", "rsvd7"
 362         },
 363         {
 364                 "sleep", "ship", "wddis", "wdrst", "rsns", "rsvd5", "rsvd6", "rsvd7"
 365         },
 366         {
 367                 "vmen", "pack", "bat", "tout", "rsvd4", "rsvd5", "rsvd6", "rsvd7"
 368         },
 369         {
 370                 "cell0", "cell1", "cal0", "cal1", "cb0", "cb1", "cb2", "cb3"
 371         },
 372         {
 373                 "olv0", "olv1", "olv2", "olv3", "olv4", "rsvd5", "rsvd6", "rsvd7"
 374         },
 375         {
 376                 "old0", "old1", "old2", "old3", "rsvd4", "rsvd5", "rsvd6", "rsvd7"
 377         },
 378         {
 379                 "sccv0", "sccv1", "sccv2", "sccv3", "sccd0", "sccd1", "sccd2", "sccd3"
 380         },
 381         {
 382                 "scdv0", "scdv1", "scdv2", "scdv3", "scdd0", "scdd1", "scdd2", "scdd3"
 383         },
 384 };
 385
 386
 387
 388 static u16 I2CWriteBlock(u16 stack_ptr)
 389 {
 390         u8 addr = r_r3;
 391         u8 cmd = r_r2;
 392         u8 cnt = r_r1;
 393         u16 dptr = (r_st(0) << 8) | r_st(1);
 394
 395         log_comment_add(
 396                 "       u8 I2CWriteBlock(\n"
 397                 "               u8 addr  = %02hhx,\n"
 398                 "               u8 cmd   = %02hhx,\n"
 399                 "               u8 cnt   = %hhu,\n"
 400                 "               u8 *data = [%04hx])\n",
 401                 addr,
 402                 cmd,
 403                 cnt,
 404                 dptr
 405         );
 406
 407         log_comment_add("\n");
 408
 409         if (1) {
 410 //      if (addr == 0x40) {
 411                 for (u8 idx = 0; idx < cnt; idx++) {
 412
 413                         if ((cmd+idx) < BQ29330_REG_NUM) {
 414                                 u8 val_old = bq29330[cmd+idx];
 415                                 u8 val_new = read_mem(dptr + idx, 0);
 416
 417                                 log_comment_reg_bits(
 418                                         bq29330_reg_names[cmd+idx],
 419                                         bq29330_bits_names[cmd+idx],
 420                                         val_old,
 421                                         val_new
 422                                 );
 423
 424                                 bq29330[cmd+idx] = val_new;
 425
 426                                 if (((cmd+idx)==1) && (val_new&1)) {
 427                                         //unlatch errors
 428                                         bq29330[0]=0;
 429                                 }
 430
 431
 432                         } else {
 433                                 log_comment_add("       unknown bq29330[%hhd]=%02hhx\n",
 434                                                                 cmd+idx,
 435                                                                 read_mem(dptr + idx, 1));
 436                         }
 437                 }
 438         } else {
 439                 for (u8 idx = 0; idx < cnt; idx++) {
 440                         log_comment_add("       unknown dev[%hhd]=%02hhx\n",
 441                                                         idx,
 442                                                         read_mem(dptr + idx, 1));
 443                 }
 444         }
 445
 446         w_r3(0);        //ret success
 447         w_r2(1);        //ret success
 448
 449         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 450
 451         return read_ip(1);
 452 }
 453
 454 static u16 I2CReadBlock(u16 stack_ptr)
 455 {
 456
 457         u8 addr = r_r3;
 458         u8 cmd = r_r2;
 459         u8 cnt = r_r1;
 460         u16 dptr = (r_st(0) << 8) | r_st(1);
 461
 462         log_comment_add(
 463                 "       u8 I2CReadBlock(\n"
 464                 "               u8 addr  = %02hhx,\n"
 465                 "               u8 cmd   = %02hhx,\n"
 466                 "               u8 cnt   = %hhu,\n"
 467                 "               u8 *data = [%04hx])\n",
 468                 addr,
 469                 cmd,
 470                 cnt,
 471                 dptr
 472         );
 473
 474         log_comment_add("\n");
 475
 476         if (1) {
 477 //      if (addr == 0x40) {
 478                 for (u8 idx = 0; idx < cnt; idx++) {
 479
 480                         if ((cmd+idx) < BQ29330_REG_NUM) {
 481                                 u8 val = bq29330[cmd + idx];
 482
 483                                 log_comment_reg_bits(
 484                                         bq29330_reg_names[cmd+idx],
 485                                         bq29330_bits_names[cmd+idx],
 486                                         val,
 487                                         val
 488                                 );
 489
 490                                 write_mem(dptr + idx, val, 1);
 491                         } else {
 492                                 log_comment_add("       unknown bq29330[%hhd]=%02hhx\n",
 493                                                                 cmd+idx,
 494                                                                 read_mem(dptr + idx, 1));
 495                         }
 496                 }
 497         } else {
 498                 for (u8 idx = 0; idx < cnt; idx++) {
 499                         log_comment_add("       unknown dev[%hhd]=%02hhx\n",
 500                                                         idx,
 501                                                         read_mem(dptr + idx, 1));
 502                 }
 503         }
 504
 505         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 506
 507         w_r3(0);        //ret success
 508         w_r2(1);        //ret success
 509
 510         return read_ip(1);
 511 }
 512
 513
 514 static u16 FdataEraseRow(u16 stack_ptr)
 515 {
 516         u8 row = r_r3;
 517         u16 flash_addr = 32*row + 0x4000;
 518
 519         log_comment_add("       void FdataEraseRow(u8 row = %hhd)\n",
 520                 row
 521         );
 522
 523         log_comment_add("\n");
 524         log_comment_add("       FLASH[%04hx]\n",
 525                 flash_addr
 526         );
 527
 528         //NOTICE erases 2 rows
 529         for (unsigned a=0;a<64;a++) {
 530                 write_mem(flash_addr + a, 0xff, 1);
 531         }
 532
 533         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 534
 535         return read_ip(1);
 536 }
 537
 538
 539 static u16 FdataProgRow(u16 stack_ptr)
 540 {
 541         u8 row = r_r3;
 542         u8 col = r_r2;
 543         u8 cnt = r_r1;
 544
 545         u16 flash_addr = 32*row + col + 0x4000;
 546         u16 dptr = (r_st(0) << 8) | r_st(1);
 547
 548         log_comment_add(
 549                 "       void FdataProgRow(\n"
 550                 "               u8 row   = %hhd,\n"
 551                 "               u8 col   = %hhd,\n"
 552                 "               u8 cnt   = %hhd,\n"
 553                 "               u8 *data = [%04hx])\n",
 554                 row,
 555                 col,
 556                 cnt,
 557                 dptr
 558         );
 559
 560         log_comment_add("\n");
 561         log_comment_add("       FLASH[%04hx]\n",
 562                 flash_addr
 563         );
 564
 565         log_comment_add("       Data: {");
 566         u8 data;
 567         for (unsigned a=0;a<cnt;a++) {
 568                 data = read_mem(dptr + a, 1);
 569                 log_comment_add("%02hhx, ", data);
 570                 write_mem(flash_addr + a, data, 1);
 571         }
 572         log_comment_add("}\n");
 573
 574         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 575
 576         return read_ip(1);
 577 }
 578
 579 static u16 FdataProgWord(u16 stack_ptr)
 580 {
 581         u16 flash_addr = (r_r3 << 8) | r_r2;
 582         u8 data = r_r1;
 583
 584         log_comment_add(
 585                 "       void FdataProgWord(\n"
 586                 "               u8 *addr = [%04hx],\n"
 587                 "               u8 data  = %02hhx)\n",
 588                 flash_addr,
 589                 data
 590         );
 591
 592         write_mem(flash_addr, data, 1);
 593
 594         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 595
 596         return read_ip(1);
 597 }
 598
 599 //program memory, read only LSB u16
 600 static u16 FlashRdRow(u16 stack_ptr)
 601 {
 602         u16 row = (r_r3 << 8) | r_r2;
 603         u8 col = r_r1;
 604         u8 cnt = r_r0;
 605         u16 dataptr = (r_st(0) << 8) | r_st(1);
 606
 607         log_comment_add(
 608                 "       void FlashRdRow(\n"
 609                 "               u16 row   = %hu,\n"
 610                 "               u8 col    = %hhd,\n"
 611                 "               u8 cnt    = %hhd,\n"
 612                 "               u16 *data = [%04hx])\n",
 613                 row, col, cnt, dataptr
 614         );
 615
 616         u16 prog_addr = row*32+col;
 617         struct opcode_word prog_word = read_code(prog_addr);
 618
 619         log_comment_add("       code[%04hx]=%06x\n",
 620                 prog_addr, prog_word.raw
 621         );
 622
 623         //TODO missing cnt > 1 (implement!)
 624         write_mem(dataptr + 0, (prog_word.raw >> 8) & 0xff, 1);
 625         write_mem(dataptr + 1, (prog_word.raw >> 0) & 0xff, 1);
 626
 627         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 628
 629         return read_ip(1);
 630 }
 631
 632
 633 static u16 FdataMassErase(u16 stack_ptr)
 634 {
 635         log_comment_add("       void FdataMassErase(void)\n");
 636
 637         for (unsigned a=0x4000;a<0x4800;a++) {
 638                 write_mem(a, 0xff, 0);  //NOTICE, that would be hell to log
 639         }
 640
 641         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 642
 643         return read_ip(1);
 644 }
 645
 646
 647 static u16 SetAddr(u16 stack_ptr)
 648 {
 649         log_comment_add("       void SetAddr(void)\n");
 650         log_comment_add("               fully transparent\n");
 651
 652         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 653
 654         return read_ip(1);
 655 }
 656
 657 static u16 PokeByte(u16 stack_ptr)
 658 {
 659         log_comment_add("       void PokeByte(void)\n");
 660         log_comment_add("               fully transparent\n");
 661
 662         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 663
 664         return read_ip(1);
 665 }
 666
 667 static u16 PeekByte(u16 stack_ptr)
 668 {
 669         log_comment_add("       void PeekByte(void)\n");
 670         log_comment_add("               fully transparent\n");
 671
 672         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 673
 674         return read_ip(1);
 675 }
 676
 677
 678 static u16 ReadRAMBlk(u16 stack_ptr)
 679 {
 680         log_comment_add("       void ReadRAMBlk(void)\n");
 681         log_comment_add("               fully transparent\n");
 682
 683         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 684
 685         return read_ip(1);
 686 }
 687
 688 static u16 smbWaitBusFree(u16 stack_ptr)
 689 {
 690         u8 status = r_r3;
 691
 692         log_comment_add("       int smbWaitBusFree(u8 status = %hhd)\n",
 693                 status
 694         );
 695
 696         w_r3(0);        //ret success
 697         w_r2(1);        //ret success
 698
 699 //      sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 700
 701         return read_ip(1);
 702 }
 703
 704
 705 //compiler fcns
 706
 707 static u16 mulhisi3(u16 stack_ptr)
 708 {
 709         s16 a = (r_r3 << 8) | r_r2;
 710         s16 b = (r_r1 << 8) | r_r0;
 711
 712         log_comment_add(
 713                 "       s32 mulhisi3(\n"
 714                 "               s16 a = %hi,\n"
 715                 "               s16 b = %hi)\n",
 716                 a, b
 717         );
 718
 719         s32 m = a * b;
 720
 721         log_comment_add("\n");
 722         log_comment_add("       mul=%li\n", m);
 723
 724         w_r3((m >> 24) & 0xff);
 725         w_r2((m >> 16) & 0xff);
 726         w_r1((m >> 8) & 0xff);
 727         w_r0((m >> 0) & 0xff);
 728
 729 //      sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 730
 731         return do_pop();
 732 }
 733
 734 static u16 umulhisi3(u16 stack_ptr)
 735 {
 736         u16 a = (r_r3 << 8) | r_r2;
 737         u16 b = (r_r1 << 8) | r_r0;
 738
 739         log_comment_add(
 740                 "       u32 umulhisi3(\n"
 741                 "               u16 a = %hu,\n"
 742                 "               u16 b = %hu)\n",
 743                 a, b
 744         );
 745
 746         u32 m = a * b;
 747
 748         log_comment_add("\n");
 749         log_comment_add("       mul=%u\n", m);
 750
 751         w_r3((m >> 24) & 0xff);
 752         w_r2((m >> 16) & 0xff);
 753         w_r1((m >> 8) & 0xff);
 754         w_r0((m >> 0) & 0xff);
 755
 756         return do_pop();
 757 }
 758
 759
 760 static u16 mulsi3(u16 stack_ptr)
 761 {
 762         u32 a;
 763         a = r_st(0) << 24;
 764         a |= r_st(1) << 16;
 765         a |= r_st(2) << 8;
 766         a |= r_st(3) << 0;
 767
 768         u32 b;
 769         b = r_st(4) << 24;
 770         b |= r_st(5) << 16;
 771         b |= r_st(6) << 8;
 772         b |= r_st(7) << 0;
 773
 774         log_comment_add(
 775                 "       u32 mulsi3(\n"
 776                 "               u32 a = %u,\n"
 777                 "               u32 b = %u)\n",
 778                 a, b
 779         );
 780
 781         u64 m = a * b;
 782
 783         log_comment_add("\n");
 784         log_comment_add("       mul=%lu\n", m);
 785
 786         w_r3((m >> 24) & 0xff);
 787         w_r2((m >> 16) & 0xff);
 788         w_r1((m >> 8) & 0xff);
 789         w_r0((m >> 0) & 0xff);
 790
 791         return do_pop();
 792 }
 793
 794 static u16 udivmodhi4(u16 stack_ptr)
 795 {
 796         u16 a = (r_r3 << 8) | r_r2;
 797         u16 b = (r_st(0) << 8) | r_st(1);
 798
 799         log_comment_add(
 800                 "       u32 udivmodhi4(\n"
 801                 "               u16 a = %hu,\n"
 802                 "               u16 b = %hu)\n",
 803                 a, b
 804         );
 805
 806         u16 q = a / b;
 807         u16 r = a % b;
 808
 809         log_comment_add("\n");
 810         log_comment_add("       rem=%hu quot=%hu\n", r, q);
 811
 812         w_r3((r >> 8) & 0xff);
 813         w_r2((r >> 0) & 0xff);
 814         w_r1((q >> 8) & 0xff);
 815         w_r0((q >> 0) & 0xff);
 816
 817         return do_pop();
 818 }
 819
 820 static u16 divmodsi4(u16 stack_ptr)
 821 {
 822         u32 a;
 823         a = (r_r3 << 24) & 0xff;
 824         a |= (r_r2 << 16) & 0xff;
 825         a |= (r_r1 << 8) & 0xff;
 826         a |= (r_r0 << 0) & 0xff;
 827
 828         u32 b;
 829         b = r_st(0) << 24;
 830         b |= r_st(1) << 16;
 831         b |= r_st(2) << 8;
 832         b |= r_st(3) << 0;
 833
 834         log_comment_add(
 835                 "       u32 divmodsi4(\n"
 836                 "               u32 a = %u,\n"
 837                 "               u32 b = %u)\n",
 838                 a, b
 839         );
 840
 841         u32 q = a / b;
 842         u32 r = a % b;
 843
 844         log_comment_add("\n");
 845         log_comment_add("       rem=%u quot=%u\n", r, q);
 846
 847         w_r3((q >> 24) & 0xff);
 848         w_r2((q >> 16) & 0xff);
 849         w_r1((q >> 8) & 0xff);
 850         w_r0((q >> 0) & 0xff);
 851
 852         w_st(0, (r >> 24) & 0xff);
 853         w_st(1, (r >> 16) & 0xff);
 854         w_st(2, (r >> 8) & 0xff);
 855         w_st(3, (r >> 0) & 0xff);
 856
 857         return do_pop();
 858 }
 859
 860 static u16 divmodhi4(u16 stack_ptr)
 861 {
 862         s16 a = (r_r1 << 8) | r_r0;
 863         s16 b = (r_st(0) << 8) | r_st(1);
 864
 865         log_comment_add(
 866                 "       s32 divmodhi4(\n"
 867                 "               s16 a = %hi,\n"
 868                 "               s16 b = %hi)\n",
 869                 a, b
 870         );
 871
 872         s16 q = a / b;
 873         s16 r = a % b;
 874
 875         log_comment_add("\n");
 876         log_comment_add("       rem=%hi quot=%hi\n", r, q);
 877
 878         w_r3((r >> 8) & 0xff);
 879         w_r2((r >> 0) & 0xff);
 880         w_r1((q >> 8) & 0xff);
 881         w_r0((q >> 0) & 0xff);
 882
 883         return do_pop();
 884 }
 885
 886 ////////weird stuff
 887
 888 static u16 IrqROMHandler(u16 stack_ptr)
 889 {
 890         log_comment_add("       void IrqROMHandler(void)\n");
 891         log_comment_add("               unknown function, maybe rom irq handler, where to return?\n");
 892
 893         sim_breakpoint_set(SIM_BREAKPOINT_CODE);
 894
 895         return read_ip(1);
 896 }
 897
 898
 899 //TODO maybe bitfield of input params, output params
 900 //will be limited to 64 probably (u64 bitfield)
 901 #define FCN_OSTACK              (1U<<0)
 902 #define FCN_ISTACK(x)   ((x)<<1)
 903
 904 struct rom_fcn {
 905         u16 pc;
 906         u16 (*fcn_call)(u16);
 907         unsigned flags;
 908 };
 909
 910 static const struct rom_fcn fcns[] = {
 911         {0x8004, rom_execute,           0},
 912 //      {0x8005, smbMasterWrWord,       0},
 913         {0x8009, smbSlaveCmd,           0},
 914         {0x800a, smbSlaveRcvWord,       0},
 915         {0x800b, smbSlaveSndWord,       0},
 916         {0x800c, smbSlaveSndBlock,      0},
 917         {0x800d, smbSlaveRcvBlock,      0},
 918         {0x800e, smbSlaveWord,          0},
 919         {0x800f, smbSlaveBlock,         FCN_ISTACK(3)},
 920         {0x8012, smb_ACK,               0},
 921         {0x8013, smb_NACK,              0},
 922         {0x8014, FlashRdRow,    FCN_ISTACK(2)},
 923         {0x8017, SetAddr,               0},             //calls+jmp
 924         {0x8018, PokeByte,              0},             //calls+jmp
 925         {0x8019, PeekByte,              0},             //calls+jmp
 926         {0x801a, ReadRAMBlk,    0},             //calls+jmp
 927         //math
 928         {0x801c, mulhisi3,              0},
 929         {0x801d, umulhisi3,             0},
 930         {0x801e, mulsi3,                FCN_ISTACK(8)},
 931         {0x8020, divmodhi4,             FCN_ISTACK(2)},
 932         {0x8021, udivmodhi4,    FCN_ISTACK(2)},
 933         {0x8022, divmodsi4,             FCN_ISTACK(4) | FCN_OSTACK},
 934         //more fcn
 935         {0x8037, FdataProgRow,          FCN_ISTACK(2)},
 936         {0x8038, FdataProgWord,         0},
 937         {0x8039, FdataEraseRow,         0},
 938         {0x803a, FdataMassErase,        0},
 939         {0x803b, I2CReadBlock,          FCN_ISTACK(2)},
 940         {0x803c, I2CWriteBlock,         FCN_ISTACK(2)},
 941 //      {0x803d, I2CDeviceAvail,        0},
 942 //      {0x803e, I2CCompareBlock,       0},
 943         {0x8043, smbWaitBusFree,        0},
 944 //
 945 //      //found calls
 946 //      {0x8040, rsvd/tramp?/irq6+},
 947         {0x8040, IrqROMHandler,         0},
 948
 949         //last entry, just dump stack
 950         {0, NULL, FCN_ISTACK(8)},
 951 };
 952
 953
 954 /*
 955  * call ABI
 956  *
 957  * args: r3, r2, r1, r0, stack 0, stack 1, 2 3 4...
 958  * (i3 = stack)
 959  * rets: r2 (lsb), r3 (msb)
 960  *
 961  * NOTICE R0 can be skipped if u16 not aligned?
 962  */
 963
 964 u16 check_rom_funcs(u16 pc)
 965 {
 966         if (pc < 0x8000)
 967                 return pc;
 968
 969         log_comment_add("=== ROM routines: ===\n");
 970
 971         unsigned flag_hit = 0;
 972         u16 sw_stack_ptr = read_ix(I_REG_3, 1);
 973         unsigned idx = 0;
 974
 975         log_comment_add("       Input: r3=%02hhx r2=%02hhx r1=%02hhx r0=%02hhx ",
 976                 read_reg8(MAIN_REG_R3, 0),
 977                 read_reg8(MAIN_REG_R2, 0),
 978                 read_reg8(MAIN_REG_R1, 0),
 979                 read_reg8(MAIN_REG_R0, 0)
 980         );
 981
 982
 983         idx = 0;
 984         while (1) {
 985                 if ((fcns[idx].pc == pc) || (fcns[idx].pc == 0)) {
 986                         for (
 987                                 unsigned sti=0;
 988                                 sti<((fcns[idx].flags >> 1) & 0xf);
 989                                 sti++)
 990                         {
 991                                 log_comment_add("st[%i]=%02hhx ",
 992                                         sti, read_mem(sw_stack_ptr + sti, 0)
 993                                 );
 994                         }
 995                         log_comment_add("\n\n");
 996
 997                         if (fcns[idx].pc == 0)
 998                                 break;
 999
1000                         pc = fcns[idx].fcn_call(sw_stack_ptr);
1001
1002                         //changed
1003                         log_comment_add("\n");
1004                         log_comment_add("       Output: r3=%02hhx r2=%02hhx r1=%02hhx r0=%02hhx ",
1005                                 read_reg8(MAIN_REG_R3, 0),
1006                                 read_reg8(MAIN_REG_R2, 0),
1007                                 read_reg8(MAIN_REG_R1, 0),
1008                                 read_reg8(MAIN_REG_R0, 0)
1009                         );
1010
1011                         if (fcns[idx].flags & 1) {
1012                                 for (unsigned sti=0;sti<4;sti++) {
1013                                         log_comment_add("st[%i]=%02hhx ",
1014                                                 sti, read_mem(sw_stack_ptr + sti, 0)
1015                                         );
1016                                 }
1017                         }
1018                         log_comment_add("\n\n");
1019
1020                         flag_hit = 1;
1021                         break;
1022                 }
1023                 idx++;
1024         }
1025
1026         if (flag_hit == 0) {
1027                 log_comment_add("       !!unknown routine @%04hx()\n", pc);
1028
1029                 sim_breakpoint_set(SIM_BREAKPOINT_CODE);
1030
1031                 pc = read_ip(1);
1032         }
1033
1034         return pc;
1035 }
1036