Esempio n. 1
0
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);

}
Esempio n. 2
0
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;
	}

}
Esempio n. 3
0
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;
    }
}
Esempio n. 4
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;

}
Esempio n. 5
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;

}
Esempio n. 6
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: ", 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);

}
Esempio n. 7
0
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;

}
Esempio n. 8
0
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;
}
Esempio n. 9
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;
	}

	/** @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);
}
Esempio n. 10
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");
		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);
	}

}
Esempio n. 11
0
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;
}
Esempio n. 12
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);

}
Esempio n. 13
0
File: csp_io.c Progetto: janbre/NUTS
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);
}
Esempio n. 14
0
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);
    }
}
Esempio n. 15
0
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;
}
Esempio n. 16
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;
}