ARM7 based quadrocopter
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.
 
 
 
 
 

207 lines
5.2 KiB

/***************************************************************************
* Copyright (C) 01/2008 by Olaf Rempel *
* razzor@kopf-tisch.de *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; version 2 of the License *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License for more details. *
* *
* You should have received a copy of the GNU General Public License *
* along with this program; if not, write to the *
* Free Software Foundation, Inc., *
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
***************************************************************************/
#include <stdio.h>
#include "AT91SAM7S256.h"
#include "atomic.h"
#include "fifo.h"
#include "memalloc.h"
#define FIFO_MASK(x) ((x)->size -1)
/*
* get used bytes (under lock)
* all other operations don't need locks:
* - only fifo_put/fifo_rxpdc are allowed to increment fifo->in
* - only fifo_get/fifo_txpdc are allowed to increment fifo->out
* a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
*/
static uint32_t fifo_used(struct fifo *fifo)
{
disable_irqs();
uint32_t used = fifo->in - fifo->out;
restore_irqs();
return used;
}
/*
* append data to fifo
* returns number of bytes copied
*/
uint32_t fifo_put(struct fifo *fifo, const char *buf, uint32_t len)
{
uint32_t left = fifo->size - fifo_used(fifo);
// TODO: check semantic
if (len > left)
len = left;
uint32_t count = len;
while (count--)
fifo->buf[fifo->in++ & FIFO_MASK(fifo)] = *buf++;
return len;
}
/*
* get data from fifo
* returns number of bytes copied
*/
uint32_t fifo_get(struct fifo *fifo, char *buf, uint32_t len)
{
uint32_t used = fifo_used(fifo);
if (len > used)
len = used;
uint32_t count = len;
while (count--)
*buf++ = fifo->buf[fifo->out++ & FIFO_MASK(fifo)];
return len;
}
/*
* returns a pointer to the data in the fifo
* (without changing internal state)
*/
char * fifo_peek(struct fifo *fifo, uint32_t len)
{
uint32_t used = fifo_used(fifo);
if (len > used)
return NULL;
return fifo->buf + (fifo->out & FIFO_MASK(fifo));
}
/*
* receive data via PDC and put it into fifo
*/
uint32_t fifo_rxpdc(struct fifo *fifo, AT91S_PDC *pdc, uint16_t maxsize)
{
/* account previous PDC transfer */
fifo->in += fifo->pdc_rx;
uint32_t left = fifo->size - fifo_used(fifo);
if (left) {
/* calc pointer for next transfer */
uint32_t first_idx = (fifo->in & FIFO_MASK(fifo));
pdc->PDC_RPR = (uint32_t)(fifo->buf + first_idx);
/* check for buffer end -> split transfer */
if (first_idx + left <= (fifo->size -1)) {
fifo->pdc_rx = left;
} else {
fifo->pdc_rx = fifo->size - first_idx;
}
/* split in maxsize chunks */
if (maxsize && fifo->pdc_rx > maxsize)
fifo->pdc_rx = maxsize;
/* start transfer */
pdc->PDC_RCR = fifo->pdc_rx;
} else {
/* no data in buffer */
fifo->pdc_rx = 0;
}
return fifo->pdc_rx;
}
/*
* transmit fifo via PDC
* returns 0 if no transfer was started
*/
uint32_t fifo_txpdc(struct fifo *fifo, AT91S_PDC *pdc, uint16_t maxsize)
{
/* account previous PDC transfer */
fifo->out += fifo->pdc_tx;
uint32_t used = fifo_used(fifo);
if (used) {
/* calc pointer for next transfer */
uint32_t first_idx = (fifo->out & FIFO_MASK(fifo));
pdc->PDC_TPR = (uint32_t)(fifo->buf + first_idx);
/* check for buffer end -> split transfer */
if (first_idx + used <= (fifo->size -1)) {
fifo->pdc_tx = used;
} else {
fifo->pdc_tx = fifo->size - first_idx;
}
/* split in maxsize chunks */
if (maxsize && fifo->pdc_tx > maxsize)
fifo->pdc_tx = maxsize;
/* start transfer */
pdc->PDC_TCR = fifo->pdc_tx;
} else {
/* no data in buffer */
fifo->pdc_tx = 0;
}
return fifo->pdc_tx;
}
/*
* put one byte into the fifo
* returns 0 if fifo was full
*/
uint32_t fifo_putbyte(struct fifo *fifo, char c)
{
uint32_t left = fifo->size - fifo_used(fifo);
if (left) {
fifo->buf[fifo->in++ & FIFO_MASK(fifo)] = c;
return 1;
}
return 0;
}
/*
* gets one byte from fifo
* returns 0 if fifo was empty
*/
uint32_t fifo_getbyte(struct fifo *fifo, char *p)
{
uint32_t used = fifo_used(fifo);
if (used) {
*p = fifo->buf[fifo->out++ & FIFO_MASK(fifo)];
return 1;
}
return 0;
}
/*
* allocs a fifo from static_alloc space
*/
struct fifo * fifo_alloc(uint32_t size)
{
size = next_powerof2(size);
struct fifo *fifo = static_alloc(sizeof(struct fifo) + size);
fifo->size = size;
fifo->in = 0;
fifo->out = 0;
fifo->pdc_tx = 0;
fifo->pdc_rx = 0;
return fifo;
}