diff --git a/src/ConfigurableFirmata.cpp b/src/ConfigurableFirmata.cpp index 834af63..70db5ca 100644 --- a/src/ConfigurableFirmata.cpp +++ b/src/ConfigurableFirmata.cpp @@ -69,6 +69,10 @@ FirmataClass::FirmataClass() firmwareVersionMajor = 0; firmwareVersionName = ""; blinkVersionDisabled = false; +#ifdef LARGE_MEM_DEVICE + readCachePos = 0; + writeCachePos = 0; +#endif systemReset(); } @@ -95,6 +99,7 @@ void FirmataClass::begin(void) void FirmataClass::begin(long speed) { Serial.begin(speed); + Serial.setTimeout(0); FirmataStream = &Serial; outputIsConsole = true; blinkVersion(); @@ -255,11 +260,25 @@ void FirmataClass::processSysexMessage(void) */ void FirmataClass::processInput(void) { - int inputData = FirmataStream->read(); // this is 'int' to handle -1 when no data - if (inputData != -1) - { - parse(inputData); - } +#ifdef LARGE_MEM_DEVICE + if (writeCachePos == readCachePos) + { + writeCachePos = readCachePos = 0; + writeCachePos = FirmataStream->readBytes(readCache, MAX_DATA_BYTES); + } + while (writeCachePos > readCachePos) + { + int inputData = readCache[readCachePos]; + readCachePos++; + parse(inputData); + } +#else + int inputData = FirmataStream->read(); + if (inputData != -1) + { + parse(inputData); + } +#endif } void FirmataClass::resetParser() @@ -268,6 +287,9 @@ void FirmataClass::resetParser() sysexBytesRead = 0; waitForData = 0; executeMultiByteCommand = 0; +#ifdef LARGE_MEM_DEVICE + writeCachePos = readCachePos = 0; +#endif } /** @@ -280,6 +302,8 @@ void FirmataClass::parse(byte inputData) // TODO make sure it handles -1 properly + // Firmata.sendStringf(F("Received byte 0x%x"), (int)inputData); + if (inputData == SYSTEM_RESET) { // A system reset shall always be done, regardless of the state of the parser. @@ -503,8 +527,11 @@ void FirmataClass::sendStringf(const FlashString* flashString, ...) #else const int maxSize = 255; #endif - // 32 bit boards. Note that sizeOfArgs may not be correct here (since all arguments are 32-bit padded) int len = strlen_P((const char*)flashString); + if (len >= maxSize) + { + return; + } va_list va; va_start (va, flashString); char bytesInput[maxSize]; @@ -593,6 +620,11 @@ void FirmataClass::write(byte c) FirmataStream->write(c); } +size_t FirmataClass::write(byte* buf, size_t length) +{ + return FirmataStream->write(buf, length); +} + /** * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE, diff --git a/src/ConfigurableFirmata.h b/src/ConfigurableFirmata.h index 079c3aa..e0175df 100644 --- a/src/ConfigurableFirmata.h +++ b/src/ConfigurableFirmata.h @@ -32,10 +32,10 @@ * Query using the REPORT_FIRMWARE message. */ #define FIRMATA_FIRMWARE_MAJOR_VERSION 2 // for non-compatible changes -#define FIRMATA_FIRMWARE_MINOR_VERSION 11 // for backwards compatible changes +#define FIRMATA_FIRMWARE_MINOR_VERSION 12 // for backwards compatible changes #define FIRMATA_FIRMWARE_BUGFIX_VERSION 0 // for bugfix releases -#ifdef ESP32 +#ifdef LARGE_MEM_DEVICE #define MAX_DATA_BYTES 255 // The ESP32 has enough RAM so we can reduce the number of packets, but the value must not exceed 2^8 - 1, because many methods use byte-indexing only #else #define MAX_DATA_BYTES 64 // max number of data bytes in incoming messages @@ -168,6 +168,9 @@ class FirmataClass void sendString(byte command, const char *string); void sendSysex(byte command, byte bytec, byte *bytev); void write(byte c); + + size_t write(byte* buf, size_t length); + void sendPackedUInt14(uint16_t value); void sendPackedUInt32(uint32_t value); void sendPackedUInt64(uint64_t value); @@ -237,6 +240,11 @@ class FirmataClass void processSysexMessage(void); void systemReset(void); void strobeBlinkPin(byte pin, int count, int onInterval, int offInterval); +#ifdef LARGE_MEM_DEVICE + byte readCache[MAX_DATA_BYTES]; + int readCachePos; + int writeCachePos; +#endif }; extern FirmataClass Firmata; diff --git a/src/Encoder7Bit.h b/src/Encoder7Bit.h index 51e5d8a..d6de63b 100644 --- a/src/Encoder7Bit.h +++ b/src/Encoder7Bit.h @@ -28,13 +28,11 @@ class Encoder7BitClass void startBinaryWrite(); void endBinaryWrite(); void writeBinary(byte data); - void readBinary(int outBytes, byte *inData, byte *outData); + static void readBinary(int outBytes, byte *inData, byte *outData); private: byte previous; int shift; }; -extern Encoder7BitClass Encoder7Bit; - #endif diff --git a/src/FirmataScheduler.cpp b/src/FirmataScheduler.cpp index d4a23f6..d7858a9 100644 --- a/src/FirmataScheduler.cpp +++ b/src/FirmataScheduler.cpp @@ -64,7 +64,7 @@ boolean FirmataScheduler::handleSysex(byte command, byte argc, byte* argv) { if (argc > 2) { int len = num7BitOutbytes(argc - 2); - Encoder7Bit.readBinary(len, argv + 2, argv + 2); //decode inplace + Encoder7BitClass::readBinary(len, argv + 2, argv + 2); //decode inplace addToTask(argv[1], len, argv + 2); //addToTask copies data... } break; @@ -73,7 +73,7 @@ boolean FirmataScheduler::handleSysex(byte command, byte argc, byte* argv) { if (argc == 6) { argv++; - Encoder7Bit.readBinary(4, argv, argv); //decode inplace + Encoder7BitClass::readBinary(4, argv, argv); //decode inplace delayTask(*(long*)((byte*)argv)); } break; @@ -81,7 +81,7 @@ boolean FirmataScheduler::handleSysex(byte command, byte argc, byte* argv) case SCHEDULE_FIRMATA_TASK: { if (argc == 7) { //one byte taskid, 5 bytes to encode 4 bytes of long - Encoder7Bit.readBinary(4, argv + 2, argv + 2); //decode inplace + Encoder7BitClass::readBinary(4, argv + 2, argv + 2); //decode inplace schedule(argv[1], *(long*)((byte*)argv + 2)); //argv[1] | argv[2]<<8 | argv[3]<<16 | argv[4]<<24 } break; @@ -208,24 +208,26 @@ void FirmataScheduler::queryTask(byte id) reportTask(id, task, false); } -void FirmataScheduler::reportTask(byte id, firmata_task *task, boolean error) +void FirmataScheduler::reportTask(byte id, firmata_task* task, boolean error) { - Firmata.write(START_SYSEX); - Firmata.write(SCHEDULER_DATA); - if (error) { - Firmata.write(ERROR_TASK_REPLY); - } else { - Firmata.write(QUERY_TASK_REPLY); - } - Firmata.write(id); - if (task) { - Encoder7Bit.startBinaryWrite(); - for (unsigned int i = 3; i < firmata_task_len(task); i++) { - Encoder7Bit.writeBinary(((byte *)task)[i]); //don't write first 3 bytes (firmata_task*, byte); makes use of AVR byteorder (LSB first) + Encoder7BitClass encoder; + Firmata.write(START_SYSEX); + Firmata.write(SCHEDULER_DATA); + if (error) { + Firmata.write(ERROR_TASK_REPLY); } - Encoder7Bit.endBinaryWrite(); - } - Firmata.write(END_SYSEX); + else { + Firmata.write(QUERY_TASK_REPLY); + } + Firmata.write(id); + if (task) { + encoder.startBinaryWrite(); + for (unsigned int i = 3; i < firmata_task_len(task); i++) { + encoder.writeBinary(((byte*)task)[i]); //don't write first 3 bytes (firmata_task*, byte); makes use of AVR byteorder (LSB first) + } + encoder.endBinaryWrite(); + } + Firmata.write(END_SYSEX); }; void FirmataScheduler::report(bool elapsed) diff --git a/src/OneWireFirmata.cpp b/src/OneWireFirmata.cpp index bf647c5..01619eb 100644 --- a/src/OneWireFirmata.cpp +++ b/src/OneWireFirmata.cpp @@ -54,6 +54,7 @@ void OneWireFirmata::oneWireConfig(byte pin, boolean power) boolean OneWireFirmata::handleSysex(byte command, byte argc, byte* argv) { if (command == ONEWIRE_DATA) { + Encoder7BitClass encoder; if (argc > 1) { byte subcommand = argv[0]; byte pin = argv[1]; @@ -70,14 +71,14 @@ boolean OneWireFirmata::handleSysex(byte command, byte argc, byte* argv) boolean isAlarmSearch = (subcommand == ONEWIRE_SEARCH_ALARMS_REQUEST); Firmata.write(isAlarmSearch ? (byte)ONEWIRE_SEARCH_ALARMS_REPLY : (byte)ONEWIRE_SEARCH_REPLY); Firmata.write(pin); - Encoder7Bit.startBinaryWrite(); + encoder.startBinaryWrite(); byte addrArray[8]; while (isAlarmSearch ? device->search(addrArray, false) : device->search(addrArray)) { for (int i = 0; i < 8; i++) { - Encoder7Bit.writeBinary(addrArray[i]); + encoder.writeBinary(addrArray[i]); } } - Encoder7Bit.endBinaryWrite(); + encoder.endBinaryWrite(); Firmata.write(END_SYSEX); break; } @@ -104,7 +105,7 @@ boolean OneWireFirmata::handleSysex(byte command, byte argc, byte* argv) int numReadBytes = 0; int correlationId; argv += 2; - Encoder7Bit.readBinary(numBytes, argv, argv); //decode inplace + Encoder7BitClass::readBinary(numBytes, argv, argv); //decode inplace if (subcommand & ONEWIRE_SELECT_REQUEST_BIT) { if (numBytes < 8) break; @@ -140,13 +141,13 @@ boolean OneWireFirmata::handleSysex(byte command, byte argc, byte* argv) Firmata.write(ONEWIRE_DATA); Firmata.write(ONEWIRE_READ_REPLY); Firmata.write(pin); - Encoder7Bit.startBinaryWrite(); - Encoder7Bit.writeBinary(correlationId & 0xFF); - Encoder7Bit.writeBinary((correlationId >> 8) & 0xFF); + encoder.startBinaryWrite(); + encoder.writeBinary(correlationId & 0xFF); + encoder.writeBinary((correlationId >> 8) & 0xFF); for (int i = 0; i < numReadBytes; i++) { - Encoder7Bit.writeBinary(device->read()); + encoder.writeBinary(device->read()); } - Encoder7Bit.endBinaryWrite(); + encoder.endBinaryWrite(); Firmata.write(END_SYSEX); } } diff --git a/src/SpiFirmata.h b/src/SpiFirmata.h index 63d53c8..f91ba59 100644 --- a/src/SpiFirmata.h +++ b/src/SpiFirmata.h @@ -18,6 +18,7 @@ #include #include "FirmataFeature.h" #include "FirmataReporting.h" +#include "Encoder7Bit.h" #define SPI_BEGIN 0x00 // Initialize the SPI bus for the given channel #define SPI_DEVICE_CONFIG 0x01 @@ -26,6 +27,11 @@ #define SPI_READ 0x04 #define SPI_REPLY 0x05 #define SPI_END 0x06 +#define SPI_WRITE_ACK 0x07 + +#define SPI_SEND_NO_REPLY 0 +#define SPI_SEND_NORMAL_REPLY 1 +#define SPI_SEND_EMPTY_REPLY 2 #define SPI_MAX_DEVICES 8 #define MAX_SPI_BUF_SIZE 32 @@ -33,9 +39,10 @@ /* Spi data */ struct spi_device_config { byte deviceIdChannel; - byte dataModeBitOrder; byte csPinOptions; byte csPin; + boolean packedData; + SPISettings spi_settings; boolean used; }; @@ -54,7 +61,7 @@ class SpiFirmata: public FirmataFeature boolean handleSpiBegin(byte argc, byte *argv); boolean handleSpiConfig(byte argc, byte *argv); boolean enableSpiPins(); - void handleSpiTransfer(byte argc, byte *argv, boolean dummySend, boolean sendReply); + void handleSpiTransfer(byte argc, byte *argv, boolean dummySend, int sendReply); void disableSpiPins(); int getConfigIndexForDevice(byte deviceIdChannel); @@ -70,6 +77,7 @@ SpiFirmata::SpiFirmata() config[i].deviceIdChannel = -1; config[i].csPin = -1; config[i].used = false; + config[i].packedData = false; } } @@ -125,13 +133,16 @@ void SpiFirmata::handleSpiRequest(byte command, byte argc, byte *argv) disableSpiPins(); break; case SPI_READ: - handleSpiTransfer(argc, argv, true, true); + handleSpiTransfer(argc, argv, true, SPI_SEND_NORMAL_REPLY); break; case SPI_WRITE: - handleSpiTransfer(argc, argv, false, false); + handleSpiTransfer(argc, argv, false, SPI_SEND_NO_REPLY); break; + case SPI_WRITE_ACK: + handleSpiTransfer(argc, argv, false, SPI_SEND_EMPTY_REPLY); + break; case SPI_TRANSFER: - handleSpiTransfer(argc, argv, false, true); + handleSpiTransfer(argc, argv, false, SPI_SEND_NORMAL_REPLY); break; default: Firmata.sendString(F("Unknown SPI command: "), command); @@ -139,14 +150,14 @@ void SpiFirmata::handleSpiRequest(byte command, byte argc, byte *argv) } } -void SpiFirmata::handleSpiTransfer(byte argc, byte *argv, boolean dummySend, boolean sendReply) +void SpiFirmata::handleSpiTransfer(byte argc, byte *argv, boolean dummySend, int sendReply) { if (!isSpiEnabled) { Firmata.sendString(F("SPI not enabled.")); return; } - byte data[MAX_SPI_BUF_SIZE]; + byte data[MAX_DATA_BYTES]; // Make sure we have enough data. No data bytes is only allowed in read-only mode if (dummySend ? argc < 4 : argc < 6) { Firmata.sendString(F("Not enough data in SPI message")); @@ -159,94 +170,155 @@ void SpiFirmata::handleSpiTransfer(byte argc, byte *argv, boolean dummySend, boo return; } - int j = 0; + int bytesToSend = 0; // In read-only mode set buffer to 0, otherwise fill buffer from request if (dummySend) { - for (byte i = 0; i < argv[3]; i += 1) { - data[j++] = 0; - } - } else { - for (byte i = 4; i < argc; i += 2) { - data[j++] = argv[i] + (argv[i + 1] << 7); - } + memset(data, 0, argv[3]); + bytesToSend = argv[3]; + } else + { + if (config[index].packedData) + { + bytesToSend = num7BitOutbytes(argc - 4); + if (bytesToSend > MAX_DATA_BYTES) + { + Firmata.sendString(F("SPI_TRANSFER: Send buffer not large enough")); + return; + } + Encoder7BitClass::readBinary(bytesToSend, argv + 4, data); + } + else + { + for (byte i = 4; i < argc; i += 2) + { + data[bytesToSend++] = argv[i] + (argv[i + 1] << 7); + } + } + } + + // If we just need to send a confirmation message, we can do so now, as we have already copied the input buffer (and we're running synchronously, anyway) + if (sendReply == SPI_SEND_EMPTY_REPLY) + { + byte reply[7]; + reply[0] = START_SYSEX; + reply[1] = SPI_DATA; + reply[2] = SPI_REPLY; + reply[3] = argv[0]; + reply[4] = argv[1]; + reply[5] = 0; + reply[6] = END_SYSEX; + Firmata.write(reply, 7); } - digitalWrite(config[index].csPin, LOW); - SPI.transfer(data, j); + + if (config[index].csPin != -1) + { + digitalWrite(config[index].csPin, LOW); + } + + SPI.beginTransaction(config[index].spi_settings); + SPI.transfer(data, bytesToSend); + SPI.endTransaction(); if (argv[2] != 0) { // Default is deselect, so only skip this if the value is 0 digitalWrite(config[index].csPin, HIGH); } - if (sendReply) { + if (sendReply == SPI_SEND_NORMAL_REPLY) { Firmata.startSysex(); Firmata.write(SPI_DATA); Firmata.write(SPI_REPLY); Firmata.write(argv[0]); Firmata.write(argv[1]); - Firmata.write(j); - for (int i = 0; i < j; i++) - { - Firmata.sendValueAsTwo7bitBytes(data[i]); - } + Firmata.write((byte)bytesToSend); // the bytes received is always equal to the bytes sent for SPI + if (config[index].packedData) + { + Encoder7BitClass encoder; + encoder.startBinaryWrite(); + for (int i = 0; i < bytesToSend; i++) + { + encoder.writeBinary(data[i]); + } + encoder.endBinaryWrite(); + } + else + { + for (int i = 0; i < bytesToSend; i++) + { + Firmata.sendValueAsTwo7bitBytes(data[i]); + } + } Firmata.endSysex(); } } -boolean SpiFirmata::handleSpiConfig(byte argc, byte *argv) +boolean SpiFirmata::handleSpiConfig(byte argc, byte* argv) { if (argc < 10) { Firmata.sendString(F("Not enough data in SPI_DEVICE_CONFIG message")); return false; } - - int index = -1; // the index where the new device will be added - for (int i = 0; i < SPI_MAX_DEVICES; i++) { - if (config[i].deviceIdChannel == argv[0]) { - index = i; // This device exists already + + int index = -1; // the index where the new device will be added + for (int i = 0; i < SPI_MAX_DEVICES; i++) { + if (config[i].deviceIdChannel == argv[0]) { + index = i; // This device exists already + } } - } - - if (index == -1) - { - for (int i = 0; i < SPI_MAX_DEVICES; i++) { - if (config[i].used == false) { - index = i; + + if (index == -1) + { + for (int i = 0; i < SPI_MAX_DEVICES; i++) { + if (config[i].used == false) { + index = i; + } } - } - } - if (index == -1) - { - Firmata.sendString(F("SPI_DEVICE_CONFIG: Max number of devices exceeded")); - return false; - } - - // Check word size. Must be 0 (default) or 8. - if (argv[7] != 0 && argv[7] != 8) - { - Firmata.sendString(F("SPI_DEVICE_CONFIG: Only 8 bit words supported")); - return false; - } - - byte deviceIdChannel = argv[0]; - if ((deviceIdChannel & 0x3) != 0) - { - Firmata.sendString(F("SPI_DEVICE_CONFIG: Only channel 0 supported: "), deviceIdChannel); - return false; - } - - if (argv[1] != 1) - { - Firmata.sendString(F("Only BitOrder = 1 and dataMode = 0 supported")); - return false; - } - - config[index].deviceIdChannel = deviceIdChannel; - config[index].dataModeBitOrder = argv[1]; - // Max speed ignored for now - config[index].csPinOptions = argv[8]; - config[index].csPin = argv[9]; - config[index].used = true; - return true; + } + if (index == -1) + { + Firmata.sendString(F("SPI_DEVICE_CONFIG: Max number of devices exceeded")); + return false; + } + + // Check word size. Must be 0 (default) or 8. + if (argv[7] != 0 && argv[7] != 8) + { + Firmata.sendString(F("SPI_DEVICE_CONFIG: Only 8 bit words supported")); + return false; + } + + byte deviceIdChannel = argv[0]; + if ((deviceIdChannel & 0x3) != 0) + { + Firmata.sendString(F("SPI_DEVICE_CONFIG: Only channel 0 supported: "), deviceIdChannel); + return false; + } + + spi_device_config& cfg = config[index]; + cfg.deviceIdChannel = deviceIdChannel; + int bitOrder = argv[1] & 0x1; + int dataMode = (argv[1] >> 1) & 0x3; + cfg.packedData = (argv[1] >> 3) & 0x1; // this is only supported for the default word length, but we're not supporting anything else anyway + uint32_t speed = Firmata.decodePackedUInt32(argv + 2); + cfg.csPinOptions = argv[8]; + cfg.csPin = argv[9]; + cfg.used = true; + if (speed == 0) + { + speed = 5000000; + } + cfg.spi_settings = SPISettings(speed, bitOrder == 1 ? MSBFIRST : LSBFIRST, dataMode); + if ((cfg.csPinOptions & 0x1) == 0) + { + cfg.csPin = -1; + } + else + { + Firmata.setPinMode(cfg.csPin, PIN_MODE_OUTPUT); + pinMode(cfg.csPin, OUTPUT); + } + + Firmata.sendStringf(F("New SPI device %d allocated with index %d and CS %d, clock speed %d Hz"), deviceIdChannel, index, config[index].csPin, speed); + return true; } int SpiFirmata::getConfigIndexForDevice(byte deviceIdChannel) @@ -267,9 +339,15 @@ boolean SpiFirmata::handleSpiBegin(byte argc, byte *argv) Firmata.sendString(F("SPI_BEGIN: Only channel 0 supported")); return false; } - enableSpiPins(); - SPI.begin(); + if (!enableSpiPins()) + { + Firmata.sendString(F("Error enabling SPI mode")); + return false; + } + + SPI.begin(); + Firmata.sendString(F("SPI.begin()")); } return isSpiEnabled; } @@ -306,6 +384,7 @@ void SpiFirmata::disableSpiPins() { isSpiEnabled = false; SPI.end(); + Firmata.sendString(F("SPI.end()")); } void SpiFirmata::reset() diff --git a/src/utility/Boards.h b/src/utility/Boards.h index a6e6717..a3d2ec1 100644 --- a/src/utility/Boards.h +++ b/src/utility/Boards.h @@ -243,6 +243,7 @@ writePort(port, value, bitmask): Write an 8 bit port. #define PIN_TO_ANALOG(p) ((p) - 54) #define PIN_TO_PWM(p) PIN_TO_DIGITAL(p) #define PIN_TO_SERVO(p) ((p) - 2) +#define LARGE_MEM_DEVICE 96 // Arduino/Genuino MKR1000 @@ -709,6 +710,7 @@ writePort(port, value, bitmask): Write an 8 bit port. #define PIN_TO_SERVO(p) (p) #define DEFAULT_PWM_RESOLUTION 13 #define DEFAULT_ADC_RESOLUTION 12 +#define LARGE_MEM_DEVICE 320 // Defined in AnalogOutputFirmata.cpp void analogWrite(uint8_t channel, uint32_t value);