1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
|
/*
* residual.c : function used to parse residual data.
*
* CopyRight (C) 1999 valette@crf.canon.fr
*
* This code is heavilly inspired by the public specification of STREAM V2
* that can be found at :
*
* The license and distribution terms for this file may be
* found in found in the file LICENSE in this distribution or at
* http://www.rtems.com/license/LICENSE.
*
* $Id$
*/
#include <string.h>
#include <bsp/residual.h>
#include <libcpu/io.h>
#include <libcpu/byteorder.h>
static int same_DevID(unsigned short vendor,
unsigned short Number,
char * str)
{
static unsigned const char hexdigit[]="0123456789ABCDEF";
if (strlen(str)!=7) return 0;
if ( ( ((vendor>>10)&0x1f)+'A'-1 == str[0]) &&
( ((vendor>>5)&0x1f)+'A'-1 == str[1]) &&
( (vendor&0x1f)+'A'-1 == str[2]) &&
(hexdigit[(Number>>12)&0x0f] == str[3]) &&
(hexdigit[(Number>>8)&0x0f] == str[4]) &&
(hexdigit[(Number>>4)&0x0f] == str[5]) &&
(hexdigit[Number&0x0f] == str[6]) ) return 1;
return 0;
}
PPC_DEVICE *residual_find_device(RESIDUAL *res,unsigned long BusMask,
unsigned char * DevID,
int BaseType,
int SubType,
int Interface,
int n)
{
int i;
if ( !res || !res->ResidualLength ) return NULL;
for (i=0; i<res->ActualNumDevices; i++) {
#define Dev res->Devices[i].DeviceId
if ( (Dev.BusId&BusMask) &&
(BaseType==-1 || Dev.BaseType==BaseType) &&
(SubType==-1 || Dev.SubType==SubType) &&
(Interface==-1 || Dev.Interface==Interface) &&
(DevID==NULL || same_DevID((Dev.DevId>>16)&0xffff,
Dev.DevId&0xffff, DevID)) &&
!(n--) ) return res->Devices+i;
#undef Dev
}
return 0;
}
PnP_TAG_PACKET *PnP_find_packet(unsigned char *p,
unsigned packet_tag,
int n)
{
unsigned mask, masked_tag, size;
if(!p) return 0;
if (tag_type(packet_tag)) mask=0xff; else mask=0xF8;
masked_tag = packet_tag&mask;
for(; *p != END_TAG; p+=size) {
if ((*p & mask) == masked_tag && !(n--))
return (PnP_TAG_PACKET *) p;
if (tag_type(*p))
size=ld_le16((unsigned short *)(p+1))+3;
else
size=tag_small_count(*p)+1;
}
return 0; /* not found */
}
PnP_TAG_PACKET *PnP_find_small_vendor_packet(unsigned char *p,
unsigned packet_type,
int n)
{
int next=0;
while (p) {
p = (unsigned char *) PnP_find_packet(p, 0x70, next);
if (p && p[1]==packet_type && !(n--))
return (PnP_TAG_PACKET *) p;
next = 1;
};
return 0; /* not found */
}
PnP_TAG_PACKET *PnP_find_large_vendor_packet(unsigned char *p,
unsigned packet_type,
int n)
{
int next=0;
while (p) {
p = (unsigned char *) PnP_find_packet(p, 0x84, next);
if (p && p[3]==packet_type && !(n--))
return (PnP_TAG_PACKET *) p;
next = 1;
};
return 0; /* not found */
}
|