/*{{{ MonitorIoctlRequestEvent*/
static int MonitorIoctlRequestEvent (struct DeviceContext_s* Context, struct monitor_event_request_s* EventRequest)
{
struct EventValue_s* StoredEvent;
MONITOR_DEBUG("\n");
if (!access_ok (VERIFY_WRITE, EventRequest, sizeof (struct monitor_event_request_s)))
{
MONITOR_ERROR ("Invalid status address = %p\n", EventRequest);
return -EFAULT;
}
if ((EventRequest->event_code & MONITOR_EVENT_INDEX_MASK) > MAX_MONITOR_EVENT_CODE)
{
MONITOR_ERROR ("Invalid event requested (%x)\n", EventRequest->event_code);
return -EINVAL;
}
StoredEvent = &(Context->StoredEventValues[EventRequest->event_code & MONITOR_EVENT_INDEX_MASK]);
memcpy (EventRequest->parameters, StoredEvent->Parameters, sizeof(StoredEvent->Parameters));
EventRequest->count = StoredEvent->Count;
if (EventRequest->reset)
StoredEvent->Count = 0;
return 0;
}
/*
* Run in the main thread on the first call to pthread_create(),
* before any new threads are created. Only called once from the
* pthread_create() override.
*/
static void
monitor_thread_list_init(void)
{
struct monitor_thread_node *main_tn;
int ret;
/*
* Give main.c functions a chance to initialize in the case that
* some library calls pthread_create() before main().
*/
monitor_early_init();
monitor_thread_name_init();
MONITOR_DEBUG1("\n");
LIST_INIT(&monitor_thread_list);
LIST_INIT(&monitor_free_list);
monitor_tn_array_pos = 0;
ret = (*real_pthread_key_create)(&monitor_pthread_key, NULL);
if (ret != 0) {
MONITOR_ERROR("pthread_key_create failed (%d)\n", ret);
}
/*
* main_tn's thread-specific data.
*/
main_tn = monitor_get_main_tn();
if (main_tn == NULL || main_tn->tn_magic != MONITOR_TN_MAGIC) {
MONITOR_ERROR1("monitor_get_main_tn failed\n");
}
main_tn->tn_self = (*real_pthread_self)();
ret = (*real_pthread_setspecific)(monitor_pthread_key, main_tn);
if (ret != 0) {
MONITOR_ERROR("pthread_setspecific failed (%d)\n", ret);
}
}
/*{{{ TransformerGetLogEvent*/
static int TransformerGetLogEvent(struct MMEContext_s *Context)
{
MME_Command_t MMECommand;
MME_ERROR MMEStatus;
/*EVENT_LOG_TransformParam_t TransformParams;*/
memset(&MMECommand, 0x00, sizeof(MME_Command_t));
MMECommand.CmdStatus.AdditionalInfoSize = sizeof(EVENT_LOG_CommandStatus_t);
MMECommand.CmdStatus.AdditionalInfo_p = (MME_GenericParams_t)(&(Context->MMECommandStatus));
MMECommand.StructSize = sizeof(MME_Command_t);
MMECommand.CmdCode = MME_TRANSFORM;
MMECommand.CmdEnd = MME_COMMAND_END_RETURN_NOTIFY;
MMECommand.DueTime = (MME_Time_t)0;
MMECommand.NumberInputBuffers = 0;
MMECommand.NumberOutputBuffers = 0;
MMECommand.DataBuffers_p = NULL;
MMECommand.ParamSize = 0;/*sizeof(EVENT_LOG_TransformParam_t);*/
MMECommand.Param_p = NULL; /*(MME_GenericParams_t)&TransformParams;*/
MMEStatus = MME_SendCommand(Context->MMEHandle, &MMECommand);
if (MMEStatus != MME_SUCCESS)
{
MONITOR_ERROR("%s: Failed to send command - Error 0x%08x.\n", Context->TransformerName, MMEStatus);
return -EFAULT;
}
return 0;
}
/*{{{ TransformerInitialize*/
static int TransformerInitialize(struct MMEContext_s *Context)
{
EVENT_LOG_InitTransformerParam_t EventLogInitParams = {0};
MME_TransformerInitParams_t MMEInitParams = {0};
MME_ERROR MMEStatus;
int Status;
Status = VerifyCapabilities(Context);
if (Status != 0)
return Status;
EventLogInitParams.StructSize = sizeof(EVENT_LOG_InitTransformerParam_t);
EventLogInitParams.TimeCodeMemoryAddress = Context->ClockAddress;
MMEInitParams.Priority = MME_PRIORITY_LOWEST;
MMEInitParams.StructSize = sizeof(MME_TransformerInitParams_t);
MMEInitParams.Callback = &TransformerCallback;
MMEInitParams.CallbackUserData = Context;
MMEInitParams.TransformerInitParamsSize = sizeof(EVENT_LOG_InitTransformerParam_t);
MMEInitParams.TransformerInitParams_p = (MME_GenericParams_t)(&EventLogInitParams);
MMEStatus = MME_InitTransformer(Context->TransformerName, &MMEInitParams, &Context->MMEHandle);
if (MMEStatus != MME_SUCCESS)
{
MONITOR_ERROR("%s: Failed to initialize transformer - Error 0x%08x.\n", Context->TransformerName, MMEStatus);
return -EFAULT;
}
Context->TransformerInitialized = true;
return 0;
}
/*{{{ MonitorIoctl*/
static int MonitorIoctl(struct inode* Inode,
struct file* File,
unsigned int IoctlCode,
unsigned long Parameter)
{
struct DeviceContext_s* Context = (struct DeviceContext_s*)File->private_data;
struct ModuleContext_s* ModuleContext = Context->ModuleContext;
int Result = 0;
/*MONITOR_DEBUG("MonitorIoctl : Ioctl %08x\n", IoctlCode);*/
if (((File->f_flags & O_ACCMODE) == O_RDONLY) &&
((IoctlCode != MONITOR_GET_STATUS) && (IoctlCode != MONITOR_REQUEST_EVENT)))
return -EPERM;
mutex_lock(&(ModuleContext->Lock));
switch (IoctlCode)
{
case MONITOR_DISABLE:
Result = MonitorIoctlDisable(Context, (monitor_subsystem_mask_t)Parameter);
break;
case MONITOR_ENABLE:
Result = MonitorIoctlEnable(Context, (monitor_subsystem_mask_t)Parameter);
break;
case MONITOR_GET_STATUS:
Result = MonitorIoctlGetStatus(Context, (struct monitor_status_s*)Parameter);
break;
case MONITOR_REQUEST_EVENT:
Result = MonitorIoctlRequestEvent(Context, (struct monitor_event_request_s*)Parameter);
break;
default:
MONITOR_ERROR("Error - invalid ioctl %08x\n", IoctlCode);
Result = -ENOIOCTLCMD;
}
mutex_unlock(&(ModuleContext->Lock));
return Result;
}
/*{{{ MonitorMMEInit*/
int MonitorMMEInit(struct DeviceContext_s *DeviceContext,
struct MMEContext_s *Context,
unsigned int Id)
{
struct sched_param Param;
struct task_struct *Taskp;
int Status;
struct clk *Tmu1Clock = clk_get(NULL, "tmu1_clk");
unsigned long long TimeStamp;
unsigned int TimeValue;
if (Tmu1Clock == NULL)
{
MONITOR_ERROR("Unable to access working clock\n");
return -ENODEV;
}
Context->DeviceContext = DeviceContext;
Context->ClockAddress = DeviceContext->TimerPhysical;
Context->ClockMaxValue = 0xffffffff;
TimeValue = *DeviceContext->Timer;
TimeStamp = ktime_to_us(ktime_get());
msleep(10); // Sleep for some random amount of time
TimeValue = *DeviceContext->Timer - TimeValue;
TimeStamp = ktime_to_us(ktime_get()) - TimeStamp;
// If my maths is correct you don't need to cope with rap case....
Context->TicksPerSecond = ((((unsigned long long)TimeValue) * 1000000ull) / (unsigned long long)TimeStamp);
MONITOR_TRACE("TicksPerSecond %lld\n", Context->TicksPerSecond);
Context->Id = Id;
Context->TransformerInitialized = 0;
sprintf(Context->TransformerName, "%s%d", EVENT_LOG_MME_TRANSFORMER_NAME, Context->Id - 1);
Status = TransformerInitialize(Context);
if (Status != 0)
return Status;
sema_init(&(Context->ThreadTerminated), 0);
sema_init(&(Context->EventReceived), 0);
Context->Monitoring = true;
Context->MonitorMMEThread = NULL;
Taskp = kthread_run(MonitorMMEThread, Context, MONITOR_MME_THREAD_NAME);
if (!Taskp)
{
Context->Monitoring = false;
MONITOR_ERROR("Unable to create transformer monitor thread\n");
return -ENODEV;
}
Param.sched_priority = 40;
sched_setscheduler(Taskp, SCHED_RR, &Param);
return 0;
}
/*{{{ MonitorIoctlDisable*/
static int MonitorIoctlDisable(struct DeviceContext_s* Context, monitor_subsystem_mask_t SubsystemMask)
{
MONITOR_DEBUG("\n");
if ((SubsystemMask & MONITOR_ENABLE_ALL) == 0)
{
MONITOR_ERROR("Invalid subsystem mask %08x\n", SubsystemMask);
return -EINVAL;
}
Context->Status.subsystem_mask &= (~(SubsystemMask & MONITOR_ENABLE_ALL));
return 0;
}
/*{{{ TransformerTerminate*/
static int TransformerTerminate(struct MMEContext_s *Context)
{
MME_ERROR MMEStatus;
if (Context->TransformerInitialized)
{
Context->TransformerInitialized = false;
MMEStatus = MME_TermTransformer(Context->MMEHandle);
if (MMEStatus != MME_SUCCESS)
{
MONITOR_ERROR("%s: Failed to terminate mme transformer - Error 0x%08x.\n", Context->TransformerName, MMEStatus);
return -EFAULT;
}
}
return 0;
}
/*{{{ MonitorIoctlGetStatus*/
static int MonitorIoctlGetStatus(struct DeviceContext_s* Context, struct monitor_status_s* Status)
{
struct EventQueue_s* EventList = &Context->EventQueue;
MONITOR_DEBUG("\n");
if (!access_ok(VERIFY_WRITE, Status, sizeof(struct monitor_status_s)))
{
MONITOR_ERROR("Invalid status address = %p\n", Status);
return -EFAULT;
}
if (EventList->Read >= EventList->Write)
Context->Status.event_count = EventList->Read - EventList->Write;
else
Context->Status.event_count = (EventList->Read + MAX_MONITOR_EVENT) - EventList->Write;
Context->Status.events_lost = EventList->LostCount;
memcpy(Status, &Context->Status, sizeof(struct monitor_status_s));
return 0;
}
/*{{{ TransformerCallback*/
static void TransformerCallback(MME_Event_t Event,
MME_Command_t *CallbackData,
void *UserData)
{
struct MMEContext_s *Context = (struct MMEContext_s *)UserData;
if (CallbackData == NULL)
{
MONITOR_ERROR("%s: ####################### No CallbackData #######################\n", Context->TransformerName);
return;
}
switch (CallbackData->CmdCode)
{
case MME_TRANSFORM:
{
up(&(Context->EventReceived));
break;
}
default:
break;
}
}
/*{{{ VerifyCapabilities*/
static int VerifyCapabilities(struct MMEContext_s *Context)
{
EVENT_LOG_TransformerCapability_t EventLogCapability = {0};
MME_TransformerCapability_t MMECapability = {0};
MME_ERROR MMEStatus;
MMECapability.StructSize = sizeof(MME_TransformerCapability_t);
MMECapability.TransformerInfoSize = sizeof(EVENT_LOG_TransformerCapability_t);
MMECapability.TransformerInfo_p = &EventLogCapability;
MMEStatus = MME_GetTransformerCapability(Context->TransformerName, &MMECapability);
if (MMEStatus == MME_UNKNOWN_TRANSFORMER)
{
MONITOR_DEBUG("%s: Transformer not found. This event source will not be monitored.\n", Context->TransformerName);
return -EFAULT;
}
if (MMEStatus != MME_SUCCESS)
{
MONITOR_ERROR("%s: Failed to retrieve transformer capabilities - Error 0x%08x.\n", Context->TransformerName, MMEStatus);
return -EFAULT;
}
MONITOR_TRACE("Found %s transformer (version %x)\n", Context->TransformerName, MMECapability.Version);
return 0;
}
void MonitorSignalEvent (monitor_event_code_t EventCode,
unsigned int Parameters[MONITOR_PARAMETER_COUNT],
const char* Description)
{
unsigned int DeviceId = 0;
struct DeviceContext_s* Context = GetDeviceContext (DeviceId);
// If no context means the driver has not been installed.
if (!Context)
{
MONITOR_ERROR("Invalid monitor device %d\n", DeviceId);
return;
}
MonitorRecordEvent (Context,
0,
EventCode,
(unsigned long long)ktime_to_us (ktime_get ()),
Parameters,
Description);
}
static int StmMonitorProbe(struct device *dev)
{
int Result;
int i;
struct platform_device *MonitorDeviceData;
unsigned int *Timer;
unsigned int TimerPhysical;
MonitorDeviceData = to_platform_device(dev);
if (!MonitorDeviceData) {
MONITOR_ERROR("%s: Device probe failed. Check your kernel SoC config!!\n",
__FUNCTION__);
return -ENODEV;
}
ModuleContext = kzalloc (sizeof (struct ModuleContext_s), GFP_KERNEL);
if (ModuleContext == NULL)
{
MONITOR_ERROR("Unable to allocate device memory\n");
return -ENOMEM;
}
TimerPhysical = platform_get_resource(MonitorDeviceData, IORESOURCE_MEM, 0)->start;
Timer = ioremap(TimerPhysical,0x4);
mutex_init (&(ModuleContext->Lock));
mutex_lock (&(ModuleContext->Lock));
Result = alloc_chrdev_region (&FirstDevice, 0, MONITOR_MAX_DEVICES, DEVICE_NAME);
if (Result < 0)
{
printk (KERN_ERR "%s: unable to allocate device numbers\n",__FUNCTION__);
return -ENODEV;
}
ModuleContext->DeviceClass = class_create (THIS_MODULE, DEVICE_NAME);
if (IS_ERR(ModuleContext->DeviceClass))
{
printk (KERN_ERR "%s: unable to create device class\n",__FUNCTION__);
ModuleContext->DeviceClass = NULL;
return -ENODEV;
}
for (i = 0; i < MONITOR_MAX_DEVICES; i++)
{
struct DeviceContext_s* DeviceContext = &ModuleContext->DeviceContext[i];
int DevNo = MKDEV(MAJOR(FirstDevice), i);
struct file_operations* FileOps;
DeviceContext->TimerPhysical = TimerPhysical;
DeviceContext->Timer = Timer;
FileOps = MonitorInit (DeviceContext);
DeviceContext->ModuleContext = ModuleContext;
cdev_init (&(DeviceContext->CDev), FileOps);
DeviceContext->CDev.owner = THIS_MODULE;
kobject_set_name (&(DeviceContext->CDev.kobj), "%s%d", DEVICE_NAME, i);
Result = cdev_add (&(DeviceContext->CDev), DevNo, 1);
if (Result != 0)
{
printk (KERN_ERR "%s: unable to add device\n",__FUNCTION__);
return -ENODEV;
}
#if defined(__TDT__) && (LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 30))
DeviceContext->ClassDevice = device_create (ModuleContext->DeviceClass,
NULL,
DeviceContext->CDev.dev,
NULL,
kobject_name (&(DeviceContext->CDev.kobj)));
#else
DeviceContext->ClassDevice = class_device_create (ModuleContext->DeviceClass,
NULL,
DeviceContext->CDev.dev,
NULL,
kobject_name (&(DeviceContext->CDev.kobj)));
#endif
if (IS_ERR(DeviceContext->ClassDevice))
{
printk (KERN_ERR "%s: unable to create class device\n",__FUNCTION__);
DeviceContext->ClassDevice = NULL;
return -ENODEV;
}
#if defined(__TDT__) && (LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 30))
class_set_devdata (DeviceContext->ClassDevice, DeviceContext);
#endif
}
mutex_unlock (&(ModuleContext->Lock));
MONITOR_DEBUG("STM monitor device loaded\n");
return 0;
}