swis.c

AT91所有开发板的资料 AT91所有开发板的资料

/* -*-C-*-
 *
 * $Revision: 1.3 $
 *   $Author: mechavar $
 *     $Date: 2000/05/01 19:37:08 $
 *
 * Copyright (c) 2000 ARM, INC.
 * All Rights Reserved.
 *
 *   Project: BootStrap Loader
 *
 *   
 */

#include <string.h>

#include "modules.h"
#include "lib.h"
#include "bsl_platform.h"
#include "bsl.h"
#include "gdata.h"
#include "swis.h"
#include "errors.h"
#include "bsl.h"

void Write0(char *s)
{
    int c;

    while (c = *s++) WriteC(c);
}

void NewLine(void)
{
    WriteC('\r');
    WriteC('\n');
}

void WriteHex(unsigned w)
{
    write_hex(w, WriteC);
}

void WriteDec(unsigned w)
{
    write_dec(w, WriteC);
}


void pretty_c(int c)
{
    static int n;

    if (c == '\t') {
	do {
	    WriteC(' ');
	} while (++n & 7);
	return;
    }
    if (c == '\n') {
	n = 0;
	WriteC('\r');
    }
    if (c >= 0x20) n++;
    WriteC(c);
}

void PrettyPrint(const char *fmt, ...)
{
    va_list ap;

    va_start(ap, fmt);
    format_string(fmt, pretty_c, ap);
    pretty_c('\n');
    va_end(ap);
}

static char *report_buff;

void report_c(int c)
{
    *report_buff++ = c;
}

void HandleReportedError(void);

CallBack ReportError(unsigned errno, const char *fmt, ...)
{
    CallBack cb;
    va_list ap;

    va_start(ap, fmt);
    report_buff = gpbuff;
    *(unsigned *)report_buff = errno;
    report_buff += 4;
    format_string(fmt, report_c, ap);
    report_c(0);
    va_end(ap);
    error_buff = gpbuff;
    return HandleReportedError;
}

CallBack ReportOK(void)
{
    return 0;
}

CallBack CLI(char *cmd)
{
    char *tail;
    ModuleHeader *head;
    ModuleEntry *entry;
    const CmdTable *cmdtbl;
    char *command;

    while (*cmd == ' ') cmd++;
    if (!*cmd)
	return ReportOK();
    tail = nextword(cmd);
    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	if (cistreq(cmd, REAL_ADDRESS(head, head->title), ' '))
	   if (head->start) 
	   {
	      return Invoke_Entry(entry->instantiation, 
				  REAL_ADDRESS(head, head->start), tail);
	   }
	cmdtbl = head->cmdtbl;
	if (!cmdtbl) continue;
	cmdtbl = REAL_ADDRESS(head, cmdtbl);
	while (command = cmdtbl->command) {
	    if (cistreq(cmd, REAL_ADDRESS(head, command), ' ')) {
		return Invoke_Entry(entry->instantiation, REAL_ADDRESS(head, cmdtbl->code), tail);
	    }
	    cmdtbl++;
	}
    }
    return ReportError(EINVAL, "Command '%s' not found. Enter 'Help' for a list of commands.", cmd);
}


/* Keep this routine in case future versions need to probe flash identity */
#define		SST29EE020_DEV_ID	0x10

static unsigned getSectorSize( void )
{
   char	devcode;

   devcode = (char)Get_Device_Code();
   switch( devcode )
   {
      case SST29EE020_DEV_ID:
	 return( 128 );
	 break;

      default:
	 /* Act like an ATMEL AT29C020 */
	 return( 256 );
	 break;
   }
}

int overwriteFlash( char *pSrc, unsigned limit )
{

   while ( limit > 0 )
   {
      if ( 0xFF == *pSrc )
      {
	 pSrc++;
	 limit--;
	 continue;
      }
      return( limit );
   }
   return( 0 );
}

void writePartialSector( char *pSrc, char *pSector, unsigned offset, 
			 unsigned extent )
{

   if ( 0 != overwriteFlash( pSector + offset, extent ) )
    {
       /* Grab the data not involved in the write, so it can be rewritten
          after the sector erase 
       */
       memcpy( pSrc, pSector, offset);
       memcpy( pSrc + offset + extent, pSector + offset + extent, 
	       SST39_SECTOR_SIZE - (offset + extent));
       Erase_Sector( (unsigned)pSector );
       Write_Sectors( 1, pSrc, (unsigned)pSector, SST39_SECTOR_SIZE );
    } /* End if we need to erase the block before writing */
    else
    {
       /* Adjust so that only valid halfwords are written  */
       if ( 1 & offset )
       {
	  offset--;
	  extent++;
	  *(pSrc + offset) = *(pSector + offset);
       }
       if ( 1 & extent )
       {
	  extent++;
	  *(pSrc + extent) = *(pSector + extent);
       }
       Write_Sectors( 1, pSrc + offset, (unsigned)(pSector + offset), extent );
    } /* end else do a write */
}

void FlashWrite(unsigned base, unsigned limit, char *source)
{
    unsigned nsectors, sector_start, sector_end, extent;

    /* break the transfer up into sector sized packets of data  */
    sector_start = GET_SECTOR_START(base);
    nsectors = (limit - sector_start) / SST39_SECTOR_SIZE;
    nsectors += (limit % SST39_SECTOR_SIZE > 0 ? 1 : 0);

    /* The first and last sectors are special cases  */
    sector_end = GET_SECTOR_START(base + SST39_SECTOR_SIZE);
    extent = ( sector_end < limit ) ? sector_end - base : limit - base;
    memcpy( (char *)sst39_sector+(base - sector_start), source, extent);

    writePartialSector( (char *)sst39_sector, (char *)sector_start, 
			base - sector_start, extent);

    nsectors--;
    base += extent;
    source += extent;
    sector_start += SST39_SECTOR_SIZE;

    if ( nsectors > 0 )
    {
       /* Handle the last sector */
       nsectors--;
       sector_start += SST39_SECTOR_SIZE * nsectors;
       extent = limit - sector_start;
       memcpy( (char *)sst39_sector, source + (SST39_SECTOR_SIZE * nsectors), 
	       extent);
       writePartialSector( (char *)sst39_sector, (char *)sector_start, 0, 
			   extent);

       /* Handle the rest */
       while ( nsectors > 0 )
       {
	  memcpy( (char *)sst39_sector, source, SST39_SECTOR_SIZE);
	  if ( 0 != overwriteFlash( (char *)base, SST39_SECTOR_SIZE ) )
	  {
	     Erase_Sector( base );
	  }
	  Write_Sectors( 1, (char *)sst39_sector, base, SST39_SECTOR_SIZE );

	  nsectors--;
	  base += SST39_SECTOR_SIZE;
	  source += SST39_SECTOR_SIZE;
       } /* End while there are full sectors to write */
    } /* End if there is more than one sector to write */

}

void FlashErase(unsigned base, unsigned limit)
{
    unsigned nbytes, nsectors, sector;

    nbytes = limit - GET_SECTOR_START(base);
    nsectors = nbytes / SST39_SECTOR_SIZE;
    nsectors += (limit % SST39_SECTOR_SIZE > 0 ? 1 : 0);
    for ( sector = GET_SECTOR_START(base); nsectors > 0; 
	  nsectors--, sector+= SST39_SECTOR_SIZE )
    {
       Erase_Sector( sector );
    }
}

__value_in_regs Env GetEnv(void)
{
    Env e;

    e.command_line = getenv_command_line;
    e.himem = applimit;
    return e;
}

CallBack SetEnv(char *command_line, unsigned limit)
{
    ServiceBlock sb;

    sb = ServiceCall(Service_SetEnv, command_line, limit);
    if (sb.r0) return sb.r0;
    getenv_command_line = (char *)sb.r1;
    applimit = sb.r2;;
    return ReportOK();
}

CallBack ModuleSWI(int swino, SWIRegs *r)
{
    ModuleEntry *entry;
    ModuleHeader *head;
    int swi_base;
    int swi_mod;

    swi_base = swino & ~63;
    swi_mod = swi_mod & 63;
    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	if (swi_base == head->swi_base) {
	    return Invoke_Entry(entry->instantiation, REAL_ADDRESS(head, head->swi_handler), swi_mod, r);
	}
    }
    return ReportError(EINVAL, "Unknown SWI %x", swino);
}

CallBack OS_SWIHandler(int swino, SwiRegs *r)
{
    switch (swino) {
	case SWI_WriteC:
	    WriteC(r->r[0]);
	    break;
	case SWI_WriteHex:
	    WriteHex(r->r[0]);
	    break;
	case SWI_Write0:
	    Write0((char *)r->r[0]);
	    break;
	case SWI_NewLine:
	    NewLine();
	    break;
	case SWI_ReadC:
	    r->r[0] = ReadC();
	    break;
	case SWI_CLI:
	    return CLI((char *)r->r[0]);
	case SWI_WriteDec:
	    WriteDec(r->r[0]);
	    break;
	case SWI_PrettyPrint:
	    PrettyPrint((const char *)r->r[0], r->r[1], r->r[2], r->r[3], r->r[4],
			r->r[5], r->r[6], r->r[7], r->r[8], r->r[9]);
	    break;
	case SWI_ReportError:
	    r->r[0] = (unsigned)ReportError(r->r[0], (const char *)r->r[1], r->r[2], r->r[3],
			r->r[4], r->r[5], r->r[6], r->r[7], r->r[8], r->r[9]);
	    break;
	case SWI_FlashWrite:
	    FlashWrite(r->r[0], r->r[1], (char *)r->r[2]);
	    break;
	case SWI_FlashErase:
	    FlashErase(r->r[0], r->r[1]);
	    break;
	case SWI_ServiceCall: {
	    ServiceBlock sb;
	    sb = ServiceCall(r->r[0], r->r[1], r->r[2], r->r[3]);
	    if (sb.r0) return sb.r0;
	    r->r[1] = sb.r1;
	    r->r[2] = sb.r2;
	    r->r[3] = sb.r3;
	    break;
	}
	case SWI_SetEnv:
	    return SetEnv((char *)r->r[0], r->r[1]);
	case SWI_ReadLine:
	    r->r[0] = ReadLine((char *)r->r[0], (char *)r->r[1], r->r[2]);
	    break;
	case SWI_GetEnv:
	    *(Env *)&r->r[0] = GetEnv();
	    break;
	case SWI_EnterOS:
	    return EnterOS;
	case SWI_Exit: {
	    unsigned rc = 0;

	    if (r->r[1] == *(unsigned *)"ABEX") rc = r->r[2];
	    PrettyPrint("Program terminated with return code %d", rc);
	    return bsl_main;
	}
	case SWI_GetErrno:
	    r->r[0] = 0;
	    break;
	case SWI_Clock:
	    r->r[0] = -1;
	    break;
	case SWI_Time:
	    r->r[0] = -1;
	    break;
	case SWI_Remove:
	    r->r[0] = -1;
	    break;
	case SWI_Rename:
	    r->r[0] = -1;
	    break;
	case SWI_Open:
	    r->r[0] = strcmp((char *)r->r[0], ":tt") == 0;
	    break;
	case SWI_Close:
	    r->r[0] = 0;
	    break;
	case SWI_Write: {
	    int i, n;
	    char *s;
	    int c;

	    s = (char *)r->r[1];
	    n = (int)r->r[2];
	    for (i = 0; i < n; i++) {
		c = *s++;
		if (c == '\n') WriteC('\r');
		WriteC(c);
	    }
	    r->r[0] = 0;
	    break;
	}
	case SWI_Read: {
	    char *s;
	    int n;
	    int i;

	    s = (char *)r->r[1];
	    n = (int)r->r[2];
	    i = ReadLine("", s, n-1);
	    if (i != -1)
		s[i] = '\n';
	    i++;
	    r->r[0] = n - i;
	    break;
	}
	case SWI_Seek:
	    r->r[0] = -1;
	    break;
	case SWI_Flen:
	    r->r[0] = -1;
	    break;
	case SWI_TmpNam:
	    r->r[0] = 0;
	    break;
	case SWI_IsTTY:
	    r->r[0] = 1;
	    break;
	case SWI_InstallHandler: {
	    unsigned handler, r11_value, address;

	    handler = r->r[0];
	    r11_value = r->r[1];
	    address = r->r[2];
	    r->r[1] = handlers[handler].r11_value;
	    r->r[2] = (unsigned)handlers[handler].handle_fun;
	    handlers[handler].r11_value = r11_value;
	    handlers[handler].handle_fun = ((void (*)(void))(address));
	    break;
	}
	case SWI_GenerateError:
	    return ErrorHandler;
	default:
	    return ReportError(EINVAL, "Unknown SWI %x", swino);
    }
    return ReportOK();
}

CallBack Boot(char *tail)
{

   ScanModules( 1 );
   BootModules( 1 );
   bsl_main();
}

/* For the moment use the same stack as the bsl, this may need a revisit  */
void DoGoS(void)
{

   ((void (*)(void))(current_pc))(); /* thanks, STheobald */
}

#define MODE_BITS	0x1f
#define USER_MODE	0x10

void DoGo(void)
{
   unsigned	tmp;
   
   __asm
   {
      mrs tmp, CPSR
      bic tmp, tmp, #MODE_BITS
      orr tmp, tmp, #USER_MODE
      msr CPSR_c, tmp
   }
   ((void (*)(void))(current_pc))(); /* thanks, STheobald */
}

#define		SWI_LOAD_ADDR	(0x00000028)

void bsl_InstallSWI(void)
{

   *(unsigned *)(SWI_LOAD_ADDR) = (unsigned)bslSWIHandler;
}

#define		IRQ_LOAD_ADDR	(0x00000038)

void bsl_InstallIRQ(void)
{

   *(unsigned *)(IRQ_LOAD_ADDR) = (unsigned)IRQHandler;
}