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