コード例 #1
0
ファイル: csp_if_can.c プロジェクト: ArduSat/libcsp_migrated
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;

}
コード例 #2
0
ファイル: csp_if_can.c プロジェクト: ArduSat/libcsp_migrated
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;
	}

}
コード例 #3
0
ファイル: csp_if_can.c プロジェクト: bfh-reseiwe/libcsp
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");
		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_rtable_find_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");
		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 */
	if (csp_bin_sem_wait(&buf->tx_sem, 0) != CSP_SEMAPHORE_OK) {
		csp_log_error("Failed to take CAN pbuf TX sem!");
		pbuf_free(buf, NULL, false);
		return CSP_ERR_DRIVER;
	}

	/* Send frame. We must free packet buffer is this fails,
	 * but the packet itself should be freed by the caller */
	if (can_send(id, frame_buf, overhead + bytes, NULL) != 0) {
		csp_log_warn("Failed to send CAN frame in csp_tx_can");
		csp_bin_sem_post(&buf->tx_sem);
		pbuf_free(buf, NULL, false);
		return CSP_ERR_DRIVER;
	}

	/* NOTE: The transmit packet is now owned by the transmission MOB and
	 * must NOT be freed by the calling thread. */

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

	/* Blocking mode */
	if (csp_bin_sem_wait(&buf->tx_sem, timeout) != CSP_SEMAPHORE_OK) {
		/* tx_sem is posted by transmission callback. The packet
		 * could still be in use by the transmission MOB, so
		 * we can not return CSP_ERR_TIMEOUT and risk that the
		 * calling thread frees the packet. */
		return CSP_ERR_NONE;
	} else {
		csp_bin_sem_post(&buf->tx_sem);
		return CSP_ERR_NONE;
	}

}