motorola_powerpc/start/residual.c

0001 /*
0002  * residual.c : function used to parse residual data.
0003  *
0004  *  CopyRight (C) 1999 valette@crf.canon.fr
0005  *
0006  *  This code is heavilly inspired by the public specification of STREAM V2
0007  *  that can be found at :
0008  *
0009  *  The license and distribution terms for this file may be
0010  *  found in the file LICENSE in this distribution or at
0011  *  http://www.rtems.org/license/LICENSE.
0012  */
0013
0014 #include <string.h>
0015
0016 #include <bsp/residual.h>
0017 #include <libcpu/io.h>
0018 #include <libcpu/byteorder.h>
0019
0020 static int same_DevID(unsigned short vendor,
0021            unsigned short Number,
0022            unsigned char * str)
0023 {
0024     static unsigned const char hexdigit[]="0123456789ABCDEF";
0025     if (strlen((char*)str)!=7) return 0;
0026     if ( ( ((vendor>>10)&0x1f)+'A'-1 == str[0])  &&
0027          ( ((vendor>>5)&0x1f)+'A'-1 == str[1])   &&
0028          ( (vendor&0x1f)+'A'-1 == str[2])        &&
0029          (hexdigit[(Number>>12)&0x0f] == str[3]) &&
0030          (hexdigit[(Number>>8)&0x0f] == str[4])  &&
0031          (hexdigit[(Number>>4)&0x0f] == str[5])  &&
0032          (hexdigit[Number&0x0f] == str[6]) ) return 1;
0033     return 0;
0034 }
0035
0036 PPC_DEVICE *residual_find_device(RESIDUAL *res,unsigned long BusMask,
0037                  unsigned char * DevID,
0038                  int BaseType,
0039                  int SubType,
0040                  int Interface,
0041                  int n)
0042 {
0043     int i;
0044     if ( !res || !res->ResidualLength ) return NULL;
0045     for (i=0; i<res->ActualNumDevices; i++) {
0046 #define Dev res->Devices[i].DeviceId
0047         if ( (Dev.BusId&BusMask)                                  &&
0048              (BaseType==-1 || Dev.BaseType==BaseType)             &&
0049              (SubType==-1 || Dev.SubType==SubType)                &&
0050              (Interface==-1 || Dev.Interface==Interface)          &&
0051              (DevID==NULL || same_DevID((Dev.DevId>>16)&0xffff,
0052                         Dev.DevId&0xffff, DevID)) &&
0053              !(n--) ) return res->Devices+i;
0054 #undef Dev
0055     }
0056     return 0;
0057 }
0058
0059 PnP_TAG_PACKET *PnP_find_packet(unsigned char *p,
0060                 unsigned packet_tag,
0061                 int n)
0062 {
0063     unsigned mask, masked_tag, size;
0064     if(!p) return 0;
0065     if (tag_type(packet_tag)) mask=0xff; else mask=0xF8;
0066     masked_tag = packet_tag&mask;
0067     for(; *p != END_TAG; p+=size) {
0068         if ((*p & mask) == masked_tag && !(n--))
0069             return (PnP_TAG_PACKET *) p;
0070         if (tag_type(*p))
0071             size=ld_le16((unsigned short *)(p+1))+3;
0072         else
0073             size=tag_small_count(*p)+1;
0074     }
0075     return 0; /* not found */
0076 }
0077
0078 PnP_TAG_PACKET *PnP_find_small_vendor_packet(unsigned char *p,
0079                          unsigned packet_type,
0080                          int n)
0081 {
0082     int next=0;
0083     while (p) {
0084         p = (unsigned char *) PnP_find_packet(p, 0x70, next);
0085         if (p && p[1]==packet_type && !(n--))
0086             return (PnP_TAG_PACKET *) p;
0087         next = 1;
0088     };
0089     return 0; /* not found */
0090 }
0091
0092 PnP_TAG_PACKET *PnP_find_large_vendor_packet(unsigned char *p,
0093                        unsigned packet_type,
0094                        int n)
0095 {
0096     int next=0;
0097     while (p) {
0098         p = (unsigned char *) PnP_find_packet(p, 0x84, next);
0099         if (p && p[3]==packet_type && !(n--))
0100             return (PnP_TAG_PACKET *) p;
0101         next = 1;
0102     };
0103     return 0; /* not found */
0104 }