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

14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
14 years ago
  1. /***************************************************************************
  2. * Copyright (C) 01/2008 by Olaf Rempel *
  3. * razzor@kopf-tisch.de *
  4. * *
  5. * This program is free software; you can redistribute it and/or modify *
  6. * it under the terms of the GNU General Public License as published by *
  7. * the Free Software Foundation; version 2 of the License *
  8. * *
  9. * This program is distributed in the hope that it will be useful, *
  10. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  11. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  12. * GNU General Public License for more details. *
  13. * *
  14. * You should have received a copy of the GNU General Public License *
  15. * along with this program; if not, write to the *
  16. * Free Software Foundation, Inc., *
  17. * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
  18. ***************************************************************************/
  19. #include <stdio.h>
  20. #include "AT91SAM7S256.h"
  21. #include "atomic.h"
  22. #include "fifo.h"
  23. #include "memalloc.h"
  24. #define FIFO_MASK(x) ((x)->size -1)
  25. /*
  26. * get used bytes (under lock)
  27. * all other operations don't need locks:
  28. * - only fifo_put/fifo_rxpdc are allowed to increment fifo->in
  29. * - only fifo_get/fifo_txpdc are allowed to increment fifo->out
  30. * a integer overflow (4gb) of fifo->in / fifo->out could cause trouble
  31. */
  32. static uint32_t fifo_used(struct fifo *fifo)
  33. {
  34. disable_irqs();
  35. uint32_t used = fifo->in - fifo->out;
  36. restore_irqs();
  37. return used;
  38. }
  39. /*
  40. * append data to fifo
  41. * returns number of bytes copied
  42. */
  43. uint32_t fifo_put(struct fifo *fifo, const char *buf, uint32_t len)
  44. {
  45. uint32_t left = fifo->size - fifo_used(fifo);
  46. // TODO: check semantic
  47. if (len > left)
  48. len = left;
  49. uint32_t count = len;
  50. while (count--)
  51. fifo->buf[fifo->in++ & FIFO_MASK(fifo)] = *buf++;
  52. return len;
  53. }
  54. /*
  55. * get data from fifo
  56. * returns number of bytes copied
  57. */
  58. uint32_t fifo_get(struct fifo *fifo, char *buf, uint32_t len)
  59. {
  60. uint32_t used = fifo_used(fifo);
  61. if (len > used)
  62. len = used;
  63. uint32_t count = len;
  64. while (count--)
  65. *buf++ = fifo->buf[fifo->out++ & FIFO_MASK(fifo)];
  66. return len;
  67. }
  68. /*
  69. * returns a pointer to the data in the fifo
  70. * (without changing internal state)
  71. */
  72. char * fifo_peek(struct fifo *fifo, uint32_t len)
  73. {
  74. uint32_t used = fifo_used(fifo);
  75. if (len > used)
  76. return NULL;
  77. return fifo->buf + (fifo->out & FIFO_MASK(fifo));
  78. }
  79. /*
  80. * receive data via PDC and put it into fifo
  81. */
  82. uint32_t fifo_rxpdc(struct fifo *fifo, AT91S_PDC *pdc, uint16_t maxsize)
  83. {
  84. /* account previous PDC transfer */
  85. fifo->in += fifo->pdc_rx;
  86. uint32_t left = fifo->size - fifo_used(fifo);
  87. if (left) {
  88. /* calc pointer for next transfer */
  89. uint32_t first_idx = (fifo->in & FIFO_MASK(fifo));
  90. pdc->PDC_RPR = (uint32_t)(fifo->buf + first_idx);
  91. /* check for buffer end -> split transfer */
  92. if (first_idx + left <= (fifo->size -1)) {
  93. fifo->pdc_rx = left;
  94. } else {
  95. fifo->pdc_rx = fifo->size - first_idx;
  96. }
  97. /* split in maxsize chunks */
  98. if (maxsize && fifo->pdc_rx > maxsize)
  99. fifo->pdc_rx = maxsize;
  100. /* start transfer */
  101. pdc->PDC_RCR = fifo->pdc_rx;
  102. } else {
  103. /* no data in buffer */
  104. fifo->pdc_rx = 0;
  105. }
  106. return fifo->pdc_rx;
  107. }
  108. /*
  109. * transmit fifo via PDC
  110. * returns 0 if no transfer was started
  111. */
  112. uint32_t fifo_txpdc(struct fifo *fifo, AT91S_PDC *pdc, uint16_t maxsize)
  113. {
  114. /* account previous PDC transfer */
  115. fifo->out += fifo->pdc_tx;
  116. uint32_t used = fifo_used(fifo);
  117. if (used) {
  118. /* calc pointer for next transfer */
  119. uint32_t first_idx = (fifo->out & FIFO_MASK(fifo));
  120. pdc->PDC_TPR = (uint32_t)(fifo->buf + first_idx);
  121. /* check for buffer end -> split transfer */
  122. if (first_idx + used <= (fifo->size -1)) {
  123. fifo->pdc_tx = used;
  124. } else {
  125. fifo->pdc_tx = fifo->size - first_idx;
  126. }
  127. /* split in maxsize chunks */
  128. if (maxsize && fifo->pdc_tx > maxsize)
  129. fifo->pdc_tx = maxsize;
  130. /* start transfer */
  131. pdc->PDC_TCR = fifo->pdc_tx;
  132. } else {
  133. /* no data in buffer */
  134. fifo->pdc_tx = 0;
  135. }
  136. return fifo->pdc_tx;
  137. }
  138. /*
  139. * put one byte into the fifo
  140. * returns 0 if fifo was full
  141. */
  142. uint32_t fifo_putbyte(struct fifo *fifo, char c)
  143. {
  144. uint32_t left = fifo->size - fifo_used(fifo);
  145. if (left) {
  146. fifo->buf[fifo->in++ & FIFO_MASK(fifo)] = c;
  147. return 1;
  148. }
  149. return 0;
  150. }
  151. /*
  152. * gets one byte from fifo
  153. * returns 0 if fifo was empty
  154. */
  155. uint32_t fifo_getbyte(struct fifo *fifo, char *p)
  156. {
  157. uint32_t used = fifo_used(fifo);
  158. if (used) {
  159. *p = fifo->buf[fifo->out++ & FIFO_MASK(fifo)];
  160. return 1;
  161. }
  162. return 0;
  163. }
  164. /*
  165. * allocs a fifo from static_alloc space
  166. */
  167. struct fifo * fifo_alloc(uint32_t size)
  168. {
  169. size = next_powerof2(size);
  170. struct fifo *fifo = static_alloc(sizeof(struct fifo) + size);
  171. fifo->size = size;
  172. fifo->in = 0;
  173. fifo->out = 0;
  174. fifo->pdc_tx = 0;
  175. fifo->pdc_rx = 0;
  176. return fifo;
  177. }