| 1 | #define __PTHREAD_EXTRA_INTERNAL |
| 2 | |
| 3 | #include <pthread.h> |
| 4 | #include <pthread_extra.h> |
| 5 | #include <signal.h> |
| 6 | #include <semaphore.h> |
| 7 | #include <errno.h> |
| 8 | #include <unistd.h> |
| 9 | #include <sys/resource.h> |
| 10 | //#include <sys/siginfo.h> |
| 11 | //#include <stdio.h> |
| 12 | #include <stdlib.h> |
| 13 | #include <assert.h> |
| 14 | //#include <sys/time.h> |
| 15 | |
| 16 | //Mutex that ensures proper serialization of (un)pause calls |
| 17 | //pthread_mutex_t pthread_pause_mutex = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP; |
| 18 | |
| 19 | //Semaphore that ensures proper serialization of (un)pause signals |
| 20 | sem_t pthread_pause_sem; |
| 21 | //Once control to init the semaphore (and possibly other stuff) |
| 22 | pthread_once_t pthread_pause_once_ctrl = PTHREAD_ONCE_INIT; |
| 23 | void pthread_pause_once(void) { |
| 24 | sem_init(&pthread_pause_sem, 0, 1); |
| 25 | } |
| 26 | void pthread_pause_init() { pthread_once(&pthread_pause_once_ctrl, &pthread_pause_once); } |
| 27 | |
| 28 | ///When this variable is nonzero, only referenced thread is allowed to run |
| 29 | ///Access has to be protected by pthread_user_data_lock() and pthread_pause_sem; |
| 30 | pthread_t pthread_pause_holder = PTHREAD_XNULL; |
| 31 | |
| 32 | void pthread_pause_handler(const int signal, siginfo_t *info, void *ptr) { |
| 33 | (void)signal; (void)info; (void)ptr; |
| 34 | int run = info->si_value.sival_int; |
| 35 | //(void)td; |
| 36 | |
| 37 | //Post semaphore to confirm that signal is handled |
| 38 | sem_post(&pthread_pause_sem); |
| 39 | |
| 40 | //Keep waiting for signals until we are supposed to be running |
| 41 | if(!run) { |
| 42 | sigset_t sigset; |
| 43 | sigfillset(&sigset); |
| 44 | sigdelset(&sigset, PTHREAD_XSIG_STOP); |
| 45 | sigsuspend(&sigset); |
| 46 | } |
| 47 | } |
| 48 | |
| 49 | void pthread_pause_enable() { |
| 50 | //Add thread to internal registry |
| 51 | //pthread_user_data_internal(pthread_self()); |
| 52 | |
| 53 | //Nesting signals too deep is not good for stack |
| 54 | //You can get runtime stats using following command: |
| 55 | //grep -i sig /proc/$(pgrep binary)/status |
| 56 | //struct rlimit sigq = {.rlim_cur = 32, .rlim_max=32}; |
| 57 | //setrlimit(RLIMIT_SIGPENDING, &sigq); |
| 58 | |
| 59 | pthread_pause_init(); //Make sure semaphore is init'd |
| 60 | |
| 61 | //Prepare signal mask |
| 62 | sigset_t sigset; |
| 63 | sigemptyset(&sigset); |
| 64 | sigaddset(&sigset, PTHREAD_XSIG_STOP); |
| 65 | |
| 66 | //Setup signal handler |
| 67 | //signal(PTHREAD_XSIG_STOP, pthread_pause_handler); |
| 68 | const struct sigaction pause_sa = { |
| 69 | .sa_sigaction = pthread_pause_handler, |
| 70 | .sa_mask = sigset, |
| 71 | .sa_flags = SA_SIGINFO | SA_RESTART, |
| 72 | .sa_restorer = NULL |
| 73 | }; |
| 74 | sigaction(PTHREAD_XSIG_STOP, &pause_sa, NULL); |
| 75 | |
| 76 | //Unblock signal |
| 77 | pthread_sigmask(SIG_UNBLOCK, &sigset, NULL); |
| 78 | } |
| 79 | |
| 80 | void pthread_pause_disable() { |
| 81 | //Add thread to internal registry |
| 82 | //pthread_user_data_internal(pthread_self()); |
| 83 | |
| 84 | pthread_pause_init(); //Make sure semaphore is init'd |
| 85 | |
| 86 | //Block signal |
| 87 | sigset_t sigset; |
| 88 | sigemptyset(&sigset); |
| 89 | sigaddset(&sigset, PTHREAD_XSIG_STOP); |
| 90 | |
| 91 | //Make sure all signals are dispatched before we block them |
| 92 | sem_wait(&pthread_pause_sem); |
| 93 | pthread_sigmask(SIG_BLOCK, &sigset, NULL); |
| 94 | sem_post(&pthread_pause_sem); |
| 95 | } |
| 96 | |
| 97 | /* |
| 98 | int pthread_pause_reschedule(pthread_t thread) { |
| 99 | //Send signal to initiate pause handler |
| 100 | //printf("SND: %p\n", (void *)pthread_user_data_internal(thread)); |
| 101 | //while(pthread_kill(thread, PTHREAD_XSIG_STOP) == EAGAIN) usleep(1000); |
| 102 | while(pthread_sigqueue(thread, PTHREAD_XSIG_STOP, |
| 103 | (const union sigval){.sival_ptr=pthread_user_data_internal(thread)} |
| 104 | ) == EAGAIN) usleep(1000); |
| 105 | return 0; |
| 106 | } |
| 107 | */ |
| 108 | |
| 109 | int pthread_pause_reschedule(pthread_t thread) { |
| 110 | //Decide if the thread should run and signal it |
| 111 | |
| 112 | //Wait for semaphore which means signal queue is empty |
| 113 | pthread_pause_init(); //Make sure semaphore is init'd |
| 114 | sem_wait(&pthread_pause_sem); |
| 115 | |
| 116 | //Only call this if you already acquired pthread_pause_sem semaphore!!!! |
| 117 | //Otherwise call pthread_pause_reschedule() |
| 118 | |
| 119 | pthread_user_data_lock(); |
| 120 | //Check if thread has running flag |
| 121 | int run = (pthread_user_data_internal(thread)->running); |
| 122 | //Check if privileged (single thread) mode is active |
| 123 | if((pthread_pause_holder != PTHREAD_XNULL) && !pthread_equal(pthread_pause_holder, thread)) { |
| 124 | run = 0; |
| 125 | } |
| 126 | pthread_user_data_unlock(); |
| 127 | |
| 128 | //Send signal to initiate pause handler (keep trying while SigQueue is full) |
| 129 | //while(pthread_kill(thread, PTHREAD_XSIG_STOP) == EAGAIN) usleep(1000); |
| 130 | while(pthread_sigqueue(thread, PTHREAD_XSIG_STOP, |
| 131 | (const union sigval){.sival_int=run} |
| 132 | ) == EAGAIN) usleep(1000); |
| 133 | |
| 134 | //Wait for signal to be delivered |
| 135 | sem_wait(&pthread_pause_sem); |
| 136 | sem_post(&pthread_pause_sem); |
| 137 | |
| 138 | return 0; |
| 139 | } |
| 140 | |
| 141 | int pthread_extra_yield() { |
| 142 | //Yield to both schedulers |
| 143 | pthread_pause_reschedule(pthread_self()); |
| 144 | return pthread_yield(); |
| 145 | } |
| 146 | |
| 147 | int pthread_pause(pthread_t thread) { |
| 148 | //Set thread as paused and notify it via signal (wait when queue full) |
| 149 | pthread_user_data_lock(); |
| 150 | pthread_user_data_internal(thread)->running = 0; |
| 151 | pthread_pause_reschedule(thread); |
| 152 | pthread_user_data_unlock(); |
| 153 | return 0; |
| 154 | } |
| 155 | |
| 156 | int pthread_unpause(pthread_t thread) { |
| 157 | //Set thread as running and notify it via signal (wait when queue full) |
| 158 | pthread_user_data_lock(); |
| 159 | pthread_user_data_internal(thread)->running = 1; |
| 160 | pthread_pause_reschedule(thread); |
| 161 | pthread_user_data_unlock(); |
| 162 | return 0; |
| 163 | } |
| 164 | |
| 165 | int pthread_pause_all() { |
| 166 | pthread_user_data_lock(); |
| 167 | if(pthread_pause_holder!=PTHREAD_XNULL) assert(pthread_equal(pthread_pause_holder, pthread_self())); |
| 168 | pthread_pause_holder = pthread_self(); |
| 169 | pthread_user_data_internal_iterate(&pthread_pause_reschedule, NULL); |
| 170 | pthread_user_data_unlock(); |
| 171 | return 0; |
| 172 | } |
| 173 | |
| 174 | int pthread_unpause_all() { |
| 175 | pthread_user_data_lock(); |
| 176 | if(pthread_pause_holder!=PTHREAD_XNULL) assert(pthread_equal(pthread_pause_holder, pthread_self())); |
| 177 | pthread_pause_holder = PTHREAD_XNULL; |
| 178 | pthread_user_data_internal_iterate(&pthread_pause_reschedule, NULL); |
| 179 | pthread_user_data_unlock(); |
| 180 | return 0; |
| 181 | } |
| 182 | |
| 183 | |
| 184 | // Wrappers /////////////////////////////////////////////////////////// |
| 185 | |
| 186 | |
| 187 | typedef struct pthread_extra_wrapper_t { |
| 188 | void *(*start_routine)(void *); |
| 189 | void *restrict arg; |
| 190 | } pthread_extra_wrapper_t; |
| 191 | |
| 192 | void *pthread_extra_thread_wrapper(void *arg) { |
| 193 | pthread_extra_wrapper_t task = *((pthread_extra_wrapper_t*)arg); |
| 194 | free(arg); |
| 195 | |
| 196 | //Register new thread to user data structure |
| 197 | pthread_user_data_internal(pthread_self()); |
| 198 | |
| 199 | //TODO: user_data should do this automaticaly? |
| 200 | pthread_cleanup_push(pthread_user_data_cleanup, (void *)pthread_self()); |
| 201 | |
| 202 | //Check if we should be running according to pthread_pause sub-scheduler |
| 203 | //pthread_pause_reschedule(pthread_self()); |
| 204 | pthread_extra_yield(); |
| 205 | |
| 206 | //Run actual task |
| 207 | return task.start_routine(task.arg); |
| 208 | |
| 209 | pthread_cleanup_pop(1); //Needed by pthread_cleanup_push() macro |
| 210 | } |
| 211 | |
| 212 | int pthread_extra_create(pthread_t *restrict thread, |
| 213 | const pthread_attr_t *restrict attr, |
| 214 | void *(*start_routine)(void *), |
| 215 | void *restrict arg) { |
| 216 | |
| 217 | pthread_extra_wrapper_t *task = malloc(sizeof(pthread_extra_wrapper_t)); |
| 218 | assert(task != NULL); |
| 219 | task->start_routine=start_routine; |
| 220 | task->arg=arg; |
| 221 | return pthread_create(thread, attr, pthread_extra_thread_wrapper, task); |
| 222 | } |