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>
#include <time.h>




/* #define USE_PTHREAD_NATIVE_RWLOCK */




#if ! defined (USE_PTHREAD_NATIVE_RWLOCK)
   typedef struct rwmutex rwmutex_test_t;
#  define rwmutex_test_create rwmutex_create
#  define rwmutex_test_rdlock rwmutex_rdlock
#  define rwmutex_test_rdunlock rwmutex_rdunlock
#  define rwmutex_test_wrlock rwmutex_wrlock
#  define rwmutex_test_wrunlock rwmutex_wrunlock
#  define rwmutex_test_destroy rwmutex_destroy
#else
   typedef pthread_rwlock_t rwmutex_test_t;
#  define rwmutex_test_create(a, b) pthread_rwlock_init(a, NULL)
#  define rwmutex_test_rdlock pthread_rwlock_rdlock
#  define rwmutex_test_rdunlock pthread_rwlock_unlock
#  define rwmutex_test_wrlock pthread_rwlock_wrlock
#  define rwmutex_test_wrunlock pthread_rwlock_unlock
#  define rwmutex_test_destroy pthread_rwlock_destroy
#endif




#define RUNS 2U
#define ITERS 9999999U
#define READERS 12U
#define PRIME 256U
#define VOLATILE volatile




struct node {
  struct node* VOLATILE next;
};


struct shm {
  struct rwmutex_shm rwshm;
  char pad1[ATOMIC_L2CACHE - sizeof(struct rwmutex_shm)];
  struct node* VOLATILE head;
  char pad2[ATOMIC_L2CACHE - sizeof(struct node*)];
};




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


static struct shm* g_shm;
static rwmutex_test_t 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 int
slist_push(void) {
  struct node* node = malloc(sizeof(*node));

  if (node) {
    rwmutex_test_wrlock(&g_rwmutex);
    node->next = g_shm->head;
    g_shm->head = node;
    rwmutex_test_wrunlock(&g_rwmutex);
    return 1;
  }

  return 0;
}


static int
slist_pop(void) {
  struct node* node;
  rwmutex_test_wrlock(&g_rwmutex);
  node = g_shm->head;
  if (! node) {
    rwmutex_test_wrunlock(&g_rwmutex);
    return 0;
  }
  g_shm->head = node->next;
  rwmutex_test_wrunlock(&g_rwmutex);
  free(node);
  return 1;
}


static void
slist_prime(void) {
  unsigned i;
  for (i = 0; i < PRIME; ++i) slist_push();
}


static void
slist_flush(void) {
  while (slist_pop());
}


static int
slist_iterate(void) {
  struct node* node;

  rwmutex_test_rdlock(&g_rwmutex);

  node = g_shm->head;

  if (! node) {
    rwmutex_test_rdunlock(&g_rwmutex);
    return 0;
  }

  while (node) {
    node = node->next;
  }

  rwmutex_test_rdunlock(&g_rwmutex);
  return 1;
}




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

  for (i = 0; i < ITERS; ++i) {
    if (! slist_iterate()) break;
  }

  DBG_PRINTF(("reader completed\n"));
  return (void*)i;
}




int main(void) {
  unsigned runs;

  g_shm = ALIGN_UP(g_shm_raw, ATOMIC_L2CACHE);
  rwmutex_shm_init(&g_shm->rwshm);
  g_shm->head = NULL;

  for (runs = 0; runs < RUNS; ++runs) {
    unsigned i;
    unsigned long iters = ITERS * READERS, seconds = 0;
    clock_t start, end;
    pthread_t tid[READERS];

    rwmutex_test_create(&g_rwmutex, &g_shm->rwshm);

    slist_prime();

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

    start = clock() / CLOCKS_PER_SEC;

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

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

    end = clock() / CLOCKS_PER_SEC;

    seconds = (unsigned long)(end - start);

    if (seconds) iters /= seconds;

    slist_flush();

    rwmutex_test_destroy(&g_rwmutex);

    printf("READERS ACHIEVED %lu ITERATIONS PER-THREAD-PER-SECOND\n\n",
      iters / READERS);

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

    fflush(stdout);
  }

  display_prompt("RWMUTEX PROGRAM COMPLETED", 0);
  return 0;
}