smp.cc

Go to the documentation of this file.

#define _GNU_SOURCE 1

#define NEED_SMP_DECLS 1
#include "tbci/basics.h"
#include "tbci/smp.h"
#include <stdio.h>
#include <time.h>

#include <errno.h>
#include <fcntl.h>

#ifdef _POSIX_THREADS
#ifdef SMP

//#include <signum.h>
#include <stdlib.h>

#ifdef HAVE_SYS_SYSINFO_H
# include <sys/sysinfo.h>
#endif

#ifdef HAVE_SCHED_H
# include <sched.h>
#endif

#ifdef HAVE_NUMA_H
# include <numa.h>
# include <numaif.h>
#endif

#ifndef MAX_THREADS
# define MAX_THREADS 160
#endif

#include "tbci/list.h"

#ifdef __linux__
# include <sys/syscall.h>
# include <unistd.h>
inline static pid_t gettid()
{
        return syscall(SYS_gettid);
}
#else
# define gettid getpid
#endif

NAMESPACE_TBCI

int num_threads = 0;
int threads_busy = 0;
int numa_avail = 0;
struct thr_struct *threads = 0;
pid_t main_thread_pid = 0;
bool threads_bound = false;
bool bound_main = false;
int main_numa_node = 0;
THREAD__ int ismainthread = 1;
THREAD__ int thrno = 0;
THREAD__ struct thr_struct *this_thread = 0;

#ifdef HAVE_LIBNUMA
unsigned page_size;
unsigned long page_mask;
#endif

#ifdef DEBUG_THREAD
# define TCHK(x) if (UNLIKELY(err = x)) fprintf (stderr, #x " failed: %s\n", CSTD__ strerror (err))
# define ERRDECL int err = 0
#else
# define TCHK(x) x
# define ERRDECL 
#endif

#ifdef HAVE_SCHED_GETAFFINITY
static cpu_set_t saved_cpuset;

#ifndef HAVE_CPU_COUNT
#undef CPU_COUNT
static int CPU_COUNT(const cpu_set_t *cpus)
{
        int cnt = 0;
        for (int i = 0; i < CPU_SETSIZE; ++i)
                if (CPU_ISSET(i, cpus))
                        ++cnt; 
        return cnt;
}
#endif
#ifndef CPU_XOR
static void CPU_XOR(cpu_set_t *dest, cpu_set_t *src1, cpu_set_t *src2)
{
        for (int i = 0; i < CPU_SETSIZE; ++i)
                if (CPU_ISSET(i, src1) ^ CPU_ISSET(i, src2))
                        CPU_SET(i, dest);
                else
                        CPU_CLR(i, dest);
}
#endif
#ifndef CPU_AND
static void CPU_AND(cpu_set_t *dest, cpu_set_t *src1, cpu_set_t *src2)
{
        for (int i = 0; i < CPU_SETSIZE; ++i)
                if (CPU_ISSET(i, src1) & CPU_ISSET(i, src2))
                        CPU_SET(i, dest);
                else
                        CPU_CLR(i, dest);
}
#endif


static int next_set_bit(const int start, const cpu_set_t *cpus)
{
        for (int i = start; i < CPU_SETSIZE; ++i)
                if (CPU_ISSET(i, cpus))
                        return i;
        return -1;
}

static char cpu_buf[1024];
static const char* cpu_str(const cpu_set_t *cpus)
{
        static volatile int off = 0;
        int old_off = off;
        if (off < 896)
                off += 128;
        else
                off = 0;
        char* ret = cpu_buf+old_off+1;
        int ptr = old_off; 
        cpu_buf[ptr+1] = 0;
        for (int i = 0; i < CPU_SETSIZE && ptr < 1016; ++i)
                if (CPU_ISSET(i, cpus))
                        ptr += sprintf(cpu_buf+ptr, " %i", i);
        return ret;
}
#define CPU_SETBYTES ((CPU_SETSIZE+7)/8)
#if defined(__linux__)

static void parse_siblings(const int cpu, cpu_set_t *cpus, const int remove)
{
        char fn[80];
        sprintf(fn, "/sys/devices/system/cpu/cpu%i/topology/thread_siblings", cpu);
        FILE *f = fopen(fn, "r");
        if (!f)
                return;
        int sibl[(CPU_SETSIZE+31)/32]; 
        int nr_ints = 0;
        bool end = false;
        /* Linux reports this as field of ints, last int covers first 32 CPUs */
        while (!end) {
                int msk;
                if (fscanf(f, "%x,", &msk) == EOF) {
                        if (fscanf(f, "%x", &msk) == EOF)
                                break;
                        end = true;
                }
                sibl[nr_ints++] = msk;
        }
        fclose(f);
        /* Convert to cpu_set_t data structure (machine endian field of longs)
         * first long covers first 32/64 CPUs */
        cpu_set_t sibl_set; CPU_ZERO(&sibl_set);
        long* sibl_ptr = (long*)&sibl_set;
#if __BYTE_ORDER == __LITTLE_ENDIAN
        for (int j = nr_ints-1; j >= 0; --j)
                *sibl_ptr++ = sibl[j];
#else
        long* sibl_ptr2 = (long*)&sibl;
        for (int j = (nr_ints*sizeof(int)/sizeof(long))-1; j >= 0; --j)
                *sibl_ptr++ = sibl_ptr2[j];
#endif
        //fprintf(stderr, "Siblings for cpu%i: %s\n", cpu, cpu_str(&sibl_set));
        //fprintf(stderr, "CPUS before rmv: %s\n", cpu_str(cpus));
        if (!CPU_ISSET(cpu, &sibl_set))
                abort();
        if (!remove) {
                memcpy(cpus, &sibl_set, CPU_SETBYTES);
                return;
        }
        CPU_CLR(cpu, &sibl_set);
        CPU_AND(&sibl_set, &sibl_set, cpus);
        CPU_XOR(cpus, cpus, &sibl_set);
        //fprintf(stderr, "CPUS after rmvl: %s\n", cpu_str(cpus));
}

static void add_siblings(const int cpu, cpu_set_t *cpus)
{
        parse_siblings(cpu, cpus, 0);
}

static void remove_hyperthreads(cpu_set_t *cpus)
{
#ifdef THREAD_STAT
        fprintf(stderr, "CPU set before HT removal: %s\n", cpu_str(cpus));
#endif
        for (int i = CPU_SETSIZE-1; i >=0; --i) {
                if (!CPU_ISSET(i, cpus))
                        continue;
                parse_siblings(i, cpus, 1);
        }
#ifdef THREAD_STAT
        fprintf(stderr, "CPU set after  HT removal: %s\n", cpu_str(cpus));
#endif
}
#else
static void remove_hyperthreads(cpu_set_t *cpus)
{
}
#endif

#endif

/* detect the # of cpus for glibc and/or linux systems */
static int detect_num_cpu (int rmv_ht)
{
        int cpus = 0;
#ifdef HAVE_SCHED_GETAFFINITY
        pid_t pid = gettid();
        if (! sched_getaffinity(pid, CPU_SETBYTES, &saved_cpuset)) {
                if (rmv_ht)
                        remove_hyperthreads(&saved_cpuset);
                return CPU_COUNT(&saved_cpuset);
        }
#endif
#ifdef HAVE_GET_NPROCS // defined(__GLIBC__) &&  __GLIBC__ >= 2
        cpus = get_nprocs ();
        if (cpus <= 0) 
                return 2;
#elif defined(__linux__) && (defined(__i386__) || defined(__x86_64__))
        FILE* cpuinfo;
        char buf[128];
        cpuinfo = fopen ("/proc/cpuinfo", "r");
        if (cpuinfo <= 0) 
                return 2;       /* return 2, if we can't find out */
        while (!feof (cpuinfo)) {
                fgets (buf, 128, cpuinfo);
                if (CSTD__ memcmp (buf+1, "rocessor", 8) == 0) 
                        cpus++;
        }
#endif
#ifdef DEBUG_THREAD
        fprintf (stderr, "%i CPUs detected.\n", cpus);
#endif
        return (cpus == 0 ? 1: cpus);
}

#ifdef HAVE_GETLOADAVG
static int loadavg ()
{
        double loads[3];
        int err = getloadavg (loads, 3);
        if (err == 1)
                return (int)loads[0];
        else if (err > 1)
                return (int)loads[1];
        else 
                return 0;
}
#endif

class cback {
        public:
                cbackfn *ctor;
                cbackfn *dtor;
                void *parm;
                cback(cbackfn ct, cbackfn dt, void *p)
                : ctor(ct), dtor(dt), parm(p) {};
                cback()
                : ctor(NULL), dtor(NULL), parm(NULL) {};
                void init(const int thr)
                { ctor(parm, thr); }
                void deinit(const int thr)
                { dtor(parm, thr); }
};

static List<cback> thread_cbacks;
/* Instantiate */
template class List<cback>;

void thread_reg_callback(cbackfn ctor, cbackfn dtor, void *parm)
{
        cback callback(ctor, dtor, parm);
        thread_cbacks.append(&callback);
        //fprintf(stderr, "Register callback %p (%p %p %p)\n", &callback, ctor, dtor, parm);
}

void thread_dereg_callback(cbackfn ctor, cbackfn dtor, void *parm)
{
        cback callback(ctor, dtor, parm);
        cback *cbk = thread_cbacks.setcurr(&callback);
        BCHK(!cbk, NumErr, Deregister unrgistered callback, (long int)ctor, );
        thread_cbacks.delcurr();
        //fprintf(stderr, "Deregister callback %p (%p %p %p)\n", &callback, ctor, dtor, parm);
}


void lina_err (struct thr_ctrl *tc)
{
        fprintf (stderr, " Thread (%i) synchronization problem!\n", tc->t_no);
        fflush (stderr); abort ();
}

void lina_empty (struct thr_ctrl *dummy)
{
        /* nothing */
}

void* empty_thread (void *dummy)
{
        return NULL;
}

#ifdef HAVE_LIBNUMA

static int cpu_to_node(int cpu)
{
        char fn[80];
        for (int n = 0; n < numa_num_possible_nodes(); ++n) {
                sprintf(fn, "/sys/devices/system/node/node%i/cpu%i", n, cpu);
                if (!access(fn, R_OK))
                        return n;
        }
        return -1;
}

int do_numa_init(unsigned int cpu, struct thr_struct *ts, unsigned long stack)
{
        int node = cpu_to_node(cpu);
        struct bitmask *nodes = numa_allocate_nodemask();
        //fprintf(stderr, "Thr %i: CPU %i Node %i\n", ts->t_no, cpu, my_node);
        cpu_set_t cpuset;
        numa_bitmask_clearall(nodes);
        numa_bitmask_setbit(nodes, node);
        numa_set_preferred(node);
#ifdef HAVE_SCHED_GETAFFINITY
        numa_set_membind(nodes);
#else
        numa_bind(nodes);
#endif
        //sched_yield();
        //numa_set_localalloc();
        ts->numa_node = node;
#ifdef HAVE_SCHED_GETAFFINITY
        CPU_ZERO(&cpuset);
        CPU_SET(cpu, &cpuset);
        parse_siblings(cpu, &cpuset, 0);
#ifdef HAVE_PTHREAD_GETAFFINITY_NP
        pthread_setaffinity_np(ts->t_id, CPU_SETBYTES, &cpuset);
#else
        sched_setaffinity(ts->t_pid, CPU_SETBYTES, &cpuset);
#endif
#endif
        /* Move stack page to my node */
        if (1) {
                void *pages[] = {(void*)(stack & page_mask)};
                const int nodes[] = {node};
                int status = node, oldstat;
                long err = 1;
                numa_move_pages(0, 1, pages, 
                                NULL, &status, MPOL_MF_MOVE);
                oldstat = status;
                if (oldstat != node)
                        err = numa_move_pages(0, 1, pages, 
                                        nodes, &status, MPOL_MF_MOVE);
#ifdef THREAD_DEBUG
                fprintf(stderr, "Numa thr %i(tid %p, lwp %i): CPUs %s Node %i (Stack @%p: %i->%i(%li)\n", 
                                ts->t_no, (void*)ts->t_id, ts->t_pid,
                                cpu_str(&cpuset), node, pages[0], oldstat,
                                status, err);
#endif
        }
        numa_free_nodemask(nodes);
        return node;
}

void numa_init_job(struct thr_ctrl *tc)
{
        do_numa_init(tc->t_size, (struct thr_struct*)tc->t_par[0], (unsigned long)tc);
}

#define PG_PC 512
int do_numa_move_pages(int node, int fault_in, unsigned long firstaddr, unsigned long lastaddr)
{
        void* pages[PG_PC];
        int nodes[PG_PC], status[PG_PC];
        int nr_pages = 0; int nr_moved = 0;
        unsigned long orig_addr = firstaddr;
#ifdef VALGRIND
        memset(status, 0, PG_PC*sizeof(int));
#endif
        if (firstaddr - (firstaddr & page_mask) >= page_size/2)
                firstaddr = firstaddr & page_mask;
        else
                firstaddr = (firstaddr + (page_size - 1)) & page_mask;
        if (lastaddr - (lastaddr & page_mask) > page_size/2)
                lastaddr = lastaddr & page_mask;
        else
                lastaddr = (lastaddr + (page_size - 1)) & page_mask;
#if 0
        fprintf(stderr, "numa_move_pages(%p -- %p to node %i: %li pg %i calls)\n",
                        (void*)firstaddr, (void*)lastaddr, node,
                        (lastaddr-firstaddr)/page_size,
                        (lastaddr-firstaddr)/page_size/PG_PC);
#endif
        while (lastaddr > firstaddr) {
                int i;
                for (i = 0; (i < PG_PC) && (firstaddr < lastaddr); ++i, firstaddr += page_size) {
                        pages[i] = (void*)firstaddr;
                        nodes[i] = node;
                        /* Note: Pages might be not allocated yet and won't be ... */
                        if (fault_in) {
                                if (UNLIKELY(firstaddr < orig_addr))
                                        *(volatile int*)orig_addr = *(volatile int*)orig_addr;
                                else
                                        *(volatile int*)firstaddr = *(volatile int*)firstaddr;
                        }
                }
                nr_pages += i;
                long err = numa_move_pages(0, i, pages,
                                NULL, status, MPOL_MF_MOVE);
                if (err)
                        fprintf(stderr, "numa_move_pages(%p -- %p to node %i): %s\n",
                                pages[0], pages[i-1], node, strerror(errno));
                int to_move = 0; err = 0;
                for (int j = 0; j < i; ++j)
                        if (status[j] != node)
                                ++to_move;
#if 0
                fprintf(stderr, "numa_move_pages(%p -- %p: %i pages, move %i to node %i (from %i and others)\n",
                                pages[0], pages[i-1], i, to_move, node, status[0]);
#endif
                nr_moved += to_move;
                if (to_move)
                        err = numa_move_pages(0, i, pages,
                                nodes, status, MPOL_MF_MOVE);
                if (err)
                        fprintf(stderr, "numa_move_pages(%p -- %p to node %i): %s\n",
                                pages[0], pages[i-1], node, strerror(errno));
        }
        return nr_moved;
}

void numa_move_pages_job(struct thr_ctrl *tc)
{
        tc->t_res_l =
                do_numa_move_pages(tc->t_size, tc->t_off,
                                (unsigned long)tc->t_par[0],
                                (unsigned long)tc->t_par[1]);
}

#endif

#if defined(__i386__) || defined(__x86_64__)
# define _cpu_relax() asm ("rep; nop")
#else
# define _cpu_relax() do {} while(0)
#endif

/* Sidenote: We do no locking, approx. numbers are OK here */
unsigned long poll_succ = 0;
unsigned long poll_usucc = 0;
unsigned long poll_fail = 0;
unsigned long poll_efail = 0;

#ifndef POLL_REP
# define POLL_REP 1
#endif
#ifndef POLL_REP2
# define POLL_REP2 168
#endif
static int busy_read(int fd, void* ptr, size_t sz, int rep=POLL_REP)
{
        size_t rd = 0;
        int w = rep;
        while (w-- && (signed)(rd = read(fd, ptr, sz)) < (signed)sz)
                _cpu_relax();
        if (sz == rd) {
                if (rep == POLL_REP)
                        ++poll_usucc;
                else
                        ++poll_succ;
                return rd;
        }
        // slow path
        if (rep == POLL_REP)
                ++poll_efail;
        else
                ++poll_fail;
        fcntl(fd, F_SETFL, 0);
        _cpu_relax();
        rd = read(fd, ptr, sz);
        PREFETCH_R(ptr, 2);
        fcntl(fd, F_SETFL, O_NONBLOCK);
        return rd;
}

void* lina_thread (void* thr)
{
        struct thr_struct *ts = (struct thr_struct*) thr;
        struct thr_ctrl tc;
        struct job_output out;
        int err;
#if defined(HAVE_TLS) || defined(HAVE_DTLS)
        ismainthread = 0;
        thrno = ts->t_no+1;
        this_thread = ts;
#endif
#ifdef VALGRIND
        memset(&tc, 0, sizeof(tc));
        memset(&out, 0, sizeof(out));
#endif
#ifdef DEBUG_THREAD
        fprintf (stderr, " Thread %i: Try to get setup lock\n", tc.t_no);
#endif
        ts->t_pid = gettid (); clock ();
        tc.t_no = ts->t_no;
#ifdef DEBUG_THREAD
        fprintf (stderr, " Thread %i setup: id %08lx, lwp %i; signal main thread ...\n", 
                 ts->t_no, ts->t_id, ts->t_pid);
#endif
        /* Now wait for some work to do. Finish when a NULL job becomes runnable */
        while (1) {
                /* Set job to error! */
                tc.t_job = lina_err;
                //memset(tc.t_res_dummy, 0, sizeof(tc.t_res_dummy));
                /* Ready to accept a job: Wait for it */
                if (UNLIKELY((err = busy_read(ts->t_pipe_to_thread[0], &tc, sizeof(struct job_input))) != sizeof(struct job_input))) {
                        fprintf(stderr, "Thread %i: Reading failed! %i\n", tc.t_no, err);
                        abort();
                }
#ifdef DEBUG_THREAD
                fprintf (stderr, " Thread %i: Start job %li @ %p\n", ts->t_no, tc.t_job_no, tc.t_job);
                fflush (stderr);
#endif
                ts->t_done_var++;
                out.t_job_output_no = tc.t_job_no;
                if (!tc.t_job)
                        break; /* The end */
                else
                        (*tc.t_job) (&tc);
#ifdef DEBUG_THREAD
                fprintf (stderr, " Thread %i: Job done!\n", ts->t_no); 
                fflush (stderr);
#endif
                memcpy((void*)out.t_res_dummy, (const void*)tc.t_res_dummy, sizeof(out.t_res_dummy));
                /* Do we need some memory barrier here? But we are cache-coherent, no? */
                if (UNLIKELY((err = write(ts->t_pipe_from_thread[1], &out, sizeof(struct job_output))) != sizeof(struct job_output))) {
                        fprintf(stderr, "Thread %i: Writing failed! %i\n", ts->t_no, err);
                        abort();
                }
#ifdef DEBUG_THREAD
                fprintf (stderr, " Thread %i: Signaled Job done!\n", ts->t_no);
                fflush (stderr);
#endif
        }
#ifdef DEBUG_THREAD
        fprintf (stderr, " Thread %i: exit!\n", ts->t_no);
#endif
        out.t_retval = clock ();
        ts->t_done_var++;

#if defined(HAVE_CLOCK_GETTIME) && defined(THREAD_STAT)
        struct timespec tm;
        if (!clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tm)) {
                const double secs = tm.tv_sec + (double)tm.tv_nsec/1e9;
                out.t_retval = (long)(secs * CLOCKS_PER_SEC);
        }
#endif
        err = write(ts->t_pipe_from_thread[1], &out, sizeof(struct job_output));
        pthread_exit(&out.t_retval);
        return NULL;
}


int init_threads (const int num_cpu, const bool load_magic)
{
        int t, err, det_cpus, all_cpus;
        pthread_mutexattr_t mutattr;
        
        main_thread_pid = getpid ();
        /* No of CPUs */
        all_cpus = detect_num_cpu(0);
        det_cpus = detect_num_cpu(1);
        /* How many ? */
        if (num_cpu <= 0) {
                num_threads = det_cpus;
#ifdef HAVE_GETLOADAVG
                if (load_magic) {
                        /* Subtract load */
                        int lavg = loadavg ();
                        /* If load exceeds no of CPUs, use 2 CPUs */
                        if (all_cpus - lavg < num_threads) {
                                num_threads = MAX(1, all_cpus - lavg);
                                if (num_threads < 2 && det_cpus > 2)
                                        num_threads = 2;
                        }
                }
#endif
                if (num_cpu && num_threads > -num_cpu)
                        num_threads = -num_cpu;
        } else
                num_threads = num_cpu;
        /* Max is MAX_THREADS */
        if (num_threads > MAX_THREADS) 
                num_threads = MAX_THREADS;
        /* No threads? */
        if (num_threads == 1)
                num_threads = 0;
        /* Sanity check */
        if (num_threads > det_cpus)
                fprintf(stderr, "Warning: Number of threads %i larger than no of CPU cores %i!\n",
                        num_threads, det_cpus);
#ifdef HAVE_SCHED_GETAFFINITY
        else
                sched_setaffinity(main_thread_pid, CPU_SETBYTES, &saved_cpuset);
#endif
#ifdef TBCI_OMP
        omp_set_num_threads(num_threads);
#endif
        /* Alloc mem for control structs */
        if (num_threads >= 1) 
                threads = (struct thr_struct *) memalign(128, sizeof(struct thr_struct) * (num_threads));
                //threads = (struct thr_ctrl *) CSTD__ malloc (sizeof(struct thr_ctrl) * (num_threads));
        else 
                threads = NULL;
        if (threads) 
                CSTD__ memset (threads, 0, sizeof(struct thr_struct) * (num_threads));
        /* Some Un*xes only start the clock after the first call to clock() */
        clock ();
#if !defined(HAVE_TLS) && !defined(HAVE_DTLS)
        // If we don't have thread local storage (and no locking), 
        // we need to disable // concurrent memory caching allocations
        // now globally :-(
        if (num_threads >= 1) {
                ismainthread = 0;
                thrno = -1;
        }
#endif
        /* Now init threads */
        for (t = 0; t < num_threads; ++t) {
                int err;
                struct thr_struct *ts = &threads[t];
                //fprintf(stderr, "Debug: thread %i @ %p\n", t, tc);
                ts->t_no = t;
                /* Create pipes for thread sync */
                err = pipe(ts->t_pipe_to_thread);
                if (err) {
                        fprintf(stderr, "Failed to create pipe for thread %i\n", t);
                        abort();
                }
                err = pipe(ts->t_pipe_from_thread);
                if (err) {
                        fprintf(stderr, "Failed to create pipe for thread %i\n", t);
                        abort();
                }
                fcntl(ts->t_pipe_to_thread[0], F_SETFL, O_NONBLOCK);
                fcntl(ts->t_pipe_from_thread[0], F_SETFL, O_NONBLOCK);
                TCHK(pthread_create (&ts->t_id, NULL, lina_thread, ts));
        }
#ifdef DEBUG_THREAD
        fprintf (stderr, "%i threads @%p (sz=%zi) set up.\n", 
                        num_threads, threads, sizeof(struct thr_struct));
#endif
        for (cback *c = thread_cbacks.getfirst(); c != NULL; c = thread_cbacks.getnext()) {
                //fprintf(stderr, "Callback %p (%p %p %p)\n", c, c->ctor, c->dtor, c->parm);
                c->ctor(c->parm, num_threads);
        }
        //fprintf(stderr, "%li Callbacks registered\n", thread_cbacks.size());

        return num_threads;
}

void bind_threads (bool bind_main, bool enable_numa, bool add_sibl)
{
#ifdef HAVE_SCHED_GETAFFINITY
        /* Save CPU affinity mask */
        CPU_ZERO(&saved_cpuset);
        sched_getaffinity (main_thread_pid, CPU_SETBYTES, &saved_cpuset);
        if (num_threads > CPU_COUNT(&saved_cpuset))
                fprintf(stderr, "TBCI SMP: More threads(%i) than allowed CPUs(%i)!\n",
                        num_threads, CPU_COUNT(&saved_cpuset));
        cpu_set_t cpuset; CPU_ZERO(&cpuset);

        /* TODO: Be clever to which CPUs to bind to, e.g. avoid hyperthreads */
        int next_cpu = next_set_bit(0, &saved_cpuset);
        if (next_cpu < 0)
                abort();
        bound_main = bind_main;
#ifdef HAVE_LIBNUMA
        numa_avail = (numa_available() == 0);
        if (numa_max_node() == 0)
                numa_avail = 0;
        if (numa_avail) {
                page_size = numa_pagesize();
                page_mask = ~(page_size-1ULL);
        }
        if (!enable_numa)
                numa_avail = 0;
        fprintf(stderr, "NUMA: avail %i, maxnode %i\n", numa_avail, numa_max_node());
#endif /* HAVE_LIBNUMA */
        if (bind_main) {
                CPU_SET(next_cpu, &cpuset);
                if (add_sibl)
                        add_siblings(next_cpu, &cpuset);
#ifdef HAVE_LIBNUMA
                if (numa_avail) {
                        struct thr_struct ts;
                        ts.t_no = -1; ts.t_pid = main_thread_pid; 
                        ts.t_id = pthread_self();
                        main_numa_node = do_numa_init(next_cpu, &ts, (unsigned long)&ts);
                } else
#endif /* HAVE_LIBNUMA */
#ifdef HAVE_SCHED_GETAFFINITY
                        sched_setaffinity(main_thread_pid, CPU_SETBYTES, &cpuset);
#else
                        do {} while(0);
#endif
#ifdef THREAD_DEBUG             
                fprintf(stderr, "Set Main Thread %i: CPU %i (%s)\n", main_thread_pid,
                        next_cpu, cpu_str(&cpuset));
#endif
#ifdef DEBUG_THREAD
                sched_getaffinity(main_thread_pid, CPU_SETBYTES, &cpuset);
                fprintf(stderr, "Get Main Thread %i: CPU %i (%s)\n", main_thread_pid,
                        next_cpu, cpu_str(&cpuset));
#endif
                CPU_ZERO(&cpuset);
        }
#endif /* HAVE_SCHED_GETAFFINITY */
        threads_bound = true;
        int several_nodes = 0;
        for (int t = 0; t < num_threads; ++t) {
                struct thr_struct *ts = &threads[t];
                next_cpu = next_set_bit(next_cpu+1, &saved_cpuset);
                if (next_cpu == -1) 
                        next_cpu = next_set_bit(next_cpu+1, &saved_cpuset);
                CPU_SET(next_cpu, &cpuset);
                if (add_sibl)
                        add_siblings(next_cpu, &cpuset);
#ifdef HAVE_LIBNUMA
                if (numa_avail && cpu_to_node(next_cpu) != main_numa_node)
                        ++several_nodes;
                if (numa_avail) {
                        thread_start(t, numa_init_job, next_cpu, 
                                        (void*)ts, (void*)0);
                } else
#endif
#ifdef HAVE_PTHREAD_GETAFFINITY_NP
                        pthread_setaffinity_np(ts->t_id, CPU_SETBYTES, &cpuset);
#else
                        do {} while(0);
#endif
#ifdef THREAD_DEBUG
                fprintf(stderr, "Set Thread %i: CPUs %s\n", t, 
                        cpu_str(&cpuset));
#endif
#ifdef DEBUG_THREAD
                pthread_getaffinity_np(ts->t_id, CPU_SETBYTES, &cpuset);
                fprintf(stderr, "Get Thread %i: CPUs %s\n", t, 
                        cpu_str(&cpuset));
#endif
                CPU_ZERO(&cpuset);
        }
#ifdef HAVE_LIBNUMA
        if (numa_avail)
                for (int t = 0; t < num_threads; ++t)
                        thread_wait(t);
#endif
#ifdef THREAD_DEBUG
#ifdef HAVE_SCHED_GETAFFINITY
        sched_getaffinity(main_thread_pid, CPU_SETBYTES, &cpuset);
        fprintf(stderr, "Main thread: CPUs %s, Node %i\n", 
                        cpu_str(&cpuset), main_numa_node);
#endif
#ifdef HAVE_PTHREAD_GETAFFINITY_NP
        for (int t = 0; t < num_threads; ++t) {
                struct thr_struct *ts = &threads[t];
                pthread_getaffinity_np(ts->t_id, CPU_SETBYTES, &cpuset);
                fprintf(stderr, "Thread %i: CPUs %s, Node %i\n", 
                                ts->t_no, cpu_str(&cpuset), ts->numa_node);
        }
#endif
        if (!several_nodes) {
                numa_avail = 0;
#ifdef THREAD_DEBUG
                fprintf(stderr, "All threads on node %i, disabling NUMA\n",
                        main_numa_node);
#endif
        }
#endif  /* THREAD_DEBUG */
#ifdef TBCI_OMP
        int omp_thr = omp_get_max_threads();
#ifdef DEBUG_THREAD
        fprintf(stderr, "Now setting affinity for %i OpenMP threads ...\n", omp_thr);
#endif
#pragma omp parallel for private(cpuset, next_cpu) schedule(static,1)
        for (int t = 0; t < omp_thr; ++t) {
                int tid = omp_get_thread_num();
                next_cpu = next_set_bit(0, &saved_cpuset);
                for (int i = 0; i < t; ++i) {
                        next_cpu = next_set_bit(next_cpu+1, &saved_cpuset);
                        if (next_cpu == -1) 
                                next_cpu = next_set_bit(next_cpu+1, &saved_cpuset);
                }
                CPU_ZERO(&cpuset);
                CPU_SET(next_cpu, &cpuset);
                if (add_sibl)
                        add_siblings(next_cpu, &cpuset);
                sched_setaffinity(0, CPU_SETBYTES, &cpuset);
#ifdef THREAD_DEBUG
#pragma omp critical
                fprintf(stderr, "Setting OMP thread %i to %i CPUs %s\n", tid, CPU_COUNT(&cpuset), cpu_str(&cpuset));
#endif
        }
#endif  /* TBCI_OMP */
}

int tot_cpu_tm;
void free_threads ()
{
        int t;
        tot_cpu_tm = 0;
        ERRDECL;
        //fprintf (stderr, "Free threads ...\n");
        for (t = 0; t < num_threads; ++t) {
                int err;
                struct thr_struct *ts = &threads[t];
                struct job_input job;
                struct job_output out;
#ifdef VALGRIND
                memset(&job, 0, sizeof(job));
#endif
                job.t_job = 0;
                fcntl(ts->t_pipe_from_thread[0], F_SETFL, 0);
                err = write(ts->t_pipe_to_thread[1], &job, sizeof(job));
                err = read(ts->t_pipe_from_thread[0], &out, sizeof(out));
                void *res;
                TCHK(pthread_join (ts->t_id, &res));
                long tm = out.t_retval;
                //long tm = *(long*)res;
                tot_cpu_tm += tm;
#ifdef THREAD_STAT
                fprintf (stderr, " CPU time for thread %i  :%7.3f s\n", 
                         ts->t_no, (double)(tm)/CLOCKS_PER_SEC);
#endif
        }
        for (cback *c = thread_cbacks.getfirst(); c != NULL; c = thread_cbacks.getnext()) {
                //fprintf(stderr, "Callback %p (%p %p %p)\n", c, c->ctor, c->dtor, c->parm);
                c->dtor(c->parm, num_threads);
        }
        clock_t mainclock = clock();
#ifdef HAVE_CLOCK_GETTIME
        struct timespec tm;
        if (!clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tm)) {
                double secs = tm.tv_sec + (double)tm.tv_nsec/1e9;
                mainclock = (long)(secs*CLOCKS_PER_SEC);
        }
#endif
        tot_cpu_tm += mainclock;
#ifdef THREAD_STAT
        fprintf (stderr, " CPU time for main thr  :%7.3f s\n", 
                 (double)mainclock/CLOCKS_PER_SEC);
        fprintf (stderr, " CPU for all threads    :%7.3f s\n",
                 (double)(tot_cpu_tm)/CLOCKS_PER_SEC);
        fprintf (stderr, " Poll successes: %li, unexp. succ.: %li, failures: %li, exp. fails: %li\n", 
                        poll_succ, poll_usucc, poll_fail, poll_efail);
        fflush (stderr);
#endif
        if (num_threads > 0) 
                CSTD__ free (threads); 
        num_threads = 0; threads = 0;
#if !defined(HAVE_TLS) && !defined(HAVE_DTLS)
        ismainthread = 1;
        thrno = 0;
#endif
#ifdef HAVE_SCHED_GETAFFINITY
        if (bound_main) {
                sched_setaffinity(main_thread_pid, CPU_SETBYTES, &saved_cpuset);
                bound_main = false;
        }
        threads_bound = false;
#endif
}

static unsigned long job_no = 0;

void _thread_start_off (const int thr_no, thr_job_t ljob, 
                        const unsigned long off, const unsigned long sz,
                        va_list vl)
{
        void * par; unsigned t = 0;
        struct thr_struct* ts = &threads[thr_no];
        struct job_input job;
        ERRDECL;
#ifdef VALGRIND 
        memset(&job, 0, sizeof(job));
#endif
        //va_list (vl);
        BCHKNR(thr_no >= num_threads, NumErr, Starting thread no outside range, thr_no);
        threads_busy++;
        job.t_job = ljob; job.t_size = sz; job.t_off = off; job.t_job_no = job_no++;
        //va_start (vl, sz);
        while ((par = va_arg (vl, void*))) 
                job.t_par[t++] = par;
        //va_end (vl);
        BCHKNR(t > THREAD_MAX_ARGS, NumErr, Too many arguments to thread_start, t-1);
#ifdef DEBUG_THREAD
        fprintf (stderr, "Signal thread %i\n", ts->t_no); 
        fflush (stderr);
#endif
        if (write(ts->t_pipe_to_thread[1], &job, sizeof(job)) != sizeof(job)) {
                fprintf(stderr, "Signaling job %li to thread %i failed", 
                        job.t_job_no, ts->t_no);
                abort();
        }
        /* Immediatly reschedule */
        //sched_yield ();
        }

void thread_start_off (const int thr_no, thr_job_t job, 
                       const unsigned long off, const unsigned long sz, ...)
{
        va_list vl;
        va_start(vl, sz);
        _thread_start_off(thr_no, job, off, sz, vl);
        va_end(vl);
}

void thread_start ( const int thr_no, thr_job_t job, 
                    const unsigned long sz, ...)
{
        va_list vl;
        va_start(vl, sz);
        _thread_start_off(thr_no, job, 0, sz, vl);
        va_end(vl);
}

void thread_wait (const int thr_no, struct job_output *out)
{
        struct thr_struct *ts = &threads[thr_no];
        struct job_output out2;
        BCHKNR (thr_no >= num_threads, NumErr, Wait for non-existing thread, thr_no);
        /* Tell the thread that we wait for completion */
#ifdef DEBUG_THREAD
        fprintf (stderr, "Wait for thread %i\n", ts->t_no);
#endif
        if (busy_read(ts->t_pipe_from_thread[0], out? out: &out2, sizeof(out2), POLL_REP2) != sizeof(out2)) {
                fprintf(stderr, "Waiting for thread %i failed!\n", ts->t_no);
                abort();
        }
        threads_busy--;
#ifdef DEBUG_THREAD
        fprintf (stderr, "Thread %i signaled completion.\n", ts->t_no);
#endif
}


double thread_wait_result (const int thr_no)
{
        struct thr_struct *ts = &threads[thr_no];
        struct job_output out2;
        int err;
        BCHKNR (thr_no >= num_threads, NumErr, Wait for non-existing thread, thr_no);
#ifdef DEBUG_THREAD
        fprintf (stderr, "Wait for result of thread %i\n", ts->t_no);
#endif
        if (busy_read(ts->t_pipe_from_thread[0], &out2, sizeof(out2), POLL_REP2) != sizeof(out2)) {
                fprintf(stderr, "Waiting for thread %i failed!\n", ts->t_no);
                abort();
        }
        threads_busy--;
#ifdef DEBUG_THREAD
        fprintf (stderr, "Thread %i signaled completion\n", ts->t_no);
#endif
        return /*(volatile double)*/out2.t_res_d;
}

void disable_threads()
{
        threads_busy++;
#ifdef TBCI_OMP
        omp_set_num_threads(0);
#endif
}

void reenable_threads ()
{
        if (threads_busy)
                threads_busy--;
        else
                fprintf (stderr, "reenable_threads(): Threads already enabled.!\n");
#ifdef TBCI_OMP
        omp_set_num_threads(num_threads);
#endif
}

unsigned int curr_n_thr, last_n_thr, prev_n_thr;

NAMESPACE_END

#else /* no SMP */
/* These are for compatibility ... */

NAMESPACE_TBCI
pid_t main_thread_pid WEAKA = 0;

WEAK(int  init_threads (const int c, const bool load))
{ main_thread_pid = getpid(); clock (); return 0; }
WEAK(void bind_threads(bool, bool, bool)) {}
WEAK(void free_threads ()) {}
WEAK(void disable_threads ())
#ifdef TBCI_OMP
{ omp_set_num_threads(0); }
#else
{}
#endif
WEAK(void reenable_threads ()) 
#ifdef TBCI_OMP
{ omp_set_num_threads(omp_get_num_procs()); }   
#else
{}
#endif
WEAK(void thread_start (const int tno, thr_job_t job,
                        const unsigned long sz, ...))
{
        void * par; 
        struct thr_ctrl thrc; struct thr_ctrl* tc = &thrc;
        unsigned int t = 0;
        va_list (vl);
        tc->t_job = job; tc->t_size = sz; tc->t_off = 0;
        va_start (vl, sz);
        while ((par = va_arg (vl, void*)))
                tc->t_par[t++] = par;
        va_end (vl);
        fprintf (stderr, "Warning: Tried to start a thread with non-SMP compiled smp.cc !\n");
# ifdef ABORT_ON_ERR
        abort ();
# endif
        (*job)(tc);
}
WEAK(void thread_start_off (const int tno, thr_job_t job,
                            const unsigned long off, const unsigned long sz, ...))
{
        void * par;
        struct thr_ctrl thrc; struct thr_ctrl* tc = &thrc;
        unsigned int t = 0;
        va_list (vl);
        tc->t_job = job; tc->t_size = sz; tc->t_off = off;
        va_start (vl, sz);
        while ((par = va_arg (vl, void*)))
                tc->t_par[t++] = par;
        va_end (vl);
        fprintf (stderr, "Warning: Tried to start a thread with non-SMP compiled smp.cc !\n");
# ifdef ABORT_ON_ERR
        abort ();
#endif
        (*job)(tc);
}
WEAK(void  thread_wait (const int t)) {}
WEAK(void* thread_wait_useful (const int t, useful_job_t j, void* a))
{ return 0; }
WEAK(double thread_wait_result (const int thr_no)) 
{ return 0; }

unsigned int curr_n_thr WEAKA;
unsigned int last_n_thr WEAKA;
unsigned int prev_n_thr WEAKA;

NAMESPACE_END

#endif /* SMP */

NAMESPACE_TBCI
unsigned int tbci_control WEAKA = _TBCI_CWD_DEFAULT;
NAMESPACE_END

#endif /* _POSIX_THREADS */