Exemplo n.º 1
0
// 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);
}
Exemplo n.º 2
0
// New data available event handler
void rs485_serial_data_available_handler(void* opaque) {
	(void)opaque;

    if(!send_verify_flag) {
        disable_all_timers();
    }

    int bytes_received = 0;
    uint32_t uid_from_packet = 0;
    int add_recipient_opaque;
    Packet empty_packet;
    Packet data_packet;
    uint64_t dummy_read_buffer;
    
    // Merge or simply save the received bytes
    if(partial_receive_flag) {
        bytes_received = read(_rs485_serial_fd,
                              &receive_buffer[partial_receive_merge_index],
                              (RECEIVE_BUFFER_SIZE - partial_receive_merge_index));

        
        printf("bytes_received %d\n", bytes_received);

        partial_receive_merge_index = partial_receive_merge_index + bytes_received;
    }
    else {
        partial_receive_merge_index = read(_rs485_serial_fd, receive_buffer, RECEIVE_BUFFER_SIZE);

    if (partial_receive_merge_index != 13)
        printf("partial_receive_merge_index %d\n", partial_receive_merge_index);
    }
    //log_info("PARTIAL_RECEIVE_MERGE_INDEX = %d", partial_receive_merge_index);
    // Checks at least 13 bytes is available in the receive buffer
    if(partial_receive_merge_index >= 13) {
        int packet_validation_code =
        is_valid_packet(receive_buffer, partial_receive_merge_index);
        
        switch(packet_validation_code) {
            case PACKET_SEND_VERIFY_OK:
                // Stop send verify timer
                if (read(_send_verify_event, &dummy_read_buffer, sizeof(uint64_t))) {}
                setup_timer(&send_verify_timer, TIME_UNIT_NSEC, 0);
                timerfd_settime(_send_verify_event, 0, &send_verify_timer, NULL);
                // Disabling TX
                gpio_output_clear(_tx_pin);
                end = microseconds();
                // Clearing send verify flag
                send_verify_flag = 0;
                // Clearing partial receive flag
                partial_receive_flag = 0;
                
                if(sent_ack_of_data_packet) {
                    sent_ack_of_data_packet = 0;
                    master_current_request_processed = 1;
                    master_poll_slave_timeout_handler(NULL);
                }
                log_debug("RS485: Send verified");
                return;
            
            case PACKET_EMPTY_OK:
                // Proper empty packet
                log_debug("RS485: Empty packet received");
                
                if(sent_current_request_from_queue){
                    queue_pop(&_rs485_extension.packet_to_modbus_queue, NULL);
                    sent_current_request_from_queue = 0;
                }
                // Updating recipient in the routing table
                memcpy(&uid_from_packet, &receive_buffer[3], sizeof(uint32_t));
                stack_add_recipient(&_rs485_extension.base, uid_from_packet, receive_buffer[0]);
                
                partial_receive_flag = 0;
                master_current_request_processed = 1;
                if(sent_ack_of_data_packet) {
                    sent_ack_of_data_packet = 0;
                }
                master_poll_slave_timeout_handler(NULL);
                return;
            
            case PACKET_DATA_OK:
                // Proper data packet
                log_info("RS485: Data packet received");
                
                // Send message into brickd dispatcher
                memset(&data_packet, 0, sizeof(Packet));
                memcpy(&data_packet, &receive_buffer[3], partial_receive_merge_index - MODBUS_PACKET_OVERHEAD);
                network_dispatch_response(&data_packet);
                log_debug("RS485: Dispatched packet to network subsystem");
                
                if(sent_current_request_from_queue){
                    queue_pop(&_rs485_extension.packet_to_modbus_queue, NULL);
                    sent_current_request_from_queue = 0;
                }
                
                // Updating recipient in the routing table
                memcpy(&uid_from_packet, &receive_buffer[3], sizeof(uint32_t));
                add_recipient_opaque = receive_buffer[0];
                stack_add_recipient(&_rs485_extension.base, uid_from_packet, add_recipient_opaque);
                
                // Send ACK to the slave
                memset(&empty_packet, 0, sizeof(PacketHeader));
                empty_packet.header.length = 8;
                send_modbus_packet(_rs485_extension.slaves[master_current_slave_to_process],
                                   current_sequence_number,
                                   &empty_packet);
                sent_ack_of_data_packet = 1;
                return;
            
            case PACKET_ERROR_SEND_VERIFY:
                // Retry the current request
                log_error("RS485: Send verify failed");
                partial_receive_flag = 0;
                master_current_retry = MASTER_RETRIES;
                master_retry_timeout_handler(NULL);
                return;
                
            case PACKET_ERROR_ADDRESS:
                // Retry the current request
                log_error("RS485: Wrong address in packet");
                partial_receive_flag = 0;
                master_current_retry = MASTER_RETRIES;
                master_retry_timeout_handler(NULL);
                return;
                
            case PACKET_ERROR_FUNCTION_CODE:
                log_error("RS485: Wrong function code in packet");
                partial_receive_flag = 0;
                master_current_retry = MASTER_RETRIES;
                master_retry_timeout_handler(NULL);
                return;
                
            case PACKET_ERROR_SEQUENCE_NUMBER:
                // Retry the current request
                log_info("RS485: Wrong sequence number in packet");
                partial_receive_flag = 0;
                master_current_retry = MASTER_RETRIES;
                master_retry_timeout_handler(NULL);
                return;
                
            case PACKET_ERROR_LENGTH:
                // Retry the current request
                log_error("RS485: Wrong length in packet");
                partial_receive_flag = 0;
                master_current_retry = MASTER_RETRIES;
                master_retry_timeout_handler(NULL);
                return;
            
            case PACKET_ERROR_LENGTH_PARTIAL:
                log_debug("RS485: Partial data packet recieved");
                handle_partial_receive();
                return;
            
            case PACKET_ERROR_CRC16:
                // Retry the current request
                log_error("RS485: Wrong CRC16 in packet");
                partial_receive_flag = 0;
                master_current_retry = MASTER_RETRIES;
                master_retry_timeout_handler(NULL);
                return;
            
            default:
                return;
        }
    }
    else {
        log_debug("RS485: Partial packet recieved");
        handle_partial_receive();
        return;
    }
    if(send_verify_flag) {
        // Disabling TX
        gpio_output_clear(_tx_pin);
        end = microseconds();
        // Stop send verify timer
        if (read(_send_verify_event, &dummy_read_buffer, sizeof(uint64_t))) {}
        setup_timer(&send_verify_timer, TIME_UNIT_NSEC, 0);
        timerfd_settime(_send_verify_event, 0, &send_verify_timer, NULL);
        // Clearing send verify flag
        send_verify_flag = 0;
    }
    abort_current_request();
    return;
}
Exemplo n.º 3
0
// New packet from BrickletStack is send into brickd event loop
static void bricklet_stack_dispatch_from_spi(void *opaque) {
	BrickletStack *bricklet_stack = (BrickletStack*)opaque;
	int i;
	eventfd_t ev;
	Packet *packet;

	(void)opaque;

	// handle at most 5 queued responses at once to avoid blocking the event
	// loop for too long
	for (i = 0; i < 5; ++i) {
		if (eventfd_read(bricklet_stack->notification_event, &ev) < 0) {
			if (errno_would_block()) {
				return; // no queue responses left
			}

			log_error("Could not read from SPI notification event: %s (%d)",
			          get_errno_name(errno), errno);

			return;
		}

		mutex_lock(&bricklet_stack->response_queue_mutex);
		packet = queue_peek(&bricklet_stack->response_queue);
		mutex_unlock(&bricklet_stack->response_queue_mutex);

		if (packet == NULL) { // eventfd indicates a response but queue is empty
			log_error("Response queue and notification event are out-of-sync");

			return;
		}

		// Update routing table (this is necessary for Co-MCU Bricklets)
		if (packet->header.function_id == CALLBACK_ENUMERATE) {
			stack_add_recipient(&bricklet_stack->base, packet->header.uid, 0);
		}

		if ((packet->header.function_id == CALLBACK_ENUMERATE) ||
		    (packet->header.function_id == FUNCTION_GET_IDENTITY)) {
			EnumerateCallback *ec = (EnumerateCallback*)packet;

			// If the Bricklet is a HAT Brick (ID 111) or HAT Zero Brick (ID 112)
			// we update the connected_uid.
			if((ec->device_identifier == 111) || (ec->device_identifier == 112)) {
				*bricklet_stack->config.connected_uid = ec->header.uid;
			}

			// If the Bricklet is connected to an isolator we don't have to
			// update the position and the connected UID. this is already
			// done by the isolator itself.
			if(ec->position != 'Z' || ec->connected_uid[0] == '\0') {
				memcpy(ec->connected_uid, PACKET_NO_CONNECTED_UID_STR, PACKET_NO_CONNECTED_UID_STR_LENGTH);
				if((*bricklet_stack->config.connected_uid != 0) && (ec->device_identifier != 111) && (ec->device_identifier != 112)) {
					char base58[BASE58_MAX_LENGTH];
					base58_encode(base58, uint32_from_le(*bricklet_stack->config.connected_uid));
					strncpy(ec->connected_uid, base58, BASE58_MAX_LENGTH);
				}

				ec->position = 'a' + bricklet_stack->config.num;
			}
		}

		// Send message into brickd dispatcher
		network_dispatch_response(packet);
		bricklet_stack->data_seen = true;

		mutex_lock(&bricklet_stack->response_queue_mutex);
		queue_pop(&bricklet_stack->response_queue, NULL);
		mutex_unlock(&bricklet_stack->response_queue_mutex);
	}
}