diropres *
nfsproc_create_2(createargs * createargp, struct svc_req * reqp)
{
static diropres diropres;
struct stat stat_holder;
char *pathp;
char fullpath[PATH_MAX];
vxfh_t *vxfhp;
int fd;
nfsd_debug("create\n");
bzero(&diropres, sizeof(diropres));
vxfhp = (vxfh_t *) & (createargp->where.dir);
if ((pathp = fh2path(vxfhp)) == NULL) {
diropres.status = NFSERR_NOENT;
return (&diropres);
}
if (pathCat(pathp, createargp->where.name, fullpath) != OK) {
diropres.status = errnoGet();
return (&diropres);
}
if (stat(fullpath, &stat_holder) == OK) {
diropres.status = NFSERR_EXIST;
return (&diropres);
}
nfsd_debug("creat: mode %x uid %x gid %x sz %x atime %x mtime %x\n",
createargp->attributes.mode,
createargp->attributes.uid,
createargp->attributes.gid,
createargp->attributes.size,
createargp->attributes.atime,
createargp->attributes.mtime
);
if ((fd = open(fullpath, O_RDWR | O_CREAT | O_TRUNC,
createargp->attributes.mode)) < 0) {
diropres.status = errnoGet();
return (&diropres);
}
if (vxfhp->type == rt11FsDrvNum) {
lseek(fd, rt11_create_size, SEEK_SET);
write(fd, "$", 1);
}
close(fd);
vxfhp = (vxfh_t *) & diropres.diropres_u.diropres.file;
if (newfh(vxfhp, fullpath) == OK &&
get_attrstat(fullpath,
&diropres.diropres_u.diropres.attributes,
vxfhp) == OK) {
diropres.status = NFS_OK;
return (&diropres);
}
diropres.status = errnoGet();
return (&diropres);
}
/**
* Handles errors generated by task related code.
*/
bool Task::HandleError(STATUS results)
{
if (results != ERROR) return true;
switch(errnoGet())
{
case S_objLib_OBJ_ID_ERROR:
wpi_setWPIErrorWithContext(TaskIDError, m_taskName);
break;
case S_objLib_OBJ_DELETED:
wpi_setWPIErrorWithContext(TaskDeletedError, m_taskName);
break;
case S_taskLib_ILLEGAL_OPTIONS:
wpi_setWPIErrorWithContext(TaskOptionsError, m_taskName);
break;
case S_memLib_NOT_ENOUGH_MEMORY:
wpi_setWPIErrorWithContext(TaskMemoryError, m_taskName);
break;
case S_taskLib_ILLEGAL_PRIORITY:
wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName);
break;
default:
printErrno(errnoGet());
wpi_setWPIErrorWithContext(TaskError, m_taskName);
}
return false;
}
int $module$$request$RqstAndRcv (CLIENT_ID clientId,
$input$
$output$
int *bilan)
{
int rqstId; /* Ou` mettre l'id de la requete */
int status;
errnoSet(0);
/* Emettre la requete */
if (csClientRqstSend (clientId, $requestNum$, (void *) $inputName$,
$inputSize$, (FUNCPTR) NULL, FALSE, 0,
TIME_WAIT_REPLY, &rqstId) == ERROR) {
*bilan = errnoGet();
return(ERROR);
}
/* Reception de la replique */
if ((status = csClientReplyRcv (clientId, rqstId, BLOCK_ON_FINAL_REPLY,
(void *) NULL, 0, (FUNCPTR) NULL,
(void *) $outputName$, $outputSize$,
(FUNCPTR) NULL)) == ERROR) {
*bilan = errnoGet();
if (H2_MODULE_ERR_FLAG(*bilan)) return(FINAL_REPLY_OK);
}
else *bilan = OK;
return(status);
}
$module$CntrlTask(void *arg)
{
static SERV_ID $module$ServId; /* Id du serveur */
/* Routine d'initialisation */
CNTRL_TASK_STATUS = $module$CntrlInitTask (&$module$ServId);
if (CNTRL_TASK_STATUS == ERROR)
CNTRL_TASK_BILAN = errnoGet();
else
CNTRL_TASK_BILAN = OK;
/* Donner le sem de fin d'initialisation */
semGive (sem$module$CntrlTaskInit) ;
/* Se suspendre en cas de probleme */
if (CNTRL_TASK_STATUS == ERROR) {
$module$CntrlTaskSuspend(FALSE);
}
moduleEventCntrl.moduleNum = $numModule$;
/* Record a signal handler */
$module$SignalAbort = FALSE;
signal(SIGTERM, $module$SignalHandler);
/* Boucler indefiniment */
FOREVER
{
/* Bloquer dans l'attente d'evenements internes et externes */
if (h2evnSusp (0) != TRUE)
$module$CntrlTaskSuspend (FALSE);
/* Prendre l'acces aux SDI */
commonStructTake ((void *) $module$CntrlStrId);
commonStructTake ((void *) $module$DataStrId);
if ($module$SignalAbort) {
$module$SignalEnd();
}
/* Traiter les evenements internes */
$module$CntrlIntEvnExec ($module$ServId);
/* Traiter les evenements externes */
$module$CntrlExtEvnExec ($module$ServId);
/* Lancer les activites en attentes */
$module$SpawnActivities($module$ServId);
/* Mettre a jour le poster de controle */
$module$CntrlPosterWrite();
CNTRL_TASK_BILAN = errnoGet();
/* Liberer l'acces aux SDI */
commonStructGive ((void *) $module$DataStrId);
commonStructGive ((void *) $module$CntrlStrId);
}
return NULL;
}
POSTER_ID init_data (const char* poster_name, const char* reference_frame,
float confidence, int* ok)
{
POSTER_ID id;
STATUS s = posterCreate (poster_name, sizeof(POM_ME_POS), &id);
if (s == ERROR)
{
char buf[1024];
h2getErrMsg(errnoGet(), buf, sizeof(buf));
printf ("Unable to create the %s poster : %s\n", poster_name, buf);
*ok = 0;
return (NULL);
}
printf ("INIT ID = %p (pointer)\n", id);
ref_name = strdup(reference_frame);
POM_ME_POS* pos = posterAddr(id);
memset(pos, 0, sizeof(POM_ME_POS));
pos->kind = POM_ME_ABSOLUTE;
pos->confidence = confidence;
*ok = 1;
return (id);
}
UGL_STATUS uglOSMsgQPost (
UGL_OS_MSG_Q_ID qId,
UGL_TIMEOUT timeout,
void *pMsg,
UGL_SIZE msgSize
) {
UGL_STATUS status;
int ern;
if (timeout != UGL_WAIT_FOREVER) {
timeout = timeout * sysClockRateGet() / 1000;
}
ern = errnoGet();
if (msgQSend(qId, (char *) pMsg, msgSize, timeout, 0)) {
switch(ern) {
case S_objLib_UNAVAILABLE:
errnoSet(ern);
status = UGL_STATUS_Q_FULL;
break;
default:
status = UGL_STATUS_ERROR;
break;
}
}
else {
status = UGL_STATUS_OK;
}
return status;
}
int et_drvTxRxInit(char * dev, int unit)
{
if (et_drv_txrx_info[unit].et_drvMuxBindID != NULL) {
return -1;
}
et_drv_txrx_info[unit].queue_rx_pkts = 0;
et_drvTxRxNetpoolCreate(unit);
/* Bind et_drvTxRx Network service to the END driver. */
et_drv_txrx_info[unit].et_drvMuxBindID = (void *) muxBind(
dev, unit,
et_drvRecv, NULL,
NULL, NULL,
MUX_PROTO_SNARF, "ET_DRV TX/RX", NULL);
if (!et_drv_txrx_info[unit].et_drvMuxBindID)
{
logMsg("muxTkBind Failed (%08x).\n", errnoGet(), 2, 3, 4, 5, 6);
return -1;
}
/* Promiscuous mode */
muxIoctl( et_drv_txrx_info[unit].et_drvMuxBindID, EIOCSFLAGS, (void *)IFF_PROMISC);
return 0;
}
STATUS virtualStackNumTaskIdSet
(
int vsNum
)
{
/*
* If we this task does not have the task variable then
* create it as part of this task.
*/
if (taskVarGet(0, (int *) &myStackNum) == ERROR)
{
if (errnoGet () == S_taskLib_TASK_VAR_NOT_FOUND)
errnoSet (0); /* Resets the error caused by taskVarGet */
taskVarAdd (0, (int *) &myStackNum);
}
if (vsNum > VSID_MAX)
return (ERROR);
if (vsTbl[vsNum] == NULL)
return (ERROR);
myStackNum = vsNum;
return (OK);
}
void roboTxRxInit()
{
END_OBJ *pEndObj = NULL;
if (robo_txrx_info.roboMuxBindID != NULL) {
return;
}
roboTxRxNetpoolCreate();
/* Bind roboTxRx Network service to the END driver. */
robo_txrx_info.roboMuxBindID = (void *) muxTkBind(
ROBO_MII_DEVICE_NAME, ROBO_MII_DEV_UNIT,
roboRecv, roboTxShutdownRtn,
roboTxRestartRtn, roboErrorRtn,
MUX_PROTO_SNARF, "ROBO TX/RX",
pEndObj,(void *) 0, (void *) 0);
if (!robo_txrx_info.roboMuxBindID)
{
logMsg("muxTkBind Failed (%08x).\n", errnoGet(), 2, 3, 4, 5, 6);
return;
}
/* Promiscuous mode */
muxIoctl( robo_txrx_info.roboMuxBindID, EIOCSFLAGS, (void *)IFF_PROMISC);
}
void createSTBMsgQs(void)
{
queue_conf_t *pQueue = &startup_queues[0];
int i, n;
for (i = 0, n = 0; pQueue->pQid != NULL; i++, pQueue++)
{
#ifdef _STB_CONF_DEBUG_
tprint0n("creating Queue %8.8s ...", pQueue->name);
#endif
*pQueue->pQid = createMsg(pQueue->name,
pQueue->max_num,
pQueue->max_len,
pQueue->priority );
if (*pQueue->pQid == NULL)
{
int err=0;
#ifdef _VXWORKS_
err = errnoGet();
#endif /* _VXWORKS_ */
printf("Create Queue[%8.8s] failed errno %08x\n", pQueue->name, err);
}
else
{
#ifdef _STB_CONF_DEBUG_
rprint1n(" qid is 0x%08x", *pQueue->pQid);
#endif
n++;
}
}
dbgprint("%3d Message queues are created", n);
return;
}
static int
Velodyne_init(Velodyne *self, PyObject *args, PyObject *kwds)
{
PyObject *images;
char* poster_name;
if (!PyArg_ParseTuple(args, "snnn", &poster_name, &(self->width), &(self->height), &(self->nb_rot)))
return -1;
size_t poster_size = sizeof(velodyne3DImage);
STATUS s = posterCreate (poster_name, poster_size, &self->id);
if (s == ERROR)
{
char buf[1024];
h2getErrMsg(errnoGet(), buf, sizeof(buf));
printf ("Unable to create the %s poster : %s\n",poster_name, buf);
return -1;
}
printf("Succesfully created poster %s of size %zd\n", poster_name, poster_size);
self->size = self->width * self->height;
self->img.width = VELODYNE_3D_IMAGE_WIDTH;
self->img.maxScanWidth = VELODYNE_3D_IMAGE_WIDTH;
self->img.height = (int)(ceil)(self->size * self->nb_rot / VELODYNE_3D_IMAGE_WIDTH);
self->current_rot = 0;
return 0;
}
bool Clean(Id*& id)
{
bool ret = true;
if (id && taskIdVerify(id->TaskId) == OK)
{
Id me(taskIdSelf());
if (me == *id) // this mean just release the resources that we allocated here (not the task itself)
{
delete id; // we can deallocate our task resources, but we cannot delete the task itself
id = 0;
}
else
{
//printf("deleting task id %d\r\n", id->TaskId);
if (taskDelete(id->TaskId) != OK)
{
ret = HandleDeleteErrors(errnoGet(), __LINE__);
}
if (ret)
{
delete id;
id = 0;
}
}
}
return ret;
}
attrstat *
nfsproc_getattr_2(nfs_fh * fhp, struct svc_req * reqp)
{
int fd;
struct fattr *fattrp;
static attrstat attrstat, *attrstatp;
char *pathp;
nfsd_debug("getattr\n");
attrstatp = &attrstat;
bzero(attrstatp, sizeof(*attrstatp));
fattrp = &attrstatp->attrstat_u.attributes;
if ((pathp = fh2path((vxfh_t *) fhp)) == NULL) {
attrstat.status = NFSERR_NOENT;
return (&attrstat);
}
if (get_attrstat(pathp, fattrp, (vxfh_t *) fhp) == OK) {
attrstat.status = NFS_OK;
return (&attrstat);
}
attrstat.status = errnoGet();
return (&attrstat);
}
diropres *
nfsproc_mkdir_2(createargs * createargp, struct svc_req * reqp)
{
static diropres diropres;
vxfh_t *vxfhp;
char *pathp;
struct stat stat_holder;
char fullpath[PATH_MAX];
nfsd_debug("mkdir\n");
bzero(&diropres, sizeof(diropres));
vxfhp = (vxfh_t *) & (createargp->where.dir);
if (vxfhp->type != dosFsDrvNum) {
diropres.status = NFSERR_IO; /* XXX bogus */
return (&diropres);
}
if ((pathp = fh2path(vxfhp)) == NULL) {
diropres.status = NFSERR_NOENT;
return (&diropres);
}
if (pathCat(pathp, createargp->where.name, fullpath) != OK) {
diropres.status = errnoGet();
return (&diropres);
}
if (stat(fullpath, &stat_holder) == OK) {
diropres.status = NFSERR_EXIST;
return (&diropres);
}
nfsd_debug("mkdir: attrib.mode 0%o\n", createargp->attributes.mode);
if (mkdir(fullpath) == ERROR) {
diropres.status = errnoGet();
return (&diropres);
}
if (newfh(vxfhp, fullpath) == OK &&
get_attrstat(fullpath,
&diropres.diropres_u.diropres.attributes,
vxfhp) == OK) {
diropres.status = NFS_OK;
return (&diropres);
}
diropres.status = errnoGet();
return (&diropres);
}
void Wait(Id* on)
{
assert(on);
if (!on->Take())
{
ErrorFunctionsHandler().CriticalError(errnoGet(), __FUNCTION__, __LINE__);
}
}
void
nfsd_debugno()
{
if (!nfsd_debug_on)
return;
printErrno(errnoGet());
}
void SignalAll(Id* on)
{
assert(on);
if (!on->ReleaseAll())
{
ErrorFunctionsHandler().CriticalError(errnoGet(), __FUNCTION__, __LINE__);
}
}
Id* Create()
{
std::auto_ptr<Id> id(new Id);
if (!id->Open())
{
ErrorFunctionsHandler().CriticalError(errnoGet(), __FUNCTION__, __LINE__);
}
return id.release(); // give the pointer back to the user
}
int
main(int argc, char **argv)
{
char *nom;
int i;
POSTER_ID id;
if (argc == 1) {
nom = "test";
} else {
nom = argv[1];
}
if (posterCreate(nom, sizeof(int), &id) != OK) {
printf("Error posterCreate %x\n", errnoGet());
h2printErrno(errnoGet());
exit(-1);
}
while (1) {
scanf("%d", &i);
/*
posterWrite(id, 0, &i, sizeof(int));
*/
if (posterTake(id, POSTER_WRITE) == ERROR) {
fprintf(stderr, "Error posterTake: ");
h2printErrno(errnoGet());
}
*(int *)posterAddr(id) = i;
if (posterGive(id) == ERROR) {
fprintf(stderr, "Error posterGive: ");
h2printErrno(errnoGet());
}
if (i == -1) {
printf("Bye\n");
posterDelete(id);
exit(0);
}
}
return 0;
}
PriorityType Priority(Id* to)
{
assert(to);
int oldPrio = -1;
if (taskPriorityGet(to->TaskId, &oldPrio) != OK)
{
ExitFunctionsThreadList().CriticalError(errnoGet(), __FUNCTION__, __LINE__);
}
return FromOSPriority(oldPrio);
}
void NewPriority(Id* to, PriorityType newPrio)
{
assert(to);
// do set a new priority if the one pass to this function is is invalid!
if (newPrio != INVALID_PRIORITY)
{
if (taskPrioritySet(to->TaskId, GetOSPriority(newPrio)) != OK)
{
ExitFunctionsThreadList().CriticalError(errnoGet(), __FUNCTION__, __LINE__);
}
}
}
bool TryWait(Id* on)
{
assert(on);
if (!on->TryTake())
{
return ErrorFunctionsHandler().TryFail(errnoGet(), __FUNCTION__, __LINE__);
}
else
{
return true;
}
}
bool TimedWait(Id* on, milliseconds_t milliDuration)
{
assert(on);
if (!on->Take(TimeUtils::Milli2Ticks(milliDuration)))
{
return ErrorFunctionsHandler().TimedOut(errnoGet(), __FUNCTION__, __LINE__);
}
else
{
return true;
}
}
/*
* Create a poster, and fill it with information which don't change during the
* execution, including bank_name, camera_name, nb_images, size of images,
* camera calibration...
*
* Return a POSTER_ID on success, NULL otherwise
*
* you must call finalize when you don't use anymore the POSTER_ID
*/
POSTER_ID init_data (char* poster_name)
{
POSTER_ID id;
STATUS s = posterCreate (poster_name, sizeof(SICK_CART_MEASURES_STR), &id);
if (s == ERROR)
{
char buf[1024];
h2getErrMsg(errnoGet(), buf, sizeof(buf));
printf ("Unable to create the %s poster : %s\n",poster_name, buf);
return (NULL);
}
return (id);
}
attrstat *
nfsproc_write_2(writeargs * writeargp, struct svc_req * reqp)
{
static attrstat attrstat;
int fd;
char *pathp;
vxfh_t *vxfhp;
nfsd_debug("write\n");
bzero(&attrstat, sizeof(attrstat));
vxfhp = (vxfh_t *) & (writeargp->file);
if ((pathp = fh2path(vxfhp)) == NULL) {
attrstat.status = NFSERR_NOENT;
return (&attrstat);
}
if ((fd = open((char *) pathp, O_WRONLY, 0)) < 0) {
attrstat.status = NFSERR_NXIO;
nfsd_debugno();
return (&attrstat);
}
nfsd_debug("writearg begoff %d off %d totlen %d len %d\n",
writeargp->beginoffset, /* unused */
writeargp->offset,
writeargp->totalcount, /* unused */
writeargp->data.data_len);
if (lseek(fd, writeargp->offset, SEEK_SET) < 0 ||
write(fd, writeargp->data.data_val,
writeargp->data.data_len) < 0) {
nfsd_debugno();
attrstat.status = NFSERR_IO;
close(fd);
return (&attrstat);
}
close(fd);
if (get_attrstat((char *) pathp,
&(attrstat.attrstat_u.attributes), vxfhp) != OK) {
attrstat.status = errnoGet();
return (&attrstat);
}
attrstat.status = NFS_OK;
return (&attrstat);
}
int Reader::Read()
{
fd_set readFdSet;
int retval = ERROR;
FD_ZERO(&readFdSet);
FD_SET(m_inputStreamFd, &readFdSet);
if (select(FD_SETSIZE, &readFdSet, NULL, NULL, NULL) != ERROR)
{
if (FD_ISSET(m_inputStreamFd, &readFdSet))
{
char readbuf;
retval = recv(m_inputStreamFd, &readbuf, 1, 0);
m_lastByte = readbuf;
if (retval != ERROR && retval > 0)
{
#ifdef DEBUG
if (m_lastByte != kNetworkTables_PING)
{
char pbuf[6];
snprintf(pbuf, 6, "I:%02X\n", m_lastByte);
printf(pbuf);
}
#endif
return m_lastByte;
}
}
}
else if (!m_connection->IsConnected())
{
// The error came from us closing the socket
return 0;
}
// TODO: Should we ignore ECONNRESET errors?
if (retval == ERROR)
{
char buf[32] = "";
int err = errnoGet();
snprintf(buf, 32, "errno=%d", err);
wpi_setStaticWPIErrorWithContext(m_connection, NetworkTablesReadError, buf);
printErrno(err);
}
m_connection->Close();
return 0;
}
Id* Create(const Attributes& attr, entry_func_t entryFunc)
{
int tid = taskSpawn((char*)attr.Name, GetOSPriority(attr.Priority),
VX_FP_TASK, attr.StackSize, (FUNCPTR)entryFunc,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
);
if (tid != ERROR)
{
//printf("created task id %d\r\n", tid);
return new Id(tid); // return the new task object
}
else
{
ExitFunctionsThreadList().CriticalError(errnoGet(), __FUNCTION__, __LINE__);
return 0; // this is just so we would not have warning, it should not return with this NULL!!
}
}
STATUS cd(
const char *path
)
{
STATUS status;
if (ioDefPathCat((char *) path) != OK)
{
fprintf(stderr, "cd: error = %#x\n", errnoGet());
status = ERROR;
}
else
{
status = OK;
}
return status;
}
fhstatus *
mountproc_mnt_1(dirpath * dirpathp, struct svc_req * reqp)
{
static fhstatus fhstatus;
DEV_HDR *dev_hdrp;
char *dummy;
vxfh_t *vxfhp;
nfsd_debug("mount mnt\n");
bzero(&fhstatus, sizeof(fhstatus));
/*
* we only export filesystem device at the root. currently no config
* file is used. all rt11 and msdos filesystem devices are exported.
* no security & option support either.
*/
if ((dev_hdrp = iosDevFind(*dirpathp, &dummy)) == NULL) {
fhstatus.fhs_status = errnoGet();
return (&fhstatus);
}
if (dev_hdrp->drvNum == rt11FsDrvNum) {
fhstatus.fhs_status = NFS_OK;
vxfhp = (vxfh_t *) & (fhstatus.fhstatus_u.fhs_fhandle);
vxfhp->type = rt11FsDrvNum;
strcpy(vxfhp->opaque, (char *) (*dirpathp));
if (nfsd_debug_on)
d(vxfhp, sizeof(*vxfhp));
return (&fhstatus);
}
if (dev_hdrp->drvNum == dosFsDrvNum) {
vxfhp = (vxfh_t *) & (fhstatus.fhstatus_u.fhs_fhandle);
if (newfh(vxfhp, *dirpathp) == ERROR) {
fhstatus.fhs_status = NFSERR_NODEV;
return (&fhstatus);
}
fhstatus.fhs_status = NFS_OK;
return (&fhstatus);
}
fhstatus.fhs_status = NFSERR_NODEV;
return (&fhstatus);
}
int testBoxing(){
#ifdef test
box toFill = {5,1,0,0,42};
box done;
int ret = 0;
MSG_Q_ID mid_boxing_todo = msgQCreate(10,4,0);
MSG_Q_ID mid_boxing_done = msgQCreate(10,4,0);
MSG_Q_ID mid_received_part = msgQCreate(10,4,0);
printf("Starting conditionning");
taskSpawn("testBoxing",10,0,15000,(FUNCPTR)startBoxing,0,0,0,0,0,0,0,0,0,0);
printf("Asking for batch production : \n");
printf("\tBatchNumber=%d\n",toFill.batchNumber);
printf("\tBoxSize=%d\n",toFill.size);
printf("\tPartsType=%d\n",toFill.partsType);
ret=msgQSend(mid_boxing_todo,(char*)&toFill,sizeof(toFill),WAIT_FOREVER,MSG_PRI_NORMAL);
if(ret==ERROR){
printf("[FAIL],");
printErrno(errnoGet());
printf("\n");
return;
}
printf("[OK]\n");
printf("Starting part type 1 generation...");
if (partProdTid!=0){
printf("[FAIL], Production already started.\n");
return;
}
partProdTid=taskSpawn("partProd1",5,0,10000,(FUNCPTR)startPartProd,1,0,0,0,0,0,0,0,0,0);
printf("[OK]\n");
for(;;){
printf("Waiting for boxes done...\n");
if (msgQReceive(mid_boxing_done,(char*)&done,sizeof(done),5)==-1){
return 1;
}
printf("Received :\n");
printf("\t Box size : %d\n",done.size);
printf("\t Part type : %d\n",done.partsType);
}
#else
printf("You must compile the application with 'test' defined to use this functionality!\n");
#endif
return 0;
}
