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