/*
* Get the current priority of a task
*/
STATUS
taskPriorityGet(long tid, int *pPriority)
{
OS_TCB *tcb = (OS_TCB *)tid;
#ifdef HAVE_PTHREAD_ATTR_SETSCHEDPOLICY
struct sched_param my_param;
int my_policy;
int status;
if (tcb->magic != TASK_MAGIC) {
errnoSet(S_portLib_INVALID_TASKID);
return ERROR;
}
status = pthread_getschedparam(tcb->tid, &my_policy, &my_param);
if (status != 0) {
errnoSet(status);
return ERROR;
}
if (pPriority != NULL) {
*pPriority = my_param.sched_priority;
}
#else
*pPriority = tcb->priority;
#endif
return OK;
}
STATUS hostGetByAddr
(
int addr, /* inet address of host */
char *name /* buffer to hold name */
)
{
STATUS status;
if (name == NULL)
{
errnoSet (S_hostLib_INVALID_PARAMETER);
return (ERROR);
}
/* Search the host table using the host address as the key */
status = hostTblSearchByAddr (addr, name);
if ((status != OK) && (_presolvHostLibGetByAddr != NULL))
{
/*
* If host name was not found in the local table. Try to get the
* name from the DNS server, if and only if the resolver library has
* been linked with the Vxworks image.
*/
status = (*_presolvHostLibGetByAddr) (addr, name);
}
if (status != OK)
errnoSet (S_hostLib_UNKNOWN_HOST);
return (status);
}
/*
* Delete a task
*/
STATUS
taskDelete(long tid)
{
OS_TCB *tcb = (OS_TCB *)tid;
int status;
if (tid == 0) {
static int status = 0;
pthread_exit(&status);
}
if (tcb->magic != TASK_MAGIC) {
errnoSet(S_portLib_INVALID_TASKID);
return ERROR;
}
status = pthread_cancel(tcb->tid);
if (status != 0) {
errnoSet(status);
return ERROR;
}
/* Removing the task from the tasklist
is done in the thread cleanup handler */
return OK;
}
/*
* define a new priority for a task
*/
STATUS
taskPrioritySet(long tid, int newPriority)
{
OS_TCB *tcb;
#ifdef HAVE_PTHREAD_ATTR_SETSCHEDPOLICY
struct sched_param my_param;
int status;
#endif
if (tid == 0)
tid = taskIdSelf();
tcb = (OS_TCB *)tid;
if (tcb->magic != TASK_MAGIC) {
errnoSet(S_portLib_INVALID_TASKID);
return ERROR;
}
#ifdef HAVE_PTHREAD_ATTR_SETSCHEDPOLICY
my_param.sched_priority = newPriority;
status = pthread_setschedparam(tcb->tid, SCHED_RR, &my_param);
if (status != 0) {
errnoSet(status);
return ERROR;
}
#endif
tcb->priority = newPriority;
return OK;
}
/*
* ios/user functions API : open,close,read
*/
int devOpen(myDev * dev, char * name, int mode)
{
fifo * i = 0;
//Create a new fifo for this app and initialize it :
fifo * newFifo = malloc(sizeof(fifo));
if(newFifo==NULL)
{
errnoSet(MEM_OVERFLOW);
return -1;
}
initFifo(newFifo);
newFifo->taskId = taskIdSelf();
//From now on we'll acces the device data, let's require the sem :
if(semTake(dev->semMData,WAIT_FOREVER)!=OK)
{
free(newFifo);
errnoSet(SEM_ERR);
return -1;
}
dev->openned++; //keep track of the number of tasks that have openned this device
dev->mode = mode; //this seems useless, todo : check
//Add fifo at end of listeFifo :
if (dev->firstFifo==NULL)
{
dev->firstFifo = newFifo;
} else {
for (i=dev->firstFifo;i->nextFifo!=NULL;i=i->nextFifo);
i->nextFifo=newFifo;
}
semGive(dev->semMData);
return (int)dev; //pointer returned is used as arg in devClose!
}
BOOL
h2rngIsFull(H2RNG_ID rngId)
{
int dp;
if (rngId == NULL) {
errnoSet(S_h2rngLib_NOT_A_RING);
return ERROR;
}
/* Determine l'ecart entre les pointeurs de lecture et d'ecriture */
dp = rngId->pRd - rngId->pWr;
/* Verifier le type du ring buffer */
switch (rngId->flgInit)
{
case H2RNG_INIT_BYTE: /* Ring buffer type byte */
return (dp == 1 || dp == 1 - rngId->size);
case H2RNG_INIT_BLOCK: /* Ring buffer type "block" */
if (dp > 0 )
return (dp < 12);
return (dp < 12 - rngId->size);
default: /* Type inconnu */
/* Indiquer l'erreur */
errnoSet (S_h2rngLib_NOT_A_RING);
return (ERROR);
}
}
/**
*
* @param desc
* @param request
* @param value
* @return OK en cas de succès, ERROR sinon.
*/
static int pe_ioctl_change_cpt(PEDEV* desc, int request, int value)
{
int i;
if(value >= 0 && value <= 255)
{
semTake(mut_table_capt, WAIT_FOREVER);
for(i=0; i < DEVICE_MAX_COUNT; ++i)
{
if(table_capt[i].specific.state == opened &&
table_capt[i].specific.address == value)
{
errnoSet(ECPTALREADYUSED);
return ERROR;
}
}
desc->specific.address = value;
semGive(mut_table_capt);
return OK;
}
else
{
errnoSet(EARG);
return ERROR;
}
}
/**
* @brief Ajouter un périphérique.
* @param i
* @return OK en cas de succès, ERROR sinon.
*/
int pe_deviceAdd(int i)
{
// Buffer pour le nom de périphérique.
char dev_name[NAME_SIZE];
if(driver_id == -1)
{
errnoSet(S_ioLib_NO_DRIVER);
return ERROR;
}
sprintf(dev_name, DEVICE_BASENAME, i);
if(iosDevAdd((DEV_HDR*)&(table_capt[i]),dev_name,driver_id) == ERROR)
{
errnoSet(ECANNOTADD);
return ERROR;
}
semTake(mut_table_capt, WAIT_FOREVER);
// Le capteur est fermé
table_capt[i].specific.state = closed;
// Il n'a pas encore de capteur associé, au niveau CAN.
// utiliser ioctl pour lui en founir une.
table_capt[i].specific.address = -1;
semGive(mut_table_capt);
return OK;
}
int clock_settime(
clockid_t id,
const struct timespec *tp
)
{
int ret;
if (id != CLOCK_REALTIME)
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
if ((tp == NULL) || (tp->tv_sec < 0) || (tp->tv_nsec >= BILLION))
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
_clockRealtime.tickBase = tick64Get();
_clockRealtime.timeBase = *tp;
ret = OK;
}
}
return ret;
}
int clock_gettime(
clockid_t id,
struct timespec *tp
)
{
int ret;
u_int64_t diff;
if (id != CLOCK_REALTIME)
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
if (tp == NULL)
{
errnoSet(EINVAL);
ret = ERROR;
}
else
{
diff = tick64Get() - _clockRealtime.tickBase;
TV_CONVERT_TO_SEC(*tp, diff);
TV_ADD(*tp, _clockRealtime.timeBase);
ret = OK;
}
}
return ret;
}
STATUS
gcomLetterReply(MBOX_ID mboxId, int sendId, int replyLetterType,
LETTER_ID letterReply)
{
LETTER_HDR_ID pText; /* Pointeur vers texte de la lettre */
/* Verifier le type de lettre de replique a envoyer */
if (replyLetterType != INTERMED_REPLY &&
replyLetterType != FINAL_REPLY) {
errnoSet (S_gcomLib_REPLY_LETTER_TYPE);
return (ERROR);
}
/* Verifier l'initialisation de la lettre */
if (letterReply->flagInit != GCOM_FLAG_INIT) {
errnoSet (S_gcomLib_NOT_A_LETTER);
return (ERROR);
}
/* Obtenir le pointeur vers le texte de la lettre */
pText = letterReply->pHdr;
/* Remplir l'en-tete de la lettre de replique */
pText->sendId = sendId;
pText->type = replyLetterType;
/* Envoyer la lettre */
return (mboxSend (mboxId, rcvMboxTab[MY_TASK_DEV], (char *) pText,
pText->dataSize + sizeof (LETTER_HDR)));
}
int hostGetByName
(
char *name /* name of host */
)
{
int retAddr;
if (name == (char *) NULL)
{
errnoSet (S_hostLib_INVALID_PARAMETER);
return (ERROR);
}
/* Search the host table using the host name as the key */
retAddr = hostTblSearchByName (name);
if ((retAddr == ERROR) && (_presolvHostLibGetByName != NULL))
{
/*
* If host address was not found in the local table. Try to get the
* IP from the DNS server, if and only if the resolver library has
* been linked with the Vxworks image.
*/
retAddr = (*_presolvHostLibGetByName) (name);
}
if (retAddr == ERROR)
errnoSet (S_hostLib_UNKNOWN_HOST);
return (retAddr);
}
/**
*
* @param desc
* @param buff
* @param nBytes
* @return
*/
static int pe_read (PEDEV* desc, char* buff, int nBytes)
{
int device;
if(nBytes < 0)
{
errnoSet(EARG);
return ERROR;
}
/*
* Pour trouver l'index du capteur dans le tableau de capteur,
* 2 solutions sont possible :
*
* - chercher une correspondance entre l'adresse
* renvoyé par le descripteur de fichier et chaucun des capteurs du tableau de capteur.
* - calculer l'indice en soustrayant l'adresse du descripteur de fichier a l'adresse
* du tableau de capteur.
*
* Pour une question d'efficacité, nous avons préféré calculer l'indice,
* cependant, cette methode ne peut pas être considéré comme sûr, nous avons donc laissé
* l'autre solution imaginé.
*/
//semTake(mut_table_capt, WAIT_FOREVER);
// On cherche si
/*for(i=0; i < DEVICE_MAX_COUNT; ++i)
{
if(table_capt[i].specific.state == opened &&
table_capt[i].specific.address == desc->specific.address)
{
semTake(mut_table_buffer, WAIT_FOREVER);
memcpy(buff, &(table_buffer[i]), nBytes);
semGive(mut_table_buffer);
break;
}
}
semGive(mut_table_capt);
*/
// On clacule l'index du capteur en comparant l'adresse de desc a celle du tableau.
device = ((int)desc - (int)&(table_capt))/sizeof(PEDEV);
semTake(mut_table_buffer, WAIT_FOREVER);
if(table_buffer[device].id == -1)
{
semGive(mut_table_buffer);
errnoSet(ENOAVAIL);
return ERROR;
}
memcpy(buff, &(table_buffer[device]), nBytes);
semGive(mut_table_buffer);
return sizeof(Message);
}
H2RNG_ID
h2rngCreate(int type, /* Type du ring buffer */
int nbytes) /* Nombre de bytes */
{
H2RNG_ID rngId; /* Pointeur vers tete */
int flgInit; /* Flag d'initialisation */
/* Verifier si le nombre de bytes est positif */
if (nbytes <= 0) {
errnoSet (S_h2rngLib_ILLEGAL_NBYTES);
return ((H2RNG_ID) NULL);
}
/* Verifier le type demande de ring buffer */
switch (type) {
case H2RNG_TYPE_BYTE: /* Ring buffer type byte */
/* Taille du ring buffer */
nbytes = nbytes + 1;
/* Indiquer l'initialisation */
flgInit = H2RNG_INIT_BYTE;
break;
case H2RNG_TYPE_BLOCK: /* Ring buffer type "block" */
/* Taille du ring buffer */
nbytes = nbytes + 12 - (nbytes & 3);
/* Indiquer l'initialisation */
flgInit = H2RNG_INIT_BLOCK;
break;
default: /* Types inconnus */
/* Indiquer l'erreur */
errnoSet (S_h2rngLib_ILLEGAL_TYPE);
return ((H2RNG_ID) NULL);
} /* switch */
/* Allouer memoire pour l'en-tete et pour le buffer */
if ((rngId = (H2RNG_ID)
smMemMalloc ((size_t) (nbytes + sizeof (H2RNG_HDR)))) == NULL) {
return ((H2RNG_ID) NULL);
}
/* Initialiser l' en-tete */
rngId->pRd = 0;
rngId->pWr = 0;
rngId->size = nbytes;
rngId->flgInit = flgInit;
/* Retourner le pointeur vers le ring buffer */
return (rngId);
}
int devDelete(char* devName)
{
myDev * i = first;
char * pNameTail;
myDev * dev = (myDev*) iosDevFind (devName,&pNameTail);
if ((*pNameTail)!='\0' || dev==NULL)
//If pNameTail is not '\0', either we don't have an exact match
//or we have no match at all
{
errnoSet(UNKNOWN_DEVICE);
semGive(semMAdmin);
return -1;
}
if(semMAdmin==0)
{
errnoSet(NOT_INSTALLED);
return -1;
}
if(semTake(semMAdmin,WAIT_FOREVER)==-1)
{
errnoSet(SEM_ERR);
return -1;
}
if (dev->openned != 0)
{
//There are still openned file descriptors on this device,
//we can't delete it, give back semaphores and leave.
semGive(dev->semMData);
errnoSet(OPENNED_DEV);
semGive(semMAdmin);
return -1;
}
iosDevDelete((DEV_HDR*)dev); //This only prevents further oppenings.
//Find and delete the device in device list :
if (dev==first)
{
first=dev->next;
} else {
for (i=first;i->next!=dev;i=i->next);
i->next=dev->next;
}
semTake(dev->semMData,WAIT_FOREVER); //Let pending ops on this dev finish
semDelete(dev->semMData); //We don't need to release it to delete it
free(dev);
semGive(semMAdmin);
return 0;
}
LOCAL STATUS ifIoctlCall(int cmd, struct ifreq *ifrp)
{
int err, fd;
/* Create a raw socket */
fd = socket(AF_INET, SOCK_RAW, 0);
if (fd < 0)
return ERROR;
/* Perform ioctl on socket */
err = ioctl(fd, cmd, (int) ifrp);
/* Close raw socket */
close(fd);
/* If error */
if (err) {
if (err != ERROR)
errnoSet(err);
} /* End if error */
return OK;
}
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);
}
STATUS msgQEvStart
(
MSG_Q_ID msgQId, /* msg Q for which to register events */
UINT32 events, /* 32 possible events */
UINT8 options /* event-related msg Q options */
)
{
if (events == 0x0)
{
errnoSet (S_eventLib_ZERO_EVENTS);
return (ERROR);
}
if (INT_RESTRICT () != OK)
return (ERROR); /* errno set by INT_RESTRICT() */
TASK_LOCK (); /* to prevent msg Q from being deleted */
if (OBJ_VERIFY(msgQId,msgQClassId) != OK)
{
TASK_UNLOCK ();
return (ERROR); /* errno is set by OBJ_VERIFY */
}
/* TASK_UNLOCK() will be done by eventStart() */
return (eventStart ((OBJ_ID)msgQId, &msgQId->events, &msgQEvIsFree,
events, options));
}
LOCAL STATUS taskHookDelete
(
FUNCPTR hook, /* routine to be deleted from table */
FUNCPTR table[], /* table from which to delete */
int maxEntries /* max entries in table */
)
{
FAST int ix;
taskLock (); /* disable task switching */
/* find hook in hook table */
for (ix = 0; ix < maxEntries; ++ix)
{
if (table [ix] == hook)
{
/* move all the remaining hooks up one slot in the table */
do
table [ix] = table [ix + 1];
while (table [++ix] != NULL);
taskUnlock (); /* re-enable task switching */
return (OK);
}
}
/* hook not found in table */
taskUnlock (); /* re-enable task switching */
errnoSet (S_taskLib_TASK_HOOK_NOT_FOUND);
return (ERROR);
}
LOCAL STATUS taskHookAdd
(
FUNCPTR hook, /* routine to be added to table */
FUNCPTR table[], /* table to which to add */
int maxEntries /* max entries in table */
)
{
FAST int ix;
taskLock (); /* disable task switching */
/* find slot after last hook in table */
for (ix = 0; ix < maxEntries; ++ix)
{
if (table[ix] == NULL)
{
table[ix] = hook;
taskUnlock (); /* re-enable task switching */
return (OK);
}
}
/* no free slot found */
taskUnlock (); /* re-enable task switching */
errnoSet (S_taskLib_TASK_HOOK_TABLE_FULL);
return (ERROR);
}
STATUS taskDeleteHookAdd
(
FUNCPTR deleteHook /* routine to be called when a task is deleted */
)
{
FAST int ix;
STATUS status = OK;
taskLock (); /* disable task switching */
if (taskDeleteTable [VX_MAX_TASK_DELETE_RTNS] != NULL)
{
/* no free slot found */
errnoSet (S_taskLib_TASK_HOOK_TABLE_FULL);
status = ERROR;
}
else
{
/* move all the hooks down one slot in the table */
for (ix = VX_MAX_TASK_DELETE_RTNS - 2; ix >= 0; --ix)
taskDeleteTable [ix + 1] = taskDeleteTable [ix];
taskDeleteTable [0] = deleteHook;
}
taskUnlock (); /* re-enable task switching */
return (status);
}
STATUS taskSwapHookAdd
(
FUNCPTR swapHook /* routine to be called at every task switch */
)
{
FAST int ix;
taskLock (); /* disable task switching */
/* find slot after last hook in table */
for (ix = 0; ix < VX_MAX_TASK_SWAP_RTNS; ++ix)
{
if (taskSwapTable[ix] == NULL)
{
taskSwapTable[ix] = swapHook;
taskSwapReference[ix] = 0;
taskUnlock (); /* re-enable task switching */
return (OK);
}
}
/* no free slot found */
taskUnlock (); /* re-enable task switching */
errnoSet (S_taskLib_TASK_HOOK_TABLE_FULL);
return (ERROR);
}
SEM_ID semBCreate(
int options,
SEM_B_STATE state
)
{
SEM_ID semId;
/* Check if lib is installed */
if (semBLibInstalled != TRUE)
{
errnoSet(S_semLib_NOT_INSTALLED);
semId = NULL;
}
else
{
/* Allocate memory */
semId = (SEM_ID) objAlloc(semClassId);
if (semId != NULL)
{
/* Initialze structure */
if (semBInit(semId, options, state) != OK)
{
objFree(semClassId, semId);
semId = NULL;
}
}
}
return semId;
}
STATUS semQInit(
Q_HEAD *pQHead,
int options
)
{
STATUS status;
/* Initilaize queue according to options */
switch (options & SEM_Q_MASK)
{
case SEM_Q_FIFO:
qInit(pQHead, qFifoClassId, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
status = OK;
break;
case SEM_Q_PRIORITY:
qInit(pQHead, qPrioClassId, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
status = OK;
break;
default:
errnoSet(S_semLib_INVALID_Q_TYPE);
status = ERROR;
}
return status;
}
STATUS vmContextInit(
VM_CONTEXT_ID context
)
{
STATUS status;
if (vmLibInstalled != TRUE)
{
errnoSet(S_vmLib_NOT_INSTALLED);
status = ERROR;
}
else
{
context->mmuTransTable = MMU_TRANS_TABLE_CREATE();
if (context->mmuTransTable == NULL)
{
status = ERROR;
}
else
{
semMInit(&context->sem, vmMutexOptions);
objCoreInit(&context->objCore, vmContextClassId);
status = OK;
}
}
return status;
}
VM_CONTEXT_ID vmContextCreate(
void
)
{
VM_CONTEXT_ID context;
if (vmLibInstalled != TRUE)
{
errnoSet(S_vmLib_NOT_INSTALLED);
context = NULL;
}
else
{
/* Allocate object */
context = (VM_CONTEXT *) objAlloc(vmContextClassId);
if (context != NULL)
{
/* Initialize structure */
if (vmContextInit(context) != OK)
{
objFree(vmContextClassId, context);
context = NULL;
}
}
}
return context;
}
LOCAL STATUS ifIoctlCall
(
int code, /* ioctl code */
struct ifreq *ifrp /* pointer to the interface ioctl request */
)
{
int so;
int status;
if ((so = socket (AF_INET, SOCK_RAW, 0)) < 0)
return (ERROR);
status = ioctl (so, code, (int)ifrp);
(void)close (so);
if (status != 0)
{
if (status != ERROR) /* iosIoctl() can return ERROR */
(void)errnoSet (status);
return (ERROR);
}
return (OK);
}
LOCAL STATUS ifAddrParamGet
(
char *interfaceName, /* name of interface to configure, i.e. ei0 */
int code, /* SIOCG ioctl code */
char *address /* address retrieved here */
)
{
struct in_addr inetAddrBuf; /* result */
char netString [INET_ADDR_LEN];
if (address == NULL)
{
(void)errnoSet (EINVAL);
return (ERROR);
}
if (ifIoctl (interfaceName, code, (int)&inetAddrBuf) == OK)
{
inet_ntoa_b (inetAddrBuf, netString);
strncpy (address, netString, INET_ADDR_LEN);
return (OK);
}
return (ERROR);
}
BOOL
gcomLetterRcv (LETTER_ID letter, MBOX_ID *pOrigMboxId, int *pSendId,
int timeout)
{
LETTER_HDR_ID pText; /* Pointeur vers texte de la lettre */
int rcvStatus; /* Etat de la reception */
/* Verifier l'initialisation de la lettre */
if (letter->flagInit != GCOM_FLAG_INIT) {
errnoSet (S_gcomLib_NOT_A_LETTER);
return (ERROR);
}
/* Obtenir le pointeur vers le texte de la lettre */
pText = letter->pHdr;
/* Attendre l'arrivee d'un message sur le mailbox */
if ((rcvStatus = mboxRcv (rcvMboxTab[MY_TASK_DEV], pOrigMboxId,
(char *) pText, letter->size, timeout)) <= 0)
return (rcvStatus);
/* Garder l'id du send */
*pSendId = pText->sendId;
return (TRUE);
}
STATUS
commonStructCopy(void *pCommonStruct, /* Adresse base structure */
int toFromFlag, /* Flag de direction de copie */
void *pBuf) /* Ou` mettre les donnees copiees */
{
COMMON_STRUCT_ID strId; /* Id de la structure */
/* Obtenir l'adresse de base de la structure */
strId = (COMMON_STRUCT_ID)pCommonStruct - 1;
/* Verifier si la structure existe */
if (strId->initFlag != COMMON_STRUCT_INIT_FLAG) {
errnoSet(S_commonStructLib_ISNT_COMMON_STRUCT);
return ERROR;
}
/* Wait for mutex */
semTake(strId->semId, WAIT_FOREVER);
/* Effectuer le transfert des donnees (en fonction de la direction) */
if (toFromFlag == TO)
memcpy (pBuf, pCommonStruct, strId->nBytes);
else memcpy (pCommonStruct, pBuf, strId->nBytes);
/* Signal mutex */
semGive(strId->semId);
return OK;
}
