source: rtems/bsps/powerpc/motorola_powerpc/start/residual.c @ 173e157

Last change on this file since 173e157 was 173e157, checked in by Sebastian Huber <sebastian.huber@…>, on Apr 24, 2018 at 5:23:19 AM

bsps: Move residual.c to bsps

This patch is a part of the BSP source reorganization.

Update #3285.

  • Property mode set to 100644
File size: 2.8 KB
Line 
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 the file LICENSE in this distribution or at
11 *  http://www.rtems.org/license/LICENSE.
12 */
13
14#include <string.h>
15
16#include <bsp/residual.h>
17#include <libcpu/io.h>
18#include <libcpu/byteorder.h>
19
20static int same_DevID(unsigned short vendor,
21               unsigned short Number,
22               unsigned char * str)
23{
24        static unsigned const char hexdigit[]="0123456789ABCDEF";
25        if (strlen((char*)str)!=7) return 0;
26        if ( ( ((vendor>>10)&0x1f)+'A'-1 == str[0])  &&
27             ( ((vendor>>5)&0x1f)+'A'-1 == str[1])   &&
28             ( (vendor&0x1f)+'A'-1 == str[2])        &&
29             (hexdigit[(Number>>12)&0x0f] == str[3]) &&
30             (hexdigit[(Number>>8)&0x0f] == str[4])  &&
31             (hexdigit[(Number>>4)&0x0f] == str[5])  &&
32             (hexdigit[Number&0x0f] == str[6]) ) return 1;
33        return 0;
34}
35
36PPC_DEVICE *residual_find_device(RESIDUAL *res,unsigned long BusMask,
37                                 unsigned char * DevID,
38                                 int BaseType,
39                                 int SubType,
40                                 int Interface,
41                                 int n)
42{
43        int i;
44        if ( !res || !res->ResidualLength ) return NULL;
45        for (i=0; i<res->ActualNumDevices; i++) {
46#define Dev res->Devices[i].DeviceId
47                if ( (Dev.BusId&BusMask)                                  &&
48                     (BaseType==-1 || Dev.BaseType==BaseType)             &&
49                     (SubType==-1 || Dev.SubType==SubType)                &&
50                     (Interface==-1 || Dev.Interface==Interface)          &&
51                     (DevID==NULL || same_DevID((Dev.DevId>>16)&0xffff,
52                                                Dev.DevId&0xffff, DevID)) &&
53                     !(n--) ) return res->Devices+i;
54#undef Dev
55        }
56        return 0;
57}
58
59PnP_TAG_PACKET *PnP_find_packet(unsigned char *p,
60                                unsigned packet_tag,
61                                int n)
62{
63        unsigned mask, masked_tag, size;
64        if(!p) return 0;
65        if (tag_type(packet_tag)) mask=0xff; else mask=0xF8;
66        masked_tag = packet_tag&mask;
67        for(; *p != END_TAG; p+=size) {
68                if ((*p & mask) == masked_tag && !(n--))
69                        return (PnP_TAG_PACKET *) p;
70                if (tag_type(*p))
71                        size=ld_le16((unsigned short *)(p+1))+3;
72                else
73                        size=tag_small_count(*p)+1;
74        }
75        return 0; /* not found */
76}
77
78PnP_TAG_PACKET *PnP_find_small_vendor_packet(unsigned char *p,
79                                             unsigned packet_type,
80                                             int n)
81{
82        int next=0;
83        while (p) {
84                p = (unsigned char *) PnP_find_packet(p, 0x70, next);
85                if (p && p[1]==packet_type && !(n--))
86                        return (PnP_TAG_PACKET *) p;
87                next = 1;
88        };
89        return 0; /* not found */
90}
91
92PnP_TAG_PACKET *PnP_find_large_vendor_packet(unsigned char *p,
93                                           unsigned packet_type,
94                                           int n)
95{
96        int next=0;
97        while (p) {
98                p = (unsigned char *) PnP_find_packet(p, 0x84, next);
99                if (p && p[3]==packet_type && !(n--))
100                        return (PnP_TAG_PACKET *) p;
101                next = 1;
102        };
103        return 0; /* not found */
104}
Note: See TracBrowser for help on using the repository browser.