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.

251 lines
6.1KB

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