1 /* $OpenBSD: data.c,v 1.8 2014/06/04 12:13:30 mpi Exp $ */
2 /* $NetBSD: data.c,v 1.1 2001/12/28 17:45:26 augustss Exp $ */
3
4 /*
5 * Copyright (c) 1999 Lennart Augustsson <augustss@netbsd.org>
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 * 1. Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * 2. Redistributions in binary form must reproduce the above copyright
14 * notice, this list of conditions and the following disclaimer in the
15 * documentation and/or other materials provided with the distribution.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
18 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27 * SUCH DAMAGE.
28 */
29
30 #include <stdlib.h>
31 #include "usbhid.h"
32
33 int32_t
hid_get_data(const void * p,const hid_item_t * h)34 hid_get_data(const void *p, const hid_item_t *h)
35 {
36 const uint8_t *buf;
37 uint32_t hpos;
38 uint32_t hsize;
39 uint32_t data;
40 int i, end, offs;
41
42 buf = p;
43
44 #if 0
45 /* Skip report ID byte. */
46 if (h->report_ID > 0)
47 buf++;
48 #endif
49
50 hpos = h->pos; /* bit position of data */
51 hsize = h->report_size; /* bit length of data */
52
53 /* Range check and limit */
54 if (hsize == 0)
55 return (0);
56 if (hsize > 32)
57 hsize = 32;
58
59 offs = hpos / 8;
60 end = (hpos + hsize) / 8 - offs;
61 data = 0;
62 for (i = 0; i <= end; i++)
63 data |= buf[offs + i] << (i*8);
64
65 /* Correctly shift down data */
66 data >>= hpos % 8;
67 hsize = 32 - hsize;
68
69 /* Mask and sign extend in one */
70 if ((h->logical_minimum < 0) || (h->logical_maximum < 0))
71 data = (int32_t)((int32_t)data << hsize) >> hsize;
72 else
73 data = (uint32_t)((uint32_t)data << hsize) >> hsize;
74
75 return (data);
76 }
77
78 void
hid_set_data(void * p,const hid_item_t * h,int32_t data)79 hid_set_data(void *p, const hid_item_t *h, int32_t data)
80 {
81 uint8_t *buf;
82 uint32_t hpos;
83 uint32_t hsize;
84 uint32_t mask;
85 int i;
86 int end;
87 int offs;
88
89 buf = p;
90
91 #if 0
92 /* Set report ID byte. */
93 if (h->report_ID > 0)
94 *buf++ = h->report_ID & 0xff;
95 #endif
96
97 hpos = h->pos; /* bit position of data */
98 hsize = h->report_size; /* bit length of data */
99
100 if (hsize != 32) {
101 mask = (1 << hsize) - 1;
102 data &= mask;
103 } else
104 mask = ~0;
105
106 data <<= (hpos % 8);
107 mask <<= (hpos % 8);
108 mask = ~mask;
109
110 offs = hpos / 8;
111 end = (hpos + hsize) / 8 - offs;
112
113 for (i = 0; i <= end; i++)
114 buf[offs + i] = (buf[offs + i] & (mask >> (i*8))) |
115 ((data >> (i*8)) & 0xff);
116 }
117