Initial commit

32 files changed, 2638 insertions, 0 deletions

diff --git a/impl/DiagnosticModule/DiagnosticModule.atsln b/impl/DiagnosticModule/DiagnosticModule.atsln
new file mode 100644
index 0000000..b4cab75
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule.atsln
@@ -0,0 +1,19 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Atmel Studio Solution File, Format Version 11.00
+VisualStudioVersion = 14.0.23107.0
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{54F91283-7BC4-4236-8FF9-10F437C3AD48}") = "DiagnosticModule", "DiagnosticModule\DiagnosticModule.cproj", "{DCE6C7E3-EE26-4D79-826B-08594B9AD897}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Release|AVR = Release|AVR
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Release|AVR.ActiveCfg = Release|AVR
+		{DCE6C7E3-EE26-4D79-826B-08594B9AD897}.Release|AVR.Build.0 = Release|AVR
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+EndGlobal
diff --git a/impl/DiagnosticModule/DiagnosticModule/DiagnosticModule.componentinfo.xml b/impl/DiagnosticModule/DiagnosticModule/DiagnosticModule.componentinfo.xml
new file mode 100644
index 0000000..e275755
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/DiagnosticModule.componentinfo.xml
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Store xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns="AtmelPackComponentManagement">
+	<ProjectComponents />
+</Store>
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/DiagnosticModule.cproj b/impl/DiagnosticModule/DiagnosticModule/DiagnosticModule.cproj
new file mode 100644
index 0000000..03cc056
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/DiagnosticModule.cproj
@@ -0,0 +1,187 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003" ToolsVersion="14.0">
+  <PropertyGroup>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectVersion>7.0</ProjectVersion>
+    <ToolchainName>com.Atmel.AVRGCC32.C</ToolchainName>
+    <ProjectGuid>dce6c7e3-ee26-4d79-826b-08594b9ad897</ProjectGuid>
+    <avrdevice>AT32UC3C2512C</avrdevice>
+    <avrdeviceseries>none</avrdeviceseries>
+    <OutputType>StaticLibrary</OutputType>
+    <Language>C</Language>
+    <OutputFileName>lib$(MSBuildProjectName)</OutputFileName>
+    <OutputFileExtension>.a</OutputFileExtension>
+    <OutputDirectory>$(MSBuildProjectDirectory)\$(Configuration)</OutputDirectory>
+    <AvrGccProjectExtensions>
+    </AvrGccProjectExtensions>
+    <AssemblyName>DiagnosticModule</AssemblyName>
+    <Name>DiagnosticModule</Name>
+    <RootNamespace>DiagnosticModule</RootNamespace>
+    <ToolchainFlavour>Native</ToolchainFlavour>
+    <KeepTimersRunning>true</KeepTimersRunning>
+    <OverrideVtor>false</OverrideVtor>
+    <CacheFlash>true</CacheFlash>
+    <ProgFlashFromRam>true</ProgFlashFromRam>
+    <RamSnippetAddress>0x20000000</RamSnippetAddress>
+    <UncachedRange />
+    <preserveEEPROM>true</preserveEEPROM>
+    <OverrideVtorValue>exception_table</OverrideVtorValue>
+    <BootSegment>2</BootSegment>
+    <eraseonlaunchrule>0</eraseonlaunchrule>
+    <AsfFrameworkConfig>
+      <framework-data xmlns="">
+        <options />
+        <configurations />
+        <files />
+        <documentation help="" />
+        <offline-documentation help="" />
+        <dependencies>
+          <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.34.1" />
+        </dependencies>
+      </framework-data>
+    </AsfFrameworkConfig>
+    <ResetRule>0</ResetRule>
+    <EraseKey />
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
+    <ToolchainSettings>
+      <Avr32Gcc>
+        <avr32gcc.common.outputfiles.hex>True</avr32gcc.common.outputfiles.hex>
+        <avr32gcc.common.outputfiles.lss>True</avr32gcc.common.outputfiles.lss>
+        <avr32gcc.common.outputfiles.eep>True</avr32gcc.common.outputfiles.eep>
+        <avr32gcc.common.outputfiles.srec>True</avr32gcc.common.outputfiles.srec>
+        <avr32gcc.compiler.symbols.DefSymbols>
+          <ListValues>
+            <Value>NDEBUG</Value>
+          </ListValues>
+        </avr32gcc.compiler.symbols.DefSymbols>
+        <avr32gcc.compiler.directories.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\UC3C_DFP\1.0.49\include\AT32UC3C2512C</Value>
+            <Value>..</Value>
+          </ListValues>
+        </avr32gcc.compiler.directories.IncludePaths>
+        <avr32gcc.compiler.optimization.level>Optimize (-O1)</avr32gcc.compiler.optimization.level>
+        <avr32gcc.compiler.optimization.PrepareFunctionsForGarbageCollection>True</avr32gcc.compiler.optimization.PrepareFunctionsForGarbageCollection>
+        <avr32gcc.compiler.optimization.UseAssemblerForPseudoInstructions>True</avr32gcc.compiler.optimization.UseAssemblerForPseudoInstructions>
+        <avr32gcc.compiler.warnings.AllWarnings>True</avr32gcc.compiler.warnings.AllWarnings>
+        <avr32gcc.compiler.warnings.ExtraWarnings>True</avr32gcc.compiler.warnings.ExtraWarnings>
+        <avr32gcc.compiler.warnings.Undefined>True</avr32gcc.compiler.warnings.Undefined>
+        <avr32gcc.compiler.warnings.WarningsAsErrors>True</avr32gcc.compiler.warnings.WarningsAsErrors>
+        <avr32gcc.linker.libraries.Libraries>
+          <ListValues>
+            <Value>libm</Value>
+          </ListValues>
+        </avr32gcc.linker.libraries.Libraries>
+        <avr32gcc.linker.optimization.GarbageCollectUnusedSections>True</avr32gcc.linker.optimization.GarbageCollectUnusedSections>
+        <avr32gcc.linker.optimization.PutReadOnlyDataInWritableDataSection>True</avr32gcc.linker.optimization.PutReadOnlyDataInWritableDataSection>
+        <avr32gcc.linker.optimization.AllowDirectReferencesToDataSection>True</avr32gcc.linker.optimization.AllowDirectReferencesToDataSection>
+        <avr32gcc.assembler.general.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\UC3C_DFP\1.0.49\include\AT32UC3C2512C</Value>
+          </ListValues>
+        </avr32gcc.assembler.general.IncludePaths>
+        <avr32gcc.preprocessingassembler.general.IncludePaths>
+          <ListValues>
+            <Value>%24(PackRepoDir)\atmel\UC3C_DFP\1.0.49\include\AT32UC3C2512C</Value>
+          </ListValues>
+        </avr32gcc.preprocessingassembler.general.IncludePaths>
+      </Avr32Gcc>
+    </ToolchainSettings>
+  </PropertyGroup>
+  <ItemGroup>
+    <Folder Include="clock\" />
+    <Folder Include="comm\" />
+    <Folder Include="diagnostic_module\" />
+    <Folder Include="flash\" />
+    <Folder Include="protocol\" />
+    <Folder Include="utils\" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="clock\pll.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="clock\pll.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="clock\wait.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="clock\wait.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="comm\bt.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="comm\bt.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="comm\usart.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="comm\usart.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\debug.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\debug.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\diagnostic_module.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\diagnostic_module.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\firmware_identification.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\firmware_identification.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="diagnostic_module\parameters.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="flash\exec.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="flash\exec.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="flash\flash.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="flash\flash.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="protocol\data.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="protocol\data.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="protocol\frame.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="protocol\frame.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="protocol\return_codes.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\byte_buffer.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\crc32q.c">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\crc32q.h">
+      <SubType>compile</SubType>
+    </Compile>
+    <Compile Include="utils\system_registers.h">
+      <SubType>compile</SubType>
+    </Compile>
+  </ItemGroup>
+  <Import Project="$(AVRSTUDIO_EXE_PATH)\\Vs\\Compiler.targets" />
+</Project>
diff --git a/impl/DiagnosticModule/DiagnosticModule/Release/libDiagnosticModule.a b/impl/DiagnosticModule/DiagnosticModule/Release/libDiagnosticModule.a
new file mode 100644
index 0000000..91ebfd7
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/Release/libDiagnosticModule.a
diff --git a/impl/DiagnosticModule/DiagnosticModule/clock/pll.c b/impl/DiagnosticModule/DiagnosticModule/clock/pll.c
new file mode 100644
index 0000000..b675f09
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/clock/pll.c
@@ -0,0 +1,91 @@
+/* Author: Jan Sucan */
+
+#include <avr32/io.h>
+#include <stdint.h>
+
+#include <clock/pll.h>
+
+static void pll_activate_xtal(void);
+static void pll_activate_pll(void);
+
+/**
+ * @brief Len aktivuje krystalovy oscilator, neprepina nan ako na zdroj hodin.
+ */
+void
+pll_activate_xtal(void)
+{
+	uint32_t tmpReg;
+
+	/* Zapnutie externeho kr. oscilatora 0 */
+	tmpReg = 0;
+	tmpReg |= 3 << AVR32_SCIF_GAIN_OFFSET; /* Frekvencia krystalu > 16 MHz*/
+	tmpReg |= AVR32_SCIF_OSCCTRL0_MODE_CRYSTAL << AVR32_SCIF_OSCCTRL_MODE_OFFSET; /* Pouzitie pinov krystalu 0 */
+	tmpReg |= AVR32_SCIF_OSCCTRL_OSCEN_ENABLE << AVR32_SCIF_OSCCTRL_OSCEN_OFFSET; /* Aktivacia oscilatora 0 */
+	/* Odomknutie registra OSCCTRL0 */
+	AVR32_SCIF.unlock = (0xAA << 24) | AVR32_SCIF_OSCCTRL;
+	/* Zapis do odomknuteho registru */
+	AVR32_SCIF.oscctrl[0] = tmpReg;
+
+	/* Cakanie na stabilizovanie krystalu */
+	while ((AVR32_SCIF.pclksr & AVR32_SCIF_PCLKSR_OSC0RDY_MASK) == 0) {
+		;
+	}	
+}
+
+/**
+ * @brief Len aktivuje PLL, neprepina nan ako na zdroj hodin.
+ *
+ *	xtalFreq  = 18.432 MHz
+ *	pllDiv    = 2
+ *	pllMul    = 13
+ *	pllOutDiv = 1
+ *	pllFreq   = 64.512 MHz
+ *	pllVco    = 129.024 MHz
+ */
+void
+pll_activate_pll(void)
+{
+	uint32_t tmpReg;
+	
+	/* Nastavenie zdroja pre PLL na Oscilator 0 */
+	tmpReg = 0;
+	/* Nastavenie nasobicky PLL */
+	tmpReg |= 13 << AVR32_SCIF_PLL_PLLMUL_OFFSET;
+	/* Nastavenie delicky PLL */
+	tmpReg |= 2 << AVR32_SCIF_PLL_PLLDIV_OFFSET;
+	/* Nastavenie rozsahu Vco */
+	tmpReg |= 0 << AVR32_SCIF_PLLOPT_OFFSET;
+	/* Nastavenie delenia frekvencie Vco */
+	tmpReg |= 1 << (AVR32_SCIF_PLLOPT_OFFSET + 1);
+	/* Povolenie PLL */
+	tmpReg |= 1 << AVR32_SCIF_PLLEN_OFFSET;
+	/* Odomknutie registra PLL0 */
+	AVR32_SCIF.unlock = (0xAA << 24) | AVR32_SCIF_PLL;
+	/* Zapis do odomknuteho registru */
+	AVR32_SCIF.pll[0] = tmpReg;
+
+	/* Cakanie na uzamknutie PLL0 aby sa dala pouzit ako zdroj hodin */
+	while ((AVR32_SCIF.pclksr & AVR32_SCIF_PCLKSR_PLL0_LOCK_MASK) == 0) {
+		;
+	}
+}
+
+/**
+ * @brief Prepne na PLL ako hlavny zdroj hodin na 64.512 MHz.
+ */
+void
+pll_use_as_main_clock(void)
+{
+	// Nastavenie Wait-State pre FLASH na 1
+	// Toto je nutne od frekvencie vyssej ako 33 MHz
+	AVR32_FLASHC.fcr |= 1 << AVR32_FLASHC_FCR_FWS_OFFSET;
+	
+	pll_activate_xtal();
+	pll_activate_pll();
+	
+	// Prepnutie na PLL0 ako hlavny zdroj hodin
+	// Odomknutie registra PLL0
+	AVR32_PM.unlock = (0xAA << 24) | AVR32_PM_MCCTRL;
+	// Zapis do odomknuteho registra
+	AVR32_PM.mcctrl = AVR32_PM_MCCTRL_MCSEL_PLL0 << AVR32_PM_MCCTRL_MCSEL_OFFSET;	
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/clock/pll.h b/impl/DiagnosticModule/DiagnosticModule/clock/pll.h
new file mode 100644
index 0000000..edc9510
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/clock/pll.h
@@ -0,0 +1,8 @@
+/* Author: Jan Sucan */
+
+#ifndef PLL_H_
+#define PLL_H_
+
+void pll_use_as_main_clock(void);
+
+#endif /* PLL_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/clock/wait.c b/impl/DiagnosticModule/DiagnosticModule/clock/wait.c
new file mode 100644
index 0000000..0fcab08
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/clock/wait.c
@@ -0,0 +1,70 @@
+/* Author: Jan Sucan */
+
+#include <stdint.h>
+#include <clock/wait.h>
+#include <utils/system_registers.h>
+
+/**
+ * @brief Hodnota systemoveho registra COUNT nacitana za 1 ms.
+ *
+ * Plati pre pre hlavne hodiny na 64,512 MHz.
+ */
+#define COUNT_DIFF_FOR_1_MS 64512
+
+/**
+ * @brief Zisti, ci bol prekroceny casovy deadline.
+ *
+ * @warning Predpoklada sa, ze od ziskania casovej znacky vo funkcii wait_get_timestamp()
+ * po ziskanie casu v tejto funkcii, neuplynie viac ako priblizne 66 sekund (2^32 - 1 tikov casovaca).
+ *
+ * @param deadline  Ukazovatel na casovy deadline ziskany funkciou wait_get_timestamp().
+ *
+ * @return true   Deadline bol prekroceny.
+ * @return false  Deadline este nebol prekroceny.
+ */
+bool
+wait_has_deadline_expired(const deadline_t * const deadline)
+{	
+	// Pocet tikov casovaca pre zadany pocet milisekund v deadline
+	uint32_t ticks_ms = deadline->ms * COUNT_DIFF_FOR_1_MS;
+		
+	uint32_t b = SYSREG_COUNT_GET;
+	if (deadline->timestamp <= b) {
+		// Casovac od ziskania deadlinu nepretiekol, uplynuty cas sa jednoducho ziska rozdielom casov
+		b = b - deadline->timestamp;
+	} else {
+		// Casovac od ziskania deadlinu pretiekol
+		b = ((uint32_t) 0) - deadline->timestamp + b;
+	}	
+
+	return (b >= ticks_ms); 
+}
+
+/**
+ * @brief Pocka zadany pocet milisekund.
+ *
+ * Funkcia implementuje blokujuce cakanie.
+ *
+ * @param ms  Pocet milisekund, kolko sa bude cakat.
+ */
+void
+wait_ms(uint16_t ms)
+{
+	deadline_t d = wait_get_deadline(ms);
+	while (!wait_has_deadline_expired(&d))
+		;
+}
+
+/**
+ * @brief Ziska deadline pre cakanie.
+ *
+ * @param ms  Pocet milisekund, za kolko bude deadline prekroceny od casu zavolania funkcie.
+ */
+deadline_t
+wait_get_deadline(uint16_t ms)
+{
+	deadline_t d;
+	d.timestamp = SYSREG_COUNT_GET;
+	d.ms = ms;
+	return d;
+}
diff --git a/impl/DiagnosticModule/DiagnosticModule/clock/wait.h b/impl/DiagnosticModule/DiagnosticModule/clock/wait.h
new file mode 100644
index 0000000..4fd8a13
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/clock/wait.h
@@ -0,0 +1,31 @@
+/* Author: Jan Sucan */
+
+#ifndef WAIT_H_
+#define WAIT_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * @brief Slucka pre aktivne cakanie.
+ *
+ * Telo obsahuje NOP ako 'asm volatile', aby sa zaranilo odstraneniu slucky
+ * optimalizujucim prekladacom.
+ *
+ * @param iterations  Pocet iteracii.
+ */
+#define WAIT_ACTIVE_LOOP(iterations) \
+	for (uint32_t d = iterations; d > 0; --d) { \
+		asm volatile ("nop"); \
+	}
+
+typedef struct deadline_s {
+	uint32_t timestamp; /**< Casova znacka kedy bol deadline ziskany */
+	uint16_t ms;        /**< Pocet milisekund od deadline_s#timestamp, za kolko deadline nastane. */
+} deadline_t;
+
+void       wait_ms(uint16_t ms);
+deadline_t wait_get_deadline(uint16_t ms);
+bool       wait_has_deadline_expired(const deadline_t * const deadline);
+
+#endif /* WAIT_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/comm/bt.c b/impl/DiagnosticModule/DiagnosticModule/comm/bt.c
new file mode 100644
index 0000000..c8b04a0
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/comm/bt.c
@@ -0,0 +1,306 @@
+/* Author: Jan Sucan */
+
+#include <avr32/io.h>
+#include <stdio.h>
+#include <stdarg.h>
+
+#include <comm/bt.h>
+#include <comm/usart.h>
+#include <diagnostic_module/parameters.h>
+
+
+typedef enum {
+	COBS_WAITING_FOR_THE_FIRST_DELIMITER,
+	COBS_WAITING_FOR_STUFFED_BYTE,
+	COBS_WAITING_FOR_NON_STUFFED_BYTE
+} cobs_state_t;
+
+static void         bt_module_to_usart_pins(void);
+static bt_retcode_t bt_retcode_from_usart_retcode(usart_retcode_t ur);
+
+/**
+ * @brief Inicializacia bluetooth modulu na diagnostickom module.
+ *
+ * Nakonfiguruje sa USART, bluetooth modul sa prepoji na USART0.
+ */
+void
+bt_init(void)
+{
+	usart_init();
+	bt_module_to_usart_pins();
+}
+
+/**
+ * @brief Prepojenie signalov RxD a TxD bluetooth modulu na USART0 MCU.
+ *
+ * Vyuziva signal PRP na diagnostickom module pre ovladanie prepinacej logiky.
+ */
+void
+bt_module_to_usart_pins(void)
+{
+	/*
+		(PC21 = PRP = GPIO number 85 = port 2 pin 21) ma byt na nule
+		(PD27 = TXD0 = GPIO number 123 = port 3 pin 27) pripojeny na RxL
+		(PD28 = RXD0 = GPIO number 124 = port 3 pin 28) pripojeny na TxL
+	*/
+	
+	/* Pre PC21 je bit v GPER na 1 */
+	AVR32_GPIO.port[2].gpers = 1 << 21;
+	/* PC21 ovladany GPIO modulom, aktivacia driveru pinu */
+	AVR32_GPIO.port[2].oders = 1 << 21;
+	/* Pomocou clear registra nastavime 0 na PC21 */
+	AVR32_GPIO.port[2].ovrc = 1 << 21;
+	AVR32_GPIO.port[2].puers = 1 << 21;
+	
+	/* PD27 a PD28 su ovladane USART, vynuluju sa prislusne bity v GPER */
+	AVR32_GPIO.port[3].gperc = 1 << 27;
+	AVR32_GPIO.port[3].gperc = 1 << 28;
+	
+	/* Pre PD27 a PD28 sa vyberie funkcia A (USART0), to je default po resete */
+}
+
+/**
+ * @brief Odoslanie bajtov cez bluetooth.
+ *
+ * Odosielane znaky prechadzaju escapovacou vrstvou, aby sa zabranilo odoslanie retazca
+ * "$$$" do bluetooth modulu a neziaducej aktivacii prikazoveho rezimu bluetooth modulu.  
+ *
+ * @param buf  Buffer s bajtmi pre odoslanie.
+ * @param n    Pocet bajtov pre odoslanie z bufferu @p buf.
+ */
+void
+bt_send_data(const uint8_t *const buf, size_t n)
+{
+	for (size_t i = 0; i < n; ++i) {
+		usart_send_byte(buf[i]);
+	}
+}
+
+/**
+ * @brief Prijem bajtov cez bluetooth.
+ *
+ * @param buf           Ukazovatel na buffer pre ulozenie prijatych bajtov.
+ * @param n             Pocet bajtov pre prijatie.
+ * @param time_deadline Ukazovatel na casovy deadline, dokedy najneskor sa musia vsetky byty prijat.
+ *
+ * @return 0 v pripade uspechu.
+ * @return != 0 pri chybe prijmu znaku, alebo chybe argumentov.
+ */
+bt_retcode_t
+bt_receive_data(uint8_t * const buf, size_t n, const deadline_t * const time_deadline)
+{
+	bt_retcode_t r = BT_RECEIVE_BAD_ARGUMENT;
+	
+	// Kontrola argumentov
+	if (buf != NULL) {
+		for (size_t i = 0; i < n; ++i) {
+			// Bajty sa musia prijat v danom casovom okne trvajcom ms milisekund od timestamp
+			usart_retcode_t ur = usart_receive_byte (buf + i, time_deadline);
+			
+			if (ur != 0) {
+				r = bt_retcode_from_usart_retcode(ur);
+				break;
+			}
+		}
+		// Prijem uspesny
+		r = BT_OK;
+	}
+		
+	return r;
+}
+
+/**
+ * @brief Stavovy automat pre prijem bajtov s COBS.
+ *
+ * @param b              Bajt prijaty cez USART.
+ * @param buf            Ukazovatel na buffer pre ukladanie odstuffovanych bajtov.
+ * @param i              Ukazovatel na index do bufferu @p buf.
+ * @param cobs_continue  Priznak, ci sa jedna o pokracovanie prijmu ramca, alebo sa bude prijimat novy ramec.
+ *
+ * @return 0     Ak je vsetko OK.
+ * @return != 0  Chyba.
+ */
+bt_retcode_t
+bt_receive_cobs_automaton(uint8_t b, uint8_t * const buf, size_t * const i, bool cobs_continue)
+{
+	static cobs_state_t cobs_state = COBS_WAITING_FOR_THE_FIRST_DELIMITER;
+	static uint8_t next_non_stuffed = 0;
+	static bool interrupted = false;
+	
+	bt_retcode_t r = BT_OK;
+	
+	if (interrupted) {
+		// Delimiter bol prijaty v minulom volani tejto funkcie, 'b' je uz nasledujuci bajt
+		interrupted = false;
+		// Jeden prazdny priechod nadradenym for cyklom a pokracuje sa v aktualnej funkcii
+		++(*i);
+	} 
+	
+	// Frame delimiter resetuje stavovy automat a prijem bajtov
+	if (b == COMM_PARAMS_COBS_DELIMITER_CHAR) {
+		if (!cobs_continue) {
+			cobs_state = COBS_WAITING_FOR_STUFFED_BYTE;
+			*i = 0;
+		} else {
+			interrupted = true;
+			r = BT_RECEIVE_COBS_INTERRUPTED;
+		}
+	} else {
+		switch (cobs_state) {
+		case COBS_WAITING_FOR_THE_FIRST_DELIMITER:
+			// Automat deaktivovany, ztial sa neprijal frame delimiter
+			break;
+				
+		case COBS_WAITING_FOR_STUFFED_BYTE:
+			// Prijal sa stuffovany byte, ziskame pocet nasledujuci nestuffovanych bajtov
+			next_non_stuffed = (b <= COMM_PARAMS_COBS_DELIMITER_CHAR) ? b : (b - 1);
+			// COBS header sa nezapisuje do dat, vsetky dalsie stuffovane ano
+			if (!cobs_continue && (*i > 1)) {
+				buf[*i - 2] = COMM_PARAMS_COBS_DELIMITER_CHAR;
+			} else {
+				buf[*i] = COMM_PARAMS_COBS_DELIMITER_CHAR;
+			}
+			cobs_state = (next_non_stuffed == 0) ? COBS_WAITING_FOR_STUFFED_BYTE : COBS_WAITING_FOR_NON_STUFFED_BYTE;
+			break;
+				
+		case COBS_WAITING_FOR_NON_STUFFED_BYTE:
+			// Prijal sa nestuffovany bajt, nic sa s nim nerobi
+			--next_non_stuffed;
+			// Len sa ulozi
+			buf[*i - ((cobs_continue) ? 0 : 2)] = b;
+			cobs_state = (next_non_stuffed == 0) ? COBS_WAITING_FOR_STUFFED_BYTE : COBS_WAITING_FOR_NON_STUFFED_BYTE;
+			break;
+				
+		default:
+			r = BT_RECEIVE_COBS_UNKNOWN_STATE;
+			break;
+		}
+	}
+
+	return r;	
+}
+
+/**
+ * @brief Prijem bajtov s COBS.
+ *
+ * @param buf            Ukazovatel na buffer kam budu ulozene prijate bajty.
+ * @param n              Pocet bajtov pre prijatie.
+ * @param time_deadline  Deadline, dokedy sa musi prijat @p n bajtov.
+ * @param cobs_continue  Priznak, ci sa jedna o pokracovanie prijmu ramca, alebo sa bude prijimat novy ramec.
+ *
+ * @return 0     Ak je vsetko OK.
+ * @return != 0  Chyba.
+ */
+bt_retcode_t
+bt_receive_cobs_data(uint8_t * const buf, size_t n, const deadline_t * const time_deadline, bool cobs_continue)
+{	
+	bt_retcode_t r = BT_RECEIVE_BAD_ARGUMENT;
+	
+	// Kontrola argumentov
+	if (buf != NULL) {
+		size_t i;
+		// Na zaciatku dat sa prijma navyse COBS frame delimiter (1B) a COBS header (1B)
+		if (!cobs_continue) {
+			n += 2;
+		}
+		
+		for (i = 0; i < n; ++i) {
+			uint8_t b;
+			const usart_retcode_t ur = usart_receive_byte (&b, time_deadline);
+		
+			if (ur) {
+				// Chyba pri prijme bajtu
+				r = bt_retcode_from_usart_retcode(ur);
+				break;
+			}
+		
+			 const bt_retcode_t ar = bt_receive_cobs_automaton(b, buf, &i, cobs_continue);
+			 
+			 if (ar != BT_OK) {
+				 r = ar;
+				 break;
+			 }
+		}
+		
+		// Uspesny prijem a odsfuttovanie vsetkych bajtov
+		if (i >= n) {
+			r = BT_OK;
+		}
+	}
+	
+	return r;
+}
+
+/**
+ * @brief Odoslanie bajtov s COBS.
+ *
+ * @param buf Ukazovatel na buffer bajtov pre odoslanie.
+ * @param n   Pocet bajtov pre odoslanie.
+ */
+void
+bt_send_cobs_data_block(const uint8_t *const buf, size_t n)
+{	
+	// Kontrola argumentov
+	if (buf == NULL) {
+		return;
+	}
+	// POZOR: neosetrujeme velkost dat, moze dojst k preteceniu hodnot na stuffovanych bajtoch
+	
+	// Odosle sa delimiter
+	usart_send_byte(COMM_PARAMS_COBS_DELIMITER_CHAR);
+	
+	uint8_t next_non_stuffed = 0;
+	size_t send_index = 0 - 1;
+		
+	for (size_t i = 0; i <= n; ++i) {
+		if ((i == n) || (buf[i] == COMM_PARAMS_COBS_DELIMITER_CHAR)) {
+			usart_send_byte((next_non_stuffed >= COMM_PARAMS_COBS_DELIMITER_CHAR) ? (next_non_stuffed + 1) : next_non_stuffed);
+			// Zacne sa odosielat az za virtualnym, alebo realnym stuffovanym bajtom
+			++send_index;
+			// Odoslu sa napocitane bajty
+			while (next_non_stuffed > 0) {
+				usart_send_byte(buf[send_index]);
+				++send_index;
+				--next_non_stuffed;
+			}
+		} else {
+			// Pocitaju sa nestuffovane bajty, zatial sa nic neposiela
+			++next_non_stuffed;
+		}
+	}
+}
+
+/**
+ * @brief Konverzia navratovej hodnoty z USART vrsty do kodov pre aktualnu vrstvu.
+ *
+ * Skonvertovany kod je vrateny vyssim vrstvam.
+ *
+ * @param ur  Navratovy kod z mnoziny usart_retcode_t.
+ *
+ * @return Navratovy kod z mnoziny bt_retcode_t.
+ */
+bt_retcode_t
+bt_retcode_from_usart_retcode(usart_retcode_t ur)
+{
+	bt_retcode_t r = BT_RECEIVE_BAD_ARGUMENT;
+	
+	switch (ur) {
+	case USART_RECEIVE_BAD_ARGUMENT:
+		r = BT_RECEIVE_BAD_ARGUMENT;
+		break;
+		
+	case USART_RECEIVE_TIMEOUT:
+		r = BT_RECEIVE_TIMEOUT;
+		break;
+			
+	case USART_RECEIVE_ERROR:
+		r = BT_RECEIVE_ERROR;
+		break;
+		
+	case USART_OK:
+		r = BT_OK;
+		break;	
+	}
+	
+	return r;
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/comm/bt.h b/impl/DiagnosticModule/DiagnosticModule/comm/bt.h
new file mode 100644
index 0000000..c079529
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/comm/bt.h
@@ -0,0 +1,43 @@
+/* Author: Jan Sucan */
+
+#ifndef BT_H_
+#define BT_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <limits.h>
+
+#include <clock/wait.h>
+
+/**
+ * @brief Konstanta pre funkciu bt_receive_cobs_data().
+ *
+ * Signalizuje, ze funkcia caka na COBS delimiter a zacne spracovavat novy ramec.
+ */
+#define BT_RECEIVE_COBS_START  false
+
+/**
+ * @brief Konstanta pre funkciu bt_receive_cobs_data().
+ *
+ * Signalizuje, ze volanie funkcie bude pokracovat v spracovavani zacateho ramca
+ * a prijem frame delimiteru bude povazovany za chybu.
+ */
+#define BT_RECEIVE_COBS_CONTINUE  true
+
+typedef enum {
+	BT_RECEIVE_BAD_ARGUMENT = INT_MIN,
+	BT_RECEIVE_TIMEOUT,
+	BT_RECEIVE_ERROR,
+	BT_RECEIVE_COBS_INTERRUPTED,
+	BT_RECEIVE_COBS_UNKNOWN_STATE,
+	BT_OK = 0
+} bt_retcode_t;
+
+void         bt_init(void);
+bt_retcode_t bt_receive_data(uint8_t * const buf, size_t n, const deadline_t *const time_deadline);
+bt_retcode_t bt_receive_cobs_data(uint8_t * const buf, size_t n, const deadline_t * const time_deadline, bool cobs_continue);
+void         bt_send_data(const uint8_t *const buf, size_t n);
+void         bt_send_cobs_data_block(const uint8_t *const buf, size_t n);
+
+#endif /* BT_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/comm/usart.c b/impl/DiagnosticModule/DiagnosticModule/comm/usart.c
new file mode 100644
index 0000000..ae85f50
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/comm/usart.c
@@ -0,0 +1,197 @@
+/* Author: Jan Sucan */
+
+#include <avr32/io.h>
+#include <stdlib.h>
+#include <stdbool.h>
+
+#include <comm/usart.h>
+#include <clock/wait.h>
+#include <diagnostic_module/parameters.h>
+
+/**
+ * @brief Maximalny pocet bajtov, co sa prijmu cez USART0 a zahodia.
+ *
+ * Tato hodnota sa pouziva pre vycistenie seriovej linky v smere prijmu. Experimentale bolo zistene, ze sa v prijimacom smere
+ * pri prijme prvych bajtov objavuju bajty, ktore neodoslal PC. Bajty od PC nasledovali az za tymyto odpadovymi bajtmi.
+ */
+#define USART_MAX_RX_JUNK_BYTES  8
+
+static bool usart_is_byte_transmitted(void);
+static bool usart_is_byte_received(void);
+static void usart_clear_error_flags(void);
+static bool usart_is_error(void);
+static bool usart_is_error_framing(void);
+static bool usart_is_error_overflow(void);
+
+
+/**
+ * @brief Inicializacia rozhrania USART0, pouziteho pre komunikaciu s bluetooth modulom.
+ *
+ * Bluetooth modul je defaultne na 115 200 Bd. Format komunikacie je 1 stop bit, 8 datovych bitov,
+ * bez parity.
+ */
+void
+usart_init(void)
+{	
+	/* Nastavenie baudrate. Bit OVER = 0 a CD = 35. Vyjde presne 115 200 Bd pri 64,512 MHz */
+	AVR32_USART0.brgr = 35;
+
+	AVR32_USART0.mr &= ~AVR32_USART_MR_OVER_MASK;
+	
+	/* Nastav format UART */
+	/* Jeden stop bit */
+	AVR32_USART0.mr &= ~AVR32_USART_MR_NBSTOP_MASK;
+	/* Bez parity */
+	AVR32_USART0.mr &= ~AVR32_USART_MR_PAR_MASK;
+	AVR32_USART0.mr |= AVR32_USART_MR_PAR_NONE << AVR32_USART_MR_PAR_OFFSET;
+	/* 8 datovych bitov */
+	AVR32_USART0.mr |= AVR32_USART_MR_CHRL_MASK;
+	
+	/* Normalny mod kanalu, ziadne echo ani loopback */
+	AVR32_USART0.mr &= ~AVR32_USART_MR_CHMODE_MASK;
+	/* Normalny mod */
+	AVR32_USART0.mr &= ~AVR32_USART_MR_MODE_MASK;
+	
+	/* Zhodenie priznakov chyby */
+	usart_clear_error_flags();
+	
+	/* Povolenia vysielania a prijmu */
+	AVR32_USART0.cr &= ~AVR32_USART_CR_TXDIS_MASK;
+	AVR32_USART0.cr |= AVR32_USART_CR_TXEN_MASK;
+	
+	AVR32_USART0.cr &= ~AVR32_USART_CR_RXDIS_MASK;
+	AVR32_USART0.cr |= AVR32_USART_CR_RXEN_MASK;	
+}
+
+/**
+ * @brief Prijem jedneho bajtu cez USART0.
+ *
+ * @param byte          Ukazovatel na buffer pre ulozenie jedneho bajtu.
+ * @param time_deadline Ukazovatel na casovy deadline, dokedy najneskor sa ma byte prijat. Ak bude NULL, bude sa cakat na prijem donekonecna.
+ *
+ * @return 0 v pripade uspechu, -1 ak bol prekroceny casovy deadline @p time_deadline pre prijem.
+ */
+usart_retcode_t
+usart_receive_byte(uint8_t *byte, const deadline_t * const time_deadline)
+{
+	usart_retcode_t r = USART_RECEIVE_BAD_ARGUMENT; // Defaultna navratova hodnota
+	
+	/* Priznaky chyby sa resetuju, aby nebranili nasledujucemu prijmu bajtov.
+	 * Priznak chyby OVR (pretecenie) sa moze nastavit vtedy, ked sa diagnostickemu modulu niekto
+	 * pokusi poslat viac ako 1 bajt vtedy, ked ich FW nie je pripraveny z prijimacieho
+	 * bufferu precitat.
+	 */
+	usart_clear_error_flags();
+
+	if (byte != NULL) {
+		/* Caka sa, az sa prijme bajt */
+		while ((AVR32_USART0.csr & AVR32_USART_CSR_RXRDY_MASK) == 0) {
+			if (usart_is_error()) {
+				/* Chyba prijmu */
+				break;
+			} else if (time_deadline == NULL) {
+				/* Timeout sa nebude kontrolovat */
+				continue;
+			} else if (wait_has_deadline_expired(time_deadline)) {
+				/* Timeout bol zadany a vyprsal */
+				break;
+			}
+		}
+
+		if (usart_is_byte_received()) {
+			/* Prijal sa byte */
+			*byte = AVR32_USART0.rhr;
+			r =  USART_OK;
+		} else if (usart_is_error()) {
+			/* Framing error (nespravny stop bit) alebo pretecenie prijiamcieho bufferu */
+			r = USART_RECEIVE_ERROR;
+		} else {
+			r = USART_RECEIVE_TIMEOUT;
+		}
+	}
+	
+	return r;
+}
+
+/**
+ * @brief Odoslanie jedneho bajtu cez USART0.
+ *
+ * @param byte  Bajt pre odoslanie cez USART0.
+ */
+void
+usart_send_byte(uint8_t byte)
+{
+	/* Pocka sa na uvolnenie vysielacieho bufferu */
+	while (!usart_is_byte_transmitted());
+	/* Bajt sa odosle */
+	AVR32_USART0.thr = byte;
+}
+
+/**
+ * @brief Zisti, ci je vysielaci buffer pripraveny pre zapis dalsieho bajtu pre odoslanie.
+ *
+ * @return true   Vysielaci buffer pripraveny.
+ * @return false  Vysielaci buffer este nie je pripraveny.
+ */
+bool
+usart_is_byte_transmitted(void)
+{
+	return ((AVR32_USART0.csr & AVR32_USART_CSR_TXRDY_MASK) != 0);
+}
+
+/**
+ * @brief Zisti, ci je prijimaci buffer pripraveny pre precitanie prijateho bajtu.
+ *
+ * @return true   Prijimaci buffer pripraveny.
+ * @return false  Prijimaci buffer este nie je pripraveny.
+ */
+bool
+usart_is_byte_received(void)
+{
+	return ((AVR32_USART0.csr & AVR32_USART_CSR_RXRDY_MASK) != 0);
+}
+
+/**
+ * @brief Zhodi priznaky chyb USART0.
+ */
+void
+usart_clear_error_flags(void)
+{
+	AVR32_USART0.cr |= AVR32_USART_CR_RSTSTA_MASK;
+}
+
+/**
+ * @brief Zisti, ci doslo k chybe USART0.
+ *
+ * @return true   Chyba protokolu seriovej linky, alebo pretecenie prijimacieho bfferu.
+ * @return false  Vsetko v poriadku.
+ */
+bool
+usart_is_error(void)
+{
+	return (usart_is_error_framing() || usart_is_error_overflow());
+}
+
+/**
+ * @brief Zisti, ci doslo k chybe protokolu seriovej linky USART0.
+ *
+ * @return true   Chyba protokolu seriovej linky.
+ * @return false  Vsetko v poriadku.
+ */
+bool
+usart_is_error_framing(void)
+{
+	return ((AVR32_USART0.csr & AVR32_USART_CSR_FRAME_MASK) != 0);
+}
+
+/**
+ * @brief Zisti, ci doslo k preteceniu prijimacieho bufferu USART0.
+ *
+ * @return true   Pretiekol prijimaci buffer.
+ * @return false  Vsetko v poriadku.
+ */
+bool
+usart_is_error_overflow(void)
+{
+	return ((AVR32_USART0.csr & AVR32_USART_CSR_OVRE_MASK) != 0);
+}
diff --git a/impl/DiagnosticModule/DiagnosticModule/comm/usart.h b/impl/DiagnosticModule/DiagnosticModule/comm/usart.h
new file mode 100644
index 0000000..694ac01
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/comm/usart.h
@@ -0,0 +1,26 @@
+/* Author: Jan Sucan */
+
+#ifndef USART_H_
+#define USART_H_
+
+#include <stdint.h>
+#include <limits.h>
+
+#include <clock/wait.h>
+
+/**
+ * @brief Navratove kody funkcie pre prijem bajtu cez USART0.
+ */
+typedef enum {
+	USART_RECEIVE_BAD_ARGUMENT = INT_MIN,
+	USART_RECEIVE_TIMEOUT,
+	USART_RECEIVE_ERROR,
+	USART_OK = 0
+} usart_retcode_t;
+
+void             usart_init(void);
+void             usart_clear_receive_line(void);
+usart_retcode_t  usart_receive_byte(uint8_t *byte, const deadline_t * const time_deadline);
+void             usart_send_byte(uint8_t byte);
+
+#endif /* USART_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/debug.c b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/debug.c
new file mode 100644
index 0000000..b50a292
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/debug.c
@@ -0,0 +1,32 @@
+/* Author: Jan Sucan */
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#include <diagnostic_module/debug.h>
+#include <comm/usart.h>
+
+/**
+ * @brief Printf-like funkcia vypisujuca na seriovu linku.
+ *
+ * Velmi uzitocna pre debugovanie programov pre diagnosticky modul, kedze
+ * ten nema ine indikacne prvky. Retazce maximalnej dlzky 1023 znakov
+ * posiela cez seriovu linku a bluetooth modul na seriovy terminal.
+ *
+ * @param format  Formatovaci retezec vypisu. Vid printf().
+ * @param[in] ... Argumenty pre formatovaci retazec. Vid printf().
+ */
+void
+debug_usart_printf(const char* format, ...) {
+	char buf[1024];
+
+	va_list args;
+	va_start(args, format);
+	const int r = vsnprintf(buf, 1024, format, args);
+	va_end(args);
+	if (r >= 0) {
+		for (size_t i = 0; buf[i] != '\0'; ++i) {
+			usart_send_byte(buf[i]);
+		}
+	}
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/debug.h b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/debug.h
new file mode 100644
index 0000000..0184632
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/debug.h
@@ -0,0 +1,8 @@
+/* Author: Jan Sucan */
+
+#ifndef DEBUG_H_
+#define DEBUG_H_
+
+void debug_usart_printf(const char* format, ...);
+
+#endif /* DEBUG_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/diagnostic_module.c b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/diagnostic_module.c
new file mode 100644
index 0000000..54864e9
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/diagnostic_module.c
@@ -0,0 +1,18 @@
+/* Author: Jan Sucan */
+
+#include <diagnostic_module/diagnostic_module.h>
+
+/**
+ * @brief Inicializacia diagnostickeho modulu.
+ *
+ * Funkcia inicializuje casti diagnostickeho modulu pouzite v kazdej aplikacii.
+ * Prepne hodiny na 64,512 MHz a nastavi komunikaciu cez bluetooth modul protokolom YUP.
+ * Predvypocita sa tabulka hodnot pre CRC32-Q.
+ */
+void
+diagnostic_module_init(void)
+{
+	pll_use_as_main_clock();
+	crc32q_init();
+	bt_init();
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/diagnostic_module.h b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/diagnostic_module.h
new file mode 100644
index 0000000..c7b1758
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/diagnostic_module.h
@@ -0,0 +1,24 @@
+/* Author: Jan Sucan */
+
+#ifndef DIAGNOSTIC_MODULE_H_
+#define DIAGNOSTIC_MODULE_H_
+
+#include <diagnostic_module/firmware_identification.h>
+
+#include <clock/pll.h>
+#include <clock/wait.h>
+
+#include <comm/bt.h>
+
+#include <flash/exec.h>
+#include <flash/flash.h>
+
+#include <protocol/data.h>
+
+#include <utils/byte_buffer.h>
+#include <utils/crc32q.h>
+#include <utils/system_registers.h>
+
+void diagnostic_module_init(void);
+
+#endif /* DIAGNOSTIC_MODULE_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/firmware_identification.c b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/firmware_identification.c
new file mode 100644
index 0000000..c35075d
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/firmware_identification.c
@@ -0,0 +1,29 @@
+/* Author: Jan Sucan */
+
+
+/**
+ * @brief Vytvori z mena a Git shorthash identifikacny retazec aplikacie.
+ *
+ * @param dest           Buffer pre ulozenie vysledneho ID retazca.
+ * @param app_name       Meno aplikacie.
+ * @param git_shorthash  Git shorthash.
+ *
+ * @return Dlzka retazca bez ukoncovacieho nuloveho bajtu.
+ */
+int
+firmware_identification_string(char * dest, const char * app_name, const char * git_shorthash)
+{
+	// Povodna hodnota pointeru na buffer pre vypocet dlzky skonstruovaneho ID retazca
+	char * dest_orig = dest;
+	// Najprv bude nazov aplikacie
+	while (*app_name != '\0') {
+		*(dest++) = *(app_name++);
+	}
+	// Medzera
+	*(dest++) = ' ';
+	// Nakoniec Git shorthash
+	while (*git_shorthash != '\0') {
+		*(dest++) = *(git_shorthash++);
+	}
+	return dest - dest_orig;
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/firmware_identification.h b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/firmware_identification.h
new file mode 100644
index 0000000..9d04299
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/firmware_identification.h
@@ -0,0 +1,8 @@
+/* Author: Jan Sucan */
+
+#ifndef FIRMWARE_IDENTIFICATION_H_
+#define FIRMWARE_IDENTIFICATION_H_
+
+int firmware_identification_string(char * dest, const char * app_name, const char * git_shorthash);
+
+#endif /* FIRMWARE_IDENTIFICATION_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/parameters.h b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/parameters.h
new file mode 100644
index 0000000..fd576de
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/diagnostic_module/parameters.h
@@ -0,0 +1,40 @@
+/* Author: Jan Sucan */
+
+#ifndef PARAMETERS_H_
+#define PARAMETERS_H_
+
+/**
+ * @brief Velkost FLASH v strankach vyhradena pre bootloader od stranky 0 (vratane).
+ */
+#define DM_BOOTLOADER_SIZE_IN_PAGES  32
+
+/**
+ * @brief Najnizsia stranka vo FLASH, na ktorej mozu byt zapisane aplikacne data.
+ */
+#define DM_APPLICATION_REAL_MIN_PAGE  DM_BOOTLOADER_SIZE_IN_PAGES
+
+/**
+ * @brief Najvyssia stranka vo FLASH, na ktorej mozu byt zapisane aplikacne data.
+ */
+#define DM_APPLICATION_REAL_MAX_PAGE  ((AVR32_FLASH_SIZE / AVR32_FLASH_PAGE_SIZE) - 1)
+
+/**
+ * @brief Najnizsia stranka vo virtualnej FLASH, na ktorej mozu byt zapisane aplikacne data.
+ *
+ * Tym, ze Bootloader obsadi cast FLASH, zostane menej miesta na aplikacne data. FLASH sa bude
+ * tvarit pre klienta, ze ma menej stranok ako v skutocnosti. Virtualne adresy su relativne k
+ * strankovemu priestoru pre aplikacne data.
+ */
+#define DM_APPLICATION_VIRT_MIN_PAGE  0
+
+/**
+ * @brief najvyssia stranka vo virtualnej FLASH, na ktorej mozu byt zapisane aplikacne data.
+ */
+#define DM_APPLICATION_VIRT_MAX_PAGE  (DM_APPLICATION_REAL_MAX_PAGE - DM_BOOTLOADER_SIZE_IN_PAGES)
+
+/**
+ * @brief Znak, ktory sa nahradzuje metodou COBS a ktory preto zaroven sluzi ako oddelovac COBS blokov dat
+ */
+#define COMM_PARAMS_COBS_DELIMITER_CHAR '$'
+
+#endif /* PARAMETERS_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/flash/exec.c b/impl/DiagnosticModule/DiagnosticModule/flash/exec.c
new file mode 100644
index 0000000..187c3c5
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/flash/exec.c
@@ -0,0 +1,33 @@
+/* Author: Jan Sucan */
+
+#include <flash/exec.h>
+#include <diagnostic_module/parameters.h>
+
+static void exec_application_firmware_at_address(uint32_t address);
+
+/**
+ * @brief Preda riadenie na zadanu adresu vo FLASH.
+ *
+ * Funkcia uz nepreda riadenie volajucemu.
+ *
+ * @param address  Adresa v adresovom priestore MCU, kam ma byt predane riadenie. 
+ */
+void
+exec_application_firmware_at_address(uint32_t address)
+{
+	void (*application_firmware)(void) = (void (*)(void)) (address);
+	application_firmware();
+}
+
+/**
+ * @brief Preda riadenie na nulte slovo zadanej stranky z FLASH pamate programu.
+ *
+ * Funkcia uz nepreda riadenie volajucemu.
+ *
+ * @param page_num  Cislo virtualnej stranky, kam ma byt riadenie predane. Hodnota sa osetruje modulo pocet virtualnych stranok.
+ */
+void
+exec_application_firmware_at_virtual_page(uint16_t page_num)
+{	
+	exec_application_firmware_at_address(AVR32_FLASH_ADDRESS + (AVR32_FLASH_PAGE_SIZE * (DM_APPLICATION_REAL_MIN_PAGE + page_num)));
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/flash/exec.h b/impl/DiagnosticModule/DiagnosticModule/flash/exec.h
new file mode 100644
index 0000000..d849b7f
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/flash/exec.h
@@ -0,0 +1,11 @@
+/* Author: Jan Sucan */
+
+#ifndef EXEC_H_
+#define EXEC_H_
+
+#include <stdint.h>
+#include <avr32/io.h>
+
+void exec_application_firmware_at_virtual_page(uint16_t page_num);
+
+#endif /* EXEC_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/flash/flash.c b/impl/DiagnosticModule/DiagnosticModule/flash/flash.c
new file mode 100644
index 0000000..4a8ac93
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/flash/flash.c
@@ -0,0 +1,246 @@
+/* Author: Jan Sucan */
+
+#include <avr32/io.h>
+#include <stdbool.h>
+
+#include <flash/flash.h>
+
+/**
+ * @brief Prevod virtualneho cisla na realne cislo stranky.
+ */
+#define FLASH_REAL_PAGE_NUMBER_FROM_VIRT(n)  (DM_APPLICATION_REAL_MIN_PAGE + n)
+
+static bool flash_is_error(void);
+static void flash_wait_until_ready(void);
+static int flash_execute_command(int command, uint16_t page_num);
+
+static int flash_erase_page(uint16_t page_num);
+static int flash_clear_page_buffer(void);
+static int flash_write_page_buffer(uint16_t page_num);
+static int flash_write_page(uint16_t page_num, const uint8_t * data, size_t data_len);
+static int flash_read_page(uint16_t page_num, uint8_t * const data);
+
+/**
+ * @brief Zisti, ci nastala chyba pri vykonavani prikazu pre kontroler FLASH pamate.
+ *
+ * @return true   Nastala chyba.
+ * @return false  Ziadna chyba nenastala.
+ */
+bool
+flash_is_error(void)
+{
+	return (((AVR32_FLASHC.fcr & AVR32_FLASHC_PROGE_MASK) == 1)
+			|| ((AVR32_FLASHC.fcr & AVR32_FLASHC_LOCKE_MASK) == 1));	
+}
+
+/**
+ * @brief Zisti, ci je kontroler FLASH pripraveny pre dalsi prikaz.
+ *
+ * @return true   Kontroler je pripraveny.
+ * @return false  Kontroler nie je pripraveny.
+ */
+void
+flash_wait_until_ready(void)
+{
+	while ((AVR32_FLASHC.fsr & AVR32_FLASHC_FSR_FRDY_MASK) == 0);
+}
+
+/**
+ * @brief Vykona zadany prikaz s FLASH pamatou na zadanej stranke.
+ *
+ * @param command   Cislo prikazu pre vykonanie.
+ * @param page_num  Cislo stranky, na ktore sa aplikuje prikaz, ak vyuziva cislo stranky.
+ *
+ * @return -1  Chyba pri vykonavani prikazu.
+ * @return  0  Prikaz prebehol v poriadku.
+ */
+int
+flash_execute_command(int command, uint16_t page_num)
+{
+	flash_wait_until_ready();
+	
+	AVR32_FLASHC.fcmd = (AVR32_FLASHC_KEY_KEY << AVR32_FLASHC_FCMD_KEY_OFFSET) | (page_num << AVR32_FLASHC_FCMD_PAGEN_OFFSET) | command;
+	
+	/* 
+		Po spusteni prikazu sa musi vzdy najprv pockat. Akakolvek ina akcia ma za nasledok nespravne vykonanie prikazu.
+	*/
+	flash_wait_until_ready();
+	
+	if (flash_is_error()) {
+		return -1;
+	} else {
+		return 0;	
+	}
+}
+
+/**
+ * @brief Zmaze stranku FLASH.
+ *
+ * @param page_num  Cislo stranky pre zmazanie.
+ *
+ * @return -1  Zle cislo stranky alebo chyba pri vykonavani prikazu.
+ * @return  0  Mazanie prebehlo v poriadku.
+ */
+int
+flash_erase_page(uint16_t page_num) {
+	/* Stranka musi byt v rozsahu stranok */
+	if (page_num >= (AVR32_FLASH_SIZE / AVR32_FLASH_PAGE_SIZE)) {
+		return -1;
+	}
+
+	return flash_execute_command(AVR32_FLASHC_FCMD_CMD_EP, page_num);
+}
+
+/**
+ * @brief Vycisti obsah page bufferu pre data zapisovane do stranky FLASH.
+ *
+ * @return -1  Chyba pri vykonavani prikazu.
+ * @return  0  Page buffer uspesne vycisteny.
+ */
+int
+flash_clear_page_buffer(void) {
+	// Na stranke FLASH nezalezi
+	return flash_execute_command(AVR32_FLASHC_FCMD_CMD_CPB, -1);
+}
+	
+/**
+ * @brief Zapise obsah page bufferu na danu stranku vo FLASH.
+ *
+ * @param page_num  Cislo stranky vo FLASH, kde ma byt zapisany obsah page bufferu.
+ *
+ * @return -1  Neplatne cislo stranky alebo chyba pri zapise page bufferu do FLASH.
+ * @return  0  Obsah page bufferu uspesne zapisany na danu stranku @p page_num.
+ */	
+int
+flash_write_page_buffer(uint16_t page_num)
+{
+	// Stranka musi byt platna
+	if (page_num >= (AVR32_FLASH_SIZE / AVR32_FLASH_PAGE_SIZE)) {
+		return -1;
+	}
+	
+	return flash_execute_command(AVR32_FLASHC_FCMD_CMD_WP, page_num);
+}
+
+/**
+ * @brief Zapis dat do stranky vo FLASH.
+ *
+ * @param page_num  Cislo stranky vo FLASH, kde maju byt data z @p data zapisane.
+ * @param data      Ukazovatel na data pre zapis do FLASH.
+ * @param data_len  Pocet bajtov z @p data pre zapis do FLASH.
+ *
+ * @return -1  Neplatne cislo stranky alebo chyba pri zapise do FLASH.
+ * @return  0  Data uspesne zapisane.
+ */	
+int
+flash_write_page(uint16_t page_num, const uint8_t * data, size_t data_len)
+{	
+	if (flash_erase_page(page_num) || flash_clear_page_buffer()) {
+		return -1;
+	}
+	
+	// Adresa pre pristup do page bufferu. Moze byt adresa stranky 0, pretoze stranku specifikujem explicitne.
+	volatile uint32_t * page_buf_addr = (volatile uint32_t *) AVR32_FLASH_ADDRESS;		
+		
+	while (data_len > 0) {
+		size_t chunk_size = ((data_len >= sizeof(uint32_t)) ? sizeof(uint32_t) : data_len);
+		uint32_t w = 0xFFFFFFFF;
+		uint8_t *dst = (uint8_t *) &w;
+		
+		for (size_t i = 0; i < chunk_size; ++i) {
+			*(dst++) = *(data++);
+		} 
+		
+		*(page_buf_addr++) = w;
+		
+		data_len -= chunk_size;
+	}
+
+	// Zapise sa page buffer do stranky FLASH 
+	if (flash_write_page_buffer(page_num)) {
+		return -1;
+	}
+		
+	return 0;
+}
+
+/**
+ * @brief Precitanie dat zo stranky vo FLASH.
+ *
+ * @param page_num  Cislo stranky vo FLASH, odkial sa budu citat data do @p data.
+ * @param data      Ukazovatel kam bude ulozenych 512 B stranky.
+ *
+ * @return 0  Data precitane.
+ */	
+int
+flash_read_page(uint16_t page_num, uint8_t * const data)
+{
+	uint32_t address = AVR32_FLASH_ADDRESS + (page_num * AVR32_FLASH_PAGE_SIZE);
+	
+	for (size_t i = 0; i < AVR32_FLASH_PAGE_SIZE; ++i, ++address) {
+		data[i] = *((uint8_t *) address);
+	}
+	
+	return 0;
+}
+
+/**
+ * @brief Zapis dat do virtualnej stranky vo FLASH.
+ *
+ * @param page_num  Cislo virtualnej stranky vo FLASH, kde maju byt data z @p data zapisane.
+ * @param data      Ukazovatel na data pre zapis do FLASH.
+ * @param data_len  Pocet bajtov z @p data pre zapis do FLASH.
+ *
+ * @return -1  Neplatne cislo stranky.
+ * @return -1  Ukazovatel @o data je NULL
+ * @return -1  Dlzka dat je 0.
+ * @return -1  Dlzka dat je vacsia ako velkost stranky.
+ * @return  0  Data uspesne zapisane.
+ */
+int
+flash_write_virtual_page(uint16_t page_num, const uint8_t * const data, size_t data_len)
+{
+	if (!FLASH_IS_VALID_VIRT_PAGE_NUMBER(page_num) || (data == NULL) || (data_len == 0) || (data_len > AVR32_FLASH_PAGE_SIZE)) {
+		return -1;
+	}
+	// Prepocita sa cislo virtualnej stranky na cislo realnej stranky a zpisu sa tam data
+	return flash_write_page(FLASH_REAL_PAGE_NUMBER_FROM_VIRT(page_num), data, data_len);
+}
+
+/**
+ * @brief Precitanie dat z virtualnej stranky vo FLASH.
+ *
+ * @param page_num  Cislo virtualnej stranky vo FLASH, odkial sa budu citat data do @p data.
+ * @param data      Ukazovatel kam bude ulozenych 512 B stranky.
+ *
+ * @return -1  Cislo virtualnej stranky je mimo rozsahu.
+ * @return -1  Ukazovatel @p data je NULL
+ * @return  0  Data precitane.
+ */
+int
+flash_read_virtual_page(uint16_t page_num, uint8_t * const data)
+{
+	if (!FLASH_IS_VALID_VIRT_PAGE_NUMBER(page_num) || (data == NULL)) {
+		return -1;
+	}
+	// Prepocita sa cislo virtualnej stranky na cislo realnej stranky a zpisu sa tam data
+	return flash_read_page(FLASH_REAL_PAGE_NUMBER_FROM_VIRT(page_num), data);
+}
+
+/**
+ * @brief Zmaze virtualnu stranku z FLASH.
+ *
+ * @param page_num  Cislo virtualnej stranky pre zmazanie.
+ *
+ * @return -1  Cislo virtualnej stranky mimo rozsahu.
+ * @return -1  Chyba mazania.
+ * @return  0  Mazanie prebehlo v poriadku.
+ */
+int
+flash_erase_virtual_page(uint16_t page_num)
+{
+	if (!FLASH_IS_VALID_VIRT_PAGE_NUMBER(page_num)) {
+		return -1;
+	}
+	return flash_erase_page(FLASH_REAL_PAGE_NUMBER_FROM_VIRT(page_num));
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/flash/flash.h b/impl/DiagnosticModule/DiagnosticModule/flash/flash.h
new file mode 100644
index 0000000..0f91857
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/flash/flash.h
@@ -0,0 +1,19 @@
+/* Author: Jan Sucan */
+
+#ifndef FLASH_H_
+#define FLASH_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <diagnostic_module/parameters.h>
+
+/**
+ * @brief Kontrola, ci je cislo virtualnej stranky FLASH z platneho rozsahu.
+ */
+#define	FLASH_IS_VALID_VIRT_PAGE_NUMBER(n)  (n <= DM_APPLICATION_VIRT_MAX_PAGE)
+
+int  flash_write_virtual_page(uint16_t page_num, const uint8_t * const data, size_t data_len);
+int  flash_read_virtual_page(uint16_t page_num, uint8_t * const data);
+int  flash_erase_virtual_page(uint16_t page_num);
+
+#endif /* FLASH_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/protocol/data.c b/impl/DiagnosticModule/DiagnosticModule/protocol/data.c
new file mode 100644
index 0000000..b11524d
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/protocol/data.c
@@ -0,0 +1,411 @@
+/* Author: Jan Sucan */
+
+#include <protocol/data.h>
+#include <protocol/frame.h>
+#include <clock/wait.h>
+#include <stdbool.h>
+
+/**
+ * Kod verzie protokolu YUP pre pouzitie v hlavicke ramcov.
+ */
+#define DATA_PROTOCOL_VERSION  0x0
+
+/**
+ * Timeout YUP relacie.
+ */
+#define DATA_RELATION_TIMEOUT_MS 60000 // ms
+
+/** 
+ * Timeout pre prijatie ACK na odoslany ramec pri odosielani dat.
+ */
+#define DATA_ACK_TIMEOUT_MS       250 // ms
+
+/** 
+ * Urcuje maximalny pocet stratenych ACK ramcov, a teda aj znovu odoslani datoveho ramca, pri odosielani dat.
+ */
+#define DATA_ACK_RETRY_COUNT       4
+
+
+yup_retcode_t data_is_data_frame     (const frame_t * const frame);
+yup_retcode_t data_is_starting_frame (const frame_t * const frame);
+yup_retcode_t data_is_ack_for_frame  (const frame_t * const ack, const frame_t * const frame);
+void          data_send_ack          (uint16_t relation_id, uint16_t seq_number);
+void          data_send_rej_for_frame(const frame_t * const frame, uint8_t err_code);
+
+
+/**
+ * @brief Kontrola ramca, ci sa jedna o datovy ramec.
+ *
+ * @param fram  Ukazovatel na kontrolovany ramec.
+ *
+ * @return  Popis chyby ramca.
+ */
+yup_retcode_t
+data_is_data_frame(const frame_t * const frame)
+{
+	yup_retcode_t yr = DATA_OK;
+		
+	if (frame_get_data_length(frame) == 0) {
+		yr = DATA_DATA_HAS_NO_PAYLOAD;
+	} else if (frame_is_flag_ack_set(frame)) {
+		yr = DATA_DATA_HAS_REJ_FLAG;
+	} else if (frame_is_flag_rej_set(frame)) {
+		yr = DATA_DATA_HAS_REJ_FLAG;
+	}
+		
+	return yr;
+}
+
+/**
+ * @brief Kontrola ramca, ci sa jedna o startovaci ramec relacie.
+ *
+ * @param frame  Ukazovatel na kontrolovany ramec.
+ *
+ * @return  Popis chyby ramca.
+ */
+yup_retcode_t
+data_is_starting_frame(const frame_t * const frame)
+{
+	yup_retcode_t yr = DATA_OK;
+		
+	if ((yr = data_is_data_frame(frame)) != DATA_OK) {
+		;
+	} else if (frame_get_seq_number(frame) != 0) {
+		yr = DATA_STARTING_HAS_NONZERO_SEQ_NUMBER;
+	}
+		
+	return yr;
+}
+
+/**
+ * @brief Kontrola ramca, ci sa jedna o platny ACK ramec na dany datovy ramec.
+ *
+ * @param ack    Ukazovatel na ACK ramec.
+ * @param framr  Ukazovatel na datovy ramec.
+ *
+ * @return  Popis chyby ACK ramca.
+ */
+yup_retcode_t
+data_is_ack_for_frame(const frame_t * const ack, const frame_t * const frame)
+{
+	yup_retcode_t yr = DATA_OK;
+	
+	if (frame_get_data_length(ack) != 0) {
+		yr = DATA_ACK_HAS_PAYLOAD;
+	} else if (!frame_is_flag_ack_set(ack)) {
+		yr = DATA_ACK_HAS_NOT_ACK_FLAG;
+	} else if (frame_is_flag_lor_set(ack)) {
+		yr = DATA_ACK_HAS_LOR_FLAG;
+	} else if (frame_is_flag_rej_set(ack)) {
+		yr = DATA_ACK_HAS_REJ_FLAG;
+	} else if (frame_get_relation_id(ack) != frame_get_relation_id(frame)) {
+		yr = DATA_ACK_RELATION_ID_MISMATCH;
+	} else if (frame_get_seq_number(ack) != frame_get_seq_number(frame)) {
+		yr = DATA_ACK_SEQ_NUMBER_MISMATCH;
+	}
+	
+	return yr;
+}
+
+/**
+ * @brief Odoslanie ACK ramca.
+ *
+ * @param relation_id  ID relacie.
+ * @param seq_number   Sekvencne cislo ramca.
+ */
+void
+data_send_ack(uint16_t relation_id, uint16_t seq_number)
+{
+	frame_t ack;
+	
+	frame_init(&ack, DATA_PROTOCOL_VERSION);
+	frame_set_flag_ack(&ack);
+	frame_set_relation_id(&ack, relation_id);
+	frame_set_seq_number(&ack, seq_number);
+	
+	frame_send(&ack);
+}
+
+/**
+ * @brief Odoslanie REJ ramca k danemu odoslanemu ramca.
+ *
+ * @param frame     Ukazovatel na nespravny ramec.
+ * @param err_code  Chybovy kod vrateny v odosielanom REJ ramci.
+ */
+void
+data_send_rej_for_frame(const frame_t * const frame, uint8_t err_code)
+{
+	frame_t rej;
+	
+	frame_init(&rej, frame_get_protocol_version(frame));
+	frame_set_flag_rej(&rej);
+	frame_set_relation_id(&rej, frame_get_relation_id(frame));
+	frame_set_seq_number(&rej, frame_get_seq_number(frame));
+	frame_set_data(&rej, &err_code, sizeof(err_code));
+	
+	frame_send(&rej);
+}
+
+
+typedef enum data_receive_automaton_states {
+	DATA_R_WAITING_FOR_STARTING_FRAME,
+	DATA_R_START_RELATION,
+	DATA_R_SAVE,
+	DATA_R_ACK_LAST_FRAME,
+	DATA_R_WAITING_NEXT_DATA_FRAME,
+	DATA_R_END
+} data_receive_automaton_state_t;
+
+typedef enum data_send_automaton_states {
+	DATA_S_START_RELATION,
+	DATA_S_PEPARE_DATA_FRAME,
+	DATA_S_SEND_DATA_FRAME,
+	DATA_S_WAIT_ACK,
+	DATA_S_END
+} data_send_automaton_state_t;
+
+/**
+ * @brief Prijatie dat protokolom YUP.
+ *
+ * @param buf         Ukazovatel na buffer pre ulozenie prijatych dat.
+ * @param bufsize     Velkost bufferu v bajtoch.
+ * @param bytes_read  Pocet skutocne prijatych bajtov.
+ */
+yup_retcode_t
+data_receive(uint8_t * const buf, size_t bufsize, size_t * const bytes_read)
+{
+	uint16_t relation_id = 0;
+	uint16_t sequence_num = 0;
+	size_t data_index = 0;
+	deadline_t timeout = wait_get_deadline(DATA_RELATION_TIMEOUT_MS);
+	data_receive_automaton_state_t state = DATA_R_WAITING_FOR_STARTING_FRAME;
+	
+	// Kontrola argumentov
+	if ((buf == NULL) || (bufsize == 0)) {
+		return DATA_ERROR;
+	}
+	
+	frame_t f;
+			
+	while (state != DATA_R_END) {
+			
+		yup_retcode_t r;
+		uint8_t l;
+				
+		switch (state) {
+		case DATA_R_WAITING_FOR_STARTING_FRAME:
+			// Na prijem startovacieho ramca sa bude cakat donekonecna
+			r = frame_receive(&f, NULL, DATA_PROTOCOL_VERSION);		
+			if (r != FRAME_OK) {
+				// Chyba pri prijme ramca, posle sa naspat REJ so spravou o chybe
+				data_send_rej_for_frame(&f, r);
+			} else if ((r = data_is_starting_frame(&f)) == DATA_OK) {
+				// Mame platny startovaci ramec, zahajime relaciu
+				state = DATA_R_START_RELATION;
+			}
+			break;
+			
+		case DATA_R_START_RELATION:
+			// Od startovacieho ramca zacne plynut cas pre timeout relacie
+			timeout = wait_get_deadline(DATA_RELATION_TIMEOUT_MS);
+			relation_id = frame_get_relation_id(&f);
+			sequence_num = frame_get_seq_number(&f);
+			data_index = 0;
+			state = DATA_R_SAVE;
+			break;
+			
+		case DATA_R_SAVE:
+			if ((bufsize - data_index) < frame_get_data_length(&f)) {
+				// Data sa nezmestia do bufferu
+				// Pocka sa na dalsi paket, co sa bude hodit
+				state = DATA_R_WAITING_NEXT_DATA_FRAME;
+			} else {
+				// Data sa ulozia
+				frame_get_data(&f, buf + data_index, &l);
+				// Ulozia sa informacie o naposledy prijatom ramci
+				sequence_num = frame_get_seq_number(&f);
+				data_index += frame_get_data_length(&f);
+				// Ramec sa potvrdi	
+				state = DATA_R_ACK_LAST_FRAME;
+			}
+			break;
+			
+		case DATA_R_ACK_LAST_FRAME:
+			// Potvrdime naposledy prijaty ramec 
+			data_send_ack(relation_id, sequence_num);
+			// Na posledny ramec relacie sa uz neocakava odpoved
+			// Je bezpecne pristupovat k ramcu f, ak ma nastaveny LOR nemoze byt prepisany dalsim ramcom
+			if (frame_is_flag_lor_set(&f)) {
+				state = DATA_R_END;
+			} else {
+				// Pockame na prijem dalsieho datoveho ramca
+				state = DATA_R_WAITING_NEXT_DATA_FRAME;
+			}
+			break;
+			
+		case DATA_R_WAITING_NEXT_DATA_FRAME:
+			r = frame_receive(&f, &timeout, DATA_PROTOCOL_VERSION);
+			if (r == FRAME_TIMEOUT) {
+				// Pri timeoute prijmu prijmu sa nebude opakovane posielat ACK
+			} else if (r != FRAME_OK) {
+				// Chyba pri prijme datoveho ramca
+				data_send_rej_for_frame(&f, r);
+			} else if ((r = data_is_data_frame(&f)) != DATA_OK) {
+				// Datovy ramec prijaty v poriadku, ale s nespavnym obsahom
+				data_send_rej_for_frame(&f, r);
+			} else if ((frame_get_seq_number(&f) == 0) && (frame_get_relation_id(&f) != relation_id)) {
+				// Jedna sa o zaciatok novej relacie
+				state = DATA_R_START_RELATION;
+			} else if (frame_get_relation_id(&f) != relation_id) {
+				// Obycajny datovy ramec, ale v odlisnej relacii
+				data_send_rej_for_frame(&f, (uint8_t) DATA_DATA_RELATION_ID_MISMATCH);
+			} else if (frame_get_seq_number(&f) != (sequence_num + 1)) {
+				// Obycajny datovy ramec v spravnej relacii, ale nenavazujuci do sekvencie
+				data_send_rej_for_frame(&f, (uint8_t) DATA_DATA_SEQ_NUMBER_MISMATCH);
+			} else {
+				// Spravny navazujuci datovy ramac, ulozia sa jeho data
+				state = DATA_R_SAVE;
+			}
+			break;
+			
+		case DATA_R_END:
+			// Sem sa nedostaneme, len sa umlci prekladac, ze stav nie je osetreny vo switch
+			break;
+		}
+		
+		// Kontrola ci vyprsal timeout
+		if (wait_has_deadline_expired(&timeout)) {
+			// Timeout relacie vyprsal
+			state = DATA_R_END;
+		}
+	}
+	
+	// Pocet skutocne prijatych bajtov
+	*bytes_read = data_index;
+
+	// Pri timeoute relacie sa posle REJ paket
+	if (wait_has_deadline_expired(&timeout)) {
+		frame_init(&f, DATA_PROTOCOL_VERSION);
+		frame_set_relation_id(&f, relation_id);
+		frame_set_seq_number(&f, sequence_num);
+		data_send_rej_for_frame(&f, DATA_RELATION_TIMEOUT);
+		return DATA_ERROR;
+	} else {
+		return DATA_OK;
+	}
+}
+
+/**
+ * @brief Odoslanie dat protokolom YUP.
+ *
+ * @param buf         Ukazovatel na buffer dat pre odoslanie.
+ * @param bufsize     Velkost bufferu v bajtoch.
+ */
+yup_retcode_t
+data_send(const uint8_t * const buf, size_t bufsize)
+{
+	static uint16_t relation_id = 0;
+	
+	uint16_t sequence_num = 0;
+	size_t data_index = 0;
+	deadline_t timeout;
+	deadline_t ack_timeout;
+	size_t ack_retry_count = DATA_ACK_RETRY_COUNT;
+	
+	data_send_automaton_state_t state = DATA_S_START_RELATION;
+	timeout = wait_get_deadline(DATA_RELATION_TIMEOUT_MS);
+	
+	// Kontrola argumentov
+	if ((buf == NULL) || (bufsize == 0)) {
+		return DATA_ERROR;
+	}
+	
+	frame_t f, a;
+	
+	while (state != DATA_S_END) {
+	
+		yup_retcode_t r;
+		
+		switch (state) {
+		case DATA_S_START_RELATION:
+			timeout = wait_get_deadline(DATA_RELATION_TIMEOUT_MS);
+			++relation_id;
+			sequence_num = 0;
+			data_index = 0;
+			ack_retry_count = DATA_ACK_RETRY_COUNT;
+			state = DATA_S_PEPARE_DATA_FRAME;
+			break;
+			
+		case DATA_S_PEPARE_DATA_FRAME:
+			// Pripravi sa ramec s datami
+			frame_init(&f, DATA_PROTOCOL_VERSION);
+			frame_set_relation_id(&f, relation_id);
+			frame_set_seq_number(&f, sequence_num);
+			size_t len = ((bufsize - data_index) >= FRAME_MAX_DATA_BYTES) ? FRAME_MAX_DATA_BYTES : (bufsize - data_index);
+			frame_set_data(&f, buf + data_index, len);
+			data_index += len;
+			// Je to posledny ramec?
+			if (data_index >= bufsize) {
+				frame_set_flag_lor(&f);
+			}
+			state = DATA_S_SEND_DATA_FRAME;
+			break;
+			
+		case DATA_S_SEND_DATA_FRAME:
+			// Odosle sa pripraveny ramec
+			frame_send(&f);
+			// Pocka sa na potvrdenie v danom timetoute
+			ack_timeout = wait_get_deadline(DATA_ACK_TIMEOUT_MS);
+			state = DATA_S_WAIT_ACK;
+			break;
+		
+		case DATA_S_WAIT_ACK:
+			r = frame_receive(&a, &ack_timeout, DATA_PROTOCOL_VERSION);
+			if (r == FRAME_TIMEOUT) {
+				// Neprijal sa ACK, znovu sa odoslu pipravene data
+				if (ack_retry_count-- > 0) {
+					state = DATA_S_SEND_DATA_FRAME;
+				} else {
+					// Bol dosiahnuty maximalny pocet pokusov o znozvu odoslanie dat
+					return DATA_ERROR;
+				}
+			} else if (r != FRAME_OK) {
+				// Chyba pri prijme ACK ramca
+				data_send_rej_for_frame(&a, r);	
+			} else if ((r = data_is_ack_for_frame(&a, &f)) != DATA_OK) {
+				// ACK ramec prijaty v poriadku, ale s nespravnym obshom
+				data_send_rej_for_frame(&a, r);
+			} else if (frame_is_flag_lor_set(&f)) {
+				// Datovy ramec, ktory som odoslal, bol posledny
+				state = DATA_S_END;	
+			} else {
+				// Naposledy vyslany datovy ramec bol potvrdeny protistranou, odosle sa dalsi
+				sequence_num++;
+				state = DATA_S_PEPARE_DATA_FRAME;						
+			}
+			break;
+			
+		case DATA_S_END:
+			// Sem sa nedostaneme, len sa umlci prekladac, ze stav nie je osetreny vo switch
+			break;
+		}
+		
+		// Kontrola ci vyprsal timeout
+		if (wait_has_deadline_expired(&timeout)) {
+			// Timeout relacie vyprsal
+			state = DATA_S_END;
+		}
+	}
+	
+	// Pri timeoute relacie sa posle REJ paket
+	if (wait_has_deadline_expired(&timeout)) {
+		frame_t f;
+		frame_init(&f, DATA_PROTOCOL_VERSION);
+		frame_set_relation_id(&f, relation_id);
+		frame_set_seq_number(&f, sequence_num);
+		data_send_rej_for_frame(&f, DATA_RELATION_TIMEOUT);
+		return DATA_ERROR;
+	} else {
+		return DATA_OK;
+	}
+}
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/protocol/data.h b/impl/DiagnosticModule/DiagnosticModule/protocol/data.h
new file mode 100644
index 0000000..6e4f032
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/protocol/data.h
@@ -0,0 +1,16 @@
+/* Author: Jan Sucan */
+
+#ifndef DATA_H_
+#define DATA_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <limits.h>
+
+#include <protocol/return_codes.h>
+
+yup_retcode_t data_receive(uint8_t * const buf, size_t bufsize, size_t * const bytes_read);
+yup_retcode_t data_send   (const uint8_t * const buf, size_t bufsize);
+
+#endif /* DATA_H_ */
+
diff --git a/impl/DiagnosticModule/DiagnosticModule/protocol/frame.c b/impl/DiagnosticModule/DiagnosticModule/protocol/frame.c
new file mode 100644
index 0000000..04739bc
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/protocol/frame.c
@@ -0,0 +1,429 @@
+/* Author: Jan Sucan */
+
+#include <protocol/frame.h>
+#include <comm/bt.h>
+#include <utils/byte_buffer.h>
+#include <utils/crc32q.h>
+#include <clock/wait.h>
+
+#include <string.h>
+
+// Bitove masky priznakov ramca
+#define FRAME_FLAG_ACK  0x1  // Prijatie paketu
+#define FRAME_FLAG_REJ  0x2  // Odmietnutie paketou
+#define FRAME_FLAG_LOR  0x8  // Oznacenie posledneho paketu relacie
+
+// Offsety hodnot v surovych datach ramca
+#define FRAME_OFFSET_PROTOCOL_VERSION  0x00
+#define FRAME_OFFSET_FLAGS             0x00
+#define FRAME_OFFSET_DATA_LENGTH       0x00
+#define FRAME_OFFSET_RELATION_ID       0x02
+#define FRAME_OFFSET_SEQ_NUMBER        0x04
+#define FRAME_OFFSET_HEADER_CHECKSUM   0x06
+#define FRAME_OFFSET_DATA              0x0A
+
+// Bitove masky pre 16-bitovu hodnotu obsahujucu verziu protokolu YUP, priznaky ramca a dlzku pripojenych dat
+#define FRAME_MASK_PROTOCOL_VERSION  0xF000
+#define FRAME_MASK_FLAGS             0x0F00
+#define FRAME_MASK_DATA_LENGTH       0x00FF
+
+// Bitove offsety v 16-bitovej hodnote, v ktorej je obsiahnuta verzia YUP a priznaky
+#define FRAME_BITOFF_PROTOCOL_VERSION  12
+#define FRAME_BITOFF_FLAGS              8
+
+// Velkost hlavicky v bajtoch
+#define FRAME_HEADER_BYTES    FRAME_OFFSET_DATA
+
+// Velkost pola kontrolneho suctu
+#define FRAME_CHECKSUM_BYTES  4
+
+// Interne pouzivany navratovy kod
+#define	FRAME_PVT_ERR_DATA_INTERRUPTED  1
+
+
+static yup_retcode_t frame_receive_header         (frame_t * const frame, const deadline_t * const time_deadline);
+static yup_retcode_t frame_verify_header_checksum (const frame_t * const frame);
+static yup_retcode_t frame_receive_data           (frame_t * const frame, const deadline_t * const time_deadline);
+static yup_retcode_t frame_verify_data_checksum   (const frame_t * const frame);
+static uint8_t       frame_get_flags              (const frame_t * const frame);
+static void          frame_set_flags              (frame_t * const frame, uint8_t f);
+static yup_retcode_t yup_retcode_from_bt_retcode  (bt_retcode_t btr);
+
+/**
+ * @brief Nastavenie predvolenych parametrov ramca.
+ *
+ * @param frame             Ukazovatel na ramec.
+ * @param protocol_version  Verzia protokolu, do ktoreho ramec spada.
+ */
+void
+frame_init(frame_t * const frame, uint8_t protocol_version)
+{	
+	frame_set_protocol_version(frame, protocol_version);
+	frame_set_flags(frame, 0);
+	frame_set_data(frame, NULL, 0);
+	frame_set_relation_id(frame, 0);
+	frame_set_seq_number(frame, 0);	
+}
+
+/**
+ * @brief Odoslanie ramca.
+ *
+ * @param frame  Ukazovatel na ramec pre odoslanie.
+ */
+void
+frame_send(frame_t * const frame)
+{
+	// Vyplni sa kontrolny sucet hlavicky
+	frame_set_header_checksum(frame);
+	
+	// Vyplni sa kontrolny sucet dat, ak nejake su
+	frame_set_data_checksum(frame);
+	
+	// Odosle sa ramec
+	bt_send_cobs_data_block(frame->raw, frame_get_length(frame));
+}
+
+/**
+ * @brief Prijatie ramca.
+ *
+ * @param frame             Ukazovatel kde sa ulozi prijaty ramec.
+ * @param time_deadline     Deadline, dokedy najneskor sa bude cakat na prijatie ramca.
+ * @param protocol_version  Verzia protokolu, ktorej ramce sa budu spracovavat.
+ */
+yup_retcode_t
+frame_receive(frame_t * const frame, const deadline_t * const time_deadline, uint8_t protocol_version)
+{	
+	yup_retcode_t retcode = FRAME_OK;
+	
+	while (1) {
+		// Hlavicka sa prijme a skontroluje
+		if ((retcode = frame_receive_header(frame, time_deadline))) {
+			break;
+		}
+		if ((retcode = frame_verify_header_checksum(frame))) {
+			break;
+		}
+		// Hlavicka ramca je OK, odteraz sa moze getterom pristupovat k informacii o verzii protokolu a dlzke dat
+		// Skontroluje sa pozadovana verzia protokolu
+		if (frame_get_protocol_version(frame) != protocol_version) {
+			retcode = FRAME_PROTOCOL_VERSION_MISMATCH;
+			break;
+		}	
+		// Prijmu sa data
+		yup_retcode_t r = frame_receive_data(frame, time_deadline);
+
+		if (r == FRAME_OK) {
+			break;
+		} else if (r == FRAME_PVT_ERR_DATA_INTERRUPTED) {
+			// Prijal sa delimiter, novy ramec zacal skor nez skoncil aktualny, zacne sa s prijmom noveho ramca
+			continue;
+		} else {
+			retcode = r;
+			break;
+		} 
+	}
+	
+	if (retcode == FRAME_OK) {
+		retcode = frame_verify_data_checksum(frame);
+	}
+	
+	return retcode;
+}
+
+/**
+ * @brief Prijatie hlavicky ramca.
+ *
+ * Hlavicka sa musi prijat zvlast, aby sa z nej ziskala dlzka datoveho nakladu.
+ *
+ * @param frame          Ukazovatel na ramec kde sa ulozi prijata hlavicka.
+ * @param time_deadline  Deadline, dokedy najneskor sa bude cakat na prijatie hlavicky.
+ */
+yup_retcode_t
+frame_receive_header(frame_t * const frame, const deadline_t * const time_deadline)
+{
+	return yup_retcode_from_bt_retcode(bt_receive_cobs_data(frame->raw, FRAME_HEADER_BYTES, time_deadline, BT_RECEIVE_COBS_START));
+}
+
+/**
+ * @brief Kontrola hlavicky ramca.
+ *
+ * @param frame  Ukazovatel na ramec obsahujuci kontrolovanu hlavicku.
+ */
+yup_retcode_t
+frame_verify_header_checksum (const frame_t * const frame)
+{
+	yup_retcode_t retcode = FRAME_OK;
+	
+	// Vypocet kontrolneho suctu prijatych dat
+	// Preskoci sa COBS delimiter (1B) a COBS header (1B)
+	// Do vypoctu nebudu zahrnute 4 bajty kontrolneho suctu
+	const uint32_t sum = crc32q(frame->raw, FRAME_HEADER_BYTES - FRAME_CHECKSUM_BYTES);
+	
+	// Vypocitany kontrolny sucet sa porovna so suctom v ramci
+	if (sum != frame_get_header_checksum(frame)) {
+		retcode = FRAME_BAD_HEADER_CHECKSUM;
+	}
+	
+	return retcode;
+}
+
+/**
+ * @brief Prijatie datovej casti ramca.
+ *
+ * @param frame          Ukazovatel na ramec kde sa ulozi prijata datova cast. 
+ * @param time_deadline  Deadline, dokedy najneskor sa bude cakat na prijatie dat.
+ */
+yup_retcode_t
+frame_receive_data (frame_t * const frame, const deadline_t * const time_deadline)
+{
+	const uint8_t data_length = frame_get_data_length(frame);
+	yup_retcode_t retcode = FRAME_OK;
+	
+	// Pri nulovych data sa nic neprijme
+	if (data_length > 0) {
+		// Treba prijat naviac 4B kontrolneho suctu
+		retcode = yup_retcode_from_bt_retcode(bt_receive_cobs_data(frame->raw + FRAME_HEADER_BYTES, data_length + FRAME_CHECKSUM_BYTES,
+	                                                                 time_deadline, BT_RECEIVE_COBS_CONTINUE));
+	}
+	
+	return retcode;
+}
+
+/**
+ * @brief Kontrola datovej casti ramca.
+ *
+ * @param frame  Ukazovatel na ramec.
+ */
+yup_retcode_t
+frame_verify_data_checksum (const frame_t * const frame)
+{
+	yup_retcode_t retcode = FRAME_OK;
+	const uint8_t data_length = frame_get_data_length(frame);
+	
+	// Ked sme sa dostali az sem, kontrolny sucet hlavicky je OK
+	// Ak nemame ziadne data, nic sa uz nemusi kontrolovat
+	if (data_length > 0) {		
+		// Kontrola suctu dat
+		// Do vypoctu nebudu zahrnute 4 bajty kontrolneho suctu
+		const uint32_t sum = crc32q(frame->raw + FRAME_HEADER_BYTES, data_length);
+		
+		// Vypocitany kontrolny sucet sa porovna so suctom v ramci
+		if (sum != frame_get_data_checksum(frame)) {
+			retcode = FRAME_BAD_DATA_CHECKSUM;
+		}
+	}
+	
+	return retcode;
+}
+
+/**
+ * @brief Konverzia navratovej hodnoty z bluetooth vrsty do kodov pre aktualnu vrstvu.
+ *
+ * Skonvertovany kod je vrateny vyssim vrstvam.
+ *
+ * @param btr  Navratovy kod z mnoziny bt_retcode_t.
+ *
+ * @return Navratovy kod z mnoziny yup_retcode_t.
+ */
+yup_retcode_t
+yup_retcode_from_bt_retcode(bt_retcode_t btr)
+{
+	yup_retcode_t r = FRAME_RECEIVE_ERROR;
+	
+	switch (btr) {
+	case BT_RECEIVE_TIMEOUT:
+		r = FRAME_TIMEOUT;
+		break;
+		
+	case BT_RECEIVE_COBS_INTERRUPTED:
+		r = FRAME_PVT_ERR_DATA_INTERRUPTED;
+		break;
+		
+	case BT_RECEIVE_BAD_ARGUMENT:
+	case BT_RECEIVE_ERROR:
+	case BT_RECEIVE_COBS_UNKNOWN_STATE:
+		r = FRAME_RECEIVE_ERROR;
+		break;
+			
+	case BT_OK:
+		r = FRAME_OK;
+		break;
+	}
+	
+	return r;
+}
+
+
+// Getter/Setter funkcie
+uint8_t
+frame_get_protocol_version(const frame_t * const frame)
+{
+	const uint16_t x = GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_PROTOCOL_VERSION);
+	return (x & FRAME_MASK_PROTOCOL_VERSION) >> FRAME_BITOFF_PROTOCOL_VERSION;
+}
+
+void
+frame_set_protocol_version(frame_t * const frame, uint8_t v)
+{
+	uint16_t x = GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_PROTOCOL_VERSION);
+	x &= ~FRAME_MASK_PROTOCOL_VERSION;
+	x |= (v & 0x0F) << FRAME_BITOFF_PROTOCOL_VERSION;
+	SET_BYTES_FROM_UINT16_T(frame->raw, FRAME_OFFSET_PROTOCOL_VERSION, x);
+}
+
+void
+frame_set_flag_ack(frame_t * const frame)
+{
+	frame_set_flags(frame, frame_get_flags(frame) | FRAME_FLAG_ACK);
+}
+
+bool
+frame_is_flag_ack_set(const frame_t * const frame)
+{
+	return frame_get_flags(frame) & FRAME_FLAG_ACK;
+}
+
+void
+frame_set_flag_rej(frame_t * const frame)
+{
+	frame_set_flags(frame, frame_get_flags(frame) | FRAME_FLAG_REJ);
+}
+
+bool
+frame_is_flag_rej_set(const frame_t * const frame)
+{
+	return frame_get_flags(frame) & FRAME_FLAG_REJ;
+}
+
+void
+frame_set_flag_lor(frame_t * const frame)
+{
+	frame_set_flags(frame, frame_get_flags(frame) | FRAME_FLAG_LOR);
+}
+
+bool
+frame_is_flag_lor_set(const frame_t * const frame)
+{
+	return frame_get_flags(frame) & FRAME_FLAG_LOR;
+}
+
+uint8_t
+frame_get_flags(const frame_t * const frame)
+{
+	const uint16_t x = GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_FLAGS);
+	return (x & FRAME_MASK_FLAGS) >> FRAME_BITOFF_FLAGS;
+}
+
+void
+frame_set_flags(frame_t * const frame, uint8_t f)
+{
+	uint16_t x = GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_FLAGS);
+	x &= ~FRAME_MASK_FLAGS;
+	x |= (f & 0x0F) << FRAME_BITOFF_FLAGS;
+	SET_BYTES_FROM_UINT16_T(frame->raw, FRAME_OFFSET_FLAGS, x);
+}
+
+uint8_t
+frame_get_data_length(const frame_t * const frame)
+{
+	const uint16_t x = GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_DATA_LENGTH);
+	return (x & FRAME_MASK_DATA_LENGTH);
+}
+
+void
+frame_set_data_length(frame_t * const frame, uint8_t v)
+{
+	uint16_t x = GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_DATA_LENGTH);
+	x &= ~FRAME_MASK_DATA_LENGTH;
+	x |= v;
+	SET_BYTES_FROM_UINT16_T(frame->raw, FRAME_OFFSET_DATA_LENGTH, x);
+}
+
+uint16_t
+frame_get_relation_id(const frame_t * const frame)
+{
+	return GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_RELATION_ID);
+}
+
+void
+frame_set_relation_id(frame_t * const frame, uint16_t v)
+{
+	SET_BYTES_FROM_UINT16_T(frame->raw, FRAME_OFFSET_RELATION_ID, v);	
+}
+
+uint16_t
+frame_get_seq_number(const frame_t * const frame)
+{
+	return GET_UINT16_T_FROM_BYTES(frame->raw, FRAME_OFFSET_SEQ_NUMBER);
+}
+
+void
+frame_set_seq_number(frame_t * const frame, uint16_t v)
+{
+	SET_BYTES_FROM_UINT16_T(frame->raw, FRAME_OFFSET_SEQ_NUMBER, v);		
+}
+
+uint32_t
+frame_get_header_checksum(const frame_t * const frame)
+{
+	return GET_UINT32_T_FROM_BYTES(frame->raw, FRAME_OFFSET_HEADER_CHECKSUM);
+}
+
+void
+frame_set_header_checksum(frame_t * const frame)
+{
+	// Nezapocitavaju sa prve dva bajty COBSu a posledne 4 bajty pre ulozenie kontrolneho suctu
+	const uint32_t cs = crc32q(frame->raw, FRAME_HEADER_BYTES - FRAME_CHECKSUM_BYTES);
+	SET_BYTES_FROM_UINT32_T(frame->raw, FRAME_OFFSET_HEADER_CHECKSUM, cs);	
+}
+
+void
+frame_get_data(const frame_t * const frame, uint8_t * const data, uint8_t * const data_length)
+{
+	*data_length = frame_get_data_length(frame);
+	memcpy(data, frame->raw + FRAME_OFFSET_DATA, *data_length);
+}
+
+void
+frame_set_data(frame_t * const frame, const uint8_t * const data, uint8_t num)
+{
+	uint8_t n = (num > FRAME_MAX_DATA_BYTES) ? FRAME_MAX_DATA_BYTES : num;
+	
+	frame_set_data_length(frame, n);
+	
+	if ((data != NULL) && (n > 0)) {
+		memcpy(frame->raw + FRAME_OFFSET_DATA, data, n);	
+	}
+}
+
+uint32_t
+frame_get_data_checksum(const frame_t * const frame)
+{
+	uint8_t data_length = frame_get_data_length(frame);
+	uint32_t x = 0;
+	// Pole checksum je pripojene len vtedy, ked su pripojene nejake data
+	if (data_length > 0) {
+		x = GET_UINT32_T_FROM_BYTES(frame->raw, FRAME_OFFSET_DATA + data_length);
+	}
+	return x;
+}
+
+void
+frame_set_data_checksum(frame_t * const frame)
+{
+	const uint8_t dl = frame_get_data_length(frame);
+	if (dl == 0) {
+		// Zadne data, nie je k comu pocitat kontrolny sucet
+		return;
+	}
+	
+	const uint32_t cs = crc32q(frame->raw + FRAME_OFFSET_DATA, dl);
+	SET_BYTES_FROM_UINT32_T(frame->raw, FRAME_OFFSET_DATA + dl, cs);	
+}
+
+uint8_t
+frame_get_length(const frame_t * const frame)
+{
+	const uint8_t dl = frame_get_data_length(frame);
+	return FRAME_OFFSET_DATA + dl + ((dl > 0) ? FRAME_CHECKSUM_BYTES : 0);
+}
diff --git a/impl/DiagnosticModule/DiagnosticModule/protocol/frame.h b/impl/DiagnosticModule/DiagnosticModule/protocol/frame.h
new file mode 100644
index 0000000..4c9e9f3
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/protocol/frame.h
@@ -0,0 +1,65 @@
+/* Author: Jan Sucan */
+
+#ifndef FRAME_H_
+#define FRAME_H_
+
+#include <clock/wait.h>
+#include <protocol/return_codes.h>
+
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+/**
+ * @brief Celkova maximalna velkost ramca v bajtoch.
+ *
+ * Dva do poctu 256 su pre COBS.
+ */
+#define FRAME_MAX_BYTES       254
+
+/**
+ * @brief Maximalna velkost datoveho nakladu ramca v bajtoch.
+ */
+#define FRAME_MAX_DATA_BYTES  240
+
+typedef struct {
+	uint8_t raw[FRAME_MAX_BYTES];
+} frame_t;
+
+void            frame_init   (frame_t * const frame, uint8_t protocol_version);
+yup_retcode_t   frame_receive(frame_t * const frame, const deadline_t * const time_deadline, uint8_t protocol_version);
+void            frame_send   (frame_t * const frame);
+
+// Protocol version
+uint8_t  frame_get_protocol_version(const frame_t * const frame);
+void     frame_set_protocol_version(frame_t * const frame, uint8_t v);
+// Flags
+void     frame_set_flag_ack(frame_t * const frame);
+bool     frame_is_flag_ack_set(const frame_t * const frame);
+void     frame_set_flag_rej(frame_t * const frame);
+bool     frame_is_flag_rej_set(const frame_t * const frame);
+void     frame_set_flag_lor(frame_t * const frame);
+bool     frame_is_flag_lor_set(const frame_t * const frame);
+// Data length
+uint8_t  frame_get_data_length(const frame_t * const frame);
+void     frame_set_data_length(frame_t * const frame, uint8_t v);
+// Relation ID
+uint16_t frame_get_relation_id(const frame_t * const frame);
+void     frame_set_relation_id(frame_t * const frame, uint16_t v);
+// Seq number
+uint16_t frame_get_seq_number(const frame_t * const frame);
+void     frame_set_seq_number(frame_t * const frame, uint16_t v);
+// Header checksum
+uint32_t frame_get_header_checksum(const frame_t * const frame);
+void     frame_set_header_checksum(frame_t * const frame);
+// Data
+void     frame_get_data(const frame_t * const frame, uint8_t * const data, uint8_t * const data_length);
+void     frame_set_data(frame_t * const frame, const uint8_t * const data, uint8_t num);
+// Data checksum
+uint32_t frame_get_data_checksum(const frame_t * const frame);
+void     frame_set_data_checksum(frame_t * const frame);
+
+// Ostatne
+uint8_t  frame_get_length(const frame_t * const frame);
+
+#endif /* FRAME_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/protocol/return_codes.h b/impl/DiagnosticModule/DiagnosticModule/protocol/return_codes.h
new file mode 100644
index 0000000..b7858a5
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/protocol/return_codes.h
@@ -0,0 +1,38 @@
+/* Author: Jan Sucan */
+
+#ifndef RETURN_CODES_H_
+#define RETURN_CODES_H_
+
+#include <limits.h>
+
+typedef enum {
+	// Privatne kody nesirene v REJ ramcoch
+	FRAME_OK = 0x00,
+	DATA_OK = 0x00,
+	DATA_ERROR,
+	FRAME_TIMEOUT,
+	
+	// Kody sirene v REJ ramcoch
+	DATA_STARTING_HAS_NONZERO_SEQ_NUMBER = 0x10,
+	DATA_RELATION_TIMEOUT,
+	
+	DATA_DATA_HAS_NO_PAYLOAD = 0x30,
+	DATA_DATA_HAS_ACK_FLAG,
+	DATA_DATA_HAS_REJ_FLAG,
+	DATA_DATA_RELATION_ID_MISMATCH,
+	DATA_DATA_SEQ_NUMBER_MISMATCH,
+	
+	DATA_ACK_HAS_PAYLOAD = 0x50,
+	DATA_ACK_HAS_NOT_ACK_FLAG,
+	DATA_ACK_HAS_LOR_FLAG,
+	DATA_ACK_HAS_REJ_FLAG,
+	DATA_ACK_RELATION_ID_MISMATCH,
+	DATA_ACK_SEQ_NUMBER_MISMATCH,
+	
+	FRAME_RECEIVE_ERROR = 0x70,
+	FRAME_BAD_HEADER_CHECKSUM,
+	FRAME_BAD_DATA_CHECKSUM,
+	FRAME_PROTOCOL_VERSION_MISMATCH
+} yup_retcode_t;
+
+#endif /* RETURN_CODES_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/utils/byte_buffer.h b/impl/DiagnosticModule/DiagnosticModule/utils/byte_buffer.h
new file mode 100644
index 0000000..a47fb4e
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/utils/byte_buffer.h
@@ -0,0 +1,70 @@
+/* Author: Jan Sucan */
+
+#ifndef BYTE_BUFFER_H_
+#define BYTE_BUFFER_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+
+/**
+ * @brief Ziskanie uint8_t hodnoty z pola bajtov.
+ *
+ * @param b  Pole bajtov.
+ * @param i  Offset hodnoty v poli @p b.
+ */
+#define GET_UINT8_T_FROM_BYTES(b, i)   ((uint8_t) b[i])
+
+/**
+ * @brief Ziskanie uint16_t hodnoty z pola bajtov vo formate little-endian.
+ *
+ * @param b  Pole bajtov.
+ * @param i  Offset hodnoty v poli @p b.
+ */
+#define GET_UINT16_T_FROM_BYTES(b, i)  ((uint16_t) ((b[i + 1] << 8) | b[i]))
+
+/**
+ * @brief Ziskanie uint32_t hodnoty z pola bajtov vo formate little-endian.
+ *
+ * @param b  Pole bajtov.
+ * @param i  Offset hodnoty v poli @p b.
+ */
+#define GET_UINT32_T_FROM_BYTES(b, i)  ((uint32_t) ((b[i + 3] << 24) | (b[i + 2] << 16) | (b[i + 1] << 8) | b[i]))
+
+/**
+ * @brief Ulozenie uint8_t hodnoty do pola bajtov.
+ *
+ * @param b  Pole bajtov.
+ * @param i  Offset hodnoty v poli @p b.
+ * @param v  Hodnota pre ulozenie.
+ */
+#define SET_BYTES_FROM_UINT8_T(b, i, v)  {\
+	b[i + 0] = v;\
+}
+
+/**
+ * @brief Ulozenie uint16_t hodnoty do pola bajtov vo formate little-endian.
+ *
+ * @param b  Pole bajtov.
+ * @param i  Offset hodnoty v poli @p b.
+ * @param v  Hodnota pre ulozenie.
+ */
+#define SET_BYTES_FROM_UINT16_T(b, i, v)  {\
+	 b[i + 1] = (v & 0xFF00) >> 8;\
+	 b[i + 0] = (v & 0x00FF);\
+}
+
+/**
+ * @brief Ulozenie uint32_t hodnoty do pola bajtov vo formate little-endian.
+ *
+ * @param b  Pole bajtov.
+ * @param i  Offset hodnoty v poli @p b.
+ * @param v  Hodnota pre ulozenie.
+ */
+#define SET_BYTES_FROM_UINT32_T(b, i, v)  {\
+	 b[i + 3] = (v & 0xFF000000) >> 24;\
+	 b[i + 2] = (v & 0x00FF0000) >> 16;\
+	 b[i + 1] = (v & 0x0000FF00) >> 8;\
+	 b[i + 0] = (v & 0x000000FF);\
+}
+
+#endif /* BYTE_BUFFER_H_ */
\ No newline at end of file
diff --git a/impl/DiagnosticModule/DiagnosticModule/utils/crc32q.c b/impl/DiagnosticModule/DiagnosticModule/utils/crc32q.c
new file mode 100644
index 0000000..8cf3580
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/utils/crc32q.c
@@ -0,0 +1,101 @@
+/* Author: Jan Sucan */
+
+#include <utils/crc32q.h>
+
+#include <stdbool.h>
+
+/**
+ * @brief Velkost tabulky hodnot pre vypocet CRC32-Q v bajtoch.
+ */
+#define CRC32_TABLE_SIZE  256
+
+/**
+ * @brief Velkost tabulky hodnot pre vypocet CRC32-Q v bajtoch.
+ */
+#define CRC32_GENERATING_POLYNOME  0x814141ab
+
+static void crc32q_create_table(uint32_t * const tab, uint32_t gen_polynome);
+
+/**
+ * @brief Vypocet hodnot tabulky pre CRC32-Q.
+ *
+ * @param tab           Tabulka 32-bitovych hodnot.
+ * @param gen_polynome  Generujuci polynom z ktoreho sa budu hodnoty pocitat.
+ */
+void
+crc32q_create_table(uint32_t * const tab, uint32_t gen_polynome)
+{	
+	int i, j;
+	uint32_t c;
+	uint32_t msbMask = 1U << ((sizeof(c) * 8) - 1);
+	
+	for (i = 0; i < CRC32_TABLE_SIZE; i++) {
+		c = i << 24;
+		for (j = 0; j < 8; j++) {
+			if (c & msbMask) {
+				c = (c << 1) ^ gen_polynome;
+			} else {
+				c = c << 1;
+			}
+		}
+		tab[i] = c;
+	}
+}
+
+/**
+ * @brief Tabulka hodnot pre vypocet CRC32-Q.
+ */
+static uint32_t crc32_tab[CRC32_TABLE_SIZE];
+
+/**
+ * @brief Priznak, ci je tabulka hodnot pre vypocet CRC32-Q inicializovana.
+ */
+static bool crc32q_table_created = false;
+
+/**
+ * @brief Inicializacia datovych struktur pre vypocet CRC32-Q.
+ *
+ * Vypocita tabulku hodnot pre CRC.
+ */
+void
+crc32q_init(void)
+{
+	crc32q_create_table(crc32_tab, CRC32_GENERATING_POLYNOME);
+	crc32q_table_created = true;
+}
+
+/**
+ * @brief Vypocet kontrolneho suctu CRC32-Q.
+ *
+ * Tento kod je prevzaty z
+ *
+ *   https://svnweb.freebsd.org/base/stable/9/sys/libkern/crc32.c?revision=225736&view=co
+ *
+ * a upraveny pre CRC32-Q.
+ *
+ * @param buf   Ukazovatel na data, z ktorych sa bude sucet pocitat.
+ * @param size  Pocet bajtov dat pre vypocet CRC.
+ *
+ * @return CRC32-Q kontrolny sucet.
+ */
+uint32_t
+crc32q(const void * const buf, size_t size)
+{	
+	// Vytvorenie tabulky pred prvym pouzitim
+	if (!crc32q_table_created) {
+		crc32q_create_table(crc32_tab, CRC32_GENERATING_POLYNOME);
+		crc32q_table_created = true;
+	}
+
+	// Vypocet CRC-32
+	const uint8_t *p;
+
+	p = buf;
+	uint32_t crc = 0U; // Inicializacna CRC hodnota
+
+	while (size--) {
+		crc = (crc << 8) ^ crc32_tab[(crc >> 24) ^ *p++];
+	}
+
+	return crc ^ 0U;
+}
diff --git a/impl/DiagnosticModule/DiagnosticModule/utils/crc32q.h b/impl/DiagnosticModule/DiagnosticModule/utils/crc32q.h
new file mode 100644
index 0000000..f2630eb
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/utils/crc32q.h
@@ -0,0 +1,12 @@
+/* Author: Jan Sucan */
+
+#ifndef CRC32Q_H_
+#define CRC32Q_H_
+
+#include <stdint.h>
+#include <stdlib.h>
+
+void     crc32q_init(void);
+uint32_t crc32q(const void * const buf, size_t size);
+
+#endif /* CRC32Q_H_ */
diff --git a/impl/DiagnosticModule/DiagnosticModule/utils/system_registers.h b/impl/DiagnosticModule/DiagnosticModule/utils/system_registers.h
new file mode 100644
index 0000000..cf3bea8
--- /dev/null
+++ b/impl/DiagnosticModule/DiagnosticModule/utils/system_registers.h
@@ -0,0 +1,46 @@
+/* Author: Jan Sucan */
+
+#ifndef SYSTEM_REGISTERS_H_
+#define SYSTEM_REGISTERS_H_
+
+/**
+ * @brief Makro pre nastavenie hodnoty systemoveho registra.
+ */
+#define SYSREG_SET(r, v) __builtin_mtsr(r, v)
+
+/**
+ * @brief Makro pre ziskanie hodnoty systemoveho registra.
+ */
+#define SYSREG_GET(r) __builtin_mfsr(r)
+
+/**
+ * @brief Adresa systemoveho registra COUNT, ktory pocita hodinove cykly CPU.
+ */
+#define SYSREG_COUNT_ADDRESS 264U
+
+/**
+ * @brief Makro pre nastavenie hodnoty systemoveho registra COUNT.
+ */
+#define SYSREG_COUNT_SET(v) SYSREG_SET(SYSREG_COUNT_ADDRESS, v)
+
+/**
+ * @brief Makro pre ziskanie hodnoty systemoveho registra COUNT.
+ */
+#define SYSREG_COUNT_GET SYSREG_GET(SYSREG_COUNT_ADDRESS)
+
+/**
+ * @brief Adresa systemoveho registra CPUCR, ktory nastavuje vlastnosti CPU.
+ */
+#define SYSREG_CPUCR_ADDRESS 12U
+
+/**
+ * @brief Makro pre nastavenie hodnoty systemoveho registra CPUSR.
+ */
+#define SYSREG_CPUCR_SET(v) SYSREG_SET(SYSREG_CPUCR_ADDRESS, v)
+
+/**
+ * @brief Makro pre ziskanie hodnoty systemoveho registra CPUSR.
+ */
+#define SYSREG_CPUCR_GET SYSREG_GET(SYSREG_CPUCR_ADDRESS)
+
+#endif /* SYSTEM_REGISTERS_H_ */
\ No newline at end of file