C++ TUTRACE函数代码示例

static uint32
wps_sendMsg(void *mcdev, TRANSPORT_TYPE trType, char * dataBuffer, uint32 dataLen)
{
	uint32 retVal = WPS_SUCCESS;

	TUTRACE((TUTRACE_INFO, "In wps_sendMsg\n"));

	if (trType < 1 || trType >= TRANSPORT_TYPE_MAX) {
		TUTRACE((TUTRACE_ERR, "Transport Type is not within the "
			"accepted range\n"));
		return WPS_ERR_INVALID_PARAMETERS;
	}

	switch (trType) {
	case TRANSPORT_TYPE_EAP:
		retVal = ap_eap_sm_sendMsg(dataBuffer, dataLen);
		break;
#ifdef WPS_UPNP_DEVICE
	case TRANSPORT_TYPE_UPNP_DEV:
		retVal = ap_upnp_sm_sendMsg(dataBuffer, dataLen);
		break;
#endif /* WPS_UPNP_DEVICE */
	default:
		break;
	}

	if (retVal != WPS_SUCCESS) {
		TUTRACE((TUTRACE_ERR,  "WriteData for "
			"trType %d failed.\n", trType));
	}

	return retVal;
}

int
wps_osl_init(char *bssid)
{
	int eap_fd = wps_eap_get_handle();
	int ret;
	struct ifreq  ifr;

	if (!if_name[0]) {
		TUTRACE((TUTRACE_ERR, "Wireless Interface not specified.\n"));
		return WPS_ERR_SYSTEM;
	}

	/* Check interface address */
	memset(&ifr, 0, sizeof(ifr));
	strncpy(ifr.ifr_name, if_name, sizeof(ifr.ifr_name));
	if ((ret = ioctl(eap_fd, SIOCGIFHWADDR, &ifr)) < 0) {
		TUTRACE((TUTRACE_ERR, "Get interface mac failed\n"));
		return WPS_ERR_SYSTEM;
	}
	/* Copy the result back */
	memcpy(my_mac, ifr.ifr_hwaddr.sa_data, 6);
	TUTRACE((TUTRACE_INFO, "set my_mac %02x:%02x:%02x:%02x:%02x:%02x\n", my_mac[0], my_mac[1],
		my_mac[2], my_mac[3], my_mac[4], my_mac[5]));

	/* record destination bssid */
	if (bssid) {
		memcpy(peer_mac, bssid, 6);
		TUTRACE((TUTRACE_INFO, "set peer_mac %02x:%02x:%02x:%02x:%02x:%02x\n", peer_mac[0],
		peer_mac[1], peer_mac[2], peer_mac[3], peer_mac[4], peer_mac[5]));
	}

	return WPS_SUCCESS;
}

uint32
wps_enrUpnpGetDeviceInfoCheck(EnrSM *e, void *inbuffer, uint32 in_len,
	void *outbuffer, uint32 *out_len)
{
	if (!inbuffer || !in_len) {
		if (false == e->reg_info->initialized) {
			TUTRACE((TUTRACE_ERR, "ENRSM: Not yet initialized.\n"));
			return WPS_ERR_NOT_INITIALIZED;
		}

		if (START != e->reg_info->e_smState) {
			TUTRACE((TUTRACE_INFO, "\n======e_lastMsgSent != M1, "
				"Step GetDeviceInfo=%d ======\n",
				wps_getUpnpDevGetDeviceInfo(e->g_mc)));
			if (wps_getUpnpDevGetDeviceInfo(e->g_mc)) {
				wps_setUpnpDevGetDeviceInfo(e->g_mc, false);

				/* copy to msg_to_send buffer */
				if (*out_len < e->reg_info->outMsg->m_dataLength) {
					e->reg_info->e_smState = FAILURE;
					TUTRACE((TUTRACE_ERR, "output message buffer to small\n"));
					return WPS_MESSAGE_PROCESSING_ERROR;
				}
				memcpy(outbuffer, (char *)e->reg_info->outMsg->pBase,
					e->reg_info->outMsg->m_dataLength);
				*out_len = e->reg_info->outMsg->m_dataLength;
				return WPS_SEND_MSG_CONT;
			}
			return WPS_SUCCESS;
		}
	}

	return WPS_CONT;
}

int
ap_eap_sm_startWPSReg(unsigned char *sta_mac, unsigned char *ap_mac)
{
	unsigned char *mac;
	int retVal;

	mac = lookupSta(sta_mac, SEARCH_ENTER);

	if (!mac) {
		TUTRACE((TUTRACE_ERR,  "no sta...\n"));
		return -1;
	}

	TUTRACE((TUTRACE_ERR,  "Build WPS Start!\n"));

	/* reset counter  */
	apEapState->eap_id = 1;

	/* store whcih if sta come from */
	memcpy(&apEapState->bssid, ap_mac, ETHER_ADDR_LEN);

	/* Request Start message */
	retVal = ap_eap_sm_req_start();

	return retVal;
}

uint32
ap_eap_sm_init(void *mc_dev, char *mac_sta, char * (*parse_msg)(char *, int, int *),
	unsigned int (*send_data)(char *, uint32), int eap_frag_threshold)
{
	TUTRACE((TUTRACE_INFO,  "Initial...\n"));

	memset(&s_apEapState, 0, sizeof(EAP_WPS_AP_STATE));
	apEapState = &s_apEapState;
	apEapState->mc_dev = mc_dev;
	memcpy(apEapState->sta_mac, mac_sta, ETHER_ADDR_LEN);
	if (parse_msg)
		apEapState->parse_msg = parse_msg;
	if (send_data)
		apEapState->send_data = send_data;

	/* For fragmentation */
	apEapState->next_frag_to_send = apEapState->msg_to_send;

	/* WSC 2.0 */
	apEap_wps_version2 = wps_get_version2(mc_dev);
	apEapState->eap_frag_threshold = EAP_WPS_FRAG_MAX;

	if (eap_frag_threshold >= 100 && eap_frag_threshold < EAP_WPS_FRAG_MAX)
		apEapState->eap_frag_threshold = eap_frag_threshold;


	TUTRACE((TUTRACE_INFO,  "ap_eap_sm_init: EAP Frag Threshold %d\n",
		apEapState->eap_frag_threshold));

	return WPS_SUCCESS;
}

/* Send EAP fragment ACK */
static int
ap_eap_sm_sendFACK()
{
	WpsEapHdr wpsEapHdr;

	/* check sta status */
	if (!WPS_STA_ACTIVE()) {
		TUTRACE((TUTRACE_ERR,  "sta not in use!\n"));
		return WPS_ERROR_MSG_TIMEOUT;
	}

	TUTRACE((TUTRACE_INFO, "Build EAP fragment ACK\n"));

	wpsEapHdr.code = EAP_CODE_REQUEST;
	wpsEapHdr.id = apEapState->eap_id;
	wpsEapHdr.length = WpsHtons(sizeof(WpsEapHdr));
	wpsEapHdr.type = EAP_TYPE_WPS;
	wpsEapHdr.vendorId[0] = WPS_VENDORID1;
	wpsEapHdr.vendorId[1] = WPS_VENDORID2;
	wpsEapHdr.vendorId[2] = WPS_VENDORID3;
	wpsEapHdr.vendorType = WpsHtonl(WPS_VENDORTYPE);
	wpsEapHdr.opcode = WPS_FRAG_ACK;
	wpsEapHdr.flags = 0;

	ap_eap_sm_sendEapol(apEapState->msg_to_send, (char *)&wpsEapHdr, sizeof(WpsEapHdr));

	return WPS_SUCCESS;
}

uint32
ap_eap_sm_sendMsg(char *dataBuffer, uint32 dataLen)
{
	uint32 retVal;
	eapol_header_t *eapolHdr = (eapol_header_t *)apEapState->msg_to_send;
	WpsEapHdr *wpsEapHdr;

	TUTRACE((TUTRACE_INFO, "In ap_eap_sm_sendMsg buffer Length = %d\n",
		dataLen));

	retVal = ap_eap_sm_create_pkt(dataBuffer, dataLen, EAP_CODE_REQUEST);
	if (retVal == WPS_SUCCESS) {
		/* Shift EAP_WPS_LF_OFFSET */
		if (dataLen < apEapState->eap_frag_threshold)
			eapolHdr = (eapol_header_t *)&apEapState->msg_to_send[EAP_WPS_LF_OFFSET];

		wpsEapHdr = (WpsEapHdr *)eapolHdr->body;
		ap_eap_sm_sendEap((char *)wpsEapHdr, WpsNtohs((uint8*)&wpsEapHdr->length));
		apEapState->state = PROCESSING_PROTOCOL;
	}
	else {
		TUTRACE((TUTRACE_ERR,  "Send EAP FAILURE to station!\n"));
		ap_eap_sm_Failure(apEap_wps_version2 ? 1 : 0);

		retVal = TREAP_ERR_SENDRECV;
	}

	return retVal;
}

bool CRegProtocol::ValidateMac(BufferObj &data, uint8 *hmac, BufferObj &key)
{
    uint8 dataMac[BUF_SIZE_256_BITS];

    //First calculate the hmac of the data
    if(HMAC(EVP_sha256(), key.GetBuf(), SIZE_256_BITS, data.GetBuf(), 
            data.Length(), dataMac, NULL) == NULL)
    {
        TUTRACE((TUTRACE_ERR, "RPROTO: HMAC failed\n"));
		if (TUTRACELEVEL & TUVERBOSE && g_verbose)
		{
		// for debugging purposes
			printf("Computed HMAC (note: only first 64 bits will be compared)\n");
			for (int i = 0; i < BUF_SIZE_256_BITS; i++) {
				printf("%2x ",dataMac[i]);
			}
			printf("\nHMAC from Authenticator(should match first 64 bits of computed HMAC)\n");
			for (int i = 0; i < 8; i++) {
				printf("%2x ",hmac[i]);
			}
			printf("\nKey is:\n");
			for (int i = 0; i < 8; i++) {
				printf("%2x ",(key.GetBuf())[i]);
			}
			printf("\n");
		}
        throw RPROT_ERR_CRYPTO;
	} else { 
		if (TUTRACELEVEL & TUVERBOSE && g_verbose)
		{
		// for debugging purposes
			printf("Computed HMAC (note: only first 64 bits will be compared)\n");
			for (int i = 0; i < BUF_SIZE_256_BITS; i++) {
				printf("%2x ",dataMac[i]);
			}
			printf("\nHMAC from Authenticator(should match first 64 bits of computed HMAC)\n");
			for (int i = 0; i < 8; i++) {
				printf("%2x ",hmac[i]);
			}
			printf("\nKey is:\n");
			for (int i = 0; i < BUF_SIZE_256_BITS; i++) {
				printf("%2x ",(key.GetBuf())[i]);
			}
			printf("\n");
		}
	}

    //next, compare it against the received hmac
    TUTRACE((TUTRACE_INFO, "RPROTO: Verifying the first 64 bits of the generated HMAC\n"));

    if(memcmp(dataMac, hmac, SIZE_64_BITS) != 0)
    {
        printf("RPROTO: HMAC results don't match\n");
        return false;
    }
    TUTRACE((TUTRACE_VERBOSE, "RPROTO: HMAC results match\n"));
    
    return true;
}

/*
 * Receive a fragmentation then send EAP_FRAG_ACK.
 */
static int
ap_eap_sm_process_frag(char *eap_msg)
{
	int ret;
	int LF_bytes = 0;
	WpsEapHdr *wpsEapHdr = (WpsEapHdr *)eap_msg;
	uint32 wps_len = WpsNtohs((uint8*)&wpsEapHdr->length) - sizeof(WpsEapHdr);
	int wps_data_received = apEapState->total_received - sizeof(WpsEapHdr);


	TUTRACE((TUTRACE_INFO, "Receive a EAP WPS fragment packet\n"));

	/* Total length checking */
	if (apEapState->total_bytes_to_recv < wps_data_received + wps_len) {
		TUTRACE((TUTRACE_ERR, "Received WPS data len %d excess total bytes to "
			"receive %d\n", wps_data_received + wps_len,
			apEapState->total_bytes_to_recv));
		return EAP_FAILURE;
	}

	/* Copy to frags_received */
	/* First fragmentation include WpsEapHdr without length field */
	if (apEapState->total_received == 0) {
		memcpy(apEapState->frags_received, eap_msg, sizeof(WpsEapHdr));
		apEapState->total_received += sizeof(WpsEapHdr);

		/* Ignore length field 2 bytes copy */
		LF_bytes = EAP_WPS_LF_OFFSET;
	}
	/* WPS data */
	memcpy(&apEapState->frags_received[apEapState->total_received],
		eap_msg + sizeof(WpsEapHdr) + LF_bytes,	wps_len - LF_bytes);
	apEapState->total_received += wps_len - LF_bytes;

	/* Is last framentation? */
	if (wpsEapHdr->flags & EAP_WPS_FLAGS_MF) {
		/* Increase eap_id */
		apEapState->eap_id++;

		/* Send WPS_FRAG_ACK */
		ret = ap_eap_sm_sendFACK();
		if (ret == WPS_SUCCESS)
			return WPS_CONT;

		return ret;
	}

	/* Got all fragmentations, adjust WpsEapHdr */
	wpsEapHdr = (WpsEapHdr *)apEapState->frags_received;
	wpsEapHdr->length = WpsHtons((uint16)apEapState->total_received);

	TUTRACE((TUTRACE_ERR, "Received all WPS fragmentations, total bytes of WPS data "
		"to receive %d, received %d\n", apEapState->total_bytes_to_recv,
		apEapState->total_received - sizeof(WpsEapHdr)));

	return WPS_SUCCESS;
}

uint32 CRegProtocol::CreatePrivateKey(char *name, 
                                      EVP_PKEY **key)
{
    TU_RET err = TU_ERROR_CRYPTO_FAILED;
    RSA *rsaKey;
    EVP_PKEY *pkey;
    FILE *fp;

    rsaKey = RSA_generate_key(1024, 65537, NULL, NULL);
    if(rsaKey == NULL)
    {
        TUTRACE((TUTRACE_ERR, "Couldn't generate RSA key\n"));
        goto EXIT;
    }

    //Now store it in a PKEY
    pkey = EVP_PKEY_new();
    if(!pkey)
    {
        TUTRACE((TUTRACE_ERR, "Couldn't generate new EVP key\n"));
        goto EXIT;
    }

    if(!EVP_PKEY_assign_RSA(pkey, rsaKey))
    {
        TUTRACE((TUTRACE_ERR, "Couldn't assign RSA key to EVP key\n"));
        RSA_free(rsaKey);
        goto EXIT;
    }

    fp = fopen(name, "w");
    
    if(!PEM_write_PKCS8PrivateKey(fp, pkey, NULL, NULL, 0, NULL, NULL))
    {
        TUTRACE((TUTRACE_ERR, "Error writing Signing key to file\n"));
        fclose(fp);
        goto ERR_EVP;
    }
    
    fclose(fp);

    if(key)
        *key = pkey;
    else
        EVP_PKEY_free(pkey);

    return TU_SUCCESS;

ERR_EVP:
    EVP_PKEY_free(pkey);
EXIT:
    return err;
}

void *
wps_init(void *bcmwps, DevInfo *ap_devinfo)
{
	WPSAPI_T *gp_mc;
	DevInfo *dev_info;

	gp_mc = (WPSAPI_T *)alloc_init(sizeof(*gp_mc));
	if (!gp_mc) {
		TUTRACE((TUTRACE_INFO, "wps_init::malloc failed!\n"));
		return 0;
	}

	gp_mc->dev_info = devinfo_new();
	if (gp_mc->dev_info == NULL)
		goto error;

	/* copy user provided DevInfo to mp_deviceInfo */
	dev_info = gp_mc->dev_info;
	memcpy(dev_info, ap_devinfo, sizeof(DevInfo));

	/* copy prebuild enrollee noce and private key */
	if (dev_info->flags & DEVINFO_FLAG_PRE_PRIV_KEY) {
		if (reg_proto_generate_prebuild_dhkeypair(
			&dev_info->DHSecret, dev_info->pre_privkey) != WPS_SUCCESS) {
			TUTRACE((TUTRACE_ERR, "wps_init::prebuild_dhkeypair failed!\n"));
			goto error;
		}
	}
	else {
		if (reg_proto_generate_dhkeypair(&dev_info->DHSecret) != WPS_SUCCESS) {
			TUTRACE((TUTRACE_ERR, "wps_init::gen dhkeypair failed!\n"));
			goto error;
		}
	}

	gp_mc->mb_initialized = true;
	TUTRACE((TUTRACE_INFO, "wps_init::Done!\n"));

	/* Everything's initialized ok */
	gp_mc->bcmwps = bcmwps;

	return (void *)gp_mc;

error:
	TUTRACE((TUTRACE_ERR, "wps_init::Init failed\n"));
	if (gp_mc) {
		wps_deinit(gp_mc);
	}

	return 0;
}

/*
 * Search for or create a STA.
 * We can only handle only ONE station at a time.
 * If `enter' is not set, do not create it when one is not found.
 */
static unsigned char*
lookupSta(unsigned char *sta_mac, sta_lookup_mode_t mode)
{
	unsigned char *mac = NULL;
	int sta_active = WPS_STA_ACTIVE();

	/* Search if the entry in on the list */
	if (sta_active &&
		(!memcmp(&apEapState->sta_mac, sta_mac, ETHER_ADDR_LEN))) {
		mac = apEapState->sta_mac;
	}

	/* create a new entry if necessary */
	if (!mac && mode == SEARCH_ENTER) {
		if (!sta_active) {
			/* Initialize entry */
			memcpy(&apEapState->sta_mac, sta_mac, ETHER_ADDR_LEN);
			/* Initial STA state: */
			apEapState->state = INIT;
			apEapState->eap_id = 0;
			mac = apEapState->sta_mac;
		}
		else
			TUTRACE((TUTRACE_ERR,  "Sta in use\n"));
	}

	return mac;
}

uint32
ap_eap_sm_sendWPSStart()
{
	WpsEapHdr wpsEapHdr;

	/* check sta status */
	if (!WPS_STA_ACTIVE()) {
		TUTRACE((TUTRACE_ERR,  "sta not in use!\n"));
		return WPS_ERROR_MSG_TIMEOUT;
	}

	wpsEapHdr.code = EAP_CODE_REQUEST;
	wpsEapHdr.id = apEapState->eap_id;
	wpsEapHdr.length = WpsHtons(sizeof(WpsEapHdr));
	wpsEapHdr.type = EAP_TYPE_WPS;
	wpsEapHdr.vendorId[0] = WPS_VENDORID1;
	wpsEapHdr.vendorId[1] = WPS_VENDORID2;
	wpsEapHdr.vendorId[2] = WPS_VENDORID3;
	wpsEapHdr.vendorType = WpsHtonl(WPS_VENDORTYPE);
	wpsEapHdr.opcode = WPS_Start;
	wpsEapHdr.flags = 0;

	ap_eap_sm_sendEapol(apEapState->msg_to_send, (char *)&wpsEapHdr, sizeof(WpsEapHdr));
	apEapState->state = EAP_START_SEND;
	apEapState->sent_msg_id = WPS_PRIVATE_ID_WPS_START;

	return WPS_SUCCESS;
}

/* Add for PF3 */
static uint32
wps_regUpnpERFilter(RegSM *r, BufferObj *msg, uint32 msgType)
{
	uint32 err;


	/*
	 * If AP have received M2 form one External Registrar,
	 * AP must ignore forwarding messages from other
	 * External Registrars.
	 */
	if (r->m_er_sentM2 == false) {
		if (msgType == WPS_ID_MESSAGE_M2) {
			/* Save R-Nonce */
			err = reg_proto_get_nonce(r->m_er_nonce, msg, WPS_ID_REGISTRAR_NONCE);
			if (err != WPS_SUCCESS) {
				TUTRACE((TUTRACE_ERR, "ENRSM: Get R-Nonce error: %d\n", err));
			}
			else {
				r->m_er_sentM2 = true;
			}
		}
	}
	else {
		/* Filter UPnP to EAP messages by R-Nonce */
		err = reg_proto_check_nonce(r->m_er_nonce, msg, WPS_ID_REGISTRAR_NONCE);
		if (err == RPROT_ERR_NONCE_MISMATCH)
			return WPS_CONT;
	}

	return WPS_SUCCESS;
}

int
wps_upnpDevSSR(WPSAPI_T *g_mc, CTlvSsrIE *ssrmsg)
{
	WPS_SCMODE sc_mode = g_mc->dev_info->sc_mode;
	uint8 scState = g_mc->dev_info->scState;

	TUTRACE((TUTRACE_INFO, "MC_Recd CB_TRUPNP_DEV_SSR\n"));

	/*
	 * Added to support SetSelectedRegistrar
	 * If we are an AP+Proxy, then add the SelectedRegistrar TLV
	 * to the WPS IE in the Beacon
	 * This call will fail if the right WLAN drivers are not used.
	 */
	if (sc_mode == SCMODE_AP_REGISTRAR) {
		wps_setWpsIE(g_mc,
			scState,
			ssrmsg->selReg.m_data,
			ssrmsg->devPwdId.m_data,
			ssrmsg->selRegCfgMethods.m_data,
			ssrmsg->authorizedMacs.m_data,
			ssrmsg->authorizedMacs.subtlvbase.m_len);
	}

	return WPS_CONT;
}