I am using ocf-linux in an openwrt application and have run into OCF CRYPTO_MAX_DATA_LEN (64K-1) and E2BIG errors in CRYPTO_AES_CBC mode.
As a consequence, I need to split the input buffer into chunks < MAX_DATA_LEN and process chunks individually and assemble the resultant plaintext / ciphertext chunks into a large output buffer.
Google has sparse results and no code regarding this issue. Wikipedia indicates this is possible by feeding the previous iv to the next block operation:
https://en.wikipedia.org/wiki/Block_cipher_mode_of_operation#Cipher-block_chaining_.28CBC.29
QUESTIONS/POINT:
Is this possible using ocf-linux?
Has anyone run into / solved this issue and have (pseudo) code = (best practice) to share to save me re-inventing the = wheel?
3 - Can anyone see any issues with my code, below?
My current decrypt function (working for size < MAX_DATA_LEN) follows:
What I am observing is incorrect decrypted data for all but the first chunk with the initial iv. I suspect that subsequent iv's from the previous block are somehow incorrect, despite using the algorithm from wikipedia.
#define AES_CHUNK_SIZE 0xFFE0
#define IV_SIZE 128 // bits
int aes_decrypt(struct cryptodev_ctx* ctx, const void* iv, const void* ciphertext, void* plaintext, size_t size)
{
struct crypt_op cryp;
void* p;
/* check plaintext and ciphertext alignment */
if (ctx->alignmask) {
p = (void*)(((unsigned long)plaintext + ctx->alignmask) & ~ctx->alignmask);
if (plaintext != p) {
DebugPrintf( DEBUG_ERR, "%s: plaintext is not aligned\n", __func__);
return ERROR;
}
p = (void*)(((unsigned long)ciphertext + ctx->alignmask) & ~ctx->alignmask);
if (ciphertext != p) {
DebugPrintf( DEBUG_ERR, "%s: ciphertext is not aligned\n", __func__);
return ERROR;
}
}
memset(&cryp, 0, sizeof(cryp));
/* Decrypt ciphertext to plaintext */
cryp.ses = ctx->sess.ses;
cryp.len = size;
cryp.src = (void*)ciphertext;
cryp.dst = plaintext;
cryp.iv = (void*)iv;
cryp.op = COP_DECRYPT;
if ( size <= CRYPTO_MAX_DATA_LEN ) {
if (ioctl(ctx->cfd, CIOCCRYPT, &cryp)) {
DebugPrintf( DEBUG_ERR, "%s: ioctl(CIOCCRYPT)\n", __func__);
return ERROR;
}
} else {
unsigned int i, remainder;
cryp.len = AES_CHUNK_SIZE;
remainder = size % AES_CHUNK_SIZE;
char byte;
DebugPrintf( DEBUG_ERR, "%s: Decrypting large block AES_CHUNK_SIZE: %x size: %x remainder: %x\n",
__func__,
(unsigned int) AES_CHUNK_SIZE,
(unsigned int) size,
remainder );
for (i = 0; i < (size / AES_CHUNK_SIZE); i++) {
DebugPrintf( DEBUG_ERR, "%s: Decrypting chunk: %x\n", __func__, i );
if (ioctl(ctx->cfd, CIOCCRYPT, &cryp)) {
DebugPrintf( DEBUG_ERR, "%s: ioctl(CIOCCRYPT)\n", __func__);
return ERROR;
}
// Print first 32 bytes of decrypted chunk data
DebugPrintf( DEBUG_ERROR, "Decrypted data, Section offset: %x\n", (unsigned int) (AES_CHUNK_SIZE * i) );
for (int j = 0; j < 32 ; j++) {
byte = cryp.dst[AES_CHUNK_SIZE * i + j];
DebugPrintf( DEBUG_ERROR, "%x ", byte);
}
DebugPrintf( DEBUG_ERROR, "\n" );
cryp.src += AES_CHUNK_SIZE;
cryp.dst += AES_CHUNK_SIZE;
cryp.iv = cryp.src - (IV_SIZE / 8);
}
if ( remainder ) {
DebugPrintf( DEBUG_ERR, "%s: Decrypting last chunk: %x\n", __func__, i );
cryp.len = remainder;
if (ioctl(ctx->cfd, CIOCCRYPT, &cryp)) {
DebugPrintf( DEBUG_ERR, "%s: ioctl(CIOCCRYPT)\n", __func__);
return ERROR;
}
DebugPrintf( DEBUG_ERROR, "Decrypted data, Section offset: %x\n", (unsigned int) (AES_CHUNK_SIZE * i) );
for (int j = 0; j < 32 ; j++) {
byte = cryp.dst[AES_CHUNK_SIZE * i + j];
DebugPrintf( DEBUG_ERROR, "%x ", byte);
}
DebugPrintf( DEBUG_ERROR, "\n" );
}
}
}
return OK;
}
Solved: The problem was that the ciphertext and plaintext buffers were the same and plaintext block n overwrote ciphertext n, including the IV required to decrypt block n+1. Working code follows:
I would appreciate feedback on how to eliminate the second IV copy.
int aes_decrypt(struct cryptodev_ctx* ctx, const void* iv, const void* ciphertext, void* plaintext, size_t size)
{
struct crypt_op cryp;
void* p;
/* check plaintext and ciphertext alignment */
if (ctx->alignmask) {
p = (void*)(((unsigned long)plaintext + ctx->alignmask) & ~ctx->alignmask);
if (plaintext != p) {
return ERROR;
}
p = (void*)(((unsigned long)ciphertext + ctx->alignmask) & ~ctx->alignmask);
if (ciphertext != p) {
return ERROR;
}
}
memset(&cryp, 0, sizeof(cryp));
/* Decrypt ciphertext to plaintext */
cryp.ses = ctx->sess.ses;
cryp.len = size;
cryp.src = (void*)ciphertext;
cryp.dst = plaintext;
cryp.iv = (void*)iv;
cryp.op = COP_DECRYPT;
if ( size <= CRYPTO_MAX_DATA_LEN ) {
if (ioctl(ctx->cfd, CIOCCRYPT, &cryp)) {
return ERROR;
}
} else {
unsigned int i, remainder;
cryp.len = AES_CHUNK_SIZE;
remainder = size % AES_CHUNK_SIZE;
char this_iv[IV_SIZE / 8];
char next_iv[IV_SIZE / 8];
for (i = 0; i < (size / AES_CHUNK_SIZE); i++) {
// For case where ciphertext and plaintext use same buffer (ciphertext overwritten by plaintext)
// need to copy the last block of ciphertext from CHUNK n
// to use as IV for chunk n + 1
memcpy( next_iv, cryp.src + AES_CHUNK_SIZE - (IV_SIZE / 8), IV_SIZE / 8);
if (ioctl(ctx->cfd, CIOCCRYPT, &cryp)) {
return ERROR;
}
cryp.src += AES_CHUNK_SIZE;
cryp.dst += AES_CHUNK_SIZE;
memcpy( this_iv, next_iv, IV_SIZE / 8);
cryp.iv = (void*) &this_iv;
}
if ( remainder ) {
cryp.len = remainder;
if (ioctl(ctx->cfd, CIOCCRYPT, &cryp)) {
return ERROR;
}
}
}
return OK;
}
Regards;
Bill Ross
In any mode other than ECB, the IV should always be fresh for every different invocation of encryption. Otherwise, IV would be guessable from the ciphertext, and security of the encryption compromised.
If while encryption, the IV for the current block coincides with one for another block, security will be weaken, hence the length limit.
Related
I am encrypting the same plain text with AES-128-CFB but ciphertext generated by "Windows Cryptography API: Next Generation" and OpenSSL are different for the same IV, and key.
Plain Text: The quick brown fox jumps over the lazy dog
IV: 1234567887654321
Key: 1234567887654321
Cipher Text generated by Windows CNG in Hex: 279CB0C2FD67B37F33A861CDDAFBEDCEEAEFC68FD5B3478E67E4A936BA7CFE75DDBAD370A1E4D6CDC3455E2F4E188A9C
Cipher Text length generated by Windows CNG: 48 Bytes
Cipher Text generated by OpenSSL in Hex: 279CB0C2FD67B37F33A861CDDAFBEDCEEAEFC68FD5B3478E67E4A936BA7CFE75DDBAD370A1E4D6CDC3455E
Cipher Text length generated by Windows CNG: 43 Bytes
Windows CNG adding extra 5 bytes(2F4E188A9C) at the end, I thought it is due to padding and sent dwFlags value as 0 instead of BCRYPT_BLOCK_PADDING then BCryptEncrypt API call failing with error code 0xc0000206/STATUS_INVALID_BUFFER_SIZE.
Windows CNG code:
#include <windows.h>
#include <stdio.h>
#include <bcrypt.h>
#pragma comment(lib, "bcrypt.lib")
#define NT_SUCCESS(Status) (((NTSTATUS)(Status)) >= 0)
#define STATUS_UNSUCCESSFUL ((NTSTATUS)0xC0000001L)
void
hexdump(const char *msg, const unsigned char *in, unsigned int len)
{
printf("%s [%d][", msg, len);
for (; len > 0; len--, in++) {
printf("%02X", *in);
}
printf("]\n");
}
const BYTE rgbPlaintext[] = "The quick brown fox jumps over the lazy dog";
static const BYTE rgbIV[] = "1234567887654321";
static const BYTE rgbAES128Key[] = "0123456789abcdefghijklmnopqrstuv";
const int ival_len = 16;
const int key_len = 16;
const DWORD block_len = 16;
void __cdecl wmain(
int argc,
__in_ecount(argc) LPWSTR *wargv)
{
BCRYPT_ALG_HANDLE hAesAlg = NULL;
BCRYPT_KEY_HANDLE hKey = NULL;
NTSTATUS status = STATUS_UNSUCCESSFUL;
DWORD cbCipherText = 0,
cbData = 0,
cbKeyObject = 0,
cbBlockLen = 0,
cbBlob = 0;
PBYTE pbCipherText = NULL,
pbKeyObject = NULL,
pbIV = NULL,
pbBlob = NULL;
UNREFERENCED_PARAMETER(argc);
UNREFERENCED_PARAMETER(wargv);
if(!NT_SUCCESS(status = BCryptOpenAlgorithmProvider(
&hAesAlg,
BCRYPT_AES_ALGORITHM,
NULL,
0)))
{
wprintf(L"**** Error 0x%x returned by BCryptOpenAlgorithmProvider\n", status);
goto Cleanup;
}
if(!NT_SUCCESS(status = BCryptGetProperty(
hAesAlg,
BCRYPT_OBJECT_LENGTH,
(PBYTE)&cbKeyObject,
sizeof(DWORD),
&cbData,
0)))
{
wprintf(L"**** Error 0x%x returned by BCryptGetProperty\n", status);
goto Cleanup;
}
pbKeyObject = (PBYTE)HeapAlloc (GetProcessHeap (), 0, cbKeyObject);
if(NULL == pbKeyObject)
{
wprintf(L"**** memory allocation failed\n");
goto Cleanup;
}
if (ival_len > sizeof (rgbIV))
{
wprintf (L"**** block length is longer than the provided IV length\n");
goto Cleanup;
}
if(ival_len > 0)
{
pbIV= (PBYTE) HeapAlloc (GetProcessHeap (), 0, ival_len + 1);
if(NULL == pbIV)
{
wprintf(L"**** memory allocation failed\n");
goto Cleanup;
}
strncpy(pbIV,rgbIV,ival_len);
pbIV[ival_len]='\0';
}
if(!NT_SUCCESS(status = BCryptSetProperty(
hAesAlg,
BCRYPT_CHAINING_MODE,
(PBYTE)BCRYPT_CHAIN_MODE_CFB,
sizeof(BCRYPT_CHAIN_MODE_CFB),
0)))
{
wprintf(L"**** Error 0x%x returned by BCryptSetProperty\n", status);
goto Cleanup;
}
if(!NT_SUCCESS(status = BCryptGetProperty(
hAesAlg,
BCRYPT_BLOCK_LENGTH,
(PBYTE)&cbBlockLen,
sizeof(DWORD),
&cbData,
0)))
{
wprintf(L"**** Error 0x%x returned by BCryptGetProperty\n", status);
goto Cleanup;
}
if(!NT_SUCCESS(status = BCryptGenerateSymmetricKey(
hAesAlg,
&hKey,
pbKeyObject,
cbKeyObject,
(PBYTE)rgbAES128Key,
key_len,
0)))
{
wprintf(L"**** Error 0x%x returned by BCryptGenerateSymmetricKey\n ", status);
fprintf(stderr, "\nError number <%x>", GetLastError());
goto Cleanup;
}
if (!NT_SUCCESS(status = BCryptSetProperty(hKey,
BCRYPT_MESSAGE_BLOCK_LENGTH,
(PBYTE)&block_len,
sizeof(DWORD),
0))) {
wprintf(L"**** Error 0x%x returned by BCryptSetProperty\n ", status);
fprintf(stderr, "\nError number <%x>", GetLastError());
goto Cleanup;
}
printf("\nplan text is [%d][%s]\n", sizeof(rgbPlaintext) - 1, rgbPlaintext);
if(!NT_SUCCESS(status = BCryptEncrypt(
hKey,
rgbPlaintext,
sizeof(rgbPlaintext) - 1,
NULL,
pbIV,
ival_len,
NULL,
0,
&cbCipherText,
BCRYPT_BLOCK_PADDING)))
{
wprintf(L"\n**** Error 1 0x%x returned by BCryptEncrypt\n", status);
goto Cleanup;
}
pbCipherText = (PBYTE)HeapAlloc (GetProcessHeap (), 0, cbCipherText);
if(NULL == pbCipherText)
{
wprintf(L"**** memory allocation failed\n");
goto Cleanup;
}
// Use the key to encrypt the plaintext buffer.
// For block sized messages, block padding will add an extra block.
if(!NT_SUCCESS(status = BCryptEncrypt(
hKey,
rgbPlaintext,
sizeof(rgbPlaintext) - 1,
NULL,
pbIV,
ival_len,
pbCipherText,
cbCipherText,
&cbData,
BCRYPT_BLOCK_PADDING)))
{
wprintf(L"**** Error 2 0x%x returned by BCryptEncrypt\n", status);
goto Cleanup;
}
hexdump("cipher is", pbCipherText, cbCipherText);
Cleanup:
if(hAesAlg)
{
BCryptCloseAlgorithmProvider(hAesAlg,0);
}
if (hKey)
{
BCryptDestroyKey(hKey);
}
if(pbCipherText)
{
HeapFree(GetProcessHeap(), 0, pbCipherText);
}
if(pbKeyObject)
{
HeapFree(GetProcessHeap(), 0, pbKeyObject);
}
if(pbIV)
{
HeapFree(GetProcessHeap(), 0, pbIV);
}
}
OpenSSL Code:
#include <openssl/conf.h>
#include <openssl/evp.h>
#include <openssl/err.h>
#include <string.h>
void
hexdump(const char *msg, const unsigned char *in, unsigned int len)
{
printf("%s [%d][", msg, len);
for (; len > 0; len--, in++) {
printf("%02X", *in);
}
printf("]\n");
}
int main(void)
{
/* A 256 bit key */
unsigned char *key = (unsigned char *)"0123456789abcdefghijklmnopqrstuv";
/* A 128 bit IV */
unsigned char *iv = (unsigned char *)"1234567887654321";
/* Message to be encrypted */
unsigned char *plaintext =
(unsigned char *)"The quick brown fox jumps over the lazy dog12312";
unsigned char ciphertext[128];
int ciphertextLen = encrypt(plaintext, strlen ((char *)plaintext), key, iv, ciphertext);
hexdump("cipher value is:", ciphertext, ciphertextLen);
return 0;
}
void handleErrors(void)
{
ERR_print_errors_fp(stderr);
abort();
}
int encrypt(unsigned char *plaintext,
int plaintext_len,
unsigned char *key,
unsigned char *iv,
unsigned char *ciphertext)
{
EVP_CIPHER_CTX *ctx;
int len;
int ciphertextLen;
/* Create and initialise the context */
if(!(ctx = EVP_CIPHER_CTX_new()))
handleErrors();
if(1 != EVP_EncryptInit_ex(ctx, EVP_aes_128_cfb(), NULL, key, iv))
handleErrors();
if(1 != EVP_EncryptUpdate(ctx, ciphertext, &len, plaintext, plaintext_len))
handleErrors();
ciphertextLen = len;
if(1 != EVP_EncryptFinal_ex(ctx, ciphertext + len, &len))
handleErrors();
ciphertextLen += len;
EVP_CIPHER_CTX_free(ctx);
return ciphertextLen;
}
i am using arduino due. what i am trying to do is to receive a string at serial. like this one:
COMSTEP 789 665 432 END
if the string starts with comstep, then to tokenize the string and get an integer array {789, 665, 432}.
is there anyway to do that?
P.S: im a noob at programming, so any help is appreciated.
I have a function that I wrote long ago to parse strings up in an easy manner. It is in use on several of my Arduino projects.
Sample usage:
char pinStr[3];
char valueStr[7];
int pinNumber, value;
getstrfld (parms_in, 0, 0, (char *)",", pinStr);
getstrfld (parms_in, 1, 0, (char *)",", valueStr);
pinNumber = atoi (pinStr);
value = atoi (valueStr);
The functions:
// My old stand-by to break delimited strings up.
char * getstrfld (char *strbuf, int fldno, int ofset, char *sep, char *retstr)
{
char *offset, *strptr;
int curfld;
offset = strptr = (char *)NULL;
curfld = 0;
strbuf += ofset;
while (*strbuf) {
strptr = !offset ? strbuf : offset;
offset = strpbrk ((!offset ? strbuf : offset), sep);
if (offset) {
offset++;
} else if (curfld != fldno) {
*retstr = 0;
break;
}
if (curfld == fldno) {
strncpy (retstr, strptr,
(int)(!offset ? strlen (strptr)+ 1 :
(int)(offset - strptr)));
if (offset)
retstr[offset - strptr - 1] = 0;
break;
}
curfld++;
}
return retstr;
}
// Included because strpbrk is not in the arduino gcc/g++ libraries
// Or I just could not find it :)
char * strpbrk (const char *s1, const char *s2)
{
const char *c = s2;
if (!*s1) {
return (char *) NULL;
}
while (*s1) {
for (c = s2; *c; c++) {
if (*s1 == *c)
break;
}
if (*c)
break;
s1++;
}
if (*c == '\0')
s1 = NULL;
return (char *) s1;
}
A light-weight approach (no strict checks on valid parses of the integers and ignoring any list elements past a fixed maximum):
char buf[32] = "COMSTEP 789 665 432 END"; // assume this has just been read
int res[8], nres = 0;
bool inlist = false;
for (char *p = strtok(buf, " "); p; p = strtok(0, " "))
if (inlist)
{
if (!strcmp(p, "END"))
{
inlist = false;
break;
}
else if (nres < sizeof(res) / sizeof(*res))
res[nres++] = atoi(p);
}
else if (!strcmp(p, "COMSTEP"))
inlist = true;
if (!inlist)
for (size_t i = 0; i < nres; ++i)
printf("%d%s", res[i], i + 1 < nres ? " " : "\n"); // do whatever
I am running the following kernel on windows 7, 64 bit, with Intel CPU and HD graphics.
I get very strange error reporting by clGetProgramBuildInfo for the following code:
#define BLOCK_SIZE 256
__kernel void reduce4(__global uint* input, __global uint* output, __local uint* sdata)
{
unsigned int tid = get_local_id(0);
unsigned int bid = get_group_id(0);
unsigned int gid = get_global_id(0);
unsigned int blockSize = get_local_size(0);
unsigned int index = bid*(BLOCK_SIZE*2) + tid;
sdata[tid] = input[index] + input[index+BLOCK_SIZE];
barrier(CLK_LOCAL_MEM_FENCE);
for(unsigned int s = BLOCK_SIZE/2; s > 64 ; s >>= 1) {
// Unrolling the last wavefront and we cut 7 iterations of this
// for-loop while we practice wavefront-programming
if(tid < s)
{
sdata[tid] += sdata[tid + s];
}
barrier(CLK_LOCAL_MEM_FENCE);
}
if (tid < 64) {
if (blockSize >= 128) sdata[tid] += sdata[tid + 64];
if (blockSize >= 64) sdata[tid] += sdata[tid + 32];
if (blockSize >= 32) sdata[tid] += sdata[tid + 16];
if (blockSize >= 16) sdata[tid] += sdata[tid + 8];
if (blockSize >= 8) sdata[tid] += sdata[tid + 4];
if (blockSize >= 4) sdata[tid] += sdata[tid + 2];
if (blockSize >= 2) sdata[tid] += sdata[tid + 1];
}
// write result for this block to global mem
if(tid == 0)
{
output[bid] = sdata[0];
}
}
It always says:
Compilation started
:38:2: error: expected identifier or '('
Compilation failed
this is for the last line, where I have put }. What is wrong here?
Update:
This is how I am reading the kernel file:
int offset = 0;
for(int i = 0; i < numOfDevices; ++i, ++offset ) {
/* Load the two source files into temporary datastores */
const char *file_names[] = {"SimpleOptimizations.cl"};
const int NUMBER_OF_FILES = 1;
char* buffer[NUMBER_OF_FILES];
size_t sizes[NUMBER_OF_FILES];
loadProgramSource(file_names, NUMBER_OF_FILES, buffer, sizes);
/* Create the OpenCL program object */
program = clCreateProgramWithSource(context, NUMBER_OF_FILES, (const char**)buffer, sizes, &error);
if(error != CL_SUCCESS) {
perror("Can't create the OpenCL program object");
exit(1);
}
Definition of loadProgramSource
void loadProgramSource(const char** files,
size_t length,
char** buffer,
size_t* sizes) {
/* Read each source file (*.cl) and store the contents into a temporary datastore */
for(size_t i=0; i < length; i++) {
FILE* file = fopen(files[i], "r");
if(file == NULL) {
perror("Couldn't read the program file");
exit(1);
}
fseek(file, 0, SEEK_END);
sizes[i] = ftell(file);
rewind(file); // reset the file pointer so that 'fread' reads from the front
buffer[i] = (char*)malloc(sizes[i]+1);
buffer[i][sizes[i]] = '\0';
fread(buffer[i], sizeof(char), sizes[i], file);
fclose(file);
}
}
I believe this is an issue with the way the Windows deals with text files opened with fopen(). If you take a look at the MSDN page for fopen(), it indicates that if you open a file with just "r" as the mode string, some translations will happen with regards to line-endings. This means that the size of the file you query may not match the amount of data read by fread().
To solve this, simply change the mode string to indicate that you wish to read the file as binary data (i.e. without any pesky translations):
FILE* file = fopen(files[i], "rb");
Edit: Solved
SOLUTION:
I'm running Arduino 1.0.5
I fixed the problem by changing /Sketchbook/library/ros_lib/ros/node_handle.h line 260 from
}else if (topic_ == TopicInfo::ID_TX_STOP){
to
}else if (topic_ == ID_TX_STOP){
However, this gave me a new error message:
/usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp: In member function 'size_t Print::print(const __FlashStringHelper*)':
/usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp:44:9: error: 'prog_char' does not name a type
/usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp:47:23: error: 'p' was not declared in this scope
So to fix this I edited usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp, line 44 from
const prog_char *p = (const prog_char *)ifsh;
to
const char PROGMEM *p = (const char PROGMEM *)ifsh;
Now it compiles!
Original Problem:
I've been using this tutorial to get everything set up: (http://wiki.ros.org/rosserial_arduino/Tutorials/Arduino%20IDE%20Setup) I can install everything without an issue I think, the ros_lib folder is placed in my sketchbook libraries. But when I do the next tutorial with the helloworld example, the code does not verify correctly. When I try to verify with the checkmark in the Arduino IDE, I get the following set of error codes:
In file included from /home/user/sketchbook/libraries/ros_lib/ros.h:38:0,
from HelloWorld.cpp:6:
/home/user/sketchbook/libraries/ros_lib/ros/node_handle.h: In member function 'virtual int ros::NodeHandle_::spinOnce()':
/home/user/sketchbook/libraries/ros_lib/ros/node_handle.h:260:45: error: expected unqualified-id before numeric constant
/home/user/sketchbook/libraries/ros_lib/ros/node_handle.h:260:45: error: expected ')' before numeric constant
I've reinstalled the ros_lib as well as rosserial and I keep getting this error, so I don't know what the problem is. I looked around line 260 of the node_handle.h file but I didn't notice anything out of place.
Here's node_handle.h: (spacing might be a little off)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote prducts derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ROS_NODE_HANDLE_H_
#define ROS_NODE_HANDLE_H_
#include "std_msgs/Time.h"
#include "rosserial_msgs/TopicInfo.h"
#include "rosserial_msgs/Log.h"
#include "rosserial_msgs/RequestParam.h"
#define SYNC_SECONDS 5
#define MODE_FIRST_FF 0
/*
* The second sync byte is a protocol version. It's value is 0xff for the first
* version of the rosserial protocol (used up to hydro), 0xfe for the second version
* (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
* detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
* rosserial_arduino. It must be changed in both this file and in
* rosserial_python/src/rosserial_python/SerialClient.py
*/
#define MODE_PROTOCOL_VER 1
#define PROTOCOL_VER1 0xff // through groovy
#define PROTOCOL_VER2 0xfe // in hydro
#define PROTOCOL_VER PROTOCOL_VER2
#define MODE_SIZE_L 2
#define MODE_SIZE_H 3
#define MODE_SIZE_CHECKSUM 4 // checksum for msg size received from size L and H
#define MODE_TOPIC_L 5 // waiting for topic id
#define MODE_TOPIC_H 6
#define MODE_MESSAGE 7
#define MODE_MSG_CHECKSUM 8 // checksum for msg and topic id
#define MSG_TIMEOUT 20 //20 milliseconds to recieve all of message data
#define ID_TX_STOP 11 //hardcode for hydro version
#include "msg.h"
namespace ros {
class NodeHandleBase_{
public:
virtual int publish(int id, const Msg* msg)=0;
virtual int spinOnce()=0;
virtual bool connected()=0;
};
}
#include "publisher.h"
#include "subscriber.h"
#include "service_server.h"
#include "service_client.h"
namespace ros {
using rosserial_msgs::TopicInfo;
/* Node Handle */
template<class Hardware,
int MAX_SUBSCRIBERS=25,
int MAX_PUBLISHERS=25,
int INPUT_SIZE=512,
int OUTPUT_SIZE=512>
class NodeHandle_ : public NodeHandleBase_
{
protected:
Hardware hardware_;
/* time used for syncing */
unsigned long rt_time;
/* used for computing current time */
unsigned long sec_offset, nsec_offset;
unsigned char message_in[INPUT_SIZE];
unsigned char message_out[OUTPUT_SIZE];
Publisher * publishers[MAX_PUBLISHERS];
Subscriber_ * subscribers[MAX_SUBSCRIBERS];
/*
* Setup Functions
*/
public:
NodeHandle_() : configured_(false) {
for(unsigned int i=0; i< MAX_PUBLISHERS; i++)
publishers[i] = 0;
for(unsigned int i=0; i< MAX_SUBSCRIBERS; i++)
subscribers[i] = 0;
for(unsigned int i=0; i< INPUT_SIZE; i++)
message_in[i] = 0;
for(unsigned int i=0; i< OUTPUT_SIZE; i++)
message_out[i] = 0;
req_param_resp.ints_length = 0;
req_param_resp.ints = NULL;
req_param_resp.floats_length = 0;
req_param_resp.floats = NULL;
req_param_resp.ints_length = 0;
req_param_resp.ints = NULL;
}
Hardware* getHardware(){
return &hardware_;
}
/* Start serial, initialize buffers */
void initNode(){
hardware_.init();
mode_ = 0;
bytes_ = 0;
index_ = 0;
topic_ = 0;
};
/* Start a named port, which may be network server IP, initialize buffers */
void initNode(char *portName){
hardware_.init(portName);
mode_ = 0;
bytes_ = 0;
index_ = 0;
topic_ = 0;
};
protected:
//State machine variables for spinOnce
int mode_;
int bytes_;
int topic_;
int index_;
int checksum_;
bool configured_;
/* used for syncing the time */
unsigned long last_sync_time;
unsigned long last_sync_receive_time;
unsigned long last_msg_timeout_time;
public:
/* This function goes in your loop() function, it handles
* serial input and callbacks for subscribers.
*/
virtual int spinOnce(){
/* restart if timed out */
unsigned long c_time = hardware_.time();
if( (c_time - last_sync_receive_time) > (SYNC_SECONDS*2200) ){
configured_ = false;
}
/* reset if message has timed out */
if ( mode_ != MODE_FIRST_FF){
if (c_time > last_msg_timeout_time){
mode_ = MODE_FIRST_FF;
}
}
/* while available buffer, read data */
while( true )
{
int data = hardware_.read();
if( data < 0 )
break;
checksum_ += data;
if( mode_ == MODE_MESSAGE ){ /* message data being recieved */
message_in[index_++] = data;
bytes_--;
if(bytes_ == 0) /* is message complete? if so, checksum */
mode_ = MODE_MSG_CHECKSUM;
}else if( mode_ == MODE_FIRST_FF ){
if(data == 0xff){
mode_++;
last_msg_timeout_time = c_time + MSG_TIMEOUT;
}
}else if( mode_ == MODE_PROTOCOL_VER ){
if(data == PROTOCOL_VER){
mode_++;
}else{
mode_ = MODE_FIRST_FF;
if (configured_ == false)
requestSyncTime(); /* send a msg back showing our protocol version */
}
}else if( mode_ == MODE_SIZE_L ){ /* bottom half of message size */
bytes_ = data;
index_ = 0;
mode_++;
checksum_ = data; /* first byte for calculating size checksum */
}else if( mode_ == MODE_SIZE_H ){ /* top half of message size */
bytes_ += data<<8;
mode_++;
}else if( mode_ == MODE_SIZE_CHECKSUM ){
if( (checksum_%256) == 255)
mode_++;
else
mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */
}else if( mode_ == MODE_TOPIC_L ){ /* bottom half of topic id */
topic_ = data;
mode_++;
checksum_ = data; /* first byte included in checksum */
}else if( mode_ == MODE_TOPIC_H ){ /* top half of topic id */
topic_ += data<<8;
mode_ = MODE_MESSAGE;
if(bytes_ == 0)
mode_ = MODE_MSG_CHECKSUM;
}else if( mode_ == MODE_MSG_CHECKSUM ){ /* do checksum */
mode_ = MODE_FIRST_FF;
if( (checksum_%256) == 255){
if(topic_ == TopicInfo::ID_PUBLISHER){
requestSyncTime();
negotiateTopics();
last_sync_time = c_time;
last_sync_receive_time = c_time;
return -1;
}else if(topic_ == TopicInfo::ID_TIME){
syncTime(message_in);
}else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
req_param_resp.deserialize(message_in);
param_recieved= true;
}else if(topic_ == TopicInfo::ID_TX_STOP){
configured_ = false;
}else{
if(subscribers[topic_-100])
subscribers[topic_-100]->callback( message_in );
}
}
}
}
/* occasionally sync time */
if( configured_ && ((c_time-last_sync_time) > (SYNC_SECONDS*500) )){
requestSyncTime();
last_sync_time = c_time;
}
return 0;
}
/* Are we connected to the PC? */
virtual bool connected() {
return configured_;
};
/********************************************************************
* Time functions
*/
void requestSyncTime()
{
std_msgs::Time t;
publish(TopicInfo::ID_TIME, &t);
rt_time = hardware_.time();
}
void syncTime( unsigned char * data )
{
std_msgs::Time t;
unsigned long offset = hardware_.time() - rt_time;
t.deserialize(data);
t.data.sec += offset/1000;
t.data.nsec += (offset%1000)*1000000UL;
this->setNow(t.data);
last_sync_receive_time = hardware_.time();
}
Time now(){
unsigned long ms = hardware_.time();
Time current_time;
current_time.sec = ms/1000 + sec_offset;
current_time.nsec = (ms%1000)*1000000UL + nsec_offset;
normalizeSecNSec(current_time.sec, current_time.nsec);
return current_time;
}
void setNow( Time & new_now )
{
unsigned long ms = hardware_.time();
sec_offset = new_now.sec - ms/1000 - 1;
nsec_offset = new_now.nsec - (ms%1000)*1000000UL + 1000000000UL;
normalizeSecNSec(sec_offset, nsec_offset);
}
/********************************************************************
* Topic Management
*/
/* Register a new publisher */
bool advertise(Publisher & p)
{
for(int i = 0; i < MAX_PUBLISHERS; i++){
if(publishers[i] == 0){ // empty slot
publishers[i] = &p;
p.id_ = i+100+MAX_SUBSCRIBERS;
p.nh_ = this;
return true;
}
}
return false;
}
/* Register a new subscriber */
template<typename MsgT>
bool subscribe(Subscriber< MsgT> & s){
for(int i = 0; i < MAX_SUBSCRIBERS; i++){
if(subscribers[i] == 0){ // empty slot
subscribers[i] = (Subscriber_*) &s;
s.id_ = i+100;
return true;
}
}
return false;
}
/* Register a new Service Server */
template<typename MReq, typename MRes>
bool advertiseService(ServiceServer<MReq,MRes>& srv){
bool v = advertise(srv.pub);
for(int i = 0; i < MAX_SUBSCRIBERS; i++){
if(subscribers[i] == 0){ // empty slot
subscribers[i] = (Subscriber_*) &srv;
srv.id_ = i+100;
return v;
}
}
return false;
}
/* Register a new Service Client */
template<typename MReq, typename MRes>
bool serviceClient(ServiceClient<MReq, MRes>& srv){
bool v = advertise(srv.pub);
for(int i = 0; i < MAX_SUBSCRIBERS; i++){
if(subscribers[i] == 0){ // empty slot
subscribers[i] = (Subscriber_*) &srv;
srv.id_ = i+100;
return v;
}
}
return false;
}
void negotiateTopics()
{
rosserial_msgs::TopicInfo ti;
int i;
for(i = 0; i < MAX_PUBLISHERS; i++)
{
if(publishers[i] != 0) // non-empty slot
{
ti.topic_id = publishers[i]->id_;
ti.topic_name = (char *) publishers[i]->topic_;
ti.message_type = (char *) publishers[i]->msg_->getType();
ti.md5sum = (char *) publishers[i]->msg_->getMD5();
ti.buffer_size = OUTPUT_SIZE;
publish( publishers[i]->getEndpointType(), &ti );
}
}
for(i = 0; i < MAX_SUBSCRIBERS; i++)
{
if(subscribers[i] != 0) // non-empty slot
{
ti.topic_id = subscribers[i]->id_;
ti.topic_name = (char *) subscribers[i]->topic_;
ti.message_type = (char *) subscribers[i]->getMsgType();
ti.md5sum = (char *) subscribers[i]->getMsgMD5();
ti.buffer_size = INPUT_SIZE;
publish( subscribers[i]->getEndpointType(), &ti );
}
}
configured_ = true;
}
virtual int publish(int id, const Msg * msg)
{
if(id >= 100 && !configured_)
return 0;
/* serialize message */
unsigned int l = msg->serialize(message_out+7);
/* setup the header */
message_out[0] = 0xff;
message_out[1] = PROTOCOL_VER;
message_out[2] = (unsigned char) ((unsigned int)l&255);
message_out[3] = (unsigned char) ((unsigned int)l>>8);
message_out[4] = 255 - ((message_out[2] + message_out[3])%256);
message_out[5] = (unsigned char) ((int)id&255);
message_out[6] = (unsigned char) ((int)id>>8);
/* calculate checksum */
int chk = 0;
for(int i =5; i<l+7; i++)
chk += message_out[i];
l += 7;
message_out[l++] = 255 - (chk%256);
if( l <= OUTPUT_SIZE ){
hardware_.write(message_out, l);
return l;
}else{
logerror("Message from device dropped: message larger than buffer.");
return -1;
}
}
/********************************************************************
* Logging
*/
private:
void log(char byte, const char * msg){
rosserial_msgs::Log l;
l.level= byte;
l.msg = (char*)msg;
publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
}
public:
void logdebug(const char* msg){
log(rosserial_msgs::Log::ROSDEBUG, msg);
}
void loginfo(const char * msg){
log(rosserial_msgs::Log::INFO, msg);
}
void logwarn(const char *msg){
log(rosserial_msgs::Log::WARN, msg);
}
void logerror(const char*msg){
log(rosserial_msgs::Log::ERROR, msg);
}
void logfatal(const char*msg){
log(rosserial_msgs::Log::FATAL, msg);
}
/********************************************************************
* Parameters
*/
private:
bool param_recieved;
rosserial_msgs::RequestParamResponse req_param_resp;
bool requestParam(const char * name, int time_out = 1000){
param_recieved = false;
rosserial_msgs::RequestParamRequest req;
req.name = (char*)name;
publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
unsigned int end_time = hardware_.time() + time_out;
while(!param_recieved ){
spinOnce();
if (hardware_.time() > end_time) return false;
}
return true;
}
public:
bool getParam(const char* name, int* param, int length =1){
if (requestParam(name) ){
if (length == req_param_resp.ints_length){
//copy it over
for(int i=0; i<length; i++)
param[i] = req_param_resp.ints[i];
return true;
}
}
return false;
}
bool getParam(const char* name, float* param, int length=1){
if (requestParam(name) ){
if (length == req_param_resp.floats_length){
//copy it over
for(int i=0; i<length; i++)
param[i] = req_param_resp.floats[i];
return true;
}
}
return false;
}
bool getParam(const char* name, char** param, int length=1){
if (requestParam(name) ){
if (length == req_param_resp.strings_length){
//copy it over
for(int i=0; i<length; i++)
strcpy(param[i],req_param_resp.strings[i]);
return true;
}
}
return false;
}
};
}
#endif
I tried commenting out line 160 and 161:
//}else if (topic_ == TopicInfo::ID_TX_STOP){
//configured_ = false;
This gave me a different error message:
/usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp: In member function 'size_t Print::print(const __FlashStringHelper*)':
/usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp:44:9: error: 'prog_char' does not name a type
/usr/share/arduino/hardware/arduino/cores/arduino/Print.cpp:47:23: error: 'p' was not declared in this scope
So here's Print.cpp
/*
Print.cpp - Base class that provides print() and println()
Copyright (c) 2008 David A. Mellis. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
Modified 23 November 2006 by David A. Mellis
*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include "Arduino.h"
#include "Print.h"
// Public Methods //////////////////////////////////////////////////////////////
/* default implementation: may be overridden */
size_t Print::write(const uint8_t *buffer, size_t size)
{
size_t n = 0;
while (size--) {
n += write(*buffer++);
}
return n;
}
size_t Print::print(const __FlashStringHelper *ifsh)
{
const prog_char *p = (const prog_char *)ifsh;
size_t n = 0;
while (1) {
unsigned char c = pgm_read_byte(p++);
if (c == 0) break;
n += write(c);
}
return n;
}
size_t Print::print(const String &s)
{
size_t n = 0;
for (uint16_t i = 0; i < s.length(); i++) {
n += write(s[i]);
}
return n;
}
size_t Print::print(const char str[])
{
return write(str);
}
size_t Print::print(char c)
{
return write(c);
}
size_t Print::print(unsigned char b, int base)
{
return print((unsigned long) b, base);
}
size_t Print::print(int n, int base)
{
return print((long) n, base);
}
size_t Print::print(unsigned int n, int base)
{
return print((unsigned long) n, base);
}
size_t Print::print(long n, int base)
{
if (base == 0) {
return write(n);
} else if (base == 10) {
if (n < 0) {
int t = print('-');
n = -n;
return printNumber(n, 10) + t;
}
return printNumber(n, 10);
} else {
return printNumber(n, base);
}
}
size_t Print::print(unsigned long n, int base)
{
if (base == 0) return write(n);
else return printNumber(n, base);
}
size_t Print::print(double n, int digits)
{
return printFloat(n, digits);
}
size_t Print::println(const __FlashStringHelper *ifsh)
{
size_t n = print(ifsh);
n += println();
return n;
}
size_t Print::print(const Printable& x)
{
return x.printTo(*this);
}
size_t Print::println(void)
{
size_t n = print('\r');
n += print('\n');
return n;
}
size_t Print::println(const String &s)
{
size_t n = print(s);
n += println();
return n;
}
size_t Print::println(const char c[])
{
size_t n = print(c);
n += println();
return n;
}
size_t Print::println(char c)
{
size_t n = print(c);
n += println();
return n;
}
size_t Print::println(unsigned char b, int base)
{
size_t n = print(b, base);
n += println();
return n;
}
size_t Print::println(int num, int base)
{
size_t n = print(num, base);
n += println();
return n;
}
size_t Print::println(unsigned int num, int base)
{
size_t n = print(num, base);
n += println();
return n;
}
size_t Print::println(long num, int base)
{
size_t n = print(num, base);
n += println();
return n;
}
size_t Print::println(unsigned long num, int base)
{
size_t n = print(num, base);
n += println();
return n;
}
size_t Print::println(double num, int digits)
{
size_t n = print(num, digits);
n += println();
return n;
}
size_t Print::println(const Printable& x)
{
size_t n = print(x);
n += println();
return n;
}
// Private Methods /////////////////////////////////////////////////////////////
size_t Print::printNumber(unsigned long n, uint8_t base) {
char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
char *str = &buf[sizeof(buf) - 1];
*str = '\0';
// prevent crash if called with base == 1
if (base < 2) base = 10;
do {
unsigned long m = n;
n /= base;
char c = m - base * n;
*--str = c < 10 ? c + '0' : c + 'A' - 10;
} while(n);
return write(str);
}
size_t Print::printFloat(double number, uint8_t digits)
{
size_t n = 0;
// Handle negative numbers
if (number < 0.0)
{
n += print('-');
number = -number;
}
// Round correctly so that print(1.999, 2) prints as "2.00"
double rounding = 0.5;
for (uint8_t i=0; i<digits; ++i)
rounding /= 10.0;
number += rounding;
// Extract the integer part of the number and print it
unsigned long int_part = (unsigned long)number;
double remainder = number - (double)int_part;
n += print(int_part);
// Print the decimal point, but only if there are digits beyond
if (digits > 0) {
n += print(".");
}
// Extract digits from the remainder one at a time
while (digits-- > 0)
{
remainder *= 10.0;
int toPrint = int(remainder);
n += print(toPrint);
remainder -= toPrint;
}
return n;
}
I want to measure the time of transferring a file from the send to the recv by the PGM/UDP protocol. Actually , I try to use clock() function to measure the execution time of both the send and recv program ,but it seems to be incorrect. What should I do to measure the time of transferring a file ?
Here is the code :
clock_t t = clock();
do
{
struct timeval tv;
size_t len;
int timeout;
pgm_error_t *pgm_err = NULL;
const int status = pgm_recv (rx_sock,
buf,
sizeof(buf),
0,
&len,
&pgm_err);
switch (status)
{
case PGM_IO_STATUS_NORMAL:
/* receive file */
if (len < 30)
{
strncpy(newFile, buf, len);
if (0 == strncmp(newFile, testNewFile, len))
{
flag = 1;
continue;
}
strncpy(endFile, buf, len);
if (0 == strncmp(endFile, testEndFile, len))
{
//g_quit = 1;
t = clock() -t;
printf("%s takes time: %f\n", fileName, ((float)t) / CLOCKS_PER_SEC);
continue;
}
}
if (flag == 1)
{
strncpy(fileName, buf, len);
// puts(fileName);
fp = fopen(fileName, "wb");
g_assert(fp);
flag = 0;
}
else
num_read = fwrite (buf, sizeof(char), len, fp);
break;
case PGM_IO_STATUS_TIMER_PENDING:
{
socklen_t optlen = sizeof (tv);
pgm_getsockopt (rx_sock, IPPROTO_PGM, PGM_TIME_REMAIN, &tv, &optlen);
}
goto block;
case PGM_IO_STATUS_RATE_LIMITED:
{
socklen_t optlen = sizeof (tv);
pgm_getsockopt (rx_sock, IPPROTO_PGM, PGM_RATE_REMAIN, &tv, &optlen);
}
case PGM_IO_STATUS_WOULD_BLOCK:
block:
timeout = PGM_IO_STATUS_WOULD_BLOCK == status ? -1 : ((tv.tv_sec * 1000) + (tv.tv_usec / 1000));
memset (fds, 0, sizeof(fds));
fds[0].fd = g_quit_pipe[0];
fds[0].events = POLLIN;
pgm_poll_info (rx_sock, &fds[1], &n_fds, POLLIN);
poll (fds, 1 + n_fds, timeout);
break;
default:
if (pgm_err)
{
g_warning ("%s", pgm_err->message);
pgm_error_free (pgm_err);
pgm_err = NULL;
}
if (PGM_IO_STATUS_ERROR == status)
break;
}
}while (!g_quit);
fclose (fp);
return NULL;