modules.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 <stdarg.h>

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

ModuleEntry *ModuleEnum(ModuleEntry *entry)
{
    if (!entry) return &module_entries[0];
    entry++;
    if (entry - module_entries >= N_MODULE_ENTRIES) return 0;
    if (entry->head) return entry;
    return ModuleEnum(entry);
}

ModuleHeader *ROMModuleEnum(ModuleHeader *head)
{
    unsigned *p;
    extern ModuleHeader ModuleHead;

    p = (unsigned *)head;
    if (!p) p = (unsigned *)ROM_BASE;
    while (++p < (unsigned *)ROM_MODULE_LIMIT) {
	if (*p == ModuleHead.magic)
	    return (ModuleHeader *)p;
    }
    return 0;
}

ModuleEntry *ModuleLookup(char *title)
{
    ModuleEntry *entry;
    ModuleHeader *head;

    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	if (cistreq(title, REAL_ADDRESS(head, head->title), ' '))
	    return entry;
    }
    return 0;
}

ModuleHeader *ROMModuleLookup(char *title)
{
    ModuleHeader *head;

    for (head = ROMModuleEnum(0); head; head = ROMModuleEnum(head)) {
	if (cistreq(title, REAL_ADDRESS(head, head->title), ' '))
	    return head;
    }
    return 0;
}

void ModuleKill(ModuleEntry *entry)
{
    ModuleHeader *head;

    head = entry->head;
    if (head->final) Invoke_Entry(entry->instantiation, REAL_ADDRESS(head, head->final));
    entry->head = 0;
    entry->instantiation = 0;
}


CallBack ModuleAdd(ModuleHeader *head)
{
    ModuleEntry *entry;
    int i;
    unsigned *base, *limit;
    unsigned sum;

    base = (unsigned *)REAL_ADDRESS(head, head->ro_base);
    limit = (unsigned *)REAL_ADDRESS(head, head->ro_limit)+(head->zi_base - head->rw_base);
    if (sum = cksum(base, limit))
	return ReportError(ENOEXEC, "Bad checksum %x", sum);
    for (i = 0; i < N_MODULE_ENTRIES; i++) 
    {
	entry = &module_entries[i];
	if (!entry->head) 
	{
	    entry->head = head;
	    if (head->init)
		entry->instantiation = (void *)Invoke_Entry(0, REAL_ADDRESS(head, head->init));
	    return ReportOK();
	} /* end if this is an empty slot */
	else
	{
	   if ( entry->head ==  head)
	   {
	      return ReportOK();
	   } /* end if this is the same module entry */
	}
    }
    return ReportError(ENOMEM, "Maximum no. of modules (%d) exceeded.", N_MODULE_ENTRIES);
}

void *real_address(ModuleHeader *head, void *address)
{
    return (char *)address + ((char *)head - (char *)head->self);
}

void list_module_cmds(ModuleHeader *hdr)
{
    const CmdTable *cmdtbl;
    char *command;
    int pos;

    Write0("Module is ");
    Write0(REAL_ADDRESS(hdr, hdr->help));
    NewLine();
    cmdtbl = hdr->cmdtbl;
    if (!cmdtbl) return;
    cmdtbl = REAL_ADDRESS(hdr, cmdtbl);
    if (command = cmdtbl->command) {
	PrettyPrint("\nHelp is available on:");
	pos = 0;
	do {
	    if (pos > 0) {
		do {
		    WriteC(' ');
		} while (++pos & 15);
	    }
	    if (pos > 70) {
		NewLine();
		pos = 0;
	    }
	    command = REAL_ADDRESS(hdr, command);
	    Write0(command);
	    pos += strlen(command);
	    cmdtbl++;
	} while (command = cmdtbl->command);
	NewLine();
    }
}

CallBack HelpCommand(char *subject)
{
    ModuleHeader *head;
    ModuleEntry *entry;
    const CmdTable *cmdtbl;
    char *command;
    
    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	if (!*subject) {
	    list_module_cmds(head);
	    NewLine();
	    continue;
	}
	if (cistreq(subject, REAL_ADDRESS(head, head->title), ' ')) {
	    list_module_cmds(head);
	    return ReportOK();
	}
	cmdtbl = head->cmdtbl;
	if (!cmdtbl) continue;
	cmdtbl = REAL_ADDRESS(head, cmdtbl);
	while (command = cmdtbl->command) {
	    if (cistreq(subject, REAL_ADDRESS(head, command), ' ')) {
		Write0("Usage: ");
		Write0(REAL_ADDRESS(head, cmdtbl->syntax));
		NewLine();
		Write0(REAL_ADDRESS(head, cmdtbl->help));
		NewLine();
		return ReportOK();
	    }
	    cmdtbl++;
	}
    }
    if (*subject)
	return ReportError(EINVAL, "No help available on %s", subject);
    return ReportOK();
}

CallBack ModulesCommand(char *tail)
{
    ModuleHeader *head;
    ModuleEntry *entry;

    PrettyPrint("Header\t Base\t  Limit    Data");
    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	PrettyPrint("%x %x %x %x %s",
	    head,
	    REAL_ADDRESS(head, head->ro_base),
	    (unsigned)REAL_ADDRESS(head, head->ro_limit)+(head->zi_base - head->rw_base),
	    entry->instantiation,
	    REAL_ADDRESS(head, head->help)
	);
    }
    return ReportOK();
}

CallBack ROMModules(char *tail)
{
    ModuleHeader *head;

    PrettyPrint("Header\t Base\t  Limit");
    for (head = ROMModuleEnum(0); head; head = ROMModuleEnum(head)) {
	PrettyPrint("%x %x %x %s",
	    head,
	    REAL_ADDRESS(head, head->ro_base),
	    (unsigned)REAL_ADDRESS(head, head->ro_limit)+(head->zi_base - head->rw_base),
	    REAL_ADDRESS(head, head->help)
	);
    }
    return ReportOK();
}

CallBack KillCommand(char *title)
{
    ModuleEntry *entry;
    ModuleHeader *head;

    entry = ModuleLookup(title);
    if (entry) {
	ModuleKill(entry);
	return ReportOK();
    }
    return ReportError(EINVAL, "Module '%s' not found. Use Modules to list modules.", title);
}

CallBack UnPlugCommand(char *title)
{
    ModuleEntry *entry;
    ModuleHeader *head;
    unsigned short flags;

    head = ROMModuleLookup(title);
    if (!head)
	return ReportError(EINVAL, "ROM Module '%s' not found. Use ROMModule to list ROM Modules.", title);
    flags = head->flags;
    flags |= UNPLUGGED_FLAG;
    FlashWrite((unsigned)&head->flags, (unsigned)(&head->flags+1), (char *)&flags);
    entry = ModuleLookup(title);
    if (entry) ModuleKill(entry);
    return ReportOK();
}

CallBack PlugInCommand(char *title)
{
    ModuleHeader *head;
    ModuleEntry *entry;
    unsigned short flags;

    head = ROMModuleLookup(title);
    if (!head)
	return ReportError(EINVAL, "Module '%s' not found. Use Modules to list modules.", title);
    flags = head->flags;
    flags &= ~UNPLUGGED_FLAG;
    FlashWrite((unsigned)&head->flags, (unsigned)(&head->flags+1), (char *)&flags);
    entry = ModuleLookup(title);
    if (!entry)
	return ModuleAdd(head);
    return ReportOK();
}

#define FLAG_INTERACTIVE	1
#define FLAG_FACTORY_TEST	2

void ScanModules(int flags)
{
    ModuleHeader *head;

    if (flags & FLAG_INTERACTIVE)
	PrettyPrint("Scanning ROM for modules ...");
    for (head = ROMModuleEnum(0); head; head = ROMModuleEnum(head)) {
	if (head->flags & UNPLUGGED_FLAG) continue;
	if (flags & FLAG_INTERACTIVE)
	    PrettyPrint("  Found module '%s' at %x",
		REAL_ADDRESS(head, head->title), head);
	ModuleAdd(head);
    }
}

#define	ANGEL_BOOT_LEDS	(0x8)

void BootModules(int flags)
{
    ModuleHeader *head;
    ModuleEntry *autoentry;
    ModuleEntry *factoryentry;
    ModuleEntry *entry;
    char *bootmodule;

    autoentry = 0;
    factoryentry = 0;
    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	if (head->flags & AUTOSTART_FLAG)
	    autoentry = entry;
	if (head->flags & FACTORY_FLAG)
	    factoryentry = entry;
    }
    bootmodule = env_lookup("boot");
    if (bootmodule) {
	SetLEDs( ANGEL_BOOT_LEDS );
	CLI(nextword(bootmodule));
    }
    if (flags & FLAG_FACTORY_TEST) {
	if (factoryentry)
	    Invoke_Entry(factoryentry->instantiation, REAL_ADDRESS(factoryentry->head, factoryentry->head->start), "");
    } else if (autoentry && !(flags & FLAG_INTERACTIVE)) {
	SetLEDs( ANGEL_BOOT_LEDS );
	Invoke_Entry(autoentry->instantiation, REAL_ADDRESS(autoentry->head, autoentry->head->start), "");
    }
}

__value_in_regs ServiceBlock ServiceCall(int service, ...)
{
    ServiceBlock sb;
    ModuleEntry *entry;
    ModuleHeader *head;
    va_list ap;

    va_start(ap, service);
    sb.r1 = va_arg(ap, unsigned);
    sb.r2 = va_arg(ap, unsigned);
    sb.r3 = va_arg(ap, unsigned);
    va_end(ap);
    for (entry = ModuleEnum(0); entry; entry = ModuleEnum(entry)) {
	head = entry->head;
	if (head->service) {
	    sb = Invoke_Service_Entry(entry->instantiation, REAL_ADDRESS(head, head->service), service, sb.r1, sb.r2, sb.r3);
	    if (sb.r0) return sb;
	}
    }
    sb.r0 = 0;
    return sb;
}