FreeRTOS RW lock

rwlock.c

#include "rwlock.h"

#define RW_LOCK_STATE_NOT_WRITING (1UL << 0)

void rw_lock_init(rw_lock_t *rwlock) {
	rwlock->write_lock = xSemaphoreCreateBinaryStatic(&rwlock->write_lock_buffer);
	xSemaphoreGive(rwlock->write_lock);
	rwlock->reader_count_lock = xSemaphoreCreateMutexStatic(&rwlock->read_count_lock_buffer);
	rwlock->reader_count = 0;
	rwlock->state = xEventGroupCreateStatic(&rwlock->state_buffer);
	xEventGroupSetBits(rwlock->state, RW_LOCK_STATE_NOT_WRITING);
}

void rw_lock_acquire_write(rw_lock_t *rwlock) {
	xEventGroupClearBits(rwlock->state, RW_LOCK_STATE_NOT_WRITING);
	xSemaphoreTake(rwlock->write_lock, portMAX_DELAY);
}

void rw_lock_release_write(rw_lock_t *rwlock) {
	xSemaphoreGive(rwlock->write_lock);
	xEventGroupSetBits(rwlock->state, RW_LOCK_STATE_NOT_WRITING);
}

void rw_lock_acquire_read(rw_lock_t *rwlock) {
	xEventGroupWaitBits(rwlock->state, RW_LOCK_STATE_NOT_WRITING, pdFALSE, pdTRUE, portMAX_DELAY);
	xSemaphoreTake(rwlock->reader_count_lock, portMAX_DELAY);
	if (!rwlock->reader_count) {
		xSemaphoreTake(rwlock->write_lock, portMAX_DELAY);
	}
	rwlock->reader_count++;
	xSemaphoreGive(rwlock->reader_count_lock);
}

void rw_lock_release_read(rw_lock_t *rwlock) {
	xSemaphoreTake(rwlock->reader_count_lock, portMAX_DELAY);
	rwlock->reader_count--;
	if (!rwlock->reader_count) {
		xSemaphoreGive(rwlock->write_lock);
	}
	xSemaphoreGive(rwlock->reader_count_lock);
}

rwlock.h

#pragma once

#include <freertos/FreeRTOS.h>
#include <freertos/semphr.h>
#include <freertos/event_groups.h>

typedef struct rw_lock {
	StaticSemaphore_t write_lock_buffer;
	SemaphoreHandle_t write_lock;
	StaticSemaphore_t read_count_lock_buffer;
	SemaphoreHandle_t reader_count_lock;
	int reader_count;
	StaticEventGroup_t state_buffer;
	EventGroupHandle_t state;
} rw_lock_t;

void rw_lock_init(rw_lock_t *rwlock);
void rw_lock_acquire_write(rw_lock_t *rwlock);
void rw_lock_release_write(rw_lock_t *rwlock);
void rw_lock_acquire_read(rw_lock_t *rwlock);
void rw_lock_release_read(rw_lock_t *rwlock);