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.

339 lines
9.1KB

  1. /***************************************************************************
  2. * sam7fc - Telemetrie Handling *
  3. * *
  4. * Copyright (C) 02/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 <stdint.h>
  23. #include <string.h>
  24. #include "board.h" // ARRAY_SIZE()
  25. #include "at91_pitc.h"
  26. #include "telemetrie.h"
  27. #include "tdc_proto.h"
  28. #include "memalloc.h"
  29. #include "fifo.h"
  30. #define TDC_OWN_ADDRESS TDC_ADDR1
  31. /* extern symbols, defined in ldscript */
  32. extern struct tdc_value _tdc_value_table;
  33. extern struct tdc_value _tdc_value_table_end;
  34. /* max. 8x 32 = 256 variables */
  35. static uint32_t tdc_varmap[8];
  36. /* array of devices, that are used to reach address X */
  37. static struct comm_device *routing_table[8];
  38. /*
  39. * returns:
  40. * -1: on routing error
  41. * 0: no space left in txfifo (caller should retry)
  42. * >0: success
  43. */
  44. int32_t tdc_transmit(uint32_t addr, struct tdc_pkt_header *head)
  45. {
  46. if (addr >= ARRAY_SIZE(routing_table) || !routing_table[addr])
  47. return -1;
  48. int32_t retval = fifo_put(routing_table[addr]->txfifo, (char *)head, head->size);
  49. if (routing_table[addr]->trigger_tx)
  50. routing_table[addr]->trigger_tx();
  51. return retval;
  52. }
  53. static int32_t tdc_get_vars(void)
  54. {
  55. /* restart point */
  56. static uint32_t id;
  57. struct tdc_value *value = &_tdc_value_table + id;
  58. while (value < &_tdc_value_table_end) {
  59. uint32_t datalen = strlen(value->name);
  60. struct tdc_getvars_reply *reply = alloc(sizeof(struct tdc_getvars_reply) + datalen);
  61. reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVARS;
  62. reply->size = sizeof(struct tdc_getvars_reply) + datalen;
  63. reply->id = (id & 0xFF);
  64. reply->flags = value->flags;
  65. reply->name_len = datalen;
  66. memcpy(reply->name, value->name, datalen);
  67. uint32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
  68. free(reply);
  69. /* push routing error(-1) and retry(0) */
  70. if (ret <= 0)
  71. return ret;
  72. id++;
  73. value++;
  74. }
  75. /* dump complete, reset restart point */
  76. id = 0;
  77. return 1;
  78. }
  79. static int32_t tdc_get_value(uint32_t id)
  80. {
  81. struct tdc_value *value = &_tdc_value_table + id;
  82. if (value >= &_tdc_value_table_end)
  83. return -1;
  84. uint32_t datalen = value->flags & TDC_SIZEMASK;
  85. struct tdc_getvalue_reply *reply = alloc(sizeof(struct tdc_getvalue_reply) + datalen);
  86. reply->cmd = TDC_REPLY | TDC_ADDR1 | TDC_GETVALUE;
  87. reply->size = sizeof(struct tdc_getvalue_reply) + datalen;
  88. reply->id = id;
  89. memcpy(reply->data, value->data, datalen);
  90. int32_t ret = tdc_transmit(TDC_ADDR0, ((struct tdc_pkt_header *)reply));
  91. free(reply);
  92. return ret;
  93. }
  94. static int32_t tdc_set_value(uint32_t id, uint8_t *data, uint32_t data_size)
  95. {
  96. struct tdc_value *value = &_tdc_value_table + id;
  97. if (value >= &_tdc_value_table_end)
  98. return -1;
  99. uint32_t len = value->flags & TDC_SIZEMASK;
  100. if (len != data_size)
  101. return -1;
  102. // TODO: atomic?
  103. memcpy(value->data, data, len);
  104. return len;
  105. }
  106. static uint32_t tdc_timer_cb(struct pitc_timer *timer)
  107. {
  108. uint32_t i, j;
  109. for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++) {
  110. uint32_t bitmask = tdc_varmap[i];
  111. for (j = 0; j < 32; j++) {
  112. if (!bitmask)
  113. break;
  114. if (bitmask & 0x01) {
  115. if (tdc_get_value(i * 32 + j) < 0)
  116. tdc_varmap[i] &= ~(1 << j);
  117. }
  118. bitmask >>= 1;
  119. }
  120. }
  121. return PITC_RESTART_TIMER;
  122. }
  123. static struct pitc_timer tdc_timer = {
  124. .func = tdc_timer_cb,
  125. };
  126. static int32_t tdc_setup_timer(uint32_t interval, uint32_t *varmap)
  127. {
  128. memcpy(tdc_varmap, varmap, sizeof(tdc_varmap));
  129. uint32_t i;
  130. uint32_t tmp = 0;
  131. for (i = 0; i < ARRAY_SIZE(tdc_varmap); i++)
  132. tmp |= tdc_varmap[i];
  133. if ((interval > 0) && (tmp != 0)) {
  134. tdc_timer.interval = interval;
  135. pitc_schedule_timer(&tdc_timer);
  136. } else {
  137. pitc_remove_timer(&tdc_timer);
  138. }
  139. return 1;
  140. }
  141. static const struct tdc_hello_reply hello_reply = {
  142. .cmd = TDC_REPLY | TDC_OWN_ADDRESS | TDC_HELLO,
  143. .size = sizeof(struct tdc_hello_reply),
  144. .name = "sam7fc-v0.01",
  145. };
  146. void tdc_register_device(uint32_t addr, struct comm_device *device)
  147. {
  148. if (addr < ARRAY_SIZE(routing_table))
  149. routing_table[addr] = device;
  150. }
  151. struct tdc_pkt_header * tdc_alloc_fullpkt(struct comm_device *device, uint32_t size)
  152. {
  153. struct tdc_pkt_header *head = alloc(size);
  154. /* peek the whole packet */
  155. uint32_t len = fifo_peek(device->rxfifo, (char *)head, size);
  156. if (len != size) {
  157. free(head);
  158. head = NULL;
  159. }
  160. return head;
  161. }
  162. static int32_t tdc_receive(struct comm_device *device)
  163. {
  164. struct tdc_pkt_header tmp_head;
  165. struct tdc_pkt_header *head = &tmp_head;
  166. /* peek the header, return retry(0) if not enough bytes are available */
  167. uint32_t len = fifo_peek(device->rxfifo, (char *)head, sizeof(tmp_head));
  168. if (len != sizeof(tmp_head))
  169. return 0;
  170. /* assume an error, remove one byte from fifo */
  171. uint32_t used_bytes = 1;
  172. int32_t ret = -1;
  173. /* remember the device as path to the host */
  174. if ((head->cmd & (TDC_REPLY | TDC_OPCODEMASK)) == TDC_HELLO) {
  175. tdc_register_device(TDC_ADDR0, device);
  176. }
  177. /* reply packets / forward packets */
  178. if (head->cmd & TDC_REPLY || (head->cmd & TDC_ADDRMASK) != TDC_OWN_ADDRESS) {
  179. /* peek complete packet, return retry(0) if not enough bytes are available */
  180. head = tdc_alloc_fullpkt(device, head->size);
  181. if (head == NULL)
  182. return 0;
  183. /* reply packets go to ADDR0, forwards to others */
  184. uint32_t addr = (head->cmd & TDC_REPLY) ? TDC_ADDR0 : ((head->cmd & TDC_ADDRMASK) >> 4);
  185. used_bytes = head->size;
  186. ret = tdc_transmit(addr, head);
  187. } else {
  188. /* parse cmd */
  189. switch (head->cmd & TDC_OPCODEMASK) {
  190. case TDC_HELLO: {
  191. /* check packet size */
  192. struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
  193. if (pkt->size != sizeof(*pkt))
  194. break;
  195. /* send reply */
  196. ret = tdc_transmit(TDC_ADDR0, (struct tdc_pkt_header *)&hello_reply);
  197. used_bytes = pkt->size;
  198. } break;
  199. case TDC_GETVARS: {
  200. struct tdc_pkt_header *pkt = (struct tdc_pkt_header *)head;
  201. if (pkt->size != sizeof(*pkt))
  202. break;
  203. /* send reply */
  204. ret = tdc_get_vars();
  205. used_bytes = pkt->size;
  206. } break;
  207. case TDC_GETVALUE: {
  208. struct tdc_getvalue_request *pkt = (struct tdc_getvalue_request *)head;
  209. if (pkt->size != sizeof(*pkt))
  210. break;
  211. /* peek complete packet, return retry(0) if not enough bytes are available */
  212. head = tdc_alloc_fullpkt(device, head->size);
  213. if (head != NULL) {
  214. pkt = (struct tdc_getvalue_request *)head;
  215. ret = tdc_get_value(pkt->id);
  216. used_bytes = pkt->size;
  217. } else {
  218. ret = 0;
  219. }
  220. } break;
  221. case TDC_SETVALUE: {
  222. struct tdc_setvalue_request *pkt = (struct tdc_setvalue_request *)head;
  223. if (pkt->size < sizeof(*pkt) +1 || pkt->size > sizeof(*pkt) +8)
  224. break;
  225. /* peek complete packet, return retry(0) if not enough bytes are available */
  226. head = tdc_alloc_fullpkt(device, head->size);
  227. if (head != NULL) {
  228. pkt = (struct tdc_setvalue_request *)head;
  229. ret = tdc_set_value(pkt->id, pkt->data, pkt->size - sizeof(*pkt));
  230. used_bytes = pkt->size;
  231. } else {
  232. ret = 0;
  233. }
  234. } break;
  235. case TDC_REQVALUES: {
  236. struct tdc_reqvalues_request *pkt = (struct tdc_reqvalues_request *)head;
  237. if (pkt->size != sizeof(*pkt))
  238. break;
  239. /* peek complete packet, return retry(0) if not enough bytes are available */
  240. head = tdc_alloc_fullpkt(device, head->size);
  241. if (head != NULL) {
  242. pkt = (struct tdc_reqvalues_request *)head;
  243. ret = tdc_setup_timer(pkt->interval, pkt->varmap);
  244. used_bytes = pkt->size;
  245. } else {
  246. ret = 0;
  247. }
  248. } break;
  249. }
  250. }
  251. /* on success(>0) or routing error(-1) remove the packet */
  252. if (ret != 0) {
  253. /* remove bytes from fifo */
  254. fifo_remove(device->rxfifo, used_bytes);
  255. }
  256. /* free allocated memory */
  257. if (head != NULL && head != &tmp_head)
  258. free(head);
  259. return ret;
  260. }
  261. void tdc_check(void)
  262. {
  263. uint32_t i;
  264. for (i = 0; i < ARRAY_SIZE(routing_table); i++) {
  265. if (routing_table[i] != NULL) {
  266. tdc_receive(routing_table[i]);
  267. // TODO: handle retry
  268. }
  269. }
  270. }
  271. void tdc_init(void)
  272. {
  273. uint32_t count = &_tdc_value_table_end - &_tdc_value_table;
  274. printf("found %ld TDC variables\n\r", count);
  275. }