src/target/arm_adi_v5.c

   1 /***************************************************************************
   2  *   Copyright (C) 2006 by Magnus Lundin                                   *
   3  *   lundin@mlu.mine.nu                                                    *
   4  *                                                                         *
   5  *   Copyright (C) 2008 by Spencer Oliver                                  *
   6  *   spen@spen-soft.co.uk                                                  *
   7  *                                                                         *
   8  *   Copyright (C) 2009 by Oyvind Harboe                                   *
   9  *   oyvind.harboe@zylin.com                                               *
  10  *                                                                                                                                                 *
  11  *   This program is free software; you can redistribute it and/or modify  *
  12  *   it under the terms of the GNU General Public License as published by  *
  13  *   the Free Software Foundation; either version 2 of the License, or     *
  14  *   (at your option) any later version.                                   *
  15  *                                                                         *
  16  *   This program is distributed in the hope that it will be useful,       *
  17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  19  *   GNU General Public License for more details.                          *
  20  *                                                                         *
  21  *   You should have received a copy of the GNU General Public License     *
  22  *   along with this program; if not, write to the                         *
  23  *   Free Software Foundation, Inc.,                                       *
  24  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  25  ***************************************************************************/
  26 /***************************************************************************
  27  *                                                                         *
  28  * This file implements support for the ARM Debug Interface v5  (ADI_V5)   *
  29  *                                                                         *
  30  * ARM(tm) Debug Interface v5 Architecture Specification    ARM IHI 0031A  *
  31  *                                                                         *
  32  * CoreSight(tm) DAP-Lite TRM, ARM DDI 0316A                               *
  33  * Cortex-M3(tm) TRM, ARM DDI 0337C                                        *
  34  *                                                                         *
  35 ***************************************************************************/
  36
  37 #ifdef HAVE_CONFIG_H
  38 #include "config.h"
  39 #endif
  40
  41 #include "replacements.h"
  42
  43 #include "arm_adi_v5.h"
  44 #include "jtag.h"
  45 #include "log.h"
  46 #include "time_support.h"
  47 #include <stdlib.h>
  48 #include <string.h>
  49
  50 /*
  51  * Transaction Mode:
  52  * swjdp->trans_mode = TRANS_MODE_COMPOSITE;
  53  * Uses Overrun checking mode and does not do actual JTAG send/receive or transaction
  54  * result checking until swjdp_end_transaction()
  55  * This must be done before using or deallocating any return variables.
  56  * swjdp->trans_mode == TRANS_MODE_ATOMIC
  57  * All reads and writes to the AHB bus are checked for valid completion, and return values
  58  * are immediatley available.
  59 */
  60
  61 /***************************************************************************
  62  *                                                                         *
  63  * DPACC and APACC scanchain access through JTAG-DP                        *
  64  *                                                                         *
  65 ***************************************************************************/
  66
  67 /* Scan out and in from target ordered u8 buffers */
  68 int adi_jtag_dp_scan(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue, u8 *ack)
  69 {
  70         scan_field_t fields[2];
  71         u8 out_addr_buf;
  72
  73         jtag_add_end_state(TAP_IDLE);
  74         arm_jtag_set_instr(jtag_info, instr, NULL);
  75
  76         fields[0].tap = jtag_info->tap;
  77         fields[0].num_bits = 3;
  78         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
  79         fields[0].out_value = &out_addr_buf;
  80
  81         fields[0].in_value = ack;
  82
  83
  84         fields[0].in_handler = NULL;
  85
  86
  87         fields[1].tap = jtag_info->tap;
  88         fields[1].num_bits = 32;
  89         fields[1].out_value = outvalue;
  90
  91         fields[1].in_value = invalue;
  92         fields[1].in_handler = NULL;
  93
  94
  95
  96
  97         jtag_add_dr_scan(2, fields, TAP_INVALID);
  98
  99         return ERROR_OK;
 100 }
 101
 102 /* Scan out and in from host ordered u32 variables */
 103 int adi_jtag_dp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue, u8 *ack)
 104 {
 105         scan_field_t fields[2];
 106         u8 out_value_buf[4];
 107         u8 out_addr_buf;
 108
 109         jtag_add_end_state(TAP_IDLE);
 110         arm_jtag_set_instr(jtag_info, instr, NULL);
 111
 112         fields[0].tap = jtag_info->tap;
 113         fields[0].num_bits = 3;
 114         buf_set_u32(&out_addr_buf, 0, 3, ((reg_addr >> 1) & 0x6) | (RnW & 0x1));
 115         fields[0].out_value = &out_addr_buf;
 116         fields[0].in_value = ack;
 117         fields[0].in_handler = NULL;
 118
 119
 120         fields[1].tap = jtag_info->tap;
 121         fields[1].num_bits = 32;
 122         buf_set_u32(out_value_buf, 0, 32, outvalue);
 123         fields[1].out_value = out_value_buf;
 124         fields[1].in_value = NULL;
 125         fields[1].in_handler = NULL;
 126
 127         if (invalue)
 128         {
 129                 u8 tmp[4];
 130                 fields[1].in_value = tmp;
 131                 jtag_add_dr_scan_now(2, fields, TAP_INVALID);
 132
 133                 *invalue=flip_u32(le_to_h_u32(tmp), 32);
 134         } else
 135         {
 136
 137                 jtag_add_dr_scan(2, fields, TAP_INVALID);
 138         }
 139
 140         return ERROR_OK;
 141 }
 142
 143 /* scan_inout_check adds one extra inscan for DPAP_READ commands to read variables */
 144 int scan_inout_check(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalue, u8 *invalue)
 145 {
 146         adi_jtag_dp_scan(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 147         if ((RnW == DPAP_READ) && (invalue != NULL))
 148         {
 149                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 150         }
 151
 152         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and the check CTRL_STAT */
 153         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 154         {
 155                 return swjdp_transaction_endcheck(swjdp);
 156         }
 157
 158         return ERROR_OK;
 159 }
 160
 161 int scan_inout_check_u32(swjdp_common_t *swjdp, u8 instr, u8 reg_addr, u8 RnW, u32 outvalue, u32 *invalue)
 162 {
 163         adi_jtag_dp_scan_u32(swjdp->jtag_info, instr, reg_addr, RnW, outvalue, NULL, NULL);
 164         if ((RnW==DPAP_READ) && (invalue != NULL))
 165         {
 166                 adi_jtag_dp_scan_u32(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, invalue, &swjdp->ack);
 167         }
 168
 169         /* In TRANS_MODE_ATOMIC all SWJDP_IR_APACC transactions wait for ack=OK/FAULT and then check CTRL_STAT */
 170         if ((instr == SWJDP_IR_APACC) && (swjdp->trans_mode == TRANS_MODE_ATOMIC))
 171         {
 172                 return swjdp_transaction_endcheck(swjdp);
 173         }
 174
 175         return ERROR_OK;
 176 }
 177
 178 int swjdp_transaction_endcheck(swjdp_common_t *swjdp)
 179 {
 180         int retval;
 181         u32 ctrlstat;
 182
 183         /* too expensive to call keep_alive() here */
 184
 185 #if 0
 186         /* Danger!!!! BROKEN!!!! */
 187         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 188         /* Danger!!!! BROKEN!!!! Why will jtag_execute_queue() fail here????
 189         R956 introduced the check on return value here and now Michael Schwingen reports
 190         that this code no longer works....
 191
 192         https://lists.berlios.de/pipermail/openocd-development/2008-September/003107.html
 193         */
 194         if ((retval=jtag_execute_queue())!=ERROR_OK)
 195         {
 196                 LOG_ERROR("BUG: Why does this fail the first time????");
 197         }
 198         /* Why??? second time it works??? */
 199 #endif
 200
 201         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 202         if ((retval=jtag_execute_queue())!=ERROR_OK)
 203                 return retval;
 204
 205         swjdp->ack = swjdp->ack & 0x7;
 206
 207         if (swjdp->ack != 2)
 208         {
 209                 long long then=timeval_ms();
 210                 while (swjdp->ack != 2)
 211                 {
 212                         if (swjdp->ack == 1)
 213                         {
 214                                 if ((timeval_ms()-then) > 1000)
 215                                 {
 216                                         LOG_WARNING("Timeout (1000ms) waiting for ACK = OK/FAULT in SWJDP transaction");
 217                                         return ERROR_JTAG_DEVICE_ERROR;
 218                                 }
 219                         }
 220                         else
 221                         {
 222                                 LOG_WARNING("Invalid ACK in SWJDP transaction");
 223                                 return ERROR_JTAG_DEVICE_ERROR;
 224                         }
 225
 226                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 227                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 228                                 return retval;
 229                         swjdp->ack = swjdp->ack & 0x7;
 230                 }
 231         } else
 232         {
 233                 /* common code path avoids fn to timeval_ms() */
 234         }
 235
 236         /* Check for STICKYERR and STICKYORUN */
 237         if (ctrlstat & (SSTICKYORUN | SSTICKYERR))
 238         {
 239                 LOG_DEBUG("swjdp: CTRL/STAT error 0x%x", ctrlstat);
 240                 /* Check power to debug regions */
 241                 if ((ctrlstat & 0xf0000000) != 0xf0000000)
 242                 {
 243                          ahbap_debugport_init(swjdp);
 244                 }
 245                 else
 246                 {
 247                         u32 mem_ap_csw, mem_ap_tar;
 248
 249                         /* Print information about last AHBAP access */
 250                         LOG_ERROR("AHBAP Cached values: dp_select 0x%x, ap_csw 0x%x, ap_tar 0x%x", swjdp->dp_select_value, swjdp->ap_csw_value, swjdp->ap_tar_value);
 251                         if (ctrlstat & SSTICKYORUN)
 252                                 LOG_ERROR("SWJ-DP OVERRUN - check clock or reduce jtag speed");
 253
 254                         if (ctrlstat & SSTICKYERR)
 255                                 LOG_ERROR("SWJ-DP STICKY ERROR");
 256
 257                         /* Clear Sticky Error Bits */
 258                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_WRITE, swjdp->dp_ctrl_stat | SSTICKYORUN | SSTICKYERR, NULL);
 259                         scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, DP_CTRL_STAT, DPAP_READ, 0, &ctrlstat);
 260                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 261                                 return retval;
 262
 263                         LOG_DEBUG("swjdp: status 0x%x", ctrlstat);
 264
 265                         dap_ap_read_reg_u32(swjdp, AP_REG_CSW, &mem_ap_csw);
 266                         dap_ap_read_reg_u32(swjdp, AP_REG_TAR, &mem_ap_tar);
 267                         if ((retval=jtag_execute_queue())!=ERROR_OK)
 268                                 return retval;
 269                         LOG_ERROR("Read MEM_AP_CSW 0x%x, MEM_AP_TAR 0x%x", mem_ap_csw, mem_ap_tar);
 270
 271                 }
 272                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 273                         return retval;
 274                 return ERROR_JTAG_DEVICE_ERROR;
 275         }
 276
 277         return ERROR_OK;
 278 }
 279
 280 /***************************************************************************
 281  *                                                                         *
 282  * DP and MEM-AP  register access  through APACC and DPACC                 *
 283  *                                                                         *
 284 ***************************************************************************/
 285
 286 int dap_dp_write_reg(swjdp_common_t *swjdp, u32 value, u8 reg_addr)
 287 {
 288         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_WRITE, value, NULL);
 289 }
 290
 291 int dap_dp_read_reg(swjdp_common_t *swjdp, u32 *value, u8 reg_addr)
 292 {
 293         return scan_inout_check_u32(swjdp, SWJDP_IR_DPACC, reg_addr, DPAP_READ, 0, value);
 294 }
 295
 296 int dap_ap_select(swjdp_common_t *swjdp,u8 apsel)
 297 {
 298         u32 select;
 299         select = (apsel<<24) & 0xFF000000;
 300
 301         if (select != swjdp->apsel)
 302         {
 303                 swjdp->apsel = select;
 304                 /* Switchin AP invalidates cached values */
 305                 swjdp->dp_select_value = -1;
 306                 swjdp->ap_csw_value = -1;
 307                 swjdp->ap_tar_value = -1;
 308         }
 309
 310         return ERROR_OK;
 311 }
 312
 313 int dap_dp_bankselect(swjdp_common_t *swjdp,u32 ap_reg)
 314 {
 315         u32 select;
 316         select = (ap_reg & 0x000000F0);
 317
 318         if (select != swjdp->dp_select_value)
 319         {
 320                 dap_dp_write_reg(swjdp, select | swjdp->apsel, DP_SELECT);
 321                 swjdp->dp_select_value = select;
 322         }
 323
 324         return ERROR_OK;
 325 }
 326
 327 int dap_ap_write_reg(swjdp_common_t *swjdp, u32 reg_addr, u8* out_value_buf)
 328 {
 329         dap_dp_bankselect(swjdp, reg_addr);
 330         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 331
 332         return ERROR_OK;
 333 }
 334
 335 int dap_ap_read_reg(swjdp_common_t *swjdp, u32 reg_addr, u8 *in_value_buf)
 336 {
 337         dap_dp_bankselect(swjdp, reg_addr);
 338         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, in_value_buf);
 339
 340         return ERROR_OK;
 341 }
 342 int dap_ap_write_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 value)
 343 {
 344         u8 out_value_buf[4];
 345
 346         buf_set_u32(out_value_buf, 0, 32, value);
 347         dap_dp_bankselect(swjdp, reg_addr);
 348         scan_inout_check(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_WRITE, out_value_buf, NULL);
 349
 350         return ERROR_OK;
 351 }
 352
 353 int dap_ap_read_reg_u32(swjdp_common_t *swjdp, u32 reg_addr, u32 *value)
 354 {
 355         dap_dp_bankselect(swjdp, reg_addr);
 356         scan_inout_check_u32(swjdp, SWJDP_IR_APACC, reg_addr, DPAP_READ, 0, value);
 357
 358         return ERROR_OK;
 359 }
 360
 361 /***************************************************************************
 362  *                                                                         *
 363  * AHB-AP access to memory and system registers on AHB bus                 *
 364  *                                                                         *
 365 ***************************************************************************/
 366
 367 int dap_setup_accessport(swjdp_common_t *swjdp, u32 csw, u32 tar)
 368 {
 369         csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT;
 370         if (csw != swjdp->ap_csw_value)
 371         {
 372                 /* LOG_DEBUG("swjdp : Set CSW %x",csw); */
 373                 dap_ap_write_reg_u32(swjdp, AP_REG_CSW, csw );
 374                 swjdp->ap_csw_value = csw;
 375         }
 376         if (tar != swjdp->ap_tar_value)
 377         {
 378                 /* LOG_DEBUG("swjdp : Set TAR %x",tar); */
 379                 dap_ap_write_reg_u32(swjdp, AP_REG_TAR, tar );
 380                 swjdp->ap_tar_value = tar;
 381         }
 382         if (csw & CSW_ADDRINC_MASK)
 383         {
 384                 /* Do not cache TAR value when autoincrementing */
 385                 swjdp->ap_tar_value = -1;
 386         }
 387         return ERROR_OK;
 388 }
 389
 390 /*****************************************************************************
 391 *                                                                            *
 392 * mem_ap_read_u32(swjdp_common_t *swjdp, u32 address, u32 *value)      *
 393 *                                                                            *
 394 * Read a u32 value from memory or system register                            *
 395 * Functionally equivalent to target_read_u32(target, address, u32 *value),   *
 396 * but with less overhead                                                     *
 397 *****************************************************************************/
 398 int mem_ap_read_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 399 {
 400         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 401
 402         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 403         dap_ap_read_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value );
 404
 405         return ERROR_OK;
 406 }
 407
 408 int mem_ap_read_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 *value)
 409 {
 410         mem_ap_read_u32(swjdp, address, value);
 411
 412         return swjdp_transaction_endcheck(swjdp);
 413 }
 414
 415 /*****************************************************************************
 416 *                                                                            *
 417 * mem_ap_write_u32(swjdp_common_t *swjdp, u32 address, u32 value)      *
 418 *                                                                            *
 419 * Write a u32 value to memory or memory mapped register                              *
 420 *                                                                            *
 421 *****************************************************************************/
 422 int mem_ap_write_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 423 {
 424         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 425
 426         dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, address & 0xFFFFFFF0);
 427         dap_ap_write_reg_u32(swjdp, AP_REG_BD0 | (address & 0xC), value );
 428
 429         return ERROR_OK;
 430 }
 431
 432 int mem_ap_write_atomic_u32(swjdp_common_t *swjdp, u32 address, u32 value)
 433 {
 434         mem_ap_write_u32(swjdp, address, value);
 435
 436         return swjdp_transaction_endcheck(swjdp);
 437 }
 438
 439 /*****************************************************************************
 440 *                                                                            *
 441 * mem_ap_write_buf(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address) *
 442 *                                                                            *
 443 * Write a buffer in target order (little endian)                             *
 444 *                                                                            *
 445 *****************************************************************************/
 446 int mem_ap_write_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 447 {
 448         int wcount, blocksize, writecount, errorcount = 0, retval = ERROR_OK;
 449         u32 adr = address;
 450         u8* pBuffer = buffer;
 451
 452         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 453
 454         count >>= 2;
 455         wcount = count;
 456
 457         /* if we have an unaligned access - reorder data */
 458         if (adr & 0x3u)
 459         {
 460                 for (writecount = 0; writecount < count; writecount++)
 461                 {
 462                         int i;
 463                         u32 outvalue;
 464                         memcpy(&outvalue, pBuffer, sizeof(u32));
 465
 466                         for (i = 0; i < 4; i++ )
 467                         {
 468                                 *((u8*)pBuffer + (adr & 0x3)) = outvalue;
 469                                 outvalue >>= 8;
 470                                 adr++;
 471                         }
 472                         pBuffer += sizeof(u32);
 473                 }
 474         }
 475
 476         while (wcount > 0)
 477         {
 478                 /* Adjust to write blocks within 4K aligned boundaries */
 479                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 480                 if (wcount < blocksize)
 481                         blocksize = wcount;
 482
 483                 /* handle unaligned data at 4k boundary */
 484                 if (blocksize == 0)
 485                         blocksize = 1;
 486
 487                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 488
 489                 for (writecount = 0; writecount < blocksize; writecount++)
 490                 {
 491                         dap_ap_write_reg(swjdp, AP_REG_DRW, buffer + 4 * writecount );
 492                 }
 493
 494                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 495                 {
 496                         wcount = wcount - blocksize;
 497                         address = address + 4 * blocksize;
 498                         buffer = buffer + 4 * blocksize;
 499                 }
 500                 else
 501                 {
 502                         errorcount++;
 503                 }
 504
 505                 if (errorcount > 1)
 506                 {
 507                         LOG_WARNING("Block write error address 0x%x, wcount 0x%x", address, wcount);
 508                         return ERROR_JTAG_DEVICE_ERROR;
 509                 }
 510         }
 511
 512         return retval;
 513 }
 514
 515 int mem_ap_write_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 516 {
 517         int retval = ERROR_OK;
 518         int wcount, blocksize, writecount, i;
 519
 520         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 521
 522         wcount = count >> 1;
 523
 524         while (wcount > 0)
 525         {
 526                 int nbytes;
 527
 528                 /* Adjust to read within 4K block boundaries */
 529                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 530
 531                 if (wcount < blocksize)
 532                         blocksize = wcount;
 533
 534                 /* handle unaligned data at 4k boundary */
 535                 if (blocksize == 0)
 536                         blocksize = 1;
 537
 538                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 539                 writecount = blocksize;
 540
 541                 do
 542                 {
 543                         nbytes = MIN((writecount << 1), 4);
 544
 545                         if (nbytes < 4 )
 546                         {
 547                                 if (mem_ap_write_buf_u16(swjdp, buffer, nbytes, address) != ERROR_OK)
 548                                 {
 549                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 550                                         return ERROR_JTAG_DEVICE_ERROR;
 551                                 }
 552
 553                                 address += nbytes >> 1;
 554                         }
 555                         else
 556                         {
 557                                 u32 outvalue;
 558                                 memcpy(&outvalue, buffer, sizeof(u32));
 559
 560                                 for (i = 0; i < nbytes; i++ )
 561                                 {
 562                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 563                                         outvalue >>= 8;
 564                                         address++;
 565                                 }
 566
 567                                 memcpy(&outvalue, buffer, sizeof(u32));
 568                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 569                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 570                                 {
 571                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 572                                         return ERROR_JTAG_DEVICE_ERROR;
 573                                 }
 574                         }
 575
 576                         buffer += nbytes >> 1;
 577                         writecount -= nbytes >> 1;
 578
 579                 } while (writecount);
 580                 wcount -= blocksize;
 581         }
 582
 583         return retval;
 584 }
 585
 586 int mem_ap_write_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 587 {
 588         int retval = ERROR_OK;
 589
 590         if (count >= 4)
 591                 return mem_ap_write_buf_packed_u16(swjdp, buffer, count, address);
 592
 593         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 594
 595         while (count > 0)
 596         {
 597                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 598                 u16 svalue;
 599                 memcpy(&svalue, buffer, sizeof(u16));
 600                 u32 outvalue = (u32)svalue << 8 * (address & 0x3);
 601                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue );
 602                 retval = swjdp_transaction_endcheck(swjdp);
 603                 count -= 2;
 604                 address += 2;
 605                 buffer += 2;
 606         }
 607
 608         return retval;
 609 }
 610
 611 int mem_ap_write_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 612 {
 613         int retval = ERROR_OK;
 614         int wcount, blocksize, writecount, i;
 615
 616         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 617
 618         wcount = count;
 619
 620         while (wcount > 0)
 621         {
 622                 int nbytes;
 623
 624                 /* Adjust to read within 4K block boundaries */
 625                 blocksize = (0x1000 - (0xFFF & address));
 626
 627                 if (wcount < blocksize)
 628                         blocksize = wcount;
 629
 630                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 631                 writecount = blocksize;
 632
 633                 do
 634                 {
 635                         nbytes = MIN(writecount, 4);
 636
 637                         if (nbytes < 4 )
 638                         {
 639                                 if (mem_ap_write_buf_u8(swjdp, buffer, nbytes, address) != ERROR_OK)
 640                                 {
 641                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 642                                         return ERROR_JTAG_DEVICE_ERROR;
 643                                 }
 644
 645                                 address += nbytes;
 646                         }
 647                         else
 648                         {
 649                                 u32 outvalue;
 650                                 memcpy(&outvalue, buffer, sizeof(u32));
 651
 652                                 for (i = 0; i < nbytes; i++ )
 653                                 {
 654                                         *((u8*)buffer + (address & 0x3)) = outvalue;
 655                                         outvalue >>= 8;
 656                                         address++;
 657                                 }
 658
 659                                 memcpy(&outvalue, buffer, sizeof(u32));
 660                                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue);
 661                                 if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 662                                 {
 663                                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 664                                         return ERROR_JTAG_DEVICE_ERROR;
 665                                 }
 666                         }
 667
 668                         buffer += nbytes;
 669                         writecount -= nbytes;
 670
 671                 } while (writecount);
 672                 wcount -= blocksize;
 673         }
 674
 675         return retval;
 676 }
 677
 678 int mem_ap_write_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 679 {
 680         int retval = ERROR_OK;
 681
 682         if (count >= 4)
 683                 return mem_ap_write_buf_packed_u8(swjdp, buffer, count, address);
 684
 685         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 686
 687         while (count > 0)
 688         {
 689                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 690                 u32 outvalue = (u32)*buffer << 8 * (address & 0x3);
 691                 dap_ap_write_reg_u32(swjdp, AP_REG_DRW, outvalue );
 692                 retval = swjdp_transaction_endcheck(swjdp);
 693                 count--;
 694                 address++;
 695                 buffer++;
 696         }
 697
 698         return retval;
 699 }
 700
 701 /*********************************************************************************
 702 *                                                                                *
 703 * mem_ap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)  *
 704 *                                                                                *
 705 * Read block fast in target order (little endian) into a buffer                  *
 706 *                                                                                *
 707 **********************************************************************************/
 708 int mem_ap_read_buf_u32(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 709 {
 710         int wcount, blocksize, readcount, errorcount = 0, retval = ERROR_OK;
 711         u32 adr = address;
 712         u8* pBuffer = buffer;
 713
 714         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 715
 716         count >>= 2;
 717         wcount = count;
 718
 719         while (wcount > 0)
 720         {
 721                 /* Adjust to read within 4K block boundaries */
 722                 blocksize = (0x1000 - (0xFFF & address)) >> 2;
 723                 if (wcount < blocksize)
 724                         blocksize = wcount;
 725
 726                 /* handle unaligned data at 4k boundary */
 727                 if (blocksize == 0)
 728                         blocksize = 1;
 729
 730                 dap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_SINGLE, address);
 731
 732                 /* Scan out first read */
 733                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, NULL, NULL);
 734                 for (readcount = 0; readcount < blocksize - 1; readcount++)
 735                 {
 736                         /* Scan out read instruction and scan in previous value */
 737                         adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_APACC, AP_REG_DRW, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 738                 }
 739
 740                 /* Scan in last value */
 741                 adi_jtag_dp_scan(swjdp->jtag_info, SWJDP_IR_DPACC, DP_RDBUFF, DPAP_READ, 0, buffer + 4 * readcount, &swjdp->ack);
 742                 if (swjdp_transaction_endcheck(swjdp) == ERROR_OK)
 743                 {
 744                         wcount = wcount - blocksize;
 745                         address += 4 * blocksize;
 746                         buffer += 4 * blocksize;
 747                 }
 748                 else
 749                 {
 750                         errorcount++;
 751                 }
 752
 753                 if (errorcount > 1)
 754                 {
 755                         LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 756                         return ERROR_JTAG_DEVICE_ERROR;
 757                 }
 758         }
 759
 760         /* if we have an unaligned access - reorder data */
 761         if (adr & 0x3u)
 762         {
 763                 for (readcount = 0; readcount < count; readcount++)
 764                 {
 765                         int i;
 766                         u32 data;
 767                         memcpy(&data, pBuffer, sizeof(u32));
 768
 769                         for (i = 0; i < 4; i++ )
 770                         {
 771                                 *((u8*)pBuffer) = (data >> 8 * (adr & 0x3));
 772                                 pBuffer++;
 773                                 adr++;
 774                         }
 775                 }
 776         }
 777
 778         return retval;
 779 }
 780
 781 int mem_ap_read_buf_packed_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 782 {
 783         u32 invalue;
 784         int retval = ERROR_OK;
 785         int wcount, blocksize, readcount, i;
 786
 787         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 788
 789         wcount = count >> 1;
 790
 791         while (wcount > 0)
 792         {
 793                 int nbytes;
 794
 795                 /* Adjust to read within 4K block boundaries */
 796                 blocksize = (0x1000 - (0xFFF & address)) >> 1;
 797                 if (wcount < blocksize)
 798                         blocksize = wcount;
 799
 800                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_PACKED, address);
 801
 802                 /* handle unaligned data at 4k boundary */
 803                 if (blocksize == 0)
 804                         blocksize = 1;
 805                 readcount = blocksize;
 806
 807                 do
 808                 {
 809                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 810                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 811                         {
 812                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 813                                 return ERROR_JTAG_DEVICE_ERROR;
 814                         }
 815
 816                         nbytes = MIN((readcount << 1), 4);
 817
 818                         for (i = 0; i < nbytes; i++ )
 819                         {
 820                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 821                                 buffer++;
 822                                 address++;
 823                         }
 824
 825                         readcount -= (nbytes >> 1);
 826                 } while (readcount);
 827                 wcount -= blocksize;
 828         }
 829
 830         return retval;
 831 }
 832
 833 int mem_ap_read_buf_u16(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 834 {
 835         u32 invalue, i;
 836         int retval = ERROR_OK;
 837
 838         if (count >= 4)
 839                 return mem_ap_read_buf_packed_u16(swjdp, buffer, count, address);
 840
 841         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 842
 843         while (count > 0)
 844         {
 845                 dap_setup_accessport(swjdp, CSW_16BIT | CSW_ADDRINC_SINGLE, address);
 846                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 847                 retval = swjdp_transaction_endcheck(swjdp);
 848                 if (address & 0x1)
 849                 {
 850                         for (i = 0; i < 2; i++ )
 851                         {
 852                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 853                                 buffer++;
 854                                 address++;
 855                         }
 856                 }
 857                 else
 858                 {
 859                         u16 svalue = (invalue >> 8 * (address & 0x3));
 860                         memcpy(buffer, &svalue, sizeof(u16));
 861                         address += 2;
 862                         buffer += 2;
 863                 }
 864                 count -= 2;
 865         }
 866
 867         return retval;
 868 }
 869
 870 int mem_ap_read_buf_packed_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 871 {
 872         u32 invalue;
 873         int retval = ERROR_OK;
 874         int wcount, blocksize, readcount, i;
 875
 876         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 877
 878         wcount = count;
 879
 880         while (wcount > 0)
 881         {
 882                 int nbytes;
 883
 884                 /* Adjust to read within 4K block boundaries */
 885                 blocksize = (0x1000 - (0xFFF & address));
 886
 887                 if (wcount < blocksize)
 888                         blocksize = wcount;
 889
 890                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_PACKED, address);
 891                 readcount = blocksize;
 892
 893                 do
 894                 {
 895                         dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 896                         if (swjdp_transaction_endcheck(swjdp) != ERROR_OK)
 897                         {
 898                                 LOG_WARNING("Block read error address 0x%x, count 0x%x", address, count);
 899                                 return ERROR_JTAG_DEVICE_ERROR;
 900                         }
 901
 902                         nbytes = MIN(readcount, 4);
 903
 904                         for (i = 0; i < nbytes; i++ )
 905                         {
 906                                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 907                                 buffer++;
 908                                 address++;
 909                         }
 910
 911                         readcount -= nbytes;
 912                 } while (readcount);
 913                 wcount -= blocksize;
 914         }
 915
 916         return retval;
 917 }
 918
 919 int mem_ap_read_buf_u8(swjdp_common_t *swjdp, u8 *buffer, int count, u32 address)
 920 {
 921         u32 invalue;
 922         int retval = ERROR_OK;
 923
 924         if (count >= 4)
 925                 return mem_ap_read_buf_packed_u8(swjdp, buffer, count, address);
 926
 927         swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 928
 929         while (count > 0)
 930         {
 931                 dap_setup_accessport(swjdp, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
 932                 dap_ap_read_reg_u32(swjdp, AP_REG_DRW, &invalue );
 933                 retval = swjdp_transaction_endcheck(swjdp);
 934                 *((u8*)buffer) = (invalue >> 8 * (address & 0x3));
 935                 count--;
 936                 address++;
 937                 buffer++;
 938         }
 939
 940         return retval;
 941 }
 942
 943 int ahbap_debugport_init(swjdp_common_t *swjdp)
 944 {
 945         u32 idreg, romaddr, dummy;
 946         u32 ctrlstat;
 947         int cnt = 0;
 948         int retval;
 949
 950         LOG_DEBUG(" ");
 951
 952         swjdp->apsel = 0;
 953         swjdp->ap_csw_value = -1;
 954         swjdp->ap_tar_value = -1;
 955         swjdp->trans_mode = TRANS_MODE_ATOMIC;
 956         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 957         dap_dp_write_reg(swjdp, SSTICKYERR, DP_CTRL_STAT);
 958         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 959
 960         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
 961
 962         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 963         dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 964         if ((retval=jtag_execute_queue())!=ERROR_OK)
 965                 return retval;
 966
 967         /* Check that we have debug power domains activated */
 968         while (!(ctrlstat & CDBGPWRUPACK) && (cnt++ < 10))
 969         {
 970                 LOG_DEBUG("swjdp: wait CDBGPWRUPACK");
 971                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 972                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 973                         return retval;
 974                 alive_sleep(10);
 975         }
 976
 977         while (!(ctrlstat & CSYSPWRUPACK) && (cnt++ < 10))
 978         {
 979                 LOG_DEBUG("swjdp: wait CSYSPWRUPACK");
 980                 dap_dp_read_reg(swjdp, &ctrlstat, DP_CTRL_STAT);
 981                 if ((retval=jtag_execute_queue())!=ERROR_OK)
 982                         return retval;
 983                 alive_sleep(10);
 984         }
 985
 986         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 987         /* With debug power on we can activate OVERRUN checking */
 988         swjdp->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
 989         dap_dp_write_reg(swjdp, swjdp->dp_ctrl_stat, DP_CTRL_STAT);
 990         dap_dp_read_reg(swjdp, &dummy, DP_CTRL_STAT);
 991
 992         dap_ap_read_reg_u32(swjdp, 0xFC, &idreg);
 993         dap_ap_read_reg_u32(swjdp, 0xF8, &romaddr);
 994
 995         LOG_DEBUG("AHB-AP ID Register 0x%x, Debug ROM Address 0x%x", idreg, romaddr);
 996
 997         return ERROR_OK;
 998 }
 999
1000
1001 char * class_description[16] ={
1002         "Reserved",
1003         "ROM table","Reserved","Reserved","Reserved","Reserved","Reserved","Reserved","Reserved",
1004         "CoreSight component","Reserved","Peripheral Test Block","Reserved","DESS","Generic IP component","Non standard layout"};
1005
1006 int dap_info_command(struct command_context_s *cmd_ctx, swjdp_common_t *swjdp, int apsel)
1007 {
1008
1009         u32 dbgbase,apid;
1010         int romtable_present = 0;
1011         u8 mem_ap;
1012         u32 apselold;
1013
1014         apselold = swjdp->apsel;
1015         dap_ap_select(swjdp, apsel);
1016         dap_ap_read_reg_u32(swjdp, 0xF8, &dbgbase);
1017         dap_ap_read_reg_u32(swjdp, 0xFC, &apid);
1018         swjdp_transaction_endcheck(swjdp);
1019         /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1020         mem_ap = ((apid&0x10000)&&((apid&0x0F)!=0));
1021         command_print(cmd_ctx, "ap identification register 0x%8.8x", apid);
1022         if (apid)
1023         {
1024                 switch (apid&0x0F)
1025                 {
1026                         case 0:
1027                                 command_print(cmd_ctx, "\tType is jtag-ap");
1028                                 break;
1029                         case 1:
1030                                 command_print(cmd_ctx, "\tType is mem-ap AHB");
1031                                 break;
1032                         case 2:
1033                                 command_print(cmd_ctx, "\tType is mem-ap APB");
1034                                 break;
1035                         default:
1036                                 command_print(cmd_ctx, "\tUnknown AP-type");
1037                         break;
1038                 }
1039                 command_print(cmd_ctx, "ap debugbase 0x%8.8x", dbgbase);
1040         }
1041         else
1042         {
1043                 command_print(cmd_ctx, "No AP found at this apsel 0x%x", apsel);
1044         }
1045
1046         romtable_present = ((mem_ap)&&(dbgbase != 0xFFFFFFFF));
1047         if (romtable_present)
1048         {
1049                 u32 cid0,cid1,cid2,cid3,memtype,romentry;
1050                 u16 entry_offset;
1051                 /* bit 16 of apid indicates a memory access port */
1052                 if (dbgbase&0x02)
1053                 {
1054                         command_print(cmd_ctx, "\tValid ROM table present");
1055                 }
1056                 else
1057                 {
1058                         command_print(cmd_ctx, "\tROM table in legacy format" );
1059                 }
1060                 /* Now we read ROM table ID registers, ref. ARM IHI 0029B sec  */
1061                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF0, &cid0);
1062                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF4, &cid1);
1063                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFF8, &cid2);
1064                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFFC, &cid3);
1065                 mem_ap_read_u32(swjdp, (dbgbase&0xFFFFF000)|0xFCC, &memtype);
1066                 swjdp_transaction_endcheck(swjdp);
1067                 command_print(cmd_ctx, "\tCID3 0x%x, CID2 0x%x, CID1 0x%x, CID0, 0x%x",cid3,cid2,cid1,cid0);
1068                 if (memtype&0x01)
1069                 {
1070                         command_print(cmd_ctx, "\tMEMTYPE system memory present on bus");
1071                 }
1072                 else
1073                 {
1074                         command_print(cmd_ctx, "\tMEMTYPE system memory not present. Dedicated debug bus" );
1075                 }
1076
1077                 /* Now we read ROM table entries from dbgbase&0xFFFFF000)|0x000 until we get 0x00000000 */
1078                 entry_offset = 0;
1079                 do
1080                 {
1081                         mem_ap_read_atomic_u32(swjdp, (dbgbase&0xFFFFF000)|entry_offset, &romentry);
1082                         command_print(cmd_ctx, "\tROMTABLE[0x%x] = 0x%x",entry_offset,romentry);
1083                         if (romentry&0x01)
1084                         {
1085                                 u32 c_cid0,c_cid1,c_cid2,c_cid3,c_pid0,c_pid1,c_pid2,c_pid3,c_pid4,component_start;
1086                                 u32 component_base = (u32)((dbgbase&0xFFFFF000)+(int)(romentry&0xFFFFF000));
1087                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE0, &c_pid0);
1088                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE4, &c_pid1);
1089                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFE8, &c_pid2);
1090                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFEC, &c_pid3);
1091                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFD0, &c_pid4);
1092                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF0, &c_cid0);
1093                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF4, &c_cid1);
1094                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFF8, &c_cid2);
1095                                 mem_ap_read_atomic_u32(swjdp, (component_base&0xFFFFF000)|0xFFC, &c_cid3);
1096                                 component_start = component_base - 0x1000*(c_pid4>>4);
1097                                 command_print(cmd_ctx, "\t\tComponent base address 0x%x, pid4 0x%x, start address 0x%x",component_base,c_pid4,component_start);
1098                                 command_print(cmd_ctx, "\t\tComponent cid1 0x%x, class is %s",c_cid1,class_description[(c_cid1>>4)&0xF]); /* Se ARM DDI 0314 C Table 2.2 */
1099                                 command_print(cmd_ctx, "\t\tCID3 0x%x, CID2 0x%x, CID1 0x%x, CID0, 0x%x",c_cid3,c_cid2,c_cid1,c_cid0);
1100                                 command_print(cmd_ctx, "\t\tPID3 0x%x, PID2 0x%x, PID1 0x%x, PID0, 0x%x",c_pid3,c_pid2,c_pid1,c_pid0);
1101                                 /* For CoreSight components,  (c_cid1>>4)&0xF==9 , we also read 0xFC8 DevId and 0xFCC DevType */
1102                         }
1103                         else
1104                         {
1105                                 if (romentry)
1106                                         command_print(cmd_ctx, "\t\tComponent not present");
1107                                 else
1108                                         command_print(cmd_ctx, "\t\tEnd of ROM table");
1109                         }
1110                         entry_offset += 4;
1111                 } while (romentry>0);
1112         }
1113         else
1114         {
1115                 command_print(cmd_ctx, "\tNo ROM table present");
1116         }
1117         dap_ap_select(swjdp, apselold);
1118
1119         return ERROR_OK;
1120 }
1121