communication.c

#include "communication.h"

#define INTC		XScuGic
#define COMM_UART_DEVICE_ID		XPAR_PS7_UART_0_DEVICE_ID
#define COMM_INTC_DEVICE_ID		XPAR_SCUGIC_SINGLE_DEVICE_ID
#define COMM_UART_INT_IRQ_ID		XPAR_PS7_UART_0_INTR//XPAR_XUARTPS_0_INTR

#define BAUD_RATE 921600
// Maximum number of bytes to be received within our loop time,
// plus the maximum packet size that might be carried over from the last call,
// plus 128 for a little buffer
// (bit/s) * (seconds) / (10 bits / byte for UART)
#define PACKET_HEADER_SIZE 7
#define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)

//#define INTERRUPT_BENCHMARK

// Declaration of interrupt handler
void Handler(void *CallBackRef, u32 Event, unsigned int EventData);

u8 packet[MAX_PACKET_SIZE];
int bytes_recv = 0;

int try_receive_packet(struct UARTDriver *uart) {
  int attempts = 0;
  while (attempts++ < MAX_PACKET_SIZE) {
    // Find start of packet
    if (bytes_recv == 0) {
      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
      if (packet[bytes_recv] != 0xBE) {
	continue; // keep looking
      }
      bytes_recv = 1;
    }

    // receive length bytes
    while (bytes_recv < PACKET_HEADER_SIZE) {
      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
      bytes_recv += 1;
    }

    // Check if incoming packet is nonsensically large
    unsigned short length = (packet[6] << 8 | packet[5]) + PACKET_HEADER_SIZE;
    if (length >= MAX_PACKET_SIZE) {
      bytes_recv = 0;
      continue; // abort packet and start over
    }

    // Receive rest of packet
    while (bytes_recv < length) {
      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
      bytes_recv += 1;
    }

    // Receive checksum and compare
    if (bytes_recv < length + 1) {
      if (uart->read(uart, &packet[bytes_recv])) return -1; // uart empty
      unsigned char calculated_checksum = 0;
      int i;
      for(i = 0; i < length; i++) {
	calculated_checksum ^= packet[i];
      }
      if (calculated_checksum != packet[bytes_recv]) {
	bytes_recv = 0;
	continue; // abort packet and start over
      }
      bytes_recv += 1;
    }

    // Packet ready
    return 0;
  }

  // Try again later
  return -1;
}

int process_packet(modular_structs_t *structs) {
  struct metadata meta;
  // parse metadata
  meta.begin_char = packet[0];
  meta.msg_type = packet[2] << 8 | packet[1];
  meta.msg_id = packet[4] << 8 | packet[3];
  meta.data_len = packet[6] << 8 | packet[5];

  // Call appropriate function for packet
  command_cb* func_ptr = MessageTypes[meta.msg_type].functionPtr;
  if (meta.msg_type < MAX_TYPE_ID && func_ptr) {
	  (* func_ptr)(structs,&meta,&packet[PACKET_HEADER_SIZE], meta.data_len);
  }
  // Done processing packets, reset state
  bytes_recv = 0;

  return 0;
}

void process_received(modular_structs_t *structs) {
  // Parse as many packets as possible
  struct UARTDriver *uart = &structs->hardware_struct.uart;
  while (!try_receive_packet(uart)) {
    process_packet(structs);
  }
}

int send_data(struct UARTDriver *uart, u16 type_id, u16 msg_id, u8* data, size_t size) {
	//----------------------------------------------------------------------------------------------
	//	   index||	   0	|	  1	   |	  2		 |	3 & 4 |		 5 & 6		 |	7+	|	end	   |
	//---------------------------------------------------------------------------------------------|
	// msg param|| beg char |       msg type         | msg id | data len (bytes) | data | checksum |
	//-------------------------------------------------------------------------------------------- |
	//	   bytes||	   1	|	       2             |   2    |        2         | var	|	 1	   |
	//----------------------------------------------------------------------------------------------

	unsigned char formattedHeader[PACKET_HEADER_SIZE];

	// Begin Char:
	formattedHeader[0] = BEGIN_CHAR;
	// Msg type 2 bytes:
	formattedHeader[1] = type_id & 0x000000ff;
	formattedHeader[2] = (type_id >> 8) & 0x000000ff;
	// Msg id 2 bytes
	formattedHeader[3] = msg_id & 0x000000ff;
	formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
	// Data length and data - bytes 5&6 for len, 7+ for data
	formattedHeader[5] = size & 0x000000ff;
	formattedHeader[6] = (size >> 8) & 0x000000ff;

	// Compute checksum
	unsigned char packet_checksum = 0;
	int i;
	for(i = 0; i < PACKET_HEADER_SIZE; i++) {
		packet_checksum ^= formattedHeader[i];
		uart->write(uart, formattedHeader[i]);
	}
	for (i = 0; i < size; i++) {
		packet_checksum ^= data[i];
		uart->write(uart, data[i]);
	}

	uart->write(uart, packet_checksum);

	return 0;
}