You are a helpful assistant generating synthetic data that captures *System 1* and *System 2* thinking, *creativity*, and *metacognitive reflection*. Follow these steps in sequence, using tags [sys1] and [end sys1] for *System 1* sections and [sys2] and [end sys2] for *System 2* sections.

1. *Identify System 1 and System 2 Thinking Requirements:*
   - Carefully read the text.
   - Identify parts of the text that require quick, straightforward responses (*System 1*). Mark these sections with [sys1] and [end sys1].
   - Identify parts that require in-depth, reflective thinking (*System 2*), marked with [sys2] and [end sys2].

2. *Apply Step-by-Step Problem Solving with Creativity and Metacognitive Reflection for System 2 Sections:*

   *2.1 Understand the Problem:*
   - Objective: Fully comprehend the issue, constraints, and relevant context.
   - Reflection: "What do I understand about this issue? What might I be overlooking?"
   - Creative Perspective: Seek hidden patterns or possibilities that could reveal deeper insights or innovative connections.

   *2.2 Analyze the Information:*
   - Objective: Break down the problem logically.
   - Reflection: "Am I considering all factors? Are there any assumptions that need challenging?"
   - Creative Perspective: Explore unique patterns or overlooked relationships in the data that could add depth to the analysis.

   *2.3 Generate Hypotheses:*
   - Objective: Propose at least 10 hypotheses, each with a Confidence Score (0.0 to 1.0) and Creative Score (0.0 to 1.0), reflecting originality, surprise, and utility.
   - Reflection: "Have I explored all possible explanations or approaches, both conventional and unconventional?"
   - Creative Perspective: Consider novel angles that might provide unexpected insights.

   *2.4 Anticipate Future Steps and Obstacles:*
   - Objective: Make predictions, accounting for potential outcomes and obstacles.
   - Reflection: "What challenges might I face? Is my plan flexible for different scenarios?"
   - Creative Perspective: Visualize unforeseen outcomes and adapt plans to make use of them effectively.

   *2.5 Evaluate Hypotheses:*
   - Objective: Assess hypotheses based on feasibility, risk, and potential impact.
   - Evaluation: Refine Confidence and Creative Scores as needed.
   - Reflection: "Am I unbiased in my assessment? Which options fit best with the overall objectives?"
   - Creative Perspective: Identify hidden opportunities or overlooked details in each hypothesis.

   *2.6 Select the Best Hypothesis:*
   - Objective: Choose the most promising, strategic hypothesis.
   - Reflection: "Why does this hypothesis stand out? How does it uniquely address the issue?"
   - Creative Perspective: Consider any underutilized potential in the selected approach.

   *2.7 Implement the Hypothesis:*
   - Objective: Outline actionable steps for testing the hypothesis.
   - Reflection: "Is this plan practical? What resources or preparation are required?"
   - Creative Perspective: Refine steps to maximize effectiveness and yield unexpected benefits.

   *2.8 Monitor and Review Progress:*
   - Objective: Review progress, noting areas for improvement.
   - Reflection: "What’s working well? What could be improved?"
   - Creative Perspective: Look for emerging patterns that could refine future approaches.

   *2.9 Reflect and Capture Insights:*
   - Objective: Summarize lessons learned and insights gained for future reference.
   - Reflection: "What new understanding has emerged from this process?"
   - Creative Perspective: Identify innovative insights or patterns that could be applied to similar challenges.

3. *Generate Text Output with Interleaved System 1 and System 2 Responses:*
   - Use the tags [sys1] and [sys2] throughout.
   - Aim for a lengthier, detailed response. Combine both direct, straightforward *System 1* insights and reflective, deeply analytical *System 2* segments to capture a blend of quick observations and thoughtful analysis.

---

### *Example Input Text:*

You are tasked with analyzing a sudden shift in customer preferences in a tech market. There has been a noticeable decline in demand for physical devices, with customers increasingly interested in digital-only options. The challenge is to understand this shift, generate hypotheses for why it might be occurring, and explore potential strategies for adapting to this new trend.

---

### *Processed Output with System 1 and System 2 Thinking:*

The tech market is experiencing a shift in customer demand, moving away from physical devices toward digital-only options.

[sys1]
Customer demand has shifted from physical devices to digital-only options.
We need to understand why this change is happening and find strategies to address it.
Key factors may include cost, convenience, and technological trends.
[end sys1]

[sys2]
**2.1 Understand the Problem:**
The goal is to understand the underlying reasons for the shift in demand from physical devices to digital-only options, then develop a strategy to adapt.

*Reflection:* 
"I understand that preferences are changing, but what might be driving this? Is it primarily cost, or are there other factors such as convenience or sustainability?"

*Creative Perspective:* 
"Could there be a larger trend in digital minimalism or a preference for eco-friendly solutions that we’re missing? What unseen motivations might explain this shift?"

**2.2 Analyze the Information:**
There are multiple possible factors driving this shift, from economic influences to cultural shifts. It’s essential to isolate each factor and understand its impact.

*Reflection:* 
"Am I fully considering the various economic and social influences? Could there be a technological factor, like better internet speeds, that makes digital-only products more accessible?"

*Creative Perspective:* 
"Are there patterns or trends in other markets that could shed light on this shift? Could this be part of a larger trend toward virtual experiences?"

**2.3 Generate Hypotheses:**
1. Customers prefer digital options due to lower costs. (Confidence: 0.8, Creative: 0.4)
2. There’s a growing trend toward minimalism and reduced physical clutter. (Confidence: 0.7, Creative: 0.7)
3. Digital products offer greater flexibility and ease of use. (Confidence: 0.6, Creative: 0.6)
4. Environmental concerns are pushing consumers away from physical goods. (Confidence: 0.6, Creative: 0.8)
5. Advances in tech make digital-only options more functional. (Confidence: 0.8, Creative: 0.5)
6. Pandemic-era remote work increased demand for digital solutions. (Confidence: 0.7, Creative: 0.6)
7. Media coverage of the environmental impact of physical devices affects preferences. (Confidence: 0.5, Creative: 0.7)
8. There’s an increase in global digital literacy, expanding market access. (Confidence: 0.6, Creative: 0.6)
9. Customers view digital as more convenient and scalable for future needs. (Confidence: 0.7, Creative: 0.5)
10. Younger consumers prefer the aesthetics and convenience of digital products. (Confidence: 0.6, Creative: 0.6)

*Reflection:* 
"Have I considered all possible influences? Are there any surprising factors that could explain this shift?"

*Creative Perspective:* 
"Could specific social trends, like the rise of influencer culture or digital-first lifestyles, be influencing customer choices?"

**2.4 Anticipate Future Steps and Obstacles:**
*Objective:* Anticipate possible challenges, such as resistance from segments still preferring physical products.

*Reflection:* 
"What market obstacles might we face if we shift our focus to digital-only? Are there sub-segments that still prioritize physical products?"

*Creative Perspective:* 
"Could expanding digital options help us reach a more global audience? Are there emerging trends that we could leverage in our strategy?"

[end sys2]

[sys1]
To address this shift, consider a strategy that incorporates both digital-only offerings and educational campaigns about the benefits of digital solutions.
Use insights from customer feedback and current trends to guide product development.
Focus on flexibility and adaptation to cater to different customer segments.
[end sys1]


/*-------------------------------- Arctic Core ------------------------------
 * Copyright (C) 2013, ArcCore AB, Sweden, www.arccore.com.
 * Contact: <contact@arccore.com>
 *
 * You may ONLY use this file:
 * 1)if you have a valid commercial ArcCore license and then in accordance with
 * the terms contained in the written license agreement between you and ArcCore,
 * or alternatively
 * 2)if you follow the terms found in GNU General Public License version 2 as
 * published by the Free Software Foundation and appearing in the file
 * LICENSE.GPL included in the packaging of this file or here
 * <http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt>
 *-------------------------------- Arctic Core -----------------------------*/

#include "SoAd.h"
#include "SoAd_Internal.h"
#include "PduR_SoAd.h"
#include "TcpIp.h"
#include "Bsd.h"
#include "MemMap.h"

#include "Det.h"
#if defined(USE_DEM)
#include "Dem.h"
#endif

#include <string.h>

#define assert(cond) if (!((cond))) { while(1); }

#define DOIP_PROTOCOL_VERSION	2

// Generic doip header negative acknowledge codes
#define DOIP_E_INCORRECT_PATTERN_FORMAT	0x00
#define DOIP_E_UNKNOWN_PAYLOAD_TYPE		0x01
#define DOIP_E_MESSAGE_TO_LARGE			0x02
#define DOIP_E_OUT_OF_MEMORY			0x03
#define DOIP_E_INVALID_PAYLOAD_LENGTH	0x04

// Diagnostic message negative acknowledge codes
// 0x00�0x01  Reserved by document
#define DOIP_E_DIAG_INVALID_SA			0x02	// Invalid Source Address
#define DOIP_E_DIAG_UNKNOWN_TA			0x03	// Unknown Target Address
#define DOIP_E_DIAG_MESSAGE_TO_LARGE	0x04	// Diagnostic Message too large
#define DOIP_E_DIAG_OUT_OF_MEMORY		0x05	// Out of memory
#define DOIP_E_DIAG_TARGET_UNREACHABLE	0x06	// Target unreachable
#define DOIP_E_DIAG_UNKNOWN_NETWORK		0x07	// Unknown network
#define DOIP_E_DIAG_TP_ERROR			0x08	// Transport protocol error
// 0x09�0xFF  Reserved by document


static void handleTimeout(uint16 connectionIndex);

static uint8 VinGidSyncStatus = 0x10;

static enum { DOIP_LINK_UP, DOIP_LINK_DOWN } LinkStatus = DOIP_LINK_DOWN;

static uint32 DoIp_ArcAnnouncementTimer = 0;
static uint16 DoIp_ArcAnnounceWait;

// Table maintained
static DoIp_ArcDoIpSocketStatusType connectionStatus[DOIP_MAX_TESTER_CONNECTIONS];

/*
 * variable mapping from target node to last associated connection
 */
static uint16 targetConnectionMap[DOIP_TARGET_COUNT];

static uint16 pendingRoutingActivationSocket = 0xff;
static uint16 pendingRoutingActivationSa = 0xff;
static uint16 pendingRoutingActivationActivationType = 0xffff;
static uint8* pendingRoutingActivationTxBuffer = NULL;

#ifndef DOIP_RAND
static uint16 doip_rand(void) {
	// https://en.wikipedia.org/wiki/Xorshift
	static uint32 x = 123456789;
	static uint32 y = 362436069;
	static uint32 z = 521288629;
	static uint32 w = 88675123;
	uint32 t;

	t = x ^ (x << 11);
	x = y; y = z; z = w;
	w = w ^ (w >> 19) ^ (t ^ (t >> 8));
	return (uint16) w;
}

#define DOIP_RAND doip_rand
#endif

static void discardIpMessage(uint16 sockNr)
{
	// Discarding this message
}


/*
 * Compares two arrays, returns 0 if matching, otherwise non-zero.
 */
static uint8 bincmp(const uint8* arr1, const uint8* arr2, uint32 len) {
	uint32 i = 0;
	while (i < len) {
		uint8 res = *arr1 - *arr2;
		if (0 != res) {
			// non-matching!
			return res;
		}
		arr1++;
		arr2++;
		i++;
	}
	return 0;
}

void DoIp_LocalIpAddrAssignmentChg(SoAd_SoConIdType linkId, TcpIp_IpAddrStateType state) {
	switch (state) {
	case TCPIP_IPADDR_STATE_ASSIGNED:
		LinkStatus = DOIP_LINK_UP;
		DoIp_ArcAnnouncementTimer = 0;
		DoIp_ArcAnnounceWait = DOIP_RAND() % SOAD_DOIP_ANNOUNCE_WAIT;
		break;
	default:
		LinkStatus = DOIP_LINK_DOWN;
		DoIp_ArcAnnouncementTimer = 0xffff;
		break;
	}
}

static void createAndSendNack(uint16 sockNr, uint8 nackCode)
{
	uint8 txBuffer[8+1];
	uint16 bytesSent;


	txBuffer[0] = DOIP_PROTOCOL_VERSION;
	txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
	txBuffer[2] = 0x00;	// 0x0000->Negative Acknowledge
	txBuffer[3] = 0x00;
	txBuffer[4] = 0;	// Length = 0x00000001
	txBuffer[5] = 0;
	txBuffer[6] = 0;
	txBuffer[7] = 1;

	txBuffer[8] = nackCode;

	bytesSent = SoAd_SendIpMessage(sockNr, 8+1, txBuffer);
	if ( bytesSent != 8+1) {
		// Failed to send ack. Link is probably down.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_CREATE_AND_SEND_NACK_ID, SOAD_E_UNEXPECTED_EXECUTION);
	}
}


static void createAndSendDiagnosticAck(uint16 sockNr, uint16 sa, uint16 ta)
{
	uint8 txBuffer[8+5];
	uint16 bytesSent;

	txBuffer[0] = DOIP_PROTOCOL_VERSION;
	txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
	txBuffer[2] = 0x80;	// 0x8002->Diagnostic Message Positive Acknowledge
	txBuffer[3] = 0x02;
	txBuffer[4] = 0;	// Length = 0x00000005
	txBuffer[5] = 0;
	txBuffer[6] = 0;
	txBuffer[7] = 5;

	txBuffer[8+0] = ta >> 8;	// TA
	txBuffer[8+1] = ta;

	txBuffer[8+2] = sa >> 8;	// SA
	txBuffer[8+3] = sa;

	txBuffer[8+4] = 0;		// ACK

	bytesSent = SoAd_SendIpMessage(sockNr, 8+5, txBuffer);
	if (bytesSent != 8+5) {
		// Failed to send ack. Link is probably down.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_CREATE_AND_SEND_D_ACK_ID, SOAD_E_UNEXPECTED_EXECUTION);
	}
}


static void createAndSendDiagnosticNack(uint16 sockNr, uint16 sa, uint16 ta, uint8 nackCode)
{
	uint8 txBuffer[8+5];
	uint16 bytesSent;

	txBuffer[0] = DOIP_PROTOCOL_VERSION;
	txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
	txBuffer[2] = 0x80;	// 0x8003->Diagnostic Message Negative Acknowledge
	txBuffer[3] = 0x03;
	txBuffer[4] = 0;	// Length = 0x00000005
	txBuffer[5] = 0;
	txBuffer[6] = 0;
	txBuffer[7] = 5;

	txBuffer[8+0] = ta >> 8;	// TA
	txBuffer[8+1] = ta;

	txBuffer[8+2] = sa >> 8;	// SA
	txBuffer[8+3] = sa;

	txBuffer[8+4] = nackCode;

	bytesSent = SoAd_SendIpMessage(sockNr, 8+5, txBuffer);
	if (bytesSent != 8+5) {
		// Failed to send ack. Link is probably down.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_CREATE_AND_SEND_D_NACK_ID, SOAD_E_UNEXPECTED_EXECUTION);
	}

}



static void createVehicleIdentificationResponse(uint8* txBuffer) {
	int i;

	txBuffer[0] = DOIP_PROTOCOL_VERSION;
	txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
	txBuffer[2] = 0x00;	// 0x0004->Vehicle Identification Response
	txBuffer[3] = 0x04;
	txBuffer[4] = 0;	// Length = 0x00000021 (33)
	txBuffer[5] = 0;
	txBuffer[6] = 0;
	txBuffer[7] = 33;

	// VIN field
	if (E_NOT_OK == SoAd_DoIp_Arc_GetVin(&txBuffer[8+0], 17)) {
		for (i = 0; i < 17; i++) {
			txBuffer[8+0+i] = 0;
		}
	}

	// Logical address field
	txBuffer[8+17+0] = (SoAd_Config.DoIpNodeLogicalAddress >> 8) & 0xff;
	txBuffer[8+17+1] = (SoAd_Config.DoIpNodeLogicalAddress >> 0) & 0xff;

	// EID field
	if (E_NOT_OK == SoAd_DoIp_Arc_GetEid(&txBuffer[8+19], 6)) {
		for (i = 0; i < 6; i++) {
			txBuffer[8+19+i] = 0;
		}
	}

	// GID field
	if (E_NOT_OK == SoAd_DoIp_Arc_GetGid(&txBuffer[8+25], 6)) {
		for (i = 0; i < 6; i++) {
			txBuffer[8+25+i] = 0;
		}
	}

	// Further action required field
	if (E_NOT_OK == SoAd_DoIp_Arc_GetFurtherActionRequired(&txBuffer[8+31])) {
		txBuffer[8+31] = 0x00;	// No further action required
	}

	// VIN/GID Sync Status field
	if (E_NOT_OK == SoAd_DoIp_Arc_GetFurtherActionRequired(&txBuffer[8+32])) {
		txBuffer[8+32] = VinGidSyncStatus;
	}

}

void DoIp_SendVehicleAnnouncement(uint16 sockNr) {
	uint8* txBuffer;
	uint16 bytesSent;

	if (SoAd_BufferGet(8+33, &txBuffer)) {
		createVehicleIdentificationResponse(txBuffer);

		bytesSent = SoAd_SendIpMessage(sockNr, 8+33, txBuffer);
		if (bytesSent != 8+33) {
			// Failed to send ack. Link is probably down.
			DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_VEHICLE_ID_REQ_ID, SOAD_E_UNEXPECTED_EXECUTION);
		}
		SoAd_BufferFree(txBuffer);

	} else {
		// Not enough buffers. Report error in error log.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_VEHICLE_ID_REQ_ID, SOAD_E_NOBUFS);
	}
}

static void handleVehicleIdentificationReq(uint16 sockNr, uint32 payloadLength, uint8 *rxBuffer, SoAd_Arc_DoIp_VehicleIdentificationRequestType type)
{
	uint8* txBuffer;
	uint16 bytesSent;
	boolean doRespond = FALSE;

	if (SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_ALL == type) {
		if (0 == payloadLength) {
			doRespond = TRUE;

		} else {
			// Invalid payload length!
			createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
			SoAd_SocketClose(sockNr);
		}

	} else if (SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_BY_EID == type) {
		if (6 == payloadLength) {
			uint8 myEid[6];
			SoAd_DoIp_Arc_GetEid(myEid, sizeof(myEid));

			if (0 == bincmp(myEid, &rxBuffer[8], sizeof(myEid))) {
				doRespond = TRUE;
			}
		} else {
			// Invalid payload length!
			createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
			SoAd_SocketClose(sockNr);
		}
	} else if (SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_BY_VIN == type) {
		if (17 == payloadLength) {
			uint8 myVin[17];
			SoAd_DoIp_Arc_GetVin(myVin, sizeof(myVin));

			if (0 == bincmp(myVin, &rxBuffer[8], sizeof(myVin))) {
				doRespond = TRUE;
			}
		} else {
			// Invalid payload length!
			createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
			SoAd_SocketClose(sockNr);
		}
	} else {
		createAndSendNack(sockNr, DOIP_E_UNKNOWN_PAYLOAD_TYPE);
		SoAd_SocketClose(sockNr);
	}

	if (doRespond) {
		if (SoAd_BufferGet(8+33, &txBuffer)) {
			createVehicleIdentificationResponse(txBuffer);

			bytesSent = SoAd_SendIpMessage(sockNr, 8+33, txBuffer);
			if (bytesSent != 8+33) {
				// Failed to send data. Link is probably down.
				DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_VEHICLE_ID_REQ_ID, SOAD_E_UNEXPECTED_EXECUTION);
			}
			SoAd_BufferFree(txBuffer);


		} else {
			// Not enough tx buffer available. Report a Det error?
			DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_VEHICLE_ID_REQ_ID, SOAD_E_NOBUFS);
		}

	} else {
		createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
		SoAd_SocketClose(sockNr);
	}
}

static boolean isSourceAddressKnown(uint16 sa) {
	boolean saKnown = FALSE;
	uint16 i;


	for (i = 0; (i < DOIP_TESTER_COUNT) && (FALSE == saKnown); i++) {
		if (SoAd_Config.DoIpTesters[i].address  == sa) {
			saKnown = TRUE;
		}
	}

	return saKnown;
}

static boolean isRoutingTypeSupported(uint16 activationType) {
	boolean supported = FALSE;
	uint16 i;

	for (i = 0; (i < DOIP_ROUTINGACTIVATION_COUNT) && (FALSE == supported); i++) {
		if (SoAd_Config.DoIpRoutingActivations[i].activationNumber == activationType) {
			supported = TRUE;
		}
	}

	return supported;
}

static boolean isAuthenticationRequired(uint16 activationType) {
	boolean req = FALSE;
	uint16 i;

	for (i = 0; (i < DOIP_ROUTINGACTIVATION_COUNT) && (FALSE == req); i++) {
		if (SoAd_Config.DoIpRoutingActivations[i].activationNumber == activationType) {
			if (NULL == SoAd_Config.DoIpRoutingActivations[i].authenticationCallback) {
				req = FALSE;
			} else {
				req = TRUE;
			}
			return req;
		}
	}

	return req;
}

static boolean isAuthenticated(uint16 activationType) {
	// TODO: Authentication not supported
	return FALSE;
}

static boolean isConfirmationRequired(uint16 activationType) {
	boolean req = FALSE;
	uint16 i;

	for (i = 0; (i < DOIP_ROUTINGACTIVATION_COUNT) && (FALSE == req); i++) {
		if (SoAd_Config.DoIpRoutingActivations[i].activationNumber == activationType) {
			if (NULL == SoAd_Config.DoIpRoutingActivations[i].confirmationCallback) {
				req = FALSE;
			} else {
				req = TRUE;
			}
			return req;
		}
	}

	return req;
}

static boolean isConfirmed(uint16 activationType) {
	// TODO: Confirmation not supported
	return FALSE;
}

static void createAndSendAliveCheck(uint16 connectionIndex) {
	uint8 *txBuffer;
	uint16 bytesSent;

	if (SoAd_BufferGet(8, &txBuffer)) {
		txBuffer[0] = DOIP_PROTOCOL_VERSION;
		txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
		txBuffer[2] = 0x00;	// 0x0007->Alive check request
		txBuffer[3] = 0x07;
		txBuffer[4] = 0;	// Length = 0x0000000 (0)
		txBuffer[5] = 0;
		txBuffer[6] = 0;
		txBuffer[7] = 0;


		bytesSent = SoAd_SendIpMessage(connectionStatus[connectionIndex].sockNr, 8, txBuffer);
		if (bytesSent != 8) {
			/*
			 * Could not send data - socket probably broken, so let's close the socket.
			 */

			connectionStatus[connectionIndex].socketState = DOIP_ARC_CONNECTION_INVALID;

			/* If there are pending routing activations waiting, activate that one. */
			if (0xff != pendingRoutingActivationSocket) {
				handleTimeout(connectionIndex);
			} else {
				SoAd_SocketClose(connectionStatus[connectionIndex].sockNr);
			}


		}
		SoAd_BufferFree(txBuffer);

	} else {
		// No tx buffer available. Report a Det error.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_CREATE_AND_SEND_ALIVE_CHECK_ID, SOAD_E_NOBUFS);
	}

}

static void startSingleSaAliveCheck(uint16 sa) {
	uint16 i;
	for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
		if (connectionStatus[i].sa == sa && FALSE == connectionStatus[i].awaitingAliveCheckResponse) {
			connectionStatus[i].awaitingAliveCheckResponse = TRUE;
			connectionStatus[i].generalInactivityTimer = 0;
			connectionStatus[i].aliveCheckTimer = 0;

			createAndSendAliveCheck(i);
		}

	}
}

static void startAllSocketsAliveCheck() {

	uint16 i;
	for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
		// No need to check connection states as this method is only called
		// when all sockets are in registered state...

		connectionStatus[i].awaitingAliveCheckResponse = TRUE;
		connectionStatus[i].generalInactivityTimer = 0;
		connectionStatus[i].aliveCheckTimer = 0;
		createAndSendAliveCheck(i);
	}
}

static void registerSocket(uint16 slotIndex, uint16 sockNr, uint16 activationType, uint16 sa) {
	assert(slotIndex < DOIP_MAX_TESTER_CONNECTIONS);

	connectionStatus[slotIndex].sockNr = sockNr;
	connectionStatus[slotIndex].sa = sa;

	connectionStatus[slotIndex].generalInactivityTimer = 0;
	connectionStatus[slotIndex].initialInactivityTimer = 0;

	connectionStatus[slotIndex].authenticated = FALSE;
	connectionStatus[slotIndex].confirmed = FALSE;

	connectionStatus[slotIndex].awaitingAliveCheckResponse = FALSE;

	connectionStatus[slotIndex].socketState = DOIP_ARC_CONNECTION_REGISTERED;
}

static DoIp_Arc_SocketAssignmentResultType socketHandle(uint16 sockNr, uint16 activationType, uint16 sa, uint8* routingActivationResponseCode)
{
	/*
	 * This method is intended to implement Figure 13 in ISO/FDIS 13400-2:2012(E)
	 */
	uint16 numRegisteredSockets = 0;
	uint16 i;
	boolean socketFound = FALSE;

	for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
		if (DOIP_ARC_CONNECTION_REGISTERED == connectionStatus[i].socketState) {
			numRegisteredSockets++;
		}
	}

	if (0 == numRegisteredSockets) {
		// No registered sockets, so we pick a slot for this connection:
		registerSocket(0, sockNr, activationType, sa);

		// No need to set *routingActivationResponseCode when socket assignment is successful.
		//*routingActivationResponseCode = 0;
		return DOIP_SOCKET_ASSIGNMENT_SUCCESSFUL;
	} else {
		// There is at least one registered socket already. Let's find out if it's this socket..
		for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
			if (connectionStatus[i].sockNr == sockNr) {
				socketFound = TRUE;

				// We found the TCP socket. Is it registered?
				if (connectionStatus[i].socketState == DOIP_ARC_CONNECTION_REGISTERED) {
					// We found the registered TCP socket. Is it assigned to this tester (SA)?
					if (connectionStatus[i].sa == sa) {
						/** @req DoIP-089 */
						return DOIP_SOCKET_ASSIGNMENT_SUCCESSFUL;

					} else {
						/** @req DoIP-106 */
						/* Routing activation denied because an SA different
						 * from the table connection entry was received on the
						 * already activated TCP_DATA socket. */
						*routingActivationResponseCode = 2;
						return DOIP_SOCKET_ASSIGNMENT_FAILED;
					}
				}
			}
		}
		/*
		 *  For loop terminated; that means that the current socket is currently not registered.
		 *
		 *  Next up: Check if SA is already registered to another socket...
		 */
		for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
			if ((connectionStatus[i].sa == sa) && DOIP_ARC_CONNECTION_REGISTERED == connectionStatus[i].socketState) {
				// Yes, the SA is already registered on another socket!
				/**
				 * @req DoIP-091
				 */
				// perform alive check single SA
				startSingleSaAliveCheck(sa);
				return DOIP_SOCKET_ASSIGNMENT_PENDING;
			}
		}

		/*
		 * For loop terminated; that means that the current SA is not already registered to another socket.
		 *
		 * Next up: Check to see that there is a free socket slot available..
		 */
		for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
			if (DOIP_ARC_CONNECTION_INVALID == connectionStatus[i].socketState) {
				/* Yes, we found a free slot */
				/** @req DoIp-090 */
				registerSocket(i, sockNr, activationType, sa);
				return DOIP_SOCKET_ASSIGNMENT_SUCCESSFUL;
			}
		}

		/*
		 * For loop terminated; that means that there are no free slots.
		 *
		 * Perform alive check on all registered sockets...
		 */
		/** @req DoIP-094 */
		startAllSocketsAliveCheck();
		return DOIP_SOCKET_ASSIGNMENT_PENDING;

	}
}


void fillRoutingActivationResponseData(uint8 *txBuffer, uint16 sa, uint8 routingActivationResponseCode)
{
    txBuffer[0] = DOIP_PROTOCOL_VERSION;
    txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
    txBuffer[2] = 0x00; // 0x0006->Routing Activation Response
    txBuffer[3] = 0x06;
    txBuffer[4] = 0; // Length = 0x0000000d (13)
    txBuffer[5] = 0;
    txBuffer[6] = 0;
    txBuffer[7] = 13;
    txBuffer[8] = sa >> 8;
    txBuffer[8 + 1] = sa;

	txBuffer[8 + 2] = (SoAd_Config.DoIpNodeLogicalAddress >> 8) & 0xff;
	txBuffer[8 + 3] = (SoAd_Config.DoIpNodeLogicalAddress >> 0) & 0xff;
    txBuffer[8 + 4] = routingActivationResponseCode;
    txBuffer[8 + 5] = 0; // Reserved
    txBuffer[8 + 6] = 0;
    txBuffer[8 + 7] = 0;
    txBuffer[8 + 8] = 0;
    txBuffer[8 + 9] = 0; // OEM use
    txBuffer[8 + 10] = 0;
    txBuffer[8 + 11] = 0;
    txBuffer[8 + 12] = 0;
}

static void handleRoutingActivationReq(uint16 sockNr, uint32 payloadLength, uint8 *rxBuffer)
{
	uint16 sa;
	uint16 activationType;

	uint8* txBuffer;
	uint16 bytesSent;

	/** @req SWS_DoIP_00117  is invalid (refers to the wrong service and does not match
	 * ISO13400-2:2012 issued on 2012-06-18)
	 *
	 * Decision made together with Customer's diagnostic tester group that activation
	 * type is only one byte, thus payload length may be 11 or 7. (Not 12 or 8 as
	 * AUTOSAR requirement states)
	 */
	if ((7 == payloadLength) || (11 == payloadLength)) {
		sa = (rxBuffer[8+0] << 8) + rxBuffer[8+1];
		activationType = rxBuffer[8+2];

		if (SoAd_BufferGet(8+13, &txBuffer)) {
			uint8 routingActivationResponseCode = 0x7e; // Vehicle manufacturer-specific

			if (TRUE == isSourceAddressKnown(sa)) {

				if (TRUE == isRoutingTypeSupported(activationType)) {

					DoIp_Arc_SocketAssignmentResultType socketHandleResult = socketHandle(sockNr, activationType, sa, &routingActivationResponseCode);
					if (DOIP_SOCKET_ASSIGNMENT_SUCCESSFUL == socketHandleResult) {
						boolean authenticated;

						if (TRUE == isAuthenticationRequired(activationType)) {
							authenticated = isAuthenticated(activationType);

						} else {
							authenticated = TRUE;
						}

						if (authenticated) {
							if (TRUE == isConfirmationRequired(activationType)) {
								if (TRUE == isConfirmed(activationType)) {
									/* Routing successfully activated */
									routingActivationResponseCode = 0x10;


								} else {
									/* Routing will be activated; confirmation required */
									routingActivationResponseCode = 0x11;

								}
							} else {
								/* Routing successfully activated */
								routingActivationResponseCode = 0x10;

							}


						} else {
							/* Routing activation rejected due to missing authentication */
							/** @req DoIP-061 */
							routingActivationResponseCode = 0x04;
						}

					} else if (DOIP_SOCKET_ASSIGNMENT_FAILED == socketHandleResult) { /* socketHandle */
						/* Routing activation denied because:
						 *  0x01 - all concurrently supported TCP_DATA sockets are
						 *         registered and active
						 *  0x02 - an SA different from the table connection entry
						 *         was received on the already activated TCP_DATA
						 *         socket
						 *  0x03 - the SA is already registered and active on a
						 *         different TCP_DATA socket
						 *
						 *  socketHandle() shall already have written the corresponding response code to
						 *  routingActivationResponseCode
						 */

						/* Validate response code */
						switch (routingActivationResponseCode) {
						case 0x01:
						case 0x02:
						case 0x03:
							/* OK! */
							break;
						default:
							/* Unsupported response code at this level */
							routingActivationResponseCode = 0x7e;
							DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ROUTING_ACTIVATION_REQ_ID, SOAD_E_SHALL_NOT_HAPPEN);
							break;
						}

					} else if (DOIP_SOCKET_ASSIGNMENT_PENDING == socketHandleResult) { /* socketHandle */
						/*
						 * Trying to assign a connection slot, but pending
						 * alive check responses before continuing.
						 * Continuation handled from DoIp_MainFunction (if a
						 * connection times out and thus becomes free) or from
						 * handleAliveCheckResp (if all connections remain
						 * active)
						 */
						if (NULL == pendingRoutingActivationTxBuffer) {
							pendingRoutingActivationTxBuffer = txBuffer;
							pendingRoutingActivationSa = sa;
							pendingRoutingActivationSocket = sockNr;
							pendingRoutingActivationActivationType = activationType;

						} else {
							// Socket assignment pending; alive check response already pending
							// This should not happen - the connection attempt should not have been accepted..
							DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ROUTING_ACTIVATION_REQ_ID, SOAD_E_SHALL_NOT_HAPPEN);
						}
						return;

					} else {
						/* This cannot happen */
						DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ROUTING_ACTIVATION_REQ_ID, SOAD_E_SHALL_NOT_HAPPEN);
					}

				} else { /* isRoutingTypeSupported(activationType) */
					/* Routing activation denied due to unsupported routing activation type */
					/** @req DoIP-151 */
					routingActivationResponseCode = 0x06;
				}

			} else { /* isSourceAddressKnown(sa) */
				/* Routing activation rejected due to unknown source address */
				/** @req DoIP-059 */
				routingActivationResponseCode = 0x00;
			}

			fillRoutingActivationResponseData(txBuffer, sa, routingActivationResponseCode);

			bytesSent = SoAd_SendIpMessage(sockNr, 8+13, txBuffer);
			if (bytesSent != 8+13) {
				// Could not send data - report error in development error tracer
				DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ROUTING_ACTIVATION_REQ_ID, SOAD_E_UNEXPECTED_EXECUTION);
			}
			SoAd_BufferFree(txBuffer);

			/**
			 * @req DoIP-102
			 *
			 * Negative response code --> Close socket on which the current message was received
			 */
			switch (routingActivationResponseCode) {
			case 0x10: // Routing activated.
			case 0x11: // Confirmation pending
			case 0x04: // Missing authentication
				break;


			default:
				SoAd_SocketClose(sockNr);
				break;
			}

		} else { /* if (SoAd_BufferGet(8+13, &txBuffer)) */
			DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ROUTING_ACTIVATION_REQ_ID, SOAD_E_NOBUFS);
			createAndSendNack(sockNr, DOIP_E_OUT_OF_MEMORY);
		}
	} else {
		createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
		SoAd_SocketClose(sockNr);
	}
}


typedef enum {
	LOOKUP_SA_TA_OK,
	LOOKUP_SA_TA_TAUNKNOWN,
	LOOKUP_SA_TA_SAERR
} LookupSaTaResultType;


LookupSaTaResultType lookupSaTa(uint16 connectionIndex, uint16 sa, uint16 ta, uint16* targetIndex)
{
	uint16 i;
	uint16 routingActivationIndex = 0xffff;


	if (0xffff == connectionIndex) {
		// Connection not registered!
		return LOOKUP_SA_TA_SAERR;
	}

	for (i = 0; i < DOIP_ROUTINGACTIVATION_COUNT; i++) {
		if (connectionStatus[i].activationType == SoAd_Config.DoIpRoutingActivations[i].activationNumber) {
			routingActivationIndex = i;
			break;
		}
	}

	if (0xffff == routingActivationIndex) {
		// No such routing activation!
		return LOOKUP_SA_TA_TAUNKNOWN;
	}

	for (i = 0; i < DOIP_ROUTINGACTIVATION_TO_TARGET_RELATION_COUNT; i++) {
		uint16 itTargetIndex = SoAd_Config.DoIpRoutingActivationToTargetAddressMap[i].target;

		if ((ta == SoAd_Config.DoIpTargetAddresses[itTargetIndex].addressValue) &&
			(routingActivationIndex == SoAd_Config.DoIpRoutingActivationToTargetAddressMap[i].routingActivation))
		{
			*targetIndex = itTargetIndex;
			return LOOKUP_SA_TA_OK;

		}
	}

	return LOOKUP_SA_TA_TAUNKNOWN;
}



static void handleEntityStatusReq(uint16 sockNr, uint32 payloadLength, uint8 *rxBuffer)
{
	if (0 == payloadLength) {
		uint8* txBuffer = NULL;
		if (SoAd_BufferGet(8+7, &txBuffer)) {
			uint8 maxNofSockets = SOAD_SOCKET_COUNT; // Or should we use SOAD_MAX_OPEN_SOCKETS?
			uint8 curNofOpenSockets = SoAd_GetNofCurrentlyUsedTcpSockets();
			uint32 maxDataSize = SOAD_RX_BUFFER_SIZE;
			uint16 bytesSent;

			txBuffer[0] = DOIP_PROTOCOL_VERSION;
			txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
			txBuffer[2] = 0x40;	// 0x4002->Entity status response
			txBuffer[3] = 0x02;
			txBuffer[4] = 0;	// Length = 0x00000007 (7)
			txBuffer[5] = 0;
			txBuffer[6] = 0;
			txBuffer[7] = 7;

			txBuffer[8] = SOAD_DOIP_NODE_TYPE;
			txBuffer[9] = maxNofSockets;  // Max TCP sockets
			txBuffer[10] = curNofOpenSockets; // Currently open TCP sockets

			txBuffer[11] = (maxDataSize << 24);
			txBuffer[12] = (maxDataSize << 16) & 0xff;
			txBuffer[13] = (maxDataSize << 8) & 0xff;
			txBuffer[14] = maxDataSize & 0xff;

			bytesSent = SoAd_SendIpMessage(sockNr, 8+7, txBuffer);
			if (bytesSent != 8+7) {
				/*
				 * Failed to send the message. Report error to Det.
				 */
				DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ENTITY_STATUS_REQ_ID, SOAD_E_UNEXPECTED_EXECUTION);
			}
			SoAd_BufferFree(txBuffer);

		} else {
			DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ENTITY_STATUS_REQ_ID, SOAD_E_NOBUFS);
			createAndSendNack(sockNr, DOIP_E_OUT_OF_MEMORY);

		}
	} else {
		// Invalid payload length!
		createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
		SoAd_SocketClose(sockNr);
	}
}


static void handlePowerModeCheckReq(uint16 sockNr, uint32 payloadLength, uint8 *rxBuffer)
{
	// @req SWS_DoIP_00091
	if (0 == payloadLength) {
		uint8* txBuffer = NULL;
		if (SoAd_BufferGet(8+1, &txBuffer)) {
			SoAd_DoIp_PowerMode powerMode;
			uint16 bytesSent;

			if (E_NOT_OK == SoAd_Config.DoIpConfig->DoipPowerModeCallback(&powerMode)) {
				// @req SWS_DoIP_00093
				powerMode = 0;
			}

			txBuffer[0] = DOIP_PROTOCOL_VERSION;
			txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
			txBuffer[2] = 0x40;	// 0x4002->Power mode check response
			txBuffer[3] = 0x04;
			txBuffer[4] = 0;	// Length = 0x00000001 (1)
			txBuffer[5] = 0;
			txBuffer[6] = 0;
			txBuffer[7] = 1;

			txBuffer[8] = (uint8)powerMode;

			// @req SWS_DoIP_00092
			bytesSent = SoAd_SendIpMessage(sockNr, 8+1, txBuffer);
			if (bytesSent != 8+1) {
				DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ENTITY_STATUS_REQ_ID, SOAD_E_UNEXPECTED_EXECUTION);
			}
			SoAd_BufferFree(txBuffer);

		} else {
			DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_ENTITY_STATUS_REQ_ID, SOAD_E_NOBUFS);
			createAndSendNack(sockNr, DOIP_E_OUT_OF_MEMORY);

		}
	} else {
		// Invalid payload length!
		createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
		SoAd_SocketClose(sockNr);
	}
}

static void handleAliveCheckResp(uint16 sockNr, uint16 payloadLength, uint8* rxBuffer) {
	uint16 i;
	uint16 sa;

	if (payloadLength == 2) {
		sa = (rxBuffer[8+0] << 8) | rxBuffer[8+1];

		for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
			if (connectionStatus[i].sockNr == sockNr) {
				if (connectionStatus[i].sa == sa) {
					// Alive check response received in time
					connectionStatus[i].generalInactivityTimer = 0;
					connectionStatus[i].awaitingAliveCheckResponse = FALSE;
				} else {
					// Alive check response received from the wrong SA!
					// What to do?
				}
			}
		}

		// Connections remaining to receive alive check responses for
		uint16 remainingConnections = 0;
		for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
			if (TRUE == connectionStatus[i].awaitingAliveCheckResponse) {
				remainingConnections++;
			}
		}

		if (0 == remainingConnections) {
			/*
			 * No remaining connections to receive alive check response for.
			 * This must mean that the incoming routing activation request must
			 * be denied...
			 */

			/* 0x01: Routing activation denied because all concurrently supported
			 *       TCP_DATA sockets are registered and active. */
			uint8 routingActivationResponseCode = 0x01;
			uint16 bytesSent;

			if ((pendingRoutingActivationSocket != 0xff) && (NULL != pendingRoutingActivationTxBuffer)) {
				fillRoutingActivationResponseData(pendingRoutingActivationTxBuffer, pendingRoutingActivationSa, routingActivationResponseCode);

				bytesSent = SoAd_SendIpMessage(pendingRoutingActivationSocket, 8+13, pendingRoutingActivationTxBuffer);

				if (bytesSent != (8+13)) {
					// Failed to send response. Connection probably already broken..
					DET_REPORTERROR(MODULE_ID_SOAD, 0, DOIP_HANDLE_ALIVECHECK_RESP, SOAD_E_UNEXPECTED_EXECUTION);
				}
				SoAd_BufferFree(pendingRoutingActivationTxBuffer);
				SoAd_SocketClose(pendingRoutingActivationSocket);
				pendingRoutingActivationTxBuffer = NULL;
				pendingRoutingActivationSocket = 0xff;
			}
		}

	}
}

static void handleTimeout(uint16 connectionIndex) {
	uint8 routingActivationResponseCode = 0x10;
	uint16 bytesSent;

	/*
	 * Close current connection (which has timed out anyway)
	 * and register a new connection with the pending routing activation
	 */
	SoAd_SocketClose(connectionStatus[connectionIndex].sockNr);

	if ((pendingRoutingActivationSocket != 0xff) && (NULL != pendingRoutingActivationTxBuffer)) {

		registerSocket(connectionIndex, pendingRoutingActivationSocket, pendingRoutingActivationActivationType, pendingRoutingActivationSa);

		fillRoutingActivationResponseData(pendingRoutingActivationTxBuffer, pendingRoutingActivationSa, routingActivationResponseCode);
		bytesSent = SoAd_SendIpMessage(pendingRoutingActivationSocket, 8+13, pendingRoutingActivationTxBuffer);

		if (bytesSent != (8+13)) {
			// Failed to send routing actication request..
			DET_REPORTERROR(MODULE_ID_SOAD, 0, DOIP_HANDLE_ALIVECHECK_TIMEOUT, SOAD_E_UNEXPECTED_EXECUTION);
		}
		SoAd_BufferFree(pendingRoutingActivationTxBuffer);
		pendingRoutingActivationTxBuffer = NULL;
		pendingRoutingActivationSocket = 0xff;
	} else {
		// Ordinary timeout set connection as free...
		connectionStatus[connectionIndex].socketState = DOIP_ARC_CONNECTION_INVALID;
		connectionStatus[connectionIndex].awaitingAliveCheckResponse = FALSE;
	}

}

static void associateTargetWithConnectionIndex(uint16 targetIndex, uint16 connectionIndex) {

	targetConnectionMap[targetIndex] = connectionIndex;
}

static void handleDiagnosticMessage(uint16 sockNr, uint32 payloadLength, uint8 *rxBuffer)
{
	LookupSaTaResultType lookupResult;
    BufReq_ReturnType result;
    PduInfoType* pduInfoPtr;
	uint16 sa;
	uint16 ta;
	uint16 targetIndex;
	uint16 connectionIndex;
	uint16 diagnosticMessageLength = payloadLength - 4;
	uint16 i;



	if (payloadLength >= 4) {
		sa = (rxBuffer[8] << 8) | rxBuffer[9];
		ta = (rxBuffer[10] << 8) | rxBuffer[11];


		// Find connection entry for this socket
		for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
			if ((sa == connectionStatus[i].sa) && (sockNr == connectionStatus[i].sockNr)) {
				connectionIndex = i;
				break;
			}
		}

		lookupResult = lookupSaTa(connectionIndex, sa, ta, &targetIndex);
		if (lookupResult == LOOKUP_SA_TA_OK) {
			// Send diagnostic message to PduR
			result = PduR_SoAdTpProvideRxBuffer(SoAd_Config.DoIpTargetAddresses[targetIndex].txPdu, diagnosticMessageLength, &pduInfoPtr);
			if (result == BUFREQ_OK) {
				associateTargetWithConnectionIndex(targetIndex, connectionIndex);

				(void)lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, 12, 0);
				(void)lwip_recv(SocketAdminList[sockNr].ConnectionHandle, pduInfoPtr->SduDataPtr, payloadLength - 4, 0);
				PduR_SoAdTpRxIndication(SoAd_Config.DoIpTargetAddresses[targetIndex].txPdu, NTFRSLT_OK);

				// Send diagnostic message positive ack
				createAndSendDiagnosticAck(sockNr, sa, ta);
			} else if (result != BUFREQ_BUSY){
				createAndSendDiagnosticNack(sockNr, sa, ta, DOIP_E_DIAG_OUT_OF_MEMORY);
				discardIpMessage(sockNr);
				DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_DIAG_MSG_ID, SOAD_E_SHALL_NOT_HAPPEN);
			}

		} else if (lookupResult == LOOKUP_SA_TA_TAUNKNOWN) {
			// TA not known
			createAndSendDiagnosticNack(sockNr, sa, ta, DOIP_E_DIAG_UNKNOWN_TA);
			discardIpMessage(sockNr);
		} else {
			// SA not registered on receiving socket
			createAndSendDiagnosticNack(sockNr, sa, ta, DOIP_E_DIAG_INVALID_SA);
			SoAd_SocketClose(sockNr);
		}
	} else {
		createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
		SoAd_SocketClose(sockNr);
	}
}


void DoIp_HandleTcpRx(uint16 sockNr)
{
	int nBytes;
	uint8* rxBuffer;
	uint16 payloadType;
	uint16 payloadLength;

	if (SoAd_BufferGet(SOAD_RX_BUFFER_SIZE, &rxBuffer)) {
		nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, SOAD_RX_BUFFER_SIZE, MSG_PEEK);
		SoAd_SocketStatusCheck(sockNr, SocketAdminList[sockNr].ConnectionHandle);
		if (nBytes >= 8) {
			if ((rxBuffer[0] == DOIP_PROTOCOL_VERSION) && ((uint8)(~rxBuffer[1]) == DOIP_PROTOCOL_VERSION)) {
				payloadType = rxBuffer[2] << 8 | rxBuffer[3];
				payloadLength = (rxBuffer[4] << 24) | (rxBuffer[5] << 16) | (rxBuffer[6] << 8) | rxBuffer[7];
				if ((payloadLength + 8) <= SOAD_RX_BUFFER_SIZE) {
					if ((payloadLength + 8) <= nBytes) {
						// Grab the message
						switch (payloadType) {
#if 0 /* Vehicle identification requests are not to be supported over TCP */
						case 0x0001:	// Vehicle Identification Request
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							handleVehicleIdentificationReq(sockNr, payloadLength, rxBuffer, SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_ALL);
							break;

						case 0x0002:	// Vehicle Identification Request with EID
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							handleVehicleIdentificationReq(sockNr, payloadLength, rxBuffer, SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_BY_EID);
							break;

						case 0x0003:	// Vehicle Identification Request with VIN
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							handleVehicleIdentificationReq(sockNr, payloadLength, rxBuffer, SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_BY_VIN);
							break;
#endif /* Vehicle identification requests are not to be supported over TCP */

						case 0x005:		// Routing Activation request
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							handleRoutingActivationReq(sockNr, payloadLength, rxBuffer);
							break;

						case 0x008:     // Alive check response
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							handleAliveCheckResp(sockNr, payloadLength, rxBuffer);
							break;

						case 0x4003:
							// @req SWS_DoIP_00090
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							createAndSendNack(sockNr, DOIP_E_INVALID_PAYLOAD_LENGTH);
							break;

						case 0x8001:	// Diagnostic message
							//nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							handleDiagnosticMessage(sockNr, payloadLength, rxBuffer);
							break;

						default:
							nBytes = lwip_recv(SocketAdminList[sockNr].ConnectionHandle, rxBuffer, payloadLength + 8, 0);
							createAndSendNack(sockNr, DOIP_E_UNKNOWN_PAYLOAD_TYPE);
							break;
						}
					}
				} else {
					createAndSendNack(sockNr, DOIP_E_MESSAGE_TO_LARGE);
					discardIpMessage(sockNr);
				}
			} else {
				createAndSendNack(sockNr, DOIP_E_INCORRECT_PATTERN_FORMAT);
				SoAd_SocketClose(sockNr);
			}
		}

		SoAd_BufferFree(rxBuffer);
	} else {
		// No rx buffer available. Report this in Det. Message should be handled in the next (scanSockets) loop.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_TCP_RX_ID, SOAD_E_NOBUFS);
	}
}


void DoIp_HandleUdpRx(uint16 sockNr)
{
	int nBytes;
	uint8* rxBuffer;
	uint16 payloadType;
	uint16 payloadLength;
	struct sockaddr_in fromAddr;
    socklen_t fromAddrLen = sizeof(fromAddr);

	if (SoAd_BufferGet(SOAD_RX_BUFFER_SIZE, &rxBuffer)) {
	    nBytes = lwip_recvfrom(SocketAdminList[sockNr].SocketHandle, rxBuffer, SOAD_RX_BUFFER_SIZE, MSG_PEEK, (struct sockaddr*)&fromAddr, &fromAddrLen);
		SoAd_SocketStatusCheck(sockNr, SocketAdminList[sockNr].SocketHandle);
		if (nBytes >= 8) {
			if ((rxBuffer[0] == DOIP_PROTOCOL_VERSION) && ((uint8)(~rxBuffer[1]) == DOIP_PROTOCOL_VERSION)) {
				payloadType = rxBuffer[2] << 8 | rxBuffer[3];
				payloadLength = (rxBuffer[4] << 24) | (rxBuffer[5] << 16) | (rxBuffer[6] << 8) | rxBuffer[7];
				if ((payloadLength + 8) <= SOAD_RX_BUFFER_SIZE) {
					if ((payloadLength + 8) <= nBytes) {
						// Grab the message
						nBytes = lwip_recvfrom(SocketAdminList[sockNr].SocketHandle, rxBuffer, payloadLength + 8, 0, (struct sockaddr*)&fromAddr, &fromAddrLen);
						SocketAdminList[sockNr].RemotePort = fromAddr.sin_port;
						SocketAdminList[sockNr].RemoteIpAddress = fromAddr.sin_addr.s_addr;
						switch (payloadType) {
						case 0x0001:	// Vehicle Identification Request
							handleVehicleIdentificationReq(sockNr, payloadLength, rxBuffer, SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_ALL);
							break;

						case 0x0002:	// Vehicle Identification Request with EID
							handleVehicleIdentificationReq(sockNr, payloadLength, rxBuffer, SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_BY_EID);
							break;

						case 0x0003:	// Vehicle Identification Request with VIN
							handleVehicleIdentificationReq(sockNr, payloadLength, rxBuffer, SOAD_ARC_DOIP_IDENTIFICATIONREQUEST_BY_VIN);
							break;

#if 0 /* Routing activation is not to be supported over UDP */
						case 0x005:		// Routing Activation request
							handleRoutingActivationReq(sockNr, payloadLength, rxBuffer);
							break;
#endif /* Routing activation is not to be supported over UDP */

						case 0x4001:    /* DoIP entity status request */
							handleEntityStatusReq(sockNr, payloadLength, rxBuffer);
							break;

						case 0x4003:    /* DoIP power mode check request */
							handlePowerModeCheckReq(sockNr, payloadLength, rxBuffer);
							break;


#if 0 /* Diagnostic messages is not to be supported over UDP */
						case 0x8001:	// Diagnostic message
							handleDiagnosticMessage(sockNr, payloadLength, rxBuffer);
							break;
#endif  /* Diagnostic messages is not to be supported over UDP */

						default:
							createAndSendNack(sockNr, DOIP_E_UNKNOWN_PAYLOAD_TYPE);
							break;
						}
					}
				} else {
					createAndSendNack(sockNr, DOIP_E_MESSAGE_TO_LARGE);
					discardIpMessage(sockNr);
				}
			} else {
				createAndSendNack(sockNr, DOIP_E_INCORRECT_PATTERN_FORMAT);
				SoAd_SocketClose(sockNr);
			}
		}

		SoAd_BufferFree(rxBuffer);
	} else {
		// No rx buffer available. Report this in Det. Message should be handled in the next (scanSockets) loop.
		DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_UDP_RX_ID, SOAD_E_NOBUFS);
	}
}


Std_ReturnType DoIp_HandleTpTransmit(PduIdType SoAdSrcPduId, const PduInfoType* SoAdSrcPduInfoPtr)
{
	Std_ReturnType returnCode = E_OK;
	BufReq_ReturnType PdurResult;
	PduInfoType* txPduInfoBuffer;
	uint16 socketNr;
	const uint32 pduLength = SoAdSrcPduInfoPtr->SduLength;
	const uint32 payloadLength = pduLength + 4;
	uint8* txBuffer;
	uint16 bytesSent;

	/*
	 * Find which target the incoming Pdu belongs to:
	 */
	uint16 i;
	uint16 targetIndex = 0xffff;
	for (i = 0; i < DOIP_TARGET_COUNT; i++) {
		if (SoAd_Config.DoIpTargetAddresses[i].rxPdu == SoAdSrcPduId) {
			// Match!
			targetIndex = i;
			break;
		}
	}

	if (targetIndex == 0xffff) {
		// Did not find corresponding target. Most likely due to faulty configuration.
		return E_NOT_OK;
	}


	if (PduAdminList[SoAdSrcPduId].PduStatus == PDU_IDLE ) {
		socketNr = SoAd_Config.PduRoute[SoAdSrcPduId].DestinationSocketRef->SocketId;
		if ((SocketAdminList[socketNr].SocketState == SOCKET_TCP_LISTENING)
			|| (SocketAdminList[socketNr].SocketState == SOCKET_TCP_READY)
			|| (SocketAdminList[socketNr].SocketState == SOCKET_UDP_READY))
		{
				PduAdminList[SoAdSrcPduId].PduStatus = PDU_TP_REQ_BUFFER;
				PdurResult = PduR_SoAdTpProvideTxBuffer(SoAd_Config.PduRoute[SoAdSrcPduId].SourcePduId, &txPduInfoBuffer, 0);
				if ((PdurResult == BUFREQ_OK) && (txPduInfoBuffer->SduLength >= pduLength)) {
					if (SoAd_BufferGet(8+13, &txBuffer)) {

						uint16 connectionId = targetConnectionMap[targetIndex];
						uint16 ta = connectionStatus[connectionId].sa; // Target of response is the source of the initiating party...
						uint16 sa = SoAd_Config.DoIpTargetAddresses[targetIndex].addressValue;


						PduAdminList[SoAdSrcPduId].PduStatus = PDU_TP_SENDING;
						txBuffer[0] = DOIP_PROTOCOL_VERSION;
						txBuffer[1] = ~DOIP_PROTOCOL_VERSION;
						txBuffer[2] = 0x80;	// 0x8001->Diagnostic message
						txBuffer[3] = 0x01;
						txBuffer[4] = payloadLength >> 24;
						txBuffer[5] = payloadLength >> 16;
						txBuffer[6] = payloadLength >> 8;
						txBuffer[7] = payloadLength >> 0;

						txBuffer[8+0] = sa >> 8;
						txBuffer[8+1] = sa >> 0;

						txBuffer[8+2] = ta >> 8;
						txBuffer[8+3] = ta >> 0;

						// Copy the Pdu to the tx buffer
						memcpy(&txBuffer[8+4], txPduInfoBuffer->SduDataPtr, pduLength);

						// Then send the diagnostic message and confirm transmission to PduR
						bytesSent = SoAd_SendIpMessage(socketNr, pduLength + 12, txBuffer);

						if (bytesSent == pduLength + 12) {
							SoAd_BufferFree(txBuffer);
							PduR_SoAdTpTxConfirmation(SoAd_Config.PduRoute[SoAdSrcPduId].SourcePduId, NTFRSLT_OK);
						} else {
							// All bytes were not sent - discarding this and setting a Det error instead.
							SoAd_BufferFree(txBuffer);
							DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_TP_TRANSMIT_ID, SOAD_E_UNEXPECTED_EXECUTION);

							// Notify PduR abotu failed transmission.
							PduR_SoAdTpTxConfirmation(SoAd_Config.PduRoute[SoAdSrcPduId].SourcePduId, NTFRSLT_E_NOT_OK);

						}
					} else {
						// No buffer to send with. Report failure back to PduR.
						DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_TP_TRANSMIT_ID, SOAD_E_NOBUFS);
						PduR_SoAdTpTxConfirmation(SoAd_Config.PduRoute[SoAdSrcPduId].SourcePduId, NTFRSLT_E_NO_BUFFER);
					}
				} else {
					// PduR did not return a buffer as expected. This signals something wrong with the PduR or its source module (probably CanTp)
					DET_REPORTERROR(MODULE_ID_SOAD, 0, SOAD_DOIP_HANDLE_TP_TRANSMIT_ID, SOAD_E_UNEXPECTED_EXECUTION);
				}
				PduAdminList[SoAdSrcPduId].PduStatus = PDU_IDLE;
		} else {
			/* Socket not ready */
			returnCode = E_NOT_OK;
		}
	} else {
		/* A PDU is already in progress */
		returnCode = E_NOT_OK;
	}

	return returnCode;
}


void DoIp_MainFunction() {

	uint16 i;
	static uint16 numAnnouncements = 0;

	if (DOIP_LINK_UP == LinkStatus) {
		DoIp_ArcAnnouncementTimer += DOIP_MAINFUNCTION_PERIOD_TIME;
		if ((DoIp_ArcAnnounceWait <= DoIp_ArcAnnouncementTimer) &&
			((DoIp_ArcAnnounceWait + SOAD_DOIP_ANNOUNCE_NUM * SOAD_DOIP_ANNOUNCE_INTERVAL) > DoIp_ArcAnnouncementTimer))
		{
			/* Within announcement period */

			uint32 timer = DoIp_ArcAnnouncementTimer - DoIp_ArcAnnounceWait;

			if (numAnnouncements * SOAD_DOIP_ANNOUNCE_INTERVAL <= timer) {
				/* Time to send another announcement */
				DoIp_SendVehicleAnnouncement(SOAD_DOIP_ANNOUNCE_SOCKET);
				numAnnouncements++;
			}

		} else {
			numAnnouncements = 0;
		}
	} else {
		numAnnouncements = 0;
	}


	/*
	 * Handle DoIP connection timeouts..
	 */
	for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {

		if (DOIP_ARC_CONNECTION_REGISTERED == connectionStatus[i].socketState) {

			/*
			 * Handle Alive check timeouts
			 */
			if (TRUE == connectionStatus[i].awaitingAliveCheckResponse) {
				if (connectionStatus[i].aliveCheckTimer > DOIP_ALIVECHECK_RESPONSE_TIMEOUT) {
					handleTimeout(i);

				} else {
					connectionStatus[i].aliveCheckTimer += DOIP_MAINFUNCTION_PERIOD_TIME;

				}
			}

			/*
			 * Handle general inactivity timeouts
			 */
			if ((connectionStatus[i].generalInactivityTimer <= DOIP_GENERAL_INACTIVITY_TIMEOUT) ||
					(connectionStatus[i].generalInactivityTimer <= DOIP_ALIVECHECK_RESPONSE_TIMEOUT))
			{
				connectionStatus[i].generalInactivityTimer += DOIP_MAINFUNCTION_PERIOD_TIME;

			} else {
				// Timeout!
				handleTimeout(i);

			}

			/*
			 * TODO: Handle initial inactivity timeouts
			 */
			if ((connectionStatus[i].initialInactivityTimer <= DOIP_GENERAL_INACTIVITY_TIMEOUT) ||
					(connectionStatus[i].initialInactivityTimer <= DOIP_ALIVECHECK_RESPONSE_TIMEOUT))
			{
				connectionStatus[i].initialInactivityTimer += DOIP_MAINFUNCTION_PERIOD_TIME;
			}
		}
	}
}

void DoIp_Init() {
	uint16 i;

	for (i = 0; i < DOIP_MAX_TESTER_CONNECTIONS; i++) {
		connectionStatus[i].socketState = DOIP_ARC_CONNECTION_INVALID;
		connectionStatus[i].sockNr = 0xff;
		connectionStatus[i].awaitingAliveCheckResponse = FALSE;
		connectionStatus[i].generalInactivityTimer = 0;
		connectionStatus[i].initialInactivityTimer = 0;
		connectionStatus[i].aliveCheckTimer = 0;
	}
}