int csp_ping(uint8_t node, uint32_t timeout, unsigned int size, uint8_t conn_options) {
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)
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_server(void *p) {
(void) p;
/* Create socket without any socket options */
csp_socket_t *sock = csp_socket(CSP_SO_NONE);
/* Bind all ports to socket */
csp_bind(sock, CSP_ANY);
/* Create 10 connections backlog queue */
csp_listen(sock, 10);
/* Pointer to current connection and packet */
csp_conn_t *conn;
csp_packet_t *packet;
/* Process incoming connections */
while (1) {
/* Wait for connection, 10000 ms timeout */
if ((conn = csp_accept(sock, 10000)) == NULL)
continue;
/* Read packets. Timout is 100 ms */
while ((packet = csp_read(conn, 100)) != NULL) {
switch (csp_conn_dport(conn)) {
case MY_PORT:
/* Process packet here */
blink(K_LED_GREEN);
csp_buffer_free(packet);
break;
default:
/* Let the service handler reply pings, buffer use, etc. */
#ifdef TARGET_LIKE_MSP430
blink(K_LED_GREEN);
blink(K_LED_GREEN);
#else
blink(K_LED_BLUE);
#endif
csp_service_handler(conn, packet);
break;
}
}
/* Close current connection, and handle next */
csp_close(conn);
}
}
int csp_sfp_recv(csp_conn_t * conn, void ** dataout, int * datasize, uint32_t timeout) {
unsigned int last_byte = 0;
csp_packet_t * packet;
while((packet = csp_read(conn, timeout)) != NULL) {
/* Check that SFP header is present */
if ((packet->id.flags & CSP_FFRAG) == 0) {
csp_debug(CSP_ERROR, "Missing SFP header\r\n");
return -1;
}
/* Read SFP header */
sfp_header_t * sfp_header = csp_sfp_header_remove(packet);
sfp_header->offset = csp_ntoh32(sfp_header->offset);
sfp_header->totalsize = csp_ntoh32(sfp_header->totalsize);
csp_debug(CSP_PROTOCOL, "SFP fragment %u/%u\r\n", sfp_header->offset + packet->length, sfp_header->totalsize);
if (sfp_header->offset > last_byte + 1) {
csp_debug(CSP_ERROR, "SFP missing %u bytes\r\n", sfp_header->offset - last_byte);
csp_buffer_free(packet);
return -1;
} else {
last_byte = sfp_header->offset + packet->length;
}
/* Allocate memory */
if (*dataout == NULL)
*dataout = csp_malloc(sfp_header->totalsize);
*datasize = sfp_header->totalsize;
/* Copy data to output */
if (*dataout != NULL)
memcpy(*dataout + sfp_header->offset, packet->data, packet->length);
if (sfp_header->offset + packet->length >= sfp_header->totalsize) {
csp_debug(CSP_PROTOCOL, "SFP complete\r\n");
csp_buffer_free(packet);
return 0;
} else {
csp_buffer_free(packet);
}
}
return -1;
}
ssize_t csp_read_exact(int fd, void *buf, size_t count)
{
ssize_t total = 0;
do {
ssize_t n = csp_read(fd, buf, count);
if (n < 0 && errno != EINTR)
return n;
count -= n;
total += n;
} while (count);
return total;
}
unsigned WINAPI task_server(void * parameters) {
/* Create socket without any socket options */
csp_socket_t *sock = csp_socket(CSP_SO_NONE);
/* Bind all ports to socket */
csp_bind(sock, CSP_ANY);
/* Create 10 connections backlog queue */
csp_listen(sock, 10);
/* Pointer to current connection and packet */
csp_conn_t *conn;
csp_packet_t *packet;
/* Process incoming connections */
while (1) {
/* Wait for connection, 10000 ms timeout */
if ((conn = csp_accept(sock, 10000)) == NULL)
continue;
/* Read packets. Timout is 100 ms */
while ((packet = csp_read(conn, 100)) != NULL) {
switch (csp_conn_dport(conn)) {
case MY_PORT:
/* Process packet here */
printf("Packet received on MY_PORT: %s\r\n", (char *) packet->data);
csp_buffer_free(packet);
default:
/* Let the service handler reply pings, buffer use, etc. */
csp_service_handler(conn, packet);
break;
}
}
/* Close current connection, and handle next */
csp_close(conn);
}
return 0;
}
/**
* Use an existing connection to perform a transaction,
* This is only possible if the next packet is on the same port and destination!
* @param conn pointer to connection structure
* @param timeout timeout in ms
* @param outbuf pointer to outgoing data buffer
* @param outlen length of request to send
* @param inbuf pointer to incoming data buffer
* @param inlen length of expected reply, -1 for unknown size (note inbuf MUST be large enough)
* @return
*/
int csp_transaction_persistent(csp_conn_t * conn, unsigned int timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
/* Stupid way to implement max() but more portable than macros */
int size = outlen;
if (inlen > outlen)
size = inlen;
csp_packet_t * packet = csp_buffer_get(size);
if (packet == NULL)
return 0;
/* Copy the request */
if (outlen > 0 && outbuf != NULL)
memcpy(packet->data, outbuf, outlen);
packet->length = outlen;
if (!csp_send(conn, packet, timeout)) {
printf("Send failed\r\n");
csp_buffer_free(packet);
return 0;
}
/* If no reply is expected, return now */
if (inlen == 0)
return 1;
packet = csp_read(conn, timeout);
if (packet == NULL) {
printf("Read failed\r\n");
return 0;
}
if ((inlen != -1) && (packet->length != inlen)) {
printf("Reply length %u expected %u\r\n", packet->length, inlen);
csp_buffer_free(packet);
return 0;
}
memcpy(inbuf, packet->data, packet->length);
int length = packet->length;
csp_buffer_free(packet);
return length;
}
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: ", node);
/* Try to send frame */
if (!csp_send(conn, packet, 0))
goto out;
/* Read incoming frame */
packet = csp_read(conn, timeout);
if (packet == NULL) {
printf(" Timeout!\r\n");
goto out;
}
/* We have a reply */
printf("PS Length %u\r\n", packet->length);
printf("%s\r\n", packet->data);
/* Clean up */
out:
if (packet != NULL)
csp_buffer_free(packet);
csp_close(conn);
}
int csp_transaction_persistent(csp_conn_t * conn, uint32_t timeout, void * outbuf, int outlen, void * inbuf, int inlen) {
int size = (inlen > outlen) ? inlen : outlen;
csp_packet_t * packet = csp_buffer_get(size);
if (packet == NULL)
return 0;
/* Copy the request */
if (outlen > 0 && outbuf != NULL)
memcpy(packet->data, outbuf, outlen);
packet->length = outlen;
if (!csp_send(conn, packet, timeout)) {
csp_buffer_free(packet);
return 0;
}
/* If no reply is expected, return now */
if (inlen == 0)
{
return 1;
}
packet = csp_read(conn, timeout);
if (packet == NULL)
return 0;
if ((inlen != -1) && ((int)packet->length != inlen)) {
csp_log_error("Reply length %u expected %u\r\n", packet->length, inlen);
csp_buffer_free(packet);
return 0;
}
memcpy(inbuf, packet->data, packet->length);
int length = packet->length;
csp_buffer_free(packet);
return length;
}
int main(int argc, char *argv[]) {
int me, other, type;
char *message = "Testing CSP";
csp_socket_t *sock = NULL;
csp_conn_t *conn = NULL;
csp_packet_t *packet = NULL;
/* Run as either server or client */
if (argc != 2) {
printf("usage: server <server/client>\r\n");
return -1;
}
/* Set type */
if (strcmp(argv[1], "server") == 0) {
me = 1;
other = 2;
type = TYPE_SERVER;
} else if (strcmp(argv[1], "client") == 0) {
me = 2;
other = 1;
type = TYPE_CLIENT;
} else {
printf("Invalid type. Must be either 'server' or 'client'\r\n");
return -1;
}
/* Init CSP and CSP buffer system */
if (csp_init(me) != CSP_ERR_NONE || csp_buffer_init(10, 300) != CSP_ERR_NONE) {
printf("Failed to init CSP\r\n");
return -1;
}
if( type == TYPE_SERVER ) {
_beginthreadex(NULL, 0, pipe_listener, NULL, 0, 0);
} else {
pipe = CreateFile(
pipeName,
GENERIC_READ | GENERIC_WRITE,
0,
NULL,
OPEN_EXISTING,
0,
NULL);
if( pipe == INVALID_HANDLE_VALUE ) {
printError();
return -1;
}
}
/* Set default route and start router */
csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC);
csp_route_start_task(0, 0);
/* Create socket and listen for incoming connections */
if (type == TYPE_SERVER) {
sock = csp_socket(CSP_SO_NONE);
csp_bind(sock, PORT);
csp_listen(sock, 5);
}
/* Super loop */
while (1) {
if (type == TYPE_SERVER) {
/* Process incoming packet */
conn = csp_accept(sock, 1000);
if (conn) {
packet = csp_read(conn, 0);
if (packet)
printf("Received: %s\r\n", packet->data);
csp_buffer_free(packet);
csp_close(conn);
}
} else {
/* Send a new packet */
packet = csp_buffer_get(strlen(message));
if (packet) {
strcpy((char *) packet->data, message);
packet->length = strlen(message);
conn = csp_connect(CSP_PRIO_NORM, other, PORT, 1000, CSP_O_NONE);
printf("Sending: %s\r\n", message);
if (!conn || !csp_send(conn, packet, 1000))
return -1;
csp_close(conn);
Sleep(1000);
}
}
}
return 0;
}
int main(int argc, char **argv) {
int me, other, type;
char *message = "Testing CSP", *rx_channel_name, *tx_channel_name;
csp_socket_t *sock;
csp_conn_t *conn;
csp_packet_t *packet;
/* Run as either server or client */
if (argc != 2) {
printf("usage: %s <server/client>\r\n", argv[0]);
return -1;
}
/* Set type */
if (strcmp(argv[1], "server") == 0) {
me = 1;
other = 2;
tx_channel_name = "server_to_client";
rx_channel_name = "client_to_server";
type = TYPE_SERVER;
} else if (strcmp(argv[1], "client") == 0) {
me = 2;
other = 1;
tx_channel_name = "client_to_server";
rx_channel_name = "server_to_client";
type = TYPE_CLIENT;
} else {
printf("Invalid type. Must be either 'server' or 'client'\r\n");
return -1;
}
/* Init CSP and CSP buffer system */
if (csp_init(me) != CSP_ERR_NONE || csp_buffer_init(10, 300) != CSP_ERR_NONE) {
printf("Failed to init CSP\r\n");
return -1;
}
tx_channel = open(tx_channel_name, O_RDWR);
if (tx_channel < 0) {
printf("Failed to open TX channel\r\n");
return -1;
}
rx_channel = open(rx_channel_name, O_RDWR);
if (rx_channel < 0) {
printf("Failed to open RX channel\r\n");
return -1;
}
/* Start fifo RX task */
pthread_create(&rx_thread, NULL, fifo_rx, NULL);
/* Set default route and start router */
csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC);
csp_route_start_task(0, 0);
/* Create socket and listen for incoming connections */
if (type == TYPE_SERVER) {
sock = csp_socket(CSP_SO_NONE);
csp_bind(sock, PORT);
csp_listen(sock, 5);
}
/* Super loop */
while (1) {
if (type == TYPE_SERVER) {
/* Process incoming packet */
conn = csp_accept(sock, 1000);
if (conn) {
packet = csp_read(conn, 0);
if (packet)
printf("Received: %s\r\n", packet->data);
csp_buffer_free(packet);
csp_close(conn);
}
} else {
/* Send a new packet */
packet = csp_buffer_get(strlen(message));
if (packet) {
strcpy((char *) packet->data, message);
packet->length = strlen(message);
conn = csp_connect(CSP_PRIO_NORM, other, PORT, 1000, CSP_O_NONE);
printf("Sending: %s\r\n", message);
if (!conn || !csp_send(conn, packet, 1000))
return -1;
csp_close(conn);
}
sleep(1);
}
}
close(rx_channel);
close(tx_channel);
return 0;
}
