lpTreeNode NewChildNode (LPSTR lpszName, UINT uNum)
{
lpTreeNode lpResult;
lpResult = (lpTreeNode) malloc (sizeof (TreeNode));
if (NULL != lpResult) {
NodeInit (lpResult);
lpResult->uNumSubID = uNum;
lpResult->lpszTextSubID = malloc (strlen (lpszName)+1);
if (NULL != lpResult->lpszTextSubID)
strcpy (lpResult->lpszTextSubID, lpszName);
else
fprintf (error_out,"NewChildNode: malloc #2 failed!\n");
} else {
fprintf (error_out,"NewChildNode: malloc #1 failed!\n");
}
return (lpResult);
}
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 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;
}
int main(int argc, char **argv) {
if (argc < 3) {
PrintUsage(stderr, argv[0]);
return -1;
}
if (!strcmp(argv[1], "create")) {
if (argc < 4) {
fprintf(stderr, "ERROR: incorrect create usage\n");
return -2;
}
char *outbfile = argv[2];
int n = atoi(argv[3]);
/* Calculate btree size */
int depth = CalcMinimumDepth(n);
int sz = CalcTreeSize2(n, depth);
fprintf(stderr, "n = %i, depth = %i, size = %i\n", n, depth, sz);
/* Create memory buffer */
mem = (char *) malloc(sz + 1);
mem[sz] = 123; // Magic Marker to detect overflow
memSize = sz;
/* Init top node */
BlockAddrT top = AllocBlock(0x0);
Node topNode;
NodeInit(&topNode);
topNode.depth = depth - 1; // total depth vs depth(rank?) of this node
NodeSave(&topNode, top);
BgTree tree;
tree.topNode = top;
/* Read in data */
KeyT highestKey = 0;
for (int i=0; i<n; i++) {
KeyT key;
BlockAddrT item;
int res = fscanf(stdin, "%x\t%x", &key, &item);
assert(res == 2);
if (key < highestKey) {
fprintf(stderr, "ERROR: Key out of order on line %i. Input must be sorted!\n", i+1);
return -3;
}
highestKey = key;
//printf("GOT %i %i\n", key, item);
TreeAppend(tree, key, item);
}
/* Set keys for non-leaf nodes */
//NodeVisitDFS(&SetKeysVisitor, top);
/* Write memory to file */
assert(mem[sz] == 123);
fprintf(stderr, "MEM: %i of %i allocated was used\n", memLast, memSize);
FILE *f = fopen(outbfile, "wb");
if (!f) {
fprintf(stderr, "ERROR: Failed to open file %s for writing\n", outbfile);
return -9;
}
int res = fwrite(mem, 1, memLast, f);
if (res != memLast) {
fprintf(stderr, "ERROR: Could not write all data to file %s\n", outbfile);
return -4;
}
fclose(f);
} // end of "create" command
if (!strcmp(argv[1], "search")) {
if (argc < 4) {
fprintf(stderr, "ERROR: search usage incorrect, not enough arguments\n");
return -11;
}
FILE *blobfile = 0x0;
if (argc > 4) {
blobfile = fopen(argv[4],"rb");
if (!blobfile) {
fprintf(stderr, "ERROR: failed to open Blob File %s\n", argv[4]);
return -19;
}
}
char *schema=0x0;
if (argc > 5) {
schema = argv[5];
}
char *inbfile = argv[2];
KeyT key;
int res;
if (argv[3][0] == 'S') {
key = CalcCRC(&argv[3][1], strlen(&argv[3][1]));
//fprintf(stderr, "CRC32=%x for %s len=%i\n", key, &argv[3][1], (int) strlen(&argv[3][1]));
printf("%x", key);
if (mem)
free(mem);
exit(0);
} else { // assume hex
res = sscanf(argv[3], "%x", &key);
if (res != 1) {
fprintf(stderr, "ERROR: Unable to parse query key argument %s\n", argv[3]);
return -12;
}
}
if (LoadBGT(inbfile) != 0)
return -99;
/* Perform Search */
BAT outParent;
int outIndex;
BAT found_temp;
BAT found = FindInternal(0, key, &outParent, &outIndex);
while (found != BAInvalid) {
// if (found == BAInvalid) {
// printf("%x NOT FOUND!\n", key);
// } else {
//printf("%x\t%08x", key, found);
if (schema && blobfile) {
for (char *p = &schema[0]; *p; ++p) {
if ((*p == 's') || (*p == 'K')) {
char buf[2000000];
fseek(blobfile, found, SEEK_SET);
int sz = UnpackStringFromFile(blobfile, buf, 2000000);
if (sz < 0) {
fprintf(stderr, "ERROR: Failed to read String from blob file\n");
return -20;
}
found += sz;
buf[sz] = 0x0; // not null terminated by default
printf("\t%s", buf);
} else if (*p == 'i') {
int32_t v;
fseek(blobfile, found, SEEK_SET);
v = UnpackIntFromFile(blobfile);
ERRORassert();
found += 4;
//printf("\t%i", v);
} else if ((*p == 'I') || (*p == 'x') || (*p == 'X')) {
uint32_t v;
fseek(blobfile, found, SEEK_SET);
v = UnpackUIntFromFile(blobfile);
ERRORassert();
//if (*p == 'I')
// printf("\t%u", v);
//else
// printf("\t%x", v);
found += 4;
} else {
fprintf(stderr, "ERROR: Unsupported schema character '%c'\n", *p);
return -23;
}
}
}
printf("\n");
// found_temp = NodeNextLeaf(NodeParent(found), found);
//found_temp = FindInternal(0, key, &outParent, &outIndex);
key++;
found_temp = FindInternal(0, key, &outParent, &outIndex);
found = found_temp;
}
if (blobfile)
fclose(blobfile);
} else if (!strcmp(argv[1],"printtree")) {
if (LoadBGT(argv[2]) != 0) {
printf("Error Loading BGT\n");
return -99;
}
BgTree dummy;
TreeInit(&dummy,0x0);
NodeVisitDFS(dummy, &PrintVisitor, 0);
}
if (mem)
free(mem);
}
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;
}
