git.gag.com Git - fw/openocd/blob - src/target/x86_32_common.c

   1 /*
   2  * Copyright(c) 2013 Intel Corporation.
   3  *
   4  * Adrian Burns (adrian.burns@intel.com)
   5  * Thomas Faust (thomas.faust@intel.com)
   6  * Ivan De Cesaris (ivan.de.cesaris@intel.com)
   7  * Julien Carreno (julien.carreno@intel.com)
   8  * Jeffrey Maxwell (jeffrey.r.maxwell@intel.com)
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License as published by
  12  * the Free Software Foundation; either version 2 of the License, or
  13  * (at your option) any later version.
  14  *
  15  * This program is distributed in the hope that it will be useful, but
  16  * WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18  * General Public License for more details.
  19  *
  20  * You should have received a copy of the GNU General Public License
  21  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  22  *
  23  * Contact Information:
  24  * Intel Corporation
  25  */
  26
  27 /*
  28  * @file
  29  * This implements generic x86 32 bit memory and breakpoint operations.
  30  */
  31
  32 #ifdef HAVE_CONFIG_H
  33 #include "config.h"
  34 #endif
  35
  36 #include <helper/log.h>
  37
  38 #include "target.h"
  39 #include "target_type.h"
  40 #include "register.h"
  41 #include "breakpoints.h"
  42 #include "x86_32_common.h"
  43
  44 static int set_debug_regs(struct target *t, uint32_t address,
  45                         uint8_t bp_num, uint8_t bp_type, uint8_t bp_length);
  46 static int unset_debug_regs(struct target *t, uint8_t bp_num);
  47 static int read_mem(struct target *t, uint32_t size,
  48                         uint32_t addr, uint8_t *buf);
  49 static int write_mem(struct target *t, uint32_t size,
  50                         uint32_t addr, const uint8_t *buf);
  51 static int calcaddr_physfromlin(struct target *t, target_addr_t addr,
  52                         target_addr_t *physaddr);
  53 static int read_phys_mem(struct target *t, uint32_t phys_address,
  54                         uint32_t size, uint32_t count, uint8_t *buffer);
  55 static int write_phys_mem(struct target *t, uint32_t phys_address,
  56                         uint32_t size, uint32_t count, const uint8_t *buffer);
  57 static int set_breakpoint(struct target *target,
  58                         struct breakpoint *breakpoint);
  59 static int unset_breakpoint(struct target *target,
  60                         struct breakpoint *breakpoint);
  61 static int set_watchpoint(struct target *target,
  62                         struct watchpoint *watchpoint);
  63 static int unset_watchpoint(struct target *target,
  64                         struct watchpoint *watchpoint);
  65 static int read_hw_reg_to_cache(struct target *t, int num);
  66 static int write_hw_reg_from_cache(struct target *t, int num);
  67
  68 int x86_32_get_gdb_reg_list(struct target *t,
  69                         struct reg **reg_list[], int *reg_list_size,
  70                         enum target_register_class reg_class)
  71 {
  72
  73         struct x86_32_common *x86_32 = target_to_x86_32(t);
  74         int i;
  75         *reg_list_size = x86_32->cache->num_regs;
  76         LOG_DEBUG("num_regs=%d, reg_class=%d", (*reg_list_size), reg_class);
  77         *reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
  78         if (*reg_list == NULL) {
  79                 LOG_ERROR("%s out of memory", __func__);
  80                 return ERROR_FAIL;
  81         }
  82         /* this will copy the values from our reg list to gdbs */
  83         for (i = 0; i < (*reg_list_size); i++) {
  84                 (*reg_list)[i] = &x86_32->cache->reg_list[i];
  85                 LOG_DEBUG("value %s = %08" PRIx32, x86_32->cache->reg_list[i].name,
  86                                 buf_get_u32(x86_32->cache->reg_list[i].value, 0, 32));
  87         }
  88         return ERROR_OK;
  89 }
  90
  91 int x86_32_common_init_arch_info(struct target *t, struct x86_32_common *x86_32)
  92 {
  93         t->arch_info = x86_32;
  94         x86_32->common_magic = X86_32_COMMON_MAGIC;
  95         x86_32->num_hw_bpoints = MAX_DEBUG_REGS;
  96         x86_32->hw_break_list = calloc(x86_32->num_hw_bpoints,
  97                                 sizeof(struct x86_32_dbg_reg));
  98         if (x86_32->hw_break_list == NULL) {
  99                 LOG_ERROR("%s out of memory", __func__);
 100                 return ERROR_FAIL;
 101         }
 102         x86_32->curr_tap = t->tap;
 103         x86_32->fast_data_area = NULL;
 104         x86_32->flush = 1;
 105         x86_32->read_hw_reg_to_cache = read_hw_reg_to_cache;
 106         x86_32->write_hw_reg_from_cache = write_hw_reg_from_cache;
 107         return ERROR_OK;
 108 }
 109
 110 int x86_32_common_mmu(struct target *t, int *enabled)
 111 {
 112         *enabled = true;
 113         return ERROR_OK;
 114 }
 115
 116 int x86_32_common_virt2phys(struct target *t, target_addr_t address, target_addr_t *physical)
 117 {
 118         struct x86_32_common *x86_32 = target_to_x86_32(t);
 119
 120         /*
 121          * We need to ignore 'segmentation' for now, as OpenOCD can't handle
 122          * segmented addresses.
 123          * In protected mode that is almost OK, as (almost) any known OS is using
 124          * flat segmentation. In real mode we use use the base of the DS segment,
 125          * as we don't know better ...
 126          */
 127
 128         uint32_t cr0 = buf_get_u32(x86_32->cache->reg_list[CR0].value, 0, 32);
 129         if (!(cr0 & CR0_PG)) {
 130                 /* target halted in real mode */
 131                 /* TODO: needs validation !!! */
 132                 uint32_t dsb = buf_get_u32(x86_32->cache->reg_list[DSB].value, 0, 32);
 133                 *physical = dsb + address;
 134
 135         } else {
 136                 /* target halted in protected mode */
 137                 if (calcaddr_physfromlin(t, address, physical) != ERROR_OK) {
 138                         LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 139                                         __func__, address);
 140                         return ERROR_FAIL;
 141                 }
 142         }
 143         return ERROR_OK;
 144 }
 145
 146 int x86_32_common_read_phys_mem(struct target *t, target_addr_t phys_address,
 147                         uint32_t size, uint32_t count, uint8_t *buffer)
 148 {
 149         struct x86_32_common *x86_32 = target_to_x86_32(t);
 150         int error;
 151
 152         error = read_phys_mem(t, phys_address, size, count, buffer);
 153         if (error != ERROR_OK)
 154                 return error;
 155
 156         /* After reading memory from target, we must replace software breakpoints
 157          * with the original instructions again.
 158          */
 159         struct swbp_mem_patch *iter = x86_32->swbbp_mem_patch_list;
 160         while (iter != NULL) {
 161                 if (iter->physaddr >= phys_address && iter->physaddr < phys_address+(size*count)) {
 162                         uint32_t offset = iter->physaddr - phys_address;
 163                         buffer[offset] = iter->orig_byte;
 164                 }
 165                 iter = iter->next;
 166         }
 167         return ERROR_OK;
 168 }
 169
 170 static int read_phys_mem(struct target *t, uint32_t phys_address,
 171                         uint32_t size, uint32_t count, uint8_t *buffer)
 172 {
 173         int retval = ERROR_OK;
 174         bool pg_disabled = false;
 175         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 176                         phys_address, size, count, buffer);
 177         struct x86_32_common *x86_32 = target_to_x86_32(t);
 178
 179         if (check_not_halted(t))
 180                 return ERROR_TARGET_NOT_HALTED;
 181         if (!count || !buffer || !phys_address) {
 182                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
 183                                 __func__, count, buffer, phys_address);
 184                 return ERROR_COMMAND_ARGUMENT_INVALID;
 185         }
 186
 187         /* to access physical memory, switch off the CR0.PG bit */
 188         if (x86_32->is_paging_enabled(t)) {
 189                 retval = x86_32->disable_paging(t);
 190                 if (retval != ERROR_OK) {
 191                         LOG_ERROR("%s could not disable paging", __func__);
 192                         return retval;
 193                 }
 194                 pg_disabled = true;
 195         }
 196
 197         for (uint32_t i = 0; i < count; i++) {
 198                 switch (size) {
 199                 case BYTE:
 200                         retval = read_mem(t, size, phys_address + i, buffer + i);
 201                         break;
 202                 case WORD:
 203                         retval = read_mem(t, size, phys_address + i * 2, buffer + i * 2);
 204                         break;
 205                 case DWORD:
 206                         retval = read_mem(t, size, phys_address + i * 4, buffer + i * 4);
 207                         break;
 208                 default:
 209                         LOG_ERROR("%s invalid read size", __func__);
 210                         break;
 211                 }
 212         }
 213         /* restore CR0.PG bit if needed (regardless of retval) */
 214         if (pg_disabled) {
 215                 retval = x86_32->enable_paging(t);
 216                 if (retval != ERROR_OK) {
 217                         LOG_ERROR("%s could not enable paging", __func__);
 218                         return retval;
 219                 }
 220                 pg_disabled = true;
 221         }
 222         /* TODO: After reading memory from target, we must replace
 223          * software breakpoints with the original instructions again.
 224          * Solve this with the breakpoint fix
 225          */
 226         return retval;
 227 }
 228
 229 int x86_32_common_write_phys_mem(struct target *t, target_addr_t phys_address,
 230                         uint32_t size, uint32_t count, const uint8_t *buffer)
 231 {
 232         struct x86_32_common *x86_32 = target_to_x86_32(t);
 233         int error = ERROR_OK;
 234         uint8_t *newbuffer = NULL;
 235
 236         check_not_halted(t);
 237         if (!count || !buffer || !phys_address) {
 238                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 239                                 __func__, count, buffer, phys_address);
 240                 return ERROR_COMMAND_ARGUMENT_INVALID;
 241         }
 242         /* Before writing memory to target, we must update software breakpoints
 243          * with the new instructions and patch the memory buffer with the
 244          * breakpoint instruction.
 245          */
 246         newbuffer = malloc(size*count);
 247         if (newbuffer == NULL) {
 248                 LOG_ERROR("%s out of memory", __func__);
 249                 return ERROR_FAIL;
 250         }
 251         memcpy(newbuffer, buffer, size*count);
 252         struct swbp_mem_patch *iter = x86_32->swbbp_mem_patch_list;
 253         while (iter != NULL) {
 254                 if (iter->physaddr >= phys_address && iter->physaddr < phys_address+(size*count)) {
 255                         uint32_t offset = iter->physaddr - phys_address;
 256                         newbuffer[offset] = SW_BP_OPCODE;
 257
 258                         /* update the breakpoint */
 259                         struct breakpoint *pbiter = t->breakpoints;
 260                         while (pbiter != NULL && pbiter->unique_id != iter->swbp_unique_id)
 261                                 pbiter = pbiter->next;
 262                         if (pbiter)
 263                                 pbiter->orig_instr[0] = buffer[offset];
 264                 }
 265                 iter = iter->next;
 266         }
 267
 268         error = write_phys_mem(t, phys_address, size, count, newbuffer);
 269         free(newbuffer);
 270         return error;
 271 }
 272
 273 static int write_phys_mem(struct target *t, uint32_t phys_address,
 274                         uint32_t size, uint32_t count, const uint8_t *buffer)
 275 {
 276         int retval = ERROR_OK;
 277         bool pg_disabled = false;
 278         struct x86_32_common *x86_32 = target_to_x86_32(t);
 279         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 280                         phys_address, size, count, buffer);
 281
 282         check_not_halted(t);
 283         if (!count || !buffer || !phys_address) {
 284                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
 285                                 __func__, count, buffer, phys_address);
 286                 return ERROR_COMMAND_ARGUMENT_INVALID;
 287         }
 288         /* TODO: Before writing memory to target, we must update
 289          * software breakpoints with the new instructions and
 290          * patch the memory buffer with the breakpoint instruction.
 291          * Solve this with the breakpoint fix
 292          */
 293
 294         /* to access physical memory, switch off the CR0.PG bit */
 295         if (x86_32->is_paging_enabled(t)) {
 296                 retval = x86_32->disable_paging(t);
 297                 if (retval != ERROR_OK) {
 298                         LOG_ERROR("%s could not disable paging", __func__);
 299                         return retval;
 300                 }
 301                 pg_disabled = true;
 302         }
 303         for (uint32_t i = 0; i < count; i++) {
 304                 switch (size) {
 305                 case BYTE:
 306                         retval = write_mem(t, size, phys_address + i, buffer + i);
 307                         break;
 308                 case WORD:
 309                         retval = write_mem(t, size, phys_address + i * 2, buffer + i * 2);
 310                         break;
 311                 case DWORD:
 312                         retval = write_mem(t, size, phys_address + i * 4, buffer + i * 4);
 313                         break;
 314                 default:
 315                         LOG_DEBUG("invalid read size");
 316                         break;
 317                 }
 318         }
 319         /* restore CR0.PG bit if needed (regardless of retval) */
 320         if (pg_disabled) {
 321                 retval = x86_32->enable_paging(t);
 322                 if (retval != ERROR_OK) {
 323                         LOG_ERROR("%s could not enable paging", __func__);
 324                         return retval;
 325                 }
 326         }
 327         return retval;
 328 }
 329
 330 static int read_mem(struct target *t, uint32_t size,
 331                         uint32_t addr, uint8_t *buf)
 332 {
 333         struct x86_32_common *x86_32 = target_to_x86_32(t);
 334
 335         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
 336         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
 337         int retval = x86_32->write_hw_reg(t, EAX, addr, 0);
 338         if (retval != ERROR_OK) {
 339                 LOG_ERROR("%s error write EAX", __func__);
 340                 return retval;
 341         }
 342
 343         switch (size) {
 344                 case BYTE:
 345                         if (use32)
 346                                 retval = x86_32->submit_instruction(t, MEMRDB32);
 347                         else
 348                                 retval = x86_32->submit_instruction(t, MEMRDB16);
 349                         break;
 350                 case WORD:
 351                         if (use32)
 352                                 retval = x86_32->submit_instruction(t, MEMRDH32);
 353                         else
 354                                 retval = x86_32->submit_instruction(t, MEMRDH16);
 355                         break;
 356                 case DWORD:
 357                         if (use32)
 358                                 retval = x86_32->submit_instruction(t, MEMRDW32);
 359                         else
 360                                 retval = x86_32->submit_instruction(t, MEMRDW16);
 361                         break;
 362                 default:
 363                         LOG_ERROR("%s invalid read mem size", __func__);
 364                         break;
 365         }
 366
 367         /* read_hw_reg() will write to 4 bytes (uint32_t)
 368          * Watch out, the buffer passed into read_mem() might be 1 or 2 bytes.
 369          */
 370         uint32_t regval;
 371         retval = x86_32->read_hw_reg(t, EDX, &regval, 0);
 372
 373         if (retval != ERROR_OK) {
 374                 LOG_ERROR("%s error read EDX", __func__);
 375                 return retval;
 376         }
 377         for (uint8_t i = 0; i < size; i++)
 378                 buf[i] = (regval >> (i*8)) & 0x000000FF;
 379
 380         retval = x86_32->transaction_status(t);
 381         if (retval != ERROR_OK) {
 382                 LOG_ERROR("%s error on mem read", __func__);
 383                 return retval;
 384         }
 385         return retval;
 386 }
 387
 388 static int write_mem(struct target *t, uint32_t size,
 389                         uint32_t addr, const uint8_t *buf)
 390 {
 391         uint32_t i = 0;
 392         uint32_t buf4bytes = 0;
 393         int retval = ERROR_OK;
 394         struct x86_32_common *x86_32 = target_to_x86_32(t);
 395
 396         for (i = 0; i < size; ++i) {
 397                 buf4bytes = buf4bytes << 8; /* first time we only shift 0s */
 398                 buf4bytes += buf[(size-1)-i]; /* it was hard to write, should be hard to read! */
 399         }
 400         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
 401         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
 402         retval = x86_32->write_hw_reg(t, EAX, addr, 0);
 403         if (retval != ERROR_OK) {
 404                 LOG_ERROR("%s error write EAX", __func__);
 405                 return retval;
 406         }
 407
 408         /* write_hw_reg() will write to 4 bytes (uint32_t)
 409          * Watch out, the buffer passed into write_mem() might be 1 or 2 bytes.
 410          */
 411         retval = x86_32->write_hw_reg(t, EDX, buf4bytes, 0);
 412         if (retval != ERROR_OK) {
 413                 LOG_ERROR("%s error write EDX", __func__);
 414                 return retval;
 415         }
 416         switch (size) {
 417                 case BYTE:
 418                         if (use32)
 419                                 retval = x86_32->submit_instruction(t, MEMWRB32);
 420                         else
 421                                 retval = x86_32->submit_instruction(t, MEMWRB16);
 422                         break;
 423                 case WORD:
 424                         if (use32)
 425                                 retval = x86_32->submit_instruction(t, MEMWRH32);
 426                         else
 427                                 retval = x86_32->submit_instruction(t, MEMWRH16);
 428                         break;
 429                 case DWORD:
 430                         if (use32)
 431                                 retval = x86_32->submit_instruction(t, MEMWRW32);
 432                         else
 433                                 retval = x86_32->submit_instruction(t, MEMWRW16);
 434                         break;
 435                 default:
 436                         LOG_ERROR("%s invalid write mem size", __func__);
 437                         return ERROR_FAIL;
 438         }
 439         retval = x86_32->transaction_status(t);
 440         if (retval != ERROR_OK) {
 441                 LOG_ERROR("%s error on mem write", __func__);
 442                 return retval;
 443         }
 444         return retval;
 445 }
 446
 447 int calcaddr_physfromlin(struct target *t, target_addr_t addr, target_addr_t *physaddr)
 448 {
 449         uint8_t entry_buffer[8];
 450
 451         if (physaddr == NULL || t == NULL)
 452                 return ERROR_FAIL;
 453
 454         struct x86_32_common *x86_32 = target_to_x86_32(t);
 455
 456         /* The 'user-visible' CR0.PG should be set - otherwise the function shouldn't be called
 457          * (Don't check the CR0.PG on the target, this might be temporally disabled at this point)
 458          */
 459         uint32_t cr0 = buf_get_u32(x86_32->cache->reg_list[CR0].value, 0, 32);
 460         if (!(cr0 & CR0_PG)) {
 461                 /* you are wrong in this function, never mind */
 462                 *physaddr = addr;
 463                 return ERROR_OK;
 464         }
 465
 466         uint32_t cr4 = buf_get_u32(x86_32->cache->reg_list[CR4].value, 0, 32);
 467         bool isPAE = cr4 & 0x00000020; /* PAE - Physical Address Extension */
 468
 469         uint32_t cr3 = buf_get_u32(x86_32->cache->reg_list[CR3].value, 0, 32);
 470         if (isPAE) {
 471                 uint32_t pdpt_base = cr3 & 0xFFFFF000; /* lower 12 bits of CR3 must always be 0 */
 472                 uint32_t pdpt_index = (addr & 0xC0000000) >> 30; /* A[31:30] index to PDPT */
 473                 uint32_t pdpt_addr = pdpt_base + (8 * pdpt_index);
 474                 if (x86_32_common_read_phys_mem(t, pdpt_addr, 4, 2, entry_buffer) != ERROR_OK) {
 475                         LOG_ERROR("%s couldn't read page directory pointer table entry at 0x%08" PRIx32,
 476                                         __func__, pdpt_addr);
 477                         return ERROR_FAIL;
 478                 }
 479                 uint64_t pdpt_entry = target_buffer_get_u64(t, entry_buffer);
 480                 if (!(pdpt_entry & 0x0000000000000001)) {
 481                         LOG_ERROR("%s page directory pointer table entry at 0x%08" PRIx32 " is not present",
 482                                         __func__, pdpt_addr);
 483                         return ERROR_FAIL;
 484                 }
 485
 486                 uint32_t pd_base = pdpt_entry & 0xFFFFF000; /* A[31:12] is PageTable/Page Base Address */
 487                 uint32_t pd_index = (addr & 0x3FE00000) >> 21; /* A[29:21] index to PD entry with PAE */
 488                 uint32_t pd_addr = pd_base + (8 * pd_index);
 489                 if (x86_32_common_read_phys_mem(t, pd_addr, 4, 2, entry_buffer) != ERROR_OK) {
 490                         LOG_ERROR("%s couldn't read page directory entry at 0x%08" PRIx32,
 491                                         __func__, pd_addr);
 492                         return ERROR_FAIL;
 493                 }
 494                 uint64_t pd_entry = target_buffer_get_u64(t, entry_buffer);
 495                 if (!(pd_entry & 0x0000000000000001)) {
 496                         LOG_ERROR("%s page directory entry at 0x%08" PRIx32 " is not present",
 497                                         __func__, pd_addr);
 498                         return ERROR_FAIL;
 499                 }
 500
 501                 /* PS bit in PD entry is indicating 4KB or 2MB page size */
 502                 if (pd_entry & 0x0000000000000080) {
 503
 504                         uint32_t page_base = (uint32_t)(pd_entry & 0x00000000FFE00000); /* [31:21] */
 505                         uint32_t offset = addr & 0x001FFFFF; /* [20:0] */
 506                         *physaddr = page_base + offset;
 507                         return ERROR_OK;
 508
 509                 } else {
 510
 511                         uint32_t pt_base = (uint32_t)(pd_entry & 0x00000000FFFFF000); /*[31:12]*/
 512                         uint32_t pt_index = (addr & 0x001FF000) >> 12; /*[20:12]*/
 513                         uint32_t pt_addr = pt_base + (8 * pt_index);
 514                         if (x86_32_common_read_phys_mem(t, pt_addr, 4, 2, entry_buffer) != ERROR_OK) {
 515                                 LOG_ERROR("%s couldn't read page table entry at 0x%08" PRIx32, __func__, pt_addr);
 516                                 return ERROR_FAIL;
 517                         }
 518                         uint64_t pt_entry = target_buffer_get_u64(t, entry_buffer);
 519                         if (!(pt_entry & 0x0000000000000001)) {
 520                                 LOG_ERROR("%s page table entry at 0x%08" PRIx32 " is not present", __func__, pt_addr);
 521                                 return ERROR_FAIL;
 522                         }
 523
 524                         uint32_t page_base = (uint32_t)(pt_entry & 0x00000000FFFFF000); /*[31:12]*/
 525                         uint32_t offset =  addr & 0x00000FFF; /*[11:0]*/
 526                         *physaddr = page_base + offset;
 527                         return ERROR_OK;
 528                 }
 529         } else {
 530                 uint32_t pd_base = cr3 & 0xFFFFF000; /* lower 12 bits of CR3 must always be 0 */
 531                 uint32_t pd_index = (addr & 0xFFC00000) >> 22; /* A[31:22] index to PD entry */
 532                 uint32_t pd_addr = pd_base + (4 * pd_index);
 533                 if (x86_32_common_read_phys_mem(t, pd_addr, 4, 1, entry_buffer) != ERROR_OK) {
 534                         LOG_ERROR("%s couldn't read page directory entry at 0x%08" PRIx32, __func__, pd_addr);
 535                         return ERROR_FAIL;
 536                 }
 537                 uint32_t pd_entry = target_buffer_get_u32(t, entry_buffer);
 538                 if (!(pd_entry & 0x00000001)) {
 539                         LOG_ERROR("%s page directory entry at 0x%08" PRIx32 " is not present", __func__, pd_addr);
 540                         return ERROR_FAIL;
 541                 }
 542
 543                 /* Bit 7 in page directory entry is page size.
 544                  */
 545                 if (pd_entry & 0x00000080) {
 546                         /* 4MB pages */
 547                         uint32_t page_base = pd_entry & 0xFFC00000;
 548                         *physaddr = page_base + (addr & 0x003FFFFF);
 549
 550                 } else {
 551                         /* 4KB pages */
 552                         uint32_t pt_base = pd_entry & 0xFFFFF000; /* A[31:12] is PageTable/Page Base Address */
 553                         uint32_t pt_index = (addr & 0x003FF000) >> 12; /* A[21:12] index to page table entry */
 554                         uint32_t pt_addr = pt_base + (4 * pt_index);
 555                         if (x86_32_common_read_phys_mem(t, pt_addr, 4, 1, entry_buffer) != ERROR_OK) {
 556                                 LOG_ERROR("%s couldn't read page table entry at 0x%08" PRIx32, __func__, pt_addr);
 557                                 return ERROR_FAIL;
 558                         }
 559                         uint32_t pt_entry = target_buffer_get_u32(t, entry_buffer);
 560                         if (!(pt_entry & 0x00000001)) {
 561                                 LOG_ERROR("%s page table entry at 0x%08" PRIx32 " is not present", __func__, pt_addr);
 562                                 return ERROR_FAIL;
 563                         }
 564                         uint32_t page_base = pt_entry & 0xFFFFF000; /* A[31:12] is PageTable/Page Base Address */
 565                         *physaddr = page_base + (addr & 0x00000FFF); /* A[11:0] offset to 4KB page in linear address */
 566                 }
 567         }
 568         return ERROR_OK;
 569 }
 570
 571 int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 572                         uint32_t size, uint32_t count, uint8_t *buf)
 573 {
 574         int retval = ERROR_OK;
 575         struct x86_32_common *x86_32 = target_to_x86_32(t);
 576         LOG_DEBUG("addr=" TARGET_ADDR_FMT ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 577                         addr, size, count, buf);
 578         check_not_halted(t);
 579         if (!count || !buf || !addr) {
 580                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 581                                 __func__, count, buf, addr);
 582                 return ERROR_COMMAND_ARGUMENT_INVALID;
 583         }
 584
 585         if (x86_32->is_paging_enabled(t)) {
 586                 /* all memory accesses from debugger must be physical (CR0.PG == 0)
 587                  * conversion to physical address space needed
 588                  */
 589                 retval = x86_32->disable_paging(t);
 590                 if (retval != ERROR_OK) {
 591                         LOG_ERROR("%s could not disable paging", __func__);
 592                         return retval;
 593                 }
 594                 target_addr_t physaddr = 0;
 595                 if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
 596                         LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 597                                           __func__, addr);
 598                         retval = ERROR_FAIL;
 599                 }
 600                 /* TODO: !!! Watch out for page boundaries
 601                  * for every 4kB, the physical address has to be re-calculated
 602                  * This should be fixed together with bulk memory reads
 603                  */
 604
 605                 if (retval == ERROR_OK
 606                         && x86_32_common_read_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
 607                         LOG_ERROR("%s failed to read memory from physical address " TARGET_ADDR_FMT,
 608                                           __func__, physaddr);
 609                         retval = ERROR_FAIL;
 610                 }
 611                 /* restore PG bit if it was cleared prior (regardless of retval) */
 612                 retval = x86_32->enable_paging(t);
 613                 if (retval != ERROR_OK) {
 614                         LOG_ERROR("%s could not enable paging", __func__);
 615                         return retval;
 616                 }
 617         } else {
 618                 /* paging is off - linear address is physical address */
 619                 if (x86_32_common_read_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
 620                         LOG_ERROR("%s failed to read memory from address " TARGET_ADDR_FMT,
 621                                           __func__, addr);
 622                         retval = ERROR_FAIL;
 623                 }
 624         }
 625
 626         return retval;
 627 }
 628
 629 int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 630                         uint32_t size, uint32_t count, const uint8_t *buf)
 631 {
 632         int retval = ERROR_OK;
 633         struct x86_32_common *x86_32 = target_to_x86_32(t);
 634         LOG_DEBUG("addr=" TARGET_ADDR_FMT ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 635                         addr, size, count, buf);
 636         check_not_halted(t);
 637         if (!count || !buf || !addr) {
 638                 LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 639                                         __func__, count, buf, addr);
 640                 return ERROR_COMMAND_ARGUMENT_INVALID;
 641         }
 642         if (x86_32->is_paging_enabled(t)) {
 643                 /* all memory accesses from debugger must be physical (CR0.PG == 0)
 644                  * conversion to physical address space needed
 645                  */
 646                 retval = x86_32->disable_paging(t);
 647                 if (retval != ERROR_OK) {
 648                         LOG_ERROR("%s could not disable paging", __func__);
 649                         return retval;
 650                 }
 651                 target_addr_t physaddr = 0;
 652                 if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
 653                         LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 654                                         __func__, addr);
 655                         retval = ERROR_FAIL;
 656                 }
 657                 /* TODO: !!! Watch out for page boundaries
 658                  * for every 4kB, the physical address has to be re-calculated
 659                  * This should be fixed together with bulk memory reads
 660                  */
 661                 if (retval == ERROR_OK
 662                         && x86_32_common_write_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
 663                         LOG_ERROR("%s failed to write memory to physical address " TARGET_ADDR_FMT,
 664                                         __func__, physaddr);
 665                         retval = ERROR_FAIL;
 666                 }
 667                 /* restore PG bit if it was cleared prior (regardless of retval) */
 668                 retval = x86_32->enable_paging(t);
 669                 if (retval != ERROR_OK) {
 670                         LOG_ERROR("%s could not enable paging", __func__);
 671                         return retval;
 672                 }
 673         } else {
 674
 675                 /* paging is off - linear address is physical address */
 676                 if (x86_32_common_write_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
 677                         LOG_ERROR("%s failed to write memory to address " TARGET_ADDR_FMT,
 678                                         __func__, addr);
 679                         retval = ERROR_FAIL;
 680                 }
 681         }
 682         return retval;
 683 }
 684
 685 int x86_32_common_read_io(struct target *t, uint32_t addr,
 686                         uint32_t size, uint8_t *buf)
 687 {
 688         struct x86_32_common *x86_32 = target_to_x86_32(t);
 689         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
 690         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
 691         int retval = ERROR_FAIL;
 692         bool pg_disabled = false;
 693         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", buf=%p", addr, size, buf);
 694         check_not_halted(t);
 695         if (!buf || !addr) {
 696                 LOG_ERROR("%s invalid params buf=%p, addr=%08" PRIx32, __func__, buf, addr);
 697                 return retval;
 698         }
 699         retval = x86_32->write_hw_reg(t, EDX, addr, 0);
 700         if (retval != ERROR_OK) {
 701                 LOG_ERROR("%s error EDX write", __func__);
 702                 return retval;
 703         }
 704         /* to access physical memory, switch off the CR0.PG bit */
 705         if (x86_32->is_paging_enabled(t)) {
 706                 retval = x86_32->disable_paging(t);
 707                 if (retval != ERROR_OK) {
 708                         LOG_ERROR("%s could not disable paging", __func__);
 709                         return retval;
 710                 }
 711                 pg_disabled = true;
 712         }
 713         switch (size) {
 714                 case BYTE:
 715                         if (use32)
 716                                 retval = x86_32->submit_instruction(t, IORDB32);
 717                         else
 718                                 retval = x86_32->submit_instruction(t, IORDB16);
 719                         break;
 720                 case WORD:
 721                         if (use32)
 722                                 retval = x86_32->submit_instruction(t, IORDH32);
 723                         else
 724                                 retval = x86_32->submit_instruction(t, IORDH16);
 725                         break;
 726                 case DWORD:
 727                         if (use32)
 728                                 retval = x86_32->submit_instruction(t, IORDW32);
 729                         else
 730                                 retval = x86_32->submit_instruction(t, IORDW16);
 731                         break;
 732                 default:
 733                         LOG_ERROR("%s invalid read io size", __func__);
 734                         return ERROR_FAIL;
 735         }
 736         /* restore CR0.PG bit if needed */
 737         if (pg_disabled) {
 738                 retval = x86_32->enable_paging(t);
 739                 if (retval != ERROR_OK) {
 740                         LOG_ERROR("%s could not enable paging", __func__);
 741                         return retval;
 742                 }
 743                 pg_disabled = false;
 744         }
 745         uint32_t regval = 0;
 746         retval = x86_32->read_hw_reg(t, EAX, &regval, 0);
 747         if (retval != ERROR_OK) {
 748                 LOG_ERROR("%s error on read EAX", __func__);
 749                 return retval;
 750         }
 751         for (uint8_t i = 0; i < size; i++)
 752                 buf[i] = (regval >> (i*8)) & 0x000000FF;
 753         retval = x86_32->transaction_status(t);
 754         if (retval != ERROR_OK) {
 755                 LOG_ERROR("%s error on io read", __func__);
 756                 return retval;
 757         }
 758         return retval;
 759 }
 760
 761 int x86_32_common_write_io(struct target *t, uint32_t addr,
 762                         uint32_t size, const uint8_t *buf)
 763 {
 764         struct x86_32_common *x86_32 = target_to_x86_32(t);
 765         /* if CS.D bit=1 then its a 32 bit code segment, else 16 */
 766         bool use32 = (buf_get_u32(x86_32->cache->reg_list[CSAR].value, 0, 32)) & CSAR_D;
 767         LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", buf=%p", addr, size, buf);
 768         check_not_halted(t);
 769         int retval = ERROR_FAIL;
 770         bool pg_disabled = false;
 771         if (!buf || !addr) {
 772                 LOG_ERROR("%s invalid params buf=%p, addr=0x%08" PRIx32, __func__, buf, addr);
 773                 return retval;
 774         }
 775         /* no do the write */
 776         retval = x86_32->write_hw_reg(t, EDX, addr, 0);
 777         if (retval != ERROR_OK) {
 778                 LOG_ERROR("%s error on EDX write", __func__);
 779                 return retval;
 780         }
 781         uint32_t regval = 0;
 782         for (uint8_t i = 0; i < size; i++)
 783                 regval += (buf[i] << (i*8));
 784         retval = x86_32->write_hw_reg(t, EAX, regval, 0);
 785         if (retval != ERROR_OK) {
 786                 LOG_ERROR("%s error on EAX write", __func__);
 787                 return retval;
 788         }
 789         /* to access physical memory, switch off the CR0.PG bit */
 790         if (x86_32->is_paging_enabled(t)) {
 791                 retval = x86_32->disable_paging(t);
 792                 if (retval != ERROR_OK) {
 793                         LOG_ERROR("%s could not disable paging", __func__);
 794                         return retval;
 795                 }
 796                 pg_disabled = true;
 797         }
 798         switch (size) {
 799                 case BYTE:
 800                         if (use32)
 801                                 retval = x86_32->submit_instruction(t, IOWRB32);
 802                         else
 803                                 retval = x86_32->submit_instruction(t, IOWRB16);
 804                         break;
 805                 case WORD:
 806                         if (use32)
 807                                 retval = x86_32->submit_instruction(t, IOWRH32);
 808                         else
 809                                 retval = x86_32->submit_instruction(t, IOWRH16);
 810                         break;
 811                 case DWORD:
 812                         if (use32)
 813                                 retval = x86_32->submit_instruction(t, IOWRW32);
 814                         else
 815                                 retval = x86_32->submit_instruction(t, IOWRW16);
 816                         break;
 817                 default:
 818                         LOG_ERROR("%s invalid write io size", __func__);
 819                         return ERROR_FAIL;
 820         }
 821         /* restore CR0.PG bit if needed */
 822         if (pg_disabled) {
 823                 retval = x86_32->enable_paging(t);
 824                 if (retval != ERROR_OK) {
 825                         LOG_ERROR("%s could not enable paging", __func__);
 826                         return retval;
 827                 }
 828                 pg_disabled = false;
 829         }
 830         retval = x86_32->transaction_status(t);
 831         if (retval != ERROR_OK) {
 832                 LOG_ERROR("%s error on io write", __func__);
 833                 return retval;
 834         }
 835         return retval;
 836 }
 837
 838 int x86_32_common_add_watchpoint(struct target *t, struct watchpoint *wp)
 839 {
 840         check_not_halted(t);
 841         /* set_watchpoint() will return ERROR_TARGET_RESOURCE_NOT_AVAILABLE if all
 842          * hardware registers are gone
 843          */
 844         return set_watchpoint(t, wp);
 845 }
 846
 847 int x86_32_common_remove_watchpoint(struct target *t, struct watchpoint *wp)
 848 {
 849         if (check_not_halted(t))
 850                 return ERROR_TARGET_NOT_HALTED;
 851         if (wp->set)
 852                 unset_watchpoint(t, wp);
 853         return ERROR_OK;
 854 }
 855
 856 int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp)
 857 {
 858         LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 859         if (check_not_halted(t))
 860                 return ERROR_TARGET_NOT_HALTED;
 861         /* set_breakpoint() will return ERROR_TARGET_RESOURCE_NOT_AVAILABLE if all
 862          * hardware registers are gone (for hardware breakpoints)
 863          */
 864         return set_breakpoint(t, bp);
 865 }
 866
 867 int x86_32_common_remove_breakpoint(struct target *t, struct breakpoint *bp)
 868 {
 869         LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 870         if (check_not_halted(t))
 871                 return ERROR_TARGET_NOT_HALTED;
 872         if (bp->set)
 873                 unset_breakpoint(t, bp);
 874
 875         return ERROR_OK;
 876 }
 877
 878 static int set_debug_regs(struct target *t, uint32_t address,
 879                         uint8_t bp_num, uint8_t bp_type, uint8_t bp_length)
 880 {
 881         struct x86_32_common *x86_32 = target_to_x86_32(t);
 882         LOG_DEBUG("addr=0x%08" PRIx32 ", bp_num=%" PRIu8 ", bp_type=%" PRIu8 ", pb_length=%" PRIu8,
 883                         address, bp_num, bp_type, bp_length);
 884
 885         /* DR7 - set global enable */
 886         uint32_t dr7 = buf_get_u32(x86_32->cache->reg_list[DR7].value, 0, 32);
 887
 888         if (bp_length != 1 && bp_length != 2 && bp_length != 4)
 889                 return ERROR_FAIL;
 890
 891         if (DR7_BP_FREE(dr7, bp_num))
 892                 DR7_GLOBAL_ENABLE(dr7, bp_num);
 893         else {
 894                 LOG_ERROR("%s dr7 error, already enabled, val=%08" PRIx32, __func__, dr7);
 895                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 896         }
 897
 898         switch (bp_type) {
 899                 case 0:
 900                         /* 00 - only on instruction execution */
 901                         DR7_SET_EXE(dr7, bp_num);
 902                         DR7_SET_LENGTH(dr7, bp_num, bp_length);
 903                 break;
 904                 case 1:
 905                         /* 01 - only on data writes */
 906                         DR7_SET_WRITE(dr7, bp_num);
 907                         DR7_SET_LENGTH(dr7, bp_num, bp_length);
 908                 break;
 909                 case 2:
 910                         /* 10 UNSUPPORTED - an I/O read and I/O write */
 911                         LOG_ERROR("%s unsupported feature bp_type=%d", __func__, bp_type);
 912                         return ERROR_FAIL;
 913                 break;
 914                 case 3:
 915                         /* on data read or data write */
 916                         DR7_SET_ACCESS(dr7, bp_num);
 917                         DR7_SET_LENGTH(dr7, bp_num, bp_length);
 918                 break;
 919                 default:
 920                         LOG_ERROR("%s invalid request [only 0-3] bp_type=%d", __func__, bp_type);
 921                         return ERROR_FAIL;
 922         }
 923
 924         /* update regs in the reg cache ready to be written to hardware
 925          * when we exit PM
 926         */
 927         buf_set_u32(x86_32->cache->reg_list[bp_num+DR0].value, 0, 32, address);
 928         x86_32->cache->reg_list[bp_num+DR0].dirty = 1;
 929         x86_32->cache->reg_list[bp_num+DR0].valid = 1;
 930         buf_set_u32(x86_32->cache->reg_list[DR6].value, 0, 32, PM_DR6);
 931         x86_32->cache->reg_list[DR6].dirty = 1;
 932         x86_32->cache->reg_list[DR6].valid = 1;
 933         buf_set_u32(x86_32->cache->reg_list[DR7].value, 0, 32, dr7);
 934         x86_32->cache->reg_list[DR7].dirty = 1;
 935         x86_32->cache->reg_list[DR7].valid = 1;
 936         return ERROR_OK;
 937 }
 938
 939 static int unset_debug_regs(struct target *t, uint8_t bp_num)
 940 {
 941         struct x86_32_common *x86_32 = target_to_x86_32(t);
 942         LOG_DEBUG("bp_num=%" PRIu8, bp_num);
 943
 944         uint32_t dr7 = buf_get_u32(x86_32->cache->reg_list[DR7].value, 0, 32);
 945
 946         if (!(DR7_BP_FREE(dr7, bp_num))) {
 947                 DR7_GLOBAL_DISABLE(dr7, bp_num);
 948         } else {
 949                 LOG_ERROR("%s dr7 error, not enabled, val=0x%08" PRIx32, __func__, dr7);
 950                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 951         }
 952         /* this will clear rw and len bits */
 953         DR7_RESET_RWLEN_BITS(dr7, bp_num);
 954
 955         /* update regs in the reg cache ready to be written to hardware
 956          * when we exit PM
 957         */
 958         buf_set_u32(x86_32->cache->reg_list[bp_num+DR0].value, 0, 32, 0);
 959         x86_32->cache->reg_list[bp_num+DR0].dirty = 1;
 960         x86_32->cache->reg_list[bp_num+DR0].valid = 1;
 961         buf_set_u32(x86_32->cache->reg_list[DR6].value, 0, 32, PM_DR6);
 962         x86_32->cache->reg_list[DR6].dirty = 1;
 963         x86_32->cache->reg_list[DR6].valid = 1;
 964         buf_set_u32(x86_32->cache->reg_list[DR7].value, 0, 32, dr7);
 965         x86_32->cache->reg_list[DR7].dirty = 1;
 966         x86_32->cache->reg_list[DR7].valid = 1;
 967         return ERROR_OK;
 968 }
 969
 970 static int set_hwbp(struct target *t, struct breakpoint *bp)
 971 {
 972         struct x86_32_common *x86_32 = target_to_x86_32(t);
 973         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
 974         uint8_t hwbp_num = 0;
 975
 976         while (debug_reg_list[hwbp_num].used && (hwbp_num < x86_32->num_hw_bpoints))
 977                 hwbp_num++;
 978         if (hwbp_num >= x86_32->num_hw_bpoints) {
 979                 LOG_ERROR("%s no free hw breakpoint bpid=0x%" PRIx32, __func__, bp->unique_id);
 980                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 981         }
 982         if (set_debug_regs(t, bp->address, hwbp_num, DR7_BP_EXECUTE, 1) != ERROR_OK)
 983                 return ERROR_FAIL;
 984         bp->set = hwbp_num + 1;
 985         debug_reg_list[hwbp_num].used = 1;
 986         debug_reg_list[hwbp_num].bp_value = bp->address;
 987         LOG_USER("%s hardware breakpoint %" PRIu32 " set at 0x%08" PRIx32 " (hwreg=%" PRIu8 ")", __func__,
 988                         bp->unique_id, debug_reg_list[hwbp_num].bp_value, hwbp_num);
 989         return ERROR_OK;
 990 }
 991
 992 static int unset_hwbp(struct target *t, struct breakpoint *bp)
 993 {
 994         struct x86_32_common *x86_32 = target_to_x86_32(t);
 995         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
 996         int hwbp_num = bp->set - 1;
 997
 998         if ((hwbp_num < 0) || (hwbp_num >= x86_32->num_hw_bpoints)) {
 999                 LOG_ERROR("%s invalid breakpoint number=%d, bpid=%" PRIu32,
1000                                 __func__, hwbp_num, bp->unique_id);
1001                 return ERROR_OK;
1002         }
1003
1004         if (unset_debug_regs(t, hwbp_num) != ERROR_OK)
1005                 return ERROR_FAIL;
1006         debug_reg_list[hwbp_num].used = 0;
1007         debug_reg_list[hwbp_num].bp_value = 0;
1008
1009         LOG_USER("%s hardware breakpoint %" PRIu32 " removed from " TARGET_ADDR_FMT " (hwreg=%d)",
1010                         __func__, bp->unique_id, bp->address, hwbp_num);
1011         return ERROR_OK;
1012 }
1013
1014 static int set_swbp(struct target *t, struct breakpoint *bp)
1015 {
1016         struct x86_32_common *x86_32 = target_to_x86_32(t);
1017         LOG_DEBUG("id %" PRIx32, bp->unique_id);
1018         target_addr_t physaddr;
1019         uint8_t opcode = SW_BP_OPCODE;
1020         uint8_t readback;
1021
1022         if (calcaddr_physfromlin(t, bp->address, &physaddr) != ERROR_OK)
1023                 return ERROR_FAIL;
1024         if (read_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
1025                 return ERROR_FAIL;
1026
1027         LOG_DEBUG("set software breakpoint - orig byte=0x%02" PRIx8 "", *bp->orig_instr);
1028
1029         /* just write the instruction trap byte */
1030         if (write_phys_mem(t, physaddr, 1, 1, &opcode))
1031                 return ERROR_FAIL;
1032
1033         /* verify that this is not invalid/read-only memory */
1034         if (read_phys_mem(t, physaddr, 1, 1, &readback))
1035                 return ERROR_FAIL;
1036
1037         if (readback != SW_BP_OPCODE) {
1038                 LOG_ERROR("%s software breakpoint error at " TARGET_ADDR_FMT ", check memory",
1039                                 __func__, bp->address);
1040                 LOG_ERROR("%s readback=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
1041                                 __func__, readback, *bp->orig_instr);
1042                 return ERROR_FAIL;
1043         }
1044         bp->set = SW_BP_OPCODE; /* just non 0 */
1045
1046         /* add the memory patch */
1047         struct swbp_mem_patch *new_patch = malloc(sizeof(struct swbp_mem_patch));
1048         if (new_patch == NULL) {
1049                 LOG_ERROR("%s out of memory", __func__);
1050                 return ERROR_FAIL;
1051         }
1052         new_patch->next = NULL;
1053         new_patch->orig_byte = *bp->orig_instr;
1054         new_patch->physaddr = physaddr;
1055         new_patch->swbp_unique_id = bp->unique_id;
1056
1057         struct swbp_mem_patch *addto = x86_32->swbbp_mem_patch_list;
1058         if (addto == NULL)
1059                 x86_32->swbbp_mem_patch_list = new_patch;
1060         else {
1061                 while (addto->next != NULL)
1062                         addto = addto->next;
1063                 addto->next = new_patch;
1064         }
1065         LOG_USER("%s software breakpoint %" PRIu32 " set at " TARGET_ADDR_FMT,
1066                         __func__, bp->unique_id, bp->address);
1067         return ERROR_OK;
1068 }
1069
1070 static int unset_swbp(struct target *t, struct breakpoint *bp)
1071 {
1072         struct x86_32_common *x86_32 = target_to_x86_32(t);
1073         LOG_DEBUG("id %" PRIx32, bp->unique_id);
1074         target_addr_t physaddr;
1075         uint8_t current_instr;
1076
1077         /* check that user program has not modified breakpoint instruction */
1078         if (calcaddr_physfromlin(t, bp->address, &physaddr) != ERROR_OK)
1079                 return ERROR_FAIL;
1080         if (read_phys_mem(t, physaddr, 1, 1, &current_instr))
1081                 return ERROR_FAIL;
1082
1083         if (current_instr == SW_BP_OPCODE) {
1084                 if (write_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
1085                         return ERROR_FAIL;
1086         } else {
1087                 LOG_ERROR("%s software breakpoint remove error at " TARGET_ADDR_FMT ", check memory",
1088                                 __func__, bp->address);
1089                 LOG_ERROR("%s current=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
1090                                 __func__, current_instr, *bp->orig_instr);
1091                 return ERROR_FAIL;
1092         }
1093
1094         /* remove from patch */
1095         struct swbp_mem_patch *iter = x86_32->swbbp_mem_patch_list;
1096         if (iter != NULL) {
1097                 if (iter->swbp_unique_id == bp->unique_id) {
1098                         /* it's the first item */
1099                         x86_32->swbbp_mem_patch_list = iter->next;
1100                         free(iter);
1101                 } else {
1102                         while (iter->next != NULL && iter->next->swbp_unique_id != bp->unique_id)
1103                                 iter = iter->next;
1104                         if (iter->next != NULL) {
1105                                 /* it's the next one */
1106                                 struct swbp_mem_patch *freeme = iter->next;
1107                                 iter->next = iter->next->next;
1108                                 free(freeme);
1109                         }
1110                 }
1111         }
1112
1113         LOG_USER("%s software breakpoint %" PRIu32 " removed from " TARGET_ADDR_FMT,
1114                         __func__, bp->unique_id, bp->address);
1115         return ERROR_OK;
1116 }
1117
1118 static int set_breakpoint(struct target *t, struct breakpoint *bp)
1119 {
1120         int error = ERROR_OK;
1121         struct x86_32_common *x86_32 = target_to_x86_32(t);
1122         LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
1123         if (bp->set) {
1124                 LOG_ERROR("breakpoint already set");
1125                 return error;
1126         }
1127         if (bp->type == BKPT_HARD) {
1128                 error = set_hwbp(t, bp);
1129                 if (error != ERROR_OK) {
1130                         LOG_ERROR("%s error setting hardware breakpoint at " TARGET_ADDR_FMT,
1131                                         __func__, bp->address);
1132                         return error;
1133                 }
1134         } else {
1135                 if (x86_32->sw_bpts_supported(t)) {
1136                         error = set_swbp(t, bp);
1137                         if (error != ERROR_OK) {
1138                                 LOG_ERROR("%s error setting software breakpoint at " TARGET_ADDR_FMT,
1139                                                 __func__, bp->address);
1140                                 return error;
1141                         }
1142                 } else {
1143                         LOG_ERROR("%s core doesn't support SW breakpoints", __func__);
1144                         error = ERROR_FAIL;
1145                         return ERROR_FAIL;
1146                 }
1147         }
1148         return error;
1149 }
1150
1151 static int unset_breakpoint(struct target *t, struct breakpoint *bp)
1152 {
1153         LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
1154         if (!bp->set) {
1155                 LOG_WARNING("breakpoint not set");
1156                 return ERROR_OK;
1157         }
1158
1159         if (bp->type == BKPT_HARD) {
1160                 if (unset_hwbp(t, bp) != ERROR_OK) {
1161                         LOG_ERROR("%s error removing hardware breakpoint at " TARGET_ADDR_FMT,
1162                                         __func__, bp->address);
1163                         return ERROR_FAIL;
1164                 }
1165         } else {
1166                 if (unset_swbp(t, bp) != ERROR_OK) {
1167                         LOG_ERROR("%s error removing software breakpoint at " TARGET_ADDR_FMT,
1168                                         __func__, bp->address);
1169                         return ERROR_FAIL;
1170                 }
1171         }
1172         bp->set = 0;
1173         return ERROR_OK;
1174 }
1175
1176 static int set_watchpoint(struct target *t, struct watchpoint *wp)
1177 {
1178         struct x86_32_common *x86_32 = target_to_x86_32(t);
1179         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
1180         int wp_num = 0;
1181         LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, wp->rw, wp->address);
1182
1183         if (wp->set) {
1184                 LOG_ERROR("%s watchpoint already set", __func__);
1185                 return ERROR_OK;
1186         }
1187
1188         if (wp->rw == WPT_READ) {
1189                 LOG_ERROR("%s no support for 'read' watchpoints, use 'access' or 'write'"
1190                                 , __func__);
1191                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1192         }
1193
1194         while (debug_reg_list[wp_num].used && (wp_num < x86_32->num_hw_bpoints))
1195                 wp_num++;
1196         if (wp_num >= x86_32->num_hw_bpoints) {
1197                 LOG_ERROR("%s no debug registers left", __func__);
1198                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1199         }
1200
1201         if (wp->length != 4 && wp->length != 2 && wp->length != 1) {
1202                 LOG_ERROR("%s only watchpoints of length 1, 2 or 4 are supported", __func__);
1203                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1204         }
1205
1206         switch (wp->rw) {
1207                 case WPT_WRITE:
1208                         if (set_debug_regs(t, wp->address, wp_num,
1209                                                 DR7_BP_WRITE, wp->length) != ERROR_OK) {
1210                                 return ERROR_FAIL;
1211                         }
1212                         break;
1213                 case WPT_ACCESS:
1214                         if (set_debug_regs(t, wp->address, wp_num, DR7_BP_READWRITE,
1215                                                 wp->length) != ERROR_OK) {
1216                                 return ERROR_FAIL;
1217                         }
1218                         break;
1219                 default:
1220                         LOG_ERROR("%s only 'access' or 'write' watchpoints are supported", __func__);
1221                         break;
1222         }
1223         wp->set = wp_num + 1;
1224         debug_reg_list[wp_num].used = 1;
1225         debug_reg_list[wp_num].bp_value = wp->address;
1226         LOG_USER("'%s' watchpoint %d set at " TARGET_ADDR_FMT " with length %" PRIu32 " (hwreg=%d)",
1227                         wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
1228                         "write" : wp->rw == WPT_ACCESS ? "access" : "?",
1229                         wp->unique_id, wp->address, wp->length, wp_num);
1230         return ERROR_OK;
1231 }
1232
1233 static int unset_watchpoint(struct target *t, struct watchpoint *wp)
1234 {
1235         struct x86_32_common *x86_32 = target_to_x86_32(t);
1236         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
1237         LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, wp->rw, wp->address);
1238         if (!wp->set) {
1239                 LOG_WARNING("watchpoint not set");
1240                 return ERROR_OK;
1241         }
1242
1243         int wp_num = wp->set - 1;
1244         if ((wp_num < 0) || (wp_num >= x86_32->num_hw_bpoints)) {
1245                 LOG_DEBUG("Invalid FP Comparator number in watchpoint");
1246                 return ERROR_OK;
1247         }
1248         if (unset_debug_regs(t, wp_num) != ERROR_OK)
1249                 return ERROR_FAIL;
1250
1251         debug_reg_list[wp_num].used = 0;
1252         debug_reg_list[wp_num].bp_value = 0;
1253         wp->set = 0;
1254
1255         LOG_USER("'%s' watchpoint %d removed from " TARGET_ADDR_FMT " with length %" PRIu32 " (hwreg=%d)",
1256                         wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
1257                         "write" : wp->rw == WPT_ACCESS ? "access" : "?",
1258                         wp->unique_id, wp->address, wp->length, wp_num);
1259
1260         return ERROR_OK;
1261 }
1262
1263 /* after reset breakpoints and watchpoints in memory are not valid anymore and
1264  * debug registers are cleared.
1265  * we can't afford to remove sw breakpoints using the default methods as the
1266  * memory doesn't have the same layout yet and an access might crash the target,
1267  * so we just clear the openocd breakpoints structures.
1268  */
1269 void x86_32_common_reset_breakpoints_watchpoints(struct target *t)
1270 {
1271         struct x86_32_common *x86_32 = target_to_x86_32(t);
1272         struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
1273         struct breakpoint *next_b;
1274         struct watchpoint *next_w;
1275
1276         while (t->breakpoints) {
1277                 next_b = t->breakpoints->next;
1278                 free(t->breakpoints->orig_instr);
1279                 free(t->breakpoints);
1280                 t->breakpoints = next_b;
1281         }
1282
1283         while (t->watchpoints) {
1284                 next_w = t->watchpoints->next;
1285                 free(t->watchpoints);
1286                 t->watchpoints = next_w;
1287         }
1288
1289         for (int i = 0; i < x86_32->num_hw_bpoints; i++) {
1290                 debug_reg_list[i].used = 0;
1291                 debug_reg_list[i].bp_value = 0;
1292         }
1293 }
1294
1295 static int read_hw_reg_to_cache(struct target *t, int num)
1296 {
1297         uint32_t reg_value;
1298         struct x86_32_common *x86_32 = target_to_x86_32(t);
1299
1300         if (check_not_halted(t))
1301                 return ERROR_TARGET_NOT_HALTED;
1302         if ((num < 0) || (num >= x86_32->get_num_user_regs(t)))
1303                 return ERROR_COMMAND_SYNTAX_ERROR;
1304         if (x86_32->read_hw_reg(t, num, &reg_value, 1) != ERROR_OK) {
1305                 LOG_ERROR("%s fail for %s", x86_32->cache->reg_list[num].name, __func__);
1306                 return ERROR_FAIL;
1307         }
1308         LOG_DEBUG("reg %s value 0x%08" PRIx32,
1309                         x86_32->cache->reg_list[num].name, reg_value);
1310         return ERROR_OK;
1311 }
1312
1313 static int write_hw_reg_from_cache(struct target *t, int num)
1314 {
1315         struct x86_32_common *x86_32 = target_to_x86_32(t);
1316         if (check_not_halted(t))
1317                 return ERROR_TARGET_NOT_HALTED;
1318         if ((num < 0) || (num >= x86_32->get_num_user_regs(t)))
1319                 return ERROR_COMMAND_SYNTAX_ERROR;
1320         if (x86_32->write_hw_reg(t, num, 0, 1) != ERROR_OK) {
1321                 LOG_ERROR("%s fail for %s", x86_32->cache->reg_list[num].name, __func__);
1322                 return ERROR_FAIL;
1323         }
1324         LOG_DEBUG("reg %s value 0x%08" PRIx32, x86_32->cache->reg_list[num].name,
1325                         buf_get_u32(x86_32->cache->reg_list[num].value, 0, 32));
1326         return ERROR_OK;
1327 }
1328
1329 /* x86 32 commands */
1330 static void handle_iod_output(struct command_context *cmd_ctx,
1331                 struct target *target, uint32_t address, unsigned size,
1332                 unsigned count, const uint8_t *buffer)
1333 {
1334         const unsigned line_bytecnt = 32;
1335         unsigned line_modulo = line_bytecnt / size;
1336
1337         char output[line_bytecnt * 4 + 1];
1338         unsigned output_len = 0;
1339
1340         const char *value_fmt;
1341         switch (size) {
1342         case 4:
1343                 value_fmt = "%8.8x ";
1344                 break;
1345         case 2:
1346                 value_fmt = "%4.4x ";
1347                 break;
1348         case 1:
1349                 value_fmt = "%2.2x ";
1350                 break;
1351         default:
1352                 /* "can't happen", caller checked */
1353                 LOG_ERROR("%s invalid memory read size: %u", __func__, size);
1354                 return;
1355         }
1356
1357         for (unsigned i = 0; i < count; i++) {
1358                 if (i % line_modulo == 0) {
1359                         output_len += snprintf(output + output_len,
1360                                         sizeof(output) - output_len,
1361                                         "0x%8.8x: ",
1362                                         (unsigned)(address + (i*size)));
1363                 }
1364
1365                 uint32_t value = 0;
1366                 const uint8_t *value_ptr = buffer + i * size;
1367                 switch (size) {
1368                 case 4:
1369                         value = target_buffer_get_u32(target, value_ptr);
1370                         break;
1371                 case 2:
1372                         value = target_buffer_get_u16(target, value_ptr);
1373                         break;
1374                 case 1:
1375                         value = *value_ptr;
1376                 }
1377                 output_len += snprintf(output + output_len,
1378                                 sizeof(output) - output_len,
1379                                 value_fmt, value);
1380
1381                 if ((i % line_modulo == line_modulo - 1) || (i == count - 1)) {
1382                         command_print(cmd_ctx, "%s", output);
1383                         output_len = 0;
1384                 }
1385         }
1386 }
1387
1388 COMMAND_HANDLER(handle_iod_command)
1389 {
1390         if (CMD_ARGC != 1)
1391                 return ERROR_COMMAND_SYNTAX_ERROR;
1392
1393         uint32_t address;
1394         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1395         if (address > 0xffff) {
1396                 LOG_ERROR("%s IA-32 I/O space is 2^16, 0x%08" PRIx32 " exceeds max", __func__, address);
1397                 return ERROR_COMMAND_SYNTAX_ERROR;
1398         }
1399
1400         unsigned size = 0;
1401         switch (CMD_NAME[2]) {
1402         case 'w':
1403                 size = 4;
1404                 break;
1405         case 'h':
1406                 size = 2;
1407                 break;
1408         case 'b':
1409                 size = 1;
1410                 break;
1411         default:
1412                 return ERROR_COMMAND_SYNTAX_ERROR;
1413         }
1414         unsigned count = 1;
1415         uint8_t *buffer = calloc(count, size);
1416         struct target *target = get_current_target(CMD_CTX);
1417         int retval = x86_32_common_read_io(target, address, size, buffer);
1418         if (ERROR_OK == retval)
1419                 handle_iod_output(CMD_CTX, target, address, size, count, buffer);
1420         free(buffer);
1421         return retval;
1422 }
1423
1424 static int target_fill_io(struct target *target,
1425                 uint32_t address,
1426                 unsigned data_size,
1427                 /* value */
1428                 uint32_t b)
1429 {
1430         LOG_DEBUG("address=0x%08" PRIx32 ", data_size=%u, b=0x%08" PRIx32,
1431                         address, data_size, b);
1432         uint8_t target_buf[data_size];
1433         switch (data_size) {
1434         case 4:
1435                 target_buffer_set_u32(target, target_buf, b);
1436                 break;
1437         case 2:
1438                 target_buffer_set_u16(target, target_buf, b);
1439                 break;
1440         case 1:
1441                 target_buf[0] = (b & 0x0ff);
1442                 break;
1443         default:
1444                 exit(-1);
1445         }
1446         return x86_32_common_write_io(target, address, data_size, target_buf);
1447 }
1448
1449 COMMAND_HANDLER(handle_iow_command)
1450 {
1451         if (CMD_ARGC != 2)
1452                 return ERROR_COMMAND_SYNTAX_ERROR;
1453         uint32_t address;
1454         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
1455         uint32_t value;
1456         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
1457         struct target *target = get_current_target(CMD_CTX);
1458
1459         unsigned wordsize;
1460         switch (CMD_NAME[2]) {
1461                 case 'w':
1462                         wordsize = 4;
1463                         break;
1464                 case 'h':
1465                         wordsize = 2;
1466                         break;
1467                 case 'b':
1468                         wordsize = 1;
1469                         break;
1470                 default:
1471                         return ERROR_COMMAND_SYNTAX_ERROR;
1472         }
1473         return target_fill_io(target, address, wordsize, value);
1474 }
1475
1476 static const struct command_registration x86_32_exec_command_handlers[] = {
1477         {
1478                 .name = "iww",
1479                 .mode = COMMAND_EXEC,
1480                 .handler = handle_iow_command,
1481                 .help = "write I/O port word",
1482                 .usage = "port data[word]",
1483         },
1484         {
1485                 .name = "iwh",
1486                 .mode = COMMAND_EXEC,
1487                 .handler = handle_iow_command,
1488                 .help = "write I/O port halfword",
1489                 .usage = "port data[halfword]",
1490         },
1491         {
1492                 .name = "iwb",
1493                 .mode = COMMAND_EXEC,
1494                 .handler = handle_iow_command,
1495                 .help = "write I/O port byte",
1496                 .usage = "port data[byte]",
1497         },
1498         {
1499                 .name = "idw",
1500                 .mode = COMMAND_EXEC,
1501                 .handler = handle_iod_command,
1502                 .help = "display I/O port word",
1503                 .usage = "port",
1504         },
1505         {
1506                 .name = "idh",
1507                 .mode = COMMAND_EXEC,
1508                 .handler = handle_iod_command,
1509                 .help = "display I/O port halfword",
1510                 .usage = "port",
1511         },
1512         {
1513                 .name = "idb",
1514                 .mode = COMMAND_EXEC,
1515                 .handler = handle_iod_command,
1516                 .help = "display I/O port byte",
1517                 .usage = "port",
1518         },
1519
1520         COMMAND_REGISTRATION_DONE
1521 };
1522
1523 const struct command_registration x86_32_command_handlers[] = {
1524         {
1525                 .name = "x86_32",
1526                 .mode = COMMAND_ANY,
1527                 .help = "x86_32 target commands",
1528                 .usage = "",
1529                 .chain = x86_32_exec_command_handlers,
1530         },
1531         COMMAND_REGISTRATION_DONE
1532 };