diff --git a/README.md b/README.md index ca1077c8..45fd0804 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,18 @@ +# sys-con-plus-mitm + +#### Adding man-in-the-middle support to the sys-con sysmodule + +## Description +This sysmodule aims to add **Wireless** support for third-party controllers to the Nintendo Switch. This will be useless once sys-con sysmodule achieves the direct bluetooth support. + +This sysmodule is inspired by the hid-mitm sysmodule (https://github.com/jakibaki/hid-mitm) which is now obsolete. + +The **USB** support provided by the original sys-con is intended to remain untouched so that both wireless and wired controllers can work together. + +## Example setup + +🎮 ))) Bluetooth ((( RaspberryPI ))) WiFi ((( NintendoSwitch + # sys-con #### A Nintendo Switch custom sysmodule for third-party controller support. No man-in-the-middle required! diff --git a/common/config/sys-con/config_network.ini b/common/config/sys-con/config_network.ini new file mode 100644 index 00000000..092c6f10 --- /dev/null +++ b/common/config/sys-con/config_network.ini @@ -0,0 +1,20 @@ +; Config for the Dualshock 4 controller +left_stick_deadzone = 10 ; from 0 to 100 +right_stick_deadzone = 10 ; from 0 to 100 +left_trigger_deadzone = 0 ; from 0 to 100 +right_trigger_deadzone = 0 ; from 0 to 100 + +color_led = 0,0,64 ; from 0 to 255 + +color_body = 77,77,77 +color_buttons = 0,0,0 + +; [9.0.0+] +color_leftGrip = 33,33,33 +color_rightGrip = 33,33,33 + +swap_dpad_and_lstick = false ; set this to true to swap the d-pad and left stick + +; For information on input mapping, see "example.ini" +;KEY_CAPTURE = LSTICK_CLICK ; Remove the semicolon at the start to take effect + diff --git a/common/config/sys-con/port_config.ini b/common/config/sys-con/port_config.ini new file mode 100644 index 00000000..f523c062 --- /dev/null +++ b/common/config/sys-con/port_config.ini @@ -0,0 +1,2 @@ +[Port] +port=8080 diff --git a/network_clients/linux_or_raspberry/Makefile b/network_clients/linux_or_raspberry/Makefile new file mode 100644 index 00000000..01c207e0 --- /dev/null +++ b/network_clients/linux_or_raspberry/Makefile @@ -0,0 +1,10 @@ +SOURCES=sys-con-client.c +OUT=sys-con-client + +$(OUT): $(SOURCES) + gcc -o $@ $< + +.PHONY: clean + +clean: + rm $(OUT) diff --git a/network_clients/linux_or_raspberry/sys-con-client.c b/network_clients/linux_or_raspberry/sys-con-client.c new file mode 100644 index 00000000..63b560a2 --- /dev/null +++ b/network_clients/linux_or_raspberry/sys-con-client.c @@ -0,0 +1,210 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#define BUFFER_SIZE 7 + +static unsigned char buffer[BUFFER_SIZE] = {0}; + +struct button_map +{ + unsigned char index; + unsigned char position; +}; + +enum hat_state +{ + HAT_UP, + HAT_UPRIGHT, + HAT_RIGHT, + HAT_DOWNRIGHT, + HAT_DOWN, + HAT_DOWNLEFT, + HAT_LEFT, + HAT_UPLEFT, + HAT_UNPRESSED, +}; + +static struct button_map buttonMaps[] = +{ + {4, 5}, + {4, 6}, + {4, 7}, + {4, 4}, + {5, 0}, + {5, 1}, + {5, 2}, + {5, 3}, + {5, 4}, + {5, 5}, + {6, 0}, + {5, 6}, + {5, 7}, + {4, 1}, + {4, 3}, + {4, 0}, + {4, 2}, +}; + +int axisToIndexMap[] = +{ + 0, + 1, + -1, + 2, + 3, +}; + +int read_event(int fd, struct js_event *event) +{ + ssize_t bytes; + + bytes = read(fd, event, sizeof(*event)); + + if (bytes == sizeof(*event)) + return 0; + + /* Error, could not read full event. */ + return -1; +} + +int main(int argc, char *argv[]) +{ + char device[] = "/dev/input/js0"; + int js, gd, yes = 1; + struct js_event event; + struct sockaddr_in saddr; + + if(argc != 3) + { + printf("\n Usage: %s \n",argv[0]); + return 1; + } + + device[13] = argv[2][0]; + + js = open(device, O_RDONLY); + + if (js == -1) + perror("Could not open joystick"); + + gd = socket(AF_INET, SOCK_STREAM, 0); + if (gd == -1) + perror("Could not open socket"); + + memset(&saddr, 0, sizeof(saddr)); + + saddr.sin_family = AF_INET; + saddr.sin_port = htons(8080); + + if (inet_pton(AF_INET, argv[1], &saddr.sin_addr) <= 0) + { + printf("\n inet_pton error occured\n"); + return 1; + } + + if (connect(gd, (struct sockaddr *)&saddr, sizeof(saddr)) < 0) + { + printf("\n Error : Connect Failed \n"); + return 1; + } + + if (setsockopt(gd, IPPROTO_TCP, TCP_NODELAY, (void *)&yes, sizeof(yes)) < 0) + { + printf("\n Error : TCP NODELAY Failed \n"); + return 1; + } + + memset(buffer, 0, sizeof(buffer)); + + // Init axis at rest + buffer[0] = 127; + buffer[1] = 127; + buffer[2] = 127; + buffer[3] = 127; + + /* This loop will exit if the controller is unplugged. */ + while (read_event(js, &event) == 0) + { + switch (event.type) + { + case JS_EVENT_BUTTON: + printf("Button %u %s\n", event.number, event.value ? + "pressed" : "released"); + + struct button_map map = buttonMaps[event.number]; + if (event.value) + { + buffer[map.index] |= 1 << map.position; + } + else + { + buffer[map.index] &= ~(1 << map.position); + } + + write(gd, buffer, BUFFER_SIZE); + break; + case JS_EVENT_AXIS: + printf("Axis %zu value %6d\n", event.number, event.value); + + switch (event.number) + { + case 6: + case 7: + { + int buttonNumber, buttonValue = 1; + buttonNumber = (event.number == 6 && event.value < 0) ? 15 : + (event.number == 6 && event.value > 0) ? 16 : + (event.number == 7 && event.value < 0) ? 13 : + (event.number == 7 && event.value > 0) ? 14 : -1; + + if (buttonNumber == -1) + { + int i; + for (i = 0; i < 2; i++) + { + struct button_map map = buttonMaps[(event.number == 7 ? 13 : 15) + i]; + buffer[map.index] &= ~(1 << map.position); + } + } + else + { + struct button_map map = buttonMaps[buttonNumber]; + buffer[map.index] |= 1 << map.position; + } + } + break; + + case 0: + case 1: + case 3: + case 4: + { + int index = axisToIndexMap[event.number]; + buffer[index] = (unsigned char) + (((int)event.value + 32767) >> 8); + printf("Normalized value %3d\n", + (unsigned char)buffer[index]); + } + break; + } + write(gd, buffer, BUFFER_SIZE); + break; + + default: + /* Ignore init events. */ + printf("Weird event: %d\n", event.type); + break; + } + + fflush(stdout); + } + + close(js); + return 0; +} diff --git a/source/ControllerLib/ControllerHelpers.cpp b/source/ControllerLib/ControllerHelpers.cpp index 621f6fbc..14fc1c59 100644 --- a/source/ControllerLib/ControllerHelpers.cpp +++ b/source/ControllerLib/ControllerHelpers.cpp @@ -52,8 +52,20 @@ bool DoesControllerSupport(ControllerType type, ControllerSupport supportType) default: return false; } + case CONTROLLER_NETWORK: + switch (supportType) + { + case SUPPORTS_RUMBLE: + return true; + case SUPPORTS_BLUETOOTH: + return true; + case SUPPORTS_SIXAXIS: + return true; + default: + return false; + } default: return false; } return false; -} \ No newline at end of file +} diff --git a/source/ControllerLib/ControllerTypes.h b/source/ControllerLib/ControllerTypes.h index 36d70949..ea470b43 100644 --- a/source/ControllerLib/ControllerTypes.h +++ b/source/ControllerLib/ControllerTypes.h @@ -9,6 +9,7 @@ enum ControllerType : uint8_t CONTROLLER_XBOXONEW, CONTROLLER_DUALSHOCK3, CONTROLLER_DUALSHOCK4, + CONTROLLER_NETWORK, }; enum VendorIDs : uint16_t diff --git a/source/ControllerLib/Controllers.h b/source/ControllerLib/Controllers.h index 93662e15..73ef0624 100644 --- a/source/ControllerLib/Controllers.h +++ b/source/ControllerLib/Controllers.h @@ -6,3 +6,4 @@ #include "Controllers/XboxOneController.h" #include "Controllers/Dualshock3Controller.h" #include "Controllers/Dualshock4Controller.h" +#include "Controllers/NetworkController.h" diff --git a/source/ControllerLib/Controllers/NetworkController.cpp b/source/ControllerLib/Controllers/NetworkController.cpp new file mode 100644 index 00000000..b8fe4fef --- /dev/null +++ b/source/ControllerLib/Controllers/NetworkController.cpp @@ -0,0 +1,198 @@ +#include "Controllers/NetworkController.h" +#include +#include "controller_handler.h" +#include "log.h" +#include "sys/socket.h" + +extern "C" { + + void registerNetworkController(int fd) { + WriteToLog("Initializing Network controller: 0x%x", + syscon::controllers::Insert(std::make_unique(fd))); + } + + void removeNetworkController(int fd) { + for (auto it = syscon::controllers::Get().begin(); it != syscon::controllers::Get().end(); ++it) + { + auto ptr = (*it)->GetController(); + if (ptr->GetType() == CONTROLLER_NETWORK) + { + auto netController = static_cast(ptr); + if (netController->GetFD() == fd) + { + WriteToLog("Erasing controller"); + syscon::controllers::Get().erase(it--); + WriteToLog("Controller erased!"); + break; + } + } + } + } +} + +static ControllerConfig _networkControllerConfig{}; +static RGBAColor _ledValue{0x00, 0x00, 0x40}; + +NetworkController::NetworkController(int fd) + : IController(nullptr), m_fd{fd} +{ +} + +NetworkController::~NetworkController() +{ + //Exit(); +} + +Result NetworkController::SendInitBytes() +{ + return 0; +} + +Result NetworkController::Initialize() +{ + Result rc; + + rc = OpenInterfaces(); + if (R_FAILED(rc)) + return rc; + + rc = SendInitBytes(); + if (R_FAILED(rc)) + return rc; + return rc; +} +void NetworkController::Exit() +{ + CloseInterfaces(); +} + +Result NetworkController::OpenInterfaces() +{ + return 0; +} +void NetworkController::CloseInterfaces() +{ +} + +Result NetworkController::GetInput() +{ + uint8_t input_bytes[7]; + ssize_t count; + + count = recv(m_fd, input_bytes, sizeof(input_bytes), 0); + if (count == 0) + return 1; + + m_buttonData = *reinterpret_cast(input_bytes); + return 0; +} + +float NetworkController::NormalizeTrigger(uint8_t deadzonePercent, uint8_t value) +{ + uint8_t deadzone = (UINT8_MAX * deadzonePercent) / 100; + //If the given value is below the trigger zone, save the calc and return 0, otherwise adjust the value to the deadzone + return value < deadzone + ? 0 + : static_cast(value - deadzone) / (UINT8_MAX - deadzone); +} + +void NetworkController::NormalizeAxis(uint8_t x, + uint8_t y, + uint8_t deadzonePercent, + float *x_out, + float *y_out) +{ + float x_val = x - 127.0f; + float y_val = 127.0f - y; + // Determine how far the stick is pushed. + //This will never exceed 32767 because if the stick is + //horizontally maxed in one direction, vertically it must be neutral(0) and vice versa + float real_magnitude = std::sqrt(x_val * x_val + y_val * y_val); + float real_deadzone = (127 * deadzonePercent) / 100; + // Check if the controller is outside a circular dead zone. + if (real_magnitude > real_deadzone) + { + // Clip the magnitude at its expected maximum value. + float magnitude = std::min(127.0f, real_magnitude); + // Adjust magnitude relative to the end of the dead zone. + magnitude -= real_deadzone; + // Normalize the magnitude with respect to its expected range giving a + // magnitude value of 0.0 to 1.0 + //ratio = (currentValue / maxValue) / realValue + float ratio = (magnitude / (127 - real_deadzone)) / real_magnitude; + *x_out = x_val * ratio; + *y_out = y_val * ratio; + } + else + { + // If the controller is in the deadzone zero out the magnitude. + *x_out = *y_out = 0.0f; + } +} + +//Pass by value should hopefully be optimized away by RVO +NormalizedButtonData NetworkController::GetNormalizedButtonData() +{ + NormalizedButtonData normalData{}; + + normalData.triggers[0] = NormalizeTrigger(_networkControllerConfig.triggerDeadzonePercent[0], +0); + normalData.triggers[1] = NormalizeTrigger(_networkControllerConfig.triggerDeadzonePercent[1], +0); + + NormalizeAxis(m_buttonData.stick_left_x, m_buttonData.stick_left_y, _networkControllerConfig.stickDeadzonePercent[0], + &normalData.sticks[0].axis_x, &normalData.sticks[0].axis_y); + NormalizeAxis(m_buttonData.stick_right_x, m_buttonData.stick_right_y, _networkControllerConfig.stickDeadzonePercent[1], + &normalData.sticks[1].axis_x, &normalData.sticks[1].axis_y); + + bool buttons[MAX_CONTROLLER_BUTTONS] = { + m_buttonData.triangle, + m_buttonData.circle, + m_buttonData.cross, + m_buttonData.square, + m_buttonData.l3, + m_buttonData.r3, + m_buttonData.l1, + m_buttonData.r1, + m_buttonData.l2, + m_buttonData.r2, + m_buttonData.share, + m_buttonData.options, + m_buttonData.dup, + m_buttonData.dright, + m_buttonData.ddown, + m_buttonData.dleft, + m_buttonData.touchpad_press, + m_buttonData.psbutton, + false, + true, + }; + + for (int i = 0; i != MAX_CONTROLLER_BUTTONS; ++i) + { + ControllerButton button = _networkControllerConfig.buttons[i]; + if (button == NONE) + continue; + + normalData.buttons[(button != DEFAULT ? button - 2 : i)] += buttons[i]; + } + + return normalData; +} + +Result NetworkController::SetRumble(uint8_t strong_magnitude, uint8_t weak_magnitude) +{ + //Not implemented yet + return 9; +} + +void NetworkController::LoadConfig(const ControllerConfig *config, RGBAColor ledValue) +{ + _networkControllerConfig = *config; + _ledValue = ledValue; +} + +ControllerConfig *NetworkController::GetConfig() +{ + return &_networkControllerConfig; +} diff --git a/source/ControllerLib/Controllers/NetworkController.h b/source/ControllerLib/Controllers/NetworkController.h new file mode 100644 index 00000000..2479934e --- /dev/null +++ b/source/ControllerLib/Controllers/NetworkController.h @@ -0,0 +1,78 @@ +#pragma once + +#ifdef __cplusplus +#include "IController.h" + +extern "C" { +#endif + +void registerNetworkController(int fd); +void removeNetworkController(int fd); + +#ifdef __cplusplus +} + +struct NetworkButtonData +{ + uint8_t stick_left_x; + uint8_t stick_left_y; + uint8_t stick_right_x; + uint8_t stick_right_y; + + bool dleft : 1; + bool dup : 1; + bool dright : 1; + bool ddown : 1; + bool square : 1; + bool cross : 1; + bool circle : 1; + bool triangle : 1; + + bool l1 : 1; + bool r1 : 1; + bool l2 : 1; + bool r2 : 1; + bool share : 1; + bool options : 1; + bool l3 : 1; + bool r3 : 1; + + bool psbutton : 1; + bool touchpad_press : 1; + uint8_t timestamp : 6; +}; + +class NetworkController : public IController +{ +private: + int m_fd; + NetworkButtonData m_buttonData{}; + void DeleteNetworkController(); +public: + NetworkController(int fd); + virtual ~NetworkController() override; + + virtual Result Initialize() override; + virtual void Exit() override; + + Result OpenInterfaces(); + void CloseInterfaces(); + + virtual Result GetInput() override; + + virtual NormalizedButtonData GetNormalizedButtonData() override; + + virtual ControllerType GetType() override { return CONTROLLER_NETWORK; } + + float NormalizeTrigger(uint8_t deadzonePercent, uint8_t value); + void NormalizeAxis(uint8_t x, uint8_t y, uint8_t deadzonePercent, float *x_out, float *y_out); + + Result SendInitBytes(); + Result SetRumble(uint8_t strong_magnitude, uint8_t weak_magnitude); + + static void LoadConfig(const ControllerConfig *config, RGBAColor ledValue); + virtual ControllerConfig *GetConfig() override; + + int GetFD() { return m_fd; } +}; +#endif diff --git a/source/Sysmodule/Makefile b/source/Sysmodule/Makefile index 109297ea..1537a9b9 100644 --- a/source/Sysmodule/Makefile +++ b/source/Sysmodule/Makefile @@ -5,7 +5,7 @@ include $(dir $(abspath $(lastword $(MAKEFILE_LIST))))/../libstratosphere/config TARGET := sys-con SOURCES += ../ControllerSwitch ../ControllerLib ../ControllerLib/Controllers ../inih -INCLUDES += ../ControllerSwitch ../ControllerLib ../inih +INCLUDES += ../ControllerSwitch ../ControllerLib ../inih source #--------------------------------------------------------------------------------- # no real need to edit anything past this point unless you need to add additional diff --git a/source/Sysmodule/source/config_handler.cpp b/source/Sysmodule/source/config_handler.cpp index e25e83a3..af4cabd0 100644 --- a/source/Sysmodule/source/config_handler.cpp +++ b/source/Sysmodule/source/config_handler.cpp @@ -234,6 +234,11 @@ namespace syscon::config Dualshock4Controller::LoadConfig(&tempConfig, tempColor); else WriteToLog("Failed to read from dualshock 4 config!"); + + if (R_SUCCEEDED(ReadFromConfig(NETWORKCONFIG))) + NetworkController::LoadConfig(&tempConfig, tempColor); + else + WriteToLog("Failed to read from dualshock 4 config!"); } bool CheckForFileChanges() @@ -332,4 +337,4 @@ namespace syscon::config threadWaitForExit(&g_config_changed_check_thread); threadClose(&g_config_changed_check_thread); } -} // namespace syscon::config \ No newline at end of file +} // namespace syscon::config diff --git a/source/Sysmodule/source/config_handler.h b/source/Sysmodule/source/config_handler.h index 883664ff..039ef474 100644 --- a/source/Sysmodule/source/config_handler.h +++ b/source/Sysmodule/source/config_handler.h @@ -9,6 +9,7 @@ #define XBOXONECONFIG CONFIG_PATH "config_xboxone.ini" #define DUALSHOCK3CONFIG CONFIG_PATH "config_dualshock3.ini" #define DUALSHOCK4CONFIG CONFIG_PATH "config_dualshock4.ini" +#define NETWORKCONFIG CONFIG_PATH "config_network.ini" namespace syscon::config { @@ -27,4 +28,4 @@ namespace syscon::config Result Enable(); void Disable(); -}; // namespace syscon::config \ No newline at end of file +}; // namespace syscon::config diff --git a/source/Sysmodule/source/log.h b/source/Sysmodule/source/log.h index f52eece7..a85f77e4 100644 --- a/source/Sysmodule/source/log.h +++ b/source/Sysmodule/source/log.h @@ -1,6 +1,6 @@ #pragma once -#include "config_handler.h" +#define CONFIG_PATH "/config/sys-con/" #define LOG_PATH CONFIG_PATH "log.txt" #ifdef __cplusplus diff --git a/source/Sysmodule/source/main.cpp b/source/Sysmodule/source/main.cpp index 57b1e820..9d7d230c 100644 --- a/source/Sysmodule/source/main.cpp +++ b/source/Sysmodule/source/main.cpp @@ -8,6 +8,12 @@ #include "psc_module.h" #include "SwitchHDLHandler.h" +extern "C" { + #include "network.h" +} + +#include + #define APP_VERSION "0.6.4" // libnx fake heap initialization @@ -20,7 +26,7 @@ extern "C" // We don't need to reserve memory for fsdev, so don't use it. u32 __nx_fsdev_direntry_cache_size = 1; -#define INNER_HEAP_SIZE 0x40'000 +#define INNER_HEAP_SIZE 0xE7'000 size_t nx_inner_heap_size = INNER_HEAP_SIZE; char nx_inner_heap[INNER_HEAP_SIZE]; @@ -55,6 +61,20 @@ namespace ams extern "C" void __appInit(void) { + static const SocketInitConfig socketInitConfig = { + .bsdsockets_version = 1, + + .tcp_tx_buf_size = 0x800, + .tcp_rx_buf_size = 0x800, + .tcp_tx_buf_max_size = 0x25000, + .tcp_rx_buf_max_size = 0x25000, + + //We don't use UDP, set all UDP buffers to 0 + .udp_tx_buf_size = 0, + .udp_rx_buf_size = 0, + + .sb_efficiency = 1, + }; R_ABORT_UNLESS(smInitialize()); // ams::sm::DoWithSession([] { @@ -65,12 +85,18 @@ extern "C" void __appInit(void) hosversionSet(MAKEHOSVERSION(fw.major, fw.minor, fw.micro)); setsysExit(); + R_ABORT_UNLESS(hiddbgInitialize()); + + R_ABORT_UNLESS(hidInitialize()); + R_ABORT_UNLESS(hidsysInitialize()); + if (hosversionAtLeast(7, 0, 0)) R_ABORT_UNLESS(hiddbgAttachHdlsWorkBuffer(&SwitchHDLHandler::GetHdlsSessionId())); R_ABORT_UNLESS(usbHsInitialize()); R_ABORT_UNLESS(pscmInitialize()); R_ABORT_UNLESS(fsInitialize()); + R_ABORT_UNLESS(socketInitialize(&socketInitConfig)); } // ); smExit(); @@ -80,6 +106,10 @@ extern "C" void __appInit(void) extern "C" void __appExit(void) { + socketExit(); + hidsysExit(); + hidExit(); + pscmExit(); usbHsExit(); hiddbgReleaseHdlsWorkBuffer(SwitchHDLHandler::GetHdlsSessionId()); @@ -90,6 +120,20 @@ extern "C" void __appExit(void) using namespace syscon; +static loop_status_t loop(loop_status_t (*callback)(void)) +{ + loop_status_t status = LOOP_CONTINUE; + + while (true) + { + svcSleepThread(1e+7); + status = callback(); + if (status != LOOP_CONTINUE) + return status; + } + return LOOP_EXIT; +} + int main(int argc, char *argv[]) { WriteToLog("\n\nNew sysmodule session started on version " APP_VERSION); @@ -98,10 +142,29 @@ int main(int argc, char *argv[]) usb::Initialize(); psc::Initialize(); - while (true) + loop_status_t status = LOOP_RESTART; + + WriteToLog("Going to pre_init"); + + network_pre_init(); + + WriteToLog("pre_init completed"); + + while (status == LOOP_RESTART) { - svcSleepThread(1e+8L); + /* initialize ftp subsystem */ + if (network_init() == 0) + { + /* ftp loop */ + status = loop(network_loop); + + /* done with ftp */ + network_exit(); + } + else + status = LOOP_EXIT; } + network_post_exit(); psc::Exit(); usb::Exit(); diff --git a/source/Sysmodule/source/network.c b/source/Sysmodule/source/network.c new file mode 100644 index 00000000..84582f02 --- /dev/null +++ b/source/Sysmodule/source/network.c @@ -0,0 +1,458 @@ +// This file is under the terms of the unlicense (https://github.com/DavidBuchanan314/ftpd/blob/master/LICENSE) + +#define ENABLE_LOGGING 1 +#include "network.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#define lstat stat +#include "log.h" +#include "Controllers/NetworkController.h" + +#define POLL_UNKNOWN (~(POLLIN | POLLPRI | POLLOUT)) + +int LISTEN_PORT; +//#define LISTEN_PORT 5000 +#define CONFIGPATH "/config/sys-con/port_config.ini" + +#include "ini.h" +#include + +int Callback(const char* section, const char* key, const char* value, void* userdata) +{ + (void)userdata; /* this parameter is not used in this example */ + printf(" [%s]\t%s=%s\n", section, key, value); + return 1; +} + +typedef struct network_session_t network_session_t; + +/*! network session */ +struct network_session_t +{ + struct sockaddr_in client_addr; /*!< listen address for PASV connection */ + int cmd_fd; /*!< socket for command connection */ + network_session_t* next; /*!< link to next session */ + network_session_t* prev; /*!< link to prev session */ +}; + +/*! appletHook cookie */ +static AppletHookCookie cookie; + +/*! server listen address */ +static struct sockaddr_in serv_addr; +/*! listen file descriptor */ +static int listenfd = -1; +/*! list of network sessions */ +static network_session_t* sessions = NULL; + +/*! close a socket + * + * @param[in] fd socket to close + * @param[in] connected whether this socket is connected + */ +static void +network_closesocket(int fd, + bool connected) +{ + int rc; + struct sockaddr_in addr; + socklen_t addrlen = sizeof(addr); + struct pollfd pollinfo; + + // WriteToLog("0x%X", socketGetLastBsdResult()); + + if (connected) + { + /* get peer address and print */ + rc = getpeername(fd, (struct sockaddr*)&addr, &addrlen); + if (rc != 0) + { + WriteToLog("getpeername: %d %s", errno, strerror(errno)); + WriteToLog("closing connection to fd=%d", fd); + } + else + WriteToLog("closing connection to %s:%u", + inet_ntoa(addr.sin_addr), ntohs(addr.sin_port)); + + /* shutdown connection */ + rc = shutdown(fd, SHUT_WR); + if (rc != 0) + WriteToLog("shutdown: %d %s", errno, strerror(errno)); + + /* wait for client to close connection */ + pollinfo.fd = fd; + pollinfo.events = POLLIN; + pollinfo.revents = 0; + rc = poll(&pollinfo, 1, 250); + if (rc < 0) + WriteToLog("poll: %d %s", errno, strerror(errno)); + } + + /* set linger to 0 */ + struct linger linger; + linger.l_onoff = 1; + linger.l_linger = 0; + rc = setsockopt(fd, SOL_SOCKET, SO_LINGER, + &linger, sizeof(linger)); + if (rc != 0) + WriteToLog("setsockopt: SO_LINGER %d %s", + errno, strerror(errno)); + + /* close socket */ + rc = close(fd); + if (rc != 0) + WriteToLog("close: %d %s", errno, strerror(errno)); +} + +/*! close command socket on network session + * + * @param[in] session network session + */ +static void +network_session_close_cmd(network_session_t* session) +{ + /* close command socket */ + if (session->cmd_fd >= 0) + network_closesocket(session->cmd_fd, true); + + session->cmd_fd = -1; +} + +/*! destroy network session + * + * @param[in] session network session + * + * @returns the next session in the list + */ +static network_session_t* +network_session_destroy(network_session_t* session) +{ + network_session_t* next = session->next; + + /* close all sockets/files */ + network_session_close_cmd(session); + + /* unlink from sessions list */ + if (session->next) + session->next->prev = session->prev; + if (session == sessions) + sessions = session->next; + else + { + session->prev->next = session->next; + if (session == sessions->prev) + sessions->prev = session->prev; + } + + /* deallocate */ + free(session); + + return next; +} + +/*! allocate new network session + * + * @param[in] listen_fd socket to accept connection from + */ +static int +network_session_new(int listen_fd) +{ + ssize_t rc; + int new_fd; + network_session_t* session; + struct sockaddr_in addr; + socklen_t addrlen = sizeof(addr); + + /* accept connection */ + new_fd = accept(listen_fd, (struct sockaddr*)&addr, &addrlen); + if (new_fd < 0) + { + WriteToLog("accept: %d %s", errno, strerror(errno)); + return -1; + } + + WriteToLog("accepted connection from %s:%u", + inet_ntoa(addr.sin_addr), ntohs(addr.sin_port)); + + registerNetworkController(new_fd); + + /* allocate a new session */ + session = (network_session_t*)calloc(1, sizeof(network_session_t)); + if (session == NULL) + { + WriteToLog("failed to allocate session"); + network_closesocket(new_fd, true); + return -1; + } + + /* initialize session */ + session->cmd_fd = new_fd; + + /* link to the sessions list */ + if (sessions == NULL) + { + sessions = session; + session->prev = session; + } + else + { + sessions->prev->next = session; + session->prev = sessions->prev; + sessions->prev = session; + } + + /* copy socket address to pasv address */ + addrlen = sizeof(session->client_addr); + rc = getsockname(new_fd, (struct sockaddr*)&session->client_addr, &addrlen); + if (rc != 0) + { + WriteToLog("getsockname: %d %s", errno, strerror(errno)); + network_session_destroy(session); + return -1; + } + + return 0; +} + +/*! poll sockets for network session + * + * @param[in] session network session + * + * @returns next session + */ +static network_session_t* +network_session_poll(network_session_t* session) +{ + int rc; + struct pollfd pollinfo[2]; + nfds_t nfds = 1; + + /* the first pollfd is the command socket */ + pollinfo[0].fd = session->cmd_fd; + pollinfo[0].events = POLLIN | POLLPRI; + pollinfo[0].revents = 0; + + /* poll the selected sockets */ + rc = poll(pollinfo, nfds, 0); + if (rc < 0) + { + WriteToLog("poll: %d %s", errno, strerror(errno)); + network_session_close_cmd(session); + } + else if (rc > 0) + { + /* check the command socket */ + if (pollinfo[0].revents != 0) + { + /* handle command */ + if (pollinfo[0].revents & POLL_UNKNOWN) + WriteToLog("cmd_fd: revents=0x%08X", pollinfo[0].revents); + + /* we need to read a new command */ + if (pollinfo[0].revents & (POLLERR | POLLHUP)) + { + WriteToLog("cmd revents=0x%x", pollinfo[0].revents); + network_session_close_cmd(session); + } + + ssize_t count; + uint8_t input_bytes[64]; + count = recv(session->cmd_fd, input_bytes, sizeof(input_bytes), MSG_PEEK); + if (count == 0) + { + removeNetworkController(session->cmd_fd); + network_session_close_cmd(session); + } + } + } + + /* still connected to peer; return next session */ + if (session->cmd_fd >= 0) + return session->next; + + /* disconnected from peer; destroy it and return next session */ + WriteToLog("disconnected from peer"); + + return network_session_destroy(session); +} + +/*! Handle applet events + * + * @param[in] type Event type + * @param[in] closure Callback closure + */ +static void +applet_hook(AppletHookType type, + void* closure) +{ + (void)closure; + (void)type; + /* stubbed for now */ + switch (type) + { + default: + break; + } +} + +void network_pre_init(void) +{ + /* register applet hook */ + appletHook(&cookie, applet_hook, NULL); +} + +static int network_parse_config_line(void *dummy, const char *section, const char *name, const char *value) +{ + if (strncmp(name, "port", 4) == 0) + { + LISTEN_PORT = atoi(value); + return 1; + } + + return 0; +} + +/*! initialize network subsystem */ +int network_init(void) +{ + int rc = 0; + + /* allocate socket to listen for clients */ + listenfd = socket(AF_INET, SOCK_STREAM, 0); + if (listenfd < 0) + { + WriteToLog("socket: %d %s", errno, strerror(errno)); + network_exit(); + return -1; + } + + /* get address to listen on */ + serv_addr.sin_family = AF_INET; + + serv_addr.sin_addr.s_addr = INADDR_ANY; + ini_parse(CONFIGPATH, network_parse_config_line, NULL); + serv_addr.sin_port = htons(LISTEN_PORT); + WriteToLog("server port: %d", LISTEN_PORT); + + /* reuse address */ + { + int yes = 1; + rc = setsockopt(listenfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)); + if (rc != 0) + { + WriteToLog("setsockopt: %d %s", errno, strerror(errno)); + network_exit(); + return -1; + } + } + + /* bind socket to listen address */ + rc = bind(listenfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)); + if (rc != 0) + { + WriteToLog("bind: %d %s", errno, strerror(errno)); + network_exit(); + return -1; + } + + /* listen on socket */ + rc = listen(listenfd, 5); + if (rc != 0) + { + WriteToLog("listen: %d %s", errno, strerror(errno)); + network_exit(); + return -1; + } + + return 0; +} + +/*! deinitialize network subsystem */ +void network_exit(void) +{ + + WriteToLog("exiting network server"); + + /* clean up all sessions */ + while (sessions != NULL) + network_session_destroy(sessions); + + /* stop listening for new clients */ + if (listenfd >= 0) + network_closesocket(listenfd, false); + + /* deinitialize socket driver */ + WriteToLog("Waiting for socketExit()..."); +} + +void network_post_exit(void) +{ +} + +/*! network look + * + * @returns whether to keep looping + */ +loop_status_t +network_loop(void) +{ + int rc; + struct pollfd pollinfo; + network_session_t* session; + + /* we will poll for new client connections */ + pollinfo.fd = listenfd; + pollinfo.events = POLLIN; + pollinfo.revents = 0; + + /* poll for a new client */ + rc = poll(&pollinfo, 1, 0); + if (rc < 0) + { + /* wifi got disabled */ + WriteToLog("poll: FAILED!"); + + if (errno == ENETDOWN) + return LOOP_RESTART; + + WriteToLog("poll: %d %s", errno, strerror(errno)); + return LOOP_EXIT; + } + else if (rc > 0) + { + if (pollinfo.revents & POLLIN) + { + /* we got a new client */ + if (network_session_new(listenfd) != 0) + { + return LOOP_RESTART; + } + } + else + { + WriteToLog("listenfd: revents=0x%08X", pollinfo.revents); + } + } + + /* poll each session */ + session = sessions; + while (session != NULL) + session = network_session_poll(session); + + return LOOP_CONTINUE; +} + diff --git a/source/Sysmodule/source/network.h b/source/Sysmodule/source/network.h new file mode 100644 index 00000000..341708fe --- /dev/null +++ b/source/Sysmodule/source/network.h @@ -0,0 +1,17 @@ +// This file is under the terms of the unlicense (https://github.com/DavidBuchanan314/ftpd/blob/master/LICENSE) + +#pragma once + +/*! Loop status */ +typedef enum +{ + LOOP_CONTINUE, /*!< Continue looping */ + LOOP_RESTART, /*!< Reinitialize */ + LOOP_EXIT, /*!< Terminate looping */ +} loop_status_t; + +void network_pre_init(void); +int network_init(void); +loop_status_t network_loop(void); +void network_exit(void); +void network_post_exit(void);