C++ LE16函数代码示例

////TODO: Move away from rpc.c to some Windows-Linux-Incompatibility-Resolver.c
short utf8_to_ucs2_char (const unsigned char * input, const unsigned char ** end_ptr)
{
    *end_ptr = input;
    if (input[0] == 0)
        return -1;
    if (input[0] < 0x80) {
        *end_ptr = input + 1;
        return LE16(input[0]);
    }
    if ((input[0] & 0xE0) == 0xE0) {
        if (input[1] == 0 || input[2] == 0)
            return -1;
        *end_ptr = input + 3;
        return
            LE16((input[0] & 0x0F)<<12 |
            (input[1] & 0x3F)<<6  |
            (input[2] & 0x3F));
    }
    if ((input[0] & 0xC0) == 0xC0) {
        if (input[1] == 0)
            return -1;
        *end_ptr = input + 2;
        return
            LE16((input[0] & 0x1F)<<6  |
            (input[1] & 0x3F));
    }
    return -1;
}

void logResponseVerbose(const char *const ePID, const BYTE *const hwid, const RESPONSE *const response, const PRINTFUNC p)
{
	char guidBuffer[GUID_STRING_LENGTH + 1];
	//SYSTEMTIME st;

	p("Protocol version                : %u.%u\n", (uint32_t)LE16(response->MajorVer), (uint32_t)LE16(response->MinorVer));
	p("KMS host extended PID           : %s\n", ePID);
	if (LE16(response->MajorVer) > 5)
#	ifndef _WIN32
		p("KMS host Hardware ID            : %016llX\n", (unsigned long long)BE64(*(uint64_t*)hwid));
#	else // _WIN32
		p("KMS host Hardware ID            : %016I64X\n", (unsigned long long)BE64(*(uint64_t*)hwid));
#	endif // WIN32

	uuid2StringLE(&response->CMID, guidBuffer);
	p("Client machine ID               : %s\n", guidBuffer);

	char mbstr[64];
	time_t st;

	st = fileTimeToUnixTime(&response->ClientTime);
	strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %X", gmtime(&st));
	p("Client request timestamp (UTC)  : %s\n", mbstr);

	p("KMS host current active clients : %u\n", (uint32_t)LE32(response->Count));
	p("Renewal interval policy         : %u\n", (uint32_t)LE32(response->VLRenewalInterval));
	p("Activation interval policy      : %u\n", (uint32_t)LE32(response->VLActivationInterval));
}

static void CheckRpcRequest(const RPC_REQUEST *const Request, const unsigned int len, const uint_fast8_t _v)
{
	if (len >_Versions[_v].RequestSize + sizeof(RPC_REQUEST))
		logger("Warning: %u excess bytes in RPC request.\n",
				len - _Versions[_v].RequestSize + sizeof(RPC_REQUEST)
		);

	if (Request->ContextId)
		logger("Warning: Context id should be 0 but is %u.\n",
				(unsigned int)LE16(Request->ContextId)
		);

	if (Request->Opnum)
		logger("Warning: OpNum should be 0 but is %u.\n",
				(unsigned int)LE16(Request->Opnum)
		);

	if (LE32(Request->AllocHint) != len - sizeof(RPC_REQUEST) + sizeof(Request->Ndr))
		logger("Warning: Allocation hint should be %u but is %u.\n",
				len + sizeof(Request->Ndr),
				LE32(Request->AllocHint)
		);

	if (LE32(Request->Ndr.DataLength) != len - sizeof(RPC_REQUEST))
		logger("Warning: NDR32 data length field should be %u but is %u.\n",
				len - sizeof(RPC_REQUEST),
				LE32(Request->Ndr.DataLength)
		);

	if (LE32(Request->Ndr.DataSizeIs) != len - sizeof(RPC_REQUEST))
		logger("Warning: NDR32 data size field should be %u but is %u.\n",
				len - sizeof(RPC_REQUEST),
				LE32(Request->Ndr.DataSizeIs)
		);
}

static bool udi_cdc_comm_enable_common(uint8_t port)
{
	// Initialize control signal management
	udi_cdc_state[PORT] = CPU_TO_LE16(0);

	uid_cdc_state_msg[PORT].header.bmRequestType =
			USB_REQ_DIR_IN | USB_REQ_TYPE_CLASS |
			USB_REQ_RECIP_INTERFACE,
	uid_cdc_state_msg[PORT].header.bNotification = USB_REQ_CDC_NOTIFY_SERIAL_STATE,
	uid_cdc_state_msg[PORT].header.wValue = LE16(0),
	uid_cdc_state_msg[PORT].header.wIndex = LE16(UDI_CDC_COMM_IFACE_NUMBER),
	uid_cdc_state_msg[PORT].header.wLength = LE16(2),
	uid_cdc_state_msg[PORT].value = CPU_TO_LE16(0);

	udi_cdc_line_coding[PORT].dwDTERate = CPU_TO_LE32(UDI_CDC_DEFAULT_RATE);
	udi_cdc_line_coding[PORT].bCharFormat = UDI_CDC_DEFAULT_STOPBITS;
	udi_cdc_line_coding[PORT].bParityType = UDI_CDC_DEFAULT_PARITY;
	udi_cdc_line_coding[PORT].bDataBits = UDI_CDC_DEFAULT_DATABITS;
	// Call application callback
	// to initialize memories or indicate that interface is enabled
#if UDI_CDC_PORT_NB == 1
	UDI_CDC_SET_CODING_EXT((&udi_cdc_line_coding[0]));
	return UDI_CDC_ENABLE_EXT();
#else
	UDI_CDC_SET_CODING_EXT(port,(&udi_cdc_line_coding[port]));
	return UDI_CDC_ENABLE_EXT(port);
#endif
}

void RpcServer(const SOCKET sock, const DWORD RpcAssocGroup)
{
	RPC_HEADER  _Header;

	RandomNumberInit();

	while (_recv(sock, &_Header, sizeof(_Header)))
	{
		unsigned int  _st, request_len, response_len, _a;
		BYTE  *_Request /* =  NULL */; //uncomment to avoid false warnings when compiling with -Og

		#if defined(_PEDANTIC) && !defined(NO_LOG)
		CheckRpcHeader(&_Header, _Header.PacketType, &logger);
		#endif // defined(_PEDANTIC) && !defined(NO_LOG)

		switch (_Header.PacketType)
		{
			case RPC_PT_BIND_REQ: _a = 0; break;
			case RPC_PT_REQUEST:  _a = 1; break;
			default: return;
		}

		if ( (_st = ( (signed)( request_len = LE16(_Header.FragLength) - sizeof(_Header) )) > 0
					&& (_Request = (BYTE*)malloc(request_len) )))
		{
			BYTE *_Response /* = NULL */; //uncomment to avoid warnings when compiling with -Og

			if ((_st = (_recv(sock, _Request, request_len))
						&& ( response_len = _Actions[_a].GetResponseSize(_Request, request_len) )
						&& (_Response = (BYTE*)malloc( response_len += sizeof(_Header) ))))
			{
				if ( (_st = _Actions[_a].GetResponse(_Request, _Response + sizeof(_Header), RpcAssocGroup, sock, request_len)) )
				{
					RPC_HEADER *rh = (RPC_HEADER *)_Response;

					if (_Actions[_a].ResponsePacketType == RPC_PT_RESPONSE)
						response_len = LE32(((RPC_RESPONSE*)(_Response + sizeof(_Header)))->AllocHint) + 24;

					/* *((WORD*)rh)           = *((WORD*)&_Header);
					rh->PacketFlags        = RPC_PF_FIRST | RPC_PF_LAST;
					rh->DataRepresentation = _Header.DataRepresentation;
					rh->AuthLength         = _Header.AuthLength;
					rh->CallId             = _Header.CallId;*/
					memcpy(rh, &_Header, sizeof(RPC_HEADER));
					rh->PacketType = _Actions[_a].ResponsePacketType;
					rh->FragLength = LE16(response_len);

					_st = _send(sock, _Response, response_len);

					if (DisconnectImmediately && rh->PacketType == RPC_PT_RESPONSE)
						shutdown(sock, VLMCSD_SHUT_RDWR);
				}
				free(_Response);
			}
			free(_Request);
		}
		if (!_st) return;
	}
}

static void  USBH_ParseHIDDesc (USBH_HIDDesc_TypeDef *desc, uint8_t *buf)
{
	desc->bLength                  = *(uint8_t  *) (buf + 0);
	desc->bDescriptorType          = *(uint8_t  *) (buf + 1);
	desc->bcdHID                   =  LE16  (buf + 2);
	desc->bCountryCode             = *(uint8_t  *) (buf + 4);
	desc->bNumDescriptors          = *(uint8_t  *) (buf + 5);
	desc->bReportDescriptorType    = *(uint8_t  *) (buf + 6);
	desc->wItemLength              =  LE16  (buf + 7);
}

static int RpcBind(const RPC_BIND_REQUEST *const Request, RPC_BIND_RESPONSE *const Response, const DWORD RpcAssocGroup, const SOCKET sock, const unsigned int len)
{
	unsigned int  i, _st = 0;

	for (i = 0; i < LE32(Request->NumCtxItems); i++)
	{
		if ( IsEqualGUID((GUID*)TransferSyntaxNDR32, &Request->CtxItems[i].TransferSyntax) )
		{
			Response->Results[i].SyntaxVersion = LE32(2);
			Response->Results[i].AckResult =
			Response->Results[i].AckReason = 0;
			memcpy(&Response->Results[i].TransferSyntax, TransferSyntaxNDR32, sizeof(GUID));
			_st = !0;
		}
		else
		{
			Response->Results[i].SyntaxVersion = 0;
			Response->Results[i].AckResult =
			Response->Results[i].AckReason = LE16(2); // Unsupported
			memset(&Response->Results[i].TransferSyntax, 0, sizeof(GUID));
		}
	}

	if ( _st )
	{
		Response->MaxXmitFrag = Request->MaxXmitFrag;
		Response->MaxRecvFrag = Request->MaxRecvFrag;
		Response->AssocGroup  = LE32(RpcAssocGroup);

		socklen_t len;
		struct sockaddr_storage addr;

		// M$ RPC does not do this. Excess bytes contain apparently random data
		memset(Response->SecondaryAddress, 0, sizeof(Response->SecondaryAddress));

		len = sizeof addr;

		if (getsockname(sock, (struct sockaddr*)&addr, &len) ||
				getnameinfo((struct sockaddr*)&addr, len, NULL, 0, (char*)Response->SecondaryAddress, sizeof(Response->SecondaryAddress), NI_NUMERICSERV))
		{
			// In case of failure (should never happen) use default port (doesn't seem to break activation)
			strcpy((char*)Response->SecondaryAddress, "1688");
		}

		uint_fast8_t temp = strlen((char*)Response->SecondaryAddress) + 1;
		////FIXME: Temporary workaround for TCP ports < 10. sizeof(Response->SecondaryAddress) must be padded to 2, 6, 10, ...
		if (temp < 3) temp = 3;

		Response->SecondaryAddressLength = LE16(temp);
		Response->NumResults = Request->NumCtxItems;
	}

	return _st;
}

void Win32ResExtractor::fix_win32_image_section_header(Win32ImageSectionHeader *obj) {
	LE32(obj->misc.physical_address);
	LE32(obj->virtual_address);
	LE32(obj->size_of_raw_data);
	LE32(obj->pointer_to_raw_data);
	LE32(obj->pointer_to_relocations);
	LE32(obj->pointer_to_linenumbers);
	LE16(obj->number_of_relocations);
	LE16(obj->number_of_linenumbers);
	LE32(obj->characteristics);
}

/* read in the dir, look for the entry */
static int ext2_dir_lookup(ext2_t *ext2, struct ext2_inode *dir_inode, const char *name, inodenum_t *inum)
{
	uint file_blocknum;
	int err;
	uint8_t *buf;
	size_t namelen = strlen(name);
	
	if (!S_ISDIR(dir_inode->i_mode))
		return ERR_NOT_DIR;

	buf = malloc(EXT2_BLOCK_SIZE(ext2->sb));

	file_blocknum = 0;
	for (;;) {
		/* read in the offset */
		err = ext2_read_inode(ext2, dir_inode, buf, file_blocknum * EXT2_BLOCK_SIZE(ext2->sb), EXT2_BLOCK_SIZE(ext2->sb));
		if (err <= 0) {
			free(buf);
			return -1;
		}

		/* walk through the directory entries, looking for the one that matches */
		struct ext2_dir_entry_2 *ent;
		uint pos = 0;
		while (pos < EXT2_BLOCK_SIZE(ext2->sb)) {
			ent = (struct ext2_dir_entry_2 *)&buf[pos];

			LTRACEF("ent %d:%d: inode 0x%x, reclen %d, namelen %d\n",
					file_blocknum, pos, LE32(ent->inode), LE16(ent->rec_len), ent->name_len/* , ent->name*/);

			/* sanity check the record length */
			if (LE16(ent->rec_len) == 0)
				break;

			if (ent->name_len == namelen && memcmp(name, ent->name, ent->name_len) == 0) {
				// match
				*inum = LE32(ent->inode);
				LTRACEF("match: inode %d\n", *inum);
				free(buf);
				return 1;
			}

			pos += ROUNDUP(LE16(ent->rec_len), 4);
		}

		file_blocknum++;

		/* sanity check the directory. 4MB should be enough */
		if (file_blocknum > 1024) {
			free(buf);
			return -1;
		}
	}
}

bool udi_cdc_comm_enable(void)
{
	uint8_t port;
	uint8_t iface_comm_num;

#if UDI_CDC_PORT_NB == 1 // To optimize code
	port = 0;
	udi_cdc_nb_comm_enabled = 0;
#else
	if (udi_cdc_nb_comm_enabled > UDI_CDC_PORT_NB) {
		udi_cdc_nb_comm_enabled = 0;
	}
	port = udi_cdc_nb_comm_enabled;
#endif

	// Initialize control signal management
	udi_cdc_state[port] = CPU_TO_LE16(0);

	uid_cdc_state_msg[port].header.bmRequestType =
			USB_REQ_DIR_IN | USB_REQ_TYPE_CLASS |
			USB_REQ_RECIP_INTERFACE;
	uid_cdc_state_msg[port].header.bNotification = USB_REQ_CDC_NOTIFY_SERIAL_STATE;
	uid_cdc_state_msg[port].header.wValue = LE16(0);

	switch (port) {
#define UDI_CDC_PORT_TO_IFACE_COMM(index, unused) \
	case index: \
		iface_comm_num = UDI_CDC_COMM_IFACE_NUMBER_##index; \
		break;
	MREPEAT(UDI_CDC_PORT_NB, UDI_CDC_PORT_TO_IFACE_COMM, ~)
#undef UDI_CDC_PORT_TO_IFACE_COMM
	default:
		iface_comm_num = UDI_CDC_COMM_IFACE_NUMBER_0;
		break;
	}

	uid_cdc_state_msg[port].header.wIndex = LE16(iface_comm_num);
	uid_cdc_state_msg[port].header.wLength = LE16(2);
	uid_cdc_state_msg[port].value = CPU_TO_LE16(0);

	udi_cdc_line_coding[port].dwDTERate = CPU_TO_LE32(UDI_CDC_DEFAULT_RATE);
	udi_cdc_line_coding[port].bCharFormat = UDI_CDC_DEFAULT_STOPBITS;
	udi_cdc_line_coding[port].bParityType = UDI_CDC_DEFAULT_PARITY;
	udi_cdc_line_coding[port].bDataBits = UDI_CDC_DEFAULT_DATABITS;
	// Call application callback
	// to initialize memories or indicate that interface is enabled
	UDI_CDC_SET_CODING_EXT(port,(&udi_cdc_line_coding[port]));
	if (!UDI_CDC_ENABLE_EXT(port)) {
		return false;
	}
	udi_cdc_nb_comm_enabled++;
	return true;
}

static int copy_bb_qst_dat(const uint8_t *buf, uint8_t *rbuf, off_t sz,
                           uint32_t dsz) {
    const bb_quest_chunk_pkt *ck;
    uint32_t ptr = 176, optr = 0;
    char fn[32];
    char *cptr;
    uint32_t clen;

    while(ptr < sz) {
        ck = (const bb_quest_chunk_pkt *)(buf + ptr);

        /* Check the chunk for validity. */
        if(ck->hdr.pkt_type != LE16(QUEST_CHUNK_TYPE) ||
           ck->hdr.pkt_len != LE16(0x041C)) {
            debug(DBG_WARN, "Unknown or damaged quest chunk!\n");
            return -1;
        }

        /* Grab the vitals... */
        strncpy(fn, ck->filename, 16);
        fn[16] = 0;
        clen = LE32(ck->length);
        cptr = strrchr(fn, '.');

        /* Sanity check... */
        if(clen > 1024 || !cptr) {
            debug(DBG_WARN, "Damaged quest chunk!\n");
            return -1;
        }

        /* See if this is part of the .dat file */
        if(!strcmp(cptr, ".dat")) {
            if(optr + clen > dsz) {
                debug(DBG_WARN, "Quest file appears to be corrupted!\n");
                return -1;
            }

            memcpy(rbuf + optr, ck->data, clen);
            optr += clen;
        }

        ptr += 0x0420;
    }

    if(optr != dsz) {
        debug(DBG_WARN, "Quest file appears to be corrupted!\n");
        return -1;
    }

    return 0;
}

__interrupt
#endif
static void tc_irq(void)
{
    // Increment the ms seconds counter
    tc_tick++;

    // Clear the interrupt flag. This is a side effect of reading the TC SR.
    tc_read_sr(EXAMPLE_TC, EXAMPLE_TC_CHANNEL);

    // specify that an interrupt has been raised
    //update_timer = true;

    /*
    if ( tc_tick % 1000 == 0 ) {
    	printf( "Rawr %d\r\n", sysclk_get_pba_hz() );
    	tc_tick = 0;
    }
    */

    if ( tc_tick == IN_FORMAT_LSBYTE_SAMPLE_FREQ ) {
        //printf( "weee %u %u\r\n", tc_tick, udi_audio_is_active() );
        tc_tick = 0;
    }

    if ( udi_audio_is_active() ) {
        int16_t a = tc_tick; //ADC_ReadSampleAndSetNextAddr( 4 );

        /*
        samples[sampleIndex] = LE16( ADC_ReadSampleAndSetNextAddr( 2 ) );
        sampleIndex++;
        */

        for ( int j = 0; j < IN_FORMAT_NB_CHANNELS; j++ ) {
            //samples[sampleIndex] = LE16( ADC_ReadSampleAndSetNextAddr( (j+1) % IN_FORMAT_NB_CHANNELS ) );
            if ( latestSelector[j] ) {
                samples[sampleIndex] = LE16( latestSamples2[j] );
            } else {
                samples[sampleIndex] = LE16( latestSamples1[j] );
            }
            //samples[sampleIndex] = LE16( latestSamples[j] );
            sampleIndex++;

            if ( sampleIndex >= SAMPLES_PER_PACKET ) {
                udi_audio_send_samples( samples, (SAMPLE_SIZE * SAMPLES_PER_PACKET) );
                sampleIndex = 0;
            }
        }
    }
}

void uuid2StringLE(const GUID *const guid, char *const string)
{
	sprintf(string,
#		ifdef _WIN32
		"%08x-%04x-%04x-%04x-%012I64x",
#		else
		"%08x-%04x-%04x-%04x-%012llx",
#		endif
		(unsigned int)LE32(guid->Data1),
		(unsigned int)LE16(guid->Data2),
		(unsigned int)LE16(guid->Data3),
		(unsigned int)BE16(*(uint16_t*)guid->Data4),
		(unsigned long long)BE64(*(uint64_t*)(guid->Data4)) & 0xffffffffffffLL
	);
}

void logRequestVerbose(const REQUEST *const Request, const PRINTFUNC p)
{
	char guidBuffer[GUID_STRING_LENGTH + 1];
	char WorkstationBuffer[3 * WORKSTATION_NAME_BUFFER];
	const char *productName;
	ProdListIndex_t index;

	p("Protocol version                : %u.%u\n", LE16(Request->MajorVer), LE16(Request->MinorVer));
	p("Client is a virtual machine     : %s\n", LE32(Request->VMInfo) ? "Yes" : "No");
	p("Licensing status                : %u (%s)\n", (uint32_t)LE32(Request->LicenseStatus), LE32(Request->LicenseStatus) < _countof(LicenseStatusText) ? LicenseStatusText[LE32(Request->LicenseStatus)] : "Unknown");
	p("Remaining time (0 = forever)    : %i minutes\n", (uint32_t)LE32(Request->BindingExpiration));

	uuid2StringLE(&Request->AppID, guidBuffer);
	productName = getProductNameLE(&Request->AppID, AppList, getAppListSize(), &index);
	p("Application ID                  : %s (%s)\n", guidBuffer, productName);

	uuid2StringLE(&Request->ActID, guidBuffer);

#	ifndef NO_EXTENDED_PRODUCT_LIST
	productName = getProductNameLE(&Request->ActID, ExtendedProductList, getExtendedProductListSize(), &index);
#	else
	productName = "Unknown";
#	endif

	p("SKU ID (aka Activation ID)      : %s (%s)\n", guidBuffer, productName);

	uuid2StringLE(&Request->KMSID, guidBuffer);
	productName = getProductNameLE(&Request->KMSID, ProductList, getProductListSize(), &index);
	p("KMS ID (aka KMS counted ID)     : %s (%s)\n", guidBuffer, productName);

	uuid2StringLE(&Request->CMID, guidBuffer);
	p("Client machine ID               : %s\n", guidBuffer);

	uuid2StringLE(&Request->CMID_prev, guidBuffer);
	p("Previous client machine ID      : %s\n", guidBuffer);


	char mbstr[64];
	time_t st;
	st = fileTimeToUnixTime(&Request->ClientTime);
	strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %X", gmtime(&st));
	p("Client request timestamp (UTC)  : %s\n", mbstr);

	ucs2_to_utf8(Request->WorkstationName, WorkstationBuffer, WORKSTATION_NAME_BUFFER, sizeof(WorkstationBuffer));

	p("Workstation name                : %s\n", WorkstationBuffer);
	p("N count policy (minimum clients): %u\n", (uint32_t)LE32(Request->N_Policy));
}

static int RpcRequest(const RPC_REQUEST *const Request, RPC_RESPONSE *const Response, const DWORD RpcAssocGroup, const SOCKET sock, const unsigned int len)
{
	uint_fast16_t  _v;
	_v = LE16(((WORD*)Request->Data)[1]) - 4;

	int ResponseSize = _Versions[_v].CreateResponse(Request->Data, Response->Data);

	if ( ResponseSize )
	{
		Response->Ndr.DataSizeIs1 = LE32(0x00020000);
		Response->Ndr.DataLength  =
		Response->Ndr.DataSizeIs2 = LE32(ResponseSize);

		int len = ResponseSize + sizeof(Response->Ndr);

		BYTE* pRpcReturnCode = ((BYTE*)&Response->Ndr) + len;
		UA32(pRpcReturnCode) = 0; //LE16 not needed for 0
		len += sizeof(DWORD);

		// Pad zeros to 32-bit align (seems not neccassary but Windows RPC does it this way)
		int pad = ((~len & 3) + 1) & 3;
		memset(pRpcReturnCode + sizeof(DWORD), 0, pad);
		len += pad;

		Response->AllocHint = LE32(len);

		Response->AllocHint +=
				Response->ContextId = Request->ContextId;

		*((WORD*)&Response->CancelCount) = 0; // CancelCount + Pad1
	}

	return ResponseSize;
}