/*
|
/*
|
* residual.c : function used to parse residual data.
|
* residual.c : function used to parse residual data.
|
*
|
*
|
* CopyRight (C) 1999 valette@crf.canon.fr
|
* CopyRight (C) 1999 valette@crf.canon.fr
|
*
|
*
|
* This code is heavilly inspired by the public specification of STREAM V2
|
* This code is heavilly inspired by the public specification of STREAM V2
|
* that can be found at :
|
* that can be found at :
|
*
|
*
|
* The license and distribution terms for this file may be
|
* The license and distribution terms for this file may be
|
* found in found in the file LICENSE in this distribution or at
|
* found in found in the file LICENSE in this distribution or at
|
* http://www.OARcorp.com/rtems/license.html.
|
* http://www.OARcorp.com/rtems/license.html.
|
*
|
*
|
* $Id: residual.c,v 1.2 2001-09-27 12:01:07 chris Exp $
|
* $Id: residual.c,v 1.2 2001-09-27 12:01:07 chris Exp $
|
*/
|
*/
|
|
|
#include <bsp/residual.h>
|
#include <bsp/residual.h>
|
#include <libcpu/io.h>
|
#include <libcpu/io.h>
|
#include <libcpu/byteorder.h>
|
#include <libcpu/byteorder.h>
|
|
|
|
|
static int same_DevID(unsigned short vendor,
|
static int same_DevID(unsigned short vendor,
|
unsigned short Number,
|
unsigned short Number,
|
char * str)
|
char * str)
|
{
|
{
|
static unsigned const char hexdigit[]="0123456789ABCDEF";
|
static unsigned const char hexdigit[]="0123456789ABCDEF";
|
if (strlen(str)!=7) return 0;
|
if (strlen(str)!=7) return 0;
|
if ( ( ((vendor>>10)&0x1f)+'A'-1 == str[0]) &&
|
if ( ( ((vendor>>10)&0x1f)+'A'-1 == str[0]) &&
|
( ((vendor>>5)&0x1f)+'A'-1 == str[1]) &&
|
( ((vendor>>5)&0x1f)+'A'-1 == str[1]) &&
|
( (vendor&0x1f)+'A'-1 == str[2]) &&
|
( (vendor&0x1f)+'A'-1 == str[2]) &&
|
(hexdigit[(Number>>12)&0x0f] == str[3]) &&
|
(hexdigit[(Number>>12)&0x0f] == str[3]) &&
|
(hexdigit[(Number>>8)&0x0f] == str[4]) &&
|
(hexdigit[(Number>>8)&0x0f] == str[4]) &&
|
(hexdigit[(Number>>4)&0x0f] == str[5]) &&
|
(hexdigit[(Number>>4)&0x0f] == str[5]) &&
|
(hexdigit[Number&0x0f] == str[6]) ) return 1;
|
(hexdigit[Number&0x0f] == str[6]) ) return 1;
|
return 0;
|
return 0;
|
}
|
}
|
|
|
PPC_DEVICE *residual_find_device(RESIDUAL *res,unsigned long BusMask,
|
PPC_DEVICE *residual_find_device(RESIDUAL *res,unsigned long BusMask,
|
unsigned char * DevID,
|
unsigned char * DevID,
|
int BaseType,
|
int BaseType,
|
int SubType,
|
int SubType,
|
int Interface,
|
int Interface,
|
int n)
|
int n)
|
{
|
{
|
int i;
|
int i;
|
if ( !res || !res->ResidualLength ) return NULL;
|
if ( !res || !res->ResidualLength ) return NULL;
|
for (i=0; i<res->ActualNumDevices; i++) {
|
for (i=0; i<res->ActualNumDevices; i++) {
|
#define Dev res->Devices[i].DeviceId
|
#define Dev res->Devices[i].DeviceId
|
if ( (Dev.BusId&BusMask) &&
|
if ( (Dev.BusId&BusMask) &&
|
(BaseType==-1 || Dev.BaseType==BaseType) &&
|
(BaseType==-1 || Dev.BaseType==BaseType) &&
|
(SubType==-1 || Dev.SubType==SubType) &&
|
(SubType==-1 || Dev.SubType==SubType) &&
|
(Interface==-1 || Dev.Interface==Interface) &&
|
(Interface==-1 || Dev.Interface==Interface) &&
|
(DevID==NULL || same_DevID((Dev.DevId>>16)&0xffff,
|
(DevID==NULL || same_DevID((Dev.DevId>>16)&0xffff,
|
Dev.DevId&0xffff, DevID)) &&
|
Dev.DevId&0xffff, DevID)) &&
|
!(n--) ) return res->Devices+i;
|
!(n--) ) return res->Devices+i;
|
#undef Dev
|
#undef Dev
|
}
|
}
|
return 0;
|
return 0;
|
}
|
}
|
|
|
PnP_TAG_PACKET *PnP_find_packet(unsigned char *p,
|
PnP_TAG_PACKET *PnP_find_packet(unsigned char *p,
|
unsigned packet_tag,
|
unsigned packet_tag,
|
int n)
|
int n)
|
{
|
{
|
unsigned mask, masked_tag, size;
|
unsigned mask, masked_tag, size;
|
if(!p) return 0;
|
if(!p) return 0;
|
if (tag_type(packet_tag)) mask=0xff; else mask=0xF8;
|
if (tag_type(packet_tag)) mask=0xff; else mask=0xF8;
|
masked_tag = packet_tag&mask;
|
masked_tag = packet_tag&mask;
|
for(; *p != END_TAG; p+=size) {
|
for(; *p != END_TAG; p+=size) {
|
if ((*p & mask) == masked_tag && !(n--))
|
if ((*p & mask) == masked_tag && !(n--))
|
return (PnP_TAG_PACKET *) p;
|
return (PnP_TAG_PACKET *) p;
|
if (tag_type(*p))
|
if (tag_type(*p))
|
size=ld_le16((unsigned short *)(p+1))+3;
|
size=ld_le16((unsigned short *)(p+1))+3;
|
else
|
else
|
size=tag_small_count(*p)+1;
|
size=tag_small_count(*p)+1;
|
}
|
}
|
return 0; /* not found */
|
return 0; /* not found */
|
}
|
}
|
|
|
PnP_TAG_PACKET *PnP_find_small_vendor_packet(unsigned char *p,
|
PnP_TAG_PACKET *PnP_find_small_vendor_packet(unsigned char *p,
|
unsigned packet_type,
|
unsigned packet_type,
|
int n)
|
int n)
|
{
|
{
|
int next=0;
|
int next=0;
|
while (p) {
|
while (p) {
|
p = (unsigned char *) PnP_find_packet(p, 0x70, next);
|
p = (unsigned char *) PnP_find_packet(p, 0x70, next);
|
if (p && p[1]==packet_type && !(n--))
|
if (p && p[1]==packet_type && !(n--))
|
return (PnP_TAG_PACKET *) p;
|
return (PnP_TAG_PACKET *) p;
|
next = 1;
|
next = 1;
|
};
|
};
|
return 0; /* not found */
|
return 0; /* not found */
|
}
|
}
|
|
|
PnP_TAG_PACKET *PnP_find_large_vendor_packet(unsigned char *p,
|
PnP_TAG_PACKET *PnP_find_large_vendor_packet(unsigned char *p,
|
unsigned packet_type,
|
unsigned packet_type,
|
int n)
|
int n)
|
{
|
{
|
int next=0;
|
int next=0;
|
while (p) {
|
while (p) {
|
p = (unsigned char *) PnP_find_packet(p, 0x84, next);
|
p = (unsigned char *) PnP_find_packet(p, 0x84, next);
|
if (p && p[3]==packet_type && !(n--))
|
if (p && p[3]==packet_type && !(n--))
|
return (PnP_TAG_PACKET *) p;
|
return (PnP_TAG_PACKET *) p;
|
next = 1;
|
next = 1;
|
};
|
};
|
return 0; /* not found */
|
return 0; /* not found */
|
}
|
}
|
|
|
|
|
