+ return ERROR_OK;
+}
+
+/**
+ * Wrapper around struct rtp_ops::mem_ap_header.
+ * Input parameter @a retval is propagated.
+ */
+static int rtp_ops_mem_ap_header(const struct rtp_ops *ops,
+ int retval, struct adiv5_ap *ap, uint64_t dbgbase, uint32_t apid, int depth)
+{
+ if (!ops->mem_ap_header)
+ return retval;
+
+ int retval1 = ops->mem_ap_header(retval, ap, dbgbase, apid, depth, ops->priv);
+ if (retval != ERROR_OK)
+ return retval;
+ return retval1;
+}
+
+/**
+ * Wrapper around struct rtp_ops::cs_component.
+ * Input parameter @a retval is propagated.
+ */
+static int rtp_ops_cs_component(const struct rtp_ops *ops,
+ int retval, struct cs_component_vals *v, int depth)
+{
+ if (!ops->cs_component)
+ return retval;
+
+ int retval1 = ops->cs_component(retval, v, depth, ops->priv);
+ if (retval != ERROR_OK)
+ return retval;
+ return retval1;
+}
+
+/**
+ * Wrapper around struct rtp_ops::rom_table_entry.
+ * Input parameter @a retval is propagated.
+ */
+static int rtp_ops_rom_table_entry(const struct rtp_ops *ops,
+ int retval, int depth, unsigned int offset, uint64_t romentry)
+{
+ if (!ops->rom_table_entry)
+ return retval;
+
+ int retval1 = ops->rom_table_entry(retval, depth, offset, romentry, ops->priv);
+ if (retval != ERROR_OK)
+ return retval;
+ return retval1;
+}
+
+/* Broken ROM tables can have circular references. Stop after a while */
+#define ROM_TABLE_MAX_DEPTH (16)
+
+/**
+ * Value used only during lookup of a CoreSight component in ROM table.
+ * Return CORESIGHT_COMPONENT_FOUND when component is found.
+ * Return ERROR_OK when component is not found yet.
+ * Return any other ERROR_* in case of error.
+ */
+#define CORESIGHT_COMPONENT_FOUND (1)
+
+static int rtp_ap(const struct rtp_ops *ops, struct adiv5_ap *ap, int depth);
+static int rtp_cs_component(enum coresight_access_mode mode, const struct rtp_ops *ops,
+ struct adiv5_ap *ap, target_addr_t dbgbase, bool *is_mem_ap, int depth);
+
+static int rtp_rom_loop(enum coresight_access_mode mode, const struct rtp_ops *ops,
+ struct adiv5_ap *ap, target_addr_t base_address, int depth,
+ unsigned int width, unsigned int max_entries)
+{
+ /* ADIv6 AP ROM table provide offset from current AP */
+ if (mode == CS_ACCESS_AP)
+ base_address = ap->ap_num;
+
+ assert(IS_ALIGNED(base_address, ARM_CS_ALIGN));
+
+ unsigned int offset = 0;
+ while (max_entries--) {
+ uint64_t romentry;
+ uint32_t romentry_low, romentry_high;
+ target_addr_t component_base;
+ unsigned int saved_offset = offset;
+
+ int retval = dap_queue_read_reg(mode, ap, base_address, offset, &romentry_low);
+ offset += 4;
+ if (retval == ERROR_OK && width == 64) {
+ retval = dap_queue_read_reg(mode, ap, base_address, offset, &romentry_high);
+ offset += 4;
+ }
+ if (retval == ERROR_OK)
+ retval = dap_run(ap->dap);
+ if (retval != ERROR_OK) {
+ LOG_DEBUG("Failed read ROM table entry");
+ return retval;
+ }
+
+ if (width == 64) {
+ romentry = (((uint64_t)romentry_high) << 32) | romentry_low;
+ component_base = base_address +
+ ((((uint64_t)romentry_high) << 32) | (romentry_low & ARM_CS_ROMENTRY_OFFSET_MASK));
+ } else {
+ romentry = romentry_low;
+ /* "romentry" is signed */
+ component_base = base_address + (int32_t)(romentry_low & ARM_CS_ROMENTRY_OFFSET_MASK);
+ if (!is_64bit_ap(ap))
+ component_base = (uint32_t)component_base;
+ }
+ retval = rtp_ops_rom_table_entry(ops, retval, depth, saved_offset, romentry);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if (romentry == 0) {
+ /* End of ROM table */
+ break;
+ }
+
+ if (!(romentry & ARM_CS_ROMENTRY_PRESENT))
+ continue;
+
+ /* Recurse */
+ if (mode == CS_ACCESS_AP) {
+ struct adiv5_ap *next_ap = dap_get_ap(ap->dap, component_base);
+ if (!next_ap) {
+ LOG_DEBUG("Wrong AP # 0x%" PRIx64, component_base);
+ continue;
+ }
+ retval = rtp_ap(ops, next_ap, depth + 1);
+ dap_put_ap(next_ap);
+ } else {
+ /* mode == CS_ACCESS_MEM_AP */
+ retval = rtp_cs_component(mode, ops, ap, component_base, NULL, depth + 1);
+ }
+ if (retval == CORESIGHT_COMPONENT_FOUND)
+ return CORESIGHT_COMPONENT_FOUND;
+ if (retval != ERROR_OK) {
+ /* TODO: do we need to send an ABORT before continuing? */
+ LOG_DEBUG("Ignore error parsing CoreSight component");
+ continue;
+ }
+ }
+
+ return ERROR_OK;
+}
+
+static int rtp_cs_component(enum coresight_access_mode mode, const struct rtp_ops *ops,
+ struct adiv5_ap *ap, target_addr_t base_address, bool *is_mem_ap, int depth)
+{
+ struct cs_component_vals v;
+ int retval;
+
+ assert(IS_ALIGNED(base_address, ARM_CS_ALIGN));
+
+ if (is_mem_ap)
+ *is_mem_ap = false;
+
+ if (depth > ROM_TABLE_MAX_DEPTH)
+ retval = ERROR_FAIL;
+ else
+ retval = rtp_read_cs_regs(mode, ap, base_address, &v);
+
+ retval = rtp_ops_cs_component(ops, retval, &v, depth);
+ if (retval == CORESIGHT_COMPONENT_FOUND)
+ return CORESIGHT_COMPONENT_FOUND;
+ if (retval != ERROR_OK)