示例#1
0
/** pbuf_cleanup
 * Purge all packets buffers that have timed out
 */
static void pbuf_cleanup(void) {

	int i;
	pbuf_element_t *buf;

	/* Lock packet buffer */
	CSP_ENTER_CRITICAL(pbuf_sem);

	/* Loop through buffer elements */
	for (i = 0; i < PBUF_ELEMENTS; i++) {
		buf = &pbuf[i];
		if (buf->state == BUF_USED) {
			/* Check timeout */
			uint32_t now = csp_get_ms();

			if (now - buf->last_used > PBUF_TIMEOUT_MS) {
				csp_log_info("CAN Buffer element timed out\r\n");
				/* Reuse packet buffer */
				pbuf_free(buf, NULL);
			}
		}
	}

	/* Unlock packet buffer */
	CSP_EXIT_CRITICAL(pbuf_sem);

}
示例#2
0
文件: csp_port.c 项目: jtrutna/libcsp
int csp_bind(csp_socket_t * socket, uint8_t port) {
	
	if (socket == NULL)
		return CSP_ERR_INVAL;

	if (port > CSP_ANY) {
		csp_log_error("Only ports from 0-%u (and CSP_ANY for default) are available for incoming ports\r\n", CSP_ANY);
		return CSP_ERR_INVAL;
	}

	/* Check if port number is valid */
	if (ports[port].state != PORT_CLOSED) {
		csp_log_error("Port %d is already in use\r\n", port);
		return CSP_ERR_USED;
	}

	csp_log_info("Binding socket %p to port %u\r\n", socket, port);

	/* Save listener */
	ports[port].socket = socket;
	ports[port].state = PORT_OPEN;

	return CSP_ERR_NONE;

}
示例#3
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);
}
示例#4
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);
	}

}
示例#5
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);

}
示例#6
0
int csp_can_tx(csp_iface_t * interface, csp_packet_t *packet, uint32_t timeout) {

	uint8_t bytes, overhead, avail, dest;
	uint8_t frame_buf[8];

	/* Get CFP identification number */
	int ident = id_get();
	if (ident < 0) {
		csp_log_warn("Failed to get CFP identification number\r\n");
		return CSP_ERR_INVAL;
	}

	/* Calculate overhead */
	overhead = sizeof(csp_id_t) + sizeof(uint16_t);

	/* Insert destination node mac address into the CFP destination field */
	dest = csp_route_get_nexthop_mac(packet->id.dst);
	if (dest == CSP_NODE_MAC)
		dest = packet->id.dst;

	/* Create CAN identifier */
	can_id_t id = 0;
	id |= CFP_MAKE_SRC(packet->id.src);
	id |= CFP_MAKE_DST(dest);
	id |= CFP_MAKE_ID(ident);
	id |= CFP_MAKE_TYPE(CFP_BEGIN);
	id |= CFP_MAKE_REMAIN((packet->length + overhead - 1) / 8);

	/* Get packet buffer */
	pbuf_element_t *buf = pbuf_new(id, NULL);

	if (buf == NULL) {
		csp_log_warn("Failed to get packet buffer for CAN\r\n");
		return CSP_ERR_NOMEM;
	}

	/* Set packet */
	buf->packet = packet;

	/* Calculate first frame data bytes */
	avail = 8 - overhead;
	bytes = (packet->length <= avail) ? packet->length : avail;

	/* Copy CSP headers and data */
	uint32_t csp_id_be = csp_hton32(packet->id.ext);
	uint16_t csp_length_be = csp_hton16(packet->length);

	memcpy(frame_buf, &csp_id_be, sizeof(csp_id_be));
	memcpy(frame_buf + sizeof(csp_id_be), &csp_length_be, sizeof(csp_length_be));
	memcpy(frame_buf + overhead, packet->data, bytes);

	/* Increment tx counter */
	buf->tx_count += bytes;

	/* Take semaphore so driver can post it later */
	csp_bin_sem_wait(&buf->tx_sem, 0);

	/* Send frame */
	if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
		csp_log_info("Failed to send CAN frame in csp_tx_can\r\n");
		return CSP_ERR_DRIVER;
	}

	/* Non blocking mode */
	if (timeout == 0)
		return CSP_ERR_NONE;

	/* Blocking mode */
	if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
		csp_bin_sem_post(&buf->tx_sem);
		return CSP_ERR_TIMEDOUT;
	} else {
		csp_bin_sem_post(&buf->tx_sem);
		return CSP_ERR_NONE;
	}

}
示例#7
0
int csp_tx_callback(can_id_t canid, can_error_t error, CSP_BASE_TYPE *task_woken) {

	int bytes;
	uint8_t dest;

	/* Match buffer element */
	pbuf_element_t *buf = pbuf_find(canid, CFP_ID_CONN_MASK, task_woken);

	if (buf == NULL) {
		csp_log_info("Failed to match buffer element in tx callback\r\n");
		csp_if_can.tx_error++;
		return CSP_ERR_INVAL;
	}

	if (buf->packet == NULL) {
		csp_log_warn("Buffer packet was NULL\r\n");
		csp_if_can.tx_error++;
		pbuf_free(buf, task_woken);
		return CSP_ERR_INVAL;
	}

	/* Free packet buffer if send failed */
	if (error != CAN_NO_ERROR) {
		csp_log_warn("Error in transmit callback\r\n");
		csp_if_can.tx_error++;
		pbuf_free(buf, task_woken);
		return CSP_ERR_DRIVER;
	}

	/* Send next frame if not complete */
	if (buf->tx_count < buf->packet->length) {
		/* Calculate frame data bytes */
		bytes = (buf->packet->length - buf->tx_count >= 8) ? 8 : buf->packet->length - buf->tx_count;

		/* Insert destination node mac address into the CFP destination field */
		dest = csp_route_get_nexthop_mac(buf->packet->id.dst);
		if (dest == CSP_NODE_MAC)
			dest = buf->packet->id.dst;

		/* Prepare identifier */
		can_id_t id	 = 0;
		id |= CFP_MAKE_SRC(buf->packet->id.src);
		id |= CFP_MAKE_DST(dest);
		id |= CFP_MAKE_ID(CFP_ID(canid));
		id |= CFP_MAKE_TYPE(CFP_MORE);
		id |= CFP_MAKE_REMAIN((buf->packet->length - buf->tx_count - bytes + 7) / 8);

		/* Increment tx counter */
		buf->tx_count += bytes;

		/* Send frame */
		if (can_send(id, buf->packet->data + buf->tx_count - bytes, bytes, task_woken) != 0) {
			csp_log_info("Failed to send CAN frame in Tx callback\r\n");
			csp_if_can.tx_error++;
			pbuf_free(buf, task_woken);
			return CSP_ERR_DRIVER;
		}
	} else {
		/* Free packet buffer */
		pbuf_free(buf, task_woken);

		/* Post semaphore if blocking mode is enabled */
		if (task_woken != NULL) {
			csp_bin_sem_post_isr(&buf->tx_sem, task_woken);
		} else {
			csp_bin_sem_post(&buf->tx_sem);
		}
	}

	return CSP_ERR_NONE;

}