Chris M Thomasson

/* Debugging Support
______________________________________________________________*/
#include <assert.h>




#if ! defined (NDEBUG)
#  include <stdio.h>
#  define DBG_PRINTF(mp_exp) printf mp_exp
#else
#  define DBG_PRINTF(mp_exp) ((void)0)
#endif








/* x86-32 Atomic API (Fetch-and-Add only!)
______________________________________________________________*/
#include <limits.h>


typedef int atomic_x86_word32;
typedef unsigned int atomic_x86_uword32;
#define ATOMIC_X86_WORD32_MAX (INT_MAX)
#define ATOMIC_X86_L2CACHE (128U)


typedef char atomic_x86_static_assert[
 sizeof(atomic_x86_word32) == 32 / CHAR_BIT &&
 sizeof(atomic_x86_uword32) == 32 / CHAR_BIT &&
 sizeof(void*) == 32 / CHAR_BIT ? 1 : -1
];


#if defined (_MSC_VER)
  __declspec(naked)
  static atomic_x86_word32
  atomic_x86_faa32(
   atomic_x86_word32 volatile* self,
   atomic_x86_word32 const addend
  ) {
    _asm {
      MOV ECX, [ESP + 4]
      MOV EAX, [ESP + 8]
      LOCK XADD [ECX], EAX
      RET
    }
  }
#elif defined (__GNUC__)
  __attribute__((always_inline))
  static __inline__
  atomic_x86_word32
  atomic_x86_faa32(
   atomic_x86_word32 volatile* self,
   atomic_x86_word32 const addend
  ) {
    atomic_x86_word32 ret;

    __asm__ __volatile__(
      "LOCK XADDL %2, %1;\n"
      : "=&r" (ret),
        "=m"  (*self)
      : "0"   (addend)
      : "memory",
        "cc"
    );

    return ret;
  }
#else
#  error Compiler Not Supported!
#endif


typedef atomic_x86_word32 atomic_word32;
#define ATOMIC_WORD32_MAX ATOMIC_X86_WORD32_MAX
#define ATOMIC_L2CACHE ATOMIC_X86_L2CACHE
#define atomic_faa32 atomic_x86_faa32








/* Alignment Support
______________________________________________________________*/
#include <stddef.h>


typedef atomic_x86_uword32 uintptr_type;


#define ALIGN_UP(mp_ptr, mp_align) \
  ((void*)( \
    (((uintptr_type)(mp_ptr)) + ((mp_align) - 1U)) \
    & ~(((mp_align) - 1U)) \
  ))


#define ALIGN_ASSERT(mp_ptr, mp_align) \
  (((void*)(mp_ptr)) == ALIGN_UP(mp_ptr, mp_align))


#define ALIGN_BUFSIZE(mp_size, mp_align) \
  ((size_t)(((mp_size) + (mp_align) - 1U)))








/* Read/Write Mutex By Chris M. Thomasson
______________________________________________________________*/
#define _XOPEN_SOURCE 600


#include <pthread.h>
#include <semaphore.h>
#include <stdlib.h>
#include <errno.h>




/* #define SEM_USE_POST_MULTIPLE */
#define RWMUTEX_COUNT_MAX ATOMIC_WORD32_MAX




static void
sem_wait_safe(
 sem_t* const self
) {
  while (sem_wait(self)) {
    if (errno != EINTR) abort();
  }
}


static void
sem_post_multiple_safe(
 sem_t* const self,
 int count
) {
#if defined (SEM_USE_POST_MULTIPLE)
  sem_post_multiple(self, count);
#else
  while (count) {
    sem_post(self);
    --count;
  }
#endif
}


struct rwmutex_shm {
  atomic_word32 count;
  atomic_word32 rdwake;
};


struct rwmutex {
  struct rwmutex_shm* shm;
  sem_t rdwset;
  sem_t wrwset;
  pthread_mutex_t wrmtx;
};


static void
rwmutex_shm_init(
 struct rwmutex_shm* const self
) {
  self->count = RWMUTEX_COUNT_MAX;
  self->rdwake = 0;
}


static int
rwmutex_create(
 struct rwmutex* const self,
 struct rwmutex_shm* shm
) {
  int state = sem_init(&self->rdwset, 0, 0);
  if (! state) {
    state = sem_init(&self->wrwset, 0, 0);
    if (! state) {
      state = pthread_mutex_init(&self->wrmtx, NULL);
      if (! state) {
        self->shm = shm;
        return 0;
      }
      sem_destroy(&self->wrwset);
    }
    sem_destroy(&self->rdwset);
  }
  return state;
}


static void
rwmutex_destroy(
 struct rwmutex* const self
) {
  pthread_mutex_destroy(&self->wrmtx);
  sem_destroy(&self->wrwset);
  sem_destroy(&self->rdwset);
}


static void
rwmutex_rdlock(
 struct rwmutex* const self
) {
  if (atomic_faa32(&self->shm->count, -1) < 1) {
    sem_wait_safe(&self->rdwset);
  }
}


static void
rwmutex_rdunlock(
 struct rwmutex* const self
) {
  if (atomic_faa32(&self->shm->count, 1) < 0) {
    if (atomic_faa32(&self->shm->rdwake, -1) == 1) {
      sem_post(&self->wrwset);
    }
  }
}


static int
rwmutex_rdtowrlock(
 struct rwmutex* const self
) {
  if (! pthread_mutex_trylock(&self->wrmtx)) {
    atomic_word32 count =
      atomic_faa32(&self->shm->count, (-RWMUTEX_COUNT_MAX) + 1) + 1;
    if (count < RWMUTEX_COUNT_MAX) {
      if (atomic_faa32(&self->shm->rdwake, RWMUTEX_COUNT_MAX - count)
            + RWMUTEX_COUNT_MAX - count) {
        sem_wait_safe(&self->wrwset);
      }
    }
    return 0;
  }
  return EBUSY;
}


static void
rwmutex_wrlock(
 struct rwmutex* const self
) {
  atomic_word32 count;
  pthread_mutex_lock(&self->wrmtx);
  count = atomic_faa32(&self->shm->count, -RWMUTEX_COUNT_MAX);
  if (count < RWMUTEX_COUNT_MAX) {
    if (atomic_faa32(&self->shm->rdwake, RWMUTEX_COUNT_MAX - count)
          + RWMUTEX_COUNT_MAX - count) {
      sem_wait_safe(&self->wrwset);
    }
  }
}


static void
rwmutex_wrunlock(
 struct rwmutex* const self
) {
  atomic_word32 count =
    atomic_faa32(&self->shm->count, RWMUTEX_COUNT_MAX);
  if (count < 0) {
    sem_post_multiple_safe(&self->rdwset, -count);
  }
  pthread_mutex_unlock(&self->wrmtx);
}








/* Simple Test Application
______________________________________________________________*/
#include <stdio.h>
#include <sched.h>




#define ITERS 99999U
#define RUNS 10U
#define READERS 7U
#define WRITERS 3U
#define THREADS (READERS + WRITERS)
#define READER_TO_WRITER 12U
#define READER_LOCK_YIELD 10U
#define READER_UNLOCK_YIELD 10U
#define WRITER_LOCK_YIELD 25U
#define WRITER_UNLOCK_YIELD 4U
#define COMPLETE ((WRITERS * ITERS) * 2U)
#define IS_COMPLETE() \
  (g_shm->state + g_shm->rdtowr == COMPLETE)




struct shm {
  struct rwmutex_shm shm;
  char pad[ATOMIC_L2CACHE - sizeof(struct rwmutex_shm)];
  unsigned volatile state;
  unsigned volatile rdtowr;
};


static unsigned char* g_raw_shm[
 ALIGN_BUFSIZE(sizeof(struct shm), ATOMIC_L2CACHE)
];


static struct shm* g_shm;
static struct rwmutex g_rwmutex;




static void
display_prompt(
 char const* msg,
 int fatal
) {
  printf("\n\n\n____________________________________________\n"
         "%s\n\nPress <ENTER> to exit...", msg);
  fflush(stdout);
  fflush(stdin);
  fflush(stderr);
  getchar();
  if (fatal) abort();
}




static void*
reader(
 void* state
) {
  unsigned i;
  DBG_PRINTF(("reader running\n"));

  for (i = 0; ; ++i) {
    rwmutex_rdlock(&g_rwmutex);

    if (! (i % READER_TO_WRITER)) {
      if (! rwmutex_rdtowrlock(&g_rwmutex)) {

        ++g_shm->rdtowr;
        if (! (i % WRITER_LOCK_YIELD)) sched_yield();
        --g_shm->state;
        if (! (i % WRITER_LOCK_YIELD)) sched_yield();
        ++g_shm->rdtowr;
        if (! (i % WRITER_LOCK_YIELD)) sched_yield();
        --g_shm->state;

        if (g_shm->state % 2U || g_shm->rdtowr % 2U) {
          display_prompt("READER-TO-WRITER: COUNTER OUT-OF-SYNC!!!!", 1);
        }

        DBG_PRINTF(("reader-to-writer (%u of %u)\n",
          g_shm->state + g_shm->rdtowr, COMPLETE));
        rwmutex_wrunlock(&g_rwmutex);
        if (! (i % WRITER_UNLOCK_YIELD)) sched_yield();
        rwmutex_rdlock(&g_rwmutex);
      }
    }

    if (! (i % READER_LOCK_YIELD)) sched_yield();

    DBG_PRINTF(("reader (%u of %u)\n",
      g_shm->state + g_shm->rdtowr, COMPLETE));

    if (g_shm->state % 2U) {
      display_prompt("READER: COUNTER OUT-OF-SYNC!!!!", 1);
    }

    if (IS_COMPLETE()) {
      rwmutex_rdunlock(&g_rwmutex);
      break;
    }

    rwmutex_rdunlock(&g_rwmutex);

    if (! (i % READER_UNLOCK_YIELD)) sched_yield();
  }

  DBG_PRINTF(("reader completed\n"));
  return NULL;
}


static void*
writer(
 void* state
) {
  unsigned i;
  DBG_PRINTF(("writer running\n"));

  for (i = 0; i < ITERS; ++i) {
    rwmutex_wrlock(&g_rwmutex);

    ++g_shm->state;

    if (! (i % WRITER_LOCK_YIELD)) sched_yield();

    ++g_shm->state;

    if (g_shm->state % 2U) {
      display_prompt("WRITER: COUNTER OUT-OF-SYNC!!!!", 1);
    }

    DBG_PRINTF(("writer (%u of %u)\n",
      g_shm->state + g_shm->rdtowr, COMPLETE));

    rwmutex_wrunlock(&g_rwmutex);

    if (! (i % WRITER_UNLOCK_YIELD)) sched_yield();
  }

  DBG_PRINTF(("writer completed\n"));
  return NULL;
}




int main(void) {
  unsigned runs;

  g_shm = ALIGN_UP(g_raw_shm, ATOMIC_L2CACHE);
  rwmutex_shm_init(&g_shm->shm);

  for (runs = 0; runs < RUNS; ++runs) {
    unsigned i;
    pthread_t tid[THREADS];

    g_shm->state = g_shm->rdtowr = 0;

    printf("TEST RUN %u of %u RUNNING...\n", runs + 1, RUNS);

    rwmutex_create(&g_rwmutex, &g_shm->shm);

    for (i = 0; i < THREADS; ++i) {
      if (i < READERS) {
        pthread_create(&tid[i], NULL, reader, NULL);
      } else {
        pthread_create(&tid[i], NULL, writer, NULL);
      }
    }

    for (i = 0; i < THREADS; ++i) {
      pthread_join(tid[i], NULL);
    }

    rwmutex_destroy(&g_rwmutex);

    if (! IS_COMPLETE()) {
      display_prompt("COUNTER OUT-OF-SYNC!!!!", 1);
    }

    printf("TEST RUN %u of %u COMPLETED!!!\n"
           "------------------------------------\n",
           runs + 1, RUNS);

    fflush(stdout);
  }

  display_prompt("RWMUTEX PROGRAM COMPLETED", 0);

  return 0;
}