data.c revision 205728
11553Srgrimes/* $NetBSD: data.c,v 1.8 2000/04/02 11:10:53 augustss Exp $ */ 21553Srgrimes 31553Srgrimes/* 41553Srgrimes * Copyright (c) 1999 Lennart Augustsson <augustss@netbsd.org> 51553Srgrimes * All rights reserved. 61553Srgrimes * 71553Srgrimes * Redistribution and use in source and binary forms, with or without 81553Srgrimes * modification, are permitted provided that the following conditions 91553Srgrimes * are met: 101553Srgrimes * 1. Redistributions of source code must retain the above copyright 111553Srgrimes * notice, this list of conditions and the following disclaimer. 121553Srgrimes * 2. Redistributions in binary form must reproduce the above copyright 131553Srgrimes * notice, this list of conditions and the following disclaimer in the 141553Srgrimes * documentation and/or other materials provided with the distribution. 151553Srgrimes * 161553Srgrimes * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 171553Srgrimes * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 181553Srgrimes * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 191553Srgrimes * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 201553Srgrimes * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 211553Srgrimes * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 221553Srgrimes * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 231553Srgrimes * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 241553Srgrimes * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 251553Srgrimes * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 261553Srgrimes * SUCH DAMAGE. 271553Srgrimes */ 281553Srgrimes 291553Srgrimes#include <sys/cdefs.h> 301553Srgrimes__FBSDID("$FreeBSD: head/lib/libusbhid/data.c 205728 2010-03-27 08:00:16Z kaiw $"); 311553Srgrimes 321553Srgrimes#include <sys/param.h> 331553Srgrimes#include <assert.h> 341553Srgrimes#include <stdlib.h> 351553Srgrimes#include "usbhid.h" 3631492Swollman 371553Srgrimesint 381553Srgrimeshid_get_data(const void *p, const hid_item_t *h) 391553Srgrimes{ 401553Srgrimes const uint8_t *buf; 411553Srgrimes uint32_t hpos; 4231492Swollman uint32_t hsize; 4315648Sjoerg uint32_t data; 4431492Swollman int i, end, offs; 4531492Swollman 4650479Speter buf = p; 471553Srgrimes 481553Srgrimes /* Skip report ID byte. */ 491553Srgrimes if (h->report_ID > 0) 501553Srgrimes buf++; 511553Srgrimes 521553Srgrimes hpos = h->pos; /* bit position of data */ 531553Srgrimes hsize = h->report_size; /* bit length of data */ 541553Srgrimes 551553Srgrimes /* Range check and limit */ 561553Srgrimes if (hsize == 0) 571553Srgrimes return (0); 581553Srgrimes if (hsize > 32) 591553Srgrimes hsize = 32; 601553Srgrimes 611553Srgrimes offs = hpos / 8; 621553Srgrimes end = (hpos + hsize) / 8 - offs; 631553Srgrimes data = 0; 641553Srgrimes for (i = 0; i <= end; i++) 651553Srgrimes data |= buf[offs + i] << (i*8); 661553Srgrimes data >>= hpos % 8; 671553Srgrimes data &= (1 << hsize) - 1; 681553Srgrimes if (h->logical_minimum < 0) { 691553Srgrimes /* Need to sign extend */ 701553Srgrimes hsize = sizeof data * 8 - hsize; 711553Srgrimes data = (data << hsize) >> hsize; 7215032Ssef } 7315032Ssef return (data); 7415703Sjoerg} 751553Srgrimes 761553Srgrimesvoid 771553Srgrimeshid_set_data(void *p, const hid_item_t *h, int data) 781553Srgrimes{ 791553Srgrimes uint8_t *buf; 8080230Sgad uint32_t hpos; 8180230Sgad uint32_t hsize; 821553Srgrimes int i, end, offs, mask; 831553Srgrimes 841553Srgrimes buf = p; 851553Srgrimes 861553Srgrimes /* Set report ID byte. */ 871553Srgrimes if (h->report_ID > 0) 881553Srgrimes *buf++ = h->report_ID & 0xff; 891553Srgrimes 901553Srgrimes hpos = h->pos; /* bit position of data */ 911553Srgrimes hsize = h->report_size; /* bit length of data */ 921553Srgrimes 931553Srgrimes if (hsize != 32) { 941553Srgrimes mask = (1 << hsize) - 1; 951553Srgrimes data &= mask; 961553Srgrimes } else 971553Srgrimes mask = ~0; 9868253Sgad 991553Srgrimes data <<= (hpos % 8); 1001553Srgrimes mask <<= (hpos % 8); 1011553Srgrimes mask = ~mask; 10224831Sbrian 1031553Srgrimes offs = hpos / 8; 1041553Srgrimes end = (hpos + hsize) / 8 - offs; 1051553Srgrimes 1061553Srgrimes for (i = 0; i <= end; i++) 10753956Sache buf[offs + i] = (buf[offs + i] & (mask >> (i*8))) | 1081553Srgrimes ((data >> (i*8)) & 0xff); 10980230Sgad} 11080230Sgad