eive-obsw/thirdparty/libcsp/src/csp_services.c
2021-03-04 18:29:28 +01:00

234 lines
5.3 KiB
C

/*
Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library 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
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <inttypes.h>
/* CSP includes */
#include <csp/csp.h>
#include <csp/csp_cmp.h>
#include <csp/csp_endian.h>
#include <csp/arch/csp_time.h>
int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) {
unsigned int i;
uint32_t start, time, status = 0;
/* Counter */
start = csp_get_ms();
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, timeout, conn_options);
if (conn == NULL)
return -1;
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(size);
if (packet == NULL)
goto out;
/* Set data to increasing numbers */
packet->length = size;
for (i = 0; i < size; i++)
packet->data[i] = i;
/* Try to send frame */
if (!csp_send(conn, packet, 0))
goto out;
/* Read incoming frame */
packet = csp_read(conn, timeout);
if (packet == NULL)
goto out;
/* Ensure that the data was actually echoed */
for (i = 0; i < size; i++)
if (packet->data[i] != i % (0xff + 1))
goto out;
status = 1;
out:
/* Clean up */
if (packet != NULL)
csp_buffer_free(packet);
csp_close(conn);
/* We have a reply */
time = (csp_get_ms() - start);
if (status) {
return time;
} else {
return -1;
}
}
void csp_ping_noreply(uint8_t node) {
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(1);
/* Check malloc */
if (packet == NULL)
return;
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PING, 0, 0);
if (conn == NULL) {
csp_buffer_free(packet);
return;
}
packet->data[0] = 0x55;
packet->length = 1;
printf("Ping ignore reply node %u.\r\n", (unsigned int) node);
/* Try to send frame */
if (!csp_send(conn, packet, 0))
csp_buffer_free(packet);
csp_close(conn);
}
void csp_reboot(uint8_t node) {
uint32_t magic_word = csp_hton32(CSP_REBOOT_MAGIC);
csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0);
}
void csp_shutdown(uint8_t node) {
uint32_t magic_word = csp_hton32(CSP_REBOOT_SHUTDOWN_MAGIC);
csp_transaction(CSP_PRIO_NORM, node, CSP_REBOOT, 0, &magic_word, sizeof(magic_word), NULL, 0);
}
void csp_ps(uint8_t node, uint32_t timeout) {
/* Open connection */
csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, node, CSP_PS, 0, 0);
if (conn == NULL)
return;
/* Prepare data */
csp_packet_t * packet;
packet = csp_buffer_get(95);
/* Check malloc */
if (packet == NULL)
goto out;
packet->data[0] = 0x55;
packet->length = 1;
printf("PS node %u: \r\n", (unsigned int) node);
/* Try to send frame */
if (!csp_send(conn, packet, 0))
goto out;
while(1) {
/* Read incoming frame */
packet = csp_read(conn, timeout);
if (packet == NULL)
break;
/* We have a reply, add our own NULL char */
packet->data[packet->length] = 0;
printf("%s", packet->data);
/* Each packet from csp_read must to be freed by user */
csp_buffer_free(packet);
}
printf("\r\n");
/* Clean up */
out:
if (packet != NULL)
csp_buffer_free(packet);
csp_close(conn);
}
void csp_memfree(uint8_t node, uint32_t timeout) {
uint32_t memfree;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_MEMFREE, timeout, NULL, 0, &memfree, sizeof(memfree));
if (status == 0) {
printf("Network error\r\n");
return;
}
/* Convert from network to host order */
memfree = csp_ntoh32(memfree);
printf("Free Memory at node %u is %"PRIu32" bytes\r\n", (unsigned int) node, memfree);
}
void csp_buf_free(uint8_t node, uint32_t timeout) {
uint32_t size = 0;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_BUF_FREE, timeout, NULL, 0, &size, sizeof(size));
if (status == 0) {
printf("Network error\r\n");
return;
}
size = csp_ntoh32(size);
printf("Free buffers at node %u is %"PRIu32"\r\n", (unsigned int) node, size);
}
void csp_uptime(uint8_t node, uint32_t timeout) {
uint32_t uptime = 0;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_UPTIME, timeout, NULL, 0, &uptime, sizeof(uptime));
if (status == 0) {
printf("Network error\r\n");
return;
}
uptime = csp_ntoh32(uptime);
printf("Uptime of node %u is %"PRIu32" s\r\n", (unsigned int) node, uptime);
}
int csp_cmp(uint8_t node, uint32_t timeout, uint8_t code, int membsize, struct csp_cmp_message * msg) {
msg->type = CSP_CMP_REQUEST;
msg->code = code;
int status = csp_transaction(CSP_PRIO_NORM, node, CSP_CMP, timeout, msg, membsize, msg, membsize);
if (status == 0)
return CSP_ERR_TIMEDOUT;
return CSP_ERR_NONE;
}