Fix ethernet accessory card bug and add capability directory

3 files changed, 94 insertions, 1 deletions

diff --git a/io-module/mts_capab.c b/io-module/mts_capab.c
new file mode 100644
index 0000000..35a91c6
--- /dev/null
+++ b/io-module/mts_capab.c
@@ -0,0 +1,69 @@
+/*
+ * capab.c
+ *
+ *  Created on: Dec 1, 2016
+ *      Author: jklug
+ */
+static ssize_t capab_show_value(struct kobject *kobj, struct kobj_attribute *attr, char *buf) {
+	ssize_t value;
+
+	if (! strcmp(attr->attr.name, "gps"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_GPS) != 0);
+	else if  (! strcmp(attr->attr.name, "din"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_DIN) != 0);
+	else if  (! strcmp(attr->attr.name, "dout"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_DOUT) != 0);
+	else if  (! strcmp(attr->attr.name, "adc"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_ADC) != 0);
+	else if  (! strcmp(attr->attr.name, "bluetooth"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_BLUETOOTH) != 0);
+	else if  (! strcmp(attr->attr.name, "wifi"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI) != 0);
+	else if  (! strcmp(attr->attr.name, "wifi_bt"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_WIFI_BT) != 0);
+	else if  (! strcmp(attr->attr.name, "gnss"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_GNSS) != 0);
+	else if  (! strcmp(attr->attr.name, "lora"))
+		value = snprintf(buf, 32, "%lu\n", DEVICE_CAPA(id_eeprom.capa, CAPA_LORA) != 0);
+	else {
+		log_error("attribute [%s] not found", attr->attr.name);
+		value = -1;
+	}
+
+	return value;
+}
+
+static char *attribute_names[] = {
+		"gps",
+		"din",
+		"dout",
+		"adc",
+		"bluetooth",
+		"wifi",
+		"wifi_bt",
+		"gnss",
+		"lora",
+};
+
+static bool capab_add_attributes(struct attribute** attrs) {
+	struct kobj_attribute* kobj_attr;
+	int i;
+	int count = sizeof(attribute_names)/sizeof(attribute_names[0]);
+
+	// We must keep the number of attributes in sync
+	BUG_ON(CAPA_COUNT != count);
+	log_info("capab_add_attributes, add %d attributes",count);
+	for(i=0;i<count;i++) {
+		kobj_attr = create_attribute(attribute_names[i], MTS_ATTR_MODE_RO);
+		if (! kobj_attr) {
+			log_error("failed to create attribute [%s]", attribute_names[i]);
+			return false;
+		}
+
+		kobj_attr->show = capab_show_value;
+		attrs[i] = &kobj_attr->attr;
+		log_error("created attribute [%s]", attribute_names[i]);
+	}
+
+	return true;
+}
diff --git a/io-module/mts_eeprom.h b/io-module/mts_eeprom.h
index 89f2439..f616e83 100644
--- a/io-module/mts_eeprom.h
+++ b/io-module/mts_eeprom.h
@@ -55,6 +55,7 @@ do { \
 
 #define DEVICE_CAPA_VALUE(index, bit)		((((index) & 0x1F) << 3) | ((bit) & 0x07))
 
+#define CAPA_COUNT 9  // See also attribute_names in capab.c
 #define CAPA_GPS				DEVICE_CAPA_VALUE(0, 7)
 #define CAPA_DIN				DEVICE_CAPA_VALUE(0, 6)
 #define CAPA_DOUT				DEVICE_CAPA_VALUE(0, 5)
diff --git a/io-module/mts_io.c b/io-module/mts_io.c
index b71eabb..54b1f76 100644
--- a/io-module/mts_io.c
+++ b/io-module/mts_io.c
@@ -653,6 +653,8 @@ static struct kobject *ap_subdirs[NUM_AP];
 static struct attribute_group ap_attr_groups[NUM_AP];
 /* info for accessory port (contains function pointers for setup and teardown and and useful info) */
 static struct ap_info* port_info[NUM_AP];
+static struct kobject *capab;
+static struct attribute_group capab_group;
 
 /* accessory card support */
 #include "mtac.c"
@@ -660,6 +662,7 @@ static struct ap_info* port_info[NUM_AP];
 #include "mtac_mfser.c"
 #include "mtac_eth.c"
 #include "mtac_lora.c"
+#include "mts_capab.c"
 
 static bool load_port(int port) {
 	int port_index = port - 1;
@@ -934,7 +937,11 @@ static void cleanup(void)
 		kfree(freelater);
 		freelater = NULL;
 	}
-	log_info("done cleaning up....");
+
+	if(capab_group.attrs)
+		kobject_put((struct kobject *)capab_group.attrs);
+
+	log_info("JAK done cleaning up....");
 }
 
 static int __init mts_io_init(void)
@@ -965,6 +972,22 @@ static int __init mts_io_init(void)
 
 	init_ports();
 
+	capab = kobject_create_and_add("capability", &mts_io_platform_device->dev.kobj);
+	if (capab == NULL) {
+		log_error("kobject_create_and_add for capability directory failed");
+		return false;
+	}
+	
+	capab_group.attrs = kzalloc(sizeof(struct attribute*) * CAPA_COUNT, GFP_KERNEL);
+    if (! capab_add_attributes(capab_group.attrs)) {
+    	return false;
+    }
+
+	if (sysfs_create_group(capab, &capab_group)) {
+		log_error("sysfs_create_group failed to create capability group");
+		return false;
+	}
+
 	ret = sysfs_create_group(&mts_io_platform_device->dev.kobj, attr_group);
 	if (ret) {
 		cleanup();