自学教程：C++ swap128函数代码示例

static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb){	struct smp_cmd_pairing *req, *rsp = (void *) skb->data;	struct smp_cmd_pairing_confirm cp;	struct crypto_blkcipher *tfm = conn->hcon->hdev->tfm;	int ret;	u8 res[16], key_size;	BT_DBG("conn %p", conn);	skb_pull(skb, sizeof(*rsp));	req = (void *) &conn->preq[1];	key_size = min(req->max_key_size, rsp->max_key_size);	if (check_enc_key_size(conn, key_size))		return SMP_ENC_KEY_SIZE;	if (rsp->oob_flag)		return SMP_OOB_NOT_AVAIL;	/* Just works */	memset(conn->tk, 0, sizeof(conn->tk));	conn->prsp[0] = SMP_CMD_PAIRING_RSP;	memcpy(&conn->prsp[1], rsp, sizeof(*rsp));	ret = smp_rand(conn->prnd);	if (ret)		return SMP_UNSPECIFIED;	ret = smp_c1(tfm, conn->tk, conn->prnd, conn->preq, conn->prsp, 0,			conn->src, conn->hcon->dst_type, conn->dst, res);	if (ret)		return SMP_UNSPECIFIED;	swap128(res, cp.confirm_val);	smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);	return 0;}

static void confirm_work(struct work_struct *work){	struct smp_chan *smp = container_of(work, struct smp_chan, confirm);	struct l2cap_conn *conn = smp->conn;	struct crypto_blkcipher *tfm;	struct smp_cmd_pairing_confirm cp;	int ret;	u8 res[16], reason;	BT_DBG("conn %p", conn);	tfm = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC);	if (IS_ERR(tfm)) {		reason = SMP_UNSPECIFIED;		goto error;	}	smp->tfm = tfm;	if (conn->hcon->out)		ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,				conn->src, conn->hcon->dst_type, conn->dst,				res);	else		ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,				conn->hcon->dst_type, conn->dst, 0, conn->src,				res);	if (ret) {		reason = SMP_UNSPECIFIED;		goto error;	}	clear_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags);	swap128(res, cp.confirm_val);	smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp);	return;error:	smp_failure(conn, reason, 1);}

void att_dump_attributes(void){    att_iterator_t it;    att_iterator_init(&it);    uint8_t uuid128[16];    while (att_iterator_has_next(&it)){        att_iterator_fetch_next(&it);        if (it.handle == 0) {            log_info("Handle: END");            return;        }        log_info("Handle: 0x%04x, flags: 0x%04x, uuid: ", it.handle, it.flags);        if (it.flags & ATT_PROPERTY_UUID128){            swap128(it.uuid, uuid128);            log_info("%s", uuid128_to_str(uuid128));        } else {            log_info("%04x", READ_BT_16(it.uuid, 0));        }        log_info(", value_len: %u, value: ", it.value_len);        hexdump(it.value, it.value_len);    }}

int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey){	struct l2cap_conn *conn = hcon->smp_conn;	struct smp_chan *smp;	u32 value;	u8 key[16];	BT_DBG("");	if (!conn)		return -ENOTCONN;	smp = conn->smp_chan;	switch (mgmt_op) {	case MGMT_OP_USER_PASSKEY_REPLY:		value = le32_to_cpu(passkey);		memset(key, 0, sizeof(key));		BT_DBG("PassKey: %d", value);		put_unaligned_le32(value, key);		swap128(key, smp->tk);		/* Fall Through */	case MGMT_OP_USER_CONFIRM_REPLY:		set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);		break;	case MGMT_OP_USER_PASSKEY_NEG_REPLY:	case MGMT_OP_USER_CONFIRM_NEG_REPLY:		smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);		return 0;	default:		smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED, 1);		return -EOPNOTSUPP;	}	/* If it is our turn to send Pairing Confirm, do so now */	if (test_bit(SMP_FLAG_CFM_PENDING, &smp->smp_flags))		queue_work(hcon->hdev->workqueue, &smp->confirm);	return 0;}

	Encoding *RegisterAllocator::movaps(OperandXMMREG r128i, OperandXMMREG r128j)	{		if(r128i == r128j) return 0;		// Register overwritten, when not used, eliminate load instruction		if(XMM[r128i.reg].loadInstruction && loadElimination)		{			XMM[r128i.reg].loadInstruction->reserve();			XMM[r128i.reg].loadInstruction = 0;		}		// Register overwritten, when not used, eliminate copy instruction		if(XMM[r128i.reg].copyInstruction && copyPropagation)		{			XMM[r128i.reg].copyInstruction->reserve();			XMM[r128i.reg].copyInstruction = 0;		}		Encoding *spillInstruction = XMM[r128i.reg].spillInstruction;		AllocationData spillAllocation = XMM[r128i.reg].spill;		Encoding *movaps = Assembler::movaps(r128i, r128j);				if(XMM[r128i.reg].reference == 0 || XMM[r128j.reg].reference == 0)   // Return if not in allocation table		{			return movaps;		}		// Attempt copy propagation		if(movaps && copyPropagation)		{			swap128(r128i.reg, r128j.reg);			XMM[r128i.reg].copyInstruction = movaps;		}		XMM[r128i.reg].spillInstruction = spillInstruction;		XMM[r128i.reg].spill = spillAllocation;				return movaps;	}

static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb){	struct smp_chan *smp = conn->smp_chan;	struct hci_dev *hdev = conn->hcon->hdev;	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");	memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf));	skb_pull(skb, sizeof(smp->pcnf));	if (conn->hcon->out) {		u8 random[16];		swap128(smp->prnd, random);		smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),								random);	} else {		queue_work(hdev->workqueue, &smp->confirm);	}	return 0;}

static void random_work(struct work_struct *work){	struct smp_chan *smp = container_of(work, struct smp_chan, random);	struct l2cap_conn *conn = smp->conn;	struct hci_conn *hcon = conn->hcon;	struct crypto_blkcipher *tfm = smp->tfm;	u8 reason, confirm[16], res[16], key[16];	int ret;	if (IS_ERR_OR_NULL(tfm)) {		reason = SMP_UNSPECIFIED;		goto error;	}	BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");	if (hcon->out)		ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,			     conn->src, hcon->dst_type, conn->dst, res);	else		ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,			     hcon->dst_type, conn->dst, 0, conn->src, res);	if (ret) {		reason = SMP_UNSPECIFIED;		goto error;	}	swap128(res, confirm);	if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) {		BT_ERR("Pairing failed (confirmation values mismatch)");		reason = SMP_CONFIRM_FAILED;		goto error;	}	if (hcon->out) {		u8 stk[16], rand[8];		__le16 ediv;		memset(rand, 0, sizeof(rand));		ediv = 0;		smp_s1(tfm, smp->tk, smp->rrnd, smp->prnd, key);		swap128(key, stk);		memset(stk + smp->enc_key_size, 0,		       SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);		if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) {			reason = SMP_UNSPECIFIED;			goto error;		}		hci_le_start_enc(hcon, ediv, rand, stk);		hcon->enc_key_size = smp->enc_key_size;	} else {		u8 stk[16], r[16], rand[8];		__le16 ediv;		memset(rand, 0, sizeof(rand));		ediv = 0;		swap128(smp->prnd, r);		smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(r), r);		smp_s1(tfm, smp->tk, smp->prnd, smp->rrnd, key);		swap128(key, stk);		memset(stk + smp->enc_key_size, 0,				SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);		hci_add_ltk(hcon->hdev, conn->dst, hcon->dst_type,			    HCI_SMP_STK_SLAVE, 0, 0, stk, smp->enc_key_size,			    ediv, rand);	}	return;error:	smp_failure(conn, reason, 1);}

static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,						u8 local_io, u8 remote_io){	struct hci_conn *hcon = conn->hcon;	struct smp_chan *smp = conn->smp_chan;	u8 method;	u32 passkey = 0;	int ret = 0;	/* Initialize key for JUST WORKS */	memset(smp->tk, 0, sizeof(smp->tk));	clear_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);	BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io);	/* If neither side wants MITM, use JUST WORKS */	/* If either side has unknown io_caps, use JUST WORKS */	/* Otherwise, look up method from the table */	if (!(auth & SMP_AUTH_MITM) ||			local_io > SMP_IO_KEYBOARD_DISPLAY ||			remote_io > SMP_IO_KEYBOARD_DISPLAY)		method = JUST_WORKS;	else		method = gen_method[remote_io][local_io];	/* If not bonding, don't ask user to confirm a Zero TK */	if (!(auth & SMP_AUTH_BONDING) && method == JUST_CFM)		method = JUST_WORKS;	/* If Just Works, Continue with Zero TK */	if (method == JUST_WORKS) {		set_bit(SMP_FLAG_TK_VALID, &smp->smp_flags);		return 0;	}	/* Not Just Works/Confirm results in MITM Authentication */	if (method != JUST_CFM)		set_bit(SMP_FLAG_MITM_AUTH, &smp->smp_flags);	/* If both devices have Keyoard-Display I/O, the master	 * Confirms and the slave Enters the passkey.	 */	if (method == OVERLAP) {		if (hcon->link_mode & HCI_LM_MASTER)			method = CFM_PASSKEY;		else			method = REQ_PASSKEY;	}	/* Generate random passkey. Not valid until confirmed. */	if (method == CFM_PASSKEY) {		u8 key[16];		memset(key, 0, sizeof(key));		get_random_bytes(&passkey, sizeof(passkey));		passkey %= 1000000;		put_unaligned_le32(passkey, key);		swap128(key, smp->tk);		BT_DBG("PassKey: %d", passkey);	}	hci_dev_lock(hcon->hdev);	if (method == REQ_PASSKEY)		ret = mgmt_user_passkey_request(hcon->hdev, conn->dst,						hcon->type, hcon->dst_type);	else		ret = mgmt_user_confirm_request(hcon->hdev, conn->dst,						hcon->type, hcon->dst_type,						cpu_to_le32(passkey), 0);	hci_dev_unlock(hcon->hdev);	return ret;}

static void sm_c1(key_t k, key_t r, uint8_t preq[7], uint8_t pres[7], uint8_t iat, uint8_t rat, bd_addr_t ia, bd_addr_t ra, key_t c1){    // p1 = pres || preq || rat’ || iat’    // "The octet of iat’ becomes the least significant octet of p1 and the most signifi-    // cant octet of pres becomes the most significant octet of p1.    // For example, if the 8-bit iat’ is 0x01, the 8-bit rat’ is 0x00, the 56-bit preq    // is 0x07071000000101 and the 56 bit pres is 0x05000800000302 then    // p1 is 0x05000800000302070710000001010001."        key_t p1_flipped;    swap56(pres, &p1_flipped[0]);    swap56(preq, &p1_flipped[7]);    p1_flipped[14] = rat;    p1_flipped[15] = iat;    printf("p1' "); hexdump(p1_flipped, 16);        // p2 = padding || ia || ra    // "The least significant octet of ra becomes the least significant octet of p2 and    // the most significant octet of padding becomes the most significant octet of p2.    // For example, if 48-bit ia is 0xA1A2A3A4A5A6 and the 48-bit ra is    // 0xB1B2B3B4B5B6 then p2 is 0x00000000A1A2A3A4A5A6B1B2B3B4B5B6.        key_t p2_flipped;    memset(p2_flipped, 0, 16);    memcpy(&p2_flipped[4],  ia, 6);    memcpy(&p2_flipped[10], ra, 6);    printf("p2' "); hexdump(p2_flipped, 16);        // t1 = r xor p1    int i;    key_t t1_flipped;    for (i=0;i<16;i++){        t1_flipped[i] = r[15-i] ^ p1_flipped[i];    }    printf("t1' "); hexdump(t1_flipped, 16);        key_t tk_flipped;    swap128(sm_tk, tk_flipped);    printf("tk' "); hexdump(tk_flipped, 16);        // setup aes decryption    unsigned long rk[RKLENGTH(KEYBITS)];    int nrounds = rijndaelSetupEncrypt(rk, &tk_flipped[0], KEYBITS);        // t2 = e(k, r_xor_p1)    key_t t2_flipped;    rijndaelEncrypt(rk, nrounds, t1_flipped, t2_flipped);        printf("t2' "); hexdump(t2_flipped, 16);        key_t t3_flipped;    for (i=0;i<16;i++){        t3_flipped[i] = t2_flipped[i] ^ p2_flipped[i];    }    printf("t3' "); hexdump(t3_flipped, 16);        key_t c1_flipped;    rijndaelEncrypt(rk, nrounds, t3_flipped, c1_flipped);        printf("c1' "); hexdump(c1_flipped, 16);        swap128(c1_flipped, c1);        printf("c1: "); hexdump(c1, 16);}

int le_smpconnect(bdaddr_t *bd,int hci){	struct sockaddr_l2cap l2c;	int s;	unsigned char buf[40];	ssize_t len;	int i;	int count;	int handle = 0;	ng_l2cap_smp_pairinfo preq, pres;	fd_set rfds,wfds;	uint8_t k[16];	int conok = 0;	struct sockaddr_l2cap myname;	s = socket(PF_BLUETOOTH, SOCK_SEQPACKET|SOCK_NONBLOCK,		   BLUETOOTH_PROTO_L2CAP);	l2c.l2cap_len = sizeof(l2c);	l2c.l2cap_family = AF_BLUETOOTH;	l2c.l2cap_psm = 0;	l2c.l2cap_cid = NG_L2CAP_SMP_CID;	l2c.l2cap_bdaddr_type = BDADDR_LE_PUBLIC;	bcopy(bd, &l2c.l2cap_bdaddr, sizeof(*bd));	printf("CONNECT/n");	if(connect(s, (struct sockaddr *) &l2c, sizeof(l2c)) == 0){	  		printf("CONNECTOK/n");			}else{	  perror("connect");	}#if 1	do{	  handle = le_connect_result(hci);	}while(handle==0);#endif	printf("handle%x/n", handle);	{	  int fl;	  fl = fcntl(s, F_GETFL, 0);	  fcntl(s, F_SETFL, fl&~O_NONBLOCK);	}			printf("HOGEHOGE/n");	{	  preq.code = NG_L2CAP_SMP_PAIRREQ;	  preq.iocap = 4;	  preq.oobflag = 0;	  preq.authreq = 1;	  preq.maxkeysize = 16;	  preq.ikeydist = 1;	  preq.rkeydist = 1;	  write(s,&preq, sizeof(preq));	  printf("A/n");	  do {	    int len;	    printf("B/n");	    len = read(s, &pres, sizeof(pres));	    printf("%d, pi.code %d/n",len, pres.code);	    	  }while(pres.code != NG_L2CAP_SMP_PAIRRES);	  printf("C/n");	  printf("%d %d %d %d %d %d %d(%d)/n", pres.code,pres.iocap ,		 pres.oobflag, pres.authreq,		 pres.maxkeysize, pres.ikeydist, pres.rkeydist, sizeof(pres));	}	{		socklen_t siz = sizeof(myname);		char bdastr[40];		if(getsockname(s, (struct sockaddr *)&myname,&siz)!=0){			perror("getsockname");		}		printf("%d/n", myname.l2cap_bdaddr_type);		printf("%s/n", bt_ntoa(&myname.l2cap_bdaddr, NULL));	}	{		ng_l2cap_smp_keyinfo mrand,mconfirm,srand,sconfirm;		ng_l2cap_smp_reqres failed;		int res;		uint8_t rval[16];		int ng = 0;		bzero(k, sizeof(k));		arc4random_buf(rval, sizeof(rval));		swap128(rval, mrand.body);		mconfirm.code = NG_L2CAP_SMP_PAIRCONF;		mrand.code = NG_L2CAP_SMP_PAIRRAND;				smp_c1(k, rval, (uint8_t *)&preq, (uint8_t *)&pres,		       (myname.l2cap_bdaddr_type == BDADDR_LE_RANDOM)? 1:0,		       &myname.l2cap_bdaddr,  0, bd);		swap128(rval, mconfirm.body);		write(s, &mconfirm, sizeof(mconfirm));	       		res = read(s, &sconfirm, sizeof(sconfirm));		printf("%d/n", res);		if(sconfirm.code != NG_L2CAP_SMP_PAIRCONF){			printf("sconfirm.code %d/n", sconfirm.code);		}		write(s, &mrand, sizeof(mrand));		res = read(s, &srand, sizeof(srand));//.........这里部分代码省略.........

/* LISTING_START(GAPLEAdvDataParsing): Parsing advertising data */static void dump_advertisement_data(uint8_t * adv_data, uint8_t adv_size){    ad_context_t context;    bd_addr_t address;    uint8_t uuid_128[16];    for (ad_iterator_init(&context, adv_size, adv_data) ; ad_iterator_has_more(&context) ; ad_iterator_next(&context)){        uint8_t data_type = ad_iterator_get_data_type(&context);        uint8_t size      = ad_iterator_get_data_len(&context);        uint8_t * data    = ad_iterator_get_data(&context);                if (data_type > 0 && data_type < 0x1B){            printf("    %s: ", ad_types[data_type]);        }         int i;        // Assigned Numbers GAP            switch (data_type){            case 0x01: // Flags                // show only first octet, ignore rest                for (i=0; i<8;i++){                    if (data[0] & (1<<i)){                        printf("%s; ", flags[i]);                    }                }                break;            case 0x02: // Incomplete List of 16-bit Service Class UUIDs            case 0x03: // Complete List of 16-bit Service Class UUIDs            case 0x14: // List of 16-bit Service Solicitation UUIDs                for (i=0; i<size;i+=2){                    printf("%02X ", READ_BT_16(data, i));                }                break;            case 0x04: // Incomplete List of 32-bit Service Class UUIDs            case 0x05: // Complete List of 32-bit Service Class UUIDs                for (i=0; i<size;i+=4){                    printf("%04X ", READ_BT_32(data, i));                }                break;            case 0x06: // Incomplete List of 128-bit Service Class UUIDs            case 0x07: // Complete List of 128-bit Service Class UUIDs            case 0x15: // List of 128-bit Service Solicitation UUIDs                swap128(data, uuid_128);                printUUID128(uuid_128);                break;            case 0x08: // Shortened Local Name            case 0x09: // Complete Local Name                for (i=0; i<size;i++){                    printf("%c", (char)(data[i]));                }                break;            case 0x0A: // Tx Power Level                 printf("%d dBm", *(int8_t*)data);                break;            case 0x12: // Slave Connection Interval Range                 printf("Connection Interval Min = %u ms, Max = %u ms", READ_BT_16(data, 0) * 5/4, READ_BT_16(data, 2) * 5/4);                break;            case 0x16: // Service Data                 printf_hexdump(data, size);                break;            case 0x17: // Public Target Address            case 0x18: // Random Target Address                bt_flip_addr(address, data);                print_bd_addr(address);                break;            case 0x19: // Appearance                 // https://developer.bluetooth.org/gatt/characteristics/Pages/CharacteristicViewer.aspx?u=org.bluetooth.characteristic.gap.appearance.xml                printf("%02X", READ_BT_16(data, 0) );                break;            case 0x1A: // Advertising Interval                 printf("%u ms", READ_BT_16(data, 0) * 5/8 );                break;            case 0x3D: // 3D Information Data                 printf_hexdump(data, size);                break;            case 0xFF: // Manufacturer Specific Data                 break;            case 0x0D: // Class of Device (3B)            case 0x0E: // Simple Pairing Hash C (16B)            case 0x0F: // Simple Pairing Randomizer R (16B)             case 0x10: // Device ID             case 0x11: // Security Manager TK Value (16B)            default:                printf("Unknown Advertising Data Type");                 break;        }                printf("/n");    }    printf("/n");}