1 files changed, 29 insertions, 12 deletions

diff --git a/io-module/mts_io.c b/io-module/mts_io.c
index 936d0d2..8b982c6 100644
--- a/io-module/mts_io.c
+++ b/io-module/mts_io.c
@@ -434,10 +434,22 @@ static bool load_port(int port) {
 	} else if (mts_ap_eeprom[port_index][0] == 0x00) {
 		log_info("no accessory card inserted in port %d", port);
 	} else {
+		port_info[port_index] = kzalloc(sizeof(struct ap_info), GFP_KERNEL);
+		if (! port_info[port_index]) {
+			log_error("alloc of port info failed");
+			return false;
+		}
+
 		if (strstr(ap_eeprom[port_index].product_id, PRODUCT_ID_MTAC_GPIOB)) {
-			port_info[port_index] = &gpiob_info;
+			if (! set_gpiob_info(port_info[port_index])) {
+				log_error("failed to set up gpiob port info");
+				return false;
+			}
 		} else if (strstr(ap_eeprom[port_index].product_id, PRODUCT_ID_MTAC_MFSER)) {
-			port_info[port_index] = &mfser_info;
+			if (! set_mfser_info(port_info[port_index])) {
+				log_error("failed to set up mfser port info");
+				return false;
+			}
 		} else {
 			log_error("unknown accessory card [%s] in port %d", ap_eeprom[port_index].product_id, port);
 			return false;
@@ -461,6 +473,7 @@ static bool load_port(int port) {
 		if (! port_info[port_index]->setup(port)) {
 			log_error("accessory port %d setup failed", port);
 			port_info[port_index]->teardown(port);
+			kfree(port_info[port]);
 			return false;
 		}
 	}
@@ -645,7 +658,8 @@ static int __init mts_io_init(void)
 	int ret;
 	size_t device_attributes_size;
 	size_t card_attributes_size;
-	int i;
+	int port;
+	int port_index;
 
 	log_info("init: " DRIVER_VERSION);
 	
@@ -655,8 +669,8 @@ static int __init mts_io_init(void)
 	}
 
 	if (NUM_AP) {
-		for (i = 0; i < NUM_AP; i++) {
-			port_info[i] = NULL;
+		for (port = 0; port < NUM_AP; port++) {
+			port_info[port] = NULL;
 		}
 		init_accessory_ports();
 	}
@@ -785,9 +799,10 @@ error2:
 	platform_device_put(mts_io_platform_device);
 error1:
 	log_error("init failed: %d", ret);
-	for (i = 0; i < NUM_AP; i++) {
-		if (port_info[i]) {
-			port_info[i]->teardown(i);
+	for (port = 0, port_index = 1; port < NUM_AP; port++, port_index++) {
+		if (port_info[port]) {
+			port_info[port]->teardown(port_index);
+			kfree(port_info[port]);
 		}
 	}
 
@@ -796,7 +811,8 @@ error1:
 
 static void __exit mts_io_exit(void)
 {
-	int i;
+	int port;
+	int port_index;
 
 	if ( mts_product_id != MT100EOCG_0_0 ) {
 		cancel_delayed_work_sync(&reset_work);
@@ -824,9 +840,10 @@ static void __exit mts_io_exit(void)
 
 	platform_device_unregister(mts_io_platform_device);
 
-	for (i = 0; i < NUM_AP; i++) {
-		if (port_info[i]) {
-			port_info[i]->teardown(i);
+	for (port = 0, port_index = 1; port < NUM_AP; port++, port_index++) {
+		if (port_info[port]) {
+			port_info[port]->teardown(port_index);
+			kfree(port_info[port]);
 		}
 	}