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