lock free queue example

msqueue.c

#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include <pthread.h>
#include <sys/time.h>

typedef struct pointer {
    struct node *ptr;
    uint32_t count;
} pointer_t;

typedef struct node {
    void *value;
    pointer_t next;
} node_t;

typedef struct queue {
    pointer_t head;
    pointer_t tail;
} queue_t;

static inline uint32_t 
cas32(void *ptr, uintptr_t oldv, uintptr_t newv){
    asm volatile(
        "lock;\n cmpxchgl %1,%2" 
        : "=a" (oldv) 
        : "q" (newv), "m" (*(uintptr_t *)ptr),"0" (oldv)
        : "memory");
    return oldv;
}
#define barrier() __asm__ __volatile__("": : :"memory")
#define CAS(ptr, oldv, newv) (cas32((ptr), (oldv), (newv)) == (oldv) ? 1 : 0)
#define VAL(val) (*((uintptr_t *)&(val)))

queue_t *
init_queue(){
    queue_t *q;
    node_t *nd;
    q = malloc(sizeof(queue_t));
    nd = malloc(sizeof(node_t));
    nd->next.ptr = NULL;
    q->head.ptr = q->tail.ptr = nd;
    return q;
}

void
enqueue(queue_t *q, void *data){
    node_t *nd;
    pointer_t tail, next;
    nd = malloc(sizeof(node_t));
    nd->value = data;
    nd->next.ptr = NULL;
    while(1){
        barrier();
        tail = q->tail;
        next = tail.ptr->next;
        if(VAL(tail) == VAL(q->tail)){
            if(next.ptr == NULL){
                pointer_t tmp;
                tmp.ptr = nd;
                tmp.count = next.count + 1;
                if(CAS(&(tail.ptr->next), VAL(next), VAL(tmp))){
                    break;
                }
            }else{
                pointer_t tmp;
                tmp.ptr = next.ptr;
                tmp.count = tail.count + 1;
                CAS(&(q->tail), VAL(tail), VAL(tmp));
            }
        }
    }
    pointer_t tmp;
    tmp.ptr = nd;
    tmp.count = tail.count +1;
    CAS(&(q->tail), VAL(tail), VAL(tmp));
}

void * 
dequeue(queue_t *q){
    pointer_t head, tail, next;
    void *pvalue;
    while(1){
        barrier();
        head = q->head;
        tail = q->tail;
        next = head.ptr->next;
        if(VAL(head) == VAL(q->head)){
            if(head.ptr == tail.ptr){
                if(next.ptr == NULL){
                    return 0;
                }
                pointer_t tmp;
                tmp.ptr = next.ptr;
                tmp.count = tail.count + 1;
                CAS(&(q->tail), VAL(tail), VAL(tmp));
            }else{
                pvalue = next.ptr->value;
                pointer_t tmp;
                tmp.ptr = next.ptr;
                tmp.count = head.count + 1;

                if(CAS(&(q->head), VAL(head), VAL(tmp))){
                    break;
                }
            }
        }
    }
    //printf("val %p free %p\n", pvalue, head.ptr);
    
    free(head.ptr);
    return pvalue;
}

#define NTHREADS 100
#define Q_SIZE 100000

typedef struct dummy{
    char* name;
} dummy_t;


void *
thread_func(void *arg){
    int i;
    void * a;
    pthread_t t;
    queue_t *q;
	q = (queue_t *)arg;
	for(i = 0; i < Q_SIZE/NTHREADS; i++){
        //enqueue(q, i);
        a = dequeue(q);
        t = pthread_self();
        printf("dequeue id=%u, %d\n", t, (int)a);
        //enqueue(q, i);
    }
}


void
thread_test(void){
	int i, ret, err;
    queue_t *q;
    pthread_t tid[NTHREADS];

	struct timeval startTime, endTime;
	double elapsed;

	q = init_queue();

	gettimeofday(&startTime, NULL);
    
    for (i = 0; i < Q_SIZE; i++) {
        enqueue(q, (void *)i);
        //printf("enqueue %d\n", i);
    }

    for (i = 0; i < NTHREADS; i++) {
		ret = pthread_create(&tid[i], NULL, thread_func, (void *)q);
		if (ret != 0) {
			fprintf(stderr, "thread create err.\n");
			exit(-1);
		}
	}

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

	gettimeofday(&endTime, NULL);

	elapsed = (double)(endTime.tv_sec - startTime.tv_sec) +
                  (double)(endTime.tv_usec - startTime.tv_usec) / (double)1.0e6;

	printf("nthreads = %d: elapsed = %f sec\n", NTHREADS, elapsed);
	printf("done\n");
	
}


int
main(void){
    thread_test();
    return 0;
}