/*{{{ 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; }