diff --git a/bmd380_peripheral.vcxproj b/bmd380_peripheral.vcxproj
index 89bec64..ec4d6ee 100644
--- a/bmd380_peripheral.vcxproj
+++ b/bmd380_peripheral.vcxproj
@@ -198,6 +198,7 @@
+
@@ -376,6 +377,7 @@
+
diff --git a/cpg.c b/cpg.c
index 54a3ffb..c9d336c 100644
--- a/cpg.c
+++ b/cpg.c
@@ -1,3 +1,4 @@
+#include "cpg10_dev_mode.h"
#include "pel.h"
#include "pel10_io.h"
@@ -34,9 +35,41 @@ static void vis_rst(uint8_t *ins, uint16_t size)
NRF_LOG_INFO("%s", __FUNCTION__);
}
-static void dev_mode(uint8_t *ins, uint16_t size)
+void dev_mode(uint8_t *ins, uint16_t size)
{
NRF_LOG_INFO("%s", __FUNCTION__);
+
+ struct __PACKED
+ {
+ uint8_t id : 4;
+ uint8_t : 4;
+ uint16_t magic : 16;
+ uint8_t dev_opcode;
+ uint8_t function_opcode;
+ uint8_t param[];
+ } *p_ins = (void *)ins;
+
+ switch (p_ins->dev_opcode)
+ {
+ case 0x90:
+ dev_mode_set_cpg10_electrodes(ins);
+ break;
+
+ case 0xA0:
+ dev_mode_gpio_function(ins);
+ break;
+
+ case 0xA1:
+ // spi
+ break;
+
+ case 0xA2:
+ // i2c
+ break;
+
+ default:
+ break;
+ }
}
const elite_instance_t cpg_elite_instance = {
diff --git a/cpg10_dev_mode.c b/cpg10_dev_mode.c
new file mode 100644
index 0000000..a64f274
--- /dev/null
+++ b/cpg10_dev_mode.c
@@ -0,0 +1,260 @@
+
+#include "cpg10_dev_mode.h"
+
+#include "nrf_log.h"
+
+// The GPIO corresponding to the pin
+const uint32_t pin_to_gpio_table[] = {
+ [6] = NRF_GPIO_PIN_MAP(0, 22),
+ [8] = NRF_GPIO_PIN_MAP(0, 25),
+ [9] = NRF_GPIO_PIN_MAP(0, 19),
+ [10] = NRF_GPIO_PIN_MAP(0, 21),
+ [11] = NRF_GPIO_PIN_MAP(1, 00),
+ [12] = NRF_GPIO_PIN_MAP(0, 18),
+ [13] = NRF_GPIO_PIN_MAP(0, 17),
+ [14] = NRF_GPIO_PIN_MAP(0, 20),
+ [16] = NRF_GPIO_PIN_MAP(0, 14),
+ [17] = NRF_GPIO_PIN_MAP(0, 13),
+ [18] = NRF_GPIO_PIN_MAP(0, 11),
+ [20] = NRF_GPIO_PIN_MAP(0, 15),
+ [25] = NRF_GPIO_PIN_MAP(1, 8),
+ [26] = NRF_GPIO_PIN_MAP(0, 12),
+ [27] = NRF_GPIO_PIN_MAP(0, 7),
+ [28] = NRF_GPIO_PIN_MAP(1, 9),
+ [29] = NRF_GPIO_PIN_MAP(0, 8),
+ [30] = NRF_GPIO_PIN_MAP(0, 6),
+ [31] = NRF_GPIO_PIN_MAP(0, 5),
+ [32] = NRF_GPIO_PIN_MAP(0, 27),
+ [33] = NRF_GPIO_PIN_MAP(0, 26),
+ [34] = NRF_GPIO_PIN_MAP(0, 4),
+ [36] = NRF_GPIO_PIN_MAP(0, 1),
+ [37] = NRF_GPIO_PIN_MAP(0, 29),
+ [38] = NRF_GPIO_PIN_MAP(0, 0),
+ [39] = NRF_GPIO_PIN_MAP(0, 31),
+ [40] = NRF_GPIO_PIN_MAP(1, 15),
+ [41] = NRF_GPIO_PIN_MAP(0, 2),
+ [42] = NRF_GPIO_PIN_MAP(0, 30),
+ [43] = NRF_GPIO_PIN_MAP(0, 28),
+ [44] = NRF_GPIO_PIN_MAP(1, 12),
+ [45] = NRF_GPIO_PIN_MAP(1, 14),
+ [46] = NRF_GPIO_PIN_MAP(0, 3),
+ [47] = NRF_GPIO_PIN_MAP(1, 13),
+ [48] = NRF_GPIO_PIN_MAP(1, 3),
+ [49] = NRF_GPIO_PIN_MAP(1, 10),
+ [50] = NRF_GPIO_PIN_MAP(1, 6),
+ [51] = NRF_GPIO_PIN_MAP(1, 11),
+ [52] = NRF_GPIO_PIN_MAP(0, 10),
+ [53] = NRF_GPIO_PIN_MAP(0, 9),
+ [59] = NRF_GPIO_PIN_MAP(1, 2),
+ [60] = NRF_GPIO_PIN_MAP(0, 24),
+ [61] = NRF_GPIO_PIN_MAP(0, 23),
+ [62] = NRF_GPIO_PIN_MAP(0, 16),
+};
+
+static uint32_t bmd380pins_convert_to_gpio(uint32_t pin)
+{
+ uint32_t gpio;
+
+ switch (pin)
+ {
+ case 6:
+ case 8:
+ case 9:
+ case 10:
+ case 11:
+ case 12:
+ case 13:
+ case 14:
+ case 16:
+ case 17:
+ case 18:
+ case 20:
+ case 25:
+ case 26:
+ case 27:
+ case 28:
+ case 29:
+ case 30:
+ case 31:
+ case 32:
+ case 33:
+ case 34:
+ case 36:
+ case 37:
+ case 38:
+ case 39:
+ case 40:
+ case 41:
+ case 42:
+ case 43:
+ case 44:
+ case 45:
+ case 46:
+ case 47:
+ case 48:
+ case 49:
+ case 50:
+ case 51:
+ case 52:
+ case 53:
+ case 59:
+ case 60:
+ case 61:
+ case 62:
+ gpio = pin_to_gpio_table[pin];
+ break;
+ default:
+ gpio = UNDEF_GPIO;
+ NRF_LOG_INFO("UNDEF_GPIO: pin %d can't convert to gpio number", pin);
+ break;
+ }
+
+ return gpio;
+}
+
+static void set_bmd380_pin_signal(uint32_t pin, uint32_t high_low)
+{
+ uint32_t gpio = bmd380pins_convert_to_gpio(pin);
+
+ if (gpio != UNDEF_GPIO)
+ {
+ nrf_gpio_pin_write(gpio, high_low);
+ NRF_LOG_INFO("set pin %d (gpio %d) = %d", pin, gpio, high_low);
+ }
+}
+
+/*
+ dev_mode_gpio_function
+ (1)0x3000FFA000ppss
+ -func: set_bmd380_pin_signal
+ -pp: pin number 06h-3Fh
+ 06h: P0.22_GPIO
+ 08h: P0.25_GPIO
+ ......
+ 3Eh:P0.16_GPIO
+ -ss: signal 00h-01h
+ 00h: low
+ 01h: high
+*/
+void dev_mode_gpio_function(uint8_t *ins)
+{
+ struct __PACKED
+ {
+ uint8_t id : 4;
+ uint8_t : 4;
+ uint16_t magic : 16;
+ uint8_t dev_opcode;
+ uint8_t gpio_function_opcode;
+ uint8_t param[];
+ } *p_ins = (void *)ins;
+
+ switch (p_ins->gpio_function_opcode)
+ {
+ case 0x00: {
+ uint32_t pin = p_ins->param[0];
+ uint32_t high_low = p_ins->param[1];
+ set_bmd380_pin_signal(pin, high_low);
+ break;
+ }
+ }
+}
+
+#define ELECTRODES_A1HB1L 0
+#define ELECTRODES_A1LB1H 1
+#define ELECTRODES_A2HB2L 2
+#define ELECTRODES_A2LB2H 3
+#define ELECTRODES_A3HB3L 4
+#define ELECTRODES_A3LB3H 5
+#define ELECTRODES_A4HB4L 6
+#define ELECTRODES_A4LB4H 7
+static void cpg10_electrodes(uint32_t electrodes_mode)
+{
+ switch (electrodes_mode)
+ {
+ case ELECTRODES_A1HB1L:
+ nrf_gpio_pin_write(VB1H_PIN, 1);
+ nrf_gpio_pin_write(VA1L_PIN, 0);
+ nrf_gpio_pin_write(VA1H_PIN, 0);
+ nrf_gpio_pin_write(VB1L_PIN, 1);
+ break;
+
+ case ELECTRODES_A1LB1H:
+ nrf_gpio_pin_write(VA1H_PIN, 1);
+ nrf_gpio_pin_write(VB1L_PIN, 0);
+ nrf_gpio_pin_write(VB1H_PIN, 0);
+ nrf_gpio_pin_write(VA1L_PIN, 1);
+ break;
+
+ case ELECTRODES_A2HB2L:
+ nrf_gpio_pin_write(VB2H_PIN, 1);
+ nrf_gpio_pin_write(VA2L_PIN, 0);
+ nrf_gpio_pin_write(VA2H_PIN, 0);
+ nrf_gpio_pin_write(VB2L_PIN, 1);
+ break;
+
+ case ELECTRODES_A2LB2H:
+ nrf_gpio_pin_write(VA2H_PIN, 1);
+ nrf_gpio_pin_write(VB2L_PIN, 0);
+ nrf_gpio_pin_write(VB2H_PIN, 0);
+ nrf_gpio_pin_write(VA2L_PIN, 1);
+ break;
+
+ case ELECTRODES_A3HB3L:
+ nrf_gpio_pin_write(VB3H_PIN, 1);
+ nrf_gpio_pin_write(VA3L_PIN, 0);
+ nrf_gpio_pin_write(VA3H_PIN, 0);
+ nrf_gpio_pin_write(VB3L_PIN, 1);
+ break;
+
+ case ELECTRODES_A3LB3H:
+ nrf_gpio_pin_write(VA3H_PIN, 1);
+ nrf_gpio_pin_write(VB3L_PIN, 0);
+ nrf_gpio_pin_write(VB3H_PIN, 0);
+ nrf_gpio_pin_write(VA3L_PIN, 1);
+ break;
+
+ case ELECTRODES_A4HB4L:
+ nrf_gpio_pin_write(VB4H_PIN, 1);
+ nrf_gpio_pin_write(VA4L_PIN, 0);
+ nrf_gpio_pin_write(VA4H_PIN, 0);
+ nrf_gpio_pin_write(VB4L_PIN, 1);
+ break;
+
+ case ELECTRODES_A4LB4H:
+ nrf_gpio_pin_write(VA4H_PIN, 1);
+ nrf_gpio_pin_write(VB4L_PIN, 0);
+ nrf_gpio_pin_write(VB4H_PIN, 0);
+ nrf_gpio_pin_write(VA4L_PIN, 1);
+ break;
+ }
+}
+
+/*
+ dev_mode_set_cpg10_electrodes
+ (1)0x3000FF90nn
+ -func: dev_mode_set_cpg10_electrodes
+ -nn: electrodes mode 01h-09h
+ 01h: ELECTRODES_A1HB1L
+ 02h: ELECTRODES_A1HB1L
+ 03h: ELECTRODES_A1LB1H
+ 04h: ELECTRODES_A2HB2L
+ 05h: ELECTRODES_A2LB2H
+ 06h: ELECTRODES_A3HB3L
+ 07h: ELECTRODES_A3LB3H
+ 08h: ELECTRODES_A4HB4L
+ 09h: ELECTRODES_A4LB4H
+*/
+void dev_mode_set_cpg10_electrodes(uint8_t *ins)
+{
+ struct __PACKED
+ {
+ uint8_t id : 4;
+ uint8_t : 4;
+ uint16_t magic : 16;
+ uint8_t dev_opcode;
+ uint8_t electrodes_opcode;
+ uint8_t param[];
+ } *p_ins = (void *)ins;
+
+ cpg10_electrodes(p_ins->electrodes_opcode);
+}
diff --git a/cpg10_dev_mode.h b/cpg10_dev_mode.h
new file mode 100644
index 0000000..1e75029
--- /dev/null
+++ b/cpg10_dev_mode.h
@@ -0,0 +1,18 @@
+#ifndef __CPG10_DEV_MODE_H__
+#define __CPG10_DEV_MODE_H__
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "elite_board.h"
+
+ void dev_mode_set_cpg10_electrodes(uint8_t *ins);
+ void dev_mode_gpio_function(uint8_t *ins);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ! __CPG10_DEV_MODE_H__ */
\ No newline at end of file
diff --git a/cpg10_io.h b/cpg10_io.h
index a23bc25..1c7a649 100644
--- a/cpg10_io.h
+++ b/cpg10_io.h
@@ -13,6 +13,7 @@ extern "C"
#if (DEF_ELITE_MODEL == DEF_CURRENT_PULSE_GANERATOR_10)
+#define UNDEF_GPIO 0xFFFFFFFF
#define ADPT0_S4_PIN NRF_GPIO_PIN_MAP(0, 22)
#define ADPT0_S3_PIN NRF_GPIO_PIN_MAP(0, 25)
#define ADPT0_S2_PIN NRF_GPIO_PIN_MAP(0, 19)