static int ide_floppy_get_flexible_disk_page(ide_drive_t *drive,
struct ide_atapi_pc *pc)
{
struct ide_disk_obj *floppy = drive->driver_data;
struct gendisk *disk = floppy->disk;
u8 *page, buf[40];
int capacity, lba_capacity;
u16 transfer_rate, sector_size, cyls, rpm;
u8 heads, sectors;
ide_floppy_create_mode_sense_cmd(pc, IDEFLOPPY_FLEXIBLE_DISK_PAGE);
if (ide_queue_pc_tail(drive, disk, pc, buf, pc->req_xfer)) {
printk(KERN_ERR PFX "Can't get flexible disk page params\n");
return 1;
}
if (buf[3] & 0x80)
drive->dev_flags |= IDE_DFLAG_WP;
else
drive->dev_flags &= ~IDE_DFLAG_WP;
set_disk_ro(disk, !!(drive->dev_flags & IDE_DFLAG_WP));
page = &buf[8];
transfer_rate = be16_to_cpup((__be16 *)&buf[8 + 2]);
sector_size = be16_to_cpup((__be16 *)&buf[8 + 6]);
cyls = be16_to_cpup((__be16 *)&buf[8 + 8]);
rpm = be16_to_cpup((__be16 *)&buf[8 + 28]);
heads = buf[8 + 4];
sectors = buf[8 + 5];
capacity = cyls * heads * sectors * sector_size;
if (memcmp(page, &floppy->flexible_disk_page, 32))
printk(KERN_INFO PFX "%s: %dkB, %d/%d/%d CHS, %d kBps, "
"%d sector size, %d rpm\n",
drive->name, capacity / 1024, cyls, heads,
sectors, transfer_rate / 8, sector_size, rpm);
memcpy(&floppy->flexible_disk_page, page, 32);
drive->bios_cyl = cyls;
drive->bios_head = heads;
drive->bios_sect = sectors;
lba_capacity = floppy->blocks * floppy->block_size;
if (capacity < lba_capacity) {
printk(KERN_NOTICE PFX "%s: The disk reports a capacity of %d "
"bytes, but the drive only handles %d\n",
drive->name, lba_capacity, capacity);
floppy->blocks = floppy->block_size ?
capacity / floppy->block_size : 0;
drive->capacity64 = floppy->blocks * floppy->bs_factor;
}
return 0;
}
static int ib_link_query_port(struct ib_device *ibdev, u8 port,
struct ib_port_attr *props)
{
struct ib_smp *in_mad = NULL;
struct ib_smp *out_mad = NULL;
int ext_active_speed;
int err = -ENOMEM;
in_mad = kzalloc(sizeof *in_mad, GFP_KERNEL);
out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
if (!in_mad || !out_mad)
goto out;
init_query_mad(in_mad);
in_mad->attr_id = IB_SMP_ATTR_PORT_INFO;
in_mad->attr_mod = cpu_to_be32(port);
err = mlx4_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL,
in_mad, out_mad);
if (err)
goto out;
props->lid = be16_to_cpup((__be16 *) (out_mad->data + 16));
props->lmc = out_mad->data[34] & 0x7;
props->sm_lid = be16_to_cpup((__be16 *) (out_mad->data + 18));
props->sm_sl = out_mad->data[36] & 0xf;
props->state = out_mad->data[32] & 0xf;
props->phys_state = out_mad->data[33] >> 4;
props->port_cap_flags = be32_to_cpup((__be32 *) (out_mad->data + 20));
props->gid_tbl_len = to_mdev(ibdev)->dev->caps.gid_table_len[port];
props->max_msg_sz = to_mdev(ibdev)->dev->caps.max_msg_sz;
props->pkey_tbl_len = to_mdev(ibdev)->dev->caps.pkey_table_len[port];
props->bad_pkey_cntr = be16_to_cpup((__be16 *) (out_mad->data + 46));
props->qkey_viol_cntr = be16_to_cpup((__be16 *) (out_mad->data + 48));
props->active_width = out_mad->data[31] & 0xf;
props->active_speed = out_mad->data[35] >> 4;
props->max_mtu = out_mad->data[41] & 0xf;
props->active_mtu = out_mad->data[36] >> 4;
props->subnet_timeout = out_mad->data[51] & 0x1f;
props->max_vl_num = out_mad->data[37] >> 4;
props->init_type_reply = out_mad->data[41] >> 4;
if (props->port_cap_flags & IB_PORT_EXTENDED_SPEEDS_SUP) {
ext_active_speed = out_mad->data[62] >> 4;
switch (ext_active_speed) {
case 1:
props->active_speed = IB_SPEED_FDR;
break;
case 2:
props->active_speed = IB_SPEED_EDR;
break;
}
}
static int mlx4_ib_query_port(struct ib_device *ibdev, u8 port,
struct ib_port_attr *props)
{
struct ib_smp *in_mad = NULL;
struct ib_smp *out_mad = NULL;
int err = -ENOMEM;
in_mad = kzalloc(sizeof *in_mad, GFP_KERNEL);
out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
if (!in_mad || !out_mad)
goto out;
memset(props, 0, sizeof *props);
init_query_mad(in_mad);
in_mad->attr_id = IB_SMP_ATTR_PORT_INFO;
in_mad->attr_mod = cpu_to_be32(port);
err = mlx4_MAD_IFC(to_mdev(ibdev), 1, 1, port, NULL, NULL, in_mad, out_mad);
if (err)
goto out;
props->lid = be16_to_cpup((__be16 *) (out_mad->data + 16));
props->lmc = out_mad->data[34] & 0x7;
props->sm_lid = be16_to_cpup((__be16 *) (out_mad->data + 18));
props->sm_sl = out_mad->data[36] & 0xf;
props->state = out_mad->data[32] & 0xf;
props->phys_state = out_mad->data[33] >> 4;
props->port_cap_flags = be32_to_cpup((__be32 *) (out_mad->data + 20));
props->gid_tbl_len = to_mdev(ibdev)->dev->caps.gid_table_len[port];
props->max_msg_sz = 0x80000000;
props->pkey_tbl_len = to_mdev(ibdev)->dev->caps.pkey_table_len[port];
props->bad_pkey_cntr = be16_to_cpup((__be16 *) (out_mad->data + 46));
props->qkey_viol_cntr = be16_to_cpup((__be16 *) (out_mad->data + 48));
props->active_width = out_mad->data[31] & 0xf;
props->active_speed = out_mad->data[35] >> 4;
props->max_mtu = out_mad->data[41] & 0xf;
props->active_mtu = out_mad->data[36] >> 4;
props->subnet_timeout = out_mad->data[51] & 0x1f;
props->max_vl_num = out_mad->data[37] >> 4;
props->init_type_reply = out_mad->data[41] >> 4;
out:
kfree(in_mad);
kfree(out_mad);
return err;
}
int ieee80211_aes_ccm_decrypt(struct crypto_aead *tfm, u8 *b_0, u8 *aad,
u8 *data, size_t data_len, u8 *mic,
size_t mic_len)
{
struct scatterlist assoc, pt, ct[2];
char aead_req_data[sizeof(struct aead_request) +
crypto_aead_reqsize(tfm)]
__aligned(__alignof__(struct aead_request));
struct aead_request *aead_req = (void *) aead_req_data;
if (data_len == 0)
return -EINVAL;
memset(aead_req, 0, sizeof(aead_req_data));
sg_init_one(&pt, data, data_len);
sg_init_one(&assoc, &aad[2], be16_to_cpup((__be16 *)aad));
sg_init_table(ct, 2);
sg_set_buf(&ct[0], data, data_len);
sg_set_buf(&ct[1], mic, mic_len);
aead_request_set_tfm(aead_req, tfm);
aead_request_set_assoc(aead_req, &assoc, assoc.length);
aead_request_set_crypt(aead_req, ct, &pt, data_len + mic_len, b_0);
return crypto_aead_decrypt(aead_req);
}
/*
* Snoop SM MADs for port info and P_Key table sets, so we can
* synthesize LID change and P_Key change events.
*/
static void smp_snoop(struct ib_device *ibdev,
u8 port_num,
struct ib_mad *mad)
{
struct ib_event event;
if ((mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_LID_ROUTED ||
mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) &&
mad->mad_hdr.method == IB_MGMT_METHOD_SET) {
if (mad->mad_hdr.attr_id == IB_SMP_ATTR_PORT_INFO) {
update_sm_ah(to_mdev(ibdev), port_num,
be16_to_cpup((__be16 *) (mad->data + 58)),
(*(u8 *) (mad->data + 76)) & 0xf);
event.device = ibdev;
event.event = IB_EVENT_LID_CHANGE;
event.element.port_num = port_num;
ib_dispatch_event(&event);
}
if (mad->mad_hdr.attr_id == IB_SMP_ATTR_PKEY_TABLE) {
event.device = ibdev;
event.event = IB_EVENT_PKEY_CHANGE;
event.element.port_num = port_num;
ib_dispatch_event(&event);
}
}
}
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device
* specific to be rolled into the core.
*/
static irqreturn_t adis16201_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->private_data;
struct adis16201_state *st = iio_priv(indio_dev);
struct iio_ring_buffer *ring = indio_dev->ring;
int i = 0;
s16 *data;
size_t datasize = ring->access->get_bytes_per_datum(ring);
data = kmalloc(datasize, GFP_KERNEL);
if (data == NULL) {
dev_err(&st->us->dev, "memory alloc failed in ring bh");
return -ENOMEM;
}
if (ring->scan_count)
if (adis16201_read_ring_data(indio_dev, st->rx) >= 0)
for (; i < ring->scan_count; i++)
data[i] = be16_to_cpup(
(__be16 *)&(st->rx[i*2]));
/* Guaranteed to be aligned with 8 byte boundary */
if (ring->scan_timestamp)
*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
ring->access->store_to(ring, (u8 *)data, pf->timestamp);
iio_trigger_notify_done(indio_dev->trig);
kfree(data);
return IRQ_HANDLED;
}
static irqreturn_t adis16209_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct adis16209_state *st = iio_priv(indio_dev);
struct iio_buffer *ring = indio_dev->buffer;
int i = 0;
s16 *data;
size_t datasize = ring->access->get_bytes_per_datum(ring);
data = kmalloc(datasize , GFP_KERNEL);
if (data == NULL) {
dev_err(&st->us->dev, "memory alloc failed in ring bh");
return -ENOMEM;
}
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) &&
adis16209_read_ring_data(&indio_dev->dev, st->rx) >= 0)
for (; i < bitmap_weight(indio_dev->active_scan_mask,
indio_dev->masklength); i++)
data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2]));
if (ring->scan_timestamp)
*((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp;
ring->access->store_to(ring, (u8 *)data, pf->timestamp);
iio_trigger_notify_done(indio_dev->trig);
kfree(data);
return IRQ_HANDLED;
}
int ieee80211_aes_ccm_decrypt(struct crypto_aead *tfm, u8 *b_0, u8 *aad,
u8 *data, size_t data_len, u8 *mic)
{
struct scatterlist assoc, pt, ct[2];
struct {
struct aead_request req;
u8 priv[crypto_aead_reqsize(tfm)];
} aead_req;
if (data_len == 0)
return -EINVAL;
memset(&aead_req, 0, sizeof(aead_req));
sg_init_one(&pt, data, data_len);
sg_init_one(&assoc, &aad[2], be16_to_cpup((__be16 *)aad));
sg_init_table(ct, 2);
sg_set_buf(&ct[0], data, data_len);
sg_set_buf(&ct[1], mic, IEEE80211_CCMP_MIC_LEN);
aead_request_set_tfm(&aead_req.req, tfm);
aead_request_set_assoc(&aead_req.req, &assoc, assoc.length);
aead_request_set_crypt(&aead_req.req, ct, &pt,
data_len + IEEE80211_CCMP_MIC_LEN, b_0);
return crypto_aead_decrypt(&aead_req.req);
}
void ieee80211_aes_ccm_encrypt(struct crypto_aead *tfm, struct sk_buff *skb,
const u64 pn, size_t mic_len)
{
u8 aad[2 * AES_BLOCK_SIZE];
u8 b_0[AES_BLOCK_SIZE];
u8 *data, *mic;
size_t data_len, hdr_len;
struct ieee80211_hdr *hdr = (void *)skb->data;
struct scatterlist sg[3];
char aead_req_data[sizeof(struct aead_request) +
crypto_aead_reqsize(tfm)]
__aligned(__alignof__(struct aead_request));
struct aead_request *aead_req = (void *) aead_req_data;
hdr_len = ieee80211_hdrlen(hdr->frame_control);
data_len = skb->len - hdr_len - IEEE80211_CCMP_HDR_LEN;
ccmp_special_blocks(hdr, hdr_len, pn, b_0, aad);
memset(aead_req, 0, sizeof(aead_req_data));
data = skb->data + hdr_len + IEEE80211_CCMP_HDR_LEN;
mic = skb_put(skb, mic_len);
sg_init_table(sg, 3);
sg_set_buf(&sg[0], &aad[2], be16_to_cpup((__be16 *)aad));
sg_set_buf(&sg[1], data, data_len);
sg_set_buf(&sg[2], mic, mic_len);
aead_request_set_tfm(aead_req, tfm);
aead_request_set_crypt(aead_req, sg, sg, data_len, b_0);
aead_request_set_ad(aead_req, sg[0].length);
crypto_aead_encrypt(aead_req);
}
int ieee80211_aes_gcm_decrypt(struct crypto_aead *tfm, u8 *j_0, u8 *aad,
u8 *data, size_t data_len, u8 *mic)
{
struct scatterlist sg[3];
char aead_req_data[sizeof(struct aead_request) +
crypto_aead_reqsize(tfm)]
__aligned(__alignof__(struct aead_request));
struct aead_request *aead_req = (void *)aead_req_data;
if (data_len == 0)
return -EINVAL;
memset(aead_req, 0, sizeof(aead_req_data));
sg_init_table(sg, 3);
sg_set_buf(&sg[0], &aad[2], be16_to_cpup((__be16 *)aad));
sg_set_buf(&sg[1], data, data_len);
sg_set_buf(&sg[2], mic, IEEE80211_GCMP_MIC_LEN);
aead_request_set_tfm(aead_req, tfm);
aead_request_set_crypt(aead_req, sg, sg,
data_len + IEEE80211_GCMP_MIC_LEN, j_0);
aead_request_set_ad(aead_req, sg[0].length);
return crypto_aead_decrypt(aead_req);
}
bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port,
struct sk_buff *clone, unsigned int type)
{
struct mv88e6xxx_chip *chip = ds->priv;
struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
__be16 *seq_ptr;
u8 *hdr;
if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
return false;
hdr = mv88e6xxx_should_tstamp(chip, port, clone, type);
if (!hdr)
return false;
seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
&ps->state))
return false;
ps->tx_skb = clone;
ps->tx_tstamp_start = jiffies;
ps->tx_seq_id = be16_to_cpup(seq_ptr);
ptp_schedule_worker(chip->ptp_clock, 0);
return true;
}
static bool
iwl_nvm_no_wide_in_5ghz(struct device *dev, const struct iwl_cfg *cfg,
const __be16 *nvm_hw)
{
/*
* Workaround a bug in Indonesia SKUs where the regulatory in
* some 7000-family OTPs erroneously allow wide channels in
* 5GHz. To check for Indonesia, we take the SKU value from
* bits 1-4 in the subsystem ID and check if it is either 5 or
* 9. In those cases, we need to force-disable wide channels
* in 5GHz otherwise the FW will throw a sysassert when we try
* to use them.
*/
if (cfg->device_family == IWL_DEVICE_FAMILY_7000) {
/*
* Unlike the other sections in the NVM, the hw
* section uses big-endian.
*/
u16 subsystem_id = be16_to_cpup(nvm_hw + SUBSYSTEM_ID);
u8 sku = (subsystem_id & 0x1e) >> 1;
if (sku == 5 || sku == 9) {
IWL_DEBUG_EEPROM(dev,
"disabling wide channels in 5GHz (0x%0x %d)\n",
subsystem_id, sku);
return true;
}
}
static int pxa27x_keypad_direct_key_parse_dt(struct pxa27x_keypad *keypad,
struct pxa27x_keypad_platform_data *pdata)
{
struct input_dev *input_dev = keypad->input_dev;
struct device *dev = input_dev->dev.parent;
struct device_node *np = dev->of_node;
const __be16 *prop;
unsigned short code;
unsigned int proplen, size;
int i;
int error;
error = of_property_read_u32(np, "marvell,direct-key-count",
&pdata->direct_key_num);
if (error) {
/*
* If do not have marvel,direct-key-count defined,
* it means direct key is not supported.
*/
return error == -EINVAL ? 0 : error;
}
error = of_property_read_u32(np, "marvell,direct-key-mask",
&pdata->direct_key_mask);
if (error) {
if (error != -EINVAL)
return error;
/*
* If marvell,direct-key-mask is not defined, driver will use
* default value. Default value is set when configure the keypad.
*/
pdata->direct_key_mask = 0;
}
pdata->direct_key_low_active = of_property_read_bool(np,
"marvell,direct-key-low-active");
prop = of_get_property(np, "marvell,direct-key-map", &proplen);
if (!prop)
return -EINVAL;
if (proplen % sizeof(u16))
return -EINVAL;
size = proplen / sizeof(u16);
/* Only MAX_DIRECT_KEY_NUM is accepted.*/
if (size > MAX_DIRECT_KEY_NUM)
return -EINVAL;
for (i = 0; i < size; i++) {
code = be16_to_cpup(prop + i);
keypad->keycodes[MAX_MATRIX_KEY_NUM + i] = code;
__set_bit(code, input_dev->keybit);
}
return 0;
}
static int elan_smbus_write_fw_block(struct i2c_client *client,
const u8 *page, u16 checksum, int idx)
{
struct device *dev = &client->dev;
int error;
u16 result;
u8 val[3];
/*
* Due to the limitation of smbus protocol limiting
* transfer to 32 bytes at a time, we must split block
* in 2 transfers.
*/
error = i2c_smbus_write_block_data(client,
ETP_SMBUS_WRITE_FW_BLOCK,
ETP_FW_PAGE_SIZE / 2,
page);
if (error) {
dev_err(dev, "Failed to write page %d (part %d): %d\n",
idx, 1, error);
return error;
}
error = i2c_smbus_write_block_data(client,
ETP_SMBUS_WRITE_FW_BLOCK,
ETP_FW_PAGE_SIZE / 2,
page + ETP_FW_PAGE_SIZE / 2);
if (error) {
dev_err(dev, "Failed to write page %d (part %d): %d\n",
idx, 2, error);
return error;
}
/* Wait for F/W to update one page ROM data. */
usleep_range(8000, 10000);
error = i2c_smbus_read_block_data(client,
ETP_SMBUS_IAP_CTRL_CMD, val);
if (error < 0) {
dev_err(dev, "Failed to read IAP write result: %d\n",
error);
return error;
}
result = be16_to_cpup((__be16 *)val);
if (result & (ETP_FW_IAP_PAGE_ERR | ETP_FW_IAP_INTF_ERR)) {
dev_err(dev, "IAP reports failed write: %04hx\n",
result);
return -EIO;
}
return 0;
}
static int ib_link_query_port(struct ib_device *ibdev, u8 port,
struct ib_port_attr *props,
struct ib_smp *in_mad,
struct ib_smp *out_mad)
{
int ext_active_speed;
int err;
props->lid = be16_to_cpup((__be16 *) (out_mad->data + 16));
props->lmc = out_mad->data[34] & 0x7;
props->sm_lid = be16_to_cpup((__be16 *) (out_mad->data + 18));
props->sm_sl = out_mad->data[36] & 0xf;
props->state = out_mad->data[32] & 0xf;
props->phys_state = out_mad->data[33] >> 4;
props->port_cap_flags = be32_to_cpup((__be32 *) (out_mad->data + 20));
props->gid_tbl_len = to_mdev(ibdev)->dev->caps.gid_table_len[port];
props->max_msg_sz = to_mdev(ibdev)->dev->caps.max_msg_sz;
props->pkey_tbl_len = to_mdev(ibdev)->dev->caps.pkey_table_len[port];
props->bad_pkey_cntr = be16_to_cpup((__be16 *) (out_mad->data + 46));
props->qkey_viol_cntr = be16_to_cpup((__be16 *) (out_mad->data + 48));
props->active_width = out_mad->data[31] & 0xf;
props->active_speed = out_mad->data[35] >> 4;
props->max_mtu = out_mad->data[41] & 0xf;
props->active_mtu = out_mad->data[36] >> 4;
props->subnet_timeout = out_mad->data[51] & 0x1f;
props->max_vl_num = out_mad->data[37] >> 4;
props->init_type_reply = out_mad->data[41] >> 4;
/* Check if extended speeds (EDR/FDR/...) are supported */
if (props->port_cap_flags & IB_PORT_EXTENDED_SPEEDS_SUP) {
ext_active_speed = out_mad->data[62] >> 4;
switch (ext_active_speed) {
case 1:
props->active_speed = 16; /* FDR */
break;
case 2:
props->active_speed = 32; /* EDR */
break;
}
}
static u64 value_read(int offset, int size, void *structure)
{
switch (size) {
case 1: return *(u8 *) (structure + offset);
case 2: return be16_to_cpup((__be16 *) (structure + offset));
case 4: return be32_to_cpup((__be32 *) (structure + offset));
case 8: return be64_to_cpup((__be64 *) (structure + offset));
default:
printk(KERN_WARNING "Field size %d bits not handled\n", size * 8);
return 0;
}
}
static int inv_selftest_read_samples(struct inv_mpu_state *st, enum INV_SENSORS
type, int *sum_result, int *s)
{
u8 w;
u16 fifo_count;
s16 vals[3];
u8 d[BYTES_PER_SENSOR];
int r, i, j, packet_count;
if (SENSOR_GYRO == type)
w = (BIT_MULTI_FIFO_CFG | BIT_GYRO_FIFO_NUM);
else
w = (BIT_MULTI_FIFO_CFG | BIT_ACCEL_FIFO_NUM);
r = inv_plat_single_write(st, REG_FIFO_CFG, w);
if (r)
return r;
r = inv_plat_read(st, REG_FIFO_COUNT_H, FIFO_COUNT_BYTE, d);
if (r)
return r;
fifo_count = be16_to_cpup((__be16 *)(&d[0]));
pr_debug("self_test fifo_count - %d\n", fifo_count);
packet_count = fifo_count / BYTES_PER_SENSOR;
i = 0;
while ((i < packet_count) && (*s < DEF_ST_SAMPLES)) {
r = inv_plat_read(st, REG_FIFO_R_W, BYTES_PER_SENSOR, d);
if (r)
return r;
for (j = 0; j < THREE_AXES; j++) {
vals[j] = (s16)be16_to_cpup((__be16 *)(&d[2 * j]));
sum_result[j] += vals[j];
}
pr_debug("self_test data - %d %+d %+d %+d",
*s, vals[0], vals[1], vals[2]);
(*s)++;
i++;
}
return 0;
}
/**
* of_property_read_u16_array - Find and read an array of u16 from a property.
*
* @np: device node from which the property value is to be read.
* @propname: name of the property to be searched.
* @out_value: pointer to return value, modified only if return value is 0.
* @sz: number of array elements to read
*
* Search for a property in a device node and read 16-bit value(s) from
* it. Returns 0 on success, -EINVAL if the property does not exist,
* -ENODATA if property does not have a value, and -EOVERFLOW if the
* property data isn't large enough.
*
* dts entry of array should be like:
* property = /bits/ 16 <0x5000 0x6000 0x7000>;
*
* The out_value is modified only if a valid u16 value can be decoded.
*/
int of_property_read_u16_array(const struct device_node *np,
const char *propname, u16 *out_values, size_t sz)
{
const __be16 *val = of_find_property_value_of_size(np, propname,
(sz * sizeof(*out_values)));
if (IS_ERR(val))
return PTR_ERR(val);
while (sz--)
*out_values++ = be16_to_cpup(val++);
return 0;
}
/* sysfs attributes */
static struct shtc1_data *shtc1_update_client(struct device *dev) {
struct i2c_client *client = to_i2c_client(dev);
struct shtc1_data *data = i2c_get_clientdata(client);
unsigned char buf[SHTC1_RESPONSE_LENGTH];
int val;
int ret;
mutex_lock(&data->update_lock);
/*
* Initialize 'ret' in case we had a valid result before, but
* read too quickly in which case we return the last values.
*/
ret = !data->valid;
if (time_after(jiffies, data->last_updated + HZ / 10) || !data->valid) {
ret = shtc1_update_values(client, data, buf, sizeof (buf));
if (ret)
goto out;
/*
* From datasheet:
* T = -45 + 175 * ST / 2^16
* RH = 100 * SRH / 2^16
*
* Adapted for integer fixed point (3 digit) arithmetic.
*/
val = be16_to_cpup((__be16 *) buf);
data->temperature = ((21875 * val) >> 13) - 45000;
val = be16_to_cpup((__be16 *) (buf + 3));
data->humidity = ((12500 * val) >> 13);
data->last_updated = jiffies;
data->valid = 1;
}
static int elan_smbus_get_product_id(struct i2c_client *client, u16 *id)
{
int error;
u8 val[I2C_SMBUS_BLOCK_MAX] = {0};
error = i2c_smbus_read_block_data(client,
ETP_SMBUS_UNIQUEID_CMD, val);
if (error < 0) {
dev_err(&client->dev, "failed to get product ID: %d\n", error);
return error;
}
*id = be16_to_cpup((__be16 *)val);
return 0;
}
static int elan_smbus_get_baseline_data(struct i2c_client *client,
bool max_baseline, u8 *value)
{
int error;
u8 val[3];
error = i2c_smbus_read_block_data(client,
max_baseline ?
ETP_SMBUS_MAX_BASELINE_CMD :
ETP_SMBUS_MIN_BASELINE_CMD,
val);
if (error < 0)
return error;
*value = be16_to_cpup((__be16 *)val);
return 0;
}
static int elan_smbus_get_checksum(struct i2c_client *client,
bool iap, u16 *csum)
{
int error;
u8 val[3];
error = i2c_smbus_read_block_data(client,
iap ? ETP_SMBUS_FW_CHECKSUM_CMD :
ETP_SMBUS_IAP_CHECKSUM_CMD,
val);
if (error < 0) {
dev_err(&client->dev, "failed to get %s checksum: %d\n",
iap ? "IAP" : "FW", error);
return error;
}
*csum = be16_to_cpup((__be16 *)val);
return 0;
}
static int ade7759_spi_read_reg_16(struct device *dev,
u8 reg_address,
u16 *val)
{
struct iio_dev *indio_dev = dev_get_drvdata(dev);
struct ade7759_state *st = iio_dev_get_devdata(indio_dev);
int ret;
ret = spi_w8r16(st->us, ADE7759_READ_REG(reg_address));
if (ret < 0) {
dev_err(&st->us->dev, "problem when reading 16 bit register 0x%02X",
reg_address);
return ret;
}
*val = ret;
*val = be16_to_cpup(val);
return 0;
}
static int elan_smbus_iap_get_mode(struct i2c_client *client,
enum tp_mode *mode)
{
int error;
u16 constant;
u8 val[3];
error = i2c_smbus_read_block_data(client, ETP_SMBUS_IAP_CTRL_CMD, val);
if (error < 0) {
dev_err(&client->dev, "failed to read iap ctrol register: %d\n",
error);
return error;
}
constant = be16_to_cpup((__be16 *)val);
dev_dbg(&client->dev, "iap control reg: 0x%04x.\n", constant);
*mode = (constant & ETP_SMBUS_IAP_MODE_ON) ? IAP_MODE : MAIN_MODE;
return 0;
}
static int regmap_mmio_gather_write(void *context,
const void *reg, size_t reg_size,
const void *val, size_t val_size)
{
struct regmap_mmio_context *ctx = context;
u32 offset;
BUG_ON(reg_size != 4);
offset = be32_to_cpup(reg);
while (val_size) {
switch (ctx->val_bytes) {
case 1:
writeb(*(u8 *)val, ctx->regs + offset);
break;
case 2:
writew(be16_to_cpup(val), ctx->regs + offset);
break;
case 4:
writel(be32_to_cpup(val), ctx->regs + offset);
break;
#ifdef CONFIG_64BIT
case 8:
writeq(be64_to_cpup(val), ctx->regs + offset);
break;
#endif
default:
/* Should be caught by regmap_mmio_check_config */
BUG();
}
val_size -= ctx->val_bytes;
val += ctx->val_bytes;
offset += ctx->val_bytes;
}
return 0;
}
void ieee80211_aes_ccm_encrypt(struct crypto_aead *tfm, u8 *b_0, u8 *aad,
u8 *data, size_t data_len, u8 *mic)
{
struct scatterlist assoc, pt, ct[2];
char aead_req_data[sizeof(struct aead_request) +
crypto_aead_reqsize(tfm)]
__aligned(__alignof__(struct aead_request));
struct aead_request *aead_req = (void *) aead_req_data;
memset(aead_req, 0, sizeof(aead_req_data));
sg_init_one(&pt, data, data_len);
sg_init_one(&assoc, &aad[2], be16_to_cpup((__be16 *)aad));
sg_init_table(ct, 2);
sg_set_buf(&ct[0], data, data_len);
sg_set_buf(&ct[1], mic, IEEE80211_CCMP_MIC_LEN);
aead_request_set_tfm(aead_req, tfm);
aead_request_set_assoc(aead_req, &assoc, assoc.length);
aead_request_set_crypt(aead_req, &pt, ct, data_len, b_0);
crypto_aead_encrypt(aead_req);
}
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device
* specific to be rolled into the core.
*/
static void adis16209_trigger_bh_to_ring(struct work_struct *work_s)
{
struct adis16209_state *st
= container_of(work_s, struct adis16209_state,
work_trigger_to_ring);
int i = 0;
s16 *data;
size_t datasize = st->indio_dev
->ring->access.get_bpd(st->indio_dev->ring);
data = kmalloc(datasize , GFP_KERNEL);
if (data == NULL) {
dev_err(&st->us->dev, "memory alloc failed in ring bh");
return;
}
if (st->indio_dev->scan_count)
if (adis16209_read_ring_data(&st->indio_dev->dev, st->rx) >= 0)
for (; i < st->indio_dev->scan_count; i++)
data[i] = be16_to_cpup(
(__be16 *)&(st->rx[i*2]));
/* Guaranteed to be aligned with 8 byte boundary */
if (st->indio_dev->scan_timestamp)
*((s64 *)(data + ((i + 3)/4)*4)) = st->last_timestamp;
st->indio_dev->ring->access.store_to(st->indio_dev->ring,
(u8 *)data,
st->last_timestamp);
iio_trigger_notify_done(st->indio_dev->trig);
kfree(data);
return;
}
void ieee80211_aes_ccm_encrypt(struct crypto_aead *tfm, u8 *b_0, u8 *aad,
u8 *data, size_t data_len, u8 *mic,
size_t mic_len)
{
struct scatterlist sg[3];
char aead_req_data[sizeof(struct aead_request) +
crypto_aead_reqsize(tfm)]
__aligned(__alignof__(struct aead_request));
struct aead_request *aead_req = (void *) aead_req_data;
memset(aead_req, 0, sizeof(aead_req_data));
sg_init_table(sg, 3);
sg_set_buf(&sg[0], &aad[2], be16_to_cpup((__be16 *)aad));
sg_set_buf(&sg[1], data, data_len);
sg_set_buf(&sg[2], mic, mic_len);
aead_request_set_tfm(aead_req, tfm);
aead_request_set_crypt(aead_req, sg, sg, data_len, b_0);
aead_request_set_ad(aead_req, sg[0].length);
crypto_aead_encrypt(aead_req);
}
/**
* of_property_read_variable_u16_array - Find and read an array of u16 from a
* property, with bounds on the minimum and maximum array size.
*
* @np: device node from which the property value is to be read.
* @propname: name of the property to be searched.
* @out_values: pointer to return value, modified only if return value is 0.
* @sz_min: minimum number of array elements to read
* @sz_max: maximum number of array elements to read, if zero there is no
* upper limit on the number of elements in the dts entry but only
* sz_min will be read.
*
* Search for a property in a device node and read 16-bit value(s) from
* it. Returns number of elements read on success, -EINVAL if the property
* does not exist, -ENODATA if property does not have a value, and -EOVERFLOW
* if the property data is smaller than sz_min or longer than sz_max.
*
* dts entry of array should be like:
* property = /bits/ 16 <0x5000 0x6000 0x7000>;
*
* The out_values is modified only if a valid u16 value can be decoded.
*/
int of_property_read_variable_u16_array(const struct device_node *np,
const char *propname, u16 *out_values,
size_t sz_min, size_t sz_max)
{
size_t sz, count;
const __be16 *val = of_find_property_value_of_size(np, propname,
(sz_min * sizeof(*out_values)),
(sz_max * sizeof(*out_values)),
&sz);
if (IS_ERR(val))
return PTR_ERR(val);
if (!sz_max)
sz = sz_min;
else
sz /= sizeof(*out_values);
count = sz;
while (count--)
*out_values++ = be16_to_cpup(val++);
return sz;
}
static int mpu6050_do_test(struct i2c_client *client, int self_test_flag,
int *gyro_result)
{
int result, i, j, packet_size;
u8 data[BYTES_PER_SENSOR * 2], d;
int fifo_count, packet_count, ind, s;
packet_size = BYTES_PER_SENSOR;
result = mpu6050_i2c_write_single_reg(client, MPUREG_INT_ENABLE, 0);
if (result)
return result;
/* disable the sensor output to FIFO */
result = mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, 0);
if (result)
return result;
/* disable fifo reading */
result = mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL, 0);
if (result)
return result;
/* clear FIFO */
result = mpu6050_i2c_write_single_reg(client,
MPUREG_USER_CTRL, BIT_FIFO_RST);
if (result)
return result;
/* setup parameters */
result = mpu6050_i2c_write_single_reg(client, MPUREG_CONFIG,
MPU_FILTER_188HZ);
if (result)
return result;
result = mpu6050_i2c_write_single_reg(client, MPUREG_SMPLRT_DIV,
0x0);
if (result)
return result;
result = mpu6050_i2c_write_single_reg(client, MPUREG_GYRO_CONFIG,
self_test_flag | (MPU_FS_250DPS << 3));
if (result)
return result;
/* wait for the output to get stable */
msleep(DEF_ST_STABLE_TIME);
/* enable FIFO reading */
result = mpu6050_i2c_write_single_reg(client, MPUREG_USER_CTRL,
BIT_FIFO_EN);
if (result)
return result;
/* enable sensor output to FIFO */
d = BITS_GYRO_OUT;
result = mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, d);
if (result)
return result;
for (i = 0; i < THREE_AXIS; i++)
gyro_result[i] = 0;
s = 0;
while (s < INIT_ST_SAMPLES) {
msleep(DEF_GYRO_WAIT_TIME);
result = mpu6050_i2c_read_reg(client, MPUREG_FIFO_COUNTH,
FIFO_COUNT_BYTE, data);
if (result)
return result;
fifo_count = be16_to_cpup((__be16 *)(&data[0]));
packet_count = fifo_count / packet_size;
result = mpu6050_i2c_read_reg(client, MPUREG_FIFO_R_W,
packet_size, data);
if (result)
return result;
i = 0;
while ((i < packet_count) && (s < INIT_ST_SAMPLES)) {
result = mpu6050_i2c_read_reg(client, MPUREG_FIFO_R_W,
packet_size, data);
if (result)
return result;
ind = 0;
for (j = 0; j < THREE_AXIS; j++)
gyro_result[j] +=
(short)be16_to_cpup(
(__be16 *)(&data[ind + 2 * j]));
s++;
i++;
}
}
/* stop sending data to FIFO */
result = mpu6050_i2c_write_single_reg(client, MPUREG_FIFO_EN, 0);
if (result)
return result;
for (j = 0; j < THREE_AXIS; j++) {
gyro_result[j] = gyro_result[j]/s;
gyro_result[j] *= DEF_ST_PRECISION;
}
return 0;
}
