Skip to content

Commit

Permalink
Pre-update 2.1.1 : GPIO thread-safe bit set/clr
Browse files Browse the repository at this point in the history
  • Loading branch information
@cgarlati
cgarlati committed Sep 16, 2021
1 parent 85cf2e3 commit 572ce05
Show file tree
Hide file tree
Showing 7 changed files with 183 additions and 118 deletions.
2 changes: 1 addition & 1 deletion Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
21 changes: 13 additions & 8 deletions ext/FreeRTOSConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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. */
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion multizone-sdk.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.gdbServerOther" value="-f ./bsp/${ConfigName}/openocd.cfg"/>
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.gdbServerTclPortNumber" value="6666"/>
<intAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.gdbServerTelnetPortNumber" value="4444"/>
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.otherInitCommands" value="add-symbol-file ${workspace_loc:/hexfive-kern}/build/${config_name:multizone-sdk}/kernel.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/bsp/${config_name:multizone-sdk}/boot/boot.elf &#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone1/zone1.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone2/zone2.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone3.1/zone3.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone4/zone4.elf"/>
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.otherInitCommands" value="add-symbol-file ${workspace_loc:/hexfive-kern}/build/${config_name:multizone-sdk}/kernel.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/bsp/${config_name:multizone-sdk}/boot/boot.elf &#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone1/zone1.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone2/zone2.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone3/zone3.elf&#10;add-symbol-file ${workspace_loc:/multizone-sdk}/zone4/zone4.elf"/>
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.otherRunCommands" value=""/>
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.openocd.secondResetType" value=""/>
<stringAttribute key="ilg.gnumcueclipse.debug.gdbjtag.svdPath" value=""/>
Expand Down
17 changes: 17 additions & 0 deletions multizone.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
167 changes: 97 additions & 70 deletions zone3.1/main.c
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,20 @@
/* Copyright(C) 2020 Hex Five Security, Inc. - All Rights Reserved */

#include <string.h> // strcmp()
#include <stdlib.h> // 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[]){

Expand All @@ -37,34 +36,38 @@ 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;
}

#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;

Expand All @@ -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();
Expand All @@ -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);

}

}

Expand All @@ -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
Expand All @@ -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"});
}
}

Expand All @@ -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<<LED);

BITSET(GPIO_BASE+GPIO_OUTPUT_VAL, 1<<LED);

vTaskDelay( (TickType_t) (25 / portTICK_PERIOD_MS) );
GPIO_REG(GPIO_OUTPUT_VAL) &= ~(1<<LED);

BITCLR(GPIO_BASE+GPIO_OUTPUT_VAL, 1<<LED);

/* loop approx every 1 sec */
vTaskDelay( (TickType_t) (975 / portTICK_PERIOD_MS) );
Expand All @@ -205,29 +231,30 @@ void robot_cmd_task( void *pvParameters ){ // robot_cmd_task

spi_rw(man_cmd = CMD_STOP);


}

}

void robot_seq_task( void *pvParameters ){ // robot_cmd_task
void robot_seq_task( void *pvParameters ){ // robot_seq_task

for( ;; ){
for (;;) {

vTaskSuspend(NULL);
vTaskSuspend(NULL);

TickType_t xLastWakeTime = xTaskGetTickCount();
TickType_t xLastWakeTime = xTaskGetTickCount();

while ( usb_state==0x12670000 && owi_sequence_next() != -1){
while (usb_state == 0x12670000 && owi_sequence_next() != -1) {

spi_rw(owi_sequence_get_cmd());
spi_rw(owi_sequence_get_cmd());

vTaskDelayUntil( &xLastWakeTime, (TickType_t) (owi_sequence_get_ms() / portTICK_PERIOD_MS) );
vTaskDelayUntil(&xLastWakeTime, (TickType_t) (owi_sequence_get_ms() / portTICK_PERIOD_MS));

}
}

}
}

}
}

void vApplicationIdleHook( void ){

Expand All @@ -241,9 +268,9 @@ void vApplicationIdleHook( void ){

MZONE_WFI();

/* alternative deep-sleep implementation:
set configUSE_TICKLESS_IDLE 1 to use MultiZone implementation of
vPortSuppressTicksAndSleep() */
/* MultiZone deep-sleep implementation:
set configUSE_TICKLESS_IDLE 1 and configUSE_IDLE_HOOK 0 to enable
MultiZone vPortSuppressTicksAndSleep() */

}

Expand Down Expand Up @@ -285,9 +312,9 @@ void trap_handler(uint32_t cause){
case 0x80000003: // msip/inbox

// read the incoming message & clear msip
;char const tmp[16];
if (MZONE_RECV(1, tmp))
memcpy((char *)msg, tmp, sizeof msg);
;char buff[16];
if (MZONE_RECV(1, buff))
memcpy((char *)inbox, buff, sizeof buff);

// Resume the suspended task.
const BaseType_t xYieldRequired = xTaskResumeFromISR( msg_handler_task_handle );
Expand Down
Loading

0 comments on commit 572ce05

Please sign in to comment.