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; }