blc_loop.cpp 11.5 KB
Newer Older
1
2
3
4
5

#include "blc_command.h"
#include "blc_realtime.h"
#include <unistd.h> //sleep
#include <errno.h> //errno
6
7
#include <math.h> //pthread_mutex

8
9
#include <time.h> //nanosleep
#include <sys/time.h> //gettimeofday
10
#include <pthread.h> //pthread_mutex
11

12
struct timeval blc_loop_timer;
13
14
15
16
17
18
19
static uint blc_period=0;
static uint blc_duration=0;
static uint blc_duration_min=UINT_MAX, blc_duration_max=0;

static long blc_current_duration;
static int intermediate_iteration;

20
long blc_command_loop_period=-2; //unitialised
21
22
uint64_t blc_loop_iteration_limit=UINT64_MAX;
uint64_t blc_loop_iteration=0;
23

24
25
26
27
28
29
30
31
32
33
34
35
36
static void(**waiting_callbacks)(void*)=NULL;
static void **waiting_callabacks_user_data=NULL;
static int waiting_callbacks_nb=0;

static sem_t **waiting_semaphores;
static int waiting_semaphores_nb;

static sem_t **posting_semaphores;
static int posting_semaphores_nb;

static pthread_t loop_thread, loop_check_stuck_thread;
static pthread_mutex_t mutex_lock_keyboard=PTHREAD_MUTEX_INITIALIZER;

37
38
39
40
41
static int step_iteration;
static struct timeval step_timer, profile_timer;

static sem_t *blocking_semaphore=NULL;

42
43
44
45
46
47
#include "blc_program.h"

type_blc_status blc_status=BLC_RUN;

void blc_fprint_stats(FILE *file)
{
48
    if (intermediate_iteration){
49
        fprintf(file, "%-16.16s %4d iterations duration:%8.3fms[%8.3f, %8.3f] frequency:%8.3fHz period:%8.3fms\n", blc_program_id, intermediate_iteration, blc_duration/(1000.f*intermediate_iteration), blc_duration_min/1000., blc_duration_max/1000., 1e6*intermediate_iteration/(double)blc_period, blc_period/(1000.*intermediate_iteration));
50
51
52
53
54
55
        intermediate_iteration=0;
        blc_period=0;
        blc_duration=0;
        blc_duration_min=UINT_MAX;
        blc_duration_max=0;
    }
56
    else fprintf(file, "%s: No new iteration yet\n", blc_program_id);
57
    gettimeofday(&blc_loop_timer, NULL);
58
59
60
61
62
63
}

static void display_stats(){
    blc_fprint_stats(stderr);
}

64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
void blc_loop_add_waiting_callback( void(*waiting_callback)(void*), void *user_data){

    MANY_REALLOCATIONS(&waiting_callbacks, waiting_callbacks_nb+1);
    MANY_REALLOCATIONS(&waiting_callabacks_user_data, waiting_callbacks_nb+1);
    
    waiting_callbacks[waiting_callbacks_nb]=waiting_callback;
    waiting_callabacks_user_data[waiting_callbacks_nb]=user_data;
    waiting_callbacks_nb++;
}

int blc_loop_try_add_waiting_semaphore(void *semaphore){
    if (semaphore==NULL) return 0;
    APPEND_ITEM(&waiting_semaphores, &waiting_semaphores_nb, &semaphore);
    return 1;
}

int blc_loop_try_add_posting_semaphore(void *semaphore){
    if (semaphore==NULL) return 0;
    APPEND_ITEM(&posting_semaphores, &posting_semaphores_nb, &semaphore);
    return 1;

}

void blc_loop_add_waiting_semaphore(void *semaphore){
    if (blc_loop_try_add_waiting_semaphore(semaphore)==0) EXIT_ON_ERROR("The semaphore you want to add is NULL");
}

void blc_loop_add_posting_semaphore(void *semaphore){
    if (blc_loop_try_add_posting_semaphore(semaphore)==0) EXIT_ON_ERROR("The semaphore you want to add is NULL");
}

95
96
97
//We send the argument to stdout if it is not a terminal
static void blc_command_send_to_stdout(char const *argument){
    if (blc_output_terminal==0){
98
99
100
    if (argument) SYSTEM_ERROR_CHECK(printf("%s", argument), -1, NULL);
    SYSTEM_ERROR_CHECK(printf("\n"), -1, NULL);
    SYSTEM_ERROR_CHECK(fflush(stdout), -1, NULL);
101
102
103
104
105
106
107
108
109
110
    }
}
//We execute the argument and send it with prefix '&' to stdout if it is not a terminal
static void blc_command_send_to_all(char const *argument){
    blc_command_interpret_string(argument, strlen(argument));
    if (blc_output_terminal==0){
    if (argument) SYSTEM_ERROR_CHECK(printf("&%s", argument), -1, NULL);
    SYSTEM_ERROR_CHECK(printf("\n"), -1, NULL);
    SYSTEM_ERROR_CHECK(fflush(stdout), -1, NULL);
    }
111
}
112
113

void blc_command_ask_quit(){
114
    if (blc_status==BLC_PAUSE) BLC_PTHREAD_CHECK(pthread_mutex_unlock(&mutex_lock_keyboard), NULL);
115
    
116
    if (blc_status!=BLC_STEPS) blc_status=BLC_QUIT;
117
    
118
119
120
121
    if (blocking_semaphore){
        if (sem_trywait(blocking_semaphore)==0){
            sem_post(blocking_semaphore); // we succeed, it was not a problem, I free it.
            blocking_semaphore=NULL;
122
        }
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
        else if (errno==EAGAIN) sem_post(blocking_semaphore);
        else EXIT_ON_SYSTEM_ERROR("unlocking semaphore for quitting");
    }
}

static void step_forward_cb(const char* steps_nb_str, void*){
    int  steps_nb, pos;
    if (steps_nb_str){
        if (sscanf(steps_nb_str, "%d%n", &steps_nb, &pos)!=1 || pos !=strlen(steps_nb_str) || steps_nb<1){
            color_eprintf(BLC_YELLOW, "'%s' is not a valid step number\n", steps_nb_str);
            return;
        }
        else {
            blc_status=BLC_STEPS;
            blc_loop_iteration_limit=blc_loop_iteration+steps_nb;
            blc_us_time_diff(&step_timer);
            BLC_PTHREAD_CHECK(pthread_mutex_unlock(&mutex_lock_keyboard), NULL);
        }
    }
    else {
        blc_status=BLC_STEPS;
        blc_loop_iteration_limit=blc_loop_iteration+1;
        blc_us_time_diff(&step_timer);
        BLC_PTHREAD_CHECK(pthread_mutex_unlock(&mutex_lock_keyboard), NULL);

148
    }
149
150
    step_iteration=blc_loop_iteration;
    blc_command_remove(">");
151
152
153
154
}

void pause_cb()
{
155
    if (blc_status==BLC_RUN || blc_status==BLC_STEPS){
156
        blc_status=BLC_PAUSE;
157
        blc_command_add(">", step_forward_cb, "|iterations", "continue for steps nb iterations", NULL);
158
        fprintf(stderr, "=== %s: pause iteration %lld ===  '' to continue, '>[<nb>]' to step one or run nb iterations ===\n", blc_program_id, blc_loop_iteration);
159
160
    }else if (blc_status==BLC_PAUSE){
        blc_command_remove(">");
161
        blc_status=BLC_RUN;
162
        fprintf(stderr, "=== %s: continue === \n", blc_program_id);
163
164
        BLC_PTHREAD_CHECK(pthread_mutex_unlock(&mutex_lock_keyboard), NULL);
    }
165
    else EXIT_ON_ERROR("Unknown status '%d'", blc_status);
166
167
168
169
170
171
172
173
174
175
176
}

static void *loop_check_stuck(void*){
    int iteration;
    int checking_time=5;
    int was_stucked=0;
    
    while(1){
        iteration = blc_loop_iteration;

        sleep(checking_time);
177
178
        if(blc_status==BLC_RUN){ //in BLC_PAUSE it is normal to block
        	if (iteration==blc_loop_iteration) {
179
            	color_eprintf(BLC_YELLOW, "'%s' seems blocked on iteration '%d'. The loop has been stopped for more than %ds\n", blc_program_id, iteration, checking_time);
180
                if (blc_profile_file) fflush(blc_profile_file); //It may help for debuggage
181
182
183
            	was_stucked=1;
        	}
        	else if (was_stucked) {
184
            	color_eprintf(BLC_GREEN, "'%s' program restart iteration '%d'.\n", blc_program_id, iteration);
185
186
            	was_stucked=0;
        	}
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
        }
    }
    return NULL;
}

/// if the parameter is not null we should display the help at each command.
static void *command_thread_interpret_loop(void *){
    
    while(blc_status!=BLC_QUIT){
        blc_command_interpret_block();
        if (blc_command_loop_period==-1) BLC_PTHREAD_CHECK(pthread_mutex_unlock(&mutex_lock_keyboard), NULL);
    }
    return NULL;
}

202
203
void blc_command_loop_init(long loop_period){
    if (blc_command_loop_period==-3) EXIT_ON_ERROR("You already are in a blc_command_loop");
204
205
206
    blc_loop_iteration=0;
    intermediate_iteration=0;
    blc_command_loop_period=loop_period;
207
    
208
209
210
211
    if (blc_command_loop_period==-2){
        blc_command_loop_period=-1;
    }
    else BLC_PTHREAD_CHECK(pthread_mutex_lock(&mutex_lock_keyboard), NULL);
212
213
214
    
    if (blc_input_terminal){
         blc_command_display_help(); //We display help only for keyboard
215
    }
216
217
    if (blc_command_loop_period!=-1) blc_command_add("", (type_blc_command_cb)pause_cb, NULL, "set on/off pause", NULL);

218
219
    blc_command_add("h", (type_blc_command_cb)blc_command_display_help, NULL, "display this help", NULL);
    blc_command_add("q", (type_blc_command_cb)blc_command_ask_quit, NULL, "quit the application", NULL);
220

221
    if (blc_command_loop_period!=-1) { //we lock on keyboard. But if the program lock we still read keyboard
222
223
        SYSTEM_ERROR_CHECK(pthread_create(&loop_check_stuck_thread, NULL, loop_check_stuck, NULL), -1, NULL);
        blc_command_add("s", (type_blc_command_cb)display_stats, NULL, "display time stats", NULL); //We did not do stat on keyboard
224
        fprintf(stderr, "=== %s: running ===\n", blc_program_id);
225
    }
226
227
    blc_command_add("|", (type_blc_command_cb)blc_command_send_to_stdout, "command", "send the command to stdout", NULL);
    blc_command_add("&", (type_blc_command_cb)blc_command_send_to_all, "command", "intepret caommdn and send it to stdout", NULL);
228
229

    SYSTEM_ERROR_CHECK(pthread_create(&loop_thread, NULL, command_thread_interpret_loop, NULL), -1, NULL);
230
231
232
233
}

int blc_command_loop_start(){
    
234
    int i;
235
236
    
    
237
    if (blc_profile_file) gettimeofday(&profile_timer, NULL); //this is only for profiling
238

239
    //We wait before counting the duration time as the time for waiting does not matter
240
    if (blc_loop_iteration==blc_loop_iteration_limit){
241
        fprintf(stderr, "%s: %d iterations executed in %.3fms\n", blc_program_id, (int)blc_loop_iteration-step_iteration, blc_us_time_diff(&step_timer)/1000.f);
242
243
        pause_cb();
    }
244
245
246
    
    //We eventiualy wait for keyboard return key
    if ((blc_status==BLC_PAUSE) || (blc_command_loop_period==-1))  BLC_PTHREAD_CHECK(pthread_mutex_lock(&mutex_lock_keyboard), NULL);
247
    else if (blc_status==BLC_RUN){
248
        //We wait for semaphore
249
        FOR(i, waiting_semaphores_nb) {
Arnaud Blanchard's avatar
Arnaud Blanchard committed
250
            blocking_semaphore=waiting_semaphores[i];  //This is used to be able to unlock all blocking semaphore on quiting
251
252
253
254
            SYSTEM_ERROR_CHECK(sem_wait(blocking_semaphore), -1, NULL);
        }
        blocking_semaphore=NULL;
            
255
256
        //Waiting general user defined callbacks
        FOR(i, waiting_callbacks_nb) waiting_callbacks[i](waiting_callabacks_user_data[i]);
257
    }
258
    else blc_command_loop_period=-3; //In case we start another loop after by mistake
259
260
    
    //We start counting the duration time
261
262
    if (intermediate_iteration==0) blc_us_time_diff(&blc_loop_timer);
    else  blc_period+=blc_current_duration + blc_us_time_diff(&blc_loop_timer);
263
    
264
265
266
267
268
    return blc_status;
}

void blc_command_loop_end(){
    div_t ratio;
269
270
    struct timespec time_left, tmp_rem;
    int diff, i;
271
    uint64_t start_time;
272

273
274
    if (blc_profile_file) start_time=blc_loop_timer.tv_sec*1000000+blc_loop_timer.tv_usec;
    blc_current_duration=blc_us_time_diff(&blc_loop_timer);
275
    if (blc_profile_file) fprintf(blc_profile_file, "%16ld\t%16lld\t%8lu\t\n",profile_timer.tv_sec*1000000+profile_timer.tv_usec, start_time, blc_current_duration);
276
277
 //   fflush(stderr);
    
278
279
    //We post the semaphore usualy to say the data is ready to read
    FOR(i, posting_semaphores_nb) SYSTEM_ERROR_CHECK(sem_post(posting_semaphores[i]), -1, NULL);
280
    
281
282
    blc_duration+=blc_current_duration;
    
283
284
    if (blc_current_duration<blc_duration_min) blc_duration_min=blc_current_duration;
    else if (blc_current_duration > blc_duration_max) blc_duration_max=blc_current_duration;
285
286
287
288
289
    
    if (blc_command_loop_period > 0){
        diff=blc_command_loop_period - blc_current_duration;
        ratio=div(diff, 1e6);
        time_left.tv_sec = ratio.quot;
290
        time_left.tv_nsec = ratio.rem*1000;
291
        if (diff  < 0) color_fprintf(BLC_YELLOW, stderr, "\r%s: Missing %.3fms in the BLC_COMMAND_LOOP requesting refresh of %.3fms\n", blc_program_id,  -diff/1000.f, blc_command_loop_period/1000.f);
292
        else SYSTEM_ERROR_RETRY_ON_SPECIFIC_ERRNO(nanosleep(&time_left, &tmp_rem), -1, EINTR, "Time left %.3fms", tmp_rem.tv_nsec/1e6f + tmp_rem.tv_sec*1000);
293
294
295
296
297
        /* Warning the nanosleep can be overtimed of 10ms !!! it is the same for usleep !! */
    }
    intermediate_iteration++;
    blc_loop_iteration++;
}