예제 #1
0
void hardware_dispatch_request(Packet *request) {
	char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
	int i;
	Stack *stack;
	int rc;
	bool dispatched = false;

	if (_stacks.count == 0) {
		log_packet_debug("No stacks connected, dropping request (%s)",
		                 packet_get_request_signature(packet_signature, request));

		return;
	}

	if (request->header.uid == 0) {
		log_packet_debug("Broadcasting request (%s) to %d stack(s)",
		                 packet_get_request_signature(packet_signature, request),
		                 _stacks.count);

		// broadcast to all stacks
		for (i = 0; i < _stacks.count; ++i) {
			stack = *(Stack **)array_get(&_stacks, i);

			stack_dispatch_request(stack, request, true);
		}
	} else {
		log_packet_debug("Dispatching request (%s) to %d stack(s)",
		                 packet_get_request_signature(packet_signature, request),
		                 _stacks.count);

		// dispatch to all stacks, not only the first one that might claim to
		// know the UID
		for (i = 0; i < _stacks.count; ++i) {
			stack = *(Stack **)array_get(&_stacks, i);

			rc = stack_dispatch_request(stack, request, false);

			if (rc < 0) {
				continue;
			} else if (rc > 0) {
				dispatched = true;
			}
		}

		if (dispatched) {
			return;
		}

		log_packet_debug("Broadcasting request because UID is currently unknown");

		// broadcast to all stacks, as no stack claimed to know the UID
		for (i = 0; i < _stacks.count; ++i) {
			stack = *(Stack **)array_get(&_stacks, i);

			stack_dispatch_request(stack, request, true);
		}
	}
}
예제 #2
0
파일: client.c 프로젝트: Tinkerforge/brickd
static void client_handle_request(Client *client, Packet *request) {
    char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
    EmptyResponse response;

    // handle requests meant for brickd
    if (uint32_from_le(request->header.uid) == UID_BRICK_DAEMON) {
        // add as pending request if response is expected
        if (packet_header_get_response_expected(&request->header)) {
            network_client_expects_response(client, request);
        }

        if (request->header.function_id == FUNCTION_GET_AUTHENTICATION_NONCE) {
            if (request->header.length != sizeof(GetAuthenticationNonceRequest)) {
                log_error("Received authentication-nonce request (%s) from client ("CLIENT_SIGNATURE_FORMAT") with wrong length, disconnecting client",
                          packet_get_request_signature(packet_signature, request),
                          client_expand_signature(client));

                client->disconnected = true;

                return;
            }

            client_handle_get_authentication_nonce_request(client, (GetAuthenticationNonceRequest *)request);
        } else if (request->header.function_id == FUNCTION_AUTHENTICATE) {
            if (request->header.length != sizeof(AuthenticateRequest)) {
                log_error("Received authenticate request (%s) from client ("CLIENT_SIGNATURE_FORMAT") with wrong length, disconnecting client",
                          packet_get_request_signature(packet_signature, request),
                          client_expand_signature(client));

                client->disconnected = true;

                return;
            }

            client_handle_authenticate_request(client, (AuthenticateRequest *)request);
        } else {
            response.header = request->header;
            response.header.length = sizeof(response);

            packet_header_set_error_code(&response.header,
                                         PACKET_E_FUNCTION_NOT_SUPPORTED);

            client_dispatch_response(client, NULL, (Packet *)&response, false, false);
        }
    } else if (client->authentication_state == CLIENT_AUTHENTICATION_STATE_DISABLED ||
               client->authentication_state == CLIENT_AUTHENTICATION_STATE_DONE) {
        // add as pending request if response is expected...
        if (packet_header_get_response_expected(&request->header)) {
            network_client_expects_response(client, request);
        }

        // ...then dispatch it to the hardware
        hardware_dispatch_request(request);
    } else {
        log_packet_debug("Client ("CLIENT_SIGNATURE_FORMAT") is not authenticated, dropping request (%s)",
                         client_expand_signature(client),
                         packet_get_request_signature(packet_signature, request));
    }
}
예제 #3
0
파일: red_stack.c 프로젝트: vszurma/brickd
// New packet from brickd event loop is queued to be written to stack via SPI
static int red_stack_dispatch_to_spi(Stack *stack, Packet *request, Recipient *recipient) {
	REDStackPacket *queued_request;

	(void)stack;

	if (request->header.uid == 0) {
		// UID = 0 -> Broadcast to all UIDs
		uint8_t is;

		for (is = 0; is < _red_stack.slave_num; is++) {
			mutex_lock(&_red_stack.slaves[is].packet_queue_mutex);
			queued_request = queue_push(&_red_stack.slaves[is].packet_to_spi_queue);
			queued_request->status = RED_STACK_PACKET_STATUS_ADDED;
			queued_request->slave = &_red_stack.slaves[is];
			memcpy(&queued_request->packet, request, request->header.length);
			mutex_unlock(&_red_stack.slaves[is].packet_queue_mutex);

			log_packet_debug("Request is queued to be broadcast to slave %d (%s)",
			                 is, packet_get_request_signature(packet_signature, request));
		}
	} else if (recipient != NULL) {
		// Get slave for recipient opaque (== stack_address)
		REDStackSlave *slave = &_red_stack.slaves[recipient->opaque];

		mutex_lock(&(slave->packet_queue_mutex));
		queued_request = queue_push(&(slave->packet_to_spi_queue));
		queued_request->status = RED_STACK_PACKET_STATUS_ADDED;
		queued_request->slave = slave;
		memcpy(&queued_request->packet, request, request->header.length);
		mutex_unlock(&(slave->packet_queue_mutex));

		log_packet_debug("Packet is queued to be send to slave %d over SPI (%s)",
		                 slave->stack_address,
		                 packet_get_request_signature(packet_signature, request));
	}

	return 0;
}
예제 #4
0
파일: network.c 프로젝트: vszurma/brickd
void network_client_expects_response(Client *client, Packet *request) {
	PendingRequest *pending_request;
	char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];

	if (client->pending_request_count >= CLIENT_MAX_PENDING_REQUESTS) {
		log_warn("Pending requests list for client ("CLIENT_SIGNATURE_FORMAT") is full, dropping %d pending request(s)",
		         client_expand_signature(client),
		         client->pending_request_count - CLIENT_MAX_PENDING_REQUESTS + 1);

		while (client->pending_request_count >= CLIENT_MAX_PENDING_REQUESTS) {
			pending_request = containerof(client->pending_request_sentinel.next, PendingRequest, client_node);

			pending_request_remove_and_free(pending_request);
		}
	}

	pending_request = calloc(1, sizeof(PendingRequest));

	if (pending_request == NULL) {
		log_error("Could not allocate pending request: %s (%d)",
		          get_errno_name(ENOMEM), ENOMEM);

		return;
	}

	node_reset(&pending_request->global_node);
	node_insert_before(&_pending_request_sentinel, &pending_request->global_node);

	node_reset(&pending_request->client_node);
	node_insert_before(&client->pending_request_sentinel, &pending_request->client_node);

	++client->pending_request_count;

	pending_request->client = client;
	pending_request->zombie = NULL;

	memcpy(&pending_request->header, &request->header, sizeof(PacketHeader));

#ifdef BRICKD_WITH_PROFILING
	pending_request->arrival_time = microseconds();
#endif

	log_packet_debug("Added pending request (%s) for client ("CLIENT_SIGNATURE_FORMAT")",
	                 packet_get_request_signature(packet_signature, request),
	                 client_expand_signature(client));
}
예제 #5
0
// New packet from brickd event loop is queued to be written to BrickletStack via SPI
static int bricklet_stack_dispatch_to_spi(Stack *stack, Packet *request, Recipient *recipient) {
	BrickletStack *bricklet_stack = (BrickletStack*)stack;
	Packet *queued_request;

	if((request->header.uid != 0) && (recipient == NULL)) {
		return 0;
	}

	if (!bricklet_stack->data_seen) {
		return 0;
	}

	mutex_lock(&bricklet_stack->request_queue_mutex);
	queued_request = queue_push(&bricklet_stack->request_queue);
	memcpy(queued_request, request, request->header.length);
	mutex_unlock(&bricklet_stack->request_queue_mutex);

	log_packet_debug("Packet is queued to be send over SPI (%s)",
					 packet_get_request_signature(packet_signature, request));

    return 0;
}
예제 #6
0
파일: client.c 프로젝트: Tinkerforge/brickd
void client_dispatch_response(Client *client, PendingRequest *pending_request,
                              Packet *response, bool force, bool ignore_authentication) {
    Node *pending_request_client_node = NULL;
    int enqueued = 0;
#ifdef BRICKD_WITH_PROFILING
    uint64_t elapsed;
#endif

    if (!ignore_authentication &&
            client->authentication_state != CLIENT_AUTHENTICATION_STATE_DISABLED &&
            client->authentication_state != CLIENT_AUTHENTICATION_STATE_DONE) {
        log_packet_debug("Ignoring non-authenticated client ("CLIENT_SIGNATURE_FORMAT")",
                         client_expand_signature(client));

        goto cleanup;
    }

    // find matching pending request if not forced and no pending request is
    // already given. do this before the disconnect check to ensure that even
    // for a disconnected client the pending request list is updated correctly
    if (!force && pending_request == NULL) {
        pending_request_client_node = client->pending_request_sentinel.next;

        while (pending_request_client_node != &client->pending_request_sentinel) {
            pending_request = containerof(pending_request_client_node, PendingRequest, client_node);

            if (packet_is_matching_response(response, &pending_request->header)) {
                break;
            }

            pending_request_client_node = pending_request_client_node->next;
        }

        if (pending_request_client_node == &client->pending_request_sentinel) {
            pending_request = NULL;

            goto cleanup;
        }
    }

    if (client->disconnected) {
        log_debug("Ignoring disconnected client ("CLIENT_SIGNATURE_FORMAT")",
                  client_expand_signature(client));

        goto cleanup;
    }

    if (force || pending_request != NULL) {
        enqueued = writer_write(&client->response_writer, response);

        if (enqueued < 0) {
            goto cleanup;
        }

        if (force) {
            log_packet_debug("Forced to %s response to client ("CLIENT_SIGNATURE_FORMAT")",
                             enqueued ? "enqueue" : "send", client_expand_signature(client));
        } else {
#ifdef BRICKD_WITH_PROFILING
            elapsed = microseconds() - pending_request->arrival_time;

            log_packet_debug("%s response to client ("CLIENT_SIGNATURE_FORMAT"), was requested %u.%03u msec ago, %d request(s) still pending",
                             enqueued ? "Enqueued" : "Sent", client_expand_signature(client),
                             (unsigned int)(elapsed / 1000), (unsigned int)(elapsed % 1000),
                             client->pending_request_count - 1);
#else
            log_packet_debug("%s response to client ("CLIENT_SIGNATURE_FORMAT"), %d request(s) still pending",
                             enqueued ? "Enqueued" : "Sent", client_expand_signature(client),
                             client->pending_request_count - 1);
#endif
        }
    }

cleanup:
    if (pending_request != NULL) {
        pending_request_remove_and_free(pending_request);
    }
}
예제 #7
0
파일: client.c 프로젝트: Tinkerforge/brickd
static void client_handle_read(void *opaque) {
    Client *client = opaque;
    int length;
    const char *message = NULL;
    char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];

    length = io_read(client->io, (uint8_t *)&client->request + client->request_used,
                     sizeof(Packet) - client->request_used);

    if (length == 0) {
        log_info("Client ("CLIENT_SIGNATURE_FORMAT") disconnected by peer",
                 client_expand_signature(client));

        client->disconnected = true;

        return;
    }

    if (length < 0) {
        if (length == IO_CONTINUE) {
            // no actual data received
        } else if (errno_interrupted()) {
            log_debug("Receiving from client ("CLIENT_SIGNATURE_FORMAT") was interrupted, retrying",
                      client_expand_signature(client));
        } else if (errno_would_block()) {
            log_debug("Receiving from client ("CLIENT_SIGNATURE_FORMAT") would block, retrying",
                      client_expand_signature(client));
        } else {
            log_error("Could not receive from client ("CLIENT_SIGNATURE_FORMAT"), disconnecting client: %s (%d)",
                      client_expand_signature(client), get_errno_name(errno), errno);

            client->disconnected = true;
        }

        return;
    }

    client->request_used += length;

    while (!client->disconnected && client->request_used > 0) {
        if (client->request_used < (int)sizeof(PacketHeader)) {
            // wait for complete header
            break;
        }

        if (!client->request_header_checked) {
            if (!packet_header_is_valid_request(&client->request.header, &message)) {
                // FIXME: include packet_get_content_dump output in the error message
                log_error("Received invalid request (%s) from client ("CLIENT_SIGNATURE_FORMAT"), disconnecting client: %s",
                          packet_get_request_signature(packet_signature, &client->request),
                          client_expand_signature(client), message);

                client->disconnected = true;

                return;
            }

            client->request_header_checked = true;
        }

        length = client->request.header.length;

        if (client->request_used < length) {
            // wait for complete packet
            break;
        }

        if (client->request.header.function_id == FUNCTION_DISCONNECT_PROBE) {
            log_packet_debug("Received disconnect probe from client ("CLIENT_SIGNATURE_FORMAT"), dropping request",
                             client_expand_signature(client));
        } else {
            log_packet_debug("Received request (%s) from client ("CLIENT_SIGNATURE_FORMAT")",
                             packet_get_request_signature(packet_signature, &client->request),
                             client_expand_signature(client));

            client_handle_request(client, &client->request);
        }

        memmove(&client->request, (uint8_t *)&client->request + length,
                client->request_used - length);

        client->request_used -= length;
        client->request_header_checked = false;
    }
}
예제 #8
0
void network_dispatch_response(Packet *response) {
	EnumerateCallback *enumerate_callback;
	char packet_signature[PACKET_MAX_SIGNATURE_LENGTH];
	int i;
	Client *client;
	Node *pending_request_global_node;
	PendingRequest *pending_request;

	if (packet_header_get_sequence_number(&response->header) == 0) {
		if (response->header.function_id == CALLBACK_ENUMERATE) {
			enumerate_callback = (EnumerateCallback *)response;

			if (enumerate_callback->enumeration_type == ENUMERATION_TYPE_CONNECTED ||
			    enumerate_callback->enumeration_type == ENUMERATION_TYPE_DISCONNECTED) {
				network_drop_pending_requests(response->header.uid);
			}
		}

		if (_clients.count == 0) {
			log_packet_debug("No clients connected, dropping %s (%s)",
			                 packet_get_response_type(response),
			                 packet_get_response_signature(packet_signature, response));

			return;
		}

		log_packet_debug("Broadcasting %s (%s) to %d client(s)",
		                 packet_get_response_type(response),
		                 packet_get_response_signature(packet_signature, response),
		                 _clients.count);

		for (i = 0; i < _clients.count; ++i) {
			client = array_get(&_clients, i);

			client_dispatch_response(client, NULL, response, true, false);
		}
	} else if (_clients.count + _zombies.count > 0) {
		log_packet_debug("Dispatching response (%s) to %d client(s) and %d zombies(s)",
		                 packet_get_response_signature(packet_signature, response),
		                 _clients.count, _zombies.count);

		pending_request_global_node = _pending_request_sentinel.next;

		while (pending_request_global_node != &_pending_request_sentinel) {
			pending_request = containerof(pending_request_global_node,
			                              PendingRequest, global_node);

			if (packet_is_matching_response(response, &pending_request->header)) {
				if (pending_request->client != NULL) {
					client_dispatch_response(pending_request->client, pending_request,
					                         response, false, false);
				} else {
					zombie_dispatch_response(pending_request->zombie, pending_request);
				}

				return;
			}

			pending_request_global_node = pending_request_global_node->next;
		}

		log_warn("Broadcasting response (%s) because no client/zombie has a matching pending request",
		         packet_get_response_signature(packet_signature, response));

		for (i = 0; i < _clients.count; ++i) {
			client = array_get(&_clients, i);

			client_dispatch_response(client, NULL, response, true, false);
		}
	} else {
		log_packet_debug("No clients/zombies connected, dropping response (%s)",
		                 packet_get_response_signature(packet_signature, response));
	}
}
예제 #9
0
static void LIBUSB_CALL usb_transfer_wrapper(struct libusb_transfer *handle) {
	USBTransfer *usb_transfer = handle->user_data;

	if (!usb_transfer->submitted) {
		log_error("%s transfer %p (%p) returned from %s, but was not submitted before",
		          usb_transfer_get_type_name(usb_transfer->type, true),
		          usb_transfer, handle, usb_transfer->usb_stack->base.name);

		return;
	}

	usb_transfer->submitted = false;
	usb_transfer->completed = true;

	if (handle->status == LIBUSB_TRANSFER_CANCELLED) {
		usb_transfer->usb_stack->expecting_disconnect = true;

		log_debug("%s transfer %p (%p) for %s was cancelled, marking device as about to be removed",
		          usb_transfer_get_type_name(usb_transfer->type, true),
		          usb_transfer, handle, usb_transfer->usb_stack->base.name);

		return;
	} else if (handle->status == LIBUSB_TRANSFER_NO_DEVICE) {
		usb_transfer->usb_stack->expecting_disconnect = true;

		log_debug("%s transfer %p (%p) for %s was aborted, device got disconnected",
		          usb_transfer_get_type_name(usb_transfer->type, true),
		          usb_transfer, handle, usb_transfer->usb_stack->base.name);

		return;
	} else if (handle->status == LIBUSB_TRANSFER_STALL) {
		// unplugging a RED Brick from Windows results in a stalled transfer
		// followed by read transfers returning garbage data and transfer
		// submission errors. all this happens before the unplug event for
		// the device is received. avoid logging pointless error messages
		// about garbage data and transfer submission errors by detecting this
		// condition here and deactivating the device
		if (usb_transfer->usb_stack->expecting_read_stall_before_removal &&
		    usb_transfer->type == USB_TRANSFER_TYPE_READ) {
			usb_transfer->usb_stack->expecting_read_stall_before_removal = false;
			usb_transfer->usb_stack->expecting_disconnect = true;

			log_debug("%s transfer %p (%p) for %s got stalled as expected before device removal, marking device as about to be removed",
			          usb_transfer_get_type_name(usb_transfer->type, true),
			          usb_transfer, handle, usb_transfer->usb_stack->base.name);
		} else {
			log_warn("%s transfer %p (%p) for %s got stalled",
			         usb_transfer_get_type_name(usb_transfer->type, true),
			         usb_transfer, handle, usb_transfer->usb_stack->base.name);

			// FIXME: maybe use libusb_clear_halt to clear halt condition?
		}

		return;
	} else if (handle->status != LIBUSB_TRANSFER_COMPLETED) {
		log_warn("%s transfer %p (%p) returned with an error from %s: %s (%d)",
		         usb_transfer_get_type_name(usb_transfer->type, true), usb_transfer,
		         handle, usb_transfer->usb_stack->base.name,
		         usb_transfer_get_status_name(handle->status), handle->status);
	} else {
		log_packet_debug("%s transfer %p (%p) returned successfully from %s%s",
		                 usb_transfer_get_type_name(usb_transfer->type, true),
		                 usb_transfer, handle, usb_transfer->usb_stack->base.name,
		                 usb_transfer->cancelled
		                 ? ", but it was cancelled in the meantime"
		                 : (usb_transfer->usb_stack->expecting_disconnect
		                    ? ", but the corresponding USB device is about to be removed"
		                    : ""));

		if (usb_transfer->cancelled ||
		    usb_transfer->usb_stack->expecting_disconnect) {
			return;
		}

		if (usb_transfer->function != NULL) {
			usb_transfer->function(usb_transfer);
		}
	}

	if (usb_transfer->type == USB_TRANSFER_TYPE_READ &&
	    !usb_transfer->cancelled &&
	    !usb_transfer->usb_stack->expecting_disconnect) {
		usb_transfer_submit(usb_transfer);
	}
}
예제 #10
0
int usb_transfer_submit(USBTransfer *usb_transfer) {
	uint8_t endpoint;
	int length;
	int rc;

	if (usb_transfer->submitted) {
		log_error("%s transfer %p (%p) is already submitted for %s",
		          usb_transfer_get_type_name(usb_transfer->type, true), usb_transfer,
		          usb_transfer->handle, usb_transfer->usb_stack->base.name);

		return -1;
	}

	switch (usb_transfer->type) {
	case USB_TRANSFER_TYPE_READ:
		endpoint = usb_transfer->usb_stack->endpoint_in;
		length = sizeof(Packet);

		break;

	case USB_TRANSFER_TYPE_WRITE:
		endpoint = usb_transfer->usb_stack->endpoint_out;
		length = usb_transfer->packet.header.length;

		break;

	default:
		log_error("Transfer for %s has invalid type",
		          usb_transfer->usb_stack->base.name);

		return -1;
	}

	usb_transfer->submitted = true;

	libusb_fill_bulk_transfer(usb_transfer->handle,
	                          usb_transfer->usb_stack->device_handle,
	                          endpoint,
	                          (unsigned char *)&usb_transfer->packet,
	                          length,
	                          usb_transfer_wrapper,
	                          usb_transfer,
	                          0);

	rc = libusb_submit_transfer(usb_transfer->handle);

	if (rc < 0) {
		log_error("Could not submit %s transfer %p (%p) to %s: %s (%d)",
		          usb_transfer_get_type_name(usb_transfer->type, false), usb_transfer,
		          usb_transfer->handle, usb_transfer->usb_stack->base.name,
		          usb_get_error_name(rc), rc);

		usb_transfer->submitted = false;

		return -1;
	}

	log_packet_debug("Submitted %s transfer %p (%p) for %u bytes to %s",
	                 usb_transfer_get_type_name(usb_transfer->type, false),
	                 usb_transfer, usb_transfer->handle, length,
	                 usb_transfer->usb_stack->base.name);

	return 0;
}
예제 #11
0
파일: red_stack.c 프로젝트: vszurma/brickd
// Main SPI loop. This runs independently from the brickd event thread.
// Data between RED Brick and SPI slave is exchanged every 500us.
// If there is no data to be send, we cycle through the slaves and request
// data. If there is data to be send the slave that ought to receive
// the data gets priority. This can greatly reduce latency in a big stack.
static void red_stack_spi_thread(void *opaque) {
	REDStackPacket *packet_to_spi = NULL;
	uint8_t stack_address_cycle;
	int ret;

	(void)opaque;

	do {
		stack_address_cycle = 0;
		_red_stack_reset_detected = 0;
		_red_stack.slave_num = 0;
		red_stack_spi_create_routing_table();

		_red_stack_spi_thread_running = false;

		if (_red_stack.slave_num > 0) {
			_red_stack_spi_thread_running = true;
		}

		// Ignore resets that we received in the meantime to prevent race conditions.
		_red_stack_reset_detected = 0;

		while (_red_stack_spi_thread_running) {
			REDStackSlave *slave = &_red_stack.slaves[stack_address_cycle];
			REDStackPacket *request = NULL;
			memset(&_red_stack.packet_from_spi, 0, sizeof(Packet));

			// Get packet from queue. The queue contains that are to be
			// send over SPI. It is filled through from the main brickd
			// event thread, so we have to make sure that there is not race
			// condition.
			if(slave->next_packet_empty) {
				slave->next_packet_empty = false;
				packet_to_spi = NULL;
			} else {
				mutex_lock(&(slave->packet_queue_mutex));
				packet_to_spi = queue_peek(&slave->packet_to_spi_queue);
				mutex_unlock(&(slave->packet_queue_mutex));
			}

			stack_address_cycle++;

			if (stack_address_cycle >= _red_stack.slave_num) {
				stack_address_cycle = 0;
			}

			// Set request if we have a packet to send
			if (packet_to_spi != NULL) {
				log_packet_debug("Packet will now be send over SPI (%s)",
				                 packet_get_request_signature(packet_signature, &packet_to_spi->packet));

				request = packet_to_spi;
			}

			ret = red_stack_spi_transceive_message(request, &_red_stack.packet_from_spi, slave);

			if ((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_SEND) == RED_STACK_TRANSCEIVE_RESULT_SEND_OK) {
				if ((!((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_READ) == RED_STACK_TRANSCEIVE_RESULT_READ_ERROR))) {
					// If we send a packet it must have come from the queue, so we can
					// pop it from the queue now.
					// If the sending didn't work (for whatever reason), we don't pop it
					// and therefore we will automatically try to send it again in the next cycle.
					mutex_lock(&(slave->packet_queue_mutex));
					queue_pop(&slave->packet_to_spi_queue, NULL);
					mutex_unlock(&(slave->packet_queue_mutex));
				}
			}

			// If we received a packet, we will dispatch it immediately.
			// We have some time until we try the next SPI communication anyway.
			if ((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_READ) == RED_STACK_TRANSCEIVE_RESULT_READ_OK) {
				// TODO: Check again if packet is valid?
				// We did already check the hash.

				// Before the dispatching we insert the stack position into an enumerate message
				red_stack_spi_insert_position(slave);

				red_stack_spi_request_dispatch_response_event();
				// Wait until message is dispatched, so we don't overwrite it
				// accidentally.
				semaphore_acquire(&_red_stack_dispatch_packet_from_spi_semaphore);
			}

			SLEEP_NS(0, 1000*_red_stack_spi_poll_delay);
		}

		if (_red_stack.slave_num == 0) {
			pthread_mutex_lock(&_red_stack_wait_for_reset_mutex);
			// Use helper to be save against spurious wakeups
			_red_stack_wait_for_reset_helper = 0;

			while (_red_stack_wait_for_reset_helper == 0) {
				pthread_cond_wait(&_red_stack_wait_for_reset_cond, &_red_stack_wait_for_reset_mutex);
			}

			pthread_mutex_unlock(&_red_stack_wait_for_reset_mutex);
		}

		if (_red_stack_reset_detected > 0) {
			red_stack_spi_handle_reset();
		}
	} while (_red_stack_reset_detected > 0);
}
예제 #12
0
파일: red_stack.c 프로젝트: vszurma/brickd
// If data should just be polled, set packet_send to NULL.
//
// If no packet is received from slave the length in packet_recv will be set to 0,
// the exact reason for that is encoded in the return value.
//
// For the return value see RED_STACK_TRANSCEIVE_RESULT_* at the top of this file.
static int red_stack_spi_transceive_message(REDStackPacket *packet_send, Packet *packet_recv, REDStackSlave *slave) {
	int retval = 0;
	uint8_t length, length_send;
	uint8_t checksum;
	int rc;
	uint8_t sequence_number_master = 0xFF;
	uint8_t sequence_number_slave = 0xFF;

	uint8_t tx[RED_STACK_SPI_PACKET_SIZE] = {0};
	uint8_t rx[RED_STACK_SPI_PACKET_SIZE] = {0};

	// We assume that we don't receive anything. If we receive a packet the
	// length will be overwritten again
	packet_recv->header.length = 0;

	// Preamble is always the same
	tx[RED_STACK_SPI_PREAMBLE] = RED_STACK_SPI_PREAMBLE_VALUE;

	if (packet_send == NULL) {
		// If packet_send is NULL
		// we send a message with empty payload (4 byte)
		tx[RED_STACK_SPI_LENGTH] = RED_STACK_SPI_PACKET_EMPTY_SIZE;
		retval = RED_STACK_TRANSCEIVE_RESULT_SEND_NONE;
	} else if (slave->status == RED_STACK_SLAVE_STATUS_AVAILABLE) {
		length = packet_send->packet.header.length;

		if (length > sizeof(Packet)) {
			retval |= RED_STACK_TRANSCEIVE_RESULT_SEND_ERROR;
			log_error("Send length is greater then allowed (actual: %d > maximum: %d)",
			          length, (int)sizeof(Packet));
			goto ret;
		}

		retval = RED_STACK_TRANSCEIVE_DATA_SEND;

		tx[RED_STACK_SPI_LENGTH] = length + RED_STACK_SPI_PACKET_EMPTY_SIZE;
		memcpy(tx+2, &packet_send->packet, length);
	} else {
		retval = RED_STACK_TRANSCEIVE_RESULT_SEND_ERROR;
		log_error("Slave with stack address %d is not present in stack", slave->stack_address);
		goto ret;
	}

	length = tx[RED_STACK_SPI_LENGTH];

	// Set master and slave sequence number
	tx[RED_STACK_SPI_INFO(length)] = slave->sequence_number_master | slave->sequence_number_slave;

	// Calculate checksum
	tx[RED_STACK_SPI_CHECKSUM(length)] = red_stack_spi_calculate_pearson_hash(tx, length-1);

	struct spi_ioc_transfer spi_transfer = {
		.tx_buf = (unsigned long)&tx,
		.rx_buf = (unsigned long)&rx,
		.len = RED_STACK_SPI_PACKET_SIZE,
	};

	red_stack_spi_select(slave);
	rc = ioctl(_red_stack_spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer);
	red_stack_spi_deselect(slave);

	if (rc < 0) {
		// Overwrite current return status with error,
		// it seems ioctl itself didn't work.
		retval = RED_STACK_TRANSCEIVE_RESULT_SEND_ERROR | RED_STACK_TRANSCEIVE_RESULT_READ_ERROR;
		if(packet_send == NULL) {
			slave->next_packet_empty = true;
		}
		log_error("ioctl failed: %s (%d)", get_errno_name(errno), errno);
		goto ret;
	}

	length_send = rc;

	if (length_send != RED_STACK_SPI_PACKET_SIZE) {
		// Overwrite current return status with error,
		// it seems ioctl itself didn't work.
		retval = RED_STACK_TRANSCEIVE_RESULT_SEND_ERROR | RED_STACK_TRANSCEIVE_RESULT_READ_ERROR;
		if(packet_send == NULL) {
			slave->next_packet_empty = true;
		}
		log_error("ioctl has unexpected result (actual: %d != expected: %d)",
		          length_send, RED_STACK_SPI_PACKET_SIZE);
		goto ret;
	}

	if (rx[RED_STACK_SPI_PREAMBLE] != RED_STACK_SPI_PREAMBLE_VALUE) {
		// Do not log by default, an "unproper preamble" is part of the protocol
		// if the slave is too busy to fill the DMA buffers fast enough
		// log_error("Received packet without proper preamble (actual: %d != expected: %d)",
		//          rx[RED_STACK_SPI_PREAMBLE], RED_STACK_SPI_PREAMBLE_VALUE);
		retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_READ)) | RED_STACK_TRANSCEIVE_RESULT_READ_ERROR;
		if(packet_send == NULL) {
			slave->next_packet_empty = true;
		}
		goto ret;
	}

	// Check length
	length = rx[RED_STACK_SPI_LENGTH];

	if ((length != RED_STACK_SPI_PACKET_EMPTY_SIZE) &&
	    ((length < (RED_STACK_SPI_PACKET_EMPTY_SIZE + sizeof(PacketHeader))) ||
	     (length > RED_STACK_SPI_PACKET_SIZE))) {
		log_error("Received packet with malformed length: %d", length);
		retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_READ)) | RED_STACK_TRANSCEIVE_RESULT_READ_ERROR;
		if(packet_send == NULL) {
			slave->next_packet_empty = true;
		}
		goto ret;
	}

	// Calculate and check checksum
	checksum = red_stack_spi_calculate_pearson_hash(rx, length-1);

	if (checksum != rx[RED_STACK_SPI_CHECKSUM(length)]) {
		log_error("Received packet with wrong checksum (actual: %x != expected: %x)",
		          checksum, rx[RED_STACK_SPI_CHECKSUM(length)]);
		retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_READ)) | RED_STACK_TRANSCEIVE_RESULT_READ_ERROR;
		if(packet_send == NULL) {
			slave->next_packet_empty = true;
		}
		goto ret;
	}

	// If we send data and the master sequence number matches to the one
	// set in the packet we know that the slave received the packet!
	if ((packet_send != NULL) /*&& (packet_send->status == RED_STACK_PACKET_STATUS_SEQUENCE_NUMBER_SET)*/) {
		sequence_number_master = rx[RED_STACK_SPI_INFO(length)] & RED_STACK_SPI_INFO_SEQUENCE_MASTER_MASK;

		if (sequence_number_master == slave->sequence_number_master) {
			retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_SEND)) | RED_STACK_TRANSCEIVE_RESULT_SEND_OK;

			// Increase sequence number for next packet
			red_stack_increase_master_sequence_number(slave);
		}
	} else {
		// If we didn't send anything we can always increase the sequence number,
		// it doesn't matter if the slave actually received it.
		red_stack_increase_master_sequence_number(slave);
	}

	// If the slave sequence number matches we already processed this packet
	sequence_number_slave = rx[RED_STACK_SPI_INFO(length)] & RED_STACK_SPI_INFO_SEQUENCE_SLAVE_MASK;

	if (sequence_number_slave == slave->sequence_number_slave) {
		retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_READ)) | RED_STACK_TRANSCEIVE_RESULT_READ_NONE;
	} else {
		// Otherwise we save the new sequence number
		slave->sequence_number_slave = sequence_number_slave;

		if (length == RED_STACK_SPI_PACKET_EMPTY_SIZE) {
			// Do not log by default, will produce 2000 log entries per second
			// log_packet_debug("Received empty packet over SPI (w/ header)");
			retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_READ)) | RED_STACK_TRANSCEIVE_RESULT_READ_NONE;
		} else {
			// Everything seems OK, we can copy to buffer
			memcpy(packet_recv, rx+2, length - RED_STACK_SPI_PACKET_EMPTY_SIZE);
			log_packet_debug("Received packet over SPI (%s)",
			                 packet_get_response_signature(packet_signature, packet_recv));
			retval = (retval & (~RED_STACK_TRANSCEIVE_RESULT_MASK_READ)) | RED_STACK_TRANSCEIVE_RESULT_READ_OK;
			retval |= RED_STACK_TRANSCEIVE_DATA_RECEIVED;
		}
	}

ret:
	return retval;
}

// Creates the "routing table", which is just the
// array of REDStackSlave structures.
static void red_stack_spi_create_routing_table(void) {
	char base58[BASE58_MAX_LENGTH];
	int tries, ret, i;
	uint8_t stack_address = 0;
	uint8_t uid_counter = 0;

	log_debug("Starting to discover SPI stack slaves");

	while (stack_address < RED_STACK_SPI_MAX_SLAVES) {
		REDStackSlave *slave = &_red_stack.slaves[stack_address];

		Packet packet;
		StackEnumerateResponse *response;

		REDStackPacket red_stack_packet = {
			slave,
			{{
				0,   // UID 0
				sizeof(StackEnumerateRequest),
				FUNCTION_STACK_ENUMERATE,
				0x08, // Return expected
				0
			}, {0}, {0}},
			RED_STACK_PACKET_STATUS_ADDED,
		};

		// We have to assume that the slave is available
		slave->status = RED_STACK_SLAVE_STATUS_AVAILABLE;

		// Send stack enumerate request
		for (tries = 0; tries < RED_STACK_SPI_ROUTING_TRIES; tries++) {
			ret = red_stack_spi_transceive_message(&red_stack_packet, &packet, slave);

			if ((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_SEND) == RED_STACK_TRANSCEIVE_RESULT_SEND_OK) {
				break;
			}

			SLEEP_NS(0, RED_STACK_SPI_ROUTING_WAIT); // Give slave some more time
		}

		if (tries == RED_STACK_SPI_ROUTING_TRIES) {
			// Slave does not seem to be available,
			slave->status = RED_STACK_SLAVE_STATUS_ABSENT;
			// This means that there can't be any more slaves above
			// and we are actually done here already!
			break;
		}

		// Receive stack enumerate response
		for (tries = 0; tries < RED_STACK_SPI_ROUTING_TRIES; tries++) {
			// We first check if we already received an answer before we try again
			if ((ret & RED_STACK_TRANSCEIVE_RESULT_MASK_READ) == RED_STACK_TRANSCEIVE_RESULT_READ_OK) {
				break;
			}

			// Here we sleep before transceive so that there is some time
			// between the sending of stack enumerate and the receiving
			// of the answer
			SLEEP_NS(0, RED_STACK_SPI_ROUTING_WAIT); // Give slave some more time

			ret = red_stack_spi_transceive_message(NULL, &packet, slave);
		}

		if (tries == RED_STACK_SPI_ROUTING_TRIES) {
			// Slave does not seem to be available,
			slave->status = RED_STACK_SLAVE_STATUS_ABSENT;
			// This means that there can't be any more slaves above
			// and we are actually done here already!
			break;
		}

		response = (StackEnumerateResponse *)&packet;

		for (i = 0; i < PACKET_MAX_STACK_ENUMERATE_UIDS; i++) {
			if (response->uids[i] != 0) {
				uid_counter++;
				stack_add_recipient(&_red_stack.base, response->uids[i], stack_address);
				log_debug("Found UID number %d of slave %d with UID %s",
				          i, stack_address,
				          base58_encode(base58, uint32_from_le(response->uids[i])));
			} else {
				break;
			}
		}

		stack_address++;
	}

	_red_stack.slave_num = stack_address;

	log_info("SPI stack slave discovery done. Found %d slave(s) with %d UID(s) in total",
	         stack_address, uid_counter);
}
예제 #13
0
파일: websocket.c 프로젝트: vszurma/brickd
int websocket_parse_header(Websocket *websocket, uint8_t *buffer, int length) {
	int websocket_frame_length = sizeof(WebsocketFrame);
	int to_copy = MIN(length, websocket_frame_length - websocket->frame_index);

	if (to_copy <= 0) {
		log_error("WebSocket frame index has invalid value (%d)", websocket->frame_index);
		return -1;
	}

	memcpy(((char*)&websocket->frame) + websocket->frame_index, buffer, to_copy);
	if (to_copy + websocket->frame_index < websocket_frame_length) {
		websocket->frame_index += to_copy;
		return IO_CONTINUE;
	} else {
		int fin = websocket_frame_get_fin(&websocket->frame.header);
		int opcode = websocket_frame_get_opcode(&websocket->frame.header);
		int payload_length = websocket_frame_get_payload_length(&websocket->frame.header);
		int mask = websocket_frame_get_mask(&websocket->frame.header);

		log_packet_debug("WebSocket header received (fin: %d, opc: %d, len: %d, key: [%d %d %d %d])",
		                 fin, opcode, payload_length,
		                 websocket->frame.masking_key[0],
		                 websocket->frame.masking_key[1],
		                 websocket->frame.masking_key[2],
		                 websocket->frame.masking_key[3]);

		if (mask != 1) {
			log_error("WebSocket frame has invalid mask (%d)", mask);

			return -1;
		}

		if (payload_length == 126 || payload_length == 127) {
			log_error("WebSocket frame with extended payload length not supported (%d)", payload_length);

			return -1;
		}

		switch (opcode) {
		case WEBSOCKET_OPCODE_CONTINUATION_FRAME:
		case WEBSOCKET_OPCODE_TEXT_FRAME:
			log_error("WebSocket opcodes 'continuation' and 'text' not supported");

			return -1;

		case WEBSOCKET_OPCODE_BINARY_FRAME:
			websocket->mask_index = 0;
			websocket->frame_index = 0;
			websocket->to_read = payload_length;
			websocket->state = WEBSOCKET_STATE_HEADER_DONE;

			if (length - to_copy > 0) {
				memmove(buffer, buffer + to_copy, length - to_copy);

				return websocket_parse_data(websocket, buffer, length - to_copy);
			}

			return IO_CONTINUE;

		case WEBSOCKET_OPCODE_CLOSE_FRAME:
			log_debug("WebSocket opcode 'close frame'");

			return 0;

		case WEBSOCKET_OPCODE_PING_FRAME:
			log_error("WebSocket opcode 'ping' not supported");

			return -1;

		case WEBSOCKET_OPCODE_PONG_FRAME:
			log_error("WebSocket opcode 'pong' not supported");

			return -1;
		}
	}

	log_error("Unknown WebSocket opcode (%d)", websocket_frame_get_opcode(&websocket->frame.header));

	return -1;
}