125 lines
3.1 KiB
C
125 lines
3.1 KiB
C
#include <socks/device.h>
|
|
#include <socks/printk.h>
|
|
#include <socks/pci.h>
|
|
|
|
static struct vm_cache pci_driver_cache = {
|
|
.c_name = "pci_driver",
|
|
.c_obj_size = sizeof(struct pci_driver),
|
|
};
|
|
|
|
static struct queue pci_drivers;
|
|
static spin_lock_t pci_drivers_lock;
|
|
|
|
kern_status_t init_pci_driver_cache(void)
|
|
{
|
|
vm_cache_init(&pci_driver_cache);
|
|
pci_drivers = QUEUE_INIT;
|
|
pci_drivers_lock = SPIN_LOCK_INIT;
|
|
|
|
return KERN_OK;
|
|
}
|
|
|
|
struct pci_driver *pci_driver_create(struct kext *self, const char *name, const struct pci_device_id *device_ids)
|
|
{
|
|
struct pci_driver *driver = vm_cache_alloc(&pci_driver_cache, VM_NORMAL);
|
|
if (!driver) {
|
|
return NULL;
|
|
}
|
|
|
|
kern_status_t status = driver_init(&driver->pci_base, self, name);
|
|
if (status != KERN_OK) {
|
|
vm_cache_free(&pci_driver_cache, driver);
|
|
return NULL;
|
|
}
|
|
|
|
driver->pci_target_devices = device_ids;
|
|
return driver;
|
|
}
|
|
|
|
kern_status_t pci_driver_destroy(struct pci_driver *driver)
|
|
{
|
|
/* TODO */
|
|
return KERN_UNIMPLEMENTED;
|
|
}
|
|
|
|
kern_status_t pci_driver_register(struct pci_driver *driver)
|
|
{
|
|
kern_status_t status = driver_register(&driver->pci_base);
|
|
if (status != KERN_OK) {
|
|
return status;
|
|
}
|
|
|
|
unsigned long flags;
|
|
spin_lock_irqsave(&pci_drivers_lock, &flags);
|
|
queue_push_back(&pci_drivers, &driver->pci_head);
|
|
spin_unlock_irqrestore(&pci_drivers_lock, flags);
|
|
|
|
return KERN_OK;
|
|
}
|
|
|
|
kern_status_t pci_driver_unregister(struct pci_driver *driver)
|
|
{
|
|
if (driver->pci_base.drv_major == DEV_MAJOR_INVALID) {
|
|
return KERN_INVALID_ARGUMENT;
|
|
}
|
|
|
|
unsigned long flags;
|
|
spin_lock_irqsave(&pci_drivers_lock, &flags);
|
|
queue_delete(&pci_drivers, &driver->pci_head);
|
|
spin_unlock_irqrestore(&pci_drivers_lock, flags);
|
|
|
|
return driver_unregister(&driver->pci_base);
|
|
}
|
|
|
|
static int scan_device_id_list(const struct pci_device_id *device_ids, const struct pci_device_id *query)
|
|
{
|
|
int best_score = 0;
|
|
|
|
for (unsigned int i = 0; ; i++) {
|
|
if (device_ids[i].pci_device_id == PCI_NONE && device_ids[i].pci_vendor_id == PCI_NONE && device_ids[i].pci_class_id == PCI_CLASS_ANY && device_ids[i].pci_subclass_id == PCI_CLASS_ANY) {
|
|
break;
|
|
}
|
|
|
|
int score = 0;
|
|
|
|
if (device_ids[i].pci_class_id == query->pci_class_id && device_ids[i].pci_subclass_id == query->pci_subclass_id && device_ids[i].pci_vendor_id == PCI_NONE && device_ids[i].pci_device_id == PCI_NONE) {
|
|
score = 1;
|
|
}
|
|
|
|
if (device_ids[i].pci_device_id == query->pci_device_id && device_ids[i].pci_vendor_id == query->pci_vendor_id) {
|
|
score = 2;
|
|
}
|
|
|
|
if (score > best_score) {
|
|
best_score = score;
|
|
}
|
|
}
|
|
|
|
return best_score;
|
|
}
|
|
|
|
struct pci_driver *find_driver_for_pci_device(const struct pci_device_id *query)
|
|
{
|
|
struct pci_driver *best_match = NULL;
|
|
int best_score = 0;
|
|
|
|
unsigned long flags;
|
|
spin_lock_irqsave(&pci_drivers_lock, &flags);
|
|
|
|
queue_foreach (struct pci_driver, driver, &pci_drivers, pci_head) {
|
|
const struct pci_device_id *device_ids = driver->pci_target_devices;
|
|
if (!device_ids) {
|
|
continue;
|
|
}
|
|
|
|
int score = scan_device_id_list(device_ids, query);
|
|
if (score > best_score) {
|
|
best_score = score;
|
|
best_match = driver;
|
|
}
|
|
}
|
|
|
|
spin_unlock_irqrestore(&pci_drivers_lock, flags);
|
|
return best_match;
|
|
}
|