UINT32 p = (UINT32)from;    UINT32 ix;    UINT32 c_size;					/* cache size */    int way, lastway;    if (p >= SH7700_P2_BASE && p <= (SH7700_P2_BASE | SH7700_PHYS_MASK))	return ERROR;					/* P2 non-cacheable */    else if (p >= SH7700_P4_BASE)	return ERROR;					/* P4 non-cacheable */    switch (cacheSh7700CCRGet () & (CCR_2WAY_MODE | CCR_1WAY_MODE))	{	case CCR_1WAY_MODE: c_size = CAC_DATA_SIZE / 4;	lastway = 0;	break;	case CCR_2WAY_MODE: c_size = CAC_DATA_SIZE / 2;	lastway = 1;	break;	default:	    c_size = CAC_DATA_SIZE;	lastway = 3;	}    if (bytes == 0)	{	return OK;	}    else if (bytes == ENTIRE_CACHE)	{	for (way = 0; way <= lastway; way++)	    {	    for (ix = 0; ix <= 0x7f0; ix += CAC_LINE_SIZE)		{		UINT32 *pt = (UINT32 *)(CAC_ADRS_ARRAY | (way << 11) | ix);		cacheSh7700MFlush (pt, -1, 0, 0);		}	    }	}    else	{	UINT32 ca_begin = p & ~(CAC_LINE_SIZE - 1);	UINT32 ca_end   = p + bytes - 1;	if (bytes < c_size) /* do associative purge */	    {	    cacheSh7700AFlush (ca_begin, ca_end);	    }	else /* check every cache tag */	    {	    for (way = 0; way <= lastway; way++)		{		for (ix = 0; ix <= 0x7f0; ix += CAC_LINE_SIZE)		    {		    UINT32 *pt = (UINT32 *)(CAC_ADRS_ARRAY | (way << 11) | ix);		    cacheSh7700MFlush (pt, ix, ca_begin, ca_end);		    }		}	    }	}    return OK;    }/******************************************************************************** cacheSh7700DmaMalloc - allocate a cache-safe buffer** This routine attempts to return a pointer to a section of memory that will* not experience cache coherency problems.  This routine is only called when* MMU support is available for cache control.** INTERNAL* We check if the cache is actually on before allocating the memory.  It is* possible that the user wants Memory Management Unit (MMU) support but does* not need caching.** RETURNS: A pointer to a cache-safe buffer, or NULL.** NOMANUAL*/LOCAL void *cacheSh7700DmaMalloc    (    size_t bytes    )    {    void *pBuf;    int   pageSize;    if (cacheDataEnabled == FALSE)	return malloc (bytes);		/* cache is off just allocate buffer */    if ((pageSize = VM_PAGE_SIZE_GET ()) == ERROR)	return NULL;    /* make sure bytes is a multiple of pageSize */    bytes = bytes / pageSize * pageSize + pageSize;    if (_func_valloc == NULL || (pBuf = (void *)(*_func_valloc)(bytes)) == NULL)        return NULL;    VM_STATE_SET (NULL, pBuf, bytes,                  VM_STATE_MASK_CACHEABLE, VM_STATE_CACHEABLE_NOT);    return pBuf;    }/******************************************************************************** cacheSh7700DmaFree - free the buffer acquired by cacheSh7700DmaMalloc()** This routine returns to the free memory pool a block of memory previously* allocated with cacheSh7700DmaMalloc().  The buffer is marked cacheable.** RETURNS: OK, or ERROR if cacheSh7700DmaMalloc() cannot be undone.** NOMANUAL*/LOCAL STATUS cacheSh7700DmaFree    (    void *pBuf    )    {    STATUS status = OK;    if (cacheDataEnabled)        {	BLOCK_HDR *pHdr = BLOCK_TO_HDR (pBuf);        status = VM_STATE_SET (NULL, pBuf,			       (pHdr->nWords * 2) - sizeof(BLOCK_HDR),                               VM_STATE_MASK_CACHEABLE, VM_STATE_CACHEABLE);        }    free (pBuf);    return status;    }/******************************************************************************** cacheSh7700P2DmaMalloc - allocate a cache-safe buffer from P2 region** This routine attempts to return a pointer to a section of memory that will* not experience cache coherency problems.  This routine may be called when* MMU support is NOT available for cache control.** RETURNS: A pointer to a cache-safe buffer, or NULL.** NOMANUAL*/LOCAL void *cacheSh7700P2DmaMalloc    (    size_t bytes    )    {    void *pBuf;    int alignment = VM_PAGE_SIZE_GET ();    if (alignment != ERROR && _func_valloc != NULL)	{	/* adjust bytes to a multiple of MMU page size */	bytes = bytes / alignment * alignment + alignment;	/* get a page aligned memory */	if ((pBuf = (void *)(*_func_valloc)(bytes)) == NULL)	    return NULL;	/* mark it as non-cacheable, non-writable on virtual space */	VM_STATE_SET (NULL, pBuf, bytes,		      VM_STATE_MASK_WRITABLE | VM_STATE_MASK_CACHEABLE,		      VM_STATE_WRITABLE_NOT  | VM_STATE_CACHEABLE_NOT);	}    else	{	/* adjust bytes to a multiple of cache line length */	alignment = _CACHE_ALIGN_SIZE;	bytes = bytes / alignment * alignment + alignment;	/* use memalign() to avoid sharing a cache-line with other buffers */	if ((pBuf = memalign (alignment, bytes)) == NULL)	    return NULL;	}    /* make sure nothing in pBuf is cached on cacheable region */    if (cacheLib.flushRtn != NULL)	cacheLib.flushRtn (DATA_CACHE, pBuf, bytes);    /* relocate the buffer to P2 (mmu-bypass, non-cacheable) region */    return (void *)(((UINT32)pBuf & SH7700_PHYS_MASK) | SH7700_P2_BASE);    }/******************************************************************************** cacheSh7700P2DmaFree - free the buffer acquired by cacheSh7700P2DmaMalloc()** This routine returns to the free memory pool a block of memory previously* allocated with cacheSh7700P2DmaMalloc().** RETURNS: OK, or ERROR if cacheSh7700DmaMalloc() cannot be undone.** NOMANUAL*/LOCAL STATUS cacheSh7700P2DmaFree    (    void *pBuf    )    {    STATUS status = OK;    UINT32 p = (UINT32)pBuf;    if (p < SH7700_P2_BASE || p > (SH7700_P2_BASE | SH7700_PHYS_MASK))	return ERROR;    if (cacheDataMode & CACHE_DMA_BYPASS_P0)	p =  p & SH7700_PHYS_MASK;			/* relocate to P0 */    else if (cacheDataMode & CACHE_DMA_BYPASS_P1)	p = (p & SH7700_PHYS_MASK) | SH7700_P1_BASE;	/* relocate to P1 */    else if (cacheDataMode & CACHE_DMA_BYPASS_P3)	p = (p & SH7700_PHYS_MASK) | SH7700_P3_BASE;	/* relocate to P3 */    else	return ERROR;    if (VM_PAGE_SIZE_GET () != ERROR && _func_valloc != NULL)	{	BLOCK_HDR *pHdr = BLOCK_TO_HDR ((void *)p);	status = VM_STATE_SET (NULL, (void *)p,			       (pHdr->nWords * 2) - sizeof(BLOCK_HDR),			       VM_STATE_MASK_WRITABLE | VM_STATE_MASK_CACHEABLE,			       VM_STATE_WRITABLE      | VM_STATE_CACHEABLE);	}    free ((void *)p);    return status;    }#undef CACHE_DEBUG#ifdef CACHE_DEBUG#include "stdio.h"LOCAL UINT32 va[512][5];	/* (128-entry * 4-way) * (tag[1] + data[4]) *//******************************************************************************** cacheSh7700Dump - dump SH7700 cache** NOMANUAL*/LOCAL int partition (UINT32 a[][5], int l, int r)    {    int i, j, pivot;    UINT32 t;    i = l - 1;    j = r;    pivot = a[r][0];    for (;;)	{	while (a[++i][0] < pivot)	    ;	while (i < --j && pivot < a[j][0])	    ;	if (i >= j)	    break;	t = a[i][0]; a[i][0] = a[j][0]; a[j][0] = t;	t = a[i][1]; a[i][1] = a[j][1]; a[j][1] = t;	t = a[i][2]; a[i][2] = a[j][2]; a[j][2] = t;	t = a[i][3]; a[i][3] = a[j][3]; a[j][3] = t;	t = a[i][4]; a[i][4] = a[j][4]; a[j][4] = t;	}    t = a[i][0]; a[i][0] = a[r][0]; a[r][0] = t;    t = a[i][1]; a[i][1] = a[r][1]; a[r][1] = t;    t = a[i][2]; a[i][2] = a[r][2]; a[r][2] = t;    t = a[i][3]; a[i][3] = a[r][3]; a[r][3] = t;    t = a[i][4]; a[i][4] = a[r][4]; a[r][4] = t;    return i;    }LOCAL void quick_sort_1 (UINT32 a[][5], int l, int r)    {    int v;    if (l >= r)	return;    v = partition (a, l, r);    quick_sort_1 (a, l, v - 1);		/* sort left partial array */    quick_sort_1 (a, v + 1, r);		/* sort right partial array */    }LOCAL void quick_sort (UINT32 a[][5], int n)    {    quick_sort_1 (a, 0, n - 1);    }LOCAL void cacheSh7700DumpOp (UINT32 a[][5], UINT32 ccr)    {    int ent, i;    BOOL t = (ccr & CCR_2WAY_MODE) ? TRUE : FALSE;	/* TRUE if 2-way mode */    for (ent = 0; ent < 128; ent++)	{	int way;	int lastway = t ? 1 : 3;	/* 2-way mode uses way 0 and 1 */	for (way = 0; way <= lastway; way++)	    {	    UINT32 tag = *(UINT32 *)(CAC_ADRS_ARRAY | (way << 11) | (ent << 4));	    i = t ? (ent * 2 + way) : (ent * 4 + way);	    a[i][0] = (tag & 0xfffffc03) |((ent & 0x3f)<<4) |((ent & 0x40)>>4);	    a[i][1] = *(UINT32 *)(0xf1000000 | (way << 11) | (ent << 4));	    a[i][2] = *(UINT32 *)(0xf1000004 | (way << 11) | (ent << 4));	    a[i][3] = *(UINT32 *)(0xf1000008 | (way << 11) | (ent << 4));	    a[i][4] = *(UINT32 *)(0xf100000c | (way << 11) | (ent << 4));	    }	}    }LOCAL void cacheSh7700Disp (UINT32 a[][5], UINT32 ccr)    {    int i;    int lines = (ccr & CCR_2WAY_MODE) ? 256 : 512;	/* cache lines */    quick_sort (a, lines);    for (i = 0; i < lines; i++)	{	UINT32 b10 = (a[i][0] & 0x4) << 8;	/* MSB of entry selector */	printf ("0x%08x: %08x %08x %08x %08x %s %s %s\n",		a[i][0] & 0xfffffff0,		a[i][1], a[i][2], a[i][3], a[i][4],		(a[i][0] & 0x400) ^ b10 ? "!"  : " ",		a[i][0] & 0x2           ? "U+" : "U-",		a[i][0] & 0x1           ? "V+" : "V-");	}    }void cacheSh7700ClearTest (int addr, int bytes)    {    UINT32 ccr = cacheSh7700CCRGet ();    int key = intLock ();				/* LOCK INTERRUPTS */    if ((ccr & (CCR_WRITE_BACK_P1 | CCR_WRITE_THRU)) == CCR_WRITE_THRU)	{	cacheSh7700CCRSet (ccr & ~CCR_CACHE_ENABLE);	/* disable caching */	cacheSh7700Clear (INSTRUCTION_CACHE, (void *)addr, bytes);	cacheSh7700DumpOp (va, ccr);	cacheSh7700CCRSet (ccr);			/* restore ccr */	intUnlock (key);				/* UNLOCK INTERRUPTS */	cacheSh7700Disp (va, ccr);	}    else	{	UINT32 pDumpRtn	    = ((UINT32)cacheSh7700DumpOp & SH7700_PHYS_MASK) | SH7700_P2_BASE;	UINT32 (*a)[512][5]	= (UINT32(*)[512][5])(((UINT32)va & SH7700_PHYS_MASK) | SH7700_P2_BASE);	cacheSh7700Clear (INSTRUCTION_CACHE, (void *)addr, bytes);	(* (VOIDFUNCPTR)pDumpRtn)(*a, ccr);	intUnlock (key);				/* UNLOCK INTERRUPTS */	cacheSh7700Disp (*a, ccr);	}    }void cacheSh7700ClearTestAll ()    {    cacheSh7700ClearTest (0, ENTIRE_CACHE);    }void cacheSh7700Dump ()    {    cacheSh7700ClearTest (0, 0);    }#endif /* CACHE_DEBUG */