void KineticController_HandleUnexpectedResponse(void *msg,
int64_t seq_id,
void *bus_udata,
void *socket_udata)
{
KineticResponse * response = msg;
KineticSession* session = socket_udata;
bool connetionInfoReceived = false;
char const * statusTag = "[PDU RX STATUS]";
char const * unexpectedTag = "[PDU RX UNEXPECTED]";
char const * logTag = unexpectedTag;
int logAtLevel, protoLogAtLevel;
(void)bus_udata;
// Handle unsolicited status PDUs
if (response->proto->authtype == COM__SEAGATE__KINETIC__PROTO__MESSAGE__AUTH_TYPE__UNSOLICITEDSTATUS) {
int64_t connectionID = KineticResponse_GetConnectionID(response);
if (connectionID != 0)
{
// Store connectionID from unsolicited status message in the session for future requests
KineticSession_SetConnectionID(session, connectionID);
LOGF2("Extracted connection ID from unsolicited status PDU (id=%lld)", connectionID);
connetionInfoReceived = true;
logTag = statusTag;
logAtLevel = 2;
protoLogAtLevel = 3;
}
else {
LOG0("WARNING: Unsolicited status received. Connection being terminated by remote!");
logTag = statusTag;
logAtLevel = 0;
protoLogAtLevel = 0;
KineticStatus status = KineticResponse_GetStatus(response);
KineticSession_SetTerminationStatus(session, status);
}
}
else {
LOG0("WARNING: Received unexpected response!");
logTag = unexpectedTag;
logAtLevel = 0;
protoLogAtLevel = 0;
}
KineticLogger_LogPrintf(logAtLevel, "%s pdu: %p, session: %p, bus: %p, "
"fd: %6d, seq: %8lld, protoLen: %8u, valueLen: %8u",
logTag,
(void*)response, (void*)session,
(void*)session->messageBus,
session->socket, (long long)seq_id,
KineticResponse_GetProtobufLength(response),
KineticResponse_GetValueLength(response));
KineticLogger_LogProtobuf(protoLogAtLevel, response->proto);
KineticAllocator_FreeKineticResponse(response);
if (connetionInfoReceived) {
KineticResourceWaiter_SetAvailable(&session->connectionReady);
}
}
bool Copy_Com__Seagate__Kinetic__Proto__Command__Range_to_ByteBufferArray(Com__Seagate__Kinetic__Proto__Command__Range* keyRange, ByteBufferArray* keys)
{
bool bufferOverflow = false;
LOGF2("Copying: keyRange=0x%0llX, keys=0x%0llX, max_keys=%lld", keyRange, keys->buffers, keys->count);
if (keyRange != NULL && keys->count > 0 && keys != NULL) {
for (size_t i = 0; i < MIN((size_t)keys->count, (size_t)keyRange->n_keys); i++) {
ByteBuffer_Reset(&keys->buffers[i]);
if (ByteBuffer_Append(&keys->buffers[i], keyRange->keys[i].data, keyRange->keys[i].len) == NULL) {
LOGF2("WANRNING: Buffer overrun for keys[%zd]", i);
bufferOverflow = true;
}
}
keys->used = keyRange->n_keys;
}
return !bufferOverflow;
}
void PlayDrillCommandClass::updateFrom(const char* buf)
{
LOGF("Updating playDrill command");
if (!PauseCommandClass::IsPaused()) {
LOGF2(ERR_PREFIX, "Expected drill to be paused");
return;
}
if (strcmp(getBuffer(), buf)== 0) {
LOGF("No changes in update command");
return;
}
clear();
setBuffer(buf);
parse();
}
/*virtual*/ void SerialComClass::sendBuf(const char* sendBuf, size_t size)
{
m_ble.print("AT+BLEUARTTX=");
// write chunks of max 20 byte
const char* pEndBuf= sendBuf + size;
for (const char* pBuf= sendBuf; pBuf < pEndBuf; pBuf += kTxMaxCharacters)
{
m_ble.write(pBuf, min(pEndBuf-pBuf, kTxMaxCharacters));
}
m_ble.println();
// check response status
if (! m_ble.waitForOK() ) {
LOGF2(ERR_PREFIX, F("Failed to send"));
}
}
void BallParamCommandClass::execute()
{
parse();
LOGF7("Exec BallParam(speed=", m_inSpeed.toString(), F(", vAngle="), m_inVertAngle.toString(), F(", hAngle="), m_inHorizAngle.toString(), ("))") );
MotorHelperClass::FeedingData data;
data.initialize(g_motorHelper.isBallInSwitch(), &g_motorHelper);
data.lastFeedTimeMs= millis();
g_motorHelper.setBallParams(m_inSpeed, m_inHorizAngle, m_inVertAngle);
g_TaskQueue.waitAndExecuteAllTasks();
g_motorHelper.feedBall(data);
if (data.result != MotorHelperClass::FR_BALL_FED)
{
LOGF2("Feeder returned Error code:", data.result);
}
}
void KineticController_HandleResult(bus_msg_result_t *res, void *udata)
{
KineticOperation* op = udata;
KINETIC_ASSERT(op);
KINETIC_ASSERT(op->session);
KineticStatus status = bus_to_kinetic_status(res->status);
if (status == KINETIC_STATUS_SUCCESS) {
KineticResponse * response = res->u.response.opaque_msg;
status = KineticResponse_GetStatus(response);
LOGF2("[PDU RX] pdu: %p, session: %p, bus: %p, "
"fd: %6d, seq: %8lld, protoLen: %8u, valueLen: %8u, op: %p, status: %s",
(void*)response,
(void*)op->session, (void*)op->session->messageBus,
op->session->socket, response->command->header->acksequence,
KineticResponse_GetProtobufLength(response),
KineticResponse_GetValueLength(response),
(void*)op,
Kinetic_GetStatusDescription(status));
KineticLogger_LogHeader(3, &response->header);
KineticLogger_LogProtobuf(3, response->proto);
if (op->response == NULL) {
op->response = response;
}
} else {
LOGF0("Error receiving response, got message bus error: %s", bus_error_string(res->status));
if (res->status == BUS_SEND_RX_TIMEOUT) {
LOG0("RX_TIMEOUT");
}
}
// Call operation-specific callback, if configured
if (op->opCallback != NULL) {
status = op->opCallback(op, status);
}
KineticOperation_Complete(op, status);
}
KineticStatus KineticBuilder_BuildPut(KineticOperation* const op,
KineticEntry* const entry)
{
KineticOperation_ValidateOperation(op);
if (entry->value.bytesUsed > KINETIC_OBJ_SIZE) {
LOGF2("Value exceeds maximum size. Packed size is: %d, Max size is: %d", entry->value.bytesUsed, KINETIC_OBJ_SIZE);
return KINETIC_STATUS_BUFFER_OVERRUN;
}
op->request->message.command.header->messagetype = COM__SEAGATE__KINETIC__PROTO__COMMAND__MESSAGE_TYPE__PUT;
op->request->message.command.header->has_messagetype = true;
op->entry = entry;
KineticMessage_ConfigureKeyValue(&op->request->message, op->entry);
op->value.data = op->entry->value.array.data;
op->value.len = op->entry->value.bytesUsed;
op->opCallback = &KineticCallbacks_Put;
return KINETIC_STATUS_SUCCESS;
}
/*virtual*/ void SerialComClass::setup(funcT fnUartCallback)
{
LOGF("BT_ADAFRUIT_BLE setup");
m_fnUartCallback= fnUartCallback;
m_ble.setMode(BLUEFRUIT_MODE_COMMAND);
if ( !m_ble.begin() )
{
LOGF2(ERR_PREFIX, F("Couldn't find Bluefruit, make sure CoMmanD mode"));
}
/* Disable command echo from Bluefruit */
m_ble.echo(false);
m_ble.sendCommandCheckOK("AT+HWModeLED=MODE");
// set all callback to enable all kind of events. Functions are not used. S. update function in this class instead.
m_ble.setConnectCallback( dummyCallback2 );
m_ble.setDisconnectCallback( dummyCallback2 );
m_ble.setBleUartRxCallback( dummyCallback );
}
KineticStatus KineticSocket_Write(int socket, ByteBuffer* src)
{
LOGF3("Writing %zu bytes to socket...", src->bytesUsed);
for (unsigned int bytesSent = 0; bytesSent < src->bytesUsed;) {
int bytesRemaining = src->bytesUsed - bytesSent;
int status = write(socket, &src->array.data[bytesSent], bytesRemaining);
if (status == -1 &&
((errno == EINTR) || (errno == EAGAIN) || (errno == EWOULDBLOCK))) {
LOG2("Write interrupted. retrying...");
continue;
}
else if (status <= 0) {
LOGF0("Failed to write to socket! status=%d, errno=%d\n", status, errno);
return KINETIC_STATUS_SOCKET_ERROR;
}
else {
bytesSent += status;
LOGF2("Wrote %d bytes (%d of %zu sent)", status, bytesSent, src->bytesUsed);
}
}
LOG3("Socket write completed successfully");
return KINETIC_STATUS_SUCCESS;
}
int KineticSocket_Connect(const char* host, int port, bool nonBlocking)
{
char port_str[32];
struct addrinfo hints;
struct addrinfo* ai_result = NULL;
struct addrinfo* ai = NULL;
socket99_result result;
// Setup server address info
socket99_config cfg = {
.host = (char*)host,
.port = port,
.nonblocking = nonBlocking,
};
sprintf(port_str, "%d", port);
// Open socket
LOGF0("Connecting to %s:%d", host, port);
if (!socket99_open(&cfg, &result)) {
LOGF0("Failed to open socket connection with host: status %d, errno %d",
result.status, result.saved_errno);
return KINETIC_SOCKET_DESCRIPTOR_INVALID;
}
// Configure the socket
socket99_set_hints(&cfg, &hints);
if (getaddrinfo(cfg.host, port_str, &hints, &ai_result) != 0) {
LOGF0("Failed to get socket address info: errno %d", errno);
close(result.fd);
return KINETIC_SOCKET_DESCRIPTOR_INVALID;
}
for (ai = ai_result; ai != NULL; ai = ai->ai_next) {
int setsockopt_result;
int buffer_size = KINETIC_OBJ_SIZE;
#if defined(SO_NOSIGPIPE) && !defined(__APPLE__)
// On BSD-like systems we can set SO_NOSIGPIPE on the socket to
// prevent it from sending a PIPE signal and bringing down the whole
// application if the server closes the socket forcibly
int enable = 1;
setsockopt_result = setsockopt(result.fd,
SOL_SOCKET, SO_NOSIGPIPE,
&enable, sizeof(enable));
// Allow ENOTSOCK because it allows tests to use pipes instead of
// real sockets
if (setsockopt_result != 0 && setsockopt_result != ENOTSOCK) {
LOG0("Failed to set SO_NOSIGPIPE on socket");
continue;
}
#endif
// Increase send buffer to KINETIC_OBJ_SIZE
// Note: OS allocates 2x this value for its overhead
setsockopt_result = setsockopt(result.fd,
SOL_SOCKET, SO_SNDBUF,
&buffer_size, sizeof(buffer_size));
if (setsockopt_result == -1) {
LOG0("Error setting socket send buffer size");
continue;
}
// Increase receive buffer to KINETIC_OBJ_SIZE
// Note: OS allocates 2x this value for its overheadbuffer_size
setsockopt_result = setsockopt(result.fd,
SOL_SOCKET, SO_RCVBUF,
&buffer_size, sizeof(buffer_size));
if (setsockopt_result == -1) {
LOG0("Error setting socket receive buffer size");
continue;
}
break;
}
freeaddrinfo(ai_result);
if (ai == NULL || result.fd == KINETIC_SOCKET_DESCRIPTOR_INVALID) {
// we went through all addresses without finding one we could bind to
LOGF0("Could not connect to %s:%d", host, port);
return KINETIC_SOCKET_DESCRIPTOR_INVALID;
}
else {
LOGF1("Successfully connected to %s:%d (fd=%d)", host, port, result.fd);
return result.fd;
}
}
void KineticSocket_Close(int socket)
{
if (socket == -1) {
LOG1("Not connected so no cleanup needed");
}
else {
LOGF0("Closing socket with fd=%d", socket);
if (close(socket) == 0) {
LOG2("Socket closed successfully");
}
else {
LOGF0("Error closing socket file descriptor!"
" (fd=%d, errno=%d, desc='%s')",
socket, errno, strerror(errno));
}
}
}
KineticWaitStatus KineticSocket_WaitUntilDataAvailable(int socket, int timeout)
{
if (socket < 0) {return -1;}
struct pollfd fd = {
.fd = socket,
.events = POLLIN,
.revents = 0,
};
int res = poll(&fd, 1, timeout);
if (res > 0) {
//if (fd.revents & POLLHUP) // hung up
if (fd.revents & POLLIN)
{
return KINETIC_WAIT_STATUS_DATA_AVAILABLE;
}
else
{
return KINETIC_WAIT_STATUS_FATAL_ERROR;
}
}
else if (res == 0)
{
return KINETIC_WAIT_STATUS_TIMED_OUT;
}
else if ((errno & (EAGAIN | EINTR)) != 0)
{
return KINETIC_WAIT_STATUS_RETRYABLE_ERROR;
}
else
{
return KINETIC_WAIT_STATUS_FATAL_ERROR;
}
}
int KineticSocket_DataBytesAvailable(int socket)
{
if (socket < 0) {return -1;}
int count = -1;
ioctl(socket, FIONREAD, &count);
return count;
}
KineticStatus KineticSocket_Read(int socket, ByteBuffer* dest, size_t len)
{
LOGF2("Reading %zd bytes into buffer @ 0x%zX from fd=%d",
len, (size_t)dest->array.data, socket);
KineticStatus status = KINETIC_STATUS_INVALID;
// Read "up to" the allocated number of bytes into dest buffer
size_t bytesToReadIntoBuffer = len;
if (dest->array.len < len) {
bytesToReadIntoBuffer = dest->array.len;
}
while (dest->bytesUsed < bytesToReadIntoBuffer) {
int opStatus;
fd_set readSet;
struct timeval timeout;
// Time out after 5 seconds
timeout.tv_sec = 5;
timeout.tv_usec = 0;
FD_ZERO(&readSet);
FD_SET(socket, &readSet);
opStatus = select(socket + 1, &readSet, NULL, NULL, &timeout);
if (opStatus < 0) { // Error occurred
LOGF0("Failed waiting to read from socket!"
" status=%d, errno=%d, desc='%s'",
opStatus, errno, strerror(errno));
return KINETIC_STATUS_SOCKET_ERROR;
}
else if (opStatus == 0) { // Timeout occurred
LOG0("Timed out waiting for socket data to arrive!");
return KINETIC_STATUS_SOCKET_TIMEOUT;
}
else if (opStatus > 0) { // Data available to read
// The socket is ready for reading
opStatus = read(socket,
&dest->array.data[dest->bytesUsed],
dest->array.len - dest->bytesUsed);
// Retry if no data yet...
if (opStatus == -1 &&
((errno == EINTR) ||
(errno == EAGAIN) ||
(errno == EWOULDBLOCK)
)) {
continue;
}
else if (opStatus <= 0) {
LOGF0("Failed to read from socket!"
" status=%d, errno=%d, desc='%s'",
opStatus, errno, strerror(errno));
return KINETIC_STATUS_SOCKET_ERROR;
}
else {
dest->bytesUsed += opStatus;
LOGF3("Received %d bytes (%zd of %zd)",
opStatus, dest->bytesUsed, len);
}
}
}
// Flush any remaining data, in case of a truncated read w/short dest buffer
if (dest->bytesUsed < len) {
bool abortFlush = false;
uint8_t* discardedBytes = malloc(len - dest->bytesUsed);
if (discardedBytes == NULL) {
LOG0("Failed allocating a socket read discard buffer!");
abortFlush = true;
status = KINETIC_STATUS_MEMORY_ERROR;
}
while (!abortFlush && dest->bytesUsed < len) {
int opStatus;
fd_set readSet;
struct timeval timeout;
size_t remainingLen = len - dest->bytesUsed;
// Time out after 5 seconds
timeout.tv_sec = 5;
timeout.tv_usec = 0;
FD_ZERO(&readSet);
FD_SET(socket, &readSet);
opStatus = select(socket + 1, &readSet, NULL, NULL, &timeout);
if (opStatus < 0) { // Error occurred
LOGF0("Failure trying to flush read socket data!"
" status=%d, errno=%d, desc='%s'",
status, errno, strerror(errno));
abortFlush = true;
status = KINETIC_STATUS_SOCKET_ERROR;
continue;
}
else if (opStatus == 0) { // Timeout occurred
LOG0("Timed out waiting to flush socket data!");
abortFlush = true;
status = KINETIC_STATUS_SOCKET_TIMEOUT;
continue;
}
else if (opStatus > 0) { // Data available to read
// The socket is ready for reading
opStatus = read(socket, discardedBytes, remainingLen);
// Retry if no data yet...
if (opStatus == -1 &&
((errno == EINTR) ||
(errno == EAGAIN) ||
(errno == EWOULDBLOCK)
)) {
continue;
}
else if (opStatus <= 0) {
LOGF0("Failed to read from socket while flushing!"
" status=%d, errno=%d, desc='%s'",
opStatus, errno, strerror(errno));
abortFlush = true;
status = KINETIC_STATUS_SOCKET_ERROR;
}
else {
dest->bytesUsed += opStatus;
LOGF3("Flushed %d bytes from socket read pipe (%zd of %zd)",
opStatus, dest->bytesUsed, len);
}
}
}
// Free up dynamically allocated memory before returning
if (discardedBytes != NULL) {
free(discardedBytes);
}
// Report any error that occurred during socket flush
if (abortFlush) {
LOG0("Socket read pipe flush aborted!");
assert(status == KINETIC_STATUS_SUCCESS);
return status;
}
// Report truncation of data for any variable length byte arrays
LOGF1("Socket read buffer was truncated due to buffer overrun!"
" received=%zu, copied=%zu",
len, dest->array.len);
return KINETIC_STATUS_BUFFER_OVERRUN;
}
LOGF3("Received %zd of %zd bytes requested", dest->bytesUsed, len);
return KINETIC_STATUS_SUCCESS;
}
KineticStatus KineticSocket_ReadProtobuf(int socket, KineticPDU* pdu)
{
size_t bytesToRead = pdu->header.protobufLength;
LOGF2("Reading %zd bytes of protobuf", bytesToRead);
uint8_t* packed = (uint8_t*)malloc(bytesToRead);
if (packed == NULL) {
LOG0("Failed allocating memory for protocol buffer");
return KINETIC_STATUS_MEMORY_ERROR;
}
ByteBuffer recvBuffer = ByteBuffer_Create(packed, bytesToRead, 0);
KineticStatus status = KineticSocket_Read(socket, &recvBuffer, bytesToRead);
if (status != KINETIC_STATUS_SUCCESS) {
LOG0("Protobuf read failed!");
free(packed);
return status;
}
else {
pdu->proto = KineticProto_Message__unpack(
NULL, recvBuffer.bytesUsed, recvBuffer.array.data);
}
free(packed);
if (pdu->proto == NULL) {
pdu->protobufDynamicallyExtracted = false;
LOG0("Error unpacking incoming Kinetic protobuf message!");
return KINETIC_STATUS_DATA_ERROR;
}
else {
pdu->protobufDynamicallyExtracted = true;
LOG3("Protobuf unpacked successfully!");
return KINETIC_STATUS_SUCCESS;
}
}
