From 572ce05016ca814ca4eeee30e12a5b9c1ed46480 Mon Sep 17 00:00:00 2001 From: cesare Date: Thu, 16 Sep 2021 11:10:45 -0700 Subject: [PATCH] Pre-update 2.1.1 : GPIO thread-safe bit set/clr --- Makefile | 2 +- ext/FreeRTOSConfig.h | 21 +++--- multizone-sdk.launch | 2 +- multizone.h | 17 +++++ zone3.1/main.c | 167 +++++++++++++++++++++++++------------------ zone3/main.c | 82 ++++++++++++--------- zone3/owi_sequence.c | 10 +-- 7 files changed, 183 insertions(+), 118 deletions(-) diff --git a/Makefile b/Makefile index 941381f..1f1fcdd 100644 --- a/Makefile +++ b/Makefile @@ -60,7 +60,7 @@ all: clean --boot bsp/$(BOARD)/boot/boot.hex \ zone1/zone1.hex \ zone2/zone2.hex \ - zone3.1/zone3.hex \ + zone3/zone3.hex \ zone4/zone4.hex .PHONY: clean diff --git a/ext/FreeRTOSConfig.h b/ext/FreeRTOSConfig.h index b1673b2..cb7cf5f 100644 --- a/ext/FreeRTOSConfig.h +++ b/ext/FreeRTOSConfig.h @@ -7,17 +7,22 @@ your application. */ #include "platform.h" +/* MultiZone deep-sleep implementation: +set configUSE_TICKLESS_IDLE 1 and configUSE_IDLE_HOOK 0 to enable +MultiZone vPortSuppressTicksAndSleep() */ + #define configISR_STACK_SIZE_WORDS (30) #define configMTIME_BASE_ADDRESS ( CLINT_BASE + CLINT_MTIME ) #define configMTIMECMP_BASE_ADDRESS ( CLINT_BASE + CLINT_MTIMECMP ) #define configUSE_PREEMPTION 1 +#define configLIST_VOLATILE volatile // #define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 -#define configUSE_TICKLESS_IDLE 1 +#define configUSE_TICKLESS_IDLE 0 #define configCPU_CLOCK_HZ ( (TickType_t) RTC_FREQ ) #define configTICK_RATE_HZ ( (TickType_t) 1000 ) -#define configMAX_PRIORITIES 3 //5 -#define configMINIMAL_STACK_SIZE 70 // 32-bit words +#define configMAX_PRIORITIES 2 +#define configMINIMAL_STACK_SIZE 72 // #define configMAX_TASK_NAME_LEN 16 #define configUSE_16_BIT_TICKS 0 // #define configIDLE_SHOULD_YIELD 1 @@ -38,14 +43,14 @@ your application. */ /* Memory allocation related definitions. */ // #define configSUPPORT_STATIC_ALLOCATION 1 // #define configSUPPORT_DYNAMIC_ALLOCATION 1 -#define configTOTAL_HEAP_SIZE 2064 +#define configTOTAL_HEAP_SIZE 2080 // #define configAPPLICATION_ALLOCATED_HEAP 1 /* Hook function related definitions. */ -#define configUSE_IDLE_HOOK 0 +#define configUSE_IDLE_HOOK 1 #define configUSE_TICK_HOOK 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 //2 -#define configUSE_MALLOC_FAILED_HOOK 1 //1 +#define configCHECK_FOR_STACK_OVERFLOW 0 //2 +#define configUSE_MALLOC_FAILED_HOOK 0 //1 #define configUSE_DAEMON_TASK_STARTUP_HOOK 0 /* Run time and task stats gathering related definitions. */ @@ -61,7 +66,7 @@ your application. */ /* Define to trap errors during development. */ // #define configASSERT( ( x ) ) if( ( x ) == 0 ) vAssertCalled( __FILE__, __LINE__ ) -#define configASSERT( x ) if( ( x ) == 0 ) { taskDISABLE_INTERRUPTS(); __asm volatile( "ebreak" ); for( ;; ); } +//#define configASSERT( x ) if( ( x ) == 0 ) { taskDISABLE_INTERRUPTS(); __asm volatile( "ebreak" ); for( ;; ); } /* Optional functions - most linkers will remove unused functions anyway. */ // #define INCLUDE_vTaskPrioritySet 1 diff --git a/multizone-sdk.launch b/multizone-sdk.launch index c940d03..9df2027 100644 --- a/multizone-sdk.launch +++ b/multizone-sdk.launch @@ -19,7 +19,7 @@ - + diff --git a/multizone.h b/multizone.h index 5009e86..9a6c741 100644 --- a/multizone.h +++ b/multizone.h @@ -204,4 +204,21 @@ }) +/* Atomic Memory Operations */ +#define BITSET(mem_addr, bit_mask) ({ \ + const register uint32_t rs1 = mem_addr; \ + const register uint32_t rs2 = bit_mask; \ + asm volatile ( "amoor.w x0, %0, (%1)" : : "r"(rs2), "r"(rs1) : "memory" ); \ +}) +#define BITCLR(mem_addr, bit_mask) ({ \ + const register uint32_t rs1 = mem_addr; \ + const register uint32_t rs2 = ~(bit_mask); \ + asm volatile ( "amoand.w x0, %0, (%1)" : : "r"(rs2), "r"(rs1) : "memory" ); \ +}) +#define BITINV(mem_addr, bit_mask) ({ \ + const register uint32_t rs1 = mem_addr; \ + const register uint32_t rs2 = bit_mask; \ + asm volatile ( "amoxor.w x0, %0, (%1)" : : "r"(rs2), "r"(rs1) : "memory" ); \ +}) + #endif /* MULTIZONE_H */ diff --git a/zone3.1/main.c b/zone3.1/main.c index c6464d2..e224e36 100644 --- a/zone3.1/main.c +++ b/zone3.1/main.c @@ -1,21 +1,20 @@ /* Copyright(C) 2020 Hex Five Security, Inc. - All Rights Reserved */ #include // strcmp() -#include // itoa() - -#include "FreeRTOS.h" -#include "task.h" /* RTOS task related API prototypes. */ #include "platform.h" #include "multizone.h" #include "owi_sequence.h" +#include "FreeRTOS.h" +#include "task.h" /* RTOS task related API prototypes. */ + typedef enum {zone1=1, zone2, zone3, zone4} Zone; #define SPI_TDI 11 // in #define SPI_TCK 10 // out (master) #define SPI_TDO 9 // out -#define SPI_SYN 8 // out - not used +//#define SPI_SYN 8 // out - not used static uint8_t CRC8(const uint8_t bytes[]){ @@ -37,26 +36,30 @@ static uint8_t CRC8(const uint8_t bytes[]){ } static uint32_t spi_rw(const uint32_t cmd){ - taskDISABLE_INTERRUPTS(); + taskENTER_CRITICAL(); const uint8_t bytes[] = {(uint8_t)cmd, (uint8_t)(cmd>>8), (uint8_t)(cmd>>16)}; const uint32_t tx_data = bytes[0]<<24 | bytes[1]<<16 | bytes[2]<<8 | CRC8(bytes); uint32_t rx_data = 0; - for (int i=32-1, bit; i>=0; i--){ + for (uint32_t i = 1<<31; i != 0; i >>= 1){ - bit = (tx_data >> i) & 1U; - GPIO_REG(GPIO_OUTPUT_VAL) = (bit==1 ? GPIO_REG(GPIO_OUTPUT_VAL) | (1 << SPI_TDO) : - GPIO_REG(GPIO_OUTPUT_VAL) & ~(1 << SPI_TDO) ); - GPIO_REG(GPIO_OUTPUT_VAL) |= (1 << SPI_TCK); for(volatile int w=0; w<150; w++){;} - GPIO_REG(GPIO_OUTPUT_VAL) ^= (1 << SPI_TCK); for(volatile int w=0; w<150; w++){;} - bit = ( GPIO_REG(GPIO_INPUT_VAL) >> SPI_TDI) & 1U; - rx_data = ( bit==1 ? rx_data | (1 << i) : rx_data & ~(1 << i) ); + if (tx_data & i) + BITSET(GPIO_BASE+GPIO_OUTPUT_VAL, 1 << SPI_TDO); + else + BITCLR(GPIO_BASE+GPIO_OUTPUT_VAL, 1 << SPI_TDO); - } + BITSET(GPIO_BASE+GPIO_OUTPUT_VAL, 1 << SPI_TCK); + + BITCLR(GPIO_BASE+GPIO_OUTPUT_VAL, 1 << SPI_TCK); - taskENABLE_INTERRUPTS(); + if( GPIO_REG(GPIO_INPUT_VAL) & (1<< SPI_TDI) ) + rx_data |= i; + + } + + taskEXIT_CRITICAL(); return rx_data; } @@ -64,7 +67,7 @@ static uint32_t spi_rw(const uint32_t cmd){ #define CMD_DUMMY 0xFFFFFF #define CMD_STOP 0x000000 -static volatile char msg[16] = {'\0'}; +static volatile char inbox[16] = {'\0'}; static volatile uint32_t usb_state = 0; static volatile uint32_t man_cmd = CMD_STOP; @@ -79,17 +82,25 @@ int main (void){ //while(1) MZONE_YIELD(); //while(1); + /* Setup hardware */ + GPIO_REG(GPIO_INPUT_EN) |= (1 << SPI_TDI); + GPIO_REG(GPIO_PULLUP_EN) |= (1 << SPI_TDI); + GPIO_REG(GPIO_OUTPUT_EN) |= ((1 << SPI_TCK) | (1<< SPI_TDO) | (1 << LED_RED) | (1 << LED_GRN)); + GPIO_REG(GPIO_DRIVE) |= ((1 << SPI_TCK) | (1<< SPI_TDO)); + + CSRS(mie, 1<<3); // enable msip/inbox interrupts + /* Create the task. */ - xTaskCreate(msg_handler_task, "msg_handler_task", configMINIMAL_STACK_SIZE, NULL, 2, &msg_handler_task_handle); + xTaskCreate(msg_handler_task, "msg_handler_task", configMINIMAL_STACK_SIZE, NULL, 1, &msg_handler_task_handle); /* Create the task. */ xTaskCreate(spi_poll_task, "spi_poll_task", configMINIMAL_STACK_SIZE, NULL, 1, &spi_poll_task_handle); /* Create the task. */ - xTaskCreate(robot_cmd_task, "robot_cmd_task", configMINIMAL_STACK_SIZE, NULL, 3, &robot_cmd_task_handle); + xTaskCreate(robot_cmd_task, "robot_cmd_task", configMINIMAL_STACK_SIZE, NULL, 1, &robot_cmd_task_handle); /* Create the task. */ - xTaskCreate(robot_seq_task, "robot_seq_task", configMINIMAL_STACK_SIZE, NULL, 3, &robot_seq_task_handle); + xTaskCreate(robot_seq_task, "robot_seq_task", configMINIMAL_STACK_SIZE, NULL, 1, &robot_seq_task_handle); /* Start the tasks and timer running. */ vTaskStartScheduler(); @@ -105,41 +116,59 @@ int main (void){ void msg_handler_task( void *pvParameters ){ // msg_handler_task - CSRS(mie, 1<<3); // enable msip/inbox interrupts - for( ;; ){ vTaskSuspend(NULL); // wait for message - if (strcmp("ping", (char *)msg)==0){ + // get a thread-safe copy of inbox[] + taskENTER_CRITICAL(); + char msg[16]; memcpy(msg, (const char *)inbox, sizeof msg); + taskEXIT_CRITICAL(); + + if (strncmp("ping", msg, sizeof msg)==0){ MZONE_SEND(zone1, (char [16]){"pong"}); } else if (usb_state==0x12670000 && man_cmd==CMD_STOP){ - if (strcmp("stop", (char *)msg)==0) owi_sequence_stop_req(); + if (strcmp("stop", msg)==0) + + owi_sequence_stop_req(); else if (!owi_sequence_is_running()){ - if (strcmp("start", (char *)msg)==0) {owi_sequence_start(MAIN); vTaskResume(robot_seq_task_handle);} - else if (strcmp("fold", (char *)msg)==0) {owi_sequence_start(FOLD); vTaskResume(robot_seq_task_handle);} - else if (strcmp("unfold",(char *)msg)==0) {owi_sequence_start(UNFOLD); vTaskResume(robot_seq_task_handle);} - - // Manual single-command adjustments - else if (strcmp("q", (char *)msg)==0) man_cmd = 0x000001; // grip close - else if (strcmp("a", (char *)msg)==0) man_cmd = 0x000002; // grip open - else if (strcmp("w", (char *)msg)==0) man_cmd = 0x000004; // wrist up - else if (strcmp("s", (char *)msg)==0) man_cmd = 0x000008; // wrist down - else if (strcmp("e", (char *)msg)==0) man_cmd = 0x000010; // elbow up - else if (strcmp("d", (char *)msg)==0) man_cmd = 0x000020; // elbow down - else if (strcmp("r", (char *)msg)==0) man_cmd = 0x000040; // shoulder up - else if (strcmp("f", (char *)msg)==0) man_cmd = 0x000080; // shoulder down - else if (strcmp("t", (char *)msg)==0) man_cmd = 0x000100; // base clockwise - else if (strcmp("g", (char *)msg)==0) man_cmd = 0x000200; // base counterclockwise - else if (strcmp("y", (char *)msg)==0) man_cmd = 0x010000; // light on - - if (man_cmd != CMD_STOP) - vTaskResume(robot_cmd_task_handle); + if (strncmp("start", msg, sizeof msg) == 0) { + owi_sequence_start(MAIN); + vTaskResume(robot_seq_task_handle); + + } else if (strncmp("fold", msg, sizeof msg) == 0) { + owi_sequence_start(FOLD); + vTaskResume(robot_seq_task_handle); + + } else if (strncmp("unfold", msg, sizeof msg) == 0) { + owi_sequence_start(UNFOLD); + vTaskResume(robot_seq_task_handle); + + } else { + + // Manual single-command adjustments + switch (msg[0]){ + case 'q' : man_cmd = 0x000001; break; // grip close + case 'a' : man_cmd = 0x000002; break; // grip open + case 'w' : man_cmd = 0x000004; break; // wrist up + case 's' : man_cmd = 0x000008; break; // wrist down + case 'e' : man_cmd = 0x000010; break; // elbow up + case 'd' : man_cmd = 0x000020; break; // elbow down + case 'r' : man_cmd = 0x000040; break; // shoulder up + case 'f' : man_cmd = 0x000080; break; // shoulder down + case 't' : man_cmd = 0x000100; break; // base clockwise + case 'g' : man_cmd = 0x000200; break; // base counterclockwise + case 'y' : man_cmd = 0x010000; break; // light on + } + + if (man_cmd != CMD_STOP) vTaskResume(robot_cmd_task_handle); + + } } @@ -151,12 +180,6 @@ void msg_handler_task( void *pvParameters ){ // msg_handler_task void spi_poll_task( void *pvParameters ){ // spi_poll_task - /* Setup hardware */ - GPIO_REG(GPIO_INPUT_EN) |= (1 << SPI_TDI); - GPIO_REG(GPIO_PULLUP_EN) |= (1 << SPI_TDI); - GPIO_REG(GPIO_OUTPUT_EN) |= ((1 << SPI_TCK) | (1<< SPI_TDO) | (1 << LED_RED) | (1 << LED_GRN)); - GPIO_REG(GPIO_DRIVE) |= ((1 << SPI_TCK) | (1<< SPI_TDO)) ; - for( ;; ){ /* Send keep alive packet, read spi, update USB state @@ -169,11 +192,11 @@ void spi_poll_task( void *pvParameters ){ // spi_poll_task if (rx_data != usb_state){ if (rx_data==0x12670000){ - MZONE_SEND(zone1, "USB ID 12670000"); + MZONE_SEND(zone1, (char [16]){"USB ID 12670000"}); } else if (usb_state==0x12670000){ - MZONE_SEND(zone1, (char [16]){"USB DISCONNECT"}); - owi_sequence_stop(); + owi_sequence_stop(); + MZONE_SEND(zone1, (char [16]){"USB DISCONNECT"}); } } @@ -182,9 +205,12 @@ void spi_poll_task( void *pvParameters ){ // spi_poll_task /* Blink LED */ const int LED = (usb_state==0x12670000 ? LED_GRN : LED_RED); - GPIO_REG(GPIO_OUTPUT_VAL) |= (1<=0; i--){ + for (uint32_t i = 1<<31; i != 0; i = i>>1){ - bit = (tx_data >> i) & 1U; - GPIO_REG(GPIO_OUTPUT_VAL) = (bit==1 ? GPIO_REG(GPIO_OUTPUT_VAL) | (1 << SPI_TDO) : - GPIO_REG(GPIO_OUTPUT_VAL) & ~(1 << SPI_TDO) ); + if (tx_data & i) + GPIO_REG(GPIO_OUTPUT_VAL) |= (1 << SPI_TDO); + else + GPIO_REG(GPIO_OUTPUT_VAL) &= ~(1 << SPI_TDO); - GPIO_REG(GPIO_OUTPUT_VAL) |= (1 << SPI_TCK); volatile int w1=0; while(w1<5) w1++; - GPIO_REG(GPIO_OUTPUT_VAL) ^= (1 << SPI_TCK); volatile int w2=0; while(w2<5) w2++; - bit = ( GPIO_REG(GPIO_INPUT_VAL) >> SPI_TDI) & 1U; - rx_data = ( bit==1 ? rx_data | (1 << i) : rx_data & ~(1 << i) ); + GPIO_REG(GPIO_OUTPUT_VAL) |= (1 << SPI_TCK); + GPIO_REG(GPIO_OUTPUT_VAL) &= ~(1 << SPI_TCK); + + if( GPIO_REG(GPIO_INPUT_VAL) & (1<< SPI_TDI) ) + rx_data |= i; } @@ -59,10 +61,10 @@ static uint32_t spi_rw(const uint32_t cmd){ #define CMD_DUMMY 0xFFFFFF #define CMD_STOP 0x000000 +static volatile char msg[16] = {'\0'}; static volatile uint32_t usb_state = 0; static volatile uint32_t man_cmd = CMD_STOP; static volatile int LED = LED_RED; -static volatile char msg[16] = {'\0'}; uint64_t task0(); // OWI Sequence uint64_t task1(); // Manual cmd stop @@ -186,27 +188,41 @@ void msg_handler(const char *msg){ else if (!owi_sequence_is_running()){ - if (strcmp("start", msg)==0) {owi_sequence_start(MAIN); timer_set(0, 0);} - else if (strcmp("fold", msg)==0) {owi_sequence_start(FOLD); timer_set(0, 0);} - else if (strcmp("unfold",msg)==0) {owi_sequence_start(UNFOLD); timer_set(0, 0);} - - // Manual single-command adjustments - if (strcmp("q", msg)==0) man_cmd = 0x000001; // grip close - else if (strcmp("a", msg)==0) man_cmd = 0x000002; // grip open - else if (strcmp("w", msg)==0) man_cmd = 0x000004; // wrist up - else if (strcmp("s", msg)==0) man_cmd = 0x000008; // wrist down - else if (strcmp("e", msg)==0) man_cmd = 0x000010; // elbow up - else if (strcmp("d", msg)==0) man_cmd = 0x000020; // elbow down - else if (strcmp("r", msg)==0) man_cmd = 0x000040; // shoulder up - else if (strcmp("f", msg)==0) man_cmd = 0x000080; // shoulder down - else if (strcmp("t", msg)==0) man_cmd = 0x000100; // base clockwise - else if (strcmp("g", msg)==0) man_cmd = 0x000200; // base counterclockwise - else if (strcmp("y", msg)==0) man_cmd = 0x010000; // light on - - if (man_cmd != CMD_STOP){ - spi_rw(man_cmd); - timer_set(1, MZONE_RDTIME() + MAN_CMD_TIME); - } + if (strcmp("start", msg) == 0) { + owi_sequence_start(MAIN); + timer_set(0, 0); + + } else if (strcmp("fold", msg) == 0) { + owi_sequence_start(FOLD); + timer_set(0, 0); + + } else if (strcmp("unfold", msg) == 0) { + owi_sequence_start(UNFOLD); + timer_set(0, 0);} + + } else { + + // Manual single-command adjustments + switch (msg[0]){ + case 'q' : man_cmd = 0x000001; break; // grip close + case 'a' : man_cmd = 0x000002; break; // grip open + case 'w' : man_cmd = 0x000004; break; // wrist up + case 's' : man_cmd = 0x000008; break; // wrist down + case 'e' : man_cmd = 0x000010; break; // elbow up + case 'd' : man_cmd = 0x000020; break; // elbow down + case 'r' : man_cmd = 0x000040; break; // shoulder up + case 'f' : man_cmd = 0x000080; break; // shoulder down + case 't' : man_cmd = 0x000100; break; // base clockwise + case 'g' : man_cmd = 0x000200; break; // base counterclockwise + case 'y' : man_cmd = 0x010000; break; // light on + } + + if (man_cmd != CMD_STOP){ + spi_rw(man_cmd); + timer_set(1, MZONE_RDTIME() + MAN_CMD_TIME); + } + + } } diff --git a/zone3/owi_sequence.c b/zone3/owi_sequence.c index 3e3293d..5d0bf80 100644 --- a/zone3/owi_sequence.c +++ b/zone3/owi_sequence.c @@ -26,7 +26,7 @@ struct sequence_step{ int duration_ms; }; -// R6 04-SEP-2021 +// R6 16-SEP-2021 #define T_STOP 1000 #define T_GRIP 1200 #define T_WRIST 1800 @@ -42,10 +42,10 @@ static struct sequence_step main_sequence[] = { { .command = STOP, .duration_ms = T_STOP }, { .command = GRIP_CLOSE, .duration_ms = T_GRIP }, { .command = BASE_COUNTERCLK, .duration_ms = T_BASE }, - { .command = BASE_COUNTERCLK, .duration_ms = T_ARM * 0.80}, // 0.75 0.68 - { .command = ARM_UP, .duration_ms = T_ARM * 1.05}, // 1.05 - { .command = ELBOW_UP, .duration_ms = T_ARM * 0.05}, // 0.05 - { .command = WRIST_DWN, .duration_ms = T_WRIST * 0.80}, // 0.80 + { .command = BASE_COUNTERCLK, .duration_ms = T_ARM * 80/100}, // 80% + { .command = ARM_UP, .duration_ms = T_ARM * 105/100}, // 105% + { .command = ELBOW_UP, .duration_ms = T_ARM * 10/100}, // 10% + { .command = WRIST_DWN, .duration_ms = T_WRIST * 80/100}, // 80% { .command = GRIP_OPEN, .duration_ms = T_GRIP }, };