bool load_isr_function_from_addr(unsigned long addr, unsigned char isr)
{
// no need to validate ISR because maximum value of unsigned char is 255
if (!disable_isr(isr))
return false;
pshm->Motor[isr].UserPhase = (PUserCtrl) addr;
pshm->UserAlgo.PhaseAddr[isr] = addr;
printf("Loaded OK\n");
return true;
}
/*
* shut_server()
* Shut down a server side
*/
int
shut_server(struct port *port)
{
struct portref *pr;
struct proc *p = curthread->t_proc;
/*
* Flag that it's going down
*/
port->p_flags |= P_CLOSING;
/*
* Remove the port's name from the system table. No
* new clients after this.
*/
p_sema(&name_sema, PRIHI);
hash_delete(portnames, port->p_name);
v_sema(&name_sema);
/*
* If we have an ISR tied to this port, disable
* that before the port goes away.
*/
if (port->p_flags & P_ISR) {
disable_isr(port);
port->p_flags &= ~P_ISR;
}
/*
* Let the mmap() layer clean anything up it might have.
*/
mmap_cleanup(port);
/*
* Dump any work left from clients in our queue
*/
do {
bounce_msgs(port);
/*
* Enumerate our current clients, shut them down
*/
while (pr = port->p_refs) {
/*
* If traffic has slipped in, re-bounce messages.
* Forward progress is guaranteed, because each
* time this happens we close off their access to
* the port, and new connections aren't possible.
*/
if (close_client(port, pr)) {
break;
} else {
p_sema(&p->p_sema, PRIHI);
ASSERT(hash_delete(p->p_prefs, (ulong)pr) == 0,
"shut_server: pr not hashed");
v_sema(&p->p_sema);
}
}
} while (port->p_refs);
/*
* Take this semaphore to flush out any lagging folks trying
* to fiddle with mappings.
*/
p_sema(&port->p_mapsema, PRIHI);
ASSERT((port->p_maps == 0) || (port->p_maps == NO_MAP_HASH),
"shut_server: maps");
FREE(port, MT_PORT);
return(0);
}
int main(int argc, char *argv[])
{
int initialized=0;
int err;
unsigned char motor;
char *function_name;
unsigned long addr;
if (argc < 3) {
goto printusage;
}
motor = (unsigned char)atoi(argv[2]);
printf("Motor: %d\n", motor);
if ((err = InitLibrary()) != 0) {
abort();
}
initialized = 1;
pshm = GetSharedMemPtr();
if (!strcmp(argv[1], "-l")) {
printf("Loading ISR function\n");
if (argc < 4)
goto printusage;
function_name = argv[3];
if (function_name[0] == '$' && strlen(function_name) > 1) {
addr = (int)strtol(&function_name[1], NULL, 16);
printf("Address: %lx\n", addr);
err = load_isr_function_from_addr(addr, motor);
} else {
printf("Function name: %s\n", function_name);
err = load_isr_function(function_name, motor);
}
if (!err)
printf("Load ISR function from addr failed\n");
} else if (!strcmp(argv[1], "-e")) {
if (!(err = enable_isr(motor))) {
printf("Enable ISR returned: %d\n", err);
}
} else if (!strcmp(argv[1], "-d")) {
if (!(err = disable_isr(motor))) {
printf("Disable ISR failed\n");
}
} else {
goto printusage;
}
CloseLibrary();
return !err;
printusage:
printf("User phase loading tool\n");
printf("%s [-l/-e/-d] motor [function_name]\n", argv[0]);
printf("Examples:\n");
printf(" Load function on motor 1: %s -l 1 function_name\n", argv[0]);
printf(" Enable motor phase: %s -e 1\n", argv[0]);
printf(" Disable motor phase: %s -d 1\n", argv[0]);
if (initialized)
CloseLibrary();
return 0;
}
