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