535 lines
13 KiB
C++
535 lines
13 KiB
C++
#include <Device/UsbDBulkInterface.h>
|
|
#include <FreeRTOS.h>
|
|
#include <task.h>
|
|
#include <stm32f7xx_hal.h>
|
|
#include <main.h>
|
|
|
|
#include <LFramework/USB/Device/USBDevice.h>
|
|
#include <LFramework/Debug.h>
|
|
#include <LFramework/IO/Terminal/TerminalAnsi.h>
|
|
#include <LFramework/Thread/Thread.h>
|
|
#include <cstring>
|
|
#include <usart.h>
|
|
|
|
#include <usbd_customhid.h>
|
|
|
|
#include <usb_device.h>
|
|
|
|
extern USBD_HandleTypeDef hUsbDeviceFS;
|
|
|
|
using namespace LFramework;
|
|
//using namespace LFramework::USB;
|
|
|
|
//UsbDBulkInterface usbInterface;
|
|
//UsbDDeviceContext usbContext(&usbInterface);
|
|
|
|
/*class PollingTransfer : public LFramework::USB::UsbDTransfer {
|
|
public:
|
|
enum class State {
|
|
Pending,
|
|
Success,
|
|
Fail
|
|
};
|
|
|
|
PollingTransfer(){
|
|
callbackIsr = &PollingTransfer::transferComleteCallbackStatic;
|
|
}
|
|
void reset(){
|
|
_state = State::Pending;
|
|
size = 0;
|
|
actualSize = 0;
|
|
}
|
|
State getState() const {
|
|
return _state;
|
|
}
|
|
private:
|
|
State _state = State::Pending;
|
|
void transferComleteCallback(bool success){
|
|
_state = success ? State::Success : State::Fail;
|
|
}
|
|
|
|
static void transferComleteCallbackStatic(LFramework::USB::UsbDTransfer* _this, bool success){
|
|
static_cast<PollingTransfer*>(_this)->transferComleteCallback(success);
|
|
}
|
|
};*/
|
|
|
|
/*void rxThreadHandler(UsbDEndpoint* ep) {
|
|
Debug::Log() << "Rx thread enter";
|
|
uint8_t buffer[64];
|
|
|
|
for(;;){
|
|
PollingTransfer rxTransfer{};
|
|
rxTransfer.buffer = buffer;
|
|
rxTransfer.size = sizeof(buffer);
|
|
ep->transferAsync(&rxTransfer);
|
|
Debug::Log() << "USB Rx begin";
|
|
while(rxTransfer.getState() == PollingTransfer::State::Pending){
|
|
vTaskDelay(1);
|
|
}
|
|
Debug::Log() << "USB Rx end";
|
|
Debug::Log() << "USB Rx size:" << rxTransfer.actualSize;
|
|
}
|
|
}
|
|
|
|
void txThreadHandler(UsbDEndpoint* ep) {
|
|
Debug::Log() << "Tx thread enter";
|
|
uint8_t buffer[31];
|
|
|
|
for(int i = 0; i < sizeof(buffer); ++i){
|
|
buffer[i] = i;
|
|
}
|
|
|
|
while(true){
|
|
PollingTransfer txTransfer;
|
|
txTransfer.buffer = nullptr;
|
|
txTransfer.size = 0;
|
|
Debug::Log() << "USB Tx begin";
|
|
ep->transferAsync(&txTransfer);
|
|
while(txTransfer.getState() == PollingTransfer::State::Pending){
|
|
vTaskDelay(1);
|
|
}
|
|
|
|
Debug::Log() << "USB Tx end";
|
|
}
|
|
}*/
|
|
|
|
|
|
struct GamepadState {
|
|
bool up;
|
|
bool down;
|
|
bool left;
|
|
bool right;
|
|
|
|
bool a;
|
|
bool b;
|
|
bool c;
|
|
|
|
bool x;
|
|
bool y;
|
|
bool z;
|
|
|
|
bool start;
|
|
bool mode;
|
|
};
|
|
|
|
static void GamepadReadDelay() {
|
|
for(volatile int i = 0; i < 100; ++i){
|
|
|
|
}
|
|
}
|
|
|
|
__STATIC_INLINE void DWT_Delay_us(volatile uint32_t microseconds)
|
|
{
|
|
|
|
/* Go to number of cycles for system */
|
|
microseconds *= (HAL_RCC_GetHCLKFreq() / 1000000);
|
|
|
|
DWT->CYCCNT = 0;
|
|
/* Delay till end */
|
|
while (DWT->CYCCNT < microseconds);
|
|
}
|
|
|
|
|
|
|
|
int eventId = 0;
|
|
|
|
void printState(bool oldState, bool newState, const char* name){
|
|
if(oldState != newState){
|
|
|
|
lfDebug() << eventId << ":" << name << (newState ? " pressed" : " released");
|
|
eventId++;
|
|
}
|
|
}
|
|
|
|
#pragma pack(push, 1)
|
|
struct GamepadReport {
|
|
uint8_t reportId;
|
|
uint16_t buttons;
|
|
};
|
|
#pragma pack(pop)
|
|
|
|
extern SPI_HandleTypeDef hspi2;
|
|
|
|
class Ps1Gamepad {
|
|
public:
|
|
static constexpr size_t HeaderSize = 3;
|
|
static constexpr size_t MaxDataSize = 9 * 2;
|
|
|
|
struct Type {
|
|
static constexpr uint8_t Digital = 4;
|
|
static constexpr uint8_t Negcon = 2;
|
|
static constexpr uint8_t AnalogueRed = 7;
|
|
static constexpr uint8_t AnalogueGreen = 5;
|
|
};
|
|
struct Response {
|
|
uint8_t dummy;
|
|
uint8_t id;
|
|
uint8_t padID;
|
|
uint8_t buttons0;
|
|
uint8_t buttons1;
|
|
uint8_t analogRX;
|
|
uint8_t analogRY;
|
|
uint8_t analogLX;
|
|
uint8_t analogLY;
|
|
|
|
struct {
|
|
uint8_t right;
|
|
uint8_t left;
|
|
uint8_t up;
|
|
uint8_t down;
|
|
uint8_t triangle;
|
|
uint8_t circle;
|
|
uint8_t cross;
|
|
uint8_t square;
|
|
uint8_t l1;
|
|
uint8_t r1;
|
|
uint8_t l2;
|
|
uint8_t r2;
|
|
|
|
}pressure;
|
|
|
|
|
|
bool hasAnalogJoyData() {
|
|
return (id & 0xf) >=3;
|
|
}
|
|
|
|
bool select() {
|
|
return (buttons0 & (1 << 0)) == 0;
|
|
}
|
|
|
|
bool l3() {
|
|
return (buttons0 & (1 << 1)) == 0;
|
|
}
|
|
bool r3() {
|
|
return (buttons0 & (1 << 2)) == 0;
|
|
}
|
|
bool start() {
|
|
return (buttons0 & (1 << 3)) == 0;
|
|
}
|
|
bool up() {
|
|
return (buttons0 & (1 << 4)) == 0;
|
|
}
|
|
bool right() {
|
|
return (buttons0 & (1 << 5)) == 0;
|
|
}
|
|
bool down() {
|
|
return (buttons0 & (1 << 6)) == 0;
|
|
}
|
|
bool left() {
|
|
return (buttons0 & (1 << 7)) == 0;
|
|
}
|
|
|
|
|
|
bool l2() {
|
|
return (buttons1 & (1 << 0)) == 0;
|
|
}
|
|
bool r2() {
|
|
return (buttons1 & (1 << 1)) == 0;
|
|
}
|
|
bool l1() {
|
|
return (buttons1 & (1 << 2)) == 0;
|
|
}
|
|
bool r1() {
|
|
return (buttons1 & (1 << 3)) == 0;
|
|
}
|
|
bool triangle() {
|
|
return (buttons1 & (1 << 4)) == 0;
|
|
}
|
|
bool circle() {
|
|
return (buttons1 & (1 << 5)) == 0;
|
|
}
|
|
bool cross() {
|
|
return (buttons1 & (1 << 6)) == 0;
|
|
}
|
|
bool square() {
|
|
return (buttons1 & (1 << 7)) == 0;
|
|
}
|
|
};
|
|
|
|
const Response& read() {
|
|
uint8_t txBuffer[5] = {0x01, 0x42, 0x00, 0x00, 0x00};
|
|
executeCommand(txBuffer, sizeof(txBuffer));
|
|
return *(Response*)&rxBuffer[0];
|
|
}
|
|
|
|
void enterConfigMode() {
|
|
uint8_t cmd[]={ 0x01,0x43,0x00,0x01,0x00};
|
|
executeCommand(cmd, sizeof(cmd));
|
|
}
|
|
|
|
void setControllerMode(bool analog, bool lockMode) {
|
|
uint8_t cmd[]={ 0x01, 0x44, 0x00, (analog ? 0x01 : 0x00), (lockMode ? 0x03 : 0x5A), 0x00, 0x00, 0x00, 0x00 };
|
|
executeCommand(cmd, sizeof(cmd));
|
|
}
|
|
|
|
|
|
struct DataMask {
|
|
struct {
|
|
uint8_t b0;
|
|
uint8_t b1;
|
|
uint8_t b2;
|
|
} value;
|
|
|
|
constexpr DataMask operator | (const DataMask& other) const{
|
|
return {{value.b0 | other.value.b0, value.b1 | other.value.b1, value.b2 | other.value.b2,}};
|
|
}
|
|
|
|
static constexpr DataMask digitalB0() { return {{0x01, 0x00, 0x00}}; }
|
|
static constexpr DataMask digitalB1() { return {{0x02, 0x00, 0x00}}; }
|
|
static constexpr DataMask allDigital() { return digitalB0() | digitalB1();}
|
|
|
|
static constexpr DataMask analogJoyRx() { return {{0x04, 0x00, 0x00}}; }
|
|
static constexpr DataMask analogJoyRy() { return {{0x08, 0x00, 0x00}}; }
|
|
static constexpr DataMask analogJoyLx() { return {{0x10, 0x00, 0x00}}; }
|
|
static constexpr DataMask analogJoyLy() { return {{0x20, 0x00, 0x00}}; }
|
|
|
|
static constexpr DataMask leftAnalogJoy() { return analogJoyLx() | analogJoyLy(); }
|
|
static constexpr DataMask rightAnalogJoy() { return analogJoyRx() | analogJoyRy(); }
|
|
static constexpr DataMask allAnalogJoy() { return leftAnalogJoy() | rightAnalogJoy(); }
|
|
|
|
static constexpr DataMask pressureR() { return {{0x40, 0x00, 0x00}}; }
|
|
static constexpr DataMask pressureL() { return {{0x80, 0x00, 0x00}}; }
|
|
static constexpr DataMask pressureU() { return {{0x00, 0x01, 0x00}}; }
|
|
static constexpr DataMask pressureD() { return {{0x00, 0x02, 0x00}}; }
|
|
|
|
static constexpr DataMask pressureTriangle() { return {{0x00, 0x04, 0x00}}; }
|
|
static constexpr DataMask pressureCircle() { return {{0x00, 0x08, 0x00}}; }
|
|
static constexpr DataMask pressureCross() { return {{0x00, 0x10, 0x00}}; }
|
|
static constexpr DataMask pressureSquare() { return {{0x00, 0x20, 0x00}}; }
|
|
|
|
static constexpr DataMask pressureL1() { return {{0x00, 0x40, 0x00}}; }
|
|
static constexpr DataMask pressureR1() { return {{0x00, 0x80, 0x00}}; }
|
|
|
|
static constexpr DataMask pressureL2() { return {{0x00, 0x00, 0x01}}; }
|
|
static constexpr DataMask pressureR2() { return {{0x00, 0x00, 0x02}}; }
|
|
|
|
static constexpr DataMask allPressure() {
|
|
return
|
|
pressureR() | pressureL() | pressureU() | pressureD() |
|
|
pressureTriangle() | pressureCircle() | pressureCross() | pressureSquare() |
|
|
pressureL1() | pressureR1() | pressureL2() | pressureR2();
|
|
}
|
|
|
|
static constexpr DataMask allDigitalAndAnalogJoy() {
|
|
return allDigital() | allAnalogJoy();
|
|
}
|
|
|
|
static constexpr DataMask all() {
|
|
return allDigitalAndAnalogJoy() | allPressure();
|
|
}
|
|
|
|
static constexpr DataMask none() {
|
|
return allDigitalAndAnalogJoy() | allPressure();
|
|
}
|
|
|
|
constexpr DataMask operator ~() {
|
|
return {{static_cast<uint8_t>(~value.b0 & 0xff), static_cast<uint8_t>(~value.b1 & 0xff), static_cast<uint8_t>(~value.b2 & 0x03)}};
|
|
}
|
|
|
|
};
|
|
|
|
//All buttons
|
|
void setDataMask(DataMask mask) {
|
|
uint8_t cmd[]={ 0x01, 0x4F, 0x00, mask.value.b0, mask.value.b1, mask.value.b2, 0x00, 0x00, 0x00 };
|
|
executeCommand(cmd, sizeof(cmd));
|
|
|
|
}
|
|
|
|
void exitConfigMode() {
|
|
uint8_t cmd[]={ 0x01, 0x43, 0x00, 0x00, 0x5A, 0x5A, 0x5A, 0x5A, 0x5A };
|
|
executeCommand(cmd, sizeof(cmd));
|
|
}
|
|
|
|
bool executeCommand(uint8_t* txBuffer, uint8_t size){
|
|
selectDevice();
|
|
int byteDelay = 1;
|
|
DWT_Delay_us(byteDelay);
|
|
|
|
//transfer header
|
|
for(uint8_t i = 0; i < HeaderSize; ++i){
|
|
HAL_SPI_TransmitReceive(&hspi2, &txBuffer[i], &rxBuffer[i], 1, HAL_MAX_DELAY);
|
|
DWT_Delay_us(byteDelay);
|
|
}
|
|
|
|
auto expectedDataLength = (rxBuffer[1] & 0x0f) * 2;
|
|
|
|
//transfer data
|
|
for(uint8_t i = HeaderSize; i < size; ++i){
|
|
HAL_SPI_TransmitReceive(&hspi2, &txBuffer[i], &rxBuffer[i], 1, HAL_MAX_DELAY);
|
|
DWT_Delay_us(byteDelay);
|
|
}
|
|
|
|
//push zeroes if user issued incorrect length command
|
|
for(uint8_t i = size; i < (HeaderSize + expectedDataLength); ++i){
|
|
uint8_t zero = 0;
|
|
HAL_SPI_TransmitReceive(&hspi2, &zero, &rxBuffer[i], 1, HAL_MAX_DELAY);
|
|
DWT_Delay_us(byteDelay);
|
|
}
|
|
|
|
deselectDevice();
|
|
return true;
|
|
}
|
|
void selectDevice() {
|
|
HAL_GPIO_WritePin(GAMEPAD_CS_GPIO_Port, GAMEPAD_CS_Pin, GPIO_PIN_RESET);
|
|
}
|
|
void deselectDevice() {
|
|
HAL_GPIO_WritePin(GAMEPAD_CS_GPIO_Port, GAMEPAD_CS_Pin, GPIO_PIN_SET);
|
|
}
|
|
private:
|
|
uint8_t rxBuffer[HeaderSize + MaxDataSize];
|
|
};
|
|
|
|
|
|
extern"C" void StartDefaultTask(void const * argument){
|
|
|
|
Terminal::out << Terminal::Ansi::Cursor::MoveHome() << Terminal::Ansi::Viewport::ClearScreen();
|
|
Debug::Log() << "Hello !";
|
|
|
|
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
|
|
DWT->LAR = 0xC5ACCE55;
|
|
DWT->CYCCNT = 0;
|
|
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
|
|
|
|
|
|
MX_USB_DEVICE_Init();
|
|
|
|
|
|
Ps1Gamepad gamepad;
|
|
|
|
Ps1Gamepad::Response oldState;
|
|
oldState.buttons0 = 0xff;
|
|
oldState.buttons1 = 0xff;
|
|
|
|
gamepad.enterConfigMode();
|
|
DWT_Delay_us(5);
|
|
gamepad.setControllerMode(true, false);
|
|
DWT_Delay_us(4); //not less than 3!!
|
|
gamepad.exitConfigMode();
|
|
DWT_Delay_us(4);
|
|
|
|
gamepad.enterConfigMode();
|
|
DWT_Delay_us(4);
|
|
gamepad.setDataMask(Ps1Gamepad::DataMask::all());
|
|
DWT_Delay_us(4); //not less than 3!!
|
|
gamepad.exitConfigMode();
|
|
DWT_Delay_us(4);
|
|
|
|
while(true){
|
|
//vTaskDelay(1000);
|
|
|
|
|
|
|
|
|
|
//USBD_CUSTOM_HID_SendReport(&hUsbDeviceFS, (uint8_t*)&report, sizeof(report));
|
|
|
|
DWT_Delay_us(16000);
|
|
//gamepad.exitConfigMode();
|
|
auto state = gamepad.read();
|
|
|
|
|
|
printState(oldState.up(), state.up(), "UP");
|
|
printState(oldState.down(), state.down(), "DOWN");
|
|
printState(oldState.left(), state.left(), "LEFT");
|
|
printState(oldState.right(), state.right(), "RIGHT");
|
|
|
|
printState(oldState.l1(), state.l1(), "L1");
|
|
printState(oldState.l2(), state.l2(), "L2");
|
|
printState(oldState.r1(), state.r1(), "R1");
|
|
printState(oldState.r2(), state.r2(), "R2");
|
|
|
|
printState(oldState.l3(), state.l3(), "L3");
|
|
printState(oldState.r3(), state.r3(), "R3");
|
|
|
|
printState(oldState.circle(), state.circle(), "CIRCLE");
|
|
printState(oldState.square(), state.square(), "SQUARE");
|
|
printState(oldState.triangle(), state.triangle(), "TRIANGLE");
|
|
printState(oldState.cross(), state.cross(), "CROSS");
|
|
|
|
printState(oldState.select(), state.select(), "SELECT");
|
|
printState(oldState.start(), state.start(), "START");
|
|
|
|
|
|
/*if(oldState.up() != state.up()){
|
|
if(state.up()){
|
|
gamepad.enterConfigMode();
|
|
gamepad.setControllerMode(true, true);
|
|
gamepad.exitConfigMode();
|
|
}
|
|
}
|
|
|
|
if(oldState.down() != state.down()){
|
|
if(state.down()){
|
|
gamepad.enterConfigMode();
|
|
gamepad.setControllerMode(false, false);
|
|
gamepad.exitConfigMode();
|
|
}
|
|
}
|
|
*/
|
|
|
|
lfDebug() << "mode: " << hex(state.id);
|
|
|
|
if(oldState.hasAnalogJoyData() != state.hasAnalogJoyData()){
|
|
if(state.hasAnalogJoyData()){
|
|
lfDebug() << "Analog data ENABLED";
|
|
}else {
|
|
lfDebug() << "Analog data DISABLED";
|
|
}
|
|
}
|
|
|
|
if(state.hasAnalogJoyData() && oldState.hasAnalogJoyData()){
|
|
if(state.analogRX != oldState.analogRX){
|
|
lfDebug() << "Rx: " << state.analogRX;
|
|
}
|
|
}
|
|
|
|
if(state.hasAnalogJoyData() && oldState.hasAnalogJoyData()){
|
|
if(state.analogLX != oldState.analogLX){
|
|
lfDebug() << "Lx: " << state.analogLX;
|
|
}
|
|
}
|
|
|
|
|
|
uint8_t* old = (uint8_t*)&oldState;
|
|
uint8_t* n = (uint8_t*)&state;
|
|
bool headerPrinted = false;
|
|
for(int i = 0; i < sizeof(state); ++i){
|
|
if(n[i] != old[i]){
|
|
if(!headerPrinted){
|
|
lfDebug() << "[!] Change:";
|
|
headerPrinted = true;
|
|
}
|
|
lfDebug() << "b" << i << ": " << n[i];
|
|
}
|
|
}
|
|
|
|
|
|
oldState = state;
|
|
|
|
|
|
|
|
/*lfDebug() << "Select device";
|
|
gamepad.selectDevice();*/
|
|
|
|
//HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
|
|
// DWT_Delay_us(500000);
|
|
|
|
/*lfDebug() << "Deselect device";
|
|
gamepad.deselectDevice();
|
|
|
|
HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
|
|
DWT_Delay_us(1000000);*/
|
|
//
|
|
|
|
|
|
}
|
|
}
|
|
|
|
extern "C" void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName){
|
|
Debug::Log() << "Stack overflow in task " << (const char*)pcTaskName;
|
|
for(;;);
|
|
}
|
|
extern "C" void vApplicationMallocFailedHook(void){
|
|
Debug::Log() << "Malloc failed";
|
|
for(;;);
|
|
}
|