/**
* Timer Task
*/
void timerloop_task_proc(void *arg)
{
int ret = 0;
getElapsedTime();
last_timeout_set = 0;
last_occured_alarm = last_time_read;
/* trigger first alarm */
SetAlarm(callback_od, 0, init_callback, 0, 0);
RTIME current_time;
RTIME real_alarm;
do{
rt_mutex_acquire(&condition_mutex, TM_INFINITE);
if(last_timeout_set == TIMEVAL_MAX)
{
ret = rt_cond_wait(
&timer_set,
&condition_mutex,
TM_INFINITE
); /* Then sleep until next message*/
rt_mutex_release(&condition_mutex);
}else{
current_time = rt_timer_read();
real_alarm = last_time_read + last_timeout_set;
ret = rt_cond_wait( /* sleep until next deadline */
&timer_set,
&condition_mutex,
(real_alarm - current_time)); /* else alarm consider expired */
if(ret == -ETIMEDOUT){
last_occured_alarm = real_alarm;
rt_mutex_release(&condition_mutex);
EnterMutex();
TimeDispatch();
LeaveMutex();
}else{
rt_mutex_release(&condition_mutex);
}
}
}while ((ret == 0 || ret == -EINTR || ret == -ETIMEDOUT) && !stop_timer);
if(exitall){
EnterMutex();
exitall(callback_od, 0);
LeaveMutex();
}
}
int
main(void)
{
char buf[200];
Task_t *cons;
dos = CreateMutex("dos");
Ready(cons =
CreateTask(consumer, 2000, "consumer", "consumer", DEFAULT_PRIO));
while ( true )
{
EnterMutex(dos);
if ( !kbhit() )
{
LeaveMutex(dos);
continue;
}
gets(buf);
if ( !strlen(buf) )
return 0;
LeaveMutex(dos);
Send(cons, buf, strlen(buf)+1);
}
}
/*
* Get an inode (but not the root inode) relative to a directory
* inode.
*
* PRE:
* Since this function is used to get all _but_ the rootinode, the
* root inode of a device must succesfully have been read using
* InodeGetRootInode.
*
* POST:
* If the inode is in the InodeTable, a reference is returned, that is,
* the nrClients is increased by one. If it's not present, a new
* inode structure is created, chanined in the table and nrClients is
* set to 1, next the inode is read from disk.
*
*
* IN:
* ParentInode : pointer to inode structure of the parent inode
* (this may _not_ be zero)
* InodeNo : Inode nr of the Inode
*
* OUT:
* <none>
*
* RETURNS:
* pointer to TInode if succesfull, otherwise 0
*
*/
TInode* InodeGetInode(TInode *ParentInode, ULONG InodeNo, char *FileName, int NameLen)
{
TInode *Inode;
/*
* Basically, we are a wrapper for doInodeGetInode
* Our Inode resides on the same filesystem (identified by
* ParentInode->Super) as our parent inode
*/
if (!ParentInode)
{
VxdDebugPrint(D_ERROR, "InodeGetInode: ParentInode==0");
return 0;
}
EnterMutex(sMutex, BLOCK_THREAD_IDLE);
/*
* First get the inode
*/
Inode = doInodeGetInode(ParentInode->Super, ParentInode, InodeNo, FileName, NameLen);
VxdDebugPrint(D_INODE, "InodeGetInode: done, Inode=%X", Inode);
LeaveMutex(sMutex);
return Inode;
}
void InodeRelease(TInode *Inode)
{
EnterMutex(sMutex, BLOCK_THREAD_IDLE);
VxdDebugPrint(D_INODE, "InodeReleaseInode: dev=%s, inono=%lu, inode=%X",
Inode->Super->DeviceName,
Inode->InodeNo,
Inode);
/*
* Inodes are reference counted, only destroy when
* we have no more clients referencing it.
*/
if (--(Inode->nrClients) == 0)
{
/*
* Remove the TInode object from the InodeTable
*/
BlockRelease(Inode->Block);
InodeDestroy(Inode);
}
VxdDebugPrint(D_INODE, "InoReleaseInode: done");
LeaveMutex(sMutex);
}
void StopTimerLoop(TimerCallback_t exitfunction)
{
EnterMutex();
del_timer (&timer);
exitfunction(NULL,0);
LeaveMutex();
}
void
PGPdiskMutex::Enter(PGPUInt32 flags)
{
pgpAssert(NULL!=(int)(mMutexHandle));
EnterMutex(mMutexHandle, flags);
}
void StartTimerLoop(TimerCallback_t init_callback)
{
EnterMutex();
// At first, TimeDispatch will call init_callback.
SetAlarm(NULL, 0, init_callback, 0, 0);
LeaveMutex();
}
void timer_notify(unsigned long data)
{
last_occured_alarm = last_alarm_set;
EnterMutex();
TimeDispatch();
LeaveMutex();
}
void StopTimerLoop(TimerCallback_t exitfunction)
{
EnterMutex();
if(timer_delete (timer)) {
perror("timer_delete()");
}
exitfunction(NULL,0);
LeaveMutex();
}
void zMutex::lock() {
#if defined(_WIN32)
EnterMutex((LPCRITICAL_SECTION)_cs);
#else
int res = pthread_mutex_lock(&_mutex);
CHECK_FATAL(res, "pthread_mutex_lock");
#endif
_lockedCount++;
}
void timer_notify(sigval_t val)
{
if(gettimeofday(&last_sig,NULL)) {
perror("gettimeofday()");
}
EnterMutex();
TimeDispatch();
LeaveMutex();
// printf("getCurrentTime() return=%u\n", p.tv_usec);
}
DWORD canReceiveLoop(CAN_PORT port)
{
Message m;
while(((CANPort*)port)->used)
{
if(m_canReceive(((CANPort*)port)->fd, &m) != 0)
break;
EnterMutex();
canDispatch(((CANPort*)port)->d, &m);
LeaveMutex();
}
return 0;
}
int ProcessFocusedCommand(char* command)
{
int ret = 0;
int sec = 0;
int NodeID;
int NodeType;
UNS32 data = 0;
char buf[50];
int size;
int index;
int subindex = 0;
int datatype = 0;
EnterMutex();
switch(command[0])
{
case 's' : /* Slave Start*/
StartNode(CurrentNode);
break;
case 't' : /* slave stop */
StopNode(CurrentNode);
break;
case 'x' : /* slave reset */
ResetNode(CurrentNode);
break;
case 'r' : /* Read device entry */
ret = sscanf(command, "r%4x,%2x", &index, &subindex);
if (ret)
ReadSDOEntry(CurrentNode,index,subindex);
break;
case 'w' : /* Write device entry */
ret = sscanf(command, "w%4x,%2x,%2x,%x", &index, &subindex, &size, &data);
if (ret)
WriteSDOEntry(CurrentNode,index,subindex,size,data);
break;
case '?' : /* Read device entry */
ReadSDOEntry(CurrentNode,0x6041,0);
break;
case 'c' : /* Write device entry */
ret = sscanf(command, "w%x", &data);
if (ret)
WriteSDOEntry(CurrentNode,0x6040,0,0x2,data);
break;
default :
help_menu();
}
LeaveMutex();
return 0;
}
void InodeCleanup()
{
VxdDebugPrint(D_SYSTEM, "InoCleanUp");
EnterMutex(sMutex, BLOCK_THREAD_IDLE);
free(sInodeTable);
LeaveMutex(sMutex);
DestroyMutex(sMutex);
VxdDebugPrint(D_SYSTEM, "InoCleanUp: done");
}
void StartTimerLoop(TimerCallback_t init_callback)
{
getElapsedTime();
last_alarm_set = last_time_read;
last_occured_alarm = last_alarm_set;
init_timer(&timer);
timer.function = timer_notify;
EnterMutex();
// At first, TimeDispatch will call init_callback.
SetAlarm(NULL, 0, init_callback, 0, 0);
LeaveMutex();
}
/*
* Get the root inode
*
* PRE:
* <none>
*
* POST:
*
*
* IN:
* ParentInode : pointer to inode structure of the parent inode
* (this may _not_ be zero)
* InodeNo : Inode nr of the Inode
*
* OUT:
* <none>
*
* RETURNS:
* pointer to TInode if succesfull, otherwise 0
*
*/
TInode* InodeGetRootInode(TSuperBlock *Super)
{
TInode *inode;
/*
* Basically, we are a wrapper for doInodeGetInode
* We need the root inode of the file system identified
* by Super. Since we are the root inode, we have no parent
* inode
*/
EnterMutex(sMutex, BLOCK_THREAD_IDLE);
inode = doInodeGetInode(Super, 0, EXT2_ROOT_INO, "", 0);
LeaveMutex(sMutex);
return inode;
}
void
consumer(void *arg)
{
char *name = arg;
char msg[100];
Task_t *from;
unsigned size;
while ( true )
{
from = NULL;
size = sizeof msg;
if ( Receive(&from, msg, &size) )
{
EnterMutex(dos);
printf("%s: %s from %s size %u\n", name, msg, from->name, size);
LeaveMutex(dos);
}
}
}
int main(int argc,char **argv)
{
struct sigaction act;
uint8_t nodeid = 2;
// register handler on SIGINT signal
act.sa_handler=sortie;
sigemptyset(&act.sa_mask);
act.sa_flags=0;
sigaction(SIGINT,&act,0);
// Check that we have the right command line parameters
if(argc != 3){
display_usage(argv[0]);
exit(1);
}
// get command line parameters
nodeid = strtoul(argv[2], NULL, 10);
SlaveBoard0.busname = argv[1];
printf("Starting on %s with node id = %u\n", SlaveBoard0.busname, nodeid);
// register the callbacks we use
RegisterSetODentryCallBack(&slavedic_Data, 0x2001, 0, callback_on_position);
slavedic_Data.initialisation=state_change;
slavedic_Data.preOperational=state_change;
slavedic_Data.operational=state_change;
slavedic_Data.stopped=state_change;
// Init Canfestival
if (LoadCanDriver("./libcanfestival_can_socket.so") == NULL){
printf("Unable to load driver library\n");
printf("please put file libcanfestival_can_socket.so in the current directory\n");
exit(1);
}
if(!canOpen(&SlaveBoard0,&slavedic_Data)){
printf("Cannot open can interface %s\n",SlaveBoard0.busname);
exit(1);
}
TimerInit();
setNodeId(&slavedic_Data, nodeid);
setState(&slavedic_Data, Initialisation);
printf("Canfestival initialisation done\n");
Run = 1;
while(Run)
{
sleep(1);
EnterMutex();
counter += nodeid;
LeaveMutex();
}
// Stop timer thread
StopTimerLoop(&Exit);
// Close CAN devices (and can threads)
canClose(&slavedic_Data);
return 0;
}
int ProcessCommand(char* command)
{
int ret = 0;
int sec = 0;
int NodeID;
int NodeType;
EnterMutex();
switch(cst_str4(command[0], command[1], command[2], command[3]))
{
case cst_str4('h', 'e', 'l', 'p') : /* Display Help*/
help_menu();
break;
case cst_str4('s', 's', 't', 'a') : /* Slave Start*/
StartNode(ExtractNodeId(command + 5));
break;
case cst_str4('s', 's', 't', 'o') : /* Slave Stop */
StopNode(ExtractNodeId(command + 5));
break;
case cst_str4('s', 'r', 's', 't') : /* Slave Reset */
ResetNode(ExtractNodeId(command + 5));
break;
case cst_str4('i', 'n', 'f', 'o') : /* Retrieve node informations */
GetSlaveNodeInfo(ExtractNodeId(command + 5));
break;
case cst_str4('r', 's', 'd', 'o') : /* Read device entry */
ReadDeviceEntry(command);
break;
case cst_str4('w', 's', 'd', 'o') : /* Write device entry */
WriteDeviceEntry(command);
break;
case cst_str4('s', 'c', 'a', 'n') : /* Display master node state */
DiscoverNodes();
break;
case cst_str4('w', 'a', 'i', 't') : /* Display master node state */
ret = sscanf(command, "wait#%d", &sec);
if(ret == 1) {
LeaveMutex();
SleepFunction(sec);
return 0;
}
break;
case cst_str4('q', 'u', 'i', 't') : /* Quit application */
LeaveMutex();
return QUIT;
case cst_str4('l', 'o', 'a', 'd') : /* Library Interface*/
ret = sscanf(command, "load#%100[^,],%30[^,],%4[^,],%d,%d",
LibraryPath,
BoardBusName,
BoardBaudRate,
&NodeID,
&NodeType);
if(ret == 5)
{
LeaveMutex();
ret = NodeInit(NodeID, NodeType);
return ret;
}
else
{
printf("Invalid load parameters\n");
}
break;
default :
help_menu();
}
LeaveMutex();
return 0;
}
int ProcessCommand(char* command)
{
int ret = 0;
int sec = 0;
int NodeID;
int NodeType;
UNS32 data = 0;
char buf[50];
EnterMutex();
switch(cst_str4(command[0], command[1], command[2], command[3]))
{
case cst_str4('h', 'e', 'l', 'p') : /* Display Help*/
help_menu();
break;
case cst_str4('c', 'l', 'e', 'a') : /* Display Help*/
system(CLEARSCREEN);
break;
case cst_str4('s', 's', 't', 'a') : /* Slave Start*/
StartNode(ExtractNodeId(command + 5));
break;
case cst_str4('s', 's', 't', 'o') : /* Slave Stop */
StopNode(ExtractNodeId(command + 5));
break;
case cst_str4('s', 'r', 's', 't') : /* Slave Reset */
ResetNode(ExtractNodeId(command + 5));
break;
case cst_str4('i', 'n', 'f', 'o') : /* Retrieve node informations */
GetSlaveNodeInfo(ExtractNodeId(command + 5));
break;
case cst_str4('r', 's', 'd', 'o') : /* Read device entry */
ReadDeviceEntry(command);
break;
case cst_str4('w', 's', 'd', 'o') : /* Write device entry */
WriteDeviceEntry(command);
break;
case cst_str4('n', 'o', 'd', 'e') : /* Write device entry */
ret = sscanf(command, "node %2x", &NodeID);
data = 0;
SDO_write(CANOpenShellOD_Data,NodeID,0x1024,0x00,1, 0, &data, 0);
CurrentNode = NodeID;
break;
case cst_str4('c', 'm', 'd', ' ') : /* Write device entry */
ret = sscanf(command, "cmd %2x,%s", &NodeID, buf );
SDO_write(CANOpenShellOD_Data,NodeID,0x1023,0x01,strlen(buf),visible_string, buf, 0);
SDO_read(CANOpenShellOD_Data,NodeID,0x1023,0x03,visible_string,0);
return 0;
break;
case cst_str4('s', 'y', 'n', '0') : /* Display master node state */
stopSYNC(CANOpenShellOD_Data);
break;
case cst_str4('s', 'y', 'n', '1') : /* Display master node state */
startSYNC(CANOpenShellOD_Data);
break;
case cst_str4('s', 't', 'a', 't') : /* Display master node state */
printf("Status3: %x\n",Status3);
Status3 = 0;
break;
case cst_str4('s', 'c', 'a', 'n') : /* Display master node state */
DiscoverNodes();
break;
case cst_str4('w', 'a', 'i', 't') : /* Display master node state */
ret = sscanf(command, "wait#%d", &sec);
if(ret == 1) {
LeaveMutex();
SleepFunction(sec);
return 0;
}
break;
case cst_str4('g', 'o', 'o', 'o') : /* Quit application */
setState(CANOpenShellOD_Data, Operational);
break;
case cst_str4('q', 'u', 'i', 't') : /* Quit application */
LeaveMutex();
return QUIT;
case cst_str4('l', 'o', 'a', 'd') : /* Library Interface*/
ret = sscanf(command, "load#%100[^,],%30[^,],%4[^,],%d,%d",
LibraryPath,
BoardBusName,
BoardBaudRate,
&NodeID,
&NodeType);
if(ret == 5)
{
LeaveMutex();
ret = NodeInit(NodeID, NodeType);
return ret;
}
else
{
printf("Invalid load parameters\n");
}
break;
default :
help_menu();
}
LeaveMutex();
return 0;
}
int main(int argc, char** argv)
{
extern char *optarg;
char command[200];
char* res;
int ret=0;
int sysret=0;
int i=0;
if (sem_init(&Write_sem, 0, 0) == -1)
handle_error("Writesem_init");
if (sem_init(&Read_sem, 0, 0) == -1)
handle_error("Readsem_init");
/* Defaults */
strcpy(LibraryPath,"/usr/lib/libcanfestival_can_peak_linux.so");
strcpy(BoardBusName,"0");
strcpy(BoardBaudRate,"1M");
/* Init stack timer */
TimerInit();
if (argc > 1){
printf("ok\n");
/* Strip command-line*/
for(i=1 ; i<argc ; i++)
{
if(ProcessCommand(argv[i]) == INIT_ERR) goto init_fail;
}
}
NodeInit(0,1);
RegisterSetODentryCallBack(CANOpenShellOD_Data, 0x2003, 0, &OnStatus3Update);
help_menu();
CurrentNode = 3;
sleep(1);
//setState(CANOpenShellOD_Data, Operational); // Put the master in operational mode
stopSYNC(CANOpenShellOD_Data);
/* Enter in a loop to read stdin command until "quit" is called */
while(ret != QUIT)
{
// wait on stdin for string command
rl_on_new_line ();
res = rl_gets();
//sysret = system(CLEARSCREEN);
if(res[0]=='.'){
ret = ProcessCommand(res+1);
}
else if(res[0]==','){
ret = ProcessFocusedCommand(res+1);
}
else if (res[0]=='\n'){
}
else {
EnterMutex();
SDO_write(CANOpenShellOD_Data,CurrentNode,0x1023,0x01,strlen(res),visible_string, res, 0);
EnterMutex();
SDO_read(CANOpenShellOD_Data,CurrentNode,0x1023,0x03,visible_string,0);
printf("%s\n",SDO_read_data);
}
fflush(stdout);
usleep(500000);
}
printf("Finishing.\n");
// Stop timer thread
StopTimerLoop(&Exit);
/* Close CAN board */
canClose(CANOpenShellOD_Data);
init_fail:
TimerCleanup();
return 0;
}
