csu3_am335x/EVSE/GPL/plc-utils.LNX.2.1.20/source/plc/Identity2.c

/*====================================================================*
	Copyright (c) 2013,2021 Qualcomm Technologies, Inc.
	All Rights Reserved.
	Confidential and Proprietary - Qualcomm Technologies, Inc.
	******************************************************************
	2013 Qualcomm Atheros, Inc.
 *--------------------------------------------------------------------*/

#ifndef IDENTITY2_SOURCE
#define IDENTITY2_SOURCE

#include <stdint.h>
#include <unistd.h>
#include <memory.h>

#include "../tools/error.h"
#include "../tools/files.h"
#include "../tools/endian.h"
#include "../tools/memory.h"
#include "../tools/flags.h"
#include "../nvm/nvm.h"
#include "../plc/plc.h"

/*====================================================================*
 *
 *   signed pibchain2 (void const * memory);
 *
 *   search panther/lynx image chain for the next PIB image; print
 *   information on stdout and return;
 *
 *   Contributor(s):
 *	Charles Maier <cmaier@qca.qualcomm.com>
 *
 *--------------------------------------------------------------------*/

static signed pibchain2 (void const * memory, char const * filename, flag_t flags)

{
	struct panther_nvm_header * nvm_header;
	uint32_t origin = ~ 0;
	uint32_t offset = 0;
	signed module = 0;
	do 
	{
		nvm_header = (struct panther_nvm_header *) ((char *) (memory) +  offset);
		if (LE16TOH (nvm_header->MajorVersion) != 1)
		{
			if (_allclr (flags, NVM_SILENCE))
			{
				error (0, errno, NVM_HDR_VERSION, filename, module);
			}
			return (-1);
		}
		if (LE16TOH (nvm_header->MinorVersion) != 1)
		{
			if (_allclr (flags, NVM_SILENCE))
			{
				error (0, errno, NVM_HDR_VERSION, filename, module);
			}
			return (-1);
		}
		if (LE32TOH (nvm_header->PrevHeader) != origin)
		{
			if (_allclr (flags, NVM_SILENCE))
			{
				error (0, errno, NVM_HDR_LINK, filename, module);
			}
			return (-1);
		}
		if (checksum32 (nvm_header, sizeof (* nvm_header), 0))
		{
			error (0, 0, NVM_HDR_CHECKSUM, filename, module);
			return (-1);
		}
		origin = offset;
		offset += sizeof (* nvm_header);
		if (LE32TOH (nvm_header->ImageType) == NVM_IMAGE_PIB)
		{
			struct pib_header * pib_header = (struct pib_header *) ((char *) (memory) +  offset);
			pib_header->PIBLENGTH = HTOLE16 ((uint16_t) (LE32TOH (nvm_header->ImageLength)));
			panther_pib_peek ((char *) (memory) +  offset);
			pib_header->PIBLENGTH = 0;
			break;
		}
		if (checksum32 ((char *) (memory) +  offset, LE32TOH (nvm_header->ImageLength), nvm_header->ImageChecksum))
		{
			if (_allclr (flags, NVM_SILENCE))
			{
				error (0, errno, NVM_IMG_CHECKSUM, filename, module);
			}
			return (-1);
		}
		offset += LE32TOH (nvm_header->ImageLength);
		module++;
	}
	while (~ nvm_header->NextHeader);
	return (0);
}

static signed pibchain2_no_pib_peek(void const* memory, char const* filename, flag_t flags, uint32_t* pib_offset)

{
	struct panther_nvm_header* nvm_header;
	uint32_t origin = ~0;
	uint32_t offset = 0;
	signed module = 0;
	do
	{
		nvm_header = (struct panther_nvm_header*)((char*)(memory)+offset);
		if (LE16TOH(nvm_header->MajorVersion) != 1)
		{
			if (_allclr(flags, NVM_SILENCE))
			{
				error(0, errno, NVM_HDR_VERSION, filename, module);
			}
			return (-1);
		}
		if (LE16TOH(nvm_header->MinorVersion) != 1)
		{
			if (_allclr(flags, NVM_SILENCE))
			{
				error(0, errno, NVM_HDR_VERSION, filename, module);
			}
			return (-1);
		}
		if (LE32TOH(nvm_header->PrevHeader) != origin)
		{
			if (_allclr(flags, NVM_SILENCE))
			{
				error(0, errno, NVM_HDR_LINK, filename, module);
			}
			return (-1);
		}
		if (checksum32(nvm_header, sizeof(*nvm_header), 0))
		{
			error(0, 0, NVM_HDR_CHECKSUM, filename, module);
			return (-1);
		}
		origin = offset;
		offset += sizeof(*nvm_header);
		if (LE32TOH(nvm_header->ImageType) == NVM_IMAGE_PIB)
		{
			struct pib_header* pib_header = (struct pib_header*)((char*)(memory)+offset);
			pib_header->PIBLENGTH = HTOLE16((uint16_t)(LE32TOH(nvm_header->ImageLength)));
			pib_header->PIBLENGTH = 0;
			*pib_offset = offset;
			break;
		}
		if (checksum32((char*)(memory)+offset, LE32TOH(nvm_header->ImageLength), nvm_header->ImageChecksum))
		{
			if (_allclr(flags, NVM_SILENCE))
			{
				error(0, errno, NVM_IMG_CHECKSUM, filename, module);
			}
			return (-1);
		}
		offset += LE32TOH(nvm_header->ImageLength);
		module++;
	} 	while (~nvm_header->NextHeader);
	return (0);
}

/*====================================================================*
 *
 *   signed Identity2 (struct plc * plc);
 *
 *   plc.h
 *
 *   read start of parameter chain from flash memory using single
 *   VS_MODULE_OPERATION message and print identity information on
 *   stdout;
 *
 *   Contributor(s):
 *      Charles Maier <cmaier@qca.qualcomm.com>
 *
 *--------------------------------------------------------------------*/

signed Identity2 (struct plc * plc)

{
	struct channel * channel = (struct channel *) (plc->channel);
	struct message * message = (struct message *) (plc->message);

#ifndef __GNUC__
#pragma pack (push,1)
#endif

	struct __packed vs_module_operation_read_request
	{
		struct ethernet_hdr ethernet;
		struct qualcomm_hdr qualcomm;
		uint32_t RESERVED;
		uint8_t NUM_OP_DATA;
		struct __packed
		{
			uint16_t MOD_OP;
			uint16_t MOD_OP_DATA_LEN;
			uint32_t MOD_OP_RSVD;
			uint16_t MODULE_ID;
			uint16_t MODULE_SUB_ID;
			uint16_t MODULE_LENGTH;
			uint32_t MODULE_OFFSET;
		}
		MODULE_SPEC;
	}
	* request = (struct vs_module_operation_read_request *) (message);
	struct __packed vs_module_operation_read_confirm
	{
		struct ethernet_hdr ethernet;
		struct qualcomm_hdr qualcomm;
		uint16_t MSTATUS;
		uint16_t ERR_REC_CODE;
		uint32_t RESERVED;
		uint8_t NUM_OP_DATA;
		struct __packed
		{
			uint16_t MOD_OP;
			uint16_t MOD_OP_DATA_LEN;
			uint32_t MOD_OP_RSVD;
			uint16_t MODULE_ID;
			uint16_t MODULE_SUB_ID;
			uint16_t MODULE_LENGTH;
			uint32_t MODULE_OFFSET;
		}
		MODULE_SPEC;
		uint8_t MODULE_DATA [PLC_MODULE_SIZE];
	}
	* confirm = (struct vs_module_operation_read_confirm *) (message);

#ifndef __GNUC__
#pragma pack (pop)
#endif

	Request (plc, "Device Identity");
	memset (message, 0, sizeof (* message));
	EthernetHeader (& request->ethernet, channel->peer, channel->host, channel->type);
	QualcommHeader (& request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
	plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
	request->NUM_OP_DATA = 1;
	request->MODULE_SPEC.MOD_OP = HTOLE16 (0);
	request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16 (sizeof (request->MODULE_SPEC));
	request->MODULE_SPEC.MOD_OP_RSVD = HTOLE32 (0);
	request->MODULE_SPEC.MODULE_ID = HTOLE16 (PLC_MODULEID_PARAMETERS);
	request->MODULE_SPEC.MODULE_SUB_ID = HTOLE16 (0);
	request->MODULE_SPEC.MODULE_LENGTH = HTOLE16 (PLC_MODULE_SIZE);
	request->MODULE_SPEC.MODULE_OFFSET = HTOLE32 (0);
	if (SendMME (plc) <= 0)
	{
		error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
		return (-1);
	}
	while (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) > 0)
	{
		if (confirm->MSTATUS)
		{
			Failure (plc, PLC_WONTDOIT);
			continue;
		}
		Confirm (plc, "-------");
		pibchain2 (confirm->MODULE_DATA, "device", plc->flags);
	}
	return (0);
}

signed get_nid_nmk_req(struct plc* plc)

{
	struct channel* channel = (struct channel*)(plc->channel);
	struct message* message = (struct message*)(plc->message);

#ifndef __GNUC__
#pragma pack (push,1)
#endif

	struct __packed vs_module_operation_read_request
	{
		struct ethernet_hdr ethernet;
		struct qualcomm_hdr qualcomm;
		uint32_t RESERVED;
		uint8_t NUM_OP_DATA;
		struct __packed
		{
			uint16_t MOD_OP;
			uint16_t MOD_OP_DATA_LEN;
			uint32_t MOD_OP_RSVD;
			uint16_t MODULE_ID;
			uint16_t MODULE_SUB_ID;
			uint16_t MODULE_LENGTH;
			uint32_t MODULE_OFFSET;
		}
		MODULE_SPEC;
	}
	*request = (struct vs_module_operation_read_request*)(message);
	
#ifndef __GNUC__
#pragma pack (pop)
#endif

	debug(0, __func__, "Send read request to get NMK NID from pev");
	memset(message, 0, sizeof(*message));
	EthernetHeader(&request->ethernet, channel->peer, channel->host, channel->type);
	QualcommHeader(&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
	plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
	request->NUM_OP_DATA = 1;
	request->MODULE_SPEC.MOD_OP = HTOLE16(0);
	request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16(sizeof(request->MODULE_SPEC));
	request->MODULE_SPEC.MOD_OP_RSVD = HTOLE32(0);
	request->MODULE_SPEC.MODULE_ID = HTOLE16(PLC_MODULEID_PARAMETERS);
	request->MODULE_SPEC.MODULE_SUB_ID = HTOLE16(0);
	request->MODULE_SPEC.MODULE_LENGTH = HTOLE16(PLC_MODULE_SIZE);
	request->MODULE_SPEC.MODULE_OFFSET = HTOLE32(0);
	if (SendMME(plc) <= 0)
	{
		error(PLC_EXIT(plc), errno, CHANNEL_CANTSEND);
		return (-1);
	}
	return (0);
}

signed process_get_nid_cnf(struct plc* plc, void* NID, void* NMK)
{
	struct message* message = (struct message*)(plc->message);

#ifndef __GNUC__
#pragma pack (push,1)
#endif

	struct __packed vs_module_operation_read_confirm
	{
		struct ethernet_hdr ethernet;
		struct qualcomm_hdr qualcomm;
		uint16_t MSTATUS;
		uint16_t ERR_REC_CODE;
		uint32_t RESERVED;
		uint8_t NUM_OP_DATA;
		struct __packed
		{
			uint16_t MOD_OP;
			uint16_t MOD_OP_DATA_LEN;
			uint32_t MOD_OP_RSVD;
			uint16_t MODULE_ID;
			uint16_t MODULE_SUB_ID;
			uint16_t MODULE_LENGTH;
			uint32_t MODULE_OFFSET;
		}
		MODULE_SPEC;
		uint8_t MODULE_DATA[PLC_MODULE_SIZE];
	}
	*confirm = (struct vs_module_operation_read_confirm*)(message);

#ifndef __GNUC__
#pragma pack (pop)
#endif

	if (confirm->MSTATUS)
	{
		Failure(plc, PLC_WONTDOIT);
		return(-1);
	}
	debug(0, __func__, "NMK NID received from pev");
	uint32_t pib_data_offset = 0;
	if (!pibchain2_no_pib_peek(confirm->MODULE_DATA, "device", plc->flags, &pib_data_offset))
	{
		const char* memory = (const char*)(confirm->MODULE_DATA + pib_data_offset);
		struct PIB3_0* PIB = (struct PIB3_0*)(memory);
		char buffer[100];
		if (_anyset(plc->flags, PLC_VERBOSE))
		{
			printf("pib offset %0x", pib_data_offset);
			printf("NMK %s\n", hexstring(buffer, sizeof(buffer), PIB->LocalDeviceConfig.NMK, sizeof(PIB->LocalDeviceConfig.NMK)));
			printf("NID %s\n", hexstring(buffer, sizeof(buffer), PIB->LocalDeviceConfig.PreferredNID, sizeof(PIB->LocalDeviceConfig.PreferredNID)));
		}
		memcpy((byte*)NMK, PIB->LocalDeviceConfig.NMK, sizeof(PIB->LocalDeviceConfig.NMK));
		memcpy((byte*)NID, PIB->LocalDeviceConfig.PreferredNID, sizeof(PIB->LocalDeviceConfig.PreferredNID));
	}
	return 0;
}

#endif