#include <pthread.h>
#include <semaphore.h>
#include <time.h>
#include <sched.h>
#include <err.h>
#include <unistd.h>
#include <sys/param.h>
#include <sys/cpuset.h>
#include <stdio.h>
#include <stdlib.h>

pthread_barrier_t barrier;
sem_t *psem;

void * postfunc(void *arg __unused)
{
    struct timespec sleeptime;
    sleeptime.tv_sec = 0;
    sleeptime.tv_nsec = 100000000;
    while (1)
    {
        pthread_barrier_wait(&barrier);
        // Give the other thread a chance to wait on the semaphore.
        nanosleep(&sleeptime, NULL);
        sem_post(psem);
    }
    return NULL;
}

void waitfunc()
{
    sem_t sem;
    sem_init(&sem, 0, 0);
    psem = &sem;
    pthread_barrier_wait(&barrier);
    sem_wait(&sem);
    sem_destroy(&sem);
}

void otherfunc()
{
// XXX: 1200000 is not right, but will do for now
#if __FreeBSD_version > 1200000
#define semfield sem._kern._count
#else
#define semfield sem._kern._has_waiters
#endif
    sem_t sem;
    semfield = 0xFFFFFFFF;
    // Give the other thread a chance on the CPU so it can
    // possibly complete the sem_post() call.
    sched_yield();
    if (semfield != 0xFFFFFFFF)
    {
        printf("sem struct changed after yielding\n");
        exit(1);
    }
}


int main()
{
    // FIFO scheduling with medium priority.
    struct sched_param param;
    param.sched_priority = (sched_get_priority_max(SCHED_FIFO) + sched_get_priority_min(SCHED_FIFO)) / 2;
    if (sched_setscheduler(getpid(), SCHED_FIFO, &param) != 0)
    {
        err(1, "sched_setscheduler");
    }

    // Pin to CPU 0.
    cpuset_t setmask;
    CPU_ZERO(&setmask);
    CPU_SET(0, &setmask);
    cpusetid_t setid;
    if (cpuset(&setid) < 0)
    {
        err(1, "cpuset()");
    }
    if (cpuset_setaffinity(CPU_LEVEL_CPUSET, CPU_WHICH_CPUSET, setid, sizeof(setmask), &setmask) != 0)
    {
        err(1, "cpuset_setaffinity()");
    }

    pthread_barrier_init(&barrier, NULL, 2);

    pthread_t t;
    pthread_create(&t, NULL, postfunc, NULL);

    while (1)
    {
        waitfunc();
        otherfunc();
    }
}