src/target/rtt.c

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