src/target/rtt.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2
   3 /*
   4  * Copyright (C) 2016-2020 by Marc Schink <dev@zapb.de>
   5  */
   6
   7 #ifdef HAVE_CONFIG_H
   8 #include "config.h"
   9 #endif
  10
  11 #include <stddef.h>
  12 #include <stdint.h>
  13 #include <helper/log.h>
  14 #include <helper/binarybuffer.h>
  15 #include <helper/command.h>
  16 #include <rtt/rtt.h>
  17 #include <target/rtt.h>
  18
  19 #include "target.h"
  20
  21 static int read_rtt_channel(struct target *target,
  22                 const struct rtt_control *ctrl, unsigned int channel_index,
  23                 enum rtt_channel_type type, struct rtt_channel *channel)
  24 {
  25         int ret;
  26         uint8_t buf[RTT_CHANNEL_SIZE];
  27         target_addr_t address;
  28
  29         address = ctrl->address + RTT_CB_SIZE + (channel_index * RTT_CHANNEL_SIZE);
  30
  31         if (type == RTT_CHANNEL_TYPE_DOWN)
  32                 address += ctrl->num_up_channels * RTT_CHANNEL_SIZE;
  33
  34         ret = target_read_buffer(target, address, RTT_CHANNEL_SIZE, buf);
  35
  36         if (ret != ERROR_OK)
  37                 return ret;
  38
  39         channel->address = address;
  40         channel->name_addr = buf_get_u32(buf + 0, 0, 32);
  41         channel->buffer_addr = buf_get_u32(buf + 4, 0, 32);
  42         channel->size = buf_get_u32(buf + 8, 0, 32);
  43         channel->write_pos = buf_get_u32(buf + 12, 0, 32);
  44         channel->read_pos = buf_get_u32(buf + 16, 0, 32);
  45         channel->flags = buf_get_u32(buf + 20, 0, 32);
  46
  47         return ERROR_OK;
  48 }
  49
  50 int target_rtt_start(struct target *target, const struct rtt_control *ctrl,
  51                 void *user_data)
  52 {
  53         return ERROR_OK;
  54 }
  55
  56 int target_rtt_stop(struct target *target, void *user_data)
  57 {
  58         return ERROR_OK;
  59 }
  60
  61 static int read_channel_name(struct target *target, target_addr_t address,
  62                 char *name, size_t length)
  63 {
  64         size_t offset;
  65
  66         offset = 0;
  67
  68         while (offset < length) {
  69                 int ret;
  70                 size_t read_length;
  71
  72                 read_length = MIN(32, length - offset);
  73                 ret = target_read_buffer(target, address + offset, read_length,
  74                         (uint8_t *)name + offset);
  75
  76                 if (ret != ERROR_OK)
  77                         return ret;
  78
  79                 if (memchr(name + offset, '\0', read_length))
  80                         return ERROR_OK;
  81
  82                 offset += read_length;
  83         }
  84
  85         name[length - 1] = '\0';
  86
  87         return ERROR_OK;
  88 }
  89
  90 static int write_to_channel(struct target *target,
  91                 const struct rtt_channel *channel, const uint8_t *buffer,
  92                 size_t *length)
  93 {
  94         int ret;
  95         uint32_t len;
  96
  97         if (!*length)
  98                 return ERROR_OK;
  99
 100         if (channel->write_pos == channel->read_pos) {
 101                 uint32_t first_length;
 102
 103                 len = MIN(*length, channel->size - 1);
 104                 first_length = MIN(len, channel->size - channel->write_pos);
 105
 106                 ret = target_write_buffer(target,
 107                         channel->buffer_addr + channel->write_pos, first_length,
 108                         buffer);
 109
 110                 if (ret != ERROR_OK)
 111                         return ret;
 112
 113                 ret = target_write_buffer(target, channel->buffer_addr,
 114                         len - first_length, buffer + first_length);
 115
 116                 if (ret != ERROR_OK)
 117                         return ret;
 118         } else if (channel->write_pos < channel->read_pos) {
 119                 len = MIN(*length, channel->read_pos - channel->write_pos - 1);
 120
 121                 if (!len) {
 122                         *length = 0;
 123                         return ERROR_OK;
 124                 }
 125
 126                 ret = target_write_buffer(target,
 127                         channel->buffer_addr + channel->write_pos, len, buffer);
 128
 129                 if (ret != ERROR_OK)
 130                         return ret;
 131         } else {
 132                 uint32_t first_length;
 133
 134                 len = MIN(*length,
 135                         channel->size - channel->write_pos + channel->read_pos - 1);
 136
 137                 if (!len) {
 138                         *length = 0;
 139                         return ERROR_OK;
 140                 }
 141
 142                 first_length = MIN(len, channel->size - channel->write_pos);
 143
 144                 ret = target_write_buffer(target,
 145                         channel->buffer_addr + channel->write_pos, first_length,
 146                         buffer);
 147
 148                 if (ret != ERROR_OK)
 149                         return ret;
 150
 151                 buffer = buffer + first_length;
 152
 153                 ret = target_write_buffer(target, channel->buffer_addr,
 154                         len - first_length, buffer);
 155
 156                 if (ret != ERROR_OK)
 157                         return ret;
 158         }
 159
 160         ret = target_write_u32(target, channel->address + 12,
 161                 (channel->write_pos + len) % channel->size);
 162
 163         if (ret != ERROR_OK)
 164                 return ret;
 165
 166         *length = len;
 167
 168         return ERROR_OK;
 169 }
 170
 171 static bool channel_is_active(const struct rtt_channel *channel)
 172 {
 173         if (!channel)
 174                 return false;
 175
 176         if (!channel->size)
 177                 return false;
 178
 179         return true;
 180 }
 181
 182 int target_rtt_write_callback(struct target *target, struct rtt_control *ctrl,
 183                 unsigned int channel_index, const uint8_t *buffer, size_t *length,
 184                 void *user_data)
 185 {
 186         int ret;
 187         struct rtt_channel channel;
 188
 189         ret = read_rtt_channel(target, ctrl, channel_index,
 190                 RTT_CHANNEL_TYPE_DOWN, &channel);
 191
 192         if (ret != ERROR_OK) {
 193                 LOG_ERROR("rtt: Failed to read down-channel %u description",
 194                         channel_index);
 195                 return ret;
 196         }
 197
 198         if (!channel_is_active(&channel)) {
 199                 LOG_WARNING("rtt: Down-channel %u is not active", channel_index);
 200                 return ERROR_OK;
 201         }
 202
 203         if (channel.size < RTT_CHANNEL_BUFFER_MIN_SIZE) {
 204                 LOG_WARNING("rtt: Down-channel %u is not large enough",
 205                         channel_index);
 206                 return ERROR_OK;
 207         }
 208
 209         ret = write_to_channel(target, &channel, buffer, length);
 210
 211         if (ret != ERROR_OK)
 212                 return ret;
 213
 214         LOG_DEBUG("rtt: Wrote %zu bytes into down-channel %u", *length,
 215                 channel_index);
 216
 217         return ERROR_OK;
 218 }
 219
 220 int target_rtt_read_control_block(struct target *target,
 221                 target_addr_t address, struct rtt_control *ctrl, void *user_data)
 222 {
 223         int ret;
 224         uint8_t buf[RTT_CB_SIZE];
 225
 226         ret = target_read_buffer(target, address, RTT_CB_SIZE, buf);
 227
 228         if (ret != ERROR_OK)
 229                 return ret;
 230
 231         memcpy(ctrl->id, buf, RTT_CB_MAX_ID_LENGTH);
 232         ctrl->id[RTT_CB_MAX_ID_LENGTH - 1] = '\0';
 233         ctrl->num_up_channels = buf_get_u32(buf + RTT_CB_MAX_ID_LENGTH + 0,
 234                 0, 32);
 235         ctrl->num_down_channels = buf_get_u32(buf + RTT_CB_MAX_ID_LENGTH + 4,
 236                 0, 32);
 237
 238         return ERROR_OK;
 239 }
 240
 241 int target_rtt_find_control_block(struct target *target,
 242                 target_addr_t *address, size_t size, const char *id, bool *found,
 243                 void *user_data)
 244 {
 245         target_addr_t address_end = *address + size;
 246         uint8_t buf[1024];
 247
 248         *found = false;
 249
 250         size_t id_matched_length = 0;
 251         const size_t id_length = strlen(id);
 252
 253         LOG_INFO("rtt: Searching for control block '%s'", id);
 254
 255         for (target_addr_t addr = *address; addr < address_end; addr += sizeof(buf)) {
 256                 int ret;
 257
 258                 const size_t buf_size = MIN(sizeof(buf), address_end - addr);
 259                 ret = target_read_buffer(target, addr, buf_size, buf);
 260
 261                 if (ret != ERROR_OK)
 262                         return ret;
 263
 264                 for (size_t buf_off = 0; buf_off < buf_size; buf_off++) {
 265                         if (id_matched_length > 0 &&
 266                             buf[buf_off] != id[id_matched_length]) {
 267                                 /* Start from beginning */
 268                                 id_matched_length = 0;
 269                         }
 270
 271                         if (buf[buf_off] == id[id_matched_length])
 272                                 id_matched_length++;
 273
 274                         if (id_matched_length == id_length) {
 275                                 *address = addr + buf_off + 1 - id_length;
 276                                 *found = true;
 277                                 return ERROR_OK;
 278                         }
 279                 }
 280         }
 281
 282         return ERROR_OK;
 283 }
 284
 285 int target_rtt_read_channel_info(struct target *target,
 286                 const struct rtt_control *ctrl, unsigned int channel_index,
 287                 enum rtt_channel_type type, struct rtt_channel_info *info,
 288                 void *user_data)
 289 {
 290         int ret;
 291         struct rtt_channel channel;
 292
 293         ret = read_rtt_channel(target, ctrl, channel_index, type, &channel);
 294
 295         if (ret != ERROR_OK) {
 296                 LOG_ERROR("rtt: Failed to read channel %u description",
 297                         channel_index);
 298                 return ret;
 299         }
 300
 301         ret = read_channel_name(target, channel.name_addr, info->name,
 302                 info->name_length);
 303
 304         if (ret != ERROR_OK)
 305                 return ret;
 306
 307         info->size = channel.size;
 308         info->flags = channel.flags;
 309
 310         return ERROR_OK;
 311 }
 312
 313 static int read_from_channel(struct target *target,
 314                 const struct rtt_channel *channel, uint8_t *buffer,
 315                 size_t *length)
 316 {
 317         int ret;
 318         uint32_t len;
 319
 320         if (!*length)
 321                 return ERROR_OK;
 322
 323         if (channel->read_pos == channel->write_pos) {
 324                 len = 0;
 325         } else if (channel->read_pos < channel->write_pos) {
 326                 len = MIN(*length, channel->write_pos - channel->read_pos);
 327
 328                 ret = target_read_buffer(target,
 329                         channel->buffer_addr + channel->read_pos, len, buffer);
 330
 331                 if (ret != ERROR_OK)
 332                         return ret;
 333         } else {
 334                 uint32_t first_length;
 335
 336                 len = MIN(*length,
 337                         channel->size - channel->read_pos + channel->write_pos);
 338                 first_length = MIN(len, channel->size - channel->read_pos);
 339
 340                 ret = target_read_buffer(target,
 341                         channel->buffer_addr + channel->read_pos, first_length, buffer);
 342
 343                 if (ret != ERROR_OK)
 344                         return ret;
 345
 346                 ret = target_read_buffer(target, channel->buffer_addr,
 347                         len - first_length, buffer + first_length);
 348
 349                 if (ret != ERROR_OK)
 350                         return ret;
 351         }
 352
 353         if (len > 0) {
 354                 ret = target_write_u32(target, channel->address + 16,
 355                         (channel->read_pos + len) % channel->size);
 356
 357                 if (ret != ERROR_OK)
 358                         return ret;
 359         }
 360
 361         *length = len;
 362
 363         return ERROR_OK;
 364 }
 365
 366 int target_rtt_read_callback(struct target *target,
 367                 const struct rtt_control *ctrl, struct rtt_sink_list **sinks,
 368                 size_t num_channels, void *user_data)
 369 {
 370         num_channels = MIN(num_channels, ctrl->num_up_channels);
 371
 372         for (size_t i = 0; i < num_channels; i++) {
 373                 int ret;
 374                 struct rtt_channel channel;
 375                 uint8_t buffer[1024];
 376                 size_t length;
 377
 378                 if (!sinks[i])
 379                         continue;
 380
 381                 ret = read_rtt_channel(target, ctrl, i, RTT_CHANNEL_TYPE_UP,
 382                         &channel);
 383
 384                 if (ret != ERROR_OK) {
 385                         LOG_ERROR("rtt: Failed to read up-channel %zu description", i);
 386                         return ret;
 387                 }
 388
 389                 if (!channel_is_active(&channel)) {
 390                         LOG_WARNING("rtt: Up-channel %zu is not active", i);
 391                         continue;
 392                 }
 393
 394                 if (channel.size < RTT_CHANNEL_BUFFER_MIN_SIZE) {
 395                         LOG_WARNING("rtt: Up-channel %zu is not large enough", i);
 396                         continue;
 397                 }
 398
 399                 length = sizeof(buffer);
 400                 ret = read_from_channel(target, &channel, buffer, &length);
 401
 402                 if (ret != ERROR_OK) {
 403                         LOG_ERROR("rtt: Failed to read from up-channel %zu", i);
 404                         return ret;
 405                 }
 406
 407                 for (struct rtt_sink_list *sink = sinks[i]; sink; sink = sink->next)
 408                         sink->read(i, buffer, length, sink->user_data);
 409         }
 410
 411         return ERROR_OK;
 412 }