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", node); /* Try to send frame */ if (!csp_send(conn, packet, 0)) csp_buffer_free(packet); csp_close(conn); }
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 gps_rx_cb (void * arg, uint8_t *buf, int len, void *pxTaskWoken) { gps_cfg_t *gps_cfg = (gps_cfg_t *) arg; DEBUG("GPS_RX: %c\n", *buf); if (*buf == '\n' || *buf == '\r') { gps_fix_t *fix = gps_incr_fix(); int len = gps_buf_cur + 1; int result = nmea_parse(gps_buf, len, fix); memset(gps_buf, '\0', len); gps_buf_cur = 0; if (result != NMEA_OK) { DEBUG("error parsing NMEA: %d\n", result); return; } // Send location fix if (NULL != gps_cfg->conn) { DEBUG("Parsed NMEA sentence, sending message\n"); DEBUG("NMEA %02d:%02d:%02d.%03d\n", fix->hour, fix->minute, fix->seconds, fix->milliseconds); csp_packet_t * msg; msg = csp_buffer_get(sizeof(gps_fix_t)); memcpy(msg->data, fix, sizeof(gps_fix_t)); //*((gps_fix_t*)msg->data) = *fix; msg->length = sizeof(gps_fix_t); if (!csp_send(gps_cfg->conn, msg, 1000)) { DEBUG("csp_send failed\n"); csp_buffer_free(msg); } } return; } gps_buf[gps_buf_cur++] = *buf; DEBUG("GPS_BUF: %.*s\n", gps_buf_cur, gps_buf); if (gps_buf_cur > GPS_BUFSIZE - 1) { DEBUG("Too much data for GPS buffer, discarding"); gps_buf_cur = 0; } }
int csp_sfp_send(csp_conn_t * conn, void * data, int totalsize, int mtu, uint32_t timeout) { int count = 0; while(count < totalsize) { /* Allocate packet */ csp_packet_t * packet = csp_buffer_get(mtu); if (packet == NULL) return -1; /* Calculate sending size */ int size = totalsize - count; if (size > mtu) size = mtu; /* Print debug */ csp_debug(CSP_PROTOCOL, "Sending SFP at %x size %u\r\n", data + count, size); /* Copy data */ memcpy(packet->data, data + count, size); packet->length = size; /* Set fragment flag */ conn->idout.flags |= CSP_FFRAG; /* Add SFP header */ sfp_header_t * sfp_header = csp_sfp_header_add(packet); sfp_header->totalsize = csp_hton32(totalsize); sfp_header->offset = csp_hton32(count); /* Send data */ if (!csp_send(conn, packet, timeout)) { csp_buffer_free(packet); return -1; } /* Increment count */ count += size; } 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 sendpacket() { csp_packet_t *packet; csp_conn_t *conn; packet = csp_buffer_get(10); if (packet == NULL) { printf("failed to get packet in sendpacket()\n"); return CSP_TASK_RETURN; } conn = csp_connect(CSP_PRIO_NORM, MY_ADDRESS, MY_PORT, 1000, CSP_O_NONE); if (conn == NULL) { printf("Connection failed in sendpacket()\n"); return CSP_TASK_RETURN; } char *msg = "42!"; strcpy((char *) packet->data, msg); packet->length = strlen(msg); if (!csp_send(conn, packet, 1000)) { printf("Sending packet from sendpacket() failed\n"); csp_buffer_free(packet); } csp_close(conn); return; }
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) { switch (csp_conn_dport(conn)) { case CSP_CMP: /* Pass to CMP handler */ if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) { csp_buffer_free(packet); return; } break; case CSP_PING: /* A ping means, just echo the packet, so no changes */ csp_log_info("SERVICE: Ping received\r\n"); break; case CSP_PS: { csp_sys_tasklist((char *)packet->data); packet->length = strlen((char *)packet->data); packet->data[packet->length] = '\0'; packet->length++; break; } case CSP_MEMFREE: { uint32_t total = csp_sys_memfree(); total = csp_hton32(total); memcpy(packet->data, &total, sizeof(total)); packet->length = sizeof(total); break; } case CSP_REBOOT: { uint32_t magic_word; memcpy(&magic_word, packet->data, sizeof(magic_word)); magic_word = csp_ntoh32(magic_word); /* If the magic word is invalid, return */ if (magic_word != 0x80078007) { csp_buffer_free(packet); return; } /* Otherwise Reboot */ csp_sys_reboot(); csp_buffer_free(packet); return; } case CSP_BUF_FREE: { uint32_t size = csp_buffer_remaining(); size = csp_hton32(size); memcpy(packet->data, &size, sizeof(size)); packet->length = sizeof(size); break; } case CSP_UPTIME: { uint32_t time = csp_get_s(); time = csp_hton32(time); memcpy(packet->data, &time, sizeof(time)); packet->length = sizeof(time); break; } /** @brief Returns route information * If no data received, sends back number of routes that exist, * otherwise returns up to nodes_requested, starting at index parameter * First parameter is index of table to start at, and * second parameter is number of indeces to return */ case CSP_GET_ROUTE: { //if there is no data, return number of routes that exist if(packet->length == 0) { uint8_t routes_left = csp_num_routes_left(0); packet->length = sizeof(routes_left); memcpy(&packet->data[0], &routes_left, sizeof(routes_left)); break; } //otherwise, return node information csp_route_t * route; uint8_t start_index,i, j, nodes_requested, routes_left; memcpy(&start_index, &packet->data[0], sizeof(start_index)); memcpy(&nodes_requested, &packet->data[1], sizeof(nodes_requested)); csp_route_info return_info[nodes_requested]; memset(return_info, 0x00, sizeof(return_info)); //don't attempt to read beyond size of routes array if(start_index >= CSP_ROUTE_COUNT){ csp_buffer_free(packet); printf("CSP_GET_ROUTE: won't read past array\r\n"); return; } i = start_index; j = 0; //populated return_info while j index less than number //of nodes requested and our i index is less than CSP_ROUTE_COUNT while((j < nodes_requested) && (i < CSP_ROUTE_COUNT)){ route = csp_route_struct(i); if(route != NULL){ return_info[j].node = i; strncpy(&return_info[j].name_buffer[0],route->interface->name, (sizeof(return_info[j].name_buffer)-1)); return_info[j].nexthop_mac_addr = route->nexthop_mac_addr; if(i != CSP_DEFAULT_ROUTE){ return_info[j].nexthop_mac_addr = route->nexthop_mac_addr == CSP_NODE_MAC ?(i) : route->nexthop_mac_addr; } else return_info[j].nexthop_mac_addr = route->nexthop_mac_addr; j++; } i++; } routes_left = csp_num_routes_left(i); //return data contains number of remaining nodes and node info packet->length = sizeof(return_info)+sizeof(routes_left); memcpy(&packet->data[0], &routes_left,sizeof(routes_left)); memcpy(&packet->data[1], return_info, sizeof(return_info)); break; } default: csp_buffer_free(packet); return; } if (!csp_send(conn, packet, 0)) csp_buffer_free(packet); }
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) { switch (csp_conn_dport(conn)) { case CSP_CMP: /* Pass to CMP handler */ if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) { csp_buffer_free(packet); return; } break; case CSP_PING: /* A ping means, just echo the packet, so no changes */ csp_log_info("SERVICE: Ping received"); break; case CSP_PS: { /* Sanity check on request */ if ((packet->length != 1) || (packet->data[0] != 0x55)) { /* Sanity check failed */ csp_buffer_free(packet); /* Clear the packet, it has been freed */ packet = NULL; break; } /* Start by allocating just the right amount of memory */ int task_list_size = csp_sys_tasklist_size(); char * pslist = csp_malloc(task_list_size); /* Check for malloc fail */ if (pslist == NULL) { /* Send out the data */ strcpy((char *)packet->data, "Not enough memory"); packet->length = strlen((char *)packet->data); /* Break and let the default handling send packet */ break; } /* Retrieve the tasklist */ csp_sys_tasklist(pslist); int pslen = strnlen(pslist, task_list_size); /* Split the potentially very long string into packets */ int i = 0; while(i < pslen) { /* Allocate packet buffer, if need be */ if (packet == NULL) packet = csp_buffer_get(CSP_RPS_MTU); if (packet == NULL) break; /* Calculate length, either full MTU or the remainder */ packet->length = (pslen - i > CSP_RPS_MTU) ? CSP_RPS_MTU : (pslen - i); /* Send out the data */ memcpy(packet->data, &pslist[i], packet->length); i += packet->length; if (!csp_send(conn, packet, 0)) csp_buffer_free(packet); /* Clear the packet reference when sent */ packet = NULL; } csp_free(pslist); break; } case CSP_MEMFREE: { uint32_t total = csp_sys_memfree(); total = csp_hton32(total); memcpy(packet->data, &total, sizeof(total)); packet->length = sizeof(total); break; } case CSP_REBOOT: { uint32_t magic_word; memcpy(&magic_word, packet->data, sizeof(magic_word)); magic_word = csp_ntoh32(magic_word); /* If the magic word is valid, reboot */ if (magic_word == CSP_REBOOT_MAGIC) { csp_sys_reboot(); } else if (magic_word == CSP_REBOOT_SHUTDOWN_MAGIC) { csp_sys_shutdown(); } csp_buffer_free(packet); return; } case CSP_BUF_FREE: { uint32_t size = csp_buffer_remaining(); size = csp_hton32(size); memcpy(packet->data, &size, sizeof(size)); packet->length = sizeof(size); break; } case CSP_UPTIME: { uint32_t time = csp_get_s(); time = csp_hton32(time); memcpy(packet->data, &time, sizeof(time)); packet->length = sizeof(time); break; } default: csp_buffer_free(packet); return; } if (packet != NULL) { if (!csp_send(conn, packet, 0)) csp_buffer_free(packet); } }
unsigned WINAPI task_client(void * parameters) { csp_packet_t * packet; csp_conn_t * conn; while (1) { /** * Try ping */ Sleep(1000); int result = csp_ping(MY_ADDRESS, 100, 100, CSP_O_NONE); printf("Ping result %d [ms]\r\n", result); Sleep(1000); /** * Try data packet to server */ /* Get packet buffer for data */ packet = csp_buffer_get(100); if (packet == NULL) { /* Could not get buffer element */ printf("Failed to get buffer element\n"); return 0; } /* Connect to host HOST, port PORT with regular UDP-like protocol and 1000 ms timeout */ conn = csp_connect(CSP_PRIO_NORM, MY_ADDRESS, MY_PORT, 1000, CSP_O_NONE); if (conn == NULL) { /* Connect failed */ printf("Connection failed\n"); /* Remember to free packet buffer */ csp_buffer_free(packet); return 0; } /* Copy dummy data to packet */ char *msg = "Hello World"; strcpy((char *) packet->data, msg); /* Set packet length */ packet->length = strlen(msg); /* Send packet */ if (!csp_send(conn, packet, 1000)) { /* Send failed */ printf("Send failed\n"); csp_buffer_free(packet); } /* Close connection */ csp_close(conn); } return 0; }
void csp_service_handler(csp_conn_t * conn, csp_packet_t * packet) { switch (csp_conn_dport(conn)) { case CSP_CMP: /* Pass to CMP handler */ if (csp_cmp_handler(conn, packet) != CSP_ERR_NONE) { csp_buffer_free(packet); return; } break; case CSP_PING: /* A ping means, just echo the packet, so no changes */ csp_log_info("SERVICE: Ping received\r\n"); break; case CSP_PS: { csp_sys_tasklist((char *)packet->data); packet->length = strlen((char *)packet->data); packet->data[packet->length] = '\0'; packet->length++; break; } case CSP_MEMFREE: { uint32_t total = csp_sys_memfree(); total = csp_hton32(total); memcpy(packet->data, &total, sizeof(total)); packet->length = sizeof(total); break; } case CSP_REBOOT: { uint32_t magic_word; memcpy(&magic_word, packet->data, sizeof(magic_word)); magic_word = csp_ntoh32(magic_word); /* If the magic word is invalid, return */ if (magic_word != 0x80078007) { csp_buffer_free(packet); return; } /* Otherwise Reboot */ csp_sys_reboot(); csp_buffer_free(packet); return; } case CSP_BUF_FREE: { uint32_t size = csp_buffer_remaining(); size = csp_hton32(size); memcpy(packet->data, &size, sizeof(size)); packet->length = sizeof(size); break; } case CSP_UPTIME: { uint32_t time = csp_get_s(); time = csp_hton32(time); memcpy(packet->data, &time, sizeof(time)); packet->length = sizeof(time); break; } default: csp_buffer_free(packet); return; } if (!csp_send(conn, packet, 0)) csp_buffer_free(packet); }
int csp_send_prio(uint8_t prio, csp_conn_t * conn, csp_packet_t * packet, uint32_t timeout) { conn->idout.pri = prio; return csp_send(conn, packet, timeout); }
void csp_client(void *p) { (void) p; csp_packet_t * packet; csp_conn_t * conn; portBASE_TYPE status; int signal; /** * Try ping */ csp_sleep_ms(200); #ifdef TARGET_LIKE_MSP430 blink(K_LED_RED); #else blink(K_LED_ORANGE); #endif int result = csp_ping(MY_ADDRESS, 100, 100, CSP_O_NONE); if (result) { #ifdef TARGET_LIKE_MSP430 blink(K_LED_RED); #else blink(K_LED_ORANGE); #endif } /** * Try data packet to server */ while (1) { status = xQueueReceive(button_queue, &signal, portMAX_DELAY); if (status != pdTRUE) { continue; } /* Get packet buffer for data */ packet = csp_buffer_get(100); if (packet == NULL) { /* Could not get buffer element */ return; } /* Connect to host HOST, port PORT with regular UDP-like protocol and 1000 ms timeout */ blink(K_LED_RED); conn = csp_connect(CSP_PRIO_NORM, MY_ADDRESS, MY_PORT, 1000, CSP_O_NONE); if (conn == NULL) { /* Connect failed */ /* Remember to free packet buffer */ csp_buffer_free(packet); return; } blink(K_LED_RED); /* Copy dummy data to packet */ char *msg = "Hello World"; strcpy((char *) packet->data, msg); /* Set packet length */ packet->length = strlen(msg); /* Send packet */ if (!csp_send(conn, packet, 1000)) { /* Send failed */ csp_buffer_free(packet); } blink(K_LED_RED); /* Close connection */ csp_close(conn); } }
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; }