addded read_mask, to read masked pin register

Feuerlabs · May 17, 2013 · 879a62a · 879a62a
1 parent e5f5362
commit 879a62a
Show file tree

Hide file tree

Showing 2 changed files with 136 additions and 5 deletions.
diff --git a/c_src/gpio_drv.c b/c_src/gpio_drv.c
@@ -63,6 +63,7 @@ typedef int  ErlDrvSSizeT;
 #define CMD_GET_INTERRUPT 11
 #define CMD_DEBUG_LEVEL   12
 #define CMD_DUMP          13
+#define CMD_GET_MASK      14
 
 // Direct access
 #define DIRECT_ACCESS_OFF 0
@@ -94,9 +95,9 @@ typedef struct _gpio_ctx_t
     bool auto_create;     // Auto export/create register
     gpio_chipset_t chipset;
     gpio_methods_t* meth;         // pin register mask access
-    volatile uint32_t *gpio_reg;  //Pointer to physical gpio memory
-    uint32_t reg0_direct_pins; // Mask for pins with direct access
-    uint32_t reg1_direct_pins; // Mask for pins with direct access
+    volatile uint32_t *gpio_reg;  // Pointer to physical gpio memory
+    uint32_t reg0_direct_pins;    // Mask for pins with direct access
+    uint32_t reg1_direct_pins;    // Mask for pins with direct access
     ErlDrvEvent epollfd;
 } gpio_ctx_t;
 
@@ -739,7 +740,7 @@ static int gpio_set_mask_on_reg(gpio_ctx_t* ctx,
 
     // Take care of rest (if any)
     while (indirect_mask) {
-	DEBUGF("Set indirect mask 0x%x on linked list.", indirect_mask);
+	DEBUGF("Set indirect mask 0x%x on array.", indirect_mask);
 	if (indirect_mask & 1) {
 	    if (gpp[pin] == NULL) {
 		if (!ctx->auto_create) 
@@ -785,6 +786,93 @@ static int gpio_set_mask_on_list(gpio_ctx_t* ctx,
     return GPIO_OK;
 }
 
+
+//--------------------------------------------------------------------
+// Get masked pinreg bits
+//--------------------------------------------------------------------
+static int gpio_get_mask_on_reg(gpio_ctx_t* ctx,
+				uint8_t pin_reg,
+				uint32_t  mask,
+				uint32_t* valuep)
+{
+    gpio_pin_t* gp;
+    gpio_pin_t** gpp;
+    uint32_t value = 0;
+    uint32_t indirect_mask = mask;
+    uint8_t pin = 0;
+
+    DEBUGF("Get mask for pinreg %d", pin_reg);
+
+    if (pin_reg == 0) {
+	uint32_t m = mask & ctx->reg0_direct_pins;
+	DEBUGF("get direct mask 0x%x on register %d", m, pin_reg);
+	if (m)
+	    value = (*ctx->meth->get_mask)(ctx->gpio_reg,pin_reg);
+	value &= m;
+	indirect_mask = mask & ~(ctx->reg0_direct_pins);
+	gpp = ctx->reg0;
+    }
+    else if (pin_reg == 1) {
+	uint32_t m = mask & ctx->reg1_direct_pins;
+	DEBUGF("get direct mask 0x%x on register %d", m, pin_reg);
+	if (m)
+	    value = (*ctx->meth->get_mask)(ctx->gpio_reg,pin_reg);
+	value &= m;
+	indirect_mask = mask & ~(ctx->reg1_direct_pins);
+	gpp = ctx->reg1;
+    }
+    else 
+	return GPIO_NOK;
+
+    while (indirect_mask) {
+	DEBUGF("Get indirect mask 0x%x from array.", indirect_mask);
+	if (indirect_mask & 1) {
+	    uint8_t val;
+	    if ((gp = gpp[pin]) != NULL) {
+		if (gpio_get_state(ctx, gp, &val) == GPIO_OK) {
+		    if (val)
+			value |= (1 << pin);
+		}
+	    }
+	}
+	indirect_mask >>= 1;
+	pin ++;
+    }
+    *valuep = value;
+    return GPIO_OK;
+}
+
+//--------------------------------------------------------------------
+// Get masked pinreg bits
+//--------------------------------------------------------------------
+
+static int gpio_get_mask_on_list(gpio_ctx_t* ctx,
+				 uint8_t pin_reg,
+				 uint32_t  mask,
+				 uint32_t* valuep)
+{
+    uint8_t pin = 0;
+    uint32_t value = 0;
+    gpio_pin_t* gp;
+
+    while (mask) {
+	DEBUGF("Get mask 0x%x on linked list.", mask);
+	if (mask & 1) {
+	    if ((gp=find_pin(ctx, pin_reg, pin, NULL)) != NULL) {
+		uint8_t val;
+		if (gpio_get_state(ctx, gp, &val) == GPIO_OK) {
+		    if (val)
+			value |= (1 << pin);
+		}
+	    }
+	}
+	mask >>= 1;
+	pin ++;
+    }
+    *valuep = value;
+    return GPIO_OK;
+}
+
 //--------------------------------------------------------------------
 // Set pin states for pins defined by mask
 // Applicable for pins without direct access
@@ -1337,6 +1425,20 @@ static ErlDrvSSizeT gpio_drv_ctl(ErlDrvData d,
 	goto ok;
     }
 
+    case CMD_GET_MASK: {
+	uint32_t mask;
+	uint32_t value;
+	if (len != 5) goto badarg;
+	pin_reg = get_uint8(buf);
+	mask = get_uint32(buf+1);
+
+	if ((pin_reg == 0) || (pin_reg == 1))
+	    gpio_get_mask_on_reg(ctx, pin_reg, mask, &value);
+	else
+	    gpio_get_mask_on_list(ctx, pin_reg, mask, &value);
+	return ctl_reply(4, &value, sizeof(value), rbuf, rsize);
+    }
+
     case CMD_SET_INTERRUPT: {
 	gpio_interrupt_t intval;
 

diff --git a/src/gpio.erl b/src/gpio.erl
@@ -50,7 +50,10 @@
 -export([set_mask/1,
 	 clr_mask/1,
 	 set_mask/2,
-	 clr_mask/2]).
+	 clr_mask/2,
+	 get_mask/1,
+	 get_mask/2]).
+
 
 %% Testing
 -export([debug/1,
@@ -71,6 +74,7 @@
 -define (CMD_GET_INTERRUPT, 11).
 -define (CMD_DEBUG_LEVEL, 12).
 -define (CMD_DUMP, 13).
+-define (CMD_GET_MASK, 14).
 
 %% Directions
 -define(DIR_IN,      1).
@@ -445,6 +449,31 @@ clr_mask(PinReg, Mask)
   when is_integer(PinReg), PinReg >= 0, is_integer(Mask), Mask >= 0 ->
     call(?GPIO_PORT, ?CMD_CLR_MASK, <<PinReg:8, Mask:32>>).
 
+
+%%--------------------------------------------------------------------
+%% @doc
+%% Read pins in mask in pin register 0
+%% @end
+%%--------------------------------------------------------------------
+-spec get_mask(PinReg::unsigned()) -> 
+		      {ok,Value::unsigned()} | {error,Reason::posix()}.
+
+get_mask(Mask) when 
+      is_integer(Mask), Mask >= 0 ->
+    get_mask(0, Mask).
+
+%%--------------------------------------------------------------------
+%% @doc
+%% Read pins in mask in pin register.
+%% @end
+%%--------------------------------------------------------------------
+-spec get_mask(PinReg::unsigned(), Mask::unsigned()) -> 
+		      {ok,Value::unsigned()} | {error,Reason::posix()}.
+
+get_mask(PinReg, Mask) 
+  when is_integer(PinReg), PinReg >= 0, is_integer(Mask), Mask >= 0 ->
+    call(?GPIO_PORT, ?CMD_GET_MASK, <<PinReg:8, Mask:32>>).
+
 %%--------------------------------------------------------------------
 %% @doc
 %% Controls debug.