main.c
#include <math.h> #include <stdio.h> #include <string.h> #include <stdlib.h> #include <stdint.h> #include <fcntl.h> #include <unistd.h> #include <pthread.h> #include <sys/ioctl.h> #define QUEUE_SIZE 1024 typedef struct { int size; int read; int write; uint8_t *buffer; pthread_mutex_t lock; } queue_t; static queue_t queue = {0}; static void queue_init(queue_t *q, size_t s) { q->buffer = (uint8_t *)malloc(s); q->size = s; q->read = q->write = 0; pthread_mutex_init(&q->lock, NULL); } static void queue_destroy(queue_t *q) { if (q->buffer) { free(q->buffer); } pthread_mutex_destroy(&q->lock); } static int queue_size_for_read(queue_t *q) { if (q->read == q->write) { return 0; } else if(q->read < q->write) { return q->write - q->read; } return (QUEUE_SIZE - q->read) + q->write; } static int queue_size_for_write(queue_t *q) { if (q->write == q->read) { return QUEUE_SIZE; } else if (q->write < q->read) { return q->read - q->write; } return (QUEUE_SIZE - q->write) + q->read; } static int queue_put(queue_t *q, uint8_t *buffer, size_t size) { int r = 0, tmp = 0, avai = 0; pthread_mutex_lock(&q->lock); avai = queue_size_for_write(q); if (size > avai) { size = avai; } r = size; if (size > 0) { if ((q->write >= q->read) && ((q->write + size) > QUEUE_SIZE)) { tmp = QUEUE_SIZE - q->write; size-= tmp; memcpy(&q->buffer[q->write], buffer, tmp); memcpy(q->buffer, &buffer[tmp], size); q->write = size; } else { memcpy(&q->buffer[q->write], buffer, size); q->write += size; } } pthread_mutex_unlock(&q->lock); return r; } static size_t queue_get(queue_t *q, uint8_t *buffer, size_t max) { int r = 0, tmp = 0, avai = 0, size = max; pthread_mutex_lock(&q->lock); avai = queue_size_for_read(q); if (size > avai) { size = avai; } r = size; if (size > 0) { if ((q->read > q->write) && (q->read + size) > QUEUE_SIZE) { tmp = QUEUE_SIZE - q->read; size-= tmp; memcpy(buffer, &q->buffer[q->read], tmp); memcpy(&buffer[tmp], q->buffer, size); q->read = size; } else { memcpy(buffer, &q->buffer[q->read], size); q->read+= size; } } pthread_mutex_unlock(&q->lock); return r; } int main(int argc, char **argv) { char buf[32] = {0}; int ret = 0, cc = 0; queue_init(&queue, (size_t)QUEUE_SIZE); if (queue.buffer == NULL) { return -1; } memset(queue.buffer, 0, QUEUE_SIZE); buf[0] = 0x00; buf[1] = 0x11; queue_put(&queue, buf, 2); buf[0] = 0x22; buf[1] = 0x33; buf[2] = 0x44; queue_put(&queue, buf, 3); ret = queue_get(&queue, buf, sizeof(buf)); printf("ret: %d\n", ret); for (cc=0; cc<ret; cc++) { printf("0x%02x, ", buf[cc]); } printf("\n"); queue_destroy(&queue); return 0; }
編譯、執行
$ gcc main.c -o test $ ./test ret: 5 0x00, 0x11, 0x22, 0x33, 0x44,