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