Add mtac_free function to mtac module1.1.0

1 files changed, 41 insertions, 5 deletions

diff --git a/mtac.c b/mtac.c
index 4837ade..2f31bbe 100644
--- a/mtac.c
+++ b/mtac.c
@@ -1,8 +1,8 @@
-#define DRIVER_VERSION  "v1.0.0"
+#define DRIVER_VERSION  "v1.1.0"
 #define DRIVER_AUTHOR   "John Klug <john.klug@multitech.com>"
 #define DRIVER_DESC "MTS driver to supervise MTAC slots"
 #define DRIVER_NAME "mtac-slots"
-/* #define DEBUG 1 */
+#define DEBUG 0
 
 #include <linux/types.h>
 #include <linux/gpio.h>
@@ -377,8 +377,10 @@ int mtac_find(void(*set_info)(struct ap_info* info), const char *target_product_
     struct mts_ap_eeprom_layout *app;
     int slot_count = 0;
 
+    log_debug("mtac_find enter");
     for (port_index = 0; port_index < NUM_AP; port_index++) {
-        log_debug("mtac_find: State of mtac mutex is %d",(*(int *)(&mtac_mutex.count)));
+        log_debug("mtac_find: port_index: %d State of mtac mutex is %d",
+                  port_index, (*(int *)(&mtac_mutex.count)));
         mutex_lock(&mtac_mutex);
         if (mtac_port_info[port_index] != NULL) {
             log_debug("Accessory Port %d of %d is in use",port_index+1,NUM_AP);
@@ -392,7 +394,7 @@ int mtac_find(void(*set_info)(struct ap_info* info), const char *target_product_
             mutex_unlock(&mtac_mutex);
             continue;
         } else if (mts_ap_eeprom[port_index][0] == 0x0) {
-            log_debug("no accessory card inserted in port %d", port_index);
+            log_debug("no accessory card inserted in port_index %d", port_index);
             mutex_unlock(&mtac_mutex);
             continue;
         } else if (strstr(app->product_id, target_product_id)) {
@@ -424,6 +426,41 @@ int mtac_find(void(*set_info)(struct ap_info* info), const char *target_product_
 }
 EXPORT_SYMBOL(mtac_find);
 
+void mtac_free(const char *product_id, bool(* setup)(enum ap port), const char *link)
+{
+    int port_index, count, port;
+    char buf[32];
+    struct mts_ap_eeprom_layout *app;
+    log_debug("mtac_free enter");
+    count = 0;
+    for (port_index = 0; port_index < NUM_AP; port_index++) {
+        port = port_index + 1;
+        app = (struct mts_ap_eeprom_layout *)mts_ap_eeprom[port_index];
+        if (app && strstr(app->product_id, product_id)) {
+            count++;
+            log_debug("Free port %d product_id: %s mtac_port_info: %p",
+                      port_index + 1, product_id,mtac_port_info[port_index]);
+            if(mtac_port_info[port_index])
+                log_debug("setup: %p setup ptr: %p",mtac_port_info[port_index]->setup,setup);
+            if (mtac_port_info[port_index] && (mtac_port_info[port_index]->setup == setup)) {
+                log_debug("port_index %d is occupied by us, teardown next",port_index);
+                           
+                if (count > 1) {
+                    sprintf(buf, "%s-%d",link,port);
+                } else {
+                    sprintf(buf, "%s",link);
+                }
+                sysfs_remove_link(mtac_port_info[port_index]->subdirs->parent, buf);
+
+                mtac_port_info[port_index]->teardown(port_index+1);
+                kfree(mtac_port_info[port_index]);
+                mtac_port_info[port_index] = NULL;  // Slot available
+            } // MTAC Slot array is in use by our module
+        } // Slot EEPROM says it is occupied by our module
+    } // Loop through the ports
+}
+EXPORT_SYMBOL(mtac_free);
+
 /*
 * Display the EEPROM for all the slots.
 */
@@ -446,7 +483,6 @@ static int __init mtac_init(void)
     return 0;
 }
 
-/* We can only tear down our own device */
 static void __exit mtac_exit(void)
 {
     log_info("exiting");