git.sur5r.net Git - openocd/blob - src/rtos/linux.c

   1 /***************************************************************************
   2  *   Copyright (C) 2011 by STEricsson                                      *
   3  *   Heythem Bouhaja heythem.bouhaja@stericsson.com   : creation           *
   4  *   Michel JAOUEN michel.jaouen@stericsson.com : adaptation to rtos       *
   5  *                                                                         *
   6  *   This program is free software; you can redistribute it and/or modify  *
   7  *   it under the terms of the GNU General Public License as published by  *
   8  *   the Free Software Foundation; either version 2 of the License, or     *
   9  *   (at your option) any later version.                                   *
  10  *                                                                         *
  11  *   This program is distributed in the hope that it will be useful,       *
  12  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  13  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  14  *   GNU General Public License for more details.                          *
  15  *                                                                         *
  16  *   You should have received a copy of the GNU General Public License     *
  17  *   along with this program; if not, write to the                         *
  18  *   Free Software Foundation, Inc.,                                       *
  19  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  20  ***************************************************************************/
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include "config.h"
  24 #endif
  25
  26 #include <helper/time_support.h>
  27 #include <jtag/jtag.h>
  28 #include "target/target.h"
  29 #include "target/target_type.h"
  30 #include "helper/log.h"
  31 #include "helper/types.h"
  32 #include "rtos.h"
  33 #include "rtos_standard_stackings.h"
  34 #include <target/register.h>
  35 #include "server/gdb_server.h"
  36
  37 #define LINUX_USER_KERNEL_BORDER 0xc0000000
  38 #include "linux_header.h"
  39 #define PHYS
  40 #define MAX_THREADS 200
  41 /*  specific task  */
  42 struct linux_os {
  43         char *name;
  44         uint32_t init_task_addr;
  45         int thread_count;
  46         int threadid_count;
  47         int preupdtate_threadid_count;
  48         int nr_cpus;
  49         int threads_lookup;
  50         int threads_needs_update;
  51         struct current_thread *current_threads;
  52         struct threads *thread_list;
  53         /*  virt2phys parameter */
  54         uint32_t phys_mask;
  55         uint32_t phys_base;
  56 };
  57
  58 struct current_thread {
  59         int64_t threadid;
  60         int32_t core_id;
  61 #ifdef PID_CHECK
  62         uint32_t pid;
  63 #endif
  64         uint32_t TS;
  65         struct current_thread *next;
  66 };
  67
  68 struct threads {
  69         char name[17];
  70         uint32_t base_addr;     /*  address to read magic */
  71         uint32_t state;         /*  magic value : filled only at creation */
  72         uint32_t pid;           /* linux pid : id for identifying a thread */
  73         uint32_t oncpu;         /* content cpu number in current thread */
  74         uint32_t asid;          /*  filled only at creation  */
  75         int64_t threadid;
  76         int status;             /* dead = 1 alive = 2 current = 3 alive and current */
  77         /*  value that should not change during the live of a thread ? */
  78         uint32_t thread_info_addr;      /*  contain latest thread_info_addr computed */
  79         /*  retrieve from thread_info */
  80         struct cpu_context *context;
  81         struct threads *next;
  82 };
  83
  84 struct cpu_context {
  85         uint32_t R4;
  86         uint32_t R5;
  87         uint32_t R6;
  88         uint32_t R7;
  89         uint32_t R8;
  90         uint32_t R9;
  91         uint32_t IP;
  92         uint32_t FP;
  93         uint32_t SP;
  94         uint32_t PC;
  95         uint32_t preempt_count;
  96 };
  97 struct cpu_context *cpu_context_read(struct target *target, uint32_t base_addr,
  98                                      uint32_t *info_addr);
  99 static int insert_into_threadlist(struct target *target, struct threads *t);
 100
 101 static int linux_os_create(struct target *target);
 102
 103 static int linux_os_dummy_update(struct rtos *rtos)
 104 {
 105         /*  update is done only when thread request come
 106          *  too many thread to do it on each stop */
 107         return 0;
 108 }
 109
 110 static int linux_compute_virt2phys(struct target *target, uint32_t address)
 111 {
 112         struct linux_os *linux_os = (struct linux_os *)
 113                 target->rtos->rtos_specific_params;
 114         uint32_t pa = 0;
 115         int retval = target->type->virt2phys(target, address, &pa);
 116         if (retval != ERROR_OK) {
 117                 LOG_ERROR("Cannot compute linux virt2phys translation");
 118                 /*  fixes default address  */
 119                 linux_os->phys_base = 0;
 120                 return ERROR_FAIL;
 121         }
 122
 123         linux_os->init_task_addr = address;
 124         address = address & linux_os->phys_mask;
 125         linux_os->phys_base = pa - address;
 126         return ERROR_OK;
 127 }
 128
 129 static int linux_read_memory(struct target *target,
 130         uint32_t address, uint32_t size, uint32_t count,
 131         uint8_t *buffer)
 132 {
 133 #ifdef PHYS
 134         struct linux_os *linux_os = (struct linux_os *)
 135                 target->rtos->rtos_specific_params;
 136         uint32_t pa = (address & linux_os->phys_mask) + linux_os->phys_base;
 137 #endif
 138         if (address < 0xc000000) {
 139                 LOG_ERROR("linux awareness : address in user space");
 140                 return ERROR_FAIL;
 141         }
 142 #ifdef PHYS
 143         target->type->read_phys_memory(target, pa, size, count, buffer);
 144 #endif
 145         target->type->read_memory(target, address, size, count, buffer);
 146         return ERROR_OK;
 147 }
 148
 149 static char *reg_converter(char *buffer, void *reg, int size)
 150 {
 151         int i;
 152
 153         for (i = 0; i < size; i++)
 154                 buffer += sprintf(buffer, "%02x", ((uint8_t *) reg)[i]);
 155
 156         return buffer;
 157 }
 158
 159 int fill_buffer(struct target *target, uint32_t addr, uint8_t *buffer)
 160 {
 161
 162         if ((addr & 0xfffffffc) != addr)
 163                 LOG_INFO("unaligned address %x!!", addr);
 164
 165         int retval = linux_read_memory(target, addr, 4, 1, buffer);
 166         return retval;
 167
 168 }
 169
 170 uint32_t get_buffer(struct target *target, const uint8_t *buffer)
 171 {
 172         uint32_t value = 0;
 173         const uint8_t *value_ptr = buffer;
 174         value = target_buffer_get_u32(target, value_ptr);
 175         return value;
 176 }
 177
 178 static int linux_os_thread_reg_list(struct rtos *rtos,
 179         int64_t thread_id, char **hex_reg_list)
 180 {
 181         struct target *target = rtos->target;
 182         struct linux_os *linux_os = (struct linux_os *)
 183                 target->rtos->rtos_specific_params;
 184         int i = 0;
 185         struct current_thread *tmp = linux_os->current_threads;
 186         struct current_thread *next;
 187         char *hex_string;
 188         int found = 0;
 189         int retval;
 190         /*  check if a current thread is requested  */
 191         next = tmp;
 192
 193         do {
 194                 if (next->threadid == thread_id)
 195                         found = 1;
 196                 else
 197                         next = next->next;
 198         } while ((found == 0) && (next != tmp) && (next != NULL));
 199
 200         if (found == 1) {
 201                 /*  search target to perfom the access  */
 202                 struct reg **reg_list;
 203                 int reg_list_size, reg_packet_size = 0;
 204                 struct target_list *head;
 205                 head = target->head;
 206                 found = 0;
 207                 do {
 208                         if (head->target->coreid == next->core_id) {
 209
 210                                 target = head->target;
 211                                 found = 1;
 212                         } else
 213                                 head = head->next;
 214
 215                 } while ((head != (struct target_list *)NULL) && (found == 0));
 216
 217                 if (found == 0) {
 218                         LOG_ERROR
 219                         (
 220                                 "current thread %" PRIx64 ": no target to perform access of core id %x",
 221                                 thread_id,
 222                                 next->core_id);
 223                         return ERROR_FAIL;
 224                 }
 225
 226                 /*LOG_INFO("thread %lx current on core %x",thread_id,
 227                  * target->coreid);*/
 228                 retval =
 229                         target_get_gdb_reg_list(target, &reg_list, &reg_list_size);
 230
 231                 if (retval != ERROR_OK)
 232                         return retval;
 233
 234                 for (i = 0; i < reg_list_size; i++)
 235                         reg_packet_size += reg_list[i]->size;
 236
 237                 assert(reg_packet_size > 0);
 238
 239                 *hex_reg_list = malloc(DIV_ROUND_UP(reg_packet_size, 8) * 2);
 240
 241                 hex_string = *hex_reg_list;
 242
 243                 for (i = 0; i < reg_list_size; i++) {
 244                         if (!reg_list[i]->valid)
 245                                 reg_list[i]->type->get(reg_list[i]);
 246
 247                         hex_string = reg_converter(hex_string,
 248                                         reg_list[i]->value,
 249                                         (reg_list[i]->size) / 8);
 250                 }
 251
 252                 free(reg_list);
 253
 254         } else {
 255                 struct threads *temp = linux_os->thread_list;
 256                 *hex_reg_list = (char *)calloc(1, 500 * sizeof(char));
 257                 hex_string = *hex_reg_list;
 258
 259                 for (i = 0; i < 16; i++)
 260                         hex_string += sprintf(hex_string, "%02x", 0);
 261
 262                 while ((temp != NULL) &&
 263                                 (temp->threadid != target->rtos->current_threadid))
 264                         temp = temp->next;
 265
 266                 if (temp != NULL) {
 267                         if (temp->context == NULL)
 268                                 temp->context = cpu_context_read(target,
 269                                                 temp->
 270                                                 base_addr,
 271                                                 &temp->
 272                                                 thread_info_addr);
 273
 274                         hex_string =
 275                                 reg_converter(hex_string, &temp->context->R4, 4);
 276                         hex_string =
 277                                 reg_converter(hex_string, &temp->context->R5, 4);
 278                         hex_string =
 279                                 reg_converter(hex_string, &temp->context->R6, 4);
 280                         hex_string =
 281                                 reg_converter(hex_string, &temp->context->R7, 4);
 282                         hex_string =
 283                                 reg_converter(hex_string, &temp->context->R8, 4);
 284                         hex_string =
 285                                 reg_converter(hex_string, &temp->context->R9, 4);
 286
 287                         for (i = 0; i < 4; i++) /*R10 = 0x0 */
 288                                 hex_string += sprintf(hex_string, "%02x", 0);
 289
 290                         hex_string =
 291                                 reg_converter(hex_string, &temp->context->FP, 4);
 292                         hex_string =
 293                                 reg_converter(hex_string, &temp->context->IP, 4);
 294                         hex_string =
 295                                 reg_converter(hex_string, &temp->context->SP, 4);
 296
 297                         for (i = 0; i < 4; i++)
 298                                 hex_string += sprintf(hex_string, "%02x", 0);
 299
 300                         hex_string =
 301                                 reg_converter(hex_string, &temp->context->PC, 4);
 302
 303                         for (i = 0; i < 100; i++)       /*100 */
 304                                 hex_string += sprintf(hex_string, "%02x", 0);
 305
 306                         uint32_t cpsr = 0x00000000;
 307                         reg_converter(hex_string, &cpsr, 4);
 308                 }
 309         }
 310         return ERROR_OK;
 311 }
 312
 313 static int linux_os_detect(struct target *target)
 314 {
 315         LOG_INFO("should no be called");
 316         return 0;
 317 }
 318
 319 static int linux_os_smp_init(struct target *target);
 320 static int linux_os_clean(struct target *target);
 321 #define INIT_TASK 0
 322 static char *linux_symbol_list[] = {
 323         "init_task",
 324         NULL
 325 };
 326
 327 static int linux_get_symbol_list_to_lookup(symbol_table_elem_t *symbol_list[])
 328 {
 329         unsigned int i;
 330         *symbol_list = (symbol_table_elem_t *)
 331                 malloc(sizeof(symbol_table_elem_t) * ARRAY_SIZE(linux_symbol_list));
 332
 333         for (i = 0; i < ARRAY_SIZE(linux_symbol_list); i++)
 334                 (*symbol_list)[i].symbol_name = linux_symbol_list[i];
 335
 336         return 0;
 337 }
 338
 339 static char *linux_ps_command(struct target *target);
 340
 341 const struct rtos_type Linux_os = {
 342         .name = "linux",
 343         .detect_rtos = linux_os_detect,
 344         .create = linux_os_create,
 345         .smp_init = linux_os_smp_init,
 346         .update_threads = linux_os_dummy_update,
 347         .get_thread_reg_list = linux_os_thread_reg_list,
 348         .get_symbol_list_to_lookup = linux_get_symbol_list_to_lookup,
 349         .clean = linux_os_clean,
 350         .ps_command = linux_ps_command,
 351 };
 352
 353 static int linux_thread_packet(struct connection *connection, char *packet,
 354                 int packet_size);
 355 static void linux_identify_current_threads(struct target *target);
 356
 357 #ifdef PID_CHECK
 358 int fill_task_pid(struct target *target, struct threads *t)
 359 {
 360         uint32_t pid_addr = t->base_addr + PID;
 361         uint8_t buffer[4];
 362         int retval = fill_buffer(target, pid_addr, buffer);
 363
 364         if (retval == ERROR_OK) {
 365                 uint32_t val = get_buffer(target, buffer);
 366                 t->pid = val;
 367         } else
 368                 LOG_ERROR("fill_task_pid: unable to read memory");
 369
 370         return retval;
 371 }
 372 #endif
 373
 374 int fill_task(struct target *target, struct threads *t)
 375 {
 376         int retval;
 377         uint32_t pid_addr = t->base_addr + PID;
 378         uint32_t mem_addr = t->base_addr + MEM;
 379         uint32_t on_cpu = t->base_addr + ONCPU;
 380         uint8_t *buffer = calloc(1, 4);
 381         retval = fill_buffer(target, t->base_addr, buffer);
 382
 383         if (retval == ERROR_OK) {
 384                 uint32_t val = get_buffer(target, buffer);
 385                 t->state = val;
 386         } else
 387                 LOG_ERROR("fill_task: unable to read memory");
 388
 389         retval = fill_buffer(target, pid_addr, buffer);
 390
 391         if (retval == ERROR_OK) {
 392                 uint32_t val = get_buffer(target, buffer);
 393                 t->pid = val;
 394         } else
 395                 LOG_ERROR("fill task: unable to read memory");
 396
 397         retval = fill_buffer(target, on_cpu, buffer);
 398
 399         if (retval == ERROR_OK) {
 400                 uint32_t val = get_buffer(target, buffer);
 401                 t->oncpu = val;
 402         } else
 403                 LOG_ERROR("fill task: unable to read memory");
 404
 405         retval = fill_buffer(target, mem_addr, buffer);
 406
 407         if (retval == ERROR_OK) {
 408                 uint32_t val = get_buffer(target, buffer);
 409
 410                 if (val != 0) {
 411                         uint32_t asid_addr = val + MM_CTX;
 412                         retval = fill_buffer(target, asid_addr, buffer);
 413
 414                         if (retval == ERROR_OK) {
 415                                 val = get_buffer(target, buffer);
 416                                 t->asid = val;
 417                         } else
 418                                 LOG_ERROR
 419                                         ("fill task: unable to read memory -- ASID");
 420                 } else
 421                         t->asid = 0;
 422         } else
 423                 LOG_ERROR("fill task: unable to read memory");
 424
 425         free(buffer);
 426
 427         return retval;
 428 }
 429
 430 int get_name(struct target *target, struct threads *t)
 431 {
 432         int retval;
 433         uint32_t full_name[4];
 434         uint32_t comm = t->base_addr + COMM;
 435         int i;
 436
 437         for (i = 0; i < 17; i++)
 438                 t->name[i] = 0;
 439
 440         retval = linux_read_memory(target, comm, 4, 4, (uint8_t *) full_name);
 441
 442         if (retval != ERROR_OK) {
 443                 LOG_ERROR("get_name: unable to read memory\n");
 444                 return ERROR_FAIL;
 445         }
 446
 447         uint32_t raw_name = target_buffer_get_u32(target,
 448                         (const uint8_t *)
 449                         &full_name[0]);
 450         t->name[3] = raw_name >> 24;
 451         t->name[2] = raw_name >> 16;
 452         t->name[1] = raw_name >> 8;
 453         t->name[0] = raw_name;
 454         raw_name =
 455                 target_buffer_get_u32(target, (const uint8_t *)&full_name[1]);
 456         t->name[7] = raw_name >> 24;
 457         t->name[6] = raw_name >> 16;
 458         t->name[5] = raw_name >> 8;
 459         t->name[4] = raw_name;
 460         raw_name =
 461                 target_buffer_get_u32(target, (const uint8_t *)&full_name[2]);
 462         t->name[11] = raw_name >> 24;
 463         t->name[10] = raw_name >> 16;
 464         t->name[9] = raw_name >> 8;
 465         t->name[8] = raw_name;
 466         raw_name =
 467                 target_buffer_get_u32(target, (const uint8_t *)&full_name[3]);
 468         t->name[15] = raw_name >> 24;
 469         t->name[14] = raw_name >> 16;
 470         t->name[13] = raw_name >> 8;
 471         t->name[12] = raw_name;
 472         return ERROR_OK;
 473
 474 }
 475
 476 int get_current(struct target *target, int create)
 477 {
 478         struct target_list *head;
 479         head = target->head;
 480         uint8_t *buf;
 481         uint32_t val;
 482         uint32_t ti_addr;
 483         uint8_t *buffer = calloc(1, 4);
 484         struct linux_os *linux_os = (struct linux_os *)
 485                 target->rtos->rtos_specific_params;
 486         struct current_thread *ctt = linux_os->current_threads;
 487
 488         /*  invalid current threads content */
 489         while (ctt != NULL) {
 490                 ctt->threadid = -1;
 491                 ctt->TS = 0xdeadbeef;
 492                 ctt = ctt->next;
 493         }
 494
 495         while (head != (struct target_list *)NULL) {
 496                 struct reg **reg_list;
 497                 int reg_list_size;
 498                 int retval;
 499
 500                 if (target_get_gdb_reg_list(head->target, &reg_list,
 501                                 &reg_list_size) != ERROR_OK) {
 502                         free(buffer);
 503                         return ERROR_TARGET_FAILURE;
 504                 }
 505
 506                 if (!reg_list[13]->valid)
 507                         reg_list[13]->type->get(reg_list[13]);
 508
 509                 buf = reg_list[13]->value;
 510                 val = get_buffer(target, buf);
 511                 ti_addr = (val & 0xffffe000);
 512                 uint32_t TS_addr = ti_addr + 0xc;
 513                 retval = fill_buffer(target, TS_addr, buffer);
 514
 515                 if (retval == ERROR_OK) {
 516                         uint32_t TS = get_buffer(target, buffer);
 517                         uint32_t cpu, on_cpu = TS + ONCPU;
 518                         retval = fill_buffer(target, on_cpu, buffer);
 519
 520                         if (retval == ERROR_OK) {
 521                                 /*uint32_t cpu = get_buffer(target, buffer);*/
 522                                 struct current_thread *ct =
 523                                         linux_os->current_threads;
 524                                 cpu = head->target->coreid;
 525
 526                                 while ((ct != NULL) && (ct->core_id != (int32_t) cpu))
 527                                         ct = ct->next;
 528
 529                                 if ((ct != NULL) && (ct->TS == 0xdeadbeef))
 530                                         ct->TS = TS;
 531                                 else
 532                                         LOG_ERROR
 533                                                 ("error in linux current thread update");
 534
 535                                 if (create) {
 536                                         struct threads *t;
 537                                         t = calloc(1, sizeof(struct threads));
 538                                         t->base_addr = ct->TS;
 539                                         fill_task(target, t);
 540                                         get_name(target, t);
 541                                         t->oncpu = cpu;
 542                                         insert_into_threadlist(target, t);
 543                                         t->status = 3;
 544                                         t->thread_info_addr = 0xdeadbeef;
 545                                         ct->threadid = t->threadid;
 546                                         linux_os->thread_count++;
 547 #ifdef PID_CHECK
 548                                         ct->pid = t->pid;
 549 #endif
 550                                         /*LOG_INFO("Creation of current thread %s",t->name);*/
 551                                 }
 552                         }
 553                 }
 554
 555                 free(reg_list);
 556                 head = head->next;
 557         }
 558
 559         free(buffer);
 560
 561         return ERROR_OK;
 562 }
 563
 564 struct cpu_context *cpu_context_read(struct target *target, uint32_t base_addr,
 565         uint32_t *thread_info_addr_old)
 566 {
 567         struct cpu_context *context = calloc(1, sizeof(struct cpu_context));
 568         uint32_t preempt_count_addr = 0;
 569         uint32_t registers[10];
 570         uint8_t *buffer = calloc(1, 4);
 571         uint32_t stack = base_addr + QAT;
 572         uint32_t thread_info_addr = 0;
 573         uint32_t thread_info_addr_update = 0;
 574         int retval = ERROR_FAIL;
 575         context->R4 = 0xdeadbeef;
 576         context->R5 = 0xdeadbeef;
 577         context->R6 = 0xdeadbeef;
 578         context->R7 = 0xdeadbeef;
 579         context->R8 = 0xdeadbeef;
 580         context->R9 = 0xdeadbeef;
 581         context->IP = 0xdeadbeef;
 582         context->FP = 0xdeadbeef;
 583         context->SP = 0xdeadbeef;
 584         context->PC = 0xdeadbeef;
 585 retry:
 586
 587         if (*thread_info_addr_old == 0xdeadbeef) {
 588                 retval = fill_buffer(target, stack, buffer);
 589
 590                 if (retval == ERROR_OK)
 591                         thread_info_addr = get_buffer(target, buffer);
 592                 else
 593                         LOG_ERROR("cpu_context: unable to read memory");
 594
 595                 thread_info_addr_update = thread_info_addr;
 596         } else
 597                 thread_info_addr = *thread_info_addr_old;
 598
 599         preempt_count_addr = thread_info_addr + PREEMPT;
 600         retval = fill_buffer(target, preempt_count_addr, buffer);
 601
 602         if (retval == ERROR_OK)
 603                 context->preempt_count = get_buffer(target, buffer);
 604         else {
 605                 if (*thread_info_addr_old != 0xdeadbeef) {
 606                         LOG_ERROR
 607                                 ("cpu_context: cannot read at thread_info_addr");
 608
 609                         if (*thread_info_addr_old < LINUX_USER_KERNEL_BORDER)
 610                                 LOG_INFO
 611                                         ("cpu_context : thread_info_addr in userspace!!!");
 612
 613                         *thread_info_addr_old = 0xdeadbeef;
 614                         goto retry;
 615                 }
 616
 617                 LOG_ERROR("cpu_context: unable to read memory");
 618         }
 619
 620         thread_info_addr += CPU_CONT;
 621
 622         retval = linux_read_memory(target, thread_info_addr, 4, 10,
 623                         (uint8_t *) registers);
 624
 625         if (retval != ERROR_OK) {
 626                 free(buffer);
 627                 LOG_ERROR("cpu_context: unable to read memory\n");
 628                 return context;
 629         }
 630
 631         context->R4 =
 632                 target_buffer_get_u32(target, (const uint8_t *)&registers[0]);
 633         context->R5 =
 634                 target_buffer_get_u32(target, (const uint8_t *)&registers[1]);
 635         context->R6 =
 636                 target_buffer_get_u32(target, (const uint8_t *)&registers[2]);
 637         context->R7 =
 638                 target_buffer_get_u32(target, (const uint8_t *)&registers[3]);
 639         context->R8 =
 640                 target_buffer_get_u32(target, (const uint8_t *)&registers[4]);
 641         context->R9 =
 642                 target_buffer_get_u32(target, (const uint8_t *)&registers[5]);
 643         context->IP =
 644                 target_buffer_get_u32(target, (const uint8_t *)&registers[6]);
 645         context->FP =
 646                 target_buffer_get_u32(target, (const uint8_t *)&registers[7]);
 647         context->SP =
 648                 target_buffer_get_u32(target, (const uint8_t *)&registers[8]);
 649         context->PC =
 650                 target_buffer_get_u32(target, (const uint8_t *)&registers[9]);
 651
 652         if (*thread_info_addr_old == 0xdeadbeef)
 653                 *thread_info_addr_old = thread_info_addr_update;
 654
 655         free(buffer);
 656
 657         return context;
 658 }
 659
 660 uint32_t next_task(struct target *target, struct threads *t)
 661 {
 662         uint8_t *buffer = calloc(1, 4);
 663         uint32_t next_addr = t->base_addr + NEXT;
 664         int retval = fill_buffer(target, next_addr, buffer);
 665
 666         if (retval == ERROR_OK) {
 667                 uint32_t val = get_buffer(target, buffer);
 668                 val = val - NEXT;
 669                 free(buffer);
 670                 return val;
 671         } else
 672                 LOG_ERROR("next task: unable to read memory");
 673
 674         free(buffer);
 675
 676         return 0;
 677 }
 678
 679 struct current_thread *add_current_thread(struct current_thread *currents,
 680         struct current_thread *ct)
 681 {
 682         ct->next = NULL;
 683
 684         if (currents == NULL) {
 685                 currents = ct;
 686                 return currents;
 687         } else {
 688                 struct current_thread *temp = currents;
 689
 690                 while (temp->next != NULL)
 691                         temp = temp->next;
 692
 693                 temp->next = ct;
 694                 return currents;
 695         }
 696 }
 697
 698 struct threads *liste_del_task(struct threads *task_list, struct threads **t,
 699         struct threads *prev)
 700 {
 701         LOG_INFO("del task %" PRId64, (*t)->threadid);
 702         prev->next = (*t)->next;
 703
 704         if (prev == task_list)
 705                 task_list = prev;
 706
 707         /*  free content of threads */
 708         if ((*t)->context)
 709                 free((*t)->context);
 710
 711         free(*t);
 712         *t = prev;
 713         return task_list;
 714 }
 715
 716 struct threads *liste_add_task(struct threads *task_list, struct threads *t,
 717         struct threads **last)
 718 {
 719         t->next = NULL;
 720
 721         if (*last == NULL)
 722                 if (task_list == NULL) {
 723                         task_list = t;
 724                         return task_list;
 725                 } else {
 726                         struct threads *temp = task_list;
 727
 728                         while (temp->next != NULL)
 729                                 temp = temp->next;
 730
 731                         temp->next = t;
 732                         *last = t;
 733                         return task_list;
 734                 } else {
 735                 (*last)->next = t;
 736                 *last = t;
 737                 return task_list;
 738         }
 739 }
 740
 741 #ifdef PID_CHECK
 742 static int current_pid(struct linux_os *linux_os, uint32_t pid)
 743 #else
 744 static int current_base_addr(struct linux_os *linux_os, uint32_t base_addr)
 745 #endif
 746 {
 747         struct current_thread *ct = linux_os->current_threads;
 748 #ifdef PID_CHECK
 749
 750         while ((ct != NULL) && (ct->pid != pid))
 751 #else
 752         while ((ct != NULL) && (ct->TS != base_addr))
 753 #endif
 754                 ct = ct->next;
 755 #ifdef PID_CHECK
 756         if ((ct != NULL) && (ct->pid == pid))
 757 #else
 758         if ((ct != NULL) && (ct->TS == base_addr))
 759 #endif
 760                 return 1;
 761
 762         return 0;
 763 }
 764
 765 int linux_get_tasks(struct target *target, int context)
 766 {
 767         int loop = 0;
 768         int retval = 0;
 769         struct linux_os *linux_os = (struct linux_os *)
 770                 target->rtos->rtos_specific_params;
 771         linux_os->thread_list = NULL;
 772         linux_os->thread_count = 0;
 773
 774         if (linux_os->init_task_addr == 0xdeadbeef) {
 775                 LOG_INFO("no init symbol\n");
 776                 return ERROR_FAIL;
 777         }
 778
 779         int64_t start = timeval_ms();
 780
 781         struct threads *t = calloc(1, sizeof(struct threads));
 782         struct threads *last = NULL;
 783         t->base_addr = linux_os->init_task_addr;
 784         /* retrieve the thread id , currently running in the different smp core */
 785         get_current(target, 1);
 786
 787         while (((t->base_addr != linux_os->init_task_addr) &&
 788                 (t->base_addr != 0)) || (loop == 0)) {
 789                 loop++;
 790                 fill_task(target, t);
 791                 retval = get_name(target, t);
 792
 793                 if (loop > MAX_THREADS) {
 794                         free(t);
 795                         LOG_INFO("more than %d threads !!", MAX_THREADS);
 796                         return ERROR_FAIL;
 797                 }
 798
 799                 if (retval != ERROR_OK) {
 800                         free(t);
 801                         return ERROR_FAIL;
 802                 }
 803
 804                 /*  check that this thread is not one the current threads already
 805                  *  created */
 806 #ifdef PID_CHECK
 807
 808                 if (!current_pid(linux_os, t->pid)) {
 809 #else
 810                 if (!current_base_addr(linux_os, t->base_addr)) {
 811 #endif
 812                         t->threadid = linux_os->threadid_count;
 813                         t->status = 1;
 814                         linux_os->threadid_count++;
 815
 816                         linux_os->thread_list =
 817                                 liste_add_task(linux_os->thread_list, t, &last);
 818                         /* no interest to fill the context if it is a current thread. */
 819                         linux_os->thread_count++;
 820                         t->thread_info_addr = 0xdeadbeef;
 821
 822                         if (context)
 823                                 t->context =
 824                                         cpu_context_read(target, t->base_addr,
 825                                                 &t->thread_info_addr);
 826                 } else {
 827                         /*LOG_INFO("thread %s is a current thread already created",t->name); */
 828                         free(t);
 829                 }
 830
 831                 uint32_t base_addr = next_task(target, t);
 832                 t = calloc(1, sizeof(struct threads));
 833                 t->base_addr = base_addr;
 834         }
 835
 836         linux_os->threads_lookup = 1;
 837         linux_os->threads_needs_update = 0;
 838         linux_os->preupdtate_threadid_count = linux_os->threadid_count - 1;
 839         /*  check that all current threads have been identified  */
 840
 841         LOG_INFO("complete time %" PRId64 ", thread mean %" PRId64 "\n",
 842                 (timeval_ms() - start),
 843                 (timeval_ms() - start) / linux_os->threadid_count);
 844
 845         LOG_INFO("threadid count %d", linux_os->threadid_count);
 846         free(t);
 847
 848         return ERROR_OK;
 849 }
 850
 851 static int clean_threadlist(struct target *target)
 852 {
 853         struct linux_os *linux_os = (struct linux_os *)
 854                 target->rtos->rtos_specific_params;
 855         struct threads *old, *temp = linux_os->thread_list;
 856
 857         while (temp != NULL) {
 858                 old = temp;
 859
 860                 if (temp->context)
 861                         free(temp->context);
 862
 863                 temp = temp->next;
 864                 free(old);
 865         }
 866
 867         return ERROR_OK;
 868 }
 869
 870 static int linux_os_clean(struct target *target)
 871 {
 872         struct linux_os *os_linux = (struct linux_os *)
 873                 target->rtos->rtos_specific_params;
 874         clean_threadlist(target);
 875         os_linux->init_task_addr = 0xdeadbeef;
 876         os_linux->name = "linux";
 877         os_linux->thread_list = NULL;
 878         os_linux->thread_count = 0;
 879         os_linux->nr_cpus = 0;
 880         os_linux->threads_lookup = 0;
 881         os_linux->threads_needs_update = 0;
 882         os_linux->threadid_count = 1;
 883         return ERROR_OK;
 884 }
 885
 886 static int insert_into_threadlist(struct target *target, struct threads *t)
 887 {
 888         struct linux_os *linux_os = (struct linux_os *)
 889                 target->rtos->rtos_specific_params;
 890         struct threads *temp = linux_os->thread_list;
 891         t->threadid = linux_os->threadid_count;
 892         linux_os->threadid_count++;
 893         t->status = 1;
 894         t->next = NULL;
 895
 896         if (temp == NULL)
 897                 linux_os->thread_list = t;
 898         else {
 899                 while (temp->next != NULL)
 900                         temp = temp->next;
 901
 902                 t->next = NULL;
 903                 temp->next = t;
 904         }
 905
 906         return ERROR_OK;
 907 }
 908
 909 static void linux_identify_current_threads(struct target *target)
 910 {
 911         struct linux_os *linux_os = (struct linux_os *)
 912                 target->rtos->rtos_specific_params;
 913         struct threads *thread_list = linux_os->thread_list;
 914         struct current_thread *ct = linux_os->current_threads;
 915         struct threads *t = NULL;
 916
 917         while ((ct != NULL)) {
 918                 if (ct->threadid == -1) {
 919
 920                         /*  un-identified thread */
 921                         int found = 0;
 922                         t = calloc(1, sizeof(struct threads));
 923                         t->base_addr = ct->TS;
 924 #ifdef PID_CHECK
 925
 926                         if (fill_task_pid(target, t) != ERROR_OK) {
 927 error_handling:
 928                                 free(t);
 929                                 LOG_ERROR
 930                                         ("linux identify_current_threads: unable to read pid");
 931                                 return;
 932                         }
 933 #endif
 934
 935                         /* search in the list of threads if pid
 936                            already present */
 937                         while ((thread_list != NULL) && (found == 0)) {
 938 #ifdef PID_CHECK
 939                                 if (thread_list->pid == t->pid) {
 940 #else
 941                                 if (thread_list->base_addr == t->base_addr) {
 942 #endif
 943                                         free(t);
 944                                         t = thread_list;
 945                                         found = 1;
 946                                 }
 947                                 thread_list = thread_list->next;
 948                         }
 949
 950                         if (!found) {
 951                                 /*  it is a new thread */
 952                                 if (fill_task(target, t) != ERROR_OK)
 953                                         goto error_handling;
 954
 955                                 get_name(target, t);
 956                                 insert_into_threadlist(target, t);
 957                                 t->thread_info_addr = 0xdeadbeef;
 958                         }
 959
 960                         t->status = 3;
 961                         ct->threadid = t->threadid;
 962 #ifdef PID_CHECK
 963                         ct->pid = t->pid;
 964 #endif
 965                         linux_os->thread_count++;
 966 #if 0
 967                         if (found == 0)
 968                                 LOG_INFO("current thread core %x identified %s",
 969                                         ct->core_id, t->name);
 970                         else
 971                                 LOG_INFO("current thread core %x, reused %s",
 972                                         ct->core_id, t->name);
 973 #endif
 974                 }
 975 #if 0
 976                 else {
 977                         struct threads tmp;
 978                         tmp.base_addr = ct->TS;
 979                         get_name(target, &tmp);
 980                         LOG_INFO("current thread core %x , already identified %s !!!",
 981                                 ct->core_id, tmp.name);
 982                 }
 983 #endif
 984                 ct = ct->next;
 985         }
 986
 987         return;
 988 #ifndef PID_CHECK
 989 error_handling:
 990         free(t);
 991         LOG_ERROR("unable to read pid");
 992         return;
 993
 994 #endif
 995 }
 996
 997 static int linux_task_update(struct target *target, int context)
 998 {
 999         struct linux_os *linux_os = (struct linux_os *)
1000                 target->rtos->rtos_specific_params;
1001         struct threads *thread_list = linux_os->thread_list;
1002         int retval;
1003         int loop = 0;
1004         linux_os->thread_count = 0;
1005
1006         /*thread_list = thread_list->next; skip init_task*/
1007         while (thread_list != NULL) {
1008                 thread_list->status = 0;        /*setting all tasks to dead state*/
1009
1010                 if (thread_list->context) {
1011                         free(thread_list->context);
1012                         thread_list->context = NULL;
1013                 }
1014
1015                 thread_list = thread_list->next;
1016         }
1017
1018         int found = 0;
1019
1020         if (linux_os->init_task_addr == 0xdeadbeef) {
1021                 LOG_INFO("no init symbol\n");
1022                 return ERROR_FAIL;
1023         }
1024         int64_t start = timeval_ms();
1025         struct threads *t = calloc(1, sizeof(struct threads));
1026         uint32_t previous = 0xdeadbeef;
1027         t->base_addr = linux_os->init_task_addr;
1028         retval = get_current(target, 0);
1029         /*check that all current threads have been identified  */
1030         linux_identify_current_threads(target);
1031
1032         while (((t->base_addr != linux_os->init_task_addr) &&
1033                 (t->base_addr != previous)) || (loop == 0)) {
1034                 /*  for avoiding any permanent loop for any reason possibly due to
1035                  *  target */
1036                 loop++;
1037                 previous = t->base_addr;
1038                 /*  read only pid */
1039 #ifdef PID_CHECK
1040                 retval = fill_task_pid(target, t);
1041 #endif
1042
1043                 if (retval != ERROR_OK) {
1044                         free(t);
1045                         return ERROR_FAIL;
1046                 }
1047
1048                 thread_list = linux_os->thread_list;
1049
1050                 while (thread_list != NULL) {
1051 #ifdef PID_CHECK
1052                         if (t->pid == thread_list->pid) {
1053 #else
1054                         if (t->base_addr == thread_list->base_addr) {
1055 #endif
1056                                 if (!thread_list->status) {
1057 #ifdef PID_CHECK
1058                                         if (t->base_addr != thread_list->base_addr)
1059                                                 LOG_INFO("thread base_addr has changed !!");
1060 #endif
1061                                         /*  this is not a current thread  */
1062                                         thread_list->base_addr = t->base_addr;
1063                                         thread_list->status = 1;
1064
1065                                         /*  we don 't update this field any more */
1066
1067                                         /*thread_list->state = t->state;
1068                                         thread_list->oncpu = t->oncpu;
1069                                         thread_list->asid = t->asid;
1070                                         */
1071                                         if (context)
1072                                                 thread_list->context =
1073                                                         cpu_context_read(target,
1074                                                                 thread_list->
1075                                                                 base_addr,
1076                                                                 &thread_list->
1077                                                                 thread_info_addr);
1078                                 } else {
1079                                         /*  it is a current thread no need to read context */
1080                                 }
1081
1082                                 linux_os->thread_count++;
1083                                 found = 1;
1084                                 break;
1085                         } else {
1086                                 found = 0;
1087                                 thread_list = thread_list->next;
1088                         }
1089                 }
1090
1091                 if (found == 0) {
1092                         uint32_t base_addr;
1093                         fill_task(target, t);
1094                         get_name(target, t);
1095                         retval = insert_into_threadlist(target, t);
1096                         t->thread_info_addr = 0xdeadbeef;
1097
1098                         if (context)
1099                                 t->context =
1100                                         cpu_context_read(target, t->base_addr,
1101                                                 &t->thread_info_addr);
1102
1103                         base_addr = next_task(target, t);
1104                         t = calloc(1, sizeof(struct threads));
1105                         t->base_addr = base_addr;
1106                         linux_os->thread_count++;
1107                 } else
1108                         t->base_addr = next_task(target, t);
1109         }
1110
1111         LOG_INFO("update thread done %" PRId64 ", mean%" PRId64 "\n",
1112                 (timeval_ms() - start), (timeval_ms() - start) / loop);
1113         free(t);
1114         linux_os->threads_needs_update = 0;
1115         return ERROR_OK;
1116 }
1117
1118 int linux_gdb_thread_packet(struct target *target,
1119         struct connection *connection, char *packet,
1120         int packet_size)
1121 {
1122         int retval;
1123         struct linux_os *linux_os =
1124                 (struct linux_os *)target->rtos->rtos_specific_params;
1125
1126         if (linux_os->init_task_addr == 0xdeadbeef) {
1127                 /* it has not been initialized */
1128                 LOG_INFO("received thread request without init task address");
1129                 gdb_put_packet(connection, "l", 1);
1130                 return ERROR_OK;
1131         }
1132
1133         retval = linux_get_tasks(target, 1);
1134
1135         if (retval != ERROR_OK)
1136                 return ERROR_TARGET_FAILURE;
1137
1138         char *out_str = (char *)calloc(1, 350 * sizeof(int64_t));
1139         char *tmp_str = out_str;
1140         tmp_str += sprintf(tmp_str, "m");
1141         struct threads *temp = linux_os->thread_list;
1142         tmp_str += sprintf(tmp_str, "%016" PRIx64, temp->threadid);
1143         temp = temp->next;
1144
1145         while (temp != NULL) {
1146                 tmp_str += sprintf(tmp_str, ",");
1147                 tmp_str += sprintf(tmp_str, "%016" PRIx64, temp->threadid);
1148                 temp = temp->next;
1149         }
1150
1151         gdb_put_packet(connection, out_str, strlen(out_str));
1152         return ERROR_OK;
1153 }
1154
1155 int linux_gdb_thread_update(struct target *target,
1156         struct connection *connection, char *packet,
1157         int packet_size)
1158 {
1159         int found = 0;
1160         struct linux_os *linux_os = (struct linux_os *)
1161                 target->rtos->rtos_specific_params;
1162         struct threads *temp = linux_os->thread_list;
1163
1164         while (temp != NULL) {
1165                 if (temp->threadid == linux_os->preupdtate_threadid_count + 1) {
1166                         /*LOG_INFO("FOUND");*/
1167                         found = 1;
1168                         break;
1169                 } else
1170                         temp = temp->next;
1171         }
1172
1173         if (found == 1) {
1174                 /*LOG_INFO("INTO GDB THREAD UPDATE FOUNDING START TASK");*/
1175                 char *out_strr = (char *)calloc(1, 350 * sizeof(int64_t));
1176                 char *tmp_strr = out_strr;
1177                 tmp_strr += sprintf(tmp_strr, "m");
1178                 /*LOG_INFO("CHAR MALLOC & M DONE");*/
1179                 tmp_strr += sprintf(tmp_strr, "%016" PRIx64, temp->threadid);
1180
1181                 temp = temp->next;
1182
1183                 while (temp != NULL) {
1184                         /*LOG_INFO("INTO GDB THREAD UPDATE WHILE");*/
1185                         tmp_strr += sprintf(tmp_strr, ",");
1186                         tmp_strr +=
1187                                 sprintf(tmp_strr, "%016" PRIx64, temp->threadid);
1188                         temp = temp->next;
1189                 }
1190
1191                 /*tmp_str[0] = 0;*/
1192                 gdb_put_packet(connection, out_strr, strlen(out_strr));
1193                 linux_os->preupdtate_threadid_count =
1194                         linux_os->threadid_count - 1;
1195                 free(out_strr);
1196         } else
1197                 gdb_put_packet(connection, "l", 1);
1198
1199         return ERROR_OK;
1200 }
1201
1202 int linux_thread_extra_info(struct target *target,
1203         struct connection *connection, char *packet,
1204         int packet_size)
1205 {
1206         int64_t threadid = 0;
1207         struct linux_os *linux_os = (struct linux_os *)
1208                 target->rtos->rtos_specific_params;
1209         sscanf(packet, "qThreadExtraInfo,%" SCNx64, &threadid);
1210         /*LOG_INFO("lookup extra info for thread %" SCNx64, threadid);*/
1211         struct threads *temp = linux_os->thread_list;
1212
1213         while (temp != NULL) {
1214                 if (temp->threadid == threadid) {
1215                         char *pid = " PID: ";
1216                         char *pid_current = "*PID: ";
1217                         char *name = "NAME: ";
1218                         int str_size = strlen(pid) + strlen(name);
1219                         char *tmp_str = (char *)calloc(1, str_size + 50);
1220                         char *tmp_str_ptr = tmp_str;
1221
1222                         /*  discriminate current task */
1223                         if (temp->status == 3)
1224                                 tmp_str_ptr += sprintf(tmp_str_ptr, "%s",
1225                                                 pid_current);
1226                         else
1227                                 tmp_str_ptr += sprintf(tmp_str_ptr, "%s", pid);
1228
1229                         tmp_str_ptr +=
1230                                 sprintf(tmp_str_ptr, "%d", (int)temp->pid);
1231                         tmp_str_ptr += sprintf(tmp_str_ptr, "%s", " | ");
1232                         sprintf(tmp_str_ptr, "%s", name);
1233                         sprintf(tmp_str_ptr, "%s", temp->name);
1234                         char *hex_str = (char *)calloc(1, strlen(tmp_str) * 2 + 1);
1235                         int pkt_len = hexify(hex_str, tmp_str, 0, strlen(tmp_str) * 2 + 1);
1236                         gdb_put_packet(connection, hex_str, pkt_len);
1237                         free(hex_str);
1238                         free(tmp_str);
1239                         return ERROR_OK;
1240                 }
1241
1242                 temp = temp->next;
1243         }
1244
1245         LOG_INFO("thread not found");
1246         return ERROR_OK;
1247 }
1248
1249 int linux_gdb_T_packet(struct connection *connection,
1250         struct target *target, char *packet, int packet_size)
1251 {
1252         int64_t threadid;
1253         struct linux_os *linux_os = (struct linux_os *)
1254                 target->rtos->rtos_specific_params;
1255         int retval = ERROR_OK;
1256         sscanf(packet, "T%" SCNx64, &threadid);
1257
1258         if (linux_os->threads_needs_update == 0) {
1259                 struct threads *temp = linux_os->thread_list;
1260                 struct threads *prev = linux_os->thread_list;
1261
1262                 while (temp != NULL) {
1263                         if (temp->threadid == threadid) {
1264                                 if (temp->status != 0) {
1265                                         gdb_put_packet(connection, "OK", 2);
1266                                         return ERROR_OK;
1267                                 } else {
1268                                         /* delete item in the list   */
1269                                         linux_os->thread_list =
1270                                                 liste_del_task(linux_os->
1271                                                         thread_list, &temp,
1272                                                         prev);
1273                                         linux_os->thread_count--;
1274                                         gdb_put_packet(connection, "E01", 3);
1275                                         return ERROR_OK;
1276                                 }
1277                         }
1278
1279                         /*  for deletion  */
1280                         prev = temp;
1281                         temp = temp->next;
1282                 }
1283
1284                 LOG_INFO("gdb requested status on non existing thread");
1285                 gdb_put_packet(connection, "E01", 3);
1286                 return ERROR_OK;
1287
1288         } else {
1289                 retval = linux_task_update(target, 1);
1290                 struct threads *temp = linux_os->thread_list;
1291
1292                 while (temp != NULL) {
1293                         if (temp->threadid == threadid) {
1294                                 if (temp->status == 1) {
1295                                         gdb_put_packet(connection, "OK", 2);
1296                                         return ERROR_OK;
1297                                 } else {
1298                                         gdb_put_packet(connection, "E01", 3);
1299                                         return ERROR_OK;
1300                                 }
1301                         }
1302
1303                         temp = temp->next;
1304                 }
1305         }
1306
1307         return retval;
1308 }
1309
1310 int linux_gdb_h_packet(struct connection *connection,
1311         struct target *target, char *packet, int packet_size)
1312 {
1313         struct linux_os *linux_os = (struct linux_os *)
1314                 target->rtos->rtos_specific_params;
1315         struct current_thread *ct = linux_os->current_threads;
1316
1317         /* select to display the current thread of the selected target */
1318         while ((ct != NULL) && (ct->core_id != target->coreid))
1319                 ct = ct->next;
1320
1321         int64_t current_gdb_thread_rq;
1322
1323         if (linux_os->threads_lookup == 1) {
1324                 if ((ct != NULL) && (ct->threadid == -1)) {
1325                         ct = linux_os->current_threads;
1326
1327                         while ((ct != NULL) && (ct->threadid == -1))
1328                                 ct = ct->next;
1329                 }
1330
1331                 if (ct == NULL) {
1332                         /*  no current thread can be identified
1333                          *  any way with smp  */
1334                         LOG_INFO("no current thread identified");
1335                         /* attempt to display the name of the 2 threads identified with
1336                          * get_current */
1337                         struct threads t;
1338                         ct = linux_os->current_threads;
1339
1340                         while ((ct != NULL) && (ct->threadid == -1)) {
1341                                 t.base_addr = ct->TS;
1342                                 get_name(target, &t);
1343                                 LOG_INFO("name of unidentified thread %s",
1344                                         t.name);
1345                                 ct = ct->next;
1346                         }
1347
1348                         gdb_put_packet(connection, "OK", 2);
1349                         return ERROR_OK;
1350                 }
1351
1352                 if (packet[1] == 'g') {
1353                         sscanf(packet, "Hg%16" SCNx64, &current_gdb_thread_rq);
1354
1355                         if (current_gdb_thread_rq == 0) {
1356                                 target->rtos->current_threadid = ct->threadid;
1357                                 gdb_put_packet(connection, "OK", 2);
1358                         } else {
1359                                 target->rtos->current_threadid =
1360                                         current_gdb_thread_rq;
1361                                 gdb_put_packet(connection, "OK", 2);
1362                         }
1363                 } else if (packet[1] == 'c') {
1364                         sscanf(packet, "Hc%16" SCNx64, &current_gdb_thread_rq);
1365
1366                         if ((current_gdb_thread_rq == 0) ||
1367                                         (current_gdb_thread_rq == ct->threadid)) {
1368                                 target->rtos->current_threadid = ct->threadid;
1369                                 gdb_put_packet(connection, "OK", 2);
1370                         } else
1371                                 gdb_put_packet(connection, "E01", 3);
1372                 }
1373         } else
1374                 gdb_put_packet(connection, "OK", 2);
1375
1376         return ERROR_OK;
1377 }
1378
1379 static int linux_thread_packet(struct connection *connection, char *packet,
1380         int packet_size)
1381 {
1382         int retval = ERROR_OK;
1383         struct current_thread *ct;
1384         struct target *target = get_target_from_connection(connection);
1385         struct linux_os *linux_os = (struct linux_os *)
1386                 target->rtos->rtos_specific_params;
1387
1388         switch (packet[0]) {
1389                 case 'T':               /* Is thread alive?*/
1390
1391                         linux_gdb_T_packet(connection, target, packet, packet_size);
1392                         break;
1393                 case 'H':               /* Set current thread */
1394                         /*  ( 'c' for step and continue, 'g' for all other operations )*/
1395                         /*LOG_INFO(" H packet received '%s'", packet);*/
1396                         linux_gdb_h_packet(connection, target, packet, packet_size);
1397                         break;
1398                 case 'q':
1399
1400                         if (strncmp(packet, "qSymbol", 7) == 0) {
1401                                 if (rtos_qsymbol(connection, packet, packet_size) == 1) {
1402                                         linux_compute_virt2phys(target,
1403                                                         target->rtos->
1404                                                         symbols[INIT_TASK].
1405                                                         address);
1406                                 }
1407
1408                                 break;
1409                         } else if (strncmp(packet, "qfThreadInfo", 12) == 0) {
1410                                 if (linux_os->thread_list == NULL) {
1411                                         retval = linux_gdb_thread_packet(target,
1412                                                         connection,
1413                                                         packet,
1414                                                         packet_size);
1415                                         break;
1416                                 } else {
1417                                         retval = linux_gdb_thread_update(target,
1418                                                         connection,
1419                                                         packet,
1420                                                         packet_size);
1421                                         break;
1422                                 }
1423                         } else if (strncmp(packet, "qsThreadInfo", 12) == 0) {
1424                                 gdb_put_packet(connection, "l", 1);
1425                                 break;
1426                         } else if (strncmp(packet, "qThreadExtraInfo,", 17) == 0) {
1427                                 linux_thread_extra_info(target, connection, packet,
1428                                                 packet_size);
1429                                 break;
1430                         } else {
1431                                 retval = GDB_THREAD_PACKET_NOT_CONSUMED;
1432                                 break;
1433                         }
1434
1435                 case 'Q':
1436                         /* previously response was : thread not found
1437                          * gdb_put_packet(connection, "E01", 3); */
1438                         retval = GDB_THREAD_PACKET_NOT_CONSUMED;
1439                         break;
1440                 case 'c':
1441                 case 's': {
1442                         if (linux_os->threads_lookup == 1) {
1443                                 ct = linux_os->current_threads;
1444
1445                                 while ((ct != NULL) && (ct->core_id) != target->coreid)
1446                                         ct = ct->next;
1447
1448                                 if ((ct != NULL) && (ct->threadid == -1)) {
1449                                         ct = linux_os->current_threads;
1450
1451                                         while ((ct != NULL) && (ct->threadid == -1))
1452                                                 ct = ct->next;
1453                                 }
1454
1455                                 if ((ct != NULL) && (ct->threadid !=
1456                                                  target->rtos->
1457                                                  current_threadid)
1458                                 && (target->rtos->current_threadid != -1))
1459                                         LOG_WARNING("WARNING! current GDB thread do not match" \
1460                                                         "current thread running." \
1461                                                         "Switch thread in GDB to threadid %d",
1462                                                         (int)ct->threadid);
1463
1464                                 LOG_INFO("threads_needs_update = 1");
1465                                 linux_os->threads_needs_update = 1;
1466                         }
1467                 }
1468
1469                 /* if a packet handler returned an error, exit input loop */
1470                 if (retval != ERROR_OK)
1471                         return retval;
1472         }
1473
1474         return retval;
1475 }
1476
1477 static int linux_os_smp_init(struct target *target)
1478 {
1479         struct target_list *head;
1480         /* keep only target->rtos */
1481         struct rtos *rtos = target->rtos;
1482         struct linux_os *os_linux =
1483                 (struct linux_os *)rtos->rtos_specific_params;
1484         struct current_thread *ct;
1485         head = target->head;
1486
1487         while (head != (struct target_list *)NULL) {
1488                 if (head->target->rtos != rtos) {
1489                         struct linux_os *smp_os_linux =
1490                                 (struct linux_os *)head->target->rtos->
1491                                 rtos_specific_params;
1492                         /*  remap smp target on rtos  */
1493                         free(head->target->rtos);
1494                         head->target->rtos = rtos;
1495                         /*  reuse allocated ct */
1496                         ct = smp_os_linux->current_threads;
1497                         ct->threadid = -1;
1498                         ct->TS = 0xdeadbeef;
1499                         ct->core_id = head->target->coreid;
1500                         os_linux->current_threads =
1501                                 add_current_thread(os_linux->current_threads, ct);
1502                         os_linux->nr_cpus++;
1503                         free(smp_os_linux);
1504                 }
1505
1506                 head = head->next;
1507         }
1508
1509         return ERROR_OK;
1510 }
1511
1512 static int linux_os_create(struct target *target)
1513 {
1514         struct linux_os *os_linux = calloc(1, sizeof(struct linux_os));
1515         struct current_thread *ct = calloc(1, sizeof(struct current_thread));
1516         LOG_INFO("linux os creation\n");
1517         os_linux->init_task_addr = 0xdeadbeef;
1518         os_linux->name = "linux";
1519         os_linux->thread_list = NULL;
1520         os_linux->thread_count = 0;
1521         target->rtos->current_threadid = -1;
1522         os_linux->nr_cpus = 1;
1523         os_linux->threads_lookup = 0;
1524         os_linux->threads_needs_update = 0;
1525         os_linux->threadid_count = 1;
1526         os_linux->current_threads = NULL;
1527         target->rtos->rtos_specific_params = (void *)os_linux;
1528         ct->core_id = target->coreid;
1529         ct->threadid = -1;
1530         ct->TS = 0xdeadbeef;
1531         os_linux->current_threads =
1532                 add_current_thread(os_linux->current_threads, ct);
1533         /*  overload rtos thread default handler */
1534         target->rtos->gdb_thread_packet = linux_thread_packet;
1535         /*  initialize a default virt 2 phys translation */
1536         os_linux->phys_mask = ~0xc0000000;
1537         os_linux->phys_base = 0x0;
1538         return JIM_OK;
1539 }
1540
1541 static char *linux_ps_command(struct target *target)
1542 {
1543         struct linux_os *linux_os = (struct linux_os *)
1544                 target->rtos->rtos_specific_params;
1545         int retval = ERROR_OK;
1546         char *display;
1547
1548         if (linux_os->threads_lookup == 0)
1549                 retval = linux_get_tasks(target, 1);
1550         else {
1551                 if (linux_os->threads_needs_update != 0)
1552                         retval = linux_task_update(target, 0);
1553         }
1554
1555         if (retval == ERROR_OK) {
1556                 struct threads *temp = linux_os->thread_list;
1557                 char *tmp;
1558                 LOG_INFO("allocation for %d threads line",
1559                         linux_os->thread_count);
1560                 display = calloc((linux_os->thread_count + 2) * 80, 1);
1561
1562                 if (!display)
1563                         goto error;
1564
1565                 tmp = display;
1566                 tmp += sprintf(tmp, "PID\t\tCPU\t\tASID\t\tNAME\n");
1567                 tmp += sprintf(tmp, "---\t\t---\t\t----\t\t----\n");
1568
1569                 while (temp != NULL) {
1570                         if (temp->status) {
1571                                 if (temp->context)
1572                                         tmp +=
1573                                                 sprintf(tmp,
1574                                                         "%d\t\t%d\t\t%x\t\t%s\n",
1575                                                         (int)temp->pid, temp->oncpu,
1576                                                         temp->asid, temp->name);
1577                                 else
1578                                         tmp +=
1579                                                 sprintf(tmp,
1580                                                         "%d\t\t%d\t\t%x\t\t%s\n",
1581                                                         (int)temp->pid, temp->oncpu,
1582                                                         temp->asid, temp->name);
1583                         }
1584
1585                         temp = temp->next;
1586                 }
1587
1588                 return display;
1589         }
1590
1591 error:
1592         display = calloc(40, 1);
1593         sprintf(display, "linux_ps_command failed\n");
1594         return display;
1595 }