You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
67 lines
1.1 KiB
67 lines
1.1 KiB
/*
|
|
* queues.c
|
|
*
|
|
* Created on: Dec 6, 2017
|
|
* Author: maximilian
|
|
*/
|
|
|
|
#include "stdlib.h"
|
|
#include "stm32f10x.h"
|
|
#include "queues.h"
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
static inline void osWrapArroundInc(osQUEUE_t* q, uint8_t* i)
|
|
{
|
|
if( (*i) < q->qsize )
|
|
(*i)++;
|
|
else
|
|
(*i) = 0;
|
|
}
|
|
|
|
static inline uint8_t osQFull(osQUEUE_t* q)
|
|
{
|
|
uint8_t next = q->write_p;
|
|
osWrapArroundInc(q, &next);
|
|
return(next == q->read_p);
|
|
}
|
|
|
|
static inline uint8_t osQEmpty(osQUEUE_t* q)
|
|
{
|
|
return(q->write_p == q->read_p);
|
|
}
|
|
|
|
void osQInit(osQUEUE_t* q, size_t ivarsize, uint16_t iqsize, void* istart)
|
|
{
|
|
q->read_p = 0;
|
|
q->write_p = 0;
|
|
q->varsize = ivarsize;
|
|
q->qsize = iqsize;
|
|
q->start = istart;
|
|
}
|
|
|
|
uint8_t osEnqueue(osQUEUE_t* q, void* data)
|
|
{
|
|
if(osQFull(q))
|
|
return 0;
|
|
else
|
|
{
|
|
/* Copy queue element to where it belongs. */
|
|
memcpy( ( q->start + (q->write_p)*(q->varsize) ), data, q->varsize);
|
|
osWrapArroundInc(q, &(q->write_p) );
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
|
|
uint8_t osDequeue(osQUEUE_t* q, void* data)
|
|
{
|
|
if(osQEmpty(q))
|
|
return 0;
|
|
else
|
|
{
|
|
memcpy(data, ( q->start + (q->read_p)*(q->varsize) ), q->varsize);
|
|
osWrapArroundInc(q, &(q->read_p) );
|
|
}
|
|
return 1;
|
|
}
|
|
|