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