📄 linuxwrapper.c
字号:
*/
clock_t get_time_convert(struct timeval *tv)
{
clock_t Clk;
tv->tv_sec %= 1000;
Clk = tv->tv_sec*1000000+tv->tv_usec;
return (Clk);
}
clock_t get_time_now (void)
{
struct timeval tv;
clock_t Clk;
gettimeofday(&tv, NULL);
tv.tv_sec %= 1000;
Clk = tv.tv_sec*1000000+tv.tv_usec;
return (Clk);
}
clock_t time_now (void)
{
struct timeval tv;
clock_t Clk = 1; /* Default value */
int USecPerClk; /* Micro seconds per clock (typically 64) */
if (ClkPerSec == 0)
ClkPerSec = STLINUX_GetClocksPerSecond(); /* =15625 */
if (ClkPerSec != 0)
{
USecPerClk = 1000000/ClkPerSec;
if (gettimeofday(&tv, NULL) == 0)
{
/* Restraints number of seconds */
/* This restriction is due to U32 maximum value which may be overflowed if no constraint was introduced */
tv.tv_sec %= TIME_NOW_CONSTRAINTS_ON_SECONDS - 1; /* -1 is here since all USec may add 1 sec more */
Clk = tv.tv_sec*ClkPerSec
+ tv.tv_usec/USecPerClk;
}
else
PrintTrace(("Time Now Failed: gettimeofday failed !!!!\n"));
}
else
PrintTrace(("Time Now Failed: no clocks per seconds !!!!\n"));
return (Clk);
}
/*
* time_after
*/
int time_after (clock_t Time1, clock_t Time2)
{
return ((Time1 > Time2) ? 1 : 0);
}
/*
* time_minus
*/
clock_t time_minus (clock_t Time1, clock_t Time2)
{
int TimeOffset;
if (! time_after (Time1, Time2))
{
if (ClkPerSec == 0)
ClkPerSec = STLINUX_GetClocksPerSecond();
TimeOffset = TIME_NOW_CONSTRAINTS_ON_SECONDS*ClkPerSec; /* Calculate Time offset in order to be still in modulo range after substraction */
}
else
TimeOffset = 0; /* No need of time offset, substraction result is in modulo range (Time1 > Time2) */
return (TimeOffset + Time1 - Time2);
}
/*
* time_plus
*/
clock_t time_plus (clock_t Time1, clock_t Time2)
{
if (ClkPerSec == 0)
ClkPerSec = STLINUX_GetClocksPerSecond();
if (ClkPerSec != 0)
return ((Time1 + Time2) % (TIME_NOW_CONSTRAINTS_ON_SECONDS*ClkPerSec));
else
{
PrintTrace(("Time Plus Failed: no clocks per seconds !!!!\n"));
return Time1 + Time2;
}
}
#endif
/**********************************************************/
/* Kernel Functions */
/**********************************************************/
#ifdef MODULE
int STLINUX_sched_setscheduler(pid_t pid, int policy, const struct sched_param *p)
{
int errno, result;
mm_segment_t old_fs = get_fs();
static __inline__ _syscall3(int, sched_setscheduler, pid_t, pid, int, policy, const struct sched_param *, p);
set_fs(KERNEL_DS);
result = sched_setscheduler (pid, policy, p);
set_fs(old_fs);
return result;
}
#endif
/**********************************************************/
/* STLINUX_task */
/**********************************************************/
#ifdef MODULE
ST_ErrorCode_t STLINUX_TaskCreate (void (*Function)(void* Param),
void* Param,
struct semaphore * sem_p,
int Priority,
BOOL RealTime,
const char* Name)
{
struct sched_param SchedParam;
task_t * Task_p;
printk("\nWARNING !!!: %s() is obsolete, please use preferably %s()\n\n", __FUNCTION__, "STOS_TaskCreate");
/* Unused parameter: To avoid warnings */
RealTime = RealTime;
Task_p = kthread_run((int(*)(void *))Function, (void *) Param, "%s", Name);
if( IS_ERR(Task_p) )
{
/* WARNING: Ugly hack !!! */
/* In order to keep already existing API, */
/* sem_p content is no more semaphore pointer but task_pointer, */
/* This modification is removed with STOS_Taskxxx functions ... */
*((task_t **)sem_p) = NULL;
printk("%s: Error task creation.\n", Name);
return(ST_ERROR_BAD_PARAMETER);
}
else
{
/* WARNING: Ugly hack !!! */
/* In order to keep already existing API, */
/* sem_p content is no more semaphore pointer but task_pointer, */
/* This modification is removed with STOS_Taskxxx functions ... */
*((task_t **)sem_p) = Task_p;
SchedParam.sched_priority = Priority;
if (STLINUX_sched_setscheduler(Task_p->pid, SCHED_RR, &SchedParam) < 0)
{
printk("%s: could not switch thread %d to RT\n", Name, Task_p->pid);
}
else
{
printk("%s: with RT priority done with syscall. PID=%d\n", Name, Task_p->pid);
}
printk("%s: task created ...\n", Name);
}
return(ST_NO_ERROR);
}
#endif
#ifdef MODULE
void STLINUX_TaskAskEnd (BOOL * EndFlag_p, semaphore_t * sem_p)
{
lock_kernel();
*EndFlag_p = TRUE;
if (sem_p != NULL)
{
/* Signal semaphore to release task that may wait infinitely if stopped */
semaphore_signal(sem_p);
}
unlock_kernel();
}
#endif
#ifdef MODULE
void STLINUX_TaskWaitEnd (struct semaphore * sem_p)
{
task_t * Task_p;
/* WARNING: Ugly hack !!! */
/* In order to keep already existing API, */
/* sem_p content is no more semaphore pointer but task_pointer, */
/* This modification is removed with STOS_Taskxxx functions ... */
Task_p = *((task_t **)sem_p);
kill_proc(Task_p->pid, SIGKILL, 1);
kthread_stop(Task_p);
}
#endif
#ifdef MODULE
void STLINUX_TaskStartUp (char * const ThreadName)
{
set_current_state(TASK_INTERRUPTIBLE);
allow_signal(SIGKILL);
}
#endif
#ifdef MODULE
void STLINUX_TaskEndUp (struct semaphore * sem_p)
{
/* To avoid warnings */
sem_p = sem_p;
while (!kthread_should_stop())
schedule_timeout(10); /* 10 jiffies */
}
#endif
/**********************************************************/
/* MEMORY ALLOCATION */
/**********************************************************/
#ifdef MODULE
void * memory_reallocate (partition_t* Partition, void* Block, size_t Requested, size_t OldSize)
{
void *new_Block = memory_allocate(Partition, Requested);
/* Copy over old. */
if (Requested < OldSize) OldSize = Requested;
memcpy(new_Block,Block,OldSize);
memory_deallocate(Partition, Block);
return new_Block;
}
#endif
#ifdef MODULE
void* memory_allocate_clear(partition_t* Partition, size_t nelem, size_t elsize)
{
void *result;
result = memory_allocate( Partition, nelem * elsize );
if (result)
memset(result, 0, nelem * elsize);
return result;
}
#endif
/**********************************************************/
/* DEVICES REGISTRATION */
/**********************************************************/
/*
* STLINUX_DeviceRegister
*/
#ifdef MODULE
int STLINUX_DeviceRegister(struct file_operations *stdevice_fops,
U32 nbdevices,
char *DeviceName,
unsigned int *DeviceMajor_p,
struct cdev **stdevice_cdev,
struct class_simple **stdevice_class)
{
struct class_device *class_err;
int err = 0;
int entries = 0;
dev_t base_dev_no;
char *ExtendedName;
ExtendedName = (char *)memory_allocate(NULL, strlen(DeviceName) + 11); /* +11 equals max place for U32 number + \0 */
SET_MODULE_OWNER(stdevice_fops);
/*** Set up the modules global lock ***/
/* CD: What is it used for ??? */
/* Deactivate it, to be checked ... */
/* sema_init(&(stdevice_lock), 1);*/
/*
* Register the major number. If major = 0 then a major number is auto
* allocated. The allocated number is returned.
* The major number can be seen in user space in '/proc/devices'
*/
if (*DeviceMajor_p == 0)
{
if (alloc_chrdev_region(&base_dev_no, 0, nbdevices, DeviceName))
{
err = -EBUSY;
printk(KERN_ALERT "No major numbers for %s by %s (pid %i)\n", DeviceName, current->comm, current->pid);
goto fail;
}
*DeviceMajor_p = MAJOR(base_dev_no);
}
else
{
base_dev_no = MKDEV(*DeviceMajor_p, 0);
if (register_chrdev_region(base_dev_no, nbdevices, DeviceName))
{
err = -EBUSY;
printk(KERN_ALERT "No major numbers for %s by %s (pid %i)\n", DeviceName, current->comm, current->pid);
goto fail;
}
}
if ((*stdevice_cdev = cdev_alloc()) == NULL)
{
err = -EBUSY;
printk(KERN_ALERT "No major numbers for %s by %s (pid %i)\n", DeviceName, current->comm, current->pid);
goto fail_reg;
}
sprintf(ExtendedName, "%s%d", DeviceName, 0);
(*stdevice_cdev)->owner = THIS_MODULE;
(*stdevice_cdev)->ops = stdevice_fops;
kobject_set_name(&((*stdevice_cdev)->kobj), ExtendedName);
/*** Register any register regions ***/
/*** Do the work ***/
/* Appears in /var/log/syslog */
printk(KERN_ALERT "Load module %s[%d] by %s (pid %i)\n", DeviceName, *DeviceMajor_p, current->comm, current->pid);
/*** Register the device nodes for this module ***/
/* Add the char device structure for this module */
if (cdev_add(*stdevice_cdev, base_dev_no, nbdevices))
{
err = -EBUSY;
printk(KERN_ALERT "Failed to add a charactor device for %s by %s (pid %i)\n", DeviceName, current->comm, current->pid);
goto fail_remap;
}
/* Create a class for this module */
*stdevice_class = class_simple_create(THIS_MODULE, DeviceName);
if (IS_ERR(*stdevice_class))
{
printk(KERN_ERR "Error creating %s class.\n", DeviceName);
err = -EBUSY;
goto fail_final;
}
/* Add entries into /sys for each minor number we use */
for (entries=0 ; entries < nbdevices ; entries++)
{
sprintf(ExtendedName, "%s%d", DeviceName, entries);
class_err = class_simple_device_add(*stdevice_class, MKDEV(*DeviceMajor_p, entries), NULL, ExtendedName);
if (IS_ERR(class_err))
{
printk(KERN_ERR "Error creating %s device %d.\n", DeviceName, entries);
err = PTR_ERR(class_err);
goto fail_dev;
}
}
memory_deallocate(NULL, ExtendedName);
return (0); /* If we get here then we have succeeded */
/**************************************************************************/
/*** Clean up on error ***/
fail_dev:
/* Remove any /sys entries we have added so far */
for (entries--; (entries >= 0); entries--)
{
class_simple_device_remove(MKDEV(*DeviceMajor_p, entries));
}
/* Remove the device class entry */
class_simple_destroy(*stdevice_class);
fail_final:
/* Remove the char device structure (has been added) */
cdev_del(*stdevice_cdev);
goto fail_reg;
fail_remap:
/* Remove the char device structure (has not been added) */
kobject_put(&((*stdevice_cdev)->kobj));
fail_reg:
/* Unregister the module */
unregister_chrdev_region(MKDEV(*DeviceMajor_p, 0), nbdevices);
fail :
memory_deallocate(NULL, ExtendedName);
return (err);
}
#endif
/*
* STLINUX_DeviceUnregister
*/
#ifdef MODULE
int STLINUX_DeviceUnregister(U32 nbdevices,
char *DeviceName,
unsigned int DeviceMajor,
struct cdev *stdevice_cdev,
struct class_simple *stdevice_class)
{
int i;
/* Remove any devices */
for (i = 0; (i < nbdevices); i++) {
class_simple_device_remove(MKDEV(DeviceMajor, i));
}
/* Remove the device class entry */
class_simple_destroy(stdevice_class);
/* Remove the char device structure (has been added) */
cdev_del(stdevice_cdev);
/* Unregister the module */
unregister_chrdev_region(MKDEV(DeviceMajor, 0), nbdevices);
printk(KERN_ALERT "Unload module %s by %s (pid %i)\n", DeviceName, current->comm, current->pid);
return(0);
}
#endif
#ifdef MODULE
/* ************* Pseudo random procedure (gsl_rng_rand) *********** */
unsigned long rand(void)
{
x = (1103515245 * x + 12345); /* % (2^31);*/
/* printk("rand:%lu\n",x); */
/* x = 0xA5A55A5A; */
return (x);
}
#endif
/*
* STLINUX_MapRegion
*/
#ifdef MODULE
void * STLINUX_MapRegion(void * Address_p, U32 Width, char * RegionName)
{
void * MappedAddress_p = NULL;
if (Address_p == NULL)
{
printk("Failed to remap region (%s) at 0x%.8x (lg:0x%.8x) !!!\n", RegionName, (U32)Address_p, Width);
return(NULL);
}
/* kernel checking with memory range to access */
if (check_mem_region(((U32)Address_p) | REGION2, Width) < 0)
{
printk("Region (%s) busy at %x lg %x\n", RegionName, ((U32)Address_p) | REGION2, Width);
}
/* Request the region */
request_mem_region(((U32)Address_p) | REGION2, Width, RegionName);
/* Now that we have control over the memory region, remap it to */
/* something safe for us to use in in writel/readl accesses. */
MappedAddress_p = (void *) ioremap_nocache(((U32)Address_p) | REGION2, Width);
if (MappedAddress_p == NULL)
{
release_mem_region(((U32)Address_p) | REGION2, Width);
printk("Failed to remap region (%s) at 0x%.8x (lg:0x%.8x) !!!\n", RegionName, (U32)Address_p, Width);
}
return(MappedAddress_p);
}
#endif
/*
* STLINUX_UnmapRegion
*/
#ifdef MODULE
void STLINUX_UnmapRegion(void * MappedAddress_p, U32 Width)
{
if (MappedAddress_p == NULL)
{
printk("Failed to unmap region 0x%.8x (lg:0x%.8x) !!!\n", (U32)MappedAddress_p, Width);
return;
}
if (MappedAddress_p != NULL)
{
if (check_mem_region((U32)MappedAddress_p, Width) < 0)
{
release_mem_region((U32)MappedAddress_p, Width);
printk ("The I/O memory region 0x%.8x is released successfully \n", (U32)MappedAddress_p);
}
else
{
printk("%s(): 0x%.8x Non existing region...\n", __FUNCTION__, (U32)MappedAddress_p);
}
}
iounmap(MappedAddress_p);
}
#endif
/* End of File */
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -