C++ put_unaligned_le16函数代码示例

/********************************************************************************************************************
 *function:  construct DELBA frame
 *   input:  u8*		dst	//DELBA frame's destination
 *	     PBA_RECORD		pBA	//BA_RECORD entry which stores the necessary information for BA
 *	     TR_SELECT	        TxRxSelect  //TX RX direction
 *	     u16		ReasonCode  //status code.
 *  output:  none
 *  return:  sk_buff*		skb     //return constructed skb to xmit
********************************************************************************************************************/
static struct sk_buff *ieee80211_DELBA(
	struct ieee80211_device  *ieee,
	u8		         *dst,
	PBA_RECORD		 pBA,
	TR_SELECT		 TxRxSelect,
	u16			 ReasonCode
	)
{
	DELBA_PARAM_SET	DelbaParamSet;
	struct sk_buff *skb = NULL;
	 struct ieee80211_hdr_3addr *Delba = NULL;
	u8 *tag = NULL;
	//len = head len + DELBA Parameter Set(2) + Reason Code(2)
	u16 len = 6 + ieee->tx_headroom;

	if (net_ratelimit())
	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "========>%s(), ReasonCode(%d) sentd to:%pM\n", __func__, ReasonCode, dst);

	memset(&DelbaParamSet, 0, 2);

	DelbaParamSet.field.Initiator	= (TxRxSelect==TX_DIR)?1:0;
	DelbaParamSet.field.TID	= pBA->BaParamSet.field.TID;

	skb = dev_alloc_skb(len + sizeof( struct ieee80211_hdr_3addr)); //need to add something others? FIXME
	if (skb == NULL)
	{
		IEEE80211_DEBUG(IEEE80211_DL_ERR, "can't alloc skb for ADDBA_REQ\n");
		return NULL;
	}
//	memset(skb->data, 0, len+sizeof( struct ieee80211_hdr_3addr));
	skb_reserve(skb, ieee->tx_headroom);

	Delba = ( struct ieee80211_hdr_3addr *) skb_put(skb,sizeof( struct ieee80211_hdr_3addr));

	memcpy(Delba->addr1, dst, ETH_ALEN);
	memcpy(Delba->addr2, ieee->dev->dev_addr, ETH_ALEN);
	memcpy(Delba->addr3, ieee->current_network.bssid, ETH_ALEN);
	Delba->frame_ctl = cpu_to_le16(IEEE80211_STYPE_MANAGE_ACT); //action frame

	tag = (u8 *)skb_put(skb, 6);

	*tag ++= ACT_CAT_BA;
	*tag ++= ACT_DELBA;

	// DELBA Parameter Set

	put_unaligned_le16(DelbaParamSet.shortData, tag);
	tag += 2;
	// Reason Code

	put_unaligned_le16(ReasonCode, tag);
	tag += 2;

	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
	if (net_ratelimit())
	IEEE80211_DEBUG(IEEE80211_DL_TRACE | IEEE80211_DL_BA, "<=====%s()\n", __func__);
	return skb;
}

static int __init am33xx_setup_sleep_sequence(void)
{
	int ret;
	int sz;
	const void *prop;
	struct device *dev;
	u32 freq_hz = 100000;
	unsigned short freq_khz;

	/*
	 * We put the device tree node in the I2C controller that will
	 * be sending the sequence. i2c1 is the only controller that can
	 * be accessed by the firmware as it is the only controller in the
	 * WKUP domain.
	 */
	dev = omap_device_get_by_hwmod_name("i2c1");
	if (IS_ERR(dev))
		return PTR_ERR(dev);

	of_property_read_u32(dev->of_node, "clock-frequency", &freq_hz);
	freq_khz = freq_hz / 1000;

	prop = of_get_property(dev->of_node, "sleep-sequence", &sz);
	if (prop) {
		/*
		 * Length is sequence length + 2 bytes for freq_khz, and 1
		 * byte for terminator.
		 */
		am33xx_i2c_sleep_sequence = kzalloc(sz + 3, GFP_KERNEL);

		if (!am33xx_i2c_sleep_sequence)
			return -ENOMEM;
		put_unaligned_le16(freq_khz, am33xx_i2c_sleep_sequence);
		memcpy(am33xx_i2c_sleep_sequence + 2, prop, sz);
		i2c_sleep_sequence_sz = sz + 3;
	}

	prop = of_get_property(dev->of_node, "wake-sequence", &sz);
	if (prop) {
		am33xx_i2c_wake_sequence = kzalloc(sz + 3, GFP_KERNEL);
		if (!am33xx_i2c_wake_sequence) {
			ret = -ENOMEM;
			goto cleanup_sleep;
		}
		put_unaligned_le16(freq_khz, am33xx_i2c_wake_sequence);
		memcpy(am33xx_i2c_wake_sequence + 2, prop, sz);
		i2c_wake_sequence_sz = sz + 3;
	}

	return 0;

cleanup_sleep:
	kfree(am33xx_i2c_sleep_sequence);
	am33xx_i2c_sleep_sequence = NULL;
	return ret;
}

bool config_parser_handle_guid(struct list_head *head, void *data)
{
	struct ptoken *t, *first;
	unsigned char *target = data;
	union
	{
		unsigned tmp_ints[16];
		struct
		{
			unsigned f[4];
			unsigned long long l;
		} s;
	} x;
	unsigned i, *tmp_id = x.tmp_ints;

	first = list_entry(head->next, struct ptoken, list);
	t = config_token_after_equal(head);
	if(!t) {
		logg(LOGF_NOTICE, "Parsing config file %[email protected]%zu: Option \"%s\" wants a value assigned, will ignore\n",
		     first->ctx->in_filename, first->ctx->line_num, first->d.t);
		return true;
	}
	if(16 != sscanf(t->d.t,
	                "%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x",
	                tmp_id, tmp_id+1, tmp_id+2, tmp_id+3, tmp_id+4, tmp_id+5,
	                tmp_id+6, tmp_id+7, tmp_id+8, tmp_id+9, tmp_id+10,
	                tmp_id+11, tmp_id+12, tmp_id+13, tmp_id+14, tmp_id+15)) {
		if(5 != sscanf(t->d.t,
		              "%08X-%04X-%04X-%04X-%012"
#ifndef WIN32
		                                       "llX",
#else
		                                       "I64X",
#endif
		              x.s.f, x.s.f+1, x.s.f+2, x.s.f+3, &x.s.l)) {
			logg(LOGF_NOTICE, "Parsing config file %[email protected]%zu: \"%s\" does not seem to be a valid guid, will ignore\n",
			     first->ctx->in_filename, first->ctx->line_num, t->d.t);
			return true;
		}
		else
		{
			/*
			 * guids come from windows systems, their format
			 * has a plattform endian touch (first 3 fields),
			 * but that plattform is "always" intel...
			 */
// TODO: GUIDs and enianess?
			put_unaligned_le32(x.s.f[0], target);
			put_unaligned_le16(x.s.f[1], target + 4);
			put_unaligned_le16(x.s.f[2], target + 6);
			put_unaligned_be16(x.s.f[3], target + 8);
			put_unaligned_be16(x.s.l >> 32, target + 10);
			put_unaligned_be32(x.s.l, target + 12);
		}
	} else {
		for(i = 0; i < 16; i++)

static int prepare_tt_output_report(struct tt_output_report *out,
		u16 length, u8 command)
{
	put_unaligned_le16(0x04, &out->reg_address);
	put_unaligned_le16(5 + length, &out->length);

	out->report_id = 0x2f;
	out->reserved = 0x00;
	out->command = command;

	return 7 + length;
}

static struct sk_buff *rtllib_DELBA(struct rtllib_device *ieee, u8 *dst,
				    struct ba_record *pBA,
				    enum tr_select TxRxSelect, u16 ReasonCode)
{
	union delba_param_set DelbaParamSet;
	struct sk_buff *skb = NULL;
	 struct rtllib_hdr_3addr *Delba = NULL;
	u8 *tag = NULL;
	u16 len = 6 + ieee->tx_headroom;

	if (net_ratelimit())
		netdev_dbg(ieee->dev, "%s(): ReasonCode(%d) sentd to: %pM\n",
			   __func__, ReasonCode, dst);

	memset(&DelbaParamSet, 0, 2);

	DelbaParamSet.field.Initiator = (TxRxSelect == TX_DIR) ? 1 : 0;
	DelbaParamSet.field.TID	= pBA->BaParamSet.field.TID;

	skb = dev_alloc_skb(len + sizeof(struct rtllib_hdr_3addr));
	if (skb == NULL)
		return NULL;

	skb_reserve(skb, ieee->tx_headroom);

	Delba = (struct rtllib_hdr_3addr *) skb_put(skb,
		 sizeof(struct rtllib_hdr_3addr));

	ether_addr_copy(Delba->addr1, dst);
	ether_addr_copy(Delba->addr2, ieee->dev->dev_addr);
	ether_addr_copy(Delba->addr3, ieee->current_network.bssid);
	Delba->frame_ctl = cpu_to_le16(RTLLIB_STYPE_MANAGE_ACT);

	tag = (u8 *)skb_put(skb, 6);

	*tag++ = ACT_CAT_BA;
	*tag++ = ACT_DELBA;


	put_unaligned_le16(DelbaParamSet.shortData, tag);
	tag += 2;

	put_unaligned_le16(ReasonCode, tag);
	tag += 2;

#ifdef VERBOSE_DEBUG
	print_hex_dump_bytes("rtllib_DELBA(): ", DUMP_PREFIX_NONE, skb->data,
			     skb->len);
#endif
	return skb;
}

/*
 * ieee80211_add_rx_radiotap_header - add radiotap header
 */
static int hdd_wlan_add_rx_radiotap_hdr (
             struct sk_buff *skb, int rtap_len, int flag )
{
    u8 rtap_temp[20] = {0};
    struct ieee80211_radiotap_header *rthdr;
    unsigned char *pos;
    u16 rx_flags = 0;

    rthdr = (struct ieee80211_radiotap_header *)(&rtap_temp[0]);

    /* radiotap header, set always present flags */
    rthdr->it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS)   |
                                    (1 << IEEE80211_RADIOTAP_RX_FLAGS));
    rthdr->it_len = cpu_to_le16(rtap_len);

    pos = (unsigned char *) (rthdr + 1);

    /* the order of the following fields is important */

    /* IEEE80211_RADIOTAP_FLAGS */
    *pos = 0;
    pos++;

    /* IEEE80211_RADIOTAP_RX_FLAGS: Length 2 Bytes */
    /* ensure 2 byte alignment for the 2 byte field as required */
    if ((pos - (u8 *)rthdr) & 1)
        pos++;
    put_unaligned_le16(rx_flags, pos);
    pos += 2;
    
    // actually push the data
    memcpy(skb_push(skb, rtap_len), &rtap_temp[0], rtap_len);

    return 0;
}

/**
 * mesh_path_error_tx - Sends a PERR mesh management frame
 *
 * @ttl: allowed remaining hops
 * @target: broken destination
 * @target_sn: SN of the broken destination
 * @target_rcode: reason code for this PERR
 * @ra: node this frame is addressed to
 * @sdata: local mesh subif
 *
 * Note: This function may be called with driver locks taken that the driver
 * also acquires in the TX path.  To avoid a deadlock we don't transmit the
 * frame directly but add it to the pending queue instead.
 */
int mesh_path_error_tx(struct ieee80211_sub_if_data *sdata,
		       u8 ttl, const u8 *target, u32 target_sn,
		       u16 target_rcode, const u8 *ra)
{
	struct ieee80211_local *local = sdata->local;
	struct sk_buff *skb;
	struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
	struct ieee80211_mgmt *mgmt;
	u8 *pos, ie_len;
	int hdr_len = offsetof(struct ieee80211_mgmt, u.action.u.mesh_action) +
		      sizeof(mgmt->u.action.u.mesh_action);

	if (time_before(jiffies, ifmsh->next_perr))
		return -EAGAIN;

	skb = dev_alloc_skb(local->tx_headroom +
			    sdata->encrypt_headroom +
			    IEEE80211_ENCRYPT_TAILROOM +
			    hdr_len +
			    2 + 15 /* PERR IE */);
	if (!skb)
		return -1;
	skb_reserve(skb, local->tx_headroom + sdata->encrypt_headroom);
	mgmt = skb_put_zero(skb, hdr_len);
	mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
					  IEEE80211_STYPE_ACTION);

	memcpy(mgmt->da, ra, ETH_ALEN);
	memcpy(mgmt->sa, sdata->vif.addr, ETH_ALEN);
	/* BSSID == SA */
	memcpy(mgmt->bssid, sdata->vif.addr, ETH_ALEN);
	mgmt->u.action.category = WLAN_CATEGORY_MESH_ACTION;
	mgmt->u.action.u.mesh_action.action_code =
					WLAN_MESH_ACTION_HWMP_PATH_SELECTION;
	ie_len = 15;
	pos = skb_put(skb, 2 + ie_len);
	*pos++ = WLAN_EID_PERR;
	*pos++ = ie_len;
	/* ttl */
	*pos++ = ttl;
	/* number of destinations */
	*pos++ = 1;
	/* Flags field has AE bit only as defined in
	 * sec 8.4.2.117 IEEE802.11-2012
	 */
	*pos = 0;
	pos++;
	memcpy(pos, target, ETH_ALEN);
	pos += ETH_ALEN;
	put_unaligned_le32(target_sn, pos);
	pos += 4;
	put_unaligned_le16(target_rcode, pos);

	/* see note in function header */
	prepare_frame_for_deferred_tx(sdata, skb);
	ifmsh->next_perr = TU_TO_EXP_TIME(
				   ifmsh->mshcfg.dot11MeshHWMPperrMinInterval);
	ieee80211_add_pending_skb(local, skb);
	return 0;
}

static int bq27xxx_battery_i2c_write(struct bq27xxx_device_info *di, u8 reg,
				     int value, bool single)
{
	struct i2c_client *client = to_i2c_client(di->dev);
	struct i2c_msg msg;
	u8 data[4];
	int ret;

	if (!client->adapter)
		return -ENODEV;

	data[0] = reg;
	if (single) {
		data[1] = (u8) value;
		msg.len = 2;
	} else {
		put_unaligned_le16(value, &data[1]);
		msg.len = 3;
	}

	msg.buf = data;
	msg.addr = client->addr;
	msg.flags = 0;

	ret = i2c_transfer(client->adapter, &msg, 1);
	if (ret < 0)
		return ret;
	if (ret != 1)
		return -EINVAL;
	return 0;
}

/**
 * usb_gadget_config_buf - builts a complete configuration descriptor
 * @config: Header for the descriptor, including characteristics such
 *	as power requirements and number of interfaces.
 * @desc: Null-terminated vector of pointers to the descriptors (interface,
 *	endpoint, etc) defining all functions in this device configuration.
 * @buf: Buffer for the resulting configuration descriptor.
 * @length: Length of buffer.  If this is not big enough to hold the
 *	entire configuration descriptor, an error code will be returned.
 *
 * This copies descriptors into the response buffer, building a descriptor
 * for that configuration.  It returns the buffer length or a negative
 * status code.  The config.wTotalLength field is set to match the length
 * of the result, but other descriptor fields (including power usage and
 * interface count) must be set by the caller.
 *
 * Gadget drivers could use this when constructing a config descriptor
 * in response to USB_REQ_GET_DESCRIPTOR.  They will need to patch the
 * resulting bDescriptorType value if USB_DT_OTHER_SPEED_CONFIG is needed.
 */
int usb_gadget_config_buf(
	const struct usb_config_descriptor	*config,
	void					*buf,
	unsigned				length,
	const struct usb_descriptor_header	**desc
)
{
	struct usb_config_descriptor		*cp = buf;
	int					len;

	/* config descriptor first */
	if (length < USB_DT_CONFIG_SIZE || !desc)
		return -EINVAL;
	/* config need not be aligned */
	memcpy(cp, config, sizeof(*cp));

	/* then interface/endpoint/class/vendor/... */
	len = usb_descriptor_fillbuf(USB_DT_CONFIG_SIZE + (u8 *)buf,
			length - USB_DT_CONFIG_SIZE, desc);
	if (len < 0)
		return len;
	len += USB_DT_CONFIG_SIZE;
	if (len > 0xffff)
		return -EINVAL;

	/* patch up the config descriptor */
	cp->bLength = USB_DT_CONFIG_SIZE;
	cp->bDescriptorType = USB_DT_CONFIG;
	put_unaligned_le16(len, &cp->wTotalLength);
	cp->bmAttributes |= USB_CONFIG_ATT_ONE;
	return len;
}

/*
 * NAME:	cifs_strtoUTF16()
 *
 * FUNCTION:	Convert character string to unicode string
 *
 */
int
cifs_strtoUTF16(__le16 *to, const char *from, int len,
	      const struct nls_table *codepage)
{
	int charlen;
	int i;
	wchar_t wchar_to; /* needed to quiet sparse */

	/* special case for utf8 to handle no plane0 chars */
	if (!strcmp(codepage->charset, "utf8")) {
		/*
		 * convert utf8 -> utf16, we assume we have enough space
		 * as caller should have assumed conversion does not overflow
		 * in destination len is length in wchar_t units (16bits)
		 */
		i  = utf8s_to_utf16s(from, len, UTF16_LITTLE_ENDIAN,
				       (wchar_t *) to, len);

		/* if success terminate and exit */
		if (i >= 0)
			goto success;
		/*
		 * if fails fall back to UCS encoding as this
		 * function should not return negative values
		 * currently can fail only if source contains
		 * invalid encoded characters
		 */
	}

	for (i = 0; len && *from; i++, from += charlen, len -= charlen) {
		charlen = codepage->char2uni(from, len, &wchar_to);
		if (charlen < 1) {
			cifs_dbg(VFS, "strtoUTF16: char2uni of 0x%x returned %d\n",
				 *from, charlen);
			/* A question mark */
			wchar_to = 0x003f;
			charlen = 1;
		}
		put_unaligned_le16(wchar_to, &to[i]);
	}

success:
	put_unaligned_le16(0, &to[i]);
	return i;
}

static void ConstructProbeRsp(struct rtw_adapter *padapter, u8 *pframe,
			      u32 *pLength, u8 *StaAddr, bool bHideSSID)
{
	struct ieee80211_mgmt *mgmt;
	u8 *mac, *bssid;
	u32 pktlen;
	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
	struct wlan_bssid_ex *cur_network = &pmlmeinfo->network;

	/* DBG_8723A("%s\n", __func__); */

	mgmt = (struct ieee80211_mgmt *)pframe;

	mac = myid(&padapter->eeprompriv);
	bssid = cur_network->MacAddress;

	mgmt->frame_control =
		cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);

	mgmt->seq_ctrl = 0;

	memcpy(mgmt->da, StaAddr, ETH_ALEN);
	memcpy(mgmt->sa, mac, ETH_ALEN);
	memcpy(mgmt->bssid, bssid, ETH_ALEN);

	put_unaligned_le64(cur_network->tsf,
			   &mgmt->u.probe_resp.timestamp);
	put_unaligned_le16(cur_network->beacon_interval,
			   &mgmt->u.probe_resp.beacon_int);
	put_unaligned_le16(cur_network->capability,
			   &mgmt->u.probe_resp.capab_info);

	pktlen = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);

	if (cur_network->IELength > MAX_IE_SZ)
		return;

	memcpy(mgmt->u.probe_resp.variable, cur_network->IEs,
	       cur_network->IELength);
	pktlen += (cur_network->IELength);

	*pLength = pktlen;
}

/*
 * This function handles the command response of P2P mode cfg.
 */
static int
mwifiex_ret_p2p_mode_cfg(struct mwifiex_private *priv,
			 struct host_cmd_ds_command *resp,
			 void *data_buf)
{
	struct host_cmd_ds_p2p_mode_cfg *mode_cfg = &resp->params.mode_cfg;

	if (data_buf)
		put_unaligned_le16(le16_to_cpu(mode_cfg->mode), data_buf);

	return 0;
}

static int klsi_105_prepare_write_buffer(struct usb_serial_port *port,
						void *dest, size_t size)
{
	unsigned char *buf = dest;
	int count;

	count = kfifo_out_locked(&port->write_fifo, buf + KLSI_HDR_LEN, size,
								&port->lock);
	put_unaligned_le16(count, buf);

	return count + KLSI_HDR_LEN;
}

static int adau1701_safeload(struct sigmadsp *sigmadsp, unsigned int addr,
	const uint8_t bytes[], size_t len)
{
	struct i2c_client *client = to_i2c_client(sigmadsp->dev);
	struct adau1701 *adau1701 = i2c_get_clientdata(client);
	unsigned int val;
	unsigned int i;
	uint8_t buf[10];
	int ret;

	ret = regmap_read(adau1701->regmap, ADAU1701_DSPCTRL, &val);
	if (ret)
		return ret;

	if (val & ADAU1701_DSPCTRL_IST)
		msleep(50);

	for (i = 0; i < len / 4; i++) {
		put_unaligned_le16(ADAU1701_SAFELOAD_DATA(i), buf);
		buf[2] = 0x00;
		memcpy(buf + 3, bytes + i * 4, 4);
		ret = i2c_master_send(client, buf, 7);
		if (ret < 0)
			return ret;
		else if (ret != 7)
			return -EIO;

		put_unaligned_le16(ADAU1701_SAFELOAD_ADDR(i), buf);
		put_unaligned_le16(addr + i, buf + 2);
		ret = i2c_master_send(client, buf, 4);
		if (ret < 0)
			return ret;
		else if (ret != 4)
			return -EIO;
	}

	return regmap_update_bits(adau1701->regmap, ADAU1701_DSPCTRL,
		ADAU1701_DSPCTRL_IST, ADAU1701_DSPCTRL_IST);
}

static int aircable_prepare_write_buffer(struct usb_serial_port *port,
        void *dest, size_t size)
{
    int count;
    unsigned char *buf = dest;

    count = kfifo_out_locked(&port->write_fifo, buf + HCI_HEADER_LENGTH,
                             size - HCI_HEADER_LENGTH, &port->lock);
    buf[0] = TX_HEADER_0;
    buf[1] = TX_HEADER_1;
    put_unaligned_le16(count, &buf[2]);

    return count + HCI_HEADER_LENGTH;
}