<repo_name>zmechanic/GccApplication4<file_sep>/common.h
#ifndef COMMON_H_
#define COMMON_H_

#include <stdlib.h>
#include <stdint.h>

const uint8_t MSB_BIT_MASK										= 0b10000000;
const uint8_t LSB_BIT_MASK										= 0b00000001;

const uint8_t COMMON_BUFFER_SIZE_IN_BYTES						= 50;
const uint8_t UPLINK_BUFFER_SIZE_IN_BYTES						= 35;
const uint8_t DEVICE_DATA_BUFFER_SIZE_IN_BYTES					= 64;
const uint8_t DEVICE_WORK_BUFFER_SIZE_IN_BYTES					= 16;

const uint8_t COMMON_BUFFER_ONE_BYTE_RESPONSE_BIT_FLAG			= 0b10000000;
const uint8_t COMMON_BUFFER_ONE_BYTE_RESPONSE_CONTENT_BIT_MASK	= 0b01111111;
const uint8_t COMMON_BUFFER_RESPONSE_CALCULATE_CRC_BIT_FLAG		= 0b01000000;
const uint8_t COMMON_BUFFER_RESPONSE_PACKET_LENGTH_BIT_MASK		= 0b00111111;

#endif /* COMMON_H_ */<file_sep>/wdt.h
#ifndef WDT_H_
#define WDT_H_

static __inline__
__attribute__ ((__always_inline__))
void wdt_enable_int_only (const uint8_t value)
{
	__asm__ __volatile__ (
	"in __tmp_reg__,__SREG__" "\n\t"
	"cli" "\n\t"
	"wdr" "\n\t"
	"sts %0, %1" "\n\t"
	"out __SREG__,__tmp_reg__" "\n\t"
	"sts %0, %2" "\n \t"
	: /* no outputs */
	: "n" (_SFR_MEM_ADDR(_WD_CONTROL_REG)),
	"r" ((uint8_t)(_BV(_WD_CHANGE_BIT) | _BV(WDE))),
	"r" ((uint8_t) ((value & 0x08 ? _WD_PS3_MASK : 0x00) |
	_BV(WDIE) | (value & 0x07)) )
	: "r0"
	);
}

static __inline__
__attribute__ ((__always_inline__))
void wdt_enable_int_and_reset (const uint8_t value)
{
	__asm__ __volatile__ (
	"in __tmp_reg__,__SREG__" "\n\t"
	"cli" "\n\t"
	"wdr" "\n\t"
	"sts %0, %1" "\n\t"
	"out __SREG__,__tmp_reg__" "\n\t"
	"sts %0, %2" "\n \t"
	: /* no outputs */
	: "n" (_SFR_MEM_ADDR(_WD_CONTROL_REG)),
	"r" ((uint8_t)(_BV(_WD_CHANGE_BIT) | _BV(WDE))),
	"r" ((uint8_t) ((value & 0x08 ? _WD_PS3_MASK : 0x00) |
	_BV(WDE) | _BV(WDIE) | (value & 0x07)) )
	: "r0"
	);
}

#endif /* WDT_H_ */<file_sep>/adc.h
#ifndef __ADC_H__
#define __ADC_H__

void ADC_init();
uint16_t ADC_read(uint8_t channel);

#endif //__ADC_H__
<file_sep>/hub_delays.h
#ifndef __HUB_DELAYS_H__
#define __HUB_DELAYS_H__

class HubDelays
{
	private:
	static const uint8_t delay_PacketStartClckHi		= 15;
	static const uint8_t delay_PacketStartDataHi		= 15;
	static const uint8_t delay_PacketStartDataLo		= 15;
	static const uint8_t delay_PacketEndClckHi			= 15;
	static const uint8_t delay_PacketEndDataHi			= 15;
	static const uint8_t delay_SendBitClckLo			= 15;
	static const uint8_t delay_SendBitData				= 0xff;
	static const uint8_t delay_SendBitClckHi			= 15;
	static const uint8_t delay_SendBit9ClckLo			= 15;
	static const uint8_t delay_SendBit9Data				= 0xff;
	static const uint8_t delay_SendBit9ClckHi			= 15;
	static const uint8_t delay_SendAbandonDataLo		= 15;
	static const uint8_t delay_SendAbandonClckHi		= 15;
	static const uint8_t delay_SendLastByteTerminate	= 0x1e;
	static const uint8_t delay_RecvBitClckLo			= 15;
	static const uint8_t delay_RecvBitData				= 15;
	static const uint8_t delay_RecvBitClckHi			= 15;
	static const uint8_t delay_RecvBit9ClckLo			= 15;
	static const uint8_t delay_RecvBit9Data				= 15;
	static const uint8_t delay_RecvBit9ClckHi			= 15;

	public:
	static const uint8_t PacketStartClckHi		= 0;
	static const uint8_t PacketStartDataHi		= 1;
	static const uint8_t PacketStartDataLo		= 2;
	static const uint8_t PacketEndClckHi		= 3;
	static const uint8_t PacketEndDataHi		= 4;
	static const uint8_t SendBitClckLo			= 5;
	static const uint8_t SendBitData			= 6;
	static const uint8_t SendBitClckHi			= 7;
	static const uint8_t SendBit9ClckLo			= 8;
	static const uint8_t SendBit9Data			= 9;
	static const uint8_t SendBit9ClckHi			= 10;
	static const uint8_t SendAbandonDataLo		= 11;
	static const uint8_t SendAbandonClckHi		= 12;
	static const uint8_t SendLastByteTerminate	= 13;
	static const uint8_t RecvBitClckLo			= 14;
	static const uint8_t RecvBitData			= 15;
	static const uint8_t RecvBitClckHi			= 16;
	static const uint8_t RecvBit9ClckLo			= 17;
	static const uint8_t RecvBit9Data			= 18;
	static const uint8_t RecvBit9ClckHi			= 19;

#ifdef SYNCHRON
	private:
	static const uint8_t ItemsCount				= RecvBit9ClckHi + 1;

	public:
	static void InitializeDelaysWithDefaults(uint8_t * delays)
	{
		for (uint8_t i = 0; i < ItemsCount; i++)
		{
			delays[i] = 0;
		}
	}
#else
	static void InitializeDelaysWithDefaults(uint8_t * delays)
	{
		delays[PacketStartClckHi] = delay_PacketStartClckHi;
		delays[PacketStartDataHi] = delay_PacketStartDataHi;
		delays[PacketStartDataLo] = delay_PacketStartDataLo;
		delays[PacketEndClckHi] = delay_PacketEndClckHi;
		delays[PacketEndDataHi] = delay_PacketEndDataHi;
		delays[SendBitClckLo] = delay_SendBitClckLo;
		delays[SendBitData] = delay_SendBitData;
		delays[SendBitClckHi] = delay_SendBitClckHi;
		delays[SendBit9ClckLo] = delay_SendBit9ClckLo;
		delays[SendBit9Data] = delay_SendBit9Data;
		delays[SendBit9ClckHi] = delay_SendBit9ClckHi;
		delays[SendAbandonDataLo] = delay_SendAbandonDataLo;
		delays[SendAbandonClckHi] = delay_SendAbandonClckHi;
		delays[SendLastByteTerminate] = delay_SendLastByteTerminate;
		delays[RecvBitClckLo] = delay_RecvBitClckLo;
		delays[RecvBitData] = delay_RecvBitData;
		delays[RecvBitClckHi] = delay_RecvBitClckHi;
		delays[RecvBit9ClckLo] = delay_RecvBit9ClckLo;
		delays[RecvBit9Data] = delay_RecvBit9Data;
		delays[RecvBit9ClckHi] = delay_RecvBit9ClckHi;
	}
#endif
};
#endif //__HUB_DELAYS_H__
<file_sep>/commands.h
#ifndef COMMANDS_H_
#define COMMANDS_H_

#include <stdlib.h>
#include <stdint.h>

class MagicCommonCommands
{
	public:
	static const uint8_t UplinkDataPump = 'U';
};

class CommonCommandTypes
{
	public:
	static const uint8_t FireAndForget = 0xAA;
	static const uint8_t NeedResponse = 0xAB;
};

class Commands
{
	public:
	// protocol support commands
	static const uint32_t CRST = (uint32_t)'C' << 24 | (uint32_t)'R' << 16 | (uint32_t)'S' << 8 | 'T';    // resets controller
	static const uint32_t CREQ = (uint32_t)'C' << 24 | (uint32_t)'R' << 16 | (uint32_t)'E' << 8 | 'Q';    // requests controller to respond to patterned slave ID match
	static const uint32_t CNEW = (uint32_t)'C' << 24 | (uint32_t)'N' << 16 | (uint32_t)'E' << 8 | 'W';    // reports whether controller is new and not been detected yet via REQA
	static const uint32_t CSLP = (uint32_t)'C' << 24 | (uint32_t)'S' << 16 | (uint32_t)'L' << 8 | 'P';    // suspends controller
	static const uint32_t CWUP = (uint32_t)'C' << 24 | (uint32_t)'W' << 16 | (uint32_t)'U' << 8 | 'P';    // wakes up controller
	static const uint32_t CGID = (uint32_t)'C' << 24 | (uint32_t)'G' << 16 | (uint32_t)'I' << 8 | 'D';    // controller returns 32-bit module ID + 16-bit firmware version
	static const uint32_t CSLB = (uint32_t)'C' << 24 | (uint32_t)'S' << 16 | (uint32_t)'L' << 8 | 'B';    // sets lock bits of controller
	static const uint32_t CGLB = (uint32_t)'C' << 24 | (uint32_t)'G' << 16 | (uint32_t)'L' << 8 | 'B';    // gets lock bits of controller
	static const uint32_t CERR = (uint32_t)'C' << 24 | (uint32_t)'E' << 16 | (uint32_t)'R' << 8 | 'R';    // queries controller for errors

	// uplink stack commands
	static const uint32_t UWRI = (uint32_t)'U' << 24 | (uint32_t)'W' << 16 | (uint32_t)'R' << 8 | 'I';    // writes to uplink input buffer
	static const uint32_t UWRO = (uint32_t)'U' << 24 | (uint32_t)'W' << 16 | (uint32_t)'R' << 8 | 'O';    // writes to uplink output buffer
	static const uint32_t UCPY = (uint32_t)'U' << 24 | (uint32_t)'C' << 16 | (uint32_t)'P' << 8 | 'Y';    // copies content of uplink input buffer to uplink output buffer
	static const uint32_t UMOV = (uint32_t)'U' << 24 | (uint32_t)'M' << 16 | (uint32_t)'O' << 8 | 'V';    // moves content of uplink input buffer to uplink output buffer and clears input buffer
	static const uint32_t URDI = (uint32_t)'U' << 24 | (uint32_t)'R' << 16 | (uint32_t)'D' << 8 | 'I';    // reads ("peek") content of uplink input buffer
	static const uint32_t URDO = (uint32_t)'U' << 24 | (uint32_t)'R' << 16 | (uint32_t)'D' << 8 | 'O';    // reads ("peek") content of uplink output buffer
	static const uint32_t UCCI = (uint32_t)'U' << 24 | (uint32_t)'C' << 16 | (uint32_t)'C' << 8 | 'I';    // checks uplink input buffer for content
	static const uint32_t UCCO = (uint32_t)'U' << 24 | (uint32_t)'C' << 16 | (uint32_t)'C' << 8 | 'O';    // checks uplink output buffer for content

	// target device (-duino) commands that are handled by module controller
	static const uint32_t DRST = (uint32_t)'D' << 24 | (uint32_t)'R' << 16 | (uint32_t)'S' << 8 | 'T';    // resets -duino
	static const uint32_t DSLP = (uint32_t)'D' << 24 | (uint32_t)'S' << 16 | (uint32_t)'L' << 8 | 'P';    // suspends -duino, so it no longer responds to uplink data
	static const uint32_t DWUP = (uint32_t)'D' << 24 | (uint32_t)'W' << 16 | (uint32_t)'U' << 8 | 'P';    // wakes up -duino
	static const uint32_t DMSG = (uint32_t)'D' << 24 | (uint32_t)'M' << 16 | (uint32_t)'S' << 8 | 'G';    // forwards message payload to -duino (this is the way to send commands that will be handled by device itself)
};

class DeviceCommands
{
	public:
	static const uint8_t DeviceCommandIdentifier = 0xff;

	// target device (-duino) commands that are forwarded to device as-is
	static const uint32_t DGID = (uint32_t)'D' << 24 | (uint32_t)'G' << 16 | (uint32_t)'I' << 8 | 'D';    // send request to -duino to return 64-bit module ID
	static const uint32_t DLCK = (uint32_t)'D' << 24 | (uint32_t)'L' << 16 | (uint32_t)'C' << 8 | 'K';    // send request to -duino to apply locks as needed
	static const uint32_t DRBE = (uint32_t)'D' << 24 | (uint32_t)'R' << 16 | (uint32_t)'B' << 8 | 'E';    // reads block from -duino EEPROM
};

class UplinkCommandTypes
{
	public:
	static const uint8_t PacketTypeMask      = 0b11000000;	//11xxxxxx
	static const uint8_t PacketSizeMask      = 0b00111111;	//xx111111
	static const uint8_t DeviceEmptyPacket   = 0b00000000;	//00 pattern is chosen to distinguish valid packet and non-responding previous module, which will always be 11
	static const uint8_t DeviceDataPacket    = 0b01000000;	//01 pattern is chosen to distinguish valid packet and non-responding previous module, which will always be 11
	static const uint8_t DeviceMessagePacket = 0b10000000;	//10 pattern is chosen to distinguish valid packet and non-responding previous module, which will always be 11
	static const uint8_t EmptyPacket         = 0b00110110;	//00110110 pattern to signify empty packet
};

#endif /* COMMANDS_H_ */<file_sep>/device.cpp
#include <string.h>

#include "common.h"
#include "commands.h"
#include "device.h"

static uint8_t InpBuffer[DEVICE_WORK_BUFFER_SIZE_IN_BYTES];
static uint8_t OutBuffer[DEVICE_WORK_BUFFER_SIZE_IN_BYTES];

static uint8_t _internalInpBuffer[DEVICE_DATA_BUFFER_SIZE_IN_BYTES];
static uint8_t _internalOutBuffer[DEVICE_DATA_BUFFER_SIZE_IN_BYTES];

uint8_t ProcessMessage(uint8_t length);
uint8_t ProcessServiceMessage(uint8_t length);

void DEV_Reset()
{

}

uint8_t DEV_ProcessMessage(uint8_t * srcBuffer, uint8_t length, uint8_t * dstBuffer)
{
	memcpy(_internalInpBuffer, srcBuffer, length);

    uint8_t readIndex = 0;
    uint8_t writeIndex = 0;

    while (readIndex <= length)
    {
	    uint8_t inpMessageLength = 0;

	    if (readIndex < length)
	    {
		    inpMessageLength = _internalInpBuffer[readIndex] + 1;
		    for (uint8_t i = 0; i < inpMessageLength; i++)
		    {
			    InpBuffer[i] = _internalInpBuffer[readIndex++];
		    }
	    }
	    else
	    {
		    // this is to abandon the loop next time
		    readIndex++;
	    }

	    uint8_t outMessageLengthAndCode = inpMessageLength > 0 && InpBuffer[1] == DeviceCommands::DeviceCommandIdentifier
			? ProcessServiceMessage(inpMessageLength)
			: ProcessMessage(inpMessageLength);
	    uint8_t outMessageCode = outMessageLengthAndCode & 192;
	    uint8_t outMessageLength = outMessageLengthAndCode & 63;

	    // copy original message before response message, if needed
	    switch (outMessageCode)
	    {
		    case MessageResponseCodes::InsertAfterMessage:
		    case MessageResponseCodes::IgnoreMessage:
		    for (uint8_t i = 0; i < inpMessageLength; i++)
		    {
			    _internalOutBuffer[writeIndex++] = InpBuffer[i];
		    }
		    break;
	    }

	    // transfer response message from device to output buffer
	    for (uint8_t i = 0; i < outMessageLength; i++)
	    {
		    _internalOutBuffer[writeIndex++] = OutBuffer[i];
	    }

	    // copy original message after response message, if needed
	    switch (outMessageCode)
	    {
		    case MessageResponseCodes::InsertBeforeMessage:
		    for (uint8_t i = 0; i < inpMessageLength; i++)
		    {
			    _internalOutBuffer[writeIndex++] = InpBuffer[i];
		    }
		    break;
	    }
    }

	memcpy(dstBuffer, _internalOutBuffer, writeIndex);
	return writeIndex;
}

uint8_t ProcessMessage(uint8_t length)
{
	return 0;
}

uint8_t ProcessServiceMessage(uint8_t length)
{
	return 0;
}
<file_sep>/Debug/makedep.mk
################################################################################
# Automatically-generated file. Do not edit or delete the file
################################################################################

adc.cpp

crc.cpp

error_logger.cpp

hub_interface.cpp

main.cpp

mod_interface.cpp

uart.cpp

usart.cpp

<file_sep>/usart.h
#ifndef __USART_H__
#define __USART_H__

void USART_Init(uint32_t baudRate);

bool USART_CopyAvailableTillLineEnd(char * target, uint8_t & cnt);
void USART_WriteChar(char data);
void USART_Write(char * stringPtr);
void USART_Write(char * stringPtr, uint8_t length);
void USART_Write(const char * stringPtr);
void USART_Write(const char * stringPtr, uint8_t length);
void USART_Write(volatile char * stringPtr);
void USART_Write(volatile char * stringPtr, uint8_t length);
void USART_WriteBoolResultCode(bool b);
void USART_WriteByteResultCode(uint8_t b);
void USART_WriteByteAsHex(uint8_t b);
void USART_WriteShortAsHex(uint16_t s);
void USART_WriteIntAsHex(uint32_t i);

#endif //__USART_H__
<file_sep>/device.h
#ifndef __DEVICE_H__
#define __DEVICE_H__

#include <stdlib.h>
#include <stdint.h>

class MessageResponseCodes
{
	public:
	static const uint8_t IgnoreMessage			= 0b00000000;
	static const uint8_t InsertAfterMessage		= 0b01000000;
	static const uint8_t InsertBeforeMessage	= 0b10000000;
	static const uint8_t DiscardMessage			= 0b11000000;
};

void DEV_Reset();
uint8_t DEV_ProcessMessage(uint8_t * buffer, uint8_t length, uint8_t * dstBuffer);

#endif //__DEVICE_H__
<file_sep>/mod_interface.h
#ifndef __MOD_INTERFACE_H__
#define __MOD_INTERFACE_H__

void MOD_MonitorConnectionHealth();
void MOD_PadPortStateChanged(uint8_t currentPortState, uint8_t previousPortState);

#endif //__MOD_INTERFACE_H__
<file_sep>/hub_interface.cpp
#include "config.h"

#include <stdlib.h>
#include <stdint.h>
#include <stdio.h>
#include <avr/io.h>
#include <util/delay.h>

#include "common.h"
#include "errors.h"
#include "crc.h"
#include "commands.h"
#include "error_logger.h"
#include "hub_delays.h"
#include "hub_interface.h"

#include "usart.h"

// port assignment for hub pins
#define PAD_PORT_CFG		DDRB
#define PAD_PORT_INP		PINB
#define PAD_PORT_OUT		PORTB

// uplink pin - only one pin needed (i.e. uplink signal out)
#define LNK_PORT_CFG		DDRB
#define LNK_PORT_INP		PINB
#define LNK_PORT_OUT		PORTB
#define LNK_UOUT			PORTB4

// hub pins - only two pins needed (i.e. clock and data)
#define PAD_CLCK			PORTB1
#define PAD_DATA			PORTB2

// configures pin for output or input
#define CFG_PAD_OUT(pad)	PAD_PORT_CFG |=  (1 << pad)
#define CFG_PAD_INP(pad)	PAD_PORT_CFG &= ~(1 << pad)

// set pin template
#define SET_PAD_HI(pad)		PAD_PORT_OUT |=  (1 << pad)
#define SET_PAD_LO(pad)		PAD_PORT_OUT &= ~(1 << pad)

// get pin state template
#define GET_PAD_HI(pad)		((PAD_PORT_INP & (1 << pad)) != 0)
#define GET_PAD_LO(pad)		((PAD_PORT_INP & (1 << pad)) == 0)

// configures pin for CLCK signal either for output or input
#define CFG_PAD_CLCK_OUT	CFG_PAD_OUT(PAD_CLCK)

// configures pin for DATA signal either for output or input
#define CFG_PAD_DATA_OUT	CFG_PAD_OUT(PAD_DATA)
#define CFG_PAD_DATA_INP	CFG_PAD_INP(PAD_DATA)

// sets state of CLCK pin either HI or LO
#define SET_PAD_CLCK_HI		SET_PAD_HI(PAD_CLCK)
#define SET_PAD_CLCK_LO		SET_PAD_LO(PAD_CLCK)

// sets state of DATA pin either HI or LO
#define SET_PAD_DATA_HI		SET_PAD_HI(PAD_DATA)
#define SET_PAD_DATA_LO		SET_PAD_LO(PAD_DATA)

// asserts CLCK pin output state is set as expected
#define ASSERT_PAD_CLCK_HI	((PAD_PORT_OUT & (1 << PAD_CLCK)) != 0)
#define ASSERT_PAD_CLCK_LO	((PAD_PORT_OUT & (1 << PAD_CLCK)) == 0)

// gets and validates whether state of DATA pin is set as expected
#define GET_PAD_DATA_HI		GET_PAD_HI(PAD_DATA)
#define GET_PAD_DATA_LO		GET_PAD_LO(PAD_DATA)

// configures pin for UPLINK signal either for output or input
#define CFG_LNK_UOUT_OUT	LNK_PORT_CFG |=  (1 << LNK_UOUT)

// sets state of UPLINK output pin either HI or LO
#define SET_LNK_HI			LNK_PORT_OUT |=  (1 << LNK_UOUT)
#define SET_LNK_LO			LNK_PORT_OUT &= ~(1 << LNK_UOUT)

static uint8_t _commonBufferTransceiverBitQueue;
static uint8_t _uplinkBufferTransmitterBitQueue;

static uint8_t _commonBuffer[COMMON_BUFFER_SIZE_IN_BYTES];
static uint8_t _uplinkBuffer[UPLINK_BUFFER_SIZE_IN_BYTES];

static uint8_t _commonBufferWriteIndex;
static uint8_t _commonBufferReadIndex;
static uint8_t _expectedResponseLength;

static bool _validateCrcForCurrentResponse;

static uint8_t _delays[32];

static inline void DelayForSyncTest() __attribute__((__always_inline__));
static inline void DelayForAsync(uint8_t delayId) __attribute__((__always_inline__));

static bool SendPacketStart();
static bool SendPacketEnd();
static bool SendEntireBuffer(bool terminate, bool transmitUplinkData);
static bool SendCommonCommandPart(uint8_t commandType, uint32_t slave, uint32_t command, uint8_t * payload, uint8_t payloadLength);
static uint8_t ReadOneByteResponseFromCommonBuffer();
static uint8_t ReadNextByteFromCommonBuffer();
static uint32_t ReadNextWordFromCommonBuffer();
static void WriteNextBytesToCommonBuffer(uint8_t * v, uint8_t length);
static void WriteNextByteToCommonBuffer(uint8_t v);
static void WriteNextWordToCommonBuffer(uint32_t v);
static void WriteNextByteToUplinkBuffer(uint8_t uplinkBufferValue, uint8_t commonBufferValue);

void HUB_Initialize()
{
	HubDelays::InitializeDelaysWithDefaults(_delays);
}

uint8_t HUB_GetDelay(uint8_t delayId)
{
	return _delays[delayId];
}

void HUB_SetDelay(uint8_t delayId, uint8_t value)
{
	_delays[delayId] = value;
}

uint8_t * HUB_GetBufferContent()
{
	return _commonBuffer;
}

bool HUB_SendEmptyDuinoCommand()
{
	uint8_t payload[1];
	payload[0] = UplinkCommandTypes::EmptyPacket;

	return HUB_SendUplinkPacket(payload, 1);
}

bool HUB_SendUplinkPumpPacket()
{
	return HUB_SendUplinkPacket(NULL, 0);
}

bool HUB_SendUplinkPacket(uint8_t * payload, uint8_t length)
{
	_commonBufferWriteIndex = 0;
	_commonBufferReadIndex = 0;

	WriteNextByteToUplinkBuffer(0, MagicCommonCommands::UplinkDataPump);

	for (uint8_t i = 0; i < length; i++)
	{
		WriteNextByteToUplinkBuffer(payload[i], 0);
	}

	for (uint8_t i = length; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
	{
		WriteNextByteToUplinkBuffer(0, 0);
	}

	bool result = false;

	if (SendPacketStart())
	{
		result = SendEntireBuffer(true, true);
	}
	
	SendPacketEnd();

	return result;
}

bool HUB_SendCommandToAll(uint32_t command)
{
	return HUB_SendCommand(0, command, NULL, 0);
}

bool HUB_SendCommandToAll(uint32_t command, uint32_t payload)
{
	return HUB_SendCommand(0, command, payload);
}

bool HUB_SendCommandToAll(uint32_t command, uint8_t * payload, uint8_t payloadLength)
{
	return HUB_SendCommand(0, command, payload, payloadLength);
}

bool HUB_SendCommand(uint32_t slave, uint32_t command)
{
	return HUB_SendCommand(slave, command, NULL, 0);
}

bool HUB_SendCommand(uint32_t slave, uint32_t command, uint32_t payload)
{
	uint8_t temp[4] = { (uint8_t)((payload >> 24) & 0xff), (uint8_t)((payload >> 16) & 0xff), (uint8_t)((payload >> 8) & 0xff), (uint8_t)(payload & 0xff) };
	return HUB_SendCommand(slave, command, temp, 4);
}

bool HUB_SendCommand(uint32_t slave, uint32_t command, uint8_t * payload, uint8_t payloadLength)
{
	bool result = SendCommonCommandPart(CommonCommandTypes::FireAndForget, slave, command, payload, payloadLength);
	SendPacketEnd();

	return result;
}

bool HUB_SendCommandToAllWithExpectedResponse(uint32_t command)
{
	return HUB_SendCommandWithExpectedResponse(0, command, NULL, 0);
}

bool HUB_SendCommandToAllWithExpectedResponse(uint32_t command, uint32_t payload)
{
	return HUB_SendCommandWithExpectedResponse(0, command, payload);
}

bool HUB_SendCommandToAllWithExpectedResponse(uint32_t command, uint8_t * payload, uint8_t payloadLength)
{
	return HUB_SendCommandWithExpectedResponse(0, command, payload, payloadLength);
}

bool HUB_SendCommandWithExpectedResponse(uint32_t slave, uint32_t command)
{
	return HUB_SendCommandWithExpectedResponse(slave, command, NULL, 0);
}

bool HUB_SendCommandWithExpectedResponse(uint32_t slave, uint32_t command, uint32_t payload)
{
	uint8_t temp[4] = { (uint8_t)((payload >> 24) & 0xff), (uint8_t)((payload >> 16) & 0xff), (uint8_t)((payload >> 8) & 0xff), (uint8_t)(payload & 0xff) };
	return HUB_SendCommandWithExpectedResponse(slave, command, temp, 4);
}

bool HUB_SendCommandWithExpectedResponse(uint32_t slave, uint32_t command, uint8_t * payload, uint8_t payloadLength)
{
	bool result = SendCommonCommandPart(CommonCommandTypes::NeedResponse, slave, command, payload, payloadLength);
	if (result == false)
	{
		// always send packet end;
		// however, module is able to recover even without it
		SendPacketEnd();
		return false;
	}

	_commonBufferTransceiverBitQueue = 0;
	_commonBufferWriteIndex = 0;
	_expectedResponseLength = 0;

	// clear buffer before writing response
	// in reality this is not needed as content length is known
	// this means, that any left overs in the buffer after response can be safely ignored
	// however, nulling the buffer makes debugging easier
	for (uint8_t i = 0; i < COMMON_BUFFER_SIZE_IN_BYTES; i++)
	{
		_commonBuffer[i] = 0;
	}

	// read response from slave(s)
	for (;;)
	{
		for (uint8_t i = 0; i < 8; i++)
		{
			DelayForSyncTest();
			SET_PAD_CLCK_LO;
			DelayForAsync(HubDelays::RecvBitClckLo);

			DelayForSyncTest();
			SET_PAD_DATA_HI;
			CFG_PAD_DATA_INP;
			DelayForAsync(HubDelays::RecvBitData);

			_commonBufferTransceiverBitQueue = _commonBufferTransceiverBitQueue << 1;
			_commonBufferTransceiverBitQueue |= GET_PAD_DATA_HI ? 1 : 0;

			DelayForSyncTest();
			SET_PAD_CLCK_HI;
			DelayForAsync(HubDelays::RecvBitClckHi);
		}

		// transmit 9th extra bit
		DelayForSyncTest();
		SET_PAD_CLCK_LO;
		DelayForAsync(HubDelays::RecvBit9ClckLo);

		DelayForSyncTest();
		SET_PAD_DATA_LO;
		DelayForAsync(HubDelays::RecvBit9Data);

		DelayForSyncTest();
		SET_PAD_CLCK_HI;
		DelayForAsync(HubDelays::RecvBit9ClckHi);

		if (_commonBufferWriteIndex == 0)
		{
			if (_commonBufferTransceiverBitQueue == 0)
			{
				_commonBuffer[0] = 0;
				break;
			}

			if (_commonBufferTransceiverBitQueue < COMMON_BUFFER_ONE_BYTE_RESPONSE_BIT_FLAG)
			{
				_expectedResponseLength = _commonBufferTransceiverBitQueue & COMMON_BUFFER_RESPONSE_PACKET_LENGTH_BIT_MASK;
				_validateCrcForCurrentResponse = (_commonBufferTransceiverBitQueue & COMMON_BUFFER_RESPONSE_CALCULATE_CRC_BIT_FLAG) != 0;
			}
			else
			{
				_expectedResponseLength = 1;
				_validateCrcForCurrentResponse = false;
			}

			if (_validateCrcForCurrentResponse)
			{
				_expectedResponseLength++;
			}
		}

		_commonBuffer[_commonBufferWriteIndex] = _commonBufferTransceiverBitQueue;

		if (_commonBufferWriteIndex == _expectedResponseLength)
		{
			break;
		}

		if (++_commonBufferWriteIndex == COMMON_BUFFER_SIZE_IN_BYTES)
		{
			break;
		}
	}

	// always send packet end; however, module is able to recover even without it
	SendPacketEnd();

	// check to prevent buffer overflow
	if (_commonBufferWriteIndex == COMMON_BUFFER_SIZE_IN_BYTES)
	{
		_commonBufferWriteIndex = 0;
		_commonBufferReadIndex = 0;
		ERRLOG_LogError(Errors::HubCommonBufferOnResponseReadOverflow);
		return false;
	}

	// validate CRC
	if (_validateCrcForCurrentResponse)
	{
		if (_commonBuffer[_commonBufferWriteIndex] != CRC_CalculateBufferCrc(_commonBuffer, _commonBufferWriteIndex))
		{
			_commonBufferWriteIndex = 0;
			_commonBufferReadIndex = 0;
			ERRLOG_LogError(Errors::HubCommonBufferOnResponseReadCrcMismatch);
			return false;
		}

		// we need to remove "calculate CRC" request flag from the header
		// this flag is embedded in the response by module, but will confuse other things when response need to be processed further
		// however, "calculate CRC" request flag is part of CRC itself, so we also can't remove it earlier
		if ((_commonBuffer[0] & COMMON_BUFFER_ONE_BYTE_RESPONSE_BIT_FLAG) == 0 &&
		(_commonBuffer[0] & COMMON_BUFFER_RESPONSE_CALCULATE_CRC_BIT_FLAG) != 0)
		{
			_commonBuffer[0] &= COMMON_BUFFER_RESPONSE_PACKET_LENGTH_BIT_MASK;
		}
	}

	_commonBufferWriteIndex = 0;
	_commonBufferReadIndex = 0;

	return true;
}

uint8_t HUB_DetectNewSlaves()
{
	bool result = HUB_SendCommandToAllWithExpectedResponse(Commands::CNEW);
	if (!result)
	{
		return 255;
	}

	uint8_t response = ReadOneByteResponseFromCommonBuffer();
	if (response > COMMON_BUFFER_ONE_BYTE_RESPONSE_CONTENT_BIT_MASK)
	{
		// error occurred
		return 255;
	}

	return response == 127 ? 0 : 1;
}

uint8_t HUB_DetectSlaves(uint32_t * foundSlaveAddresses, uint8_t maxSlavesToReturn)
{
	uint8_t slavesFound = 0;
	uint8_t maxIterations = 200;
	uint32_t wdLastFoundSlaveId = 0;

	// wake-up all slaves first
	// as some sleeping slaves will prevent full enumeration
	HUB_SendCommandToAll(Commands::CWUP);

	for (;;)
	{
		BeginAgain:

		// use greedy pattern first
		bool result = HUB_SendCommandToAllWithExpectedResponse(Commands::CREQ, 0);
		if (!result)
		{
			if (wdLastFoundSlaveId == 0)
			{
				return 255;
			}

			break;
		}

		uint8_t responseLength = ReadNextByteFromCommonBuffer();
		if (responseLength != 8)
		{
			if (wdLastFoundSlaveId == 0)
			{
				return 255;
			}

			break;
		}

		uint32_t idNormal = ReadNextWordFromCommonBuffer();
		uint32_t idInvert = ReadNextWordFromCommonBuffer();
		uint32_t idVarial = idNormal ^ idInvert ^ 0xffffffff;

		if (idVarial == 0xffffffff)
		{
			return 255;
		}

		if (idVarial == 0)
		{
			foundSlaveAddresses[slavesFound++] = idNormal;
			return slavesFound;
		}

		uint8_t usedIdBitsCount = 0;
		uint32_t maxPossibleSlaveId = 1;

		// allocate as much space as potentially can be used
		uint32_t usedBitsToInts[32];

		if (idVarial != 0)
		{
			for (uint8_t i = 0; i < 32; i++)
			{
				if (((idVarial >> i) & 1) == 1)
				{
					usedBitsToInts[usedIdBitsCount++] = 1 << i;
				}
			}

			maxPossibleSlaveId = 1 << usedIdBitsCount;
		}

		// this prevents eternal loop when no slave responded, which happens when protocol fails
		uint8_t protocolFailurePrevention = 0;

		for (uint32_t i = 1; i <= maxPossibleSlaveId; i++)
		{
			if (protocolFailurePrevention++ == 32)
			{
				maxIterations = 0;
				break;
			}

			// iterate only when more that one slave responded
			if (idVarial != 0)
			{
				uint32_t slaveMatchTemplate = 0;

				for (uint8_t s = 0; s < usedIdBitsCount; s++)
				{
					if (((i >> s) & 1) == 1)
					{
						slaveMatchTemplate |= usedBitsToInts[s];
					}
				}

				// create detailed pattern
				result = HUB_SendCommandToAllWithExpectedResponse(Commands::CREQ, slaveMatchTemplate);
				if (!result)
				{
					continue;
				}

				responseLength = ReadNextByteFromCommonBuffer();
				if (responseLength != 8)
				{
					continue;
				}

				idNormal = ReadNextWordFromCommonBuffer();
				idInvert = ReadNextWordFromCommonBuffer();
				idVarial = idNormal ^ idInvert ^ 0xffffffff;

				if (idVarial == 0xffffffff)
				{
					maxIterations = 0;
					break;
				}

				protocolFailurePrevention = 0;
			}

			// check if no bit-collision detected
			// this would mean that we've found an actual slave ID
			if (idVarial == 0)
			{
				// check if we found the same slave
				// this will indicate an issue in slave suspension
				if (idNormal == wdLastFoundSlaveId)
				{
					// we can't recover from this, so
					// terminate detection loop
					maxIterations = 0;
					break;
				}

				// add found slave ID to the list
				foundSlaveAddresses[slavesFound++] = idNormal;
				if (slavesFound >= maxSlavesToReturn)
				{
					break;
				}

				// suspend found slave so it can no longer contribute into list of IDs
				HUB_SendCommand(idNormal, Commands::CSLP);

				// reset collision detection flag
				idVarial = 0xffffffff;

				// arm watch dog
				wdLastFoundSlaveId = idNormal;

				// restart from the beginning
				goto BeginAgain;
			}
		}

		// prevent endless loop
		if (maxIterations > 0)
		{
			maxIterations--;
			continue;
		}

		break;
	}

	// wake-up all slaves
	HUB_SendCommandToAll(Commands::CWUP);

	return slavesFound;
}

uint8_t HUB_DetectSlavesSequence(uint32_t * foundSlaveAddresses, uint8_t maxSlavesToReturn)
{
	// create test packet
	const uint8_t payloadLength = 4;
	uint8_t payload[payloadLength];

	payload[0] = 0b00000010;
	payload[1] = 0b00001010;
	payload[2] = 0b00101010;
	payload[3] = 0b10101010;

	// wake-up all slaves first
	// as some sleeping slaves will prevent full enumeration
	HUB_SendCommandToAll(Commands::CWUP);

	// send packet to first connected module
	bool result = HUB_SendUplinkPacket(payload, payloadLength);
	if (!result)
	{
		return 255;
	}

	uint8_t slavesFound = 0;

	for (uint8_t i = 0; i < 255; i++)
	{
		result = HUB_SendCommandToAllWithExpectedResponse(Commands::UCCI, payload, payloadLength);
		if (!result)
		{
			if (i == 0)
			{
				return 255;
			}

			break;
		}

		uint8_t responseLength = ReadNextByteFromCommonBuffer();
		if (responseLength != 8)
		{
			if (i == 0)
			{
				return 255;
			}

			break;
		}

		uint32_t idNormal = ReadNextWordFromCommonBuffer();
		uint32_t idInvert = ReadNextWordFromCommonBuffer();
		uint32_t idVarial = idNormal ^ idInvert ^ 0xffffffff;

		if (idVarial != 0)
		{
			if (i == 0 || idVarial == 0xffffffff)
			{
				return 255;
			}

			break;
		}

		// add found slave ID to the list
		foundSlaveAddresses[slavesFound++] = idNormal;
		if (slavesFound >= maxSlavesToReturn)
		{
			break;
		}

		// move test packet from input to output uplink buffer within the same module
		result = HUB_SendCommand(idNormal, Commands::UMOV);
		if (!result)
		{
			if (i == 0)
			{
				return 255;
			}

			break;
		}

		// pump test packet to the next module
		result = HUB_SendUplinkPumpPacket();
		if (!result)
		{
			if (i == 0)
			{
				return 255;
			}

			break;
		}
	}

	return slavesFound;
}

bool SendCommonCommandPart(uint8_t commandType, uint32_t slave, uint32_t command, uint8_t * payload, uint8_t payloadLength)
{
	_commonBufferWriteIndex = 0;
	_commonBufferReadIndex = 0;

	WriteNextByteToCommonBuffer(commandType);
	WriteNextWordToCommonBuffer(slave);

	// command always takes 4 bytes
	uint8_t commandLength = 4;
	if (payload != NULL && payloadLength > 0)
	{
		commandLength += payloadLength;
	}

	WriteNextByteToCommonBuffer(commandLength);
	WriteNextWordToCommonBuffer(command);

	if (payload != NULL && payloadLength > 0)
	{
		WriteNextBytesToCommonBuffer(payload, payloadLength);
	}

	uint8_t crc = CRC_CalculateBufferCrc(_commonBuffer, _commonBufferWriteIndex);
	WriteNextByteToCommonBuffer(crc);

	if (SendPacketStart())
	{
		return SendEntireBuffer(commandType == CommonCommandTypes::FireAndForget, false);
	}
	
	return false;
}

bool SendEntireBuffer(bool terminate, bool transmitUplinkData)
{
	if (transmitUplinkData)
	{
		CFG_LNK_UOUT_OUT;
	}

	while (_commonBufferReadIndex < _commonBufferWriteIndex)
	{
		_commonBufferTransceiverBitQueue = _commonBuffer[_commonBufferReadIndex];

		if (_commonBufferReadIndex > 0 && _commonBufferReadIndex <= UPLINK_BUFFER_SIZE_IN_BYTES)
		{
			_uplinkBufferTransmitterBitQueue = _uplinkBuffer[_commonBufferReadIndex - 1];
		}
		else
		{
			_uplinkBufferTransmitterBitQueue = 0;
		}

		// configure DAT pin for output, but set CLK to LO first,
		// so that DAT transition is valid
		SET_PAD_CLCK_LO;
		CFG_PAD_DATA_OUT;
		SET_LNK_LO;

		for (uint8_t i = 0; i < 8; i++)
		{
			DelayForSyncTest();
			SET_PAD_CLCK_LO;
			DelayForAsync(HubDelays::SendBitClckLo);

			DelayForSyncTest();
			if ((_commonBufferTransceiverBitQueue & MSB_BIT_MASK) > 0)
			{
				SET_PAD_DATA_HI;
			}
			else
			{
				SET_PAD_DATA_LO;
			}

			if (transmitUplinkData == true)
			{
				if ((_uplinkBufferTransmitterBitQueue & MSB_BIT_MASK) > 0)
				{
					SET_LNK_HI;
				}
				else
				{
					SET_LNK_LO;
				}
			}

			DelayForAsync(HubDelays::SendBitData);

			DelayForSyncTest();
			SET_PAD_CLCK_HI;
			DelayForAsync(HubDelays::SendBitClckHi);

			_commonBufferTransceiverBitQueue = _commonBufferTransceiverBitQueue << 1;
			_uplinkBufferTransmitterBitQueue = _uplinkBufferTransmitterBitQueue << 1;
		}

		_commonBufferReadIndex++;

		// now we append extra 9th bit
		DelayForSyncTest();
		SET_PAD_CLCK_LO;
		DelayForAsync(HubDelays::SendBit9ClckLo);

		// we set 9th DATA bit to Z state, so that slave can drive it low acknowledging byte receive
		DelayForSyncTest();
		SET_PAD_DATA_HI;
		CFG_PAD_DATA_INP;

		if (transmitUplinkData == true)
		{
			SET_LNK_LO;
		}

		DelayForAsync(HubDelays::SendBit9Data);

		// get state of DATA pin
		// check if any slave has driven DATA low
		// at least one must do that to acknowledge byte receive
		//if (PinData.GetState() == OpenDrainPinState.Hi)
		if (GET_PAD_DATA_HI)
		{
			// no slave responded
			// stop sending data
			DelayForSyncTest();
			CFG_PAD_DATA_OUT;
			SET_PAD_DATA_LO;
			DelayForAsync(HubDelays::SendAbandonDataLo);

			DelayForSyncTest();
			SET_PAD_CLCK_HI;
			DelayForAsync(HubDelays::SendAbandonClckHi);

			return false;
		}

		if (_commonBufferReadIndex == _commonBufferWriteIndex)
		{
			DelayForSyncTest();
			if (terminate)
			{
				// we need to set DATA pin Lo, so we can always rise DATA pin, in order to indicate end of data packet
				// if actual 8-bit aligned data ends with 1 (Hi level), we can't rise DATA pin as it's already Hi
				// so, we append unconditional extra bit, which is always 0 (zero)
				CFG_PAD_DATA_OUT;
				SET_PAD_DATA_LO;
			}
			else
			{
				// we need to set DATA pin to Z state, as we expect slave to send some response
				// in this case we need to release DATA pin so that slave can start driving it
				SET_PAD_DATA_HI;
				CFG_PAD_DATA_INP;
			}

			DelayForAsync(HubDelays::SendLastByteTerminate);
		}

		DelayForSyncTest();
		SET_PAD_CLCK_HI;
		DelayForAsync(HubDelays::SendBit9ClckHi);
	}

	return true;
}

bool SendPacketStart()
{
	//      XXX+-------------
	//         | HI
	// CLCK XXX+
	//
	//      XXXXXX+-----+
	//            | ??? | LO
	// DATA XXXXXX+     +-----

	// 1. Rise CLCK to HI
	// 2. Configure DATA as input, with pull-up it will go HI
	// 3. Read DATA state
	// 4. Make sure that DATA is HI, if it is LO then it means one of the modules is driving it LO, which is wrong
	// 5. Pull DATA to LO, this will indicate start of the packet

	DelayForSyncTest();
	SET_PAD_CLCK_HI;
	DelayForAsync(HubDelays::PacketStartClckHi);

	DelayForSyncTest();
	SET_PAD_DATA_HI;
	CFG_PAD_DATA_INP;
	DelayForAsync(HubDelays::PacketStartDataHi);

	if (GET_PAD_DATA_LO)
	{
		ERRLOG_LogError(Errors::HubCommonInterfaceDataPinDrivenLowOnPacketStart);

		for (uint8_t i = 8; i >= 0; i--)
		{
			SendPacketEnd();

			if (i == 0)
			{
				return false;
			}

			DelayForSyncTest();
			SET_PAD_DATA_HI;
			CFG_PAD_DATA_INP;
			DelayForAsync(HubDelays::PacketStartDataHi);

			if (!GET_PAD_DATA_LO)
			{
				break;
			}
		}
	}

	DelayForSyncTest();
	CFG_PAD_DATA_OUT;
	SET_PAD_DATA_LO;
	DelayForAsync(HubDelays::PacketStartDataLo);

	return true;
}

bool SendPacketEnd()
{
	//      -----------------
	//
	// CLCK
	//
	//           +-----------
	//           | HI
	// DATA -----+

	// 1. CLCK must be HI and must remain HI
	// 2. Configure DATA as input, with pull-up it will go HI, this will indicate end of the packet

	// CLCK must be already HI, report an error if not
	if (ASSERT_PAD_CLCK_LO)
	{
		ERRLOG_LogError(Errors::HubCommonInterfaceClckPinIsLowOnPacketEnd);
	}

	// rise CLCK to HI as this what it has to be at the end of packet
	DelayForSyncTest();
	SET_PAD_CLCK_HI;
	DelayForAsync(HubDelays::PacketEndClckHi);

	DelayForSyncTest();
	SET_PAD_DATA_HI;
	CFG_PAD_DATA_INP;
	DelayForAsync(HubDelays::PacketEndDataHi);

	// nothing should drive DATA to LO when packet ends, report an error if not
	if (GET_PAD_DATA_LO)
	{
		ERRLOG_LogError(Errors::HubCommonInterfaceDataPinDrivenLowOnPacketEnd);
		return false;
	}

	return true;
}

uint8_t ReadOneByteResponseFromCommonBuffer()
{
	uint8_t v = _commonBuffer[0];

	if (v < COMMON_BUFFER_ONE_BYTE_RESPONSE_BIT_FLAG)
	{
		return Errors::InvalidOneByteResponse;
	}

	return v & COMMON_BUFFER_ONE_BYTE_RESPONSE_CONTENT_BIT_MASK;
}

uint8_t ReadNextByteFromCommonBuffer()
{
	return _commonBuffer[_commonBufferReadIndex++];
}

uint32_t ReadNextWordFromCommonBuffer()
{
	uint32_t word = _commonBuffer[_commonBufferReadIndex++];

	word = (word << 8) | _commonBuffer[_commonBufferReadIndex++];
	word = (word << 8) | _commonBuffer[_commonBufferReadIndex++];
	word = (word << 8) | _commonBuffer[_commonBufferReadIndex++];

	return word;
}

void WriteNextBytesToCommonBuffer(uint8_t * v, uint8_t length)
{
	if (_commonBufferWriteIndex + length >= COMMON_BUFFER_SIZE_IN_BYTES)
	{
		ERRLOG_LogError(Errors::HubCommonBufferOnWriteByteOverflow);
		return;
	}

	for (uint8_t i = 0; i < length; i++)
	{
		_commonBuffer[_commonBufferWriteIndex++] = v[i];
	}
}

void WriteNextByteToCommonBuffer(uint8_t v)
{
	if (_commonBufferWriteIndex >= COMMON_BUFFER_SIZE_IN_BYTES)
	{
		ERRLOG_LogError(Errors::HubCommonBufferOnWriteByteOverflow);
		return;
	}

	_commonBuffer[_commonBufferWriteIndex++] = v;
}

void WriteNextWordToCommonBuffer(uint32_t v)
{
	if (_commonBufferWriteIndex + 4 >= COMMON_BUFFER_SIZE_IN_BYTES)
	{
		ERRLOG_LogError(Errors::HubCommonBufferOnWriteWordOverflow);
		return;
	}

	_commonBuffer[_commonBufferWriteIndex++] = (v >> 24) & 0xff;
	_commonBuffer[_commonBufferWriteIndex++] = (v >> 16) & 0xff;
	_commonBuffer[_commonBufferWriteIndex++] = (v >> 8) & 0xff;
	_commonBuffer[_commonBufferWriteIndex++] = v & 0xff;
}

void WriteNextByteToUplinkBuffer(uint8_t uplinkBufferValue, uint8_t commonBufferValue)
{
	if (_commonBufferWriteIndex >= COMMON_BUFFER_SIZE_IN_BYTES ||
	_commonBufferWriteIndex > UPLINK_BUFFER_SIZE_IN_BYTES)
	{
		ERRLOG_LogError(Errors::HubUplinkBufferOnWriteByteOverflow);
		return;
	}

	_commonBuffer[_commonBufferWriteIndex] = commonBufferValue;

	if (_commonBufferWriteIndex > 0)
	{
		_uplinkBuffer[_commonBufferWriteIndex - 1] = uplinkBufferValue;
	}

	_commonBufferWriteIndex++;
}

void DelayForSyncTest()
{
#ifdef SYNCHRON
	// reset 16-bit profiling Timer 1 only when in synchronous mode
	// when in release mode the entire function body is omitted, and compiler removes all calls into this function during optimization
	TCNT1 = 0;
	__builtin_avr_delay_cycles(1);
#endif
}

void DelayForAsync(uint8_t delayId)
{
#ifdef SYNCHRON
	__builtin_avr_delay_cycles(1);
	uint16_t timenow = TCNT1;
	if (timenow <= 0xff)
	{
		if (_delays[delayId] < timenow)
		{
			_delays[delayId] = timenow;
		}
	}
	else
	{
		_delays[delayId] = 0;
	}
#else
	for (uint8_t i = 0; i < _delays[delayId]; i++)
	{
		_delay_us(1);
	}
#endif
}
<file_sep>/mod_interface.cpp
#include <stdlib.h>
#include <stdint.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <avr/eeprom.h>

#include "common.h"
#include "errors.h"
#include "crc.h"
#include "commands.h"
#include "error_logger.h"
#include "wdt.h"
#include "device.h"
#include "mod_interface.h"

#include "usart.h"

// port assignment for physical connection pins
#define PAD_PORT_CFG		DDRC
#define PAD_PORT_INP		PINC
#define PAD_PORT_OUT		PORTC

// uplink pins - two pins needed (i.e. uplink signal in and out)
#define LNK_PORT_CFG		DDRC
#define LNK_PORT_INP		PINC
#define LNK_PORT_OUT		PORTC
#define LNK_UINP			PINC4
#define LNK_UOUT			PORTC5

// all available pins for physical connection
#define INPUT_PAD_1			PINC0
#define INPUT_PAD_2			PINC1
#define INPUT_PAD_3			PINC2
#define INPUT_PAD_4			PINC3

// configures pin for output or input
#define CFG_PAD_OUT(pad)	PAD_PORT_CFG |=  (1 << pad)
#define CFG_PAD_INP(pad)	PAD_PORT_CFG &= ~(1 << pad)

// set pin state template
#define SET_PAD_HI(pad)		PAD_PORT_OUT |=  (1 << pad)
#define SET_PAD_LO(pad)		PAD_PORT_OUT &= ~(1 << pad)

// get pin state template
#define GET_PAD_HI(pad)		((PAD_PORT_INP & (1 << pad)) != 0)
#define GET_PAD_LO(pad)		((PAD_PORT_INP & (1 << pad)) == 0)

// gets state of UPLINK input pin
#define GET_LNK_HI			((LNK_PORT_INP & (1 << LNK_UINP)) != 0)
#define GET_LNK_LO			((LNK_PORT_INP & (1 << LNK_UINP)) == 0)

// sets state of UPLINK output pin either HI or LO
#define SET_LNK_HI			LNK_PORT_OUT |=  (1 << LNK_UOUT)
#define SET_LNK_LO			LNK_PORT_OUT &= ~(1 << LNK_UOUT)

// asserts CLCK pin output state is set as expected
#define ASSERT_PAD_HI(pad)	((PAD_PORT_OUT & (1 << pad)) != 0)
#define ASSERT_PAD_LO(pad)	((PAD_PORT_OUT & (1 << pad)) == 0)

static const uint8_t MAX_POWER_FLIPS_FOR_RESET						= 190;

static const uint8_t POWER_DETECTION_MIN_STATE_CHANGES				= 20;
static const uint8_t POWER_DETECTION_MAX_POWER_FLIPS				= 3;
static const uint8_t POWER_DETECTION_MIN_STATE_FLIPS				= 8;
static const uint8_t CLOCK_DETECTION_MIN_STATE_CHANGES				= 20;
static const uint8_t CLOCK_DETECTION_MIN_DATA_FLIPS					= 2;

static const uint8_t VERSION_MAJOR									= 7;
static const uint8_t VERSION_MINOR									= 11;

static const uint8_t UPLINK_ACTIVATED_BIT_FLAG						= 0b10000000;
static const uint8_t UPLINK_PACKET_LENGTH_BIT_MASK					= 0b00111111;

static const uint8_t COMMON_BUFFER_MODE_INITIAL						= 0;
static const uint8_t COMMON_BUFFER_MODE_RECEIVE						= 1;
static const uint8_t COMMON_BUFFER_MODE_BEFORE_RESPOND				= 2;
static const uint8_t COMMON_BUFFER_MODE_RESPOND						= 3;
static const uint8_t COMMON_BUFFER_MODE_BEFORE_TERMINATE			= 4;

static const uint8_t COMMON_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES	= 6;   // 1 byte command ID + 4 byte slave ID + 1 byte CRC
static const uint8_t COMMON_COMMAND_MAX_PAYLOAD_LENGTH_IN_BYTES		= COMMON_BUFFER_SIZE_IN_BYTES - (COMMON_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES + 4);
static const uint8_t UPLINK_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES	= 2;   // 1 byte command ID and length + 1 byte CRC
static const uint8_t UPLINK_COMMAND_MAX_PAYLOAD_LENGTH_IN_BYTES		= UPLINK_BUFFER_SIZE_IN_BYTES - UPLINK_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES;

static uint32_t _address = 0x01020304;

static uint8_t PAD_CLCK = 0xff;
static uint8_t PAD_DATA = 0xff;
static uint8_t PAD_VCC = 0xff;
static uint8_t PAD_GND = 0xff;

static uint8_t _resetOnPowerChangeCounter = 0;
static uint8_t _directionDetectionState = 0xff;
static uint8_t _p1DetectionCounter = 0;
static uint8_t _p2DetectionCounter = 0;
static uint8_t _p3DetectionCounter = 0;
static uint8_t _p4DetectionCounter = 0;

static bool _isDiscovered = false;
static bool _isControllerSuspended = false;
static bool _isDeviceSuspended = false;

static uint8_t _commonBuffer[COMMON_BUFFER_SIZE_IN_BYTES];
static uint8_t _commonBufferWriteIndex;
static uint8_t _commonBufferReadIndex;
static uint8_t _commonBufferTransmitReciveMode;
static uint8_t _commonBufferTransceiverBitQueue;
static uint8_t _commonBufferTransceiverBitCounter;

static uint8_t _expectedCommandLengthIncludingPayload;

static uint8_t _uplinkBufferInp[UPLINK_BUFFER_SIZE_IN_BYTES];
static uint8_t _uplinkBufferOut[UPLINK_BUFFER_SIZE_IN_BYTES];
static uint8_t _uplinkBufferModeAndIndex;
static uint8_t _uplinkBufferReceiverBitQueue;
static uint8_t _uplinkBufferTransmitterBitQueue;

static bool CheckCanHandleCommand();
static bool ProcessCommand();
static uint8_t ReadNextByteFromCommonBuffer();
static uint32_t ReadNextWordFromCommonBuffer();
static void WriteResponseHeaderByteToCommonBuffer(uint8_t length, bool calculateCrc);
static void WriteOneByteResponseToCommonBuffer(uint8_t v);
static void WriteNextByteToCommonBuffer(uint8_t v);
static void WriteNextWordToCommonBuffer(uint32_t v);
static void ClearUplinkInpBuffer();
static void ClearUplinkOutBuffer();
static void SendDataToDevice();
static void ResetController();
static void ResetDevice();
static uint8_t GetPinBitIndexInInputPort(uint8_t pinIndex);

void MOD_MonitorConnectionHealth()
{
	if (_directionDetectionState != 0)
	{
		return;
	}

	if (GET_PAD_HI(PAD_GND) || GET_PAD_LO(PAD_VCC))
	{
		if (++_resetOnPowerChangeCounter > MAX_POWER_FLIPS_FOR_RESET)
		{
			ResetController();
			return;
		}
	}
}

void MOD_PadPortStateChanged(uint8_t currentPortState, uint8_t previousPortState)
{
	uint8_t change = currentPortState ^ previousPortState;

    if (_directionDetectionState != 0)
    {
		bool p1s = (currentPortState & (1 << INPUT_PAD_1)) != 0;
		bool p2s = (currentPortState & (1 << INPUT_PAD_2)) != 0;
		bool p3s = (currentPortState & (1 << INPUT_PAD_3)) != 0;
		bool p4s = (currentPortState & (1 << INPUT_PAD_4)) != 0;

        // detect GND pin
        if (_directionDetectionState == 0xff)
        {
	        _p1DetectionCounter = _p2DetectionCounter = _p3DetectionCounter = _p4DetectionCounter = 0;

	        if (!p1s && p2s && p3s && p4s)
	        {
		        _directionDetectionState = 1;
	        }
	        else if (p1s && !p2s && p3s && p4s)
	        {
		        _directionDetectionState = 2;
	        }
	        else if (p1s && p2s && !p3s && p4s)
	        {
		        _directionDetectionState = 3;
	        }
	        else if (p1s && p2s && p3s && !p4s)
	        {
		        _directionDetectionState = 4;
	        }
        }

		// detect VCC pin
		if (_directionDetectionState >= 1 && _directionDetectionState <= 4)
		{
			if (change & (1 << INPUT_PAD_1)) _p1DetectionCounter++;
			if (change & (1 << INPUT_PAD_2)) _p2DetectionCounter++;
			if (change & (1 << INPUT_PAD_3)) _p3DetectionCounter++;
			if (change & (1 << INPUT_PAD_4)) _p4DetectionCounter++;

			if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_CHANGES ||
				_p2DetectionCounter > POWER_DETECTION_MIN_STATE_CHANGES ||
				_p3DetectionCounter > POWER_DETECTION_MIN_STATE_CHANGES ||
				_p4DetectionCounter > POWER_DETECTION_MIN_STATE_CHANGES)
			{
				switch (_directionDetectionState)
				{
					case 1:
						if (_p1DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
						{
							if (_p2DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 12;
							}
							else if (_p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p3DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 13;
							}
							else if (_p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p4DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
							{
								_directionDetectionState = 14;
							}
						}
						break;

					case 2:
						if (_p2DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
						{
							if (_p1DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 21;
							}
							else if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p3DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 23;
							}
							else if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p4DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
							{
								_directionDetectionState = 24;
							}
						}
						break;

					case 3:
						if (_p3DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
						{
							if (_p1DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 31;
							}
							else if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p2DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 32;
							}
							else if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p4DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
							{
								_directionDetectionState = 34;
							}
						}
						break;

					case 4:
						if (_p4DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
						{
							if (_p1DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 41;
							}
							else if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p2DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS && _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS)
							{
								_directionDetectionState = 42;
							}
							else if (_p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS && _p3DetectionCounter < POWER_DETECTION_MAX_POWER_FLIPS)
							{
								_directionDetectionState = 43;
							}
						}
						break;
				}

				if (_directionDetectionState > 4)
				{
					_p1DetectionCounter = _p1DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS ? 1 : 0;
					_p2DetectionCounter = _p2DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS ? 1 : 0;
					_p3DetectionCounter = _p3DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS ? 1 : 0;
					_p4DetectionCounter = _p4DetectionCounter > POWER_DETECTION_MIN_STATE_FLIPS ? 1 : 0;
				}
				else
				{
					_directionDetectionState = 0xff;
				}
			}

			return;
		}
		
		// detect CLK and DAT pins
		if (_directionDetectionState < 44)
		{
			// detect if pin #1 need to be monitored + current change is on pin #1
			if (_p1DetectionCounter > 0 && (change & (1 << INPUT_PAD_1)))
			{
				// check that pin #1 has just transitioned while other monitored pins are Hi at this moment
				if ((_p2DetectionCounter > 0 && p2s) || (_p3DetectionCounter > 0 && p3s) || (_p4DetectionCounter > 0 && p4s))
				{
					if (_p1DetectionCounter < 255)
					{
						_p1DetectionCounter++;
					}
				}
			}

			// detect if pin #2 need to be monitored + current change is on pin #2
			if (_p2DetectionCounter > 0 && (change & (1 << INPUT_PAD_2)))
			{
				// check that pin #2 has just transitioned while other monitored pins are Hi at this moment
				if ((_p1DetectionCounter > 0 && p1s) || (_p3DetectionCounter > 0 && p3s) || (_p4DetectionCounter > 0 && p4s))
				{
					if (_p2DetectionCounter < 255)
					{
						_p2DetectionCounter++;
					}
				}
			}

			// detect if pin #3 need to be monitored + current change is on pin #3
			if (_p3DetectionCounter > 0 && (change & (1 << INPUT_PAD_3)))
			{
				// check that pin #3 has just transitioned while other monitored pins are Hi at this moment
				if ((_p1DetectionCounter > 0 && p1s) || (_p2DetectionCounter > 0 && p2s) || (_p4DetectionCounter > 0 && p4s))
				{
					if (_p3DetectionCounter < 255)
					{
						_p3DetectionCounter++;
					}
				}
			}

			// detect if pin #4 need to be monitored + current change is on pin #4
			if (_p4DetectionCounter > 0 && (change & (1 << INPUT_PAD_4)))
			{
				// check that pin #4 has just transitioned while other monitored pins are Hi at this moment
				if ((_p1DetectionCounter > 0 && p1s) || (_p2DetectionCounter > 0 && p2s) || (_p3DetectionCounter > 0 && p3s))
				{
					if (_p4DetectionCounter < 255)
					{
						_p4DetectionCounter++;
					}
				}
			}

			// detecting clock and data pins
			if (_p1DetectionCounter > CLOCK_DETECTION_MIN_STATE_CHANGES ||
				_p2DetectionCounter > CLOCK_DETECTION_MIN_STATE_CHANGES ||
				_p3DetectionCounter > CLOCK_DETECTION_MIN_STATE_CHANGES ||
				_p4DetectionCounter > CLOCK_DETECTION_MIN_STATE_CHANGES)
			{
				if ((_p1DetectionCounter == 0 || _p1DetectionCounter > CLOCK_DETECTION_MIN_DATA_FLIPS) && (_p2DetectionCounter == 0 || _p2DetectionCounter > CLOCK_DETECTION_MIN_DATA_FLIPS) && (_p3DetectionCounter == 0 || _p3DetectionCounter > CLOCK_DETECTION_MIN_DATA_FLIPS) && (_p4DetectionCounter == 0 || _p4DetectionCounter > CLOCK_DETECTION_MIN_DATA_FLIPS))
				{
					// at this point GND and VCC pin indices are encoded in decimal format as following:
					//     |                        | GND
					//     | 00 | 01 | 02 | 03 | 04 | n/a
					//     | 10 | 11 | 12 | 13 | 14 |  1
					//     | 20 | 21 | 22 | 23 | 24 |  2
					//     | 30 | 31 | 32 | 33 | 34 |  3
					//     | 40 | 41 | 42 | 43 | 44 |  4
					// ----|----|----|----|----|----|----
					// VCC | n/a|  1 |  2 |  3 |  4 |
					uint8_t gndPinIndex = 0;
					uint8_t vccPinIndex = 0;
					uint8_t clkPinIndex = 0;
					uint8_t datPinIndex = 0;

					if (_directionDetectionState > 40 && _directionDetectionState < 50)
					{
						gndPinIndex = 4;
						vccPinIndex = _directionDetectionState - 40;
					}
					else if (_directionDetectionState > 30 && _directionDetectionState < 40)
					{
						gndPinIndex = 3;
						vccPinIndex = _directionDetectionState - 30;
					}
					else if (_directionDetectionState > 20 && _directionDetectionState < 30)
					{
						gndPinIndex = 2;
						vccPinIndex = _directionDetectionState - 20;
					}
					else if (_directionDetectionState > 10 && _directionDetectionState < 20)
					{
						gndPinIndex = 1;
						vccPinIndex = _directionDetectionState - 10;
					}

					// out of which:
					// only few will be a DATA transitioning when CLK is Hi, which indicates command start
					// the majority will be CLK transitioning Hi->Lo when DATA is Hi
					if ((gndPinIndex == 1 && vccPinIndex == 2) || (gndPinIndex == 2 && vccPinIndex == 1))
					{
						if (_p3DetectionCounter < _p4DetectionCounter)
						{
							// pin #3 is DATA
							datPinIndex = 3;
							clkPinIndex = 4;
						}
						else
						{
							// pin #4 is DATA
							datPinIndex = 4;
							clkPinIndex = 3;
						}
					}
					else if ((gndPinIndex == 1 && vccPinIndex == 3) || (gndPinIndex == 3 && vccPinIndex == 1))
					{
						if (_p2DetectionCounter < _p4DetectionCounter)
						{
							// pin #2 is DATA
							datPinIndex = 2;
							clkPinIndex = 4;
						}
						else
						{
							// pin #4 is DATA
							datPinIndex = 4;
							clkPinIndex = 2;
						}
					}
					else if ((gndPinIndex == 1 && vccPinIndex == 4) || (gndPinIndex == 4 && vccPinIndex == 1))
					{
						if (_p2DetectionCounter < _p3DetectionCounter)
						{
							// pin #2 is DATA
							datPinIndex = 2;
							clkPinIndex = 3;
						}
						else
						{
							// pin #3 is DATA
							datPinIndex = 3;
							clkPinIndex = 2;
						}
					}
					else if ((gndPinIndex == 2 && vccPinIndex == 3) || (gndPinIndex == 3 && vccPinIndex == 2))
					{
						if (_p1DetectionCounter < _p4DetectionCounter)
						{
							// pin #1 is DATA
							datPinIndex = 1;
							clkPinIndex = 4;
						}
						else
						{
							// pin #4 is DATA
							datPinIndex = 4;
							clkPinIndex = 1;
						}
					}
					else if ((gndPinIndex == 2 && vccPinIndex == 4) || (gndPinIndex == 4 && vccPinIndex == 2))
					{
						if (_p1DetectionCounter < _p3DetectionCounter)
						{
							// pin #1 is DATA
							datPinIndex = 1;
							clkPinIndex = 3;
						}
						else
						{
							// pin #3 is DATA
							datPinIndex = 3;
							clkPinIndex = 1;
						}
					}
					else if ((gndPinIndex == 3 && vccPinIndex == 4) || (gndPinIndex == 4 && vccPinIndex == 3))
					{
						if (_p1DetectionCounter < _p2DetectionCounter)
						{
							// pin #1 is DATA
							datPinIndex = 1;
							clkPinIndex = 2;
						}
						else
						{
							// pin #2 is DATA
							datPinIndex = 2;
							clkPinIndex = 1;
						}
					}

					PAD_CLCK = GetPinBitIndexInInputPort(clkPinIndex);
					PAD_DATA = GetPinBitIndexInInputPort(datPinIndex);
					PAD_GND = GetPinBitIndexInInputPort(gndPinIndex);
					PAD_VCC = GetPinBitIndexInInputPort(vccPinIndex);

					if (PAD_CLCK > 7 || PAD_DATA > 7 || PAD_GND > 7 || PAD_VCC > 7)
					{
						_directionDetectionState = 0xff;
						return;
					}

					_directionDetectionState = 0xf0;
				}
			}
		}

		// all pins detected, wait for current packet end
		if (_directionDetectionState == 0xf0)
		{
			// packet end is indicated by Lo=>Hi transition of DAT pin
			// while CLK pin is Hi
			if (change & (1 << PAD_DATA))
			{
				bool dataPinState = (currentPortState & (1 << PAD_DATA)) != 0;
				bool clckPinState = (currentPortState & (1 << PAD_CLCK)) != 0;

				if (dataPinState && clckPinState)
				{
					_resetOnPowerChangeCounter = 0;
					_directionDetectionState = 0;
					_commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_INITIAL;
					goto main;
				}
			}

			return;
		}

		return;
	}

	if (change & (1 << PAD_VCC) || change & (1 << PAD_GND))
	{
		if (++_resetOnPowerChangeCounter > MAX_POWER_FLIPS_FOR_RESET)
		{
			ResetController();
			return;
		}
	}

	main:

	bool dataPinState = (currentPortState & (1 << PAD_DATA)) != 0;
	bool clckPinState = (currentPortState & (1 << PAD_CLCK)) != 0;

    // check for transition of DAT pin
    if (change & (1 << PAD_DATA))
    {
	    // check for CLK to be Hi, as transition of DAT pin is only valid when CLK is Hi
	    if (clckPinState)
	    {
		    // check for packet "start"
		    // "start" is indicated by Hi=>Lo transition of DAT pin
		    if (!dataPinState)
		    {
			    _commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_RECEIVE;
			    _commonBufferTransceiverBitQueue = 0;
			    _commonBufferTransceiverBitCounter = 0;
			    _commonBufferWriteIndex = 0;

			    _uplinkBufferModeAndIndex = 0;
			    _uplinkBufferReceiverBitQueue = 0;
			    _uplinkBufferTransmitterBitQueue = 0;

			    _expectedCommandLengthIncludingPayload = 0xff;
			    //_datPinDriveState = OpenDrainPinState.Z;
				SET_PAD_HI(PAD_DATA);
				CFG_PAD_INP(PAD_DATA);

				return;
		    }

		    // check for packet "end"
		    // "end" is indicated by Lo=>Hi transition of DAT pin
		    if (dataPinState)
		    {
			    // check if any data received and packet didn't overflow
			    if (_commonBufferWriteIndex > 0 && _commonBufferTransmitReciveMode == COMMON_BUFFER_MODE_RECEIVE)
			    {
				    //decrement receive buffer index as a new char will not be received
				    _commonBufferWriteIndex--;

				    // process command if it looks like a command
				    if (_commonBuffer[0] == CommonCommandTypes::FireAndForget)
				    {
					    ProcessCommand();
				    }
			    }

			    _commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_INITIAL;
			    _commonBufferTransceiverBitQueue = 0;
			    _commonBufferTransceiverBitCounter = 0;
			    _commonBufferWriteIndex = 0;

			    // process data in uplink buffer
			    if (_uplinkBufferModeAndIndex != 0)
			    {
				    // clear uplink output buffer as everything has now been sent out
				    ClearUplinkOutBuffer();

				    // check that -duino is not suspended
				    // and the packet is intended for -duino consumption
				    if (!_isDeviceSuspended &&
						((_uplinkBufferInp[0] & UplinkCommandTypes::PacketTypeMask) == UplinkCommandTypes::DeviceDataPacket ||
						_uplinkBufferInp[0] == UplinkCommandTypes::EmptyPacket))
				    {
					    SendDataToDevice();
				    }
			    }

			    _uplinkBufferModeAndIndex = 0;
			    _uplinkBufferReceiverBitQueue = 0;
			    _uplinkBufferTransmitterBitQueue = 0;

			    _expectedCommandLengthIncludingPayload = 0xff;
			    //_datPinDriveState = OpenDrainPinState.Z;
				SET_PAD_HI(PAD_DATA);
				CFG_PAD_INP(PAD_DATA);

				return;
		    }

			return;
	    }

		return;
    }
            
    // check for transition of CLK pin
    if (change & (1 << PAD_CLCK))
    {
	    // receive mode logic
	    if (_commonBufferTransmitReciveMode == COMMON_BUFFER_MODE_RECEIVE)
	    {
		    // detect CLK pin going Lo
		    // this has no significance during listening phase for bits 0..7,
			// as state of DAT pin is undefined at this moment (DAT pin state will be valid later on, when CLK pin goes Hi)
		    // however, we need to take some extra care when we receive extra 9th bit
		    if (!clckPinState)
		    {
			    // we need to drive DAT pin Lo when we receive ACK bit (extra 9th bit)
			    // plus some analysis of the buffer content
			    if (_commonBufferTransceiverBitCounter == 8)
			    {
					_resetOnPowerChangeCounter = 0;

				    bool isCommandValid = true;

				    // run command specific logic only when uplink is not active
				    // this works for meaningful data being transmitted over physical pin only
				    if (_uplinkBufferModeAndIndex == 0)
				    {
					    // check if we are receiving a command, and slave address is matching
					    if (_commonBufferWriteIndex == 4)
					    {
						    if (CheckCanHandleCommand() == false)
						    {
							    isCommandValid = false;
						    }
					    }
					    // extract command length
					    else if (_commonBufferWriteIndex == 5)
					    {
							if (_commonBufferTransceiverBitQueue + COMMON_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES >= COMMON_BUFFER_SIZE_IN_BYTES)
							{
								_commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_INITIAL;
								ERRLOG_LogError(Errors::ModuleCommonCommandReceiveOverflow);
								isCommandValid = false;
							}
							else
							{
								// check if command requires response within same packet
								if (_commonBuffer[0] == CommonCommandTypes::NeedResponse)
								{
									_expectedCommandLengthIncludingPayload = _commonBufferTransceiverBitQueue + COMMON_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES;
								}
								else
								{
									_expectedCommandLengthIncludingPayload = 0xff;
								}
							}
					    }
					    // check if command need to be processed before "end" packet condition is received
					    else if (_commonBufferWriteIndex == _expectedCommandLengthIncludingPayload)
					    {
						    // process command with expected response
						    if (ProcessCommand())
						    {
							    // prepare for response mode
							    _commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_BEFORE_RESPOND;
						    }
						    else
						    {
							    isCommandValid = false;
						    }
					    }
				    }

				    if (isCommandValid)
				    {
					    // drive DATA pin Lo when state is valid
					    //_datPinDriveState = OpenDrainPinState.Lo;
						CFG_PAD_OUT(PAD_DATA);
						SET_PAD_LO(PAD_DATA);
				    }
				    else
				    {
					    // release DATA pin as we no longer need to occupy it
					    //_datPinDriveState = OpenDrainPinState.Z;
						SET_PAD_HI(PAD_DATA);
						CFG_PAD_INP(PAD_DATA);

					    // switch to termination mode
					    _commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_BEFORE_TERMINATE;
				    }

					return;
			    }

				return;
		    }

			// check for data bit "clock-in", it is indicated by Lo=>Hi transition of CLK pin
			// run "uplink" logic only when activated by magic packet
			if (_uplinkBufferModeAndIndex != 0)
			{
				// check if we at the very first bit of every byte
				if (_commonBufferTransceiverBitCounter == 0)
				{
					// check for uplink transmit mode
					if ((_uplinkBufferModeAndIndex & UPLINK_ACTIVATED_BIT_FLAG) != 0)
					{
						// get value from output buffer into the output bit queue
						uint8_t index = _uplinkBufferModeAndIndex & UPLINK_PACKET_LENGTH_BIT_MASK;
						_uplinkBufferTransmitterBitQueue = _uplinkBufferOut[index];
					}
				}

				// this logic runs when we receive ACK bit (extra 9th bit)
				if (_commonBufferTransceiverBitCounter == 8)
				{
					// check for uplink receive mode
					if ((_uplinkBufferModeAndIndex & UPLINK_ACTIVATED_BIT_FLAG) != 0)
					{
						// save input value from input bit queue into input buffer
						uint8_t index = _uplinkBufferModeAndIndex & UPLINK_PACKET_LENGTH_BIT_MASK;
						_uplinkBufferInp[index] = _uplinkBufferReceiverBitQueue;
					}

					// reset all counters and advance to the next position
					_uplinkBufferReceiverBitQueue = 0;
					_uplinkBufferTransmitterBitQueue = 0;
					_uplinkBufferModeAndIndex++;
				}
				// for bits 0..7 we read/write IR data
				else
				{
					// check for uplink activation mode
					if ((_uplinkBufferModeAndIndex & UPLINK_ACTIVATED_BIT_FLAG) != 0)
					{
                        // gets bit value from output bit queue and set IR transmitter pin state
                        if ((_uplinkBufferTransmitterBitQueue & MSB_BIT_MASK) != 0)
                        {
	                        //_pinIROut.SetState(OpenDrainPinState.Hi);
	                        SET_LNK_HI;
                        }
                        else
                        {
	                        //_pinIROut.SetState(OpenDrainPinState.Lo);
	                        SET_LNK_LO;
                        }

						// shift IR transmitter buffer
                        _uplinkBufferTransmitterBitQueue = _uplinkBufferTransmitterBitQueue << 1;

						// shift IR receiver buffer
						_uplinkBufferReceiverBitQueue = _uplinkBufferReceiverBitQueue << 1;

						// get IR receiver pin state and persist its value into input bit queue
						//if (_pinIRInp.GetState() == OpenDrainPinState.Hi)
						if (GET_LNK_HI)
						{
							_uplinkBufferReceiverBitQueue |= LSB_BIT_MASK;
						}
					}
				}
			}

			// check for extra 9th bit
			if (_commonBufferTransceiverBitCounter == 8)
			{
				// check for special instructions
				// like uplink data pump, for example
				if (_commonBufferWriteIndex == 0)
				{
					if (_commonBuffer[0] == MagicCommonCommands::UplinkDataPump)
					{
						// set flag to indicate uplink buffer activation
						_uplinkBufferModeAndIndex = UPLINK_ACTIVATED_BIT_FLAG;

						// clear input buffer before receiving anything
						ClearUplinkInpBuffer();
					}
				}

				// release DATA pin, as we no longer need to occupy it
				//_datPinDriveState = OpenDrainPinState.Z;
				SET_PAD_HI(PAD_DATA);
				CFG_PAD_INP(PAD_DATA);

				// reset counters, as we are now ready to receive next byte
				_commonBufferTransceiverBitQueue = 0;
				_commonBufferTransceiverBitCounter = 0;

				// check for buffer overflow
				if (++_commonBufferWriteIndex > COMMON_BUFFER_SIZE_IN_BYTES)
				{
					_commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_INITIAL;
					ERRLOG_LogError(Errors::ModuleCommonCommandReceiveOverflow);
				}

				return;
			}

			// shift DATA buffer
			_commonBufferTransceiverBitQueue = _commonBufferTransceiverBitQueue << 1;

			// get DATA pin state and persist its value into input bit queue
			if (dataPinState)
			{
				_commonBufferTransceiverBitQueue |= LSB_BIT_MASK;
			}

			if (++_commonBufferTransceiverBitCounter == 8)
			{
				// append received byte into buffer
				_commonBuffer[_commonBufferWriteIndex] = _commonBufferTransceiverBitQueue;
			}

			return;
	    }
	    
		// logic for transition from receive to response
	    if (_commonBufferTransmitReciveMode == COMMON_BUFFER_MODE_BEFORE_RESPOND)
	    {
		    // command is processed while CLK pin is driven Lo by master during transmission of 9th extra bit
		    // we now need to do some extra logic when CLK goes Hi and prepare slave for transmitting data back to master
		    if (clckPinState)
		    {
			    // release DATA pin, as we no longer need to occupy it
			    //_datPinDriveState = OpenDrainPinState.Z;
				SET_PAD_HI(PAD_DATA);
				CFG_PAD_INP(PAD_DATA);

			    // reset read index as we will use it for sending data back to master
			    _commonBufferReadIndex = 0;

			    // load first byte into transmission queue
			    _commonBufferTransceiverBitCounter = 0;
			    _commonBufferTransceiverBitQueue = _commonBuffer[_commonBufferReadIndex];

			    // switch to response mode
			    _commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_RESPOND;

				return;
		    }

			return;
	    }
	    
		// logic for transition from receive to terminate
	    if (_commonBufferTransmitReciveMode == COMMON_BUFFER_MODE_BEFORE_TERMINATE)
	    {
		    // command is processed while CLK pin is driven Lo by master during transmission of 9th extra bit
		    // we now need to do some extra logic when CLK goes Hi and prepare slave for transmitting data back to master
		    if (clckPinState)
		    {
			    // release DATA pin, as we no longer need to occupy it
			    //_datPinDriveState = OpenDrainPinState.Z;
				SET_PAD_HI(PAD_DATA);
				CFG_PAD_INP(PAD_DATA);

			    // reset read index as we will use it for sending data back to master
			    _commonBufferReadIndex = 0;

			    // load first byte into transmission queue
			    _commonBufferTransceiverBitCounter = 0;
			    _commonBufferTransceiverBitQueue = 0;

			    // switch to initial mode
			    _commonBufferTransmitReciveMode = COMMON_BUFFER_MODE_INITIAL;

				return;
		    }

			return;
	    }
	    
		// response mode logic
	    if (_commonBufferTransmitReciveMode == COMMON_BUFFER_MODE_RESPOND)
	    {
		    // check if it is OK to set data bit
		    // it is indicated by Hi=>Lo transition of CLK pin
		    if (!clckPinState)
		    {
			    if (_commonBufferTransceiverBitCounter == 8)
			    {
                    // for extra 9th bit we need to release data pin as hub will drive it low by itself
                    //_datPinDriveState = OpenDrainPinState.Z;
					SET_PAD_HI(PAD_DATA);
					CFG_PAD_INP(PAD_DATA);

					// and move to next byte now
				    _commonBufferTransceiverBitCounter = 0;

				    if (++_commonBufferReadIndex == COMMON_BUFFER_SIZE_IN_BYTES)
				    {
					    _commonBufferReadIndex--;
				    }

				    _commonBufferTransceiverBitQueue = _commonBuffer[_commonBufferReadIndex];

					return;
			    }

				// for bits 0..7 (8 actual bits) we actively drive data pin when needed (need to transmit 0 zero)
                if ((_commonBufferTransceiverBitQueue & MSB_BIT_MASK) != 0)
                {
	                //_datPinDriveState = OpenDrainPinState.Z;
					SET_PAD_HI(PAD_DATA);
					CFG_PAD_INP(PAD_DATA);
                }
                else
                {
	                //_datPinDriveState = OpenDrainPinState.Lo;
					CFG_PAD_OUT(PAD_DATA);
					SET_PAD_LO(PAD_DATA);
                }

				_commonBufferTransceiverBitQueue = _commonBufferTransceiverBitQueue << 1;
				_commonBufferTransceiverBitCounter++;

				return;
		    }

			return;
	    }

		return;
    }
}

bool CheckCanHandleCommand()
{
    // command structure so far is as following:
    // byte 0 - command header (0xAA or 0xAB)
    // byte 1..4 - 32-bit slave address

    // reset read index as we will be reading now
    _commonBufferReadIndex = 0;

    // check command header
    uint8_t commandType = ReadNextByteFromCommonBuffer();
    if (commandType != CommonCommandTypes::FireAndForget && commandType != CommonCommandTypes::NeedResponse)
    {
	    return false;
    }

    // extract target slave ID from command and compare
    uint32_t targetAddress = ReadNextWordFromCommonBuffer();
    if (targetAddress != 0 && _address != targetAddress)
    {
	    return false;
    }

    // reset read index again as something else will process buffer later
    _commonBufferReadIndex = 0;

    return true;
}

bool ProcessCommand()
{
    // command structure is as following:
    // byte 0 - command header (0xAA or 0xAB)
    // byte 1..4 - 32-bit slave address
    // byte 5 - packet length minus 2 for header and CRC
    // byte 6..9 - 32-bit command
    // byte 10..N-1 - payload
    // byte N - CRC8

    // reset read index as we will be reading now
    _commonBufferReadIndex = 0;

    // check command header
    uint8_t commandType = ReadNextByteFromCommonBuffer();
    if (commandType != CommonCommandTypes::FireAndForget && commandType != CommonCommandTypes::NeedResponse)
    {
	    ERRLOG_LogError(Errors::ModuleCommonCommandInvalidPrefix);
	    return false;
    }

    // extract target slave ID from command and compare
    uint32_t targetAddress = ReadNextWordFromCommonBuffer();
    if (targetAddress != 0)
    {
	    if (_address != targetAddress)
	    {
		    return false;
	    }

	    _isDiscovered = true;
    }

    // get command length
    uint8_t commandLengthIncludingPayload = ReadNextByteFromCommonBuffer();

    // validate buffer position
    if (commandLengthIncludingPayload + COMMON_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES != _commonBufferWriteIndex)
    {
	    ERRLOG_LogError(Errors::ModuleCommonCommandLengthMismatch);
	    return false;
    }

    // compare CRC
    if (_commonBuffer[_commonBufferWriteIndex] != CRC_CalculateBufferCrc(_commonBuffer, _commonBufferWriteIndex))
    {
	    ERRLOG_LogError(Errors::ModuleCommonCommandCrcMismatch);
	    return false;
    }

    uint8_t commandPayloadLength = commandLengthIncludingPayload - 4;

    uint32_t commandId = ReadNextWordFromCommonBuffer();

    // reset write index as we might need to write now
    _commonBufferWriteIndex = 0;

	switch (commandId)
	{
	    case Commands::CRST:
		{
			ResetController();
			break;
		}

        case Commands::CREQ:
		{
		    if (_isControllerSuspended) return false;

			uint32_t slaveAddressBitMask = ReadNextWordFromCommonBuffer();
			if (slaveAddressBitMask == 0 || (_address & slaveAddressBitMask) == slaveAddressBitMask)
			{
				WriteResponseHeaderByteToCommonBuffer(8, false);
				WriteNextWordToCommonBuffer(_address);
				WriteNextWordToCommonBuffer(_address ^ 0xffffffff);
				break;
			}

			return false;
		}

        case Commands::CGID:
		{
		    if (_isControllerSuspended) return false;

			WriteResponseHeaderByteToCommonBuffer(6, true);
			WriteNextWordToCommonBuffer(_address);
			WriteNextByteToCommonBuffer(VERSION_MAJOR);
			WriteNextByteToCommonBuffer(VERSION_MINOR);
			break;
		}

        case Commands::CSLB:
		{
		    if (_isControllerSuspended) return false;

			eeprom_write_byte((uint8_t*)0, ReadNextByteFromCommonBuffer());
			break;
		}

        case Commands::CGLB:
		{
		    if (_isControllerSuspended) return false;

			WriteResponseHeaderByteToCommonBuffer(1, true);
	        WriteNextByteToCommonBuffer(eeprom_read_byte((uint8_t*)0));
		    break;
		}

        case Commands::CERR:
		{
			if (_isControllerSuspended) return false;

			uint8_t errors[ERRLOG_MAX_ERRORS];
			ERRLOG_GetErrors(errors);
			WriteResponseHeaderByteToCommonBuffer(ERRLOG_MAX_ERRORS, true);

			for (uint8_t i = 0; i < ERRLOG_MAX_ERRORS; i++)
			{
				WriteNextByteToCommonBuffer(errors[i]);
			}

			break;
		}

        case Commands::CSLP:
			_isControllerSuspended = true;
			break;

        case Commands::CWUP:
			_isControllerSuspended = false;
			break;

        case Commands::CNEW:
		{
		    if (_isControllerSuspended) return false;

			// it is counter-intuitive to return 0 when controller is new
			// but when multiple bricks are connected, then a new brick will pull signal line low,
			// this way a new brick can be discovered among others
			WriteOneByteResponseToCommonBuffer(_isDiscovered ? 127 : 0);
			break;
		}

		case Commands::UWRI:
		{
			if (targetAddress == 0) return false;

			if (commandPayloadLength > UPLINK_BUFFER_SIZE_IN_BYTES)
			{
				ERRLOG_LogError(Errors::ModuleUplinkCommandTooLongPayload);
				break;
			}

			for (uint8_t i = 0; i < commandPayloadLength; i++)
			{
				_uplinkBufferInp[i] = ReadNextByteFromCommonBuffer();
			}

			for (uint8_t i = commandPayloadLength; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
			{
				_uplinkBufferInp[i] = 0;
			}

			break;
		}

		case Commands::UWRO:
		{
			if (targetAddress == 0) return false;

			if (commandPayloadLength > UPLINK_BUFFER_SIZE_IN_BYTES)
			{
				ERRLOG_LogError(Errors::ModuleUplinkCommandTooLongPayload);
				break;
			}

			for (uint8_t i = 0; i < commandPayloadLength; i++)
			{
				_uplinkBufferOut[i] = ReadNextByteFromCommonBuffer();
			}

			for (uint8_t i = commandPayloadLength; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
			{
				_uplinkBufferOut[i] = 0;
			}

			break;
		}

		case Commands::URDI:
		{
			if (targetAddress == 0) return false;

			WriteResponseHeaderByteToCommonBuffer(UPLINK_BUFFER_SIZE_IN_BYTES, true);

			for (uint8_t i = 0; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
			{
				WriteNextByteToCommonBuffer(_uplinkBufferInp[i]);
			}

			break;
		}

		case Commands::URDO:
		{
			if (targetAddress == 0) return false;

			WriteResponseHeaderByteToCommonBuffer(UPLINK_BUFFER_SIZE_IN_BYTES, true);

			for (uint8_t i = 0; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
			{
				WriteNextByteToCommonBuffer(_uplinkBufferOut[i]);
			}

			break;
		}

		case Commands::UCPY:
		{
			for (uint8_t i = 0; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
			{
				_uplinkBufferOut[i] = _uplinkBufferInp[i];
			}

			break;
		}

		case Commands::UMOV:
		{
			for (uint8_t i = 0; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
			{
				_uplinkBufferOut[i] = _uplinkBufferInp[i];
				_uplinkBufferInp[i] = 0;
			}

			break;
		}

		case Commands::UCCI:
		{
			if (commandPayloadLength > UPLINK_BUFFER_SIZE_IN_BYTES)
			{
				ERRLOG_LogError(Errors::ModuleUplinkCommandTooLongPayload);
				break;
			}

			for (uint8_t i = 0; i < commandPayloadLength; i++)
			{
				if (_uplinkBufferInp[i] != ReadNextByteFromCommonBuffer())
				{
					return false;
				}
			}

			WriteResponseHeaderByteToCommonBuffer(8, false);
			WriteNextWordToCommonBuffer(_address);
			WriteNextWordToCommonBuffer(_address ^ 0xffffffff);
		
			break;
		}

		case Commands::UCCO:
		{
			if (commandPayloadLength > UPLINK_BUFFER_SIZE_IN_BYTES)
			{
				ERRLOG_LogError(Errors::ModuleUplinkCommandTooLongPayload);
				break;
			}

			for (uint8_t i = 0; i < commandPayloadLength; i++)
			{
				if (_uplinkBufferOut[i] != ReadNextByteFromCommonBuffer())
				{
					return false;
				}
			}

			WriteResponseHeaderByteToCommonBuffer(8, false);
			WriteNextWordToCommonBuffer(_address);
			WriteNextWordToCommonBuffer(_address ^ 0xffffffff);

			break;
		}

		case Commands::DRST:
		{
		    if (_isControllerSuspended) return false;

			ResetDevice();
			break;
		}

		case Commands::DSLP:
		{
		    if (_isControllerSuspended) return false;

			_isDeviceSuspended = true;
			break;
		}

		case Commands::DWUP:
		{
		    if (_isControllerSuspended) return false;

			_isDeviceSuspended = false;
			break;
		}

        case Commands::DMSG:
		{
			if (_isDeviceSuspended)
			{
				break;
			}

			uint8_t writeIndexWithinPacket = 0;

			// prepend mandatory packet header
			_uplinkBufferInp[writeIndexWithinPacket++] = UplinkCommandTypes::DeviceMessagePacket | (commandPayloadLength + 2);

			// prepend mandatory message length
			_uplinkBufferInp[writeIndexWithinPacket++] = commandPayloadLength + 1;

			// prepend message type to signify forwarded command
			_uplinkBufferInp[writeIndexWithinPacket++] = DeviceCommands::DeviceCommandIdentifier;

			for (uint8_t i = 0; i < commandPayloadLength; i++)
			{
				_uplinkBufferInp[writeIndexWithinPacket++] = ReadNextByteFromCommonBuffer();
			}

			_uplinkBufferModeAndIndex = 0;

			SendDataToDevice();

			uint8_t responseLength = _uplinkBufferOut[1];

			WriteResponseHeaderByteToCommonBuffer(responseLength, true);

			for (uint8_t i = 2; i < 2 + responseLength; i++)
			{
				WriteNextByteToCommonBuffer(_uplinkBufferOut[i]);
			}

			break;
		}

        default:
	        return false;
	}

    // by default, calculate CRC only for commands that need response
    if (commandType == CommonCommandTypes::NeedResponse &&
		_commonBufferWriteIndex > 1 &&
		(_commonBuffer[0] & COMMON_BUFFER_ONE_BYTE_RESPONSE_BIT_FLAG) == 0 &&
		(_commonBuffer[0] & COMMON_BUFFER_RESPONSE_CALCULATE_CRC_BIT_FLAG) != 0)
    {
	    WriteNextByteToCommonBuffer(CRC_CalculateBufferCrc(_commonBuffer, _commonBufferWriteIndex));
    }

    return true;
}

uint8_t ReadNextByteFromCommonBuffer()
{
	return _commonBuffer[_commonBufferReadIndex++];
}

uint32_t ReadNextWordFromCommonBuffer()
{
	uint32_t word = _commonBuffer[_commonBufferReadIndex++];
	
	word = (word << 8) | _commonBuffer[_commonBufferReadIndex++];
	word = (word << 8) | _commonBuffer[_commonBufferReadIndex++];
	word = (word << 8) | _commonBuffer[_commonBufferReadIndex++];
	
	return word;
}

void WriteResponseHeaderByteToCommonBuffer(uint8_t length, bool calculateCrc)
{
	if (length > COMMON_BUFFER_RESPONSE_PACKET_LENGTH_BIT_MASK)
	{
		return;
	}

	if (length == 0)
	{
		WriteNextByteToCommonBuffer(0);
		return;
	}

	uint8_t b = length;
	if (calculateCrc)
	{
		b |= COMMON_BUFFER_RESPONSE_CALCULATE_CRC_BIT_FLAG;
	}

	WriteNextByteToCommonBuffer(b);
}

void WriteOneByteResponseToCommonBuffer(uint8_t v)
{
	if (v > COMMON_BUFFER_ONE_BYTE_RESPONSE_CONTENT_BIT_MASK)
	{
		ERRLOG_LogError(Errors::ModuleCommonBufferSingleByteResponseToLarge);
		return;
	}

	WriteNextByteToCommonBuffer(v | COMMON_BUFFER_ONE_BYTE_RESPONSE_BIT_FLAG);
}

void WriteNextByteToCommonBuffer(uint8_t v)
{
	if (_commonBufferWriteIndex >= COMMON_BUFFER_SIZE_IN_BYTES)
	{
		ERRLOG_LogError(Errors::ModuleCommonBufferOnWriteByteOverflow);
		return;
	}

	_commonBuffer[_commonBufferWriteIndex++] = v;
}

void WriteNextWordToCommonBuffer(uint32_t v)
{
	if (_commonBufferWriteIndex + 4 >= COMMON_BUFFER_SIZE_IN_BYTES)
	{
		ERRLOG_LogError(Errors::ModuleCommonBufferOnWriteWordOverflow);
		return;
	}

	_commonBuffer[_commonBufferWriteIndex++] = (v >> 24) & 0xff;
	_commonBuffer[_commonBufferWriteIndex++] = (v >> 16) & 0xff;
	_commonBuffer[_commonBufferWriteIndex++] = (v >> 8) & 0xff;
	_commonBuffer[_commonBufferWriteIndex++] = (v & 0xff);
}

void ClearUplinkInpBuffer()
{
    for (uint8_t i = 0; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
    {
	    _uplinkBufferInp[i] = 0;
    }
}

void ClearUplinkOutBuffer()
{
    for (uint8_t i = 0; i < UPLINK_BUFFER_SIZE_IN_BYTES; i++)
    {
	    _uplinkBufferOut[i] = 0;
    }
}

void SendDataToDevice()
{
	//USART_Write("SendDataToDevice");
	//USART_WriteChar('\r');

    uint8_t commandType = _uplinkBufferInp[0] & UplinkCommandTypes::PacketTypeMask;
    if (commandType == UplinkCommandTypes::PacketTypeMask)
    {
	    ERRLOG_LogError(Errors::DeviceUplinkDisconnected);
	    return;
    }

    uint8_t packetLength = _uplinkBufferInp[0] & UplinkCommandTypes::PacketSizeMask;

    if (commandType == UplinkCommandTypes::DeviceEmptyPacket)
    {
	    if (_uplinkBufferInp[0] != UplinkCommandTypes::EmptyPacket)
	    {
		    ERRLOG_LogError(Errors::DeviceUplinkShortToGround);
		    return;
	    }
    }
    else if (commandType == UplinkCommandTypes::DeviceDataPacket)
    {
	    if (packetLength + UPLINK_COMMAND_SERVICE_DATA_LENGTH_IN_BYTES > (_uplinkBufferModeAndIndex & UPLINK_PACKET_LENGTH_BIT_MASK))
	    {
		    ERRLOG_LogError(Errors::DeviceCommandInsufficientPacketLength);
		    return;
	    }

	    // exit if CRC on data do not match
	    if (CRC_CalculateBufferCrc(_uplinkBufferInp, packetLength + 1) != _uplinkBufferInp[packetLength + 1])
	    {
		    ERRLOG_LogError(Errors::DeviceCommandCrcMismatch);
		    return;
	    }
    }
    else if (commandType == UplinkCommandTypes::DeviceMessagePacket)
    {
	    if (_uplinkBufferModeAndIndex != 0)
	    {
		    ERRLOG_LogError(Errors::DeviceForwardMessageInvalidOrigin);
		    return;
	    }
    }

	uint8_t deviceResponseDataLength = DEV_ProcessMessage(&_uplinkBufferInp[1], packetLength - 1, &_uplinkBufferOut[1]);

	if (deviceResponseDataLength > UplinkCommandTypes::PacketSizeMask + 1)
	{
		ERRLOG_LogError(Errors::DeviceRespondedWithTooLongPacket);
		return;
	}

	if (deviceResponseDataLength == 0)
	{
		_uplinkBufferOut[0] = UplinkCommandTypes::EmptyPacket;
	}
	else
	{
		_uplinkBufferOut[0] = UplinkCommandTypes::DeviceDataPacket | deviceResponseDataLength;
		_uplinkBufferOut[deviceResponseDataLength + 1] = CRC_CalculateBufferCrc(_uplinkBufferOut, deviceResponseDataLength + 1);
	}
}

void ResetController()
{
	//wdt_enable(WDTO_250MS);
	DEV_Reset();
	//while (true) {};

	_resetOnPowerChangeCounter = 0;
	PAD_CLCK = PAD_DATA = PAD_VCC = PAD_GND = 0xff;
	_directionDetectionState = 0xff;
	_p1DetectionCounter = _p2DetectionCounter = _p3DetectionCounter = _p4DetectionCounter = 0;
	_isDiscovered = false;
	_isControllerSuspended = false;
	_isDeviceSuspended = false;

	_commonBufferWriteIndex = 0;
	_commonBufferReadIndex = 0;
	_commonBufferTransmitReciveMode = 0;
	_commonBufferTransceiverBitQueue = 0;
	_commonBufferTransceiverBitCounter = 0;
	
	_expectedCommandLengthIncludingPayload = 0;
	
	_uplinkBufferModeAndIndex = 0;
	_uplinkBufferReceiverBitQueue = 0;
	_uplinkBufferTransmitterBitQueue = 0;
}

void ResetDevice()
{
	DEV_Reset();
}

uint8_t GetPinBitIndexInInputPort(uint8_t pinIndex)
{
	switch (pinIndex)
	{
		case 1:
			return INPUT_PAD_1;
		case 2:
			return INPUT_PAD_2;
		case 3:
			return INPUT_PAD_3;
		case 4:
			return INPUT_PAD_4;
		default:
			return 0xff;
	}
}<file_sep>/config.h
#ifndef CONFIG_H_
#define CONFIG_H_

//#define SYNCHRON

#define F_CPU								16000000UL
#define UART_RX0_BUFFER_SIZE				32
#define UART_TX0_BUFFER_SIZE				32

#endif /* CONFIG_H_ */<file_sep>/error_logger.h
#ifndef __ERROR_LOGGER_H__
#define __ERROR_LOGGER_H__

const uint8_t ERRLOG_MAX_ERRORS = 16;

void ERRLOG_LogError(uint8_t errorCode);
void ERRLOG_ClearErrors();
void ERRLOG_GetErrors(uint8_t * target);
uint8_t ERRLOG_GetErrorCount();

#endif //__ERROR_LOGGER_H__
<file_sep>/adc.cpp
#include <stdlib.h>
#include <stdint.h>
#include <avr/io.h>

#include "adc.h"

void ADC_init()
{
	// Set the ADC pre-scaler to 128 (i.e., 16MHz/128 = 125KHz)
	ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

	// Set the voltage reference from AVcc (i.e., 5V).
	ADMUX |= (1 << REFS0 );

	// Turn on the ADC.
	ADCSRA |= (1 << ADEN);
	ADCSRA |= (1 << ADSC);
}

uint16_t ADC_read(uint8_t channel)
{
	// Clear the previously read channel.
	ADMUX &= 0xf0;

	// Select the ADC channel to be read.
	ADMUX |= channel;

	// Start conversion.
	ADCSRA |= (1 << ADSC);

	// Wait till conversion finishes.
	while ((ADCSRA & (1 << ADIF)) == 0) {}

	// Get ADC value.
	uint16_t result = ADC;

	// Clear the ADIF flag.
	ADCSRA |= ( 1 << ADIF );

	return result;
}<file_sep>/usart.cpp
#include "config.h"

#include <stdlib.h>
#include <stdint.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#include "uart.h"
#include "usart.h"

void USART_Init(uint32_t baudRate)
{
	uart0_init(UART_BAUD_SELECT(baudRate, F_CPU));
}

bool USART_CopyAvailableTillLineEnd(char * target, uint8_t & cnt)
{
	bool lineEndReceived = false;

	for (;;)
	{
		uint16_t fc = uart0_getc();
		if (fc > 0xFF) break;
		char c = fc & 0xFF;
		
		if (c == '\r' || c == '\n')
		{
			lineEndReceived = true;

			fc = uart0_peek();
			if (fc > 0xFF) break;
			c = fc & 0xFF;

			if (c != '\r' && c != '\n')
			{
				break;
			}
		}
		else
		{
			target[cnt++] = c;
		}
	}

	return lineEndReceived;
}

void USART_WriteChar(char data)
{
	uart0_putc(data);
}

void USART_Write(char * stringPtr)
{
	while (*stringPtr != 0)
	{
		uart0_putc(*stringPtr++);
	}
}

void USART_Write(char * stringPtr, uint8_t length)
{
	for (uint8_t i = 0; i < length; i++)
	{
		uart0_putc(stringPtr[i]);
	}
}

void USART_Write(const char * stringPtr)
{
	while (*stringPtr != 0)
	{
		uart0_putc(*stringPtr++);
	}
}

void USART_Write(const char * stringPtr, uint8_t length)
{
	for (uint8_t i = 0; i < length; i++)
	{
		uart0_putc(stringPtr[i]);
	}
}

void USART_Write(volatile char * stringPtr)
{
	while (*stringPtr != 0)
	{
		uart0_putc(*stringPtr++);
	}
}

void USART_Write(volatile char * stringPtr, uint8_t length)
{
	for (uint8_t i = 0; i < length; i++)
	{
		uart0_putc(stringPtr[i]);
	}
}

void USART_WriteBoolResultCode(bool b)
{
	uart0_putc('$');
	USART_WriteByteAsHex(b ? 0x00 : 0xFF);
}

void USART_WriteByteResultCode(uint8_t b)
{
	uart0_putc('$');
	USART_WriteByteAsHex(b);
}

void USART_WriteByteAsHex(uint8_t b)
{
	uint8_t v = b >> 4;
	v += v > 9 ? 55 : '0';
	uart0_putc(v);

	v = b & 0x0F;
	v += v > 9 ? 55 : '0';
	uart0_putc(v);
}

void USART_WriteShortAsHex(uint16_t s)
{
	USART_WriteByteAsHex((uint8_t)(s >> 8));
	USART_WriteByteAsHex((uint8_t)s);
}

void USART_WriteIntAsHex(uint32_t i)
{
	USART_WriteByteAsHex((uint8_t)(i >> 24));
	USART_WriteByteAsHex((uint8_t)(i >> 16));
	USART_WriteByteAsHex((uint8_t)(i >> 8));
	USART_WriteByteAsHex((uint8_t)i);
}<file_sep>/hub_interface.h
#ifndef __HUB_INTERFACE_H__
#define __HUB_INTERFACE_H__

#include <stdlib.h>
#include <stdint.h>

void HUB_Initialize();

void HUB_SetDelay(uint8_t delayId, uint8_t value);
uint8_t HUB_GetDelay(uint8_t delayId);

uint8_t * HUB_GetBufferContent();

bool HUB_SendCommandToAll(uint32_t command);
bool HUB_SendCommandToAll(uint32_t command, uint32_t payload);
bool HUB_SendCommandToAll(uint32_t command, uint8_t * payload, uint8_t payloadLength);
bool HUB_SendCommand(uint32_t slave, uint32_t command);
bool HUB_SendCommand(uint32_t slave, uint32_t command, uint32_t payload);
bool HUB_SendCommand(uint32_t slave, uint32_t command, uint8_t * payload, uint8_t payloadLength);

bool HUB_SendCommandToAllWithExpectedResponse(uint32_t command);
bool HUB_SendCommandToAllWithExpectedResponse(uint32_t command, uint32_t payload);
bool HUB_SendCommandToAllWithExpectedResponse(uint32_t command, uint8_t * payload, uint8_t payloadLength);
bool HUB_SendCommandWithExpectedResponse(uint32_t slave, uint32_t command);
bool HUB_SendCommandWithExpectedResponse(uint32_t slave, uint32_t command, uint32_t payload);
bool HUB_SendCommandWithExpectedResponse(uint32_t slave, uint32_t command, uint8_t * payload, uint8_t payloadLength);

uint8_t HUB_DetectNewSlaves();
uint8_t HUB_DetectSlaves(uint32_t * foundSlaveAddresses, uint8_t maxSlavesToReturn);
uint8_t HUB_DetectSlavesSequence(uint32_t * foundSlaveAddresses, uint8_t maxSlavesToReturn);
bool HUB_SendEmptyDuinoCommand();
bool HUB_SendUplinkPumpPacket();
bool HUB_SendUplinkPacket(uint8_t * payload, uint8_t length);

#endif //__HUB_INTERFACE_H__
<file_sep>/error_logger.cpp
#include <stdint.h>

#include "error_logger.h"

static uint8_t _errors[ERRLOG_MAX_ERRORS];
static uint8_t _errorCount;

void ERRLOG_LogError(uint8_t errorCode)
{
	if (errorCode == 0)
	{
		return;
	}

	for (uint8_t i = ERRLOG_MAX_ERRORS - 1; i > 0; i--)
	{
		_errors[i] = _errors[i - 1];
	}

	_errors[0] = errorCode;

	if (_errorCount < 255)
	{
		_errorCount++;
	}
}

void ERRLOG_GetErrors(uint8_t * target)
{
	for (uint8_t i = 0; i < ERRLOG_MAX_ERRORS; i++)
	{
		target[i] = _errors[i];
	}
}

uint8_t ERRLOG_GetErrorCount()
{
	return _errorCount;
}

void ERRLOG_ClearErrors()
{
	for (uint8_t i = 0; i < ERRLOG_MAX_ERRORS; i++)
	{
		_errors[i] = 0;
	}

	_errorCount = 0;
}
<file_sep>/crc.h
#ifndef __CRC_H__
#define __CRC_H__

uint8_t CRC_CalculateBufferCrc(uint8_t * buffer, uint8_t length);

#endif //__CRC_H__
<file_sep>/errors.h
#ifndef ERRORS_H_
#define ERRORS_H_

class Errors
{
	public:
	static const uint8_t HubCommonInterfaceDataPinDrivenLowOnPacketStart = 0x11;
	static const uint8_t HubCommonInterfaceClckPinIsLowOnPacketEnd = 0x12;
	static const uint8_t HubCommonInterfaceDataPinDrivenLowOnPacketEnd = 0x13;

	static const uint8_t HubCommonBufferOnResponseReadOverflow = 0x21;
	static const uint8_t HubCommonBufferOnResponseReadCrcMismatch = 0x22;
	static const uint8_t HubCommonBufferOnWriteByteOverflow = 0x23;
	static const uint8_t HubCommonBufferOnWriteWordOverflow = 0x24;
	static const uint8_t HubUplinkBufferOnWriteByteOverflow = 0x2a;

	static const uint8_t ModuleCommonBufferSingleByteResponseToLarge = 0x30;
	static const uint8_t ModuleCommonBufferOnWriteByteOverflow = 0x33;
	static const uint8_t ModuleCommonBufferOnWriteWordOverflow = 0x34;

	static const uint8_t ModuleCommonCommandLengthMismatch = 0x40;
	static const uint8_t ModuleCommonCommandCrcMismatch = 0x41;
	static const uint8_t ModuleCommonCommandReceiveOverflow = 0x42;
	static const uint8_t ModuleCommonCommandInvalidPrefix = 0x43;
	static const uint8_t ModuleUplinkCommandTooLongPayload = 0x4a;

	static const uint8_t DeviceCommandInsufficientPacketLength = 0x80;
	static const uint8_t DeviceCommandCrcMismatch = 0x81;
	static const uint8_t DeviceNotReadyForPacket = 0x82;
	static const uint8_t DeviceNotAcknowledgedPacketStart = 0x83;
	static const uint8_t DeviceNotAcknowledgedPacketEnd = 0x84;
	static const uint8_t DeviceRespondedWithTooLongPacket = 0x85;
	static const uint8_t DeviceUplinkDisconnected = 0x86;
	static const uint8_t DeviceUplinkShortToGround = 0x87;
	static const uint8_t DeviceForwardMessageInvalidOrigin = 0x88;

	static const uint8_t InvalidOneByteResponse = 0xff;
};

#endif /* ERRORS_H_ */<file_sep>/main.cpp
#include "config.h"

#include <stdlib.h>
#include <stdint.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <util/atomic.h>

#include "usart.h"
#include "hub_interface.h"
#include "mod_interface.h"
#include "error_logger.h"
#include "commands.h"
#include "common.h"

static volatile uint8_t _previousInputPinsState;

static uint8_t HexToByte(uint8_t h1, uint8_t h2);

// This function is called upon a HARDWARE RESET:
void get_mcusr(void) __attribute__((naked)) __attribute__((section(".init3")));
void get_mcusr(void)
{
	MCUSR = 0;
	wdt_disable();
}

//ISR (_VECTOR(1)) { USART_WriteChar('1'); }
//ISR (_VECTOR(2)) { USART_WriteChar('2'); }
//ISR (_VECTOR(3)) { USART_WriteChar('3'); }
//ISR (_VECTOR(4)) { USART_WriteChar('4'); }
//ISR (_VECTOR(5)) { USART_WriteChar('5'); }
//ISR (_VECTOR(6)) { USART_WriteChar('6'); }
//ISR (_VECTOR(7)) { USART_WriteChar('7'); }
//ISR (_VECTOR(8)) { USART_WriteChar('8'); }
//ISR (_VECTOR(9)) { USART_WriteChar('9'); }
//ISR (_VECTOR(10)) { USART_WriteChar('A'); }
//ISR (_VECTOR(11)) { USART_WriteChar('B'); }
//ISR (_VECTOR(12)) { USART_WriteChar('C'); }
//ISR (_VECTOR(13)) { USART_WriteChar('D'); }
//ISR (_VECTOR(14)) { USART_WriteChar('E'); }
//ISR (_VECTOR(15)) { USART_WriteChar('F'); }
//ISR (_VECTOR(16)) { USART_WriteChar('G'); }
//ISR (_VECTOR(17)) { USART_WriteChar('H'); }
//ISR (_VECTOR(18)) { USART_WriteChar('I'); }
//ISR (_VECTOR(19)) { USART_WriteChar('J'); }
//ISR (_VECTOR(20)) { USART_WriteChar('K'); }
//ISR (_VECTOR(21)) { USART_WriteChar('L'); }
//ISR (_VECTOR(22)) { USART_WriteChar('M'); }
//ISR (_VECTOR(23)) { USART_WriteChar('N'); }
//ISR (_VECTOR(24)) { USART_WriteChar('O'); }
//ISR (_VECTOR(25)) { USART_WriteChar('P'); }
//ISR (_VECTOR(26)) { USART_WriteChar('Q'); }
//ISR (_VECTOR(27)) { USART_WriteChar('R'); }
//ISR (_VECTOR(28)) { USART_WriteChar('S'); }
//ISR (_VECTOR(29)) { USART_WriteChar('T'); }
//ISR (_VECTOR(30)) { USART_WriteChar('U'); }
//ISR (_VECTOR(31)) { USART_WriteChar('V'); }
//ISR (_VECTOR(32)) { USART_WriteChar('W'); }
//ISR (_VECTOR(33)) { USART_WriteChar('X'); }
//ISR (_VECTOR(34)) { USART_WriteChar('Y'); }
//ISR (_VECTOR(35)) { USART_WriteChar('Z'); }

ISR (BADISR_vect)
{
	USART_WriteChar('!');
}

// ==================================================
// Watchdog timer elapsed interrupt handling routine.
// ==================================================
ISR (WDT_vect)
{
	USART_WriteChar('W');
}

// ==================================================
// Pin state change interrupt handling routine.
// ==================================================
ISR (PCINT1_vect)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		uint8_t currentInputPinsState = PINC;

		MOD_PadPortStateChanged(currentInputPinsState, _previousInputPinsState);

		_previousInputPinsState = PINC;

		// DAT pin toggling between input and output modes will cause a new pin change interrupt.
		// This is due to pull-up resistor on DAT pin.
		// For example if pin was in output mode and driven Lo, then when it is released (switched to input mode),
		// the pull-up register will pull it up Hi. This change will be detected as external change to pin state,
		// and pin change interrupt will occur.
		// To prevent this, we clear interrupt flag which could have been sen as logic within interrupt runs.
		PCIFR &= ~(1 << PCIF1);
	}
}

int main(void)
{
	// Disable interrupts.
	cli();

	HUB_Initialize();

	// DDRx:  0 - input
	//        1 - output
	// PINx:  read pin state
	// PORTx: write port state
	//        0 - sink (Lo)
	//        1 - source (Hi)
	// PUDx:  0 - pull up enabled
	//        1 - pull up disabled for entire register (i.e. all bits in 'x' port)

	// Configure mod:
	// common connection pads: DDC0-DDC3
	// uplink input: DDC4
	// uplink output: DDC5
	DDRC = (0 << DDC0) | (0 << DDC1) | (0 << DDC2) | (0 << DDC3) | (0 << DDC4) | (1 << DDC5);

	// Configure hub:
	// output pins: DDB1-DDB4
	//      D9            D10           D12           D11
	DDRB = (1 << DDB1) | (1 << DDB2) | (1 << DDB3) | (1 << DDB4);
	PORTB &= ~((1 << PORTB1) | (1 << PORTB2) | (1 << PORTB3) | (1 << PORTB4));

	// Set PCIE1 to enable PCMSK1 scan.
	PCICR = 0;
	PCICR |= (1 << PCIE1);

	// Set PCINTx to trigger an interrupt on pin state change.
	PCMSK1 = 0;
	PCMSK1 |= (1 << PCINT8) | (1 << PCINT9) | (1 << PCINT10) | (1 << PCINT11);

	USART_Init(57600);

	TCCR1A = 0;
	TCCR1B =  (0 << CS12) | (0 << CS11) | (1 << CS10);
	
	// Enable interrupts.
	sei();

	uint8_t commandBufferReadIndex = 0;
	uint8_t commandBufferWriteIndex = 0;
	char commandBuffer[2 + 8 + 8 + (COMMON_BUFFER_SIZE_IN_BYTES * 2)];

	uint8_t payloadLength = 0;
	uint8_t payload[COMMON_BUFFER_SIZE_IN_BYTES];

	for (;;)
	{
		MOD_MonitorConnectionHealth();

		if (!USART_CopyAvailableTillLineEnd(commandBuffer, commandBufferWriteIndex))
		{
			continue;
		}

		commandBufferReadIndex = 0;
		payloadLength = 0;

		char usartReceivedByte = commandBuffer[commandBufferReadIndex++];

		if (usartReceivedByte == 'L')
		{
			while (commandBufferReadIndex < commandBufferWriteIndex)
			{
				char c = commandBuffer[commandBufferReadIndex++];
				USART_WriteChar(c);
			}

			commandBufferWriteIndex = 0;
			USART_WriteChar('\n');
			continue;
		}
		
		if (usartReceivedByte == 'T')
		{
			char c = commandBuffer[commandBufferReadIndex++];
			char hh = commandBuffer[commandBufferReadIndex++];
			char hl = commandBuffer[commandBufferReadIndex++];
			uint8_t delayId = HexToByte(hh, hl);

			uint8_t value;

			switch (c)
			{
				case 'S':
					hh = commandBuffer[commandBufferReadIndex++];
					hl = commandBuffer[commandBufferReadIndex++];
					value = HexToByte(hh, hl);

					HUB_SetDelay(delayId, value);
					break;

				case 'G':
					value = HUB_GetDelay(delayId);
					USART_WriteByteAsHex(value);
					break;
			}


			commandBufferWriteIndex = 0;
			USART_WriteChar('\n');
			continue;
		}

		if (usartReceivedByte == '#')
		{
			char c = commandBuffer[commandBufferReadIndex++];
			bool isRequest = c == '?';

			uint32_t w1 = 0;
			for (uint8_t i = 0; i < 4; i++)
			{
				if (commandBufferWriteIndex == commandBufferReadIndex) break;
				char hh = commandBuffer[commandBufferReadIndex++];
				if (hh == '-') break;
				char hl = commandBuffer[commandBufferReadIndex++];

				w1 = w1 << 8;
				w1 |= HexToByte(hh, hl);
			}

			uint32_t w2 = 0;
			for (uint8_t i = 0; i < 4; i++)
			{
				if (commandBufferWriteIndex == commandBufferReadIndex) break;
				char hh = commandBuffer[commandBufferReadIndex++];
				if (hh == '-') break;
				char hl = commandBuffer[commandBufferReadIndex++];

				w2 = w2 << 8;
				w2 |= HexToByte(hh, hl);
			}

			if (w2 != 0)
			{
				for (uint8_t i = 0; i < COMMON_BUFFER_SIZE_IN_BYTES; i++)
				{
					if (commandBufferWriteIndex == commandBufferReadIndex) break;
					char hh = commandBuffer[commandBufferReadIndex++];
					if (hh == '-') break;
					char hl = commandBuffer[commandBufferReadIndex++];

					payload[i] = HexToByte(hh, hl);
					payloadLength++;
				}
			}

			bool result = false;

			if (w1 != 0 && w2 == 0)
			{
				if (isRequest)
				{
					result = HUB_SendCommandToAllWithExpectedResponse(w1);
				}
				else
				{
					result = HUB_SendCommandToAll(w1);
				}
			}
			else
			{
				if (isRequest)
				{
					result = HUB_SendCommandWithExpectedResponse(w1, w2, payload, payloadLength);
				}
				else
				{
					result = HUB_SendCommand(w1, w2, payload, payloadLength);
				}
			}

			if (result && isRequest)
			{
				uint8_t * buff = HUB_GetBufferContent();

				if (buff[0] < COMMON_BUFFER_SIZE_IN_BYTES)
				{
					for (uint8_t i = 1; i <= buff[0]; i++)
					{
						USART_WriteByteAsHex(buff[i]);
					}
				}
			}
			else
			{
				USART_WriteBoolResultCode(result);
			}
			
			USART_WriteChar('\n');

			commandBufferWriteIndex = 0;
			continue;
		}

		if (usartReceivedByte == '?')
		{
			uint8_t * buff = HUB_GetBufferContent();
			for (uint8_t i = 0; i < COMMON_BUFFER_SIZE_IN_BYTES; i++)
			{
				USART_WriteByteAsHex(buff[i]);
			}
				
			USART_WriteChar('\n');

			commandBufferWriteIndex = 0;
			continue;
		}

		if (usartReceivedByte == 'D')
		{
			char c = commandBuffer[commandBufferReadIndex++];
			switch (c)
			{
				case 'N':
					USART_WriteByteResultCode(HUB_DetectNewSlaves());
					USART_WriteChar('\n');
					break;

				case 'A':
				case 'S':
					{
						uint32_t slaves[32];
						uint8_t cnt = c == 'S' ? HUB_DetectSlavesSequence(slaves, 32) : HUB_DetectSlaves(slaves, 32);
						if (cnt == 255)
						{
							USART_WriteBoolResultCode(false);
						}
						else
						{
							for (uint8_t i = 0; i < cnt; i++)
							{
								USART_WriteIntAsHex(slaves[i]);
							}
						}

						USART_WriteChar('\n');
					}
					break;
			}

			commandBufferWriteIndex = 0;
			continue;
		}

		if (usartReceivedByte == 'E')
		{
			char c = commandBuffer[commandBufferReadIndex++];
			switch (c)
			{
				case 'Q':
					{
						uint8_t errors[ERRLOG_MAX_ERRORS];
						ERRLOG_GetErrors(errors);
						for (uint8_t i = 0; i < ERRLOG_MAX_ERRORS; i++)
						{
							USART_WriteByteAsHex(errors[i]);
						}

						USART_WriteChar('\n');
					}
					break;

				case 'C':
					ERRLOG_ClearErrors();
					USART_WriteBoolResultCode(true);
					USART_WriteChar('\n');
					break;
			}

			commandBufferWriteIndex = 0;
			continue;
		}

		if (usartReceivedByte == 'U')
		{
			bool result = false;

			char c = commandBuffer[commandBufferReadIndex++];
			switch (c)
			{
				case 'E':
					result = HUB_SendEmptyDuinoCommand();
					break;

				case 'P':
					result = HUB_SendUplinkPumpPacket();
					break;

				case 'D':
					{
						payloadLength = 0;
						for (uint8_t i = 0; i < COMMON_BUFFER_SIZE_IN_BYTES; i++)
						{
							if (commandBufferWriteIndex == commandBufferReadIndex) break;
							char hh = commandBuffer[commandBufferReadIndex++];
							if (hh == '-') break;
							char hl = commandBuffer[commandBufferReadIndex++];

							payload[i] = HexToByte(hh, hl);
							payloadLength++;
						}

						result = HUB_SendUplinkPacket(payload, payloadLength);
					}
					break;
			}

			USART_WriteBoolResultCode(result);
			USART_WriteChar('\n');

			commandBufferWriteIndex = 0;
			continue;
		}

		commandBufferReadIndex = 0;
		commandBufferWriteIndex = 0;
		USART_WriteByteResultCode(0xF0);
		USART_WriteChar('\n');

		//uint16_t adcValue = ADC_read(ADC5D);
		//itoa(adcValue, str, 10);
		//USART_putstring(str);
		//USART_send('\r');
	}

	return 0;
}

uint8_t HexToByte(uint8_t hh, uint8_t hl)
{
	uint8_t result = 0;

	if (hh >= '0' && hh <= '9')
	{
		result = hh - '0';
	}
	else if (hh >= 'A' && hh <= 'F')
	{
		result = (hh - 'A') + 0xA;
	}
	else if (hh >= 'a' && hh <= 'f')
	{
		result = (hh - 'a') + 0xA;
	}

	result = result << 4;

	if (hl >= '0' && hl <= '9')
	{
		result |= hl - '0';
	}
	else if (hl >= 'A' && hl <= 'F')
	{
		result |= (hl - 'A') + 0xA;
	}
	else if (hl >= 'a' && hl <= 'f')
	{
		result |= (hl - 'a') + 0xA;
	}

	return result;
}
