Assign threads to individual cores on Linux

pthread_hook.c

// gcc pthread_hook.c -shared -o libpthread_hook.so
// LD_PRELOAD=./libpthread_hook.so ./test_app

#define _GNU_SOURCE
#include <sched.h> // cpu_set_t, CPU_SET
#include <dlfcn.h>
#include <errno.h> // EINVAL
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h> // sysconf

static int stick_this_thread_to_core(int core_id)
{
	int num_cores = sysconf(_SC_NPROCESSORS_ONLN);
	if (core_id < 0 || core_id >= num_cores)
		return EINVAL;

	cpu_set_t cpuset;
	CPU_ZERO(&cpuset);
	CPU_SET(core_id, &cpuset);

	pthread_t current_thread = pthread_self();
	return pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset);
}

typedef void* (*start_routine_t)(void* arg);

struct pthread_args_t
{
	start_routine_t start_routine_real;
	void* arg_real;
};

static void* pthread_start_routine(void* arg)
{
        static int next_core = 4;
        if (stick_this_thread_to_core(next_core) != 0)
        {
                fprintf(stderr, "Cannot assign CPU core %d to thread %llu\n", next_core, (unsigned long long)pthread_self());
                exit(-1);
        }
        printf("Bound thread %llu to core %d\n", (unsigned long long)pthread_self(), next_core);
        next_core++;

	struct pthread_args_t* args = (struct pthread_args_t*)arg;
	void* result = args->start_routine_real(args->arg_real);
	free(args);
	return result;
}

int pthread_create(pthread_t *thread, const pthread_attr_t *attr,
	start_routine_t start_routine, void *arg)
{
	static void* lib = NULL;
	lib = dlopen("libpthread.so", RTLD_LAZY);
	if (!lib)
	{
		fprintf(stderr, "Cannot open libpthread.so: %s\n", dlerror()); 
		exit(-1);
	}

	typedef int (*pthread_create_t)(pthread_t *thread, const pthread_attr_t *attr,
		void *(*start_routine) (void *), void *arg);
	static pthread_create_t pthread_create_real = NULL;
	pthread_create_real = dlsym(lib, "pthread_create");
	if (!pthread_create_real)
	{
		fprintf(stderr, "Cannot find symbol pthread_create: %s\n", dlerror());
		exit(-1);
	}

	struct pthread_args_t* args = (struct pthread_args_t*)malloc(sizeof(struct pthread_args_t));
	args->start_routine_real = start_routine;
	args->arg_real = arg;

	return pthread_create_real(thread, attr, pthread_start_routine, (void*)args);
}