]> git.sur5r.net Git - openocd/blob - src/rtos/rtos.c
3cdd912505575ef5597364c648ab2ab2aea71467
[openocd] / src / rtos / rtos.c
1 /***************************************************************************
2  *   Copyright (C) 2011 by Broadcom Corporation                            *
3  *   Evan Hunter - ehunter@broadcom.com                                    *
4  *                                                                         *
5  *   This program is free software; you can redistribute it and/or modify  *
6  *   it under the terms of the GNU General Public License as published by  *
7  *   the Free Software Foundation; either version 2 of the License, or     *
8  *   (at your option) any later version.                                   *
9  *                                                                         *
10  *   This program is distributed in the hope that it will be useful,       *
11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
13  *   GNU General Public License for more details.                          *
14  *                                                                         *
15  *   You should have received a copy of the GNU General Public License     *
16  *   along with this program; if not, write to the                         *
17  *   Free Software Foundation, Inc.,                                       *
18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
19  ***************************************************************************/
20
21 #ifdef HAVE_CONFIG_H
22 #include "config.h"
23 #endif
24
25
26 #include "rtos.h"
27 #include "target/target.h"
28 #include "helper/log.h"
29 #include "server/gdb_server.h"
30
31
32 static int64_t current_threadid = -1;
33
34 static void hex_to_str( char* dst, char * hex_src );
35 static int str_to_hex( char* hex_dst, char* src );
36
37
38 /* RTOSs */
39 extern struct rtos_type FreeRTOS_rtos;
40 extern struct rtos_type ThreadX_rtos;
41
42 static struct rtos_type *rtos_types[] =
43 {
44         &ThreadX_rtos,
45         &FreeRTOS_rtos,
46         NULL
47 };
48
49
50 int rtos_create(Jim_GetOptInfo *goi, struct target * target)
51 {
52         int x;
53         char *cp;
54
55         if (! goi->isconfigure) {
56                 if (goi->argc != 0) {
57                         if (goi->argc != 0) {
58                                 Jim_WrongNumArgs(goi->interp,
59                                                 goi->argc, goi->argv,
60                                                 "NO PARAMS");
61                                 return JIM_ERR;
62                         }
63
64                         Jim_SetResultString(goi->interp,
65                                         target_type_name(target), -1);
66                 }
67         }
68
69         if (target->rtos) {
70                 free((void *)(target->rtos));
71         }
72 //                      e = Jim_GetOpt_String(goi, &cp, NULL);
73 //                      target->rtos = strdup(cp);
74
75         Jim_GetOpt_String(goi, &cp, NULL);
76         /* now does target type exist */
77
78         if ( 0 == strcmp( cp, "auto") )
79         {
80                 // auto detection of RTOS
81                 target->rtos_auto_detect = true;
82                 x = 0;
83         }
84         else
85         {
86
87                 for (x = 0 ; rtos_types[x] ; x++) {
88                         if (0 == strcmp(cp, rtos_types[x]->name)) {
89                                 /* found */
90                                 break;
91                         }
92                 }
93                 if (rtos_types[x] == NULL) {
94                         Jim_SetResultFormatted(goi->interp, "Unknown rtos type %s, try one of ", cp);
95                         for (x = 0 ; rtos_types[x] ; x++) {
96                                 if (rtos_types[x + 1]) {
97                                         Jim_AppendStrings(goi->interp,
98                                                                            Jim_GetResult(goi->interp),
99                                                                            rtos_types[x]->name,
100                                                                            ", ", NULL);
101                                 } else {
102                                         Jim_AppendStrings(goi->interp,
103                                                                            Jim_GetResult(goi->interp),
104                                                                            " or ",
105                                                                            rtos_types[x]->name,NULL);
106                                 }
107                         }
108                         return JIM_ERR;
109                 }
110         }
111         /* Create it */
112         target->rtos = calloc(1,sizeof(struct rtos));
113         target->rtos->type = rtos_types[x];
114         target->rtos->current_thread = 0;
115         target->rtos->symbols = NULL;
116         target->rtos->target = target;
117
118         if ( 0 != strcmp( cp, "auto") )
119         {
120                 target->rtos->type->create( target );
121         }
122
123         return JIM_OK;
124 }
125
126
127
128
129 int gdb_thread_packet(struct connection *connection, struct target *target, char *packet, int packet_size)
130 {
131         if (strstr(packet, "qP"))
132         {
133                 #define TAG_THREADID 1          /* Echo the thread identifier */
134                 #define TAG_EXISTS 2            /* Is this process defined enough to
135                                                    fetch registers and its stack */
136                 #define TAG_DISPLAY 4           /* A short thing maybe to put on a window */
137                 #define TAG_THREADNAME 8        /* string, maps 1-to-1 with a thread is */
138                 #define TAG_MOREDISPLAY 16      /* Whatever the kernel wants to say about */
139
140                         // TODO: need to scanf the mode variable (or it with the tags), and the threadid
141
142                 unsigned long mode;
143                 threadid_t threadid = 0;
144                 struct thread_detail* detail;
145                 sscanf(packet, "qP%8lx%16" SCNx64, &mode, &threadid);
146
147
148                 int found = -1;
149
150                 if ((target->rtos != NULL) && (target->rtos->thread_details
151                                 != NULL)) {
152                         int thread_num;
153                         for (thread_num = 0; thread_num
154                                         < target->rtos->thread_count; thread_num++) {
155                                 if (target->rtos->thread_details[thread_num].threadid
156                                                 == threadid) {
157                                         if (target->rtos->thread_details[thread_num].exists) {
158                                                 found = thread_num;
159                                         }
160                                 }
161                         }
162                 }
163                 if (found == -1) {
164                         gdb_put_packet(connection, "E01", 3); // thread not found
165                         return ERROR_OK;
166                 }
167
168                 detail = &target->rtos->thread_details[found];
169
170                 if ( detail->display_str != NULL )
171                 {
172                         mode &= TAG_DISPLAY;
173                 }
174                 if ( detail->thread_name_str != NULL )
175                 {
176                         mode &= TAG_THREADNAME;
177                 }
178                 if ( detail->extra_info_str != NULL )
179                 {
180                         mode &= TAG_MOREDISPLAY;
181                 }
182
183
184                 mode &= TAG_THREADID | TAG_EXISTS;
185
186                 char thread_str[1000];
187
188                 sprintf(thread_str, "%08lx", mode);
189                 sprintf(thread_str, "%016" PRIx64, threadid);
190
191
192                 if (mode & TAG_THREADID) {
193                         sprintf(thread_str, "%08" PRIx32 "10%016" PRIx64, TAG_THREADID, threadid);
194                 }
195                 if (mode & TAG_EXISTS) {
196                         sprintf(thread_str, "%08" PRIx32 "08%08" PRIx32, TAG_EXISTS, (detail->exists==true)?1:0);
197                 }
198                 if (mode & TAG_DISPLAY) {
199                         sprintf(thread_str, "%08" PRIx32 "%02x%s", TAG_DISPLAY, (unsigned char)strlen(detail->display_str), detail->display_str );
200                 }
201                 if (mode & TAG_MOREDISPLAY) {
202                         sprintf(thread_str, "%08" PRIx32 "%02x%s", TAG_MOREDISPLAY, (unsigned char)strlen(detail->extra_info_str), detail->extra_info_str );
203                 }
204                 if (mode & TAG_THREADNAME) {
205                         sprintf(thread_str, "%08" PRIx32 "%02x%s", TAG_THREADNAME, (unsigned char)strlen(detail->thread_name_str), detail->thread_name_str );
206                 }
207
208                 //gdb_put_packet(connection, tmpstr, sizeof(tmpstr)-1);
209                 gdb_put_packet(connection, thread_str, strlen(thread_str));
210
211                 //                      gdb_put_packet(connection, "", 0);
212                 //              gdb_put_packet(connection, "OK", 2); // all threads alive
213                 return ERROR_OK;
214         }
215         else if (strstr(packet, "qThreadExtraInfo,"))
216         {
217                 if ((target->rtos != NULL) && (target->rtos->thread_details != NULL) && (target->rtos->thread_count != 0))
218                 {
219                         threadid_t threadid = 0;
220                         int found = -1;
221                         sscanf(packet, "qThreadExtraInfo,%" SCNx64, &threadid );
222
223                         if ((target->rtos != NULL) && (target->rtos->thread_details
224                                         != NULL)) {
225                                 int thread_num;
226                                 for (thread_num = 0; thread_num
227                                                 < target->rtos->thread_count; thread_num++) {
228                                         if (target->rtos->thread_details[thread_num].threadid
229                                                         == threadid) {
230                                                 if (target->rtos->thread_details[thread_num].exists) {
231                                                         found = thread_num;
232                                                 }
233                                         }
234                                 }
235                         }
236                         if (found == -1) {
237                                 gdb_put_packet(connection, "E01", 3); // thread not found
238                                 return ERROR_OK;
239                         }
240
241                         struct thread_detail* detail = &target->rtos->thread_details[found];
242
243                         int str_size = 0;
244                         if ( detail->display_str != NULL )
245                         {
246                                 str_size += strlen(detail->display_str);
247                         }
248                         if ( detail->thread_name_str != NULL )
249                         {
250                                 str_size += strlen(detail->thread_name_str);
251                         }
252                         if ( detail->extra_info_str != NULL )
253                         {
254                                 str_size += strlen(detail->extra_info_str);
255                         }
256
257                         char * tmp_str = (char*) malloc( str_size + 7 );
258                         char*  tmp_str_ptr = tmp_str;
259
260                         if ( detail->display_str != NULL )
261                         {
262                                 tmp_str_ptr += sprintf( tmp_str_ptr, "%s", detail->display_str );
263                         }
264                         if ( detail->thread_name_str != NULL )
265                         {
266                                 if ( tmp_str_ptr != tmp_str )
267                                 {
268                                         tmp_str_ptr += sprintf( tmp_str_ptr, " : " );
269                                 }
270                                 tmp_str_ptr += sprintf( tmp_str_ptr, "%s", detail->thread_name_str );
271                         }
272                         if ( detail->extra_info_str != NULL )
273                         {
274                                 if ( tmp_str_ptr != tmp_str )
275                                 {
276                                         tmp_str_ptr += sprintf( tmp_str_ptr, " : " );
277                                 }
278                                 tmp_str_ptr += sprintf( tmp_str_ptr, " : %s", detail->extra_info_str );
279                         }
280
281                         char * hex_str = (char*) malloc( strlen(tmp_str)*2 +1 );
282                         str_to_hex( hex_str, tmp_str );
283
284                         gdb_put_packet(connection, hex_str, strlen(hex_str));
285                         free(hex_str);
286                         free(tmp_str);
287                         return ERROR_OK;
288
289                 }
290                 gdb_put_packet(connection, "", 0);
291                 return ERROR_OK;
292         }
293         else if (strstr(packet, "qSymbol"))
294         {
295                 if ( target->rtos != NULL )
296                 {
297                         int next_symbol_num = -1;
298                         if (target->rtos->symbols == NULL)
299                         {
300                                 target->rtos->type->get_symbol_list_to_lookup( &target->rtos->symbols );
301                         }
302                         if (0 == strcmp( "qSymbol::", packet ) )
303                         {
304                                 // first query -
305                                 next_symbol_num = 0;
306                         }
307                         else
308                         {
309                                 int64_t value = 0;
310                                 char * hex_name_str = malloc( strlen(packet));
311                                 char * name_str;
312                                 int symbol_num;
313
314                                 char* found = strstr( packet, "qSymbol::" );
315                                 int numconv;
316                                 if (0 == found )
317                                 {
318                                         numconv =sscanf(packet, "qSymbol:%" SCNx64 ":%s", &value, hex_name_str);
319                                 }
320                                 else
321                                 {
322                                         // No value returned by GDB - symbol was not found
323                                         numconv =sscanf(packet, "qSymbol::%s", hex_name_str);
324                                 }
325                                 name_str = (char*) malloc( 1+ strlen(hex_name_str) / 2 );
326
327                                 hex_to_str( name_str, hex_name_str );
328
329
330                                 symbol_num = 0;
331                                 while ( ( target->rtos->symbols[ symbol_num ].symbol_name != NULL ) && ( 0 != strcmp( target->rtos->symbols[ symbol_num ].symbol_name, name_str ) ) )
332                                 {
333                                         symbol_num++;
334                                 }
335
336
337                                 if ( target->rtos->symbols[ symbol_num ].symbol_name == NULL )
338                                 {
339                                         LOG_OUTPUT("ERROR: unknown symbol\r\n");
340                                         gdb_put_packet(connection, "OK", 2);
341                                         return ERROR_OK;
342                                 }
343
344                                 target->rtos->symbols[ symbol_num ].address = value;
345
346                                 next_symbol_num = symbol_num+1;
347                                 free( hex_name_str );
348                                 free( name_str );
349
350                         }
351
352                         int symbols_done = 0;
353                         if ( target->rtos->symbols[ next_symbol_num ].symbol_name == NULL )
354                         {
355                                 if ( ( target->rtos_auto_detect == false ) ||
356                                          ( 1 == target->rtos->type->detect_rtos( target ) ) )
357                                 {
358                                         // Found correct RTOS or not autodetecting
359                                         if ( target->rtos_auto_detect == true )
360                                         {
361                                                 LOG_OUTPUT( "Auto-detected RTOS: %s\r\n",target->rtos->type->name );
362                                         }
363                                         symbols_done = 1;
364                                 }
365                                 else
366                                 {
367                                         // Auto detecting RTOS and currently not found
368                                         if( 1 != rtos_try_next( target ) )
369                                         {
370                                                 // No more RTOS's to try
371                                                 symbols_done = 1;
372                                         }
373                                         else
374                                         {
375                                                 next_symbol_num = 0;
376                                                 target->rtos->type->get_symbol_list_to_lookup( &target->rtos->symbols );
377                                         }
378
379                                 }
380                         }
381
382
383                         if ( symbols_done == 1 )
384                         {
385                                 target->rtos_auto_detect = false;
386                                 target->rtos->type->create( target );
387                                 target->rtos->type->update_threads(target->rtos);
388                                 // No more symbols needed
389                                 gdb_put_packet(connection, "OK", 2);
390                                 return ERROR_OK;
391
392                         }
393                         else
394                         {
395                                 char* symname = target->rtos->symbols[ next_symbol_num ].symbol_name;
396                                 char qsymstr[] = "qSymbol:";
397                                 char * opstring = (char*)malloc(sizeof(qsymstr)+strlen(symname)*2+1);
398                                 char * posptr = opstring;
399                                 posptr += sprintf( posptr, "%s", qsymstr );
400                                 str_to_hex( posptr, symname );
401                                 gdb_put_packet(connection, opstring, strlen(opstring));
402                                 free(opstring);
403                                 return ERROR_OK;
404                         }
405
406                 }
407                 gdb_put_packet(connection, "OK", 2);
408                 return ERROR_OK;
409         }
410         else if (strstr(packet, "qfThreadInfo"))
411         {
412                 int i;
413                 if ( ( target->rtos != NULL ) && ( target->rtos->thread_count != 0 ) )
414                 {
415
416                         char* out_str = (char*) malloc(17 * target->rtos->thread_count + 5);
417                         char* tmp_str = out_str;
418                         tmp_str += sprintf(tmp_str, "m");
419                         for (i = 0; i < target->rtos->thread_count; i++) {
420                                 if (i != 0) {
421                                         tmp_str += sprintf(tmp_str, ",");
422                                 }
423                                 tmp_str += sprintf(tmp_str, "%016" PRIx64,
424                                                 target->rtos->thread_details[i].threadid);
425                         }
426                         tmp_str[0] = 0;
427                         gdb_put_packet(connection, out_str, strlen(out_str));
428                 }
429                 else
430                 {
431                         gdb_put_packet(connection, "", 0);
432                 }
433
434                 return ERROR_OK;
435         }
436         else if (strstr(packet, "qsThreadInfo"))
437         {
438                 gdb_put_packet(connection, "l", 1);
439                 return ERROR_OK;
440         }
441         else if (strstr(packet, "qAttached"))
442         {
443                 gdb_put_packet(connection, "1", 1);
444                 return ERROR_OK;
445         }
446         else if (strstr(packet, "qOffsets"))
447         {
448                 char offsets[] = "Text=0;Data=0;Bss=0";
449                 gdb_put_packet(connection, offsets, sizeof(offsets)-1);
450                 return ERROR_OK;
451         }
452         else if (strstr(packet, "qC"))
453         {
454                 gdb_put_packet(connection, "QC0", 3);
455                 return ERROR_OK;
456         }
457         else if ( packet[0] == 'T' ) // Is thread alive?
458         {
459                 threadid_t threadid;
460                 int found = -1;
461                 sscanf(packet, "T%" SCNx64, &threadid);
462                 if ((target->rtos != NULL) && (target->rtos->thread_details
463                                 != NULL)) {
464                         int thread_num;
465                         for (thread_num = 0; thread_num
466                                         < target->rtos->thread_count; thread_num++) {
467                                 if (target->rtos->thread_details[thread_num].threadid
468                                                 == threadid) {
469                                         if (target->rtos->thread_details[thread_num].exists) {
470                                                 found = thread_num;
471                                         }
472                                 }
473                         }
474                 }
475                 if (found != -1) {
476                         gdb_put_packet(connection, "OK", 2); // thread alive
477                 } else {
478                         gdb_put_packet(connection, "E01", 3); // thread not found
479                 }
480         }
481         else if ( packet[0] == 'H') // Set current thread ( 'c' for step and continue, 'g' for all other operations )
482         {
483                 if (packet[1] == 'g')
484                 {
485                         sscanf(packet, "Hg%16" SCNx64, &current_threadid);
486                 }
487                 gdb_put_packet(connection, "OK", 2);
488         }
489
490         return GDB_THREAD_PACKET_NOT_CONSUMED;
491 }
492
493 int rtos_get_gdb_reg_list(struct connection *connection, struct target *target, struct reg **reg_list[], int *reg_list_size)
494 {
495         if ( ( target->rtos != NULL ) &&
496                  ( current_threadid != -1 ) &&
497                  ( current_threadid != 0 ) &&
498                  ( current_threadid != target->rtos->current_thread ) )
499         {
500                 char * hex_reg_list;
501                 target->rtos->type->get_thread_reg_list( target->rtos, current_threadid, &hex_reg_list );
502
503                 if ( hex_reg_list != NULL )
504                 {
505                         gdb_put_packet(connection, hex_reg_list, strlen(hex_reg_list));
506                         free(hex_reg_list);
507                         return ERROR_OK;
508                 }
509         }
510         return ERROR_FAIL;
511 }
512
513
514
515 int rtos_generic_stack_read( struct target * target, const struct rtos_register_stacking* stacking, int64_t stack_ptr, char ** hex_reg_list )
516 {
517         int list_size = 0;
518         char * tmp_str_ptr;
519         int64_t new_stack_ptr;
520         int i;
521         int retval;
522
523         if ( stack_ptr == 0)
524         {
525                 LOG_OUTPUT("Error: null stack pointer in thread\r\n");
526                 return -5;
527         }
528         // Read the stack
529         uint8_t * stack_data = (uint8_t*) malloc( stacking->stack_registers_size );
530         uint32_t address = stack_ptr;
531
532         if ( stacking->stack_growth_direction == 1 )
533         {
534                 address -=  stacking->stack_registers_size;
535         }
536         retval = target_read_buffer( target, address, stacking->stack_registers_size, stack_data);
537         if ( retval != ERROR_OK )
538         {
539                 LOG_OUTPUT("Error reading stack frame from FreeRTOS thread\r\n");
540                 return retval;
541         }
542 /*
543         LOG_OUTPUT("Stack Data :");
544         for(i = 0; i < stacking->stack_registers_size; i++ )
545         {
546                 LOG_OUTPUT("%02X",stack_data[i]);
547         }
548         LOG_OUTPUT("\r\n");
549 */
550         for( i = 0; i < stacking->num_output_registers; i++ )
551         {
552                 list_size += stacking->register_offsets[i].width_bits/8;
553         }
554         *hex_reg_list = (char*)malloc( list_size*2 +1 );
555         tmp_str_ptr = *hex_reg_list;
556         new_stack_ptr = stack_ptr - stacking->stack_growth_direction * stacking->stack_registers_size;
557         for( i = 0; i < stacking->num_output_registers; i++ )
558         {
559                 int j;
560                 for ( j = 0; j < stacking->register_offsets[i].width_bits/8; j++ )
561                 {
562                         if ( stacking->register_offsets[i].offset == -1 )
563                         {
564                                 tmp_str_ptr += sprintf( tmp_str_ptr, "%02x", 0 );
565                         }
566                         else if ( stacking->register_offsets[i].offset == -2 )
567                         {
568                                 tmp_str_ptr += sprintf( tmp_str_ptr, "%02x", ((uint8_t*)&new_stack_ptr)[j] );
569                         }
570                         else
571                         {
572                                 tmp_str_ptr += sprintf( tmp_str_ptr,"%02x", stack_data[ stacking->register_offsets[i].offset + j ] );
573                         }
574                 }
575         }
576 //      LOG_OUTPUT("Output register string: %s\r\n", *hex_reg_list);
577         return ERROR_OK;
578 }
579
580 int rtos_try_next( struct target * target )
581 {
582         int x;
583
584         if ( target->rtos == NULL )
585         {
586                 return -1;
587         }
588
589         for (x = 0 ; rtos_types[x] ; x++) {
590                 if (target->rtos->type == rtos_types[x] ) {
591                         /* found */
592                         if ( rtos_types[x+1] != NULL )
593                         {
594                                 target->rtos->type = rtos_types[x+1];
595                                 if ( target->rtos->symbols != NULL )
596                                 {
597                                         free( target->rtos->symbols );
598                                 }
599                                 return 1;
600                         }
601                         else
602                         {
603                                 // No more rtos types
604                                 return 0;
605                         }
606
607                 }
608         }
609         return 0;
610
611 }
612
613 static void hex_to_str( char* dst, char * hex_src )
614 {
615         int src_pos = 0;
616         int dst_pos = 0;
617
618         while ( hex_src[src_pos] != '\x00' )
619         {
620                 char hex_char = hex_src[src_pos];
621                 char hex_digit_val = (hex_char>='a')?hex_char-'a'+10:(hex_char>='A')?hex_char-'A'+10:hex_char-'0';
622                 if ( 0 == (src_pos & 0x01) )
623                 {
624                         dst[dst_pos] = hex_digit_val;
625                         dst[dst_pos+1] = 0;
626                 }
627                 else
628                 {
629                         ((unsigned char*)dst)[dst_pos] <<= 4;
630                         ((unsigned char*)dst)[dst_pos] += hex_digit_val;
631                         dst_pos++;
632                 }
633                 src_pos++;
634         }
635
636 }
637
638 static int str_to_hex( char* hex_dst, char* src )
639 {
640         char * posptr = hex_dst;
641         unsigned i;
642         for( i = 0; i < strlen(src); i++)
643         {
644                 posptr += sprintf( posptr, "%02x", (unsigned char)src[i] );
645         }
646         return (posptr-hex_dst);
647 }
648
649
650 int rtos_update_threads( struct target* target )
651 {
652         if ((target->rtos != NULL) && (target->rtos->type != NULL))
653         {
654                 target->rtos->type->update_threads(target->rtos);
655         }
656         return ERROR_OK;
657 }