41 lines
1016 B
C
41 lines
1016 B
C
#include "queue.h"
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
queue_t* queue_init() {
|
|
queue_t* queue = calloc(1, sizeof(queue_t));
|
|
|
|
pthread_mutex_init(&queue->lock, NULL);
|
|
pthread_cond_init(&queue->trigger, NULL);
|
|
|
|
queue->len = 0;
|
|
queue->packet = calloc(QUEUE_MAX_LEN, sizeof(uint8_t *));
|
|
for (int i = 0; i < QUEUE_MAX_LEN; ++i) {
|
|
queue->packet[i] = malloc(PACKET_MAX_LEN);
|
|
}
|
|
queue->packet_len = calloc(QUEUE_MAX_LEN, sizeof(uint16_t));
|
|
queue->destination = malloc(QUEUE_MAX_LEN);
|
|
|
|
return queue;
|
|
}
|
|
|
|
int queue_add(queue_t* queue, uint8_t *packet, uint16_t packet_len, uint8_t destination) {
|
|
int rv = 0;
|
|
|
|
pthread_mutex_lock(&queue->lock);
|
|
|
|
if (queue->len < QUEUE_MAX_LEN) {
|
|
queue->packet_len[queue->len] = packet_len;
|
|
memcpy(queue->packet[queue->len], packet, packet_len);
|
|
queue->destination[queue->len] = destination;
|
|
++queue->len;
|
|
} else {
|
|
rv = 1;
|
|
}
|
|
|
|
pthread_mutex_unlock(&queue->lock);
|
|
|
|
return rv;
|
|
}
|