u_char *
mac_compute(Mac *mac, u_int32_t seqno, u_char *data, int datalen)
{
static u_char m[EVP_MAX_MD_SIZE];
u_char b[4], nonce[8];
if (mac->mac_len > sizeof(m))
fatal("mac_compute: mac too long %u %lu",
mac->mac_len, (u_long)sizeof(m));
switch (mac->type) {
case SSH_EVP:
put_u32(b, seqno);
/* reset HMAC context */
HMAC_Init(&mac->evp_ctx, NULL, 0, NULL);
HMAC_Update(&mac->evp_ctx, b, sizeof(b));
HMAC_Update(&mac->evp_ctx, data, datalen);
HMAC_Final(&mac->evp_ctx, m, NULL);
break;
case SSH_UMAC:
put_u64(nonce, seqno);
umac_update(mac->umac_ctx, data, datalen);
umac_final(mac->umac_ctx, m, nonce);
break;
case SSH_UMAC128:
put_u64(nonce, seqno);
umac128_update(mac->umac_ctx, data, datalen);
umac128_final(mac->umac_ctx, m, nonce);
break;
default:
fatal("mac_compute: unknown MAC type");
}
return (m);
}
int send_cmd_hdd_file_send_with_offset(libusb_device_handle* fd, __u8 dir, char *path, __u64 offset)
{
struct tf_packet req;
__u16 packetSize, pad;
int pathLen = strlen(path) + 1;
trace(3, printf("send_cmd_hdd_file_send_with_offset: %s [%ld]\n",path,offset));
if((PACKET_HEAD_SIZE + 1 + 2 + pathLen) >= MAXIMUM_PACKET_SIZE)
{
fprintf(stdout, "ERROR: Path is too long.\n");
return -1;
}
packetSize = PACKET_HEAD_SIZE + 1 + 2 + pathLen +8;
pad = ((packetSize + 1) & ~1 ) - packetSize;
//printf("Packet padding: %d\n",pad);
pad = 0;//((packetSize + 1) & ~1 ) - packetSize;
//printf("Packet padding: %d\n",pad);
packetSize +=pad;
put_u16(&req.length, packetSize);
put_u32(&req.cmd, CMD_HDD_FILE_SEND);
req.data[0] = dir;
put_u16(&req.data[1], (__u16) pathLen);
strcpy((char *) &req.data[3], path);
put_u64( &req.data[3+pathLen+pad],offset);
return send_tf_packet(fd, &req);
}
static
long lttng_metadata_ring_buffer_compat_ioctl(struct file *filp,
unsigned int cmd, unsigned long arg)
{
int ret;
struct lttng_metadata_stream *stream = filp->private_data;
struct lib_ring_buffer *buf = stream->priv;
switch (cmd) {
case RING_BUFFER_GET_NEXT_SUBBUF:
{
struct lttng_metadata_stream *stream = filp->private_data;
struct lib_ring_buffer *buf = stream->priv;
struct channel *chan = buf->backend.chan;
ret = lttng_metadata_output_channel(stream, chan);
if (ret > 0) {
lib_ring_buffer_switch_slow(buf, SWITCH_ACTIVE);
ret = 0;
} else if (ret < 0)
goto err;
break;
}
case RING_BUFFER_GET_SUBBUF:
{
/*
* Random access is not allowed for metadata channel.
*/
return -ENOSYS;
}
default:
break;
}
/* PUT_SUBBUF is the one from lib ring buffer, unmodified. */
/* Performing lib ring buffer ioctl after our own. */
ret = lib_ring_buffer_compat_ioctl(filp, cmd, arg, buf);
if (ret < 0)
goto err;
switch (cmd) {
case RING_BUFFER_PUT_NEXT_SUBBUF:
{
lttng_metadata_ring_buffer_ioctl_put_next_subbuf(filp,
cmd, arg);
break;
}
case RING_BUFFER_GET_METADATA_VERSION:
{
struct lttng_metadata_stream *stream = filp->private_data;
return put_u64(stream->version, arg);
}
default:
break;
}
err:
return ret;
}
void
buffer_put_int64(Buffer *buffer, u_int64_t value)
{
char buf[8];
put_u64(buf, value);
buffer_append(buffer, buf, 8);
}
u_char *
mac_compute(Mac *mac, u_int32_t seqno, u_char *data, int datalen)
{
static union {
u_char m[EVP_MAX_MD_SIZE];
u_int64_t for_align;
} u;
u_char b[4];
#ifdef UMAC_HAS_BEEN_UNBROKEN
u_char nonce[8];
#endif
if (mac->mac_len > sizeof(u))
fatal("mac_compute: mac too long %u %lu",
mac->mac_len, (u_long)sizeof(u));
switch (mac->type) {
case SSH_EVP:
put_u32(b, seqno);
/* reset HMAC context */
HMAC_Init(&mac->evp_ctx, NULL, 0, NULL);
HMAC_Update(&mac->evp_ctx, b, sizeof(b));
HMAC_Update(&mac->evp_ctx, data, datalen);
HMAC_Final(&mac->evp_ctx, u.m, NULL);
break;
#ifdef UMAC_HAS_BEEN_UNBROKEN
case SSH_UMAC:
put_u64(nonce, seqno);
umac_update(mac->umac_ctx, data, datalen);
umac_final(mac->umac_ctx, u.m, nonce);
break;
case SSH_UMAC128:
put_u64(nonce, seqno);
umac128_update(mac->umac_ctx, data, datalen);
umac128_final(mac->umac_ctx, u.m, nonce);
break;
#endif
default:
fatal("mac_compute: unknown MAC type");
}
return (u.m);
}
u_char *
mac_compute(Mac *mac, u_int32_t seqno, u_char *data, int datalen)
{
static union {
u_char m[EVP_MAX_MD_SIZE];
u_int64_t for_align;
} u;
u_char b[4];
u_char nonce[8];
if (mac->mac_len > sizeof(u))
fatal("mac_compute: mac too long %u %zu",
mac->mac_len, sizeof(u));
switch (mac->type) {
case SSH_DIGEST:
put_u32(b, seqno);
/* reset HMAC context */
if (ssh_hmac_init(mac->hmac_ctx, NULL, 0) < 0 ||
ssh_hmac_update(mac->hmac_ctx, b, sizeof(b)) < 0 ||
ssh_hmac_update(mac->hmac_ctx, data, datalen) < 0 ||
ssh_hmac_final(mac->hmac_ctx, u.m, sizeof(u.m)) < 0)
fatal("ssh_hmac failed");
break;
case SSH_UMAC:
put_u64(nonce, seqno);
umac_update(mac->umac_ctx, data, datalen);
umac_final(mac->umac_ctx, u.m, nonce);
break;
case SSH_UMAC128:
put_u64(nonce, seqno);
umac128_update(mac->umac_ctx, data, datalen);
umac128_final(mac->umac_ctx, u.m, nonce);
break;
default:
fatal("mac_compute: unknown MAC type");
}
return (u.m);
}
/* Decrypt and extract the encrypted packet length */
int
chachapoly_get_length(struct chachapoly_ctx *ctx,
u_int *plenp, u_int seqnr, const u_char *cp, u_int len)
{
u_char buf[4], seqbuf[8];
if (len < 4)
return -1; /* Insufficient length */
put_u64(seqbuf, seqnr);
//chacha_ivsetup(&ctx->header_ctx, seqbuf, NULL);
//chacha_encrypt_bytes(&ctx->header_ctx, cp, buf, 4);
*plenp = get_u32(buf);
return 0;
}
/*
* chachapoly_crypt() operates as following:
* En/decrypt with header key 'aadlen' bytes from 'src', storing result
* to 'dest'. The ciphertext here is treated as additional authenticated
* data for MAC calculation.
* En/decrypt 'len' bytes at offset 'aadlen' from 'src' to 'dest'. Use
* POLY1305_TAGLEN bytes at offset 'len'+'aadlen' as the authentication
* tag. This tag is written on encryption and verified on decryption.
*/
int
chachapoly_crypt(struct chachapoly_ctx *ctx, u_int seqnr, u_char *dest,
const u_char *src, u_int len, u_int aadlen, u_int authlen, int do_encrypt)
{
u_char seqbuf[8];
const u_char one[8] = { 1, 0, 0, 0, 0, 0, 0, 0 }; /* NB little-endian */
u_char expected_tag[POLY1305_TAGLEN], poly_key[POLY1305_KEYLEN];
int r = -1;
/*
* Run ChaCha20 once to generate the Poly1305 key. The IV is the
* packet sequence number.
*/
bzero(poly_key, sizeof(poly_key));
put_u64(seqbuf, seqnr);
chacha_ivsetup(&ctx->main_ctx, seqbuf, NULL);
chacha_encrypt_bytes(&ctx->main_ctx,
poly_key, poly_key, sizeof(poly_key));
/* Set Chacha's block counter to 1 */
chacha_ivsetup(&ctx->main_ctx, seqbuf, one);
/* If decrypting, check tag before anything else */
if (!do_encrypt) {
const u_char *tag = src + aadlen + len;
poly1305_auth(expected_tag, src, aadlen + len, poly_key);
if (timingsafe_bcmp(expected_tag, tag, POLY1305_TAGLEN) != 0)
goto out;
}
/* Crypt additional data */
if (aadlen) {
chacha_ivsetup(&ctx->header_ctx, seqbuf, NULL);
chacha_encrypt_bytes(&ctx->header_ctx, src, dest, aadlen);
}
chacha_encrypt_bytes(&ctx->main_ctx, src + aadlen,
dest + aadlen, len);
/* If encrypting, calculate and append tag */
if (do_encrypt) {
poly1305_auth(dest + aadlen + len, dest, aadlen + len,
poly_key);
}
r = 0;
out:
bzero(expected_tag, sizeof(expected_tag));
bzero(seqbuf, sizeof(seqbuf));
bzero(poly_key, sizeof(poly_key));
return r;
}
int
mac_compute(struct sshmac *mac, u_int32_t seqno, const u_char *data, int datalen,
u_char *digest, size_t dlen)
{
static union {
u_char m[MAC_DIGEST_LEN_MAX];
u_int64_t for_align;
} u;
u_char b[4], nonce[8];
if (mac->mac_len > sizeof(u))
return SSH_ERR_INTERNAL_ERROR;
switch (mac->type) {
case SSH_EVP:
POKE_U32(b, seqno);
/* reset HMAC context */
if (HMAC_Init(&mac->evp_ctx, NULL, 0, NULL) != 1 ||
HMAC_Update(&mac->evp_ctx, b, sizeof(b)) != 1 ||
HMAC_Update(&mac->evp_ctx, data, datalen) != 1 ||
HMAC_Final(&mac->evp_ctx, u.m, NULL) != 1)
return SSH_ERR_LIBCRYPTO_ERROR;
break;
case SSH_UMAC:
POKE_U64(nonce, seqno);
umac_update(mac->umac_ctx, data, datalen);
umac_final(mac->umac_ctx, u.m, nonce);
break;
case SSH_UMAC128:
put_u64(nonce, seqno);
umac128_update(mac->umac_ctx, data, datalen);
umac128_final(mac->umac_ctx, u.m, nonce);
break;
default:
return SSH_ERR_INVALID_ARGUMENT;
}
if (digest != NULL) {
if (dlen > mac->mac_len)
dlen = mac->mac_len;
memcpy(digest, u.m, dlen);
}
return 0;
}
static int
put_file_func (CameraFilesystem *fs, const char *folder, const char *filename, CameraFile *file,
void *data, GPContext *context)
{
Camera *camera = data;
/*
* Upload the file to the camera. Use gp_file_get_data_and_size etc.
*/
int result = -EPROTO;
time_t startTime = time(NULL);
enum {
START,
DATA,
END,
FINISHED
} state;
int src = -1;
int r;
int update = 0;
struct stat srcStat;
__u64 fileSize;
__u64 byteCount = 0;
const char *filename;
char *path;
struct tf_packet reply;
if(0 != fstat(src, &srcStat))
{
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Can not examine source file: %s\n",
strerror(errno));
result = errno;
goto out;
}
fileSize = srcStat.st_size;
if(fileSize == 0)
{
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Source file is empty - not transfering.\n");
result = -ENODATA;
goto out;
}
path = get_path (camera, folder, filename);
r = send_cmd_hdd_file_send(camera, PUT, path, context);
if(r < 0)
goto out;
state = START;
while(0 < get_tf_packet(camera, &reply, context)) {
update = (update + 1) % 16;
switch (get_u32(&reply.cmd)) {
case SUCCESS:
switch (state) {
case START: {
/* Send start */
struct typefile *tf = (struct typefile *) packet.data;
put_u16(&packet.length, PACKET_HEAD_SIZE + 114);
put_u32(&packet.cmd, DATA_HDD_FILE_START);
time_to_tfdt(srcStat.st_mtime, &tf->stamp);
tf->filetype = 2;
put_u64(&tf->size, srcStat.st_size);
strncpy((char *) tf->name, path, 94);
tf->name[94] = '\0';
tf->unused = 0;
tf->attrib = 0;
gp_log (GP_LOG_DEBUG, "topfield", "%s: DATA_HDD_FILE_START\n", __func__);
r = send_tf_packet(camera, &packet, context);
if(r < 0)
{
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Incomplete send.\n");
goto out;
}
state = DATA;
break;
}
case DATA: {
int payloadSize = sizeof(packet.data) - 9;
ssize_t w = read(src, &packet.data[8], payloadSize);
/* Detect a Topfield protcol bug and prevent the sending of packets
that are a multiple of 512 bytes. */
if((w > 4) && (((((PACKET_HEAD_SIZE + 8 + w) + 1) & ~1) % 0x200) == 0)) {
lseek(src, -4, SEEK_CUR);
w -= 4;
payloadSize -= 4;
}
put_u16(&packet.length, PACKET_HEAD_SIZE + 8 + w);
put_u32(&packet.cmd, DATA_HDD_FILE_DATA);
put_u64(packet.data, byteCount);
byteCount += w;
/* Detect EOF and transition to END */
if((w < 0) || (byteCount >= fileSize)) {
state = END;
}
if(w > 0) {
gp_log (GP_LOG_DEBUG, "topfield", "%s: DATA_HDD_FILE_DATA\n", __func__);
r = send_tf_packet(camera, &packet, context);
if(r < w) {
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Incomplete send.\n");
goto out;
}
}
if(!update && !quiet) {
progressStats(fileSize, byteCount, startTime);
}
break;
}
case END:
/* Send end */
put_u16(&packet.length, PACKET_HEAD_SIZE);
put_u32(&packet.cmd, DATA_HDD_FILE_END);
gp_log (GP_LOG_DEBUG, "topfield", "%s: DATA_HDD_FILE_END\n", __func__);
r = send_tf_packet(camera, &packet, context);
if(r < 0) {
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Incomplete send.\n");
goto out;
}
state = FINISHED;
break;
case FINISHED:
result = 0;
goto out;
break;
}
break;
case FAIL:
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Device reports %s\n", decode_error(&reply));
goto out;
break;
default:
gp_log (GP_LOG_ERROR, "topfield", "ERROR: Unhandled packet (%d)\n", get_u32(&reply.cmd));
break;
}
}
finalStats(byteCount, startTime);
out:
close(src);
return result;
}
int do_hdd_file_put(int fd, char *srcPath, char *dstPath)
{
int result = -EPROTO;
time_t startTime = time(NULL);
enum
{
START,
DATA,
END,
FINISHED
} state;
int src = -1;
int r;
int update = 0;
struct stat64 srcStat;
__u64 fileSize;
__u64 byteCount = 0;
trace(4, fprintf(stderr, "%s\n", __func__));
src = open64(srcPath, O_RDONLY);
if(src < 0)
{
fprintf(stderr, "ERROR: Can not open source file: %s\n",
strerror(errno));
return errno;
}
if(0 != fstat64(src, &srcStat))
{
fprintf(stderr, "ERROR: Can not examine source file: %s\n",
strerror(errno));
result = errno;
goto out;
}
fileSize = srcStat.st_size;
if(fileSize == 0)
{
fprintf(stderr, "ERROR: Source file is empty - not transfering.\n");
result = -ENODATA;
goto out;
}
r = send_cmd_hdd_file_send(fd, PUT, dstPath);
if(r < 0)
{
goto out;
}
state = START;
while(0 < get_tf_packet(fd, &reply))
{
update = (update + 1) % 16;
switch (get_u32(&reply.cmd))
{
case SUCCESS:
switch (state)
{
case START:
{
/* Send start */
struct typefile *tf = (struct typefile *) packet.data;
put_u16(&packet.length, PACKET_HEAD_SIZE + 114);
put_u32(&packet.cmd, DATA_HDD_FILE_START);
time_to_tfdt(srcStat.st_mtime, &tf->stamp);
tf->filetype = 2;
put_u64(&tf->size, srcStat.st_size);
strncpy((char *) tf->name, dstPath, 94);
tf->name[94] = '\0';
tf->unused = 0;
tf->attrib = 0;
trace(3,
fprintf(stderr, "%s: DATA_HDD_FILE_START\n",
__func__));
r = send_tf_packet(fd, &packet);
if(r < 0)
{
fprintf(stderr, "ERROR: Incomplete send.\n");
goto out;
}
state = DATA;
break;
}
case DATA:
{
int payloadSize = sizeof(packet.data) - 9;
ssize_t w = read(src, &packet.data[8], payloadSize);
/* Detect a Topfield protcol bug and prevent the sending of packets
that are a multiple of 512 bytes. */
if((w > 4)
&&
(((((PACKET_HEAD_SIZE + 8 + w) +
1) & ~1) % 0x200) == 0))
{
lseek64(src, -4, SEEK_CUR);
w -= 4;
payloadSize -= 4;
}
put_u16(&packet.length, PACKET_HEAD_SIZE + 8 + w);
put_u32(&packet.cmd, DATA_HDD_FILE_DATA);
put_u64(packet.data, byteCount);
byteCount += w;
/* Detect EOF and transition to END */
if((w < 0) || (byteCount >= fileSize))
{
state = END;
}
if(w > 0)
{
trace(3,
fprintf(stderr, "%s: DATA_HDD_FILE_DATA\n",
__func__));
r = send_tf_packet(fd, &packet);
if(r < w)
{
fprintf(stderr, "ERROR: Incomplete send.\n");
goto out;
}
}
if(!update && !quiet)
{
progressStats(fileSize, byteCount, startTime);
}
break;
}
case END:
/* Send end */
put_u16(&packet.length, PACKET_HEAD_SIZE);
put_u32(&packet.cmd, DATA_HDD_FILE_END);
trace(3,
fprintf(stderr, "%s: DATA_HDD_FILE_END\n",
__func__));
r = send_tf_packet(fd, &packet);
if(r < 0)
{
fprintf(stderr, "ERROR: Incomplete send.\n");
goto out;
}
state = FINISHED;
break;
case FINISHED:
result = 0;
goto out;
break;
}
break;
case FAIL:
fprintf(stderr, "ERROR: Device reports %s\n",
decode_error(&reply));
goto out;
break;
default:
fprintf(stderr, "ERROR: Unhandled packet\n");
break;
}
}
finalStats(byteCount, startTime);
out:
close(src);
return result;
}
static long lttng_stream_ring_buffer_compat_ioctl(struct file *filp,
unsigned int cmd, unsigned long arg)
{
struct lib_ring_buffer *buf = filp->private_data;
struct channel *chan = buf->backend.chan;
const struct lib_ring_buffer_config *config = &chan->backend.config;
const struct lttng_channel_ops *ops = chan->backend.priv_ops;
int ret;
if (atomic_read(&chan->record_disabled))
return -EIO;
switch (cmd) {
case LTTNG_RING_BUFFER_COMPAT_GET_TIMESTAMP_BEGIN:
{
uint64_t ts;
ret = ops->timestamp_begin(config, buf, &ts);
if (ret < 0)
goto error;
return put_u64(ts, arg);
}
case LTTNG_RING_BUFFER_COMPAT_GET_TIMESTAMP_END:
{
uint64_t ts;
ret = ops->timestamp_end(config, buf, &ts);
if (ret < 0)
goto error;
return put_u64(ts, arg);
}
case LTTNG_RING_BUFFER_COMPAT_GET_EVENTS_DISCARDED:
{
uint64_t ed;
ret = ops->events_discarded(config, buf, &ed);
if (ret < 0)
goto error;
return put_u64(ed, arg);
}
case LTTNG_RING_BUFFER_COMPAT_GET_CONTENT_SIZE:
{
uint64_t cs;
ret = ops->content_size(config, buf, &cs);
if (ret < 0)
goto error;
return put_u64(cs, arg);
}
case LTTNG_RING_BUFFER_COMPAT_GET_PACKET_SIZE:
{
uint64_t ps;
ret = ops->packet_size(config, buf, &ps);
if (ret < 0)
goto error;
return put_u64(ps, arg);
}
case LTTNG_RING_BUFFER_COMPAT_GET_STREAM_ID:
{
uint64_t si;
ret = ops->stream_id(config, buf, &si);
if (ret < 0)
goto error;
return put_u64(si, arg);
}
case LTTNG_RING_BUFFER_GET_CURRENT_TIMESTAMP:
{
uint64_t ts;
ret = ops->current_timestamp(config, buf, &ts);
if (ret < 0)
goto error;
return put_u64(ts, arg);
}
case LTTNG_RING_BUFFER_COMPAT_GET_SEQ_NUM:
{
uint64_t seq;
ret = ops->sequence_number(config, buf, &seq);
if (ret < 0)
goto error;
return put_u64(seq, arg);
}
case LTTNG_RING_BUFFER_COMPAT_INSTANCE_ID:
{
uint64_t id;
ret = ops->instance_id(config, buf, &id);
if (ret < 0)
goto error;
return put_u64(id, arg);
}
default:
return lib_ring_buffer_file_operations.compat_ioctl(filp,
cmd, arg);
}
error:
return -ENOSYS;
}
static
long lttng_metadata_ring_buffer_compat_ioctl(struct file *filp,
unsigned int cmd, unsigned long arg)
{
int ret;
struct lttng_metadata_stream *stream = filp->private_data;
struct lib_ring_buffer *buf = stream->priv;
switch (cmd) {
case RING_BUFFER_GET_NEXT_SUBBUF:
{
struct lttng_metadata_stream *stream = filp->private_data;
struct lib_ring_buffer *buf = stream->priv;
struct channel *chan = buf->backend.chan;
ret = lttng_metadata_output_channel(stream, chan);
if (ret > 0) {
lib_ring_buffer_switch_slow(buf, SWITCH_ACTIVE);
ret = 0;
} else if (ret < 0)
goto err;
break;
}
case RING_BUFFER_GET_SUBBUF:
{
/*
* Random access is not allowed for metadata channel.
*/
return -ENOSYS;
}
case RING_BUFFER_FLUSH:
{
struct lttng_metadata_stream *stream = filp->private_data;
struct lib_ring_buffer *buf = stream->priv;
struct channel *chan = buf->backend.chan;
/*
* Before doing the actual ring buffer flush, write up to one
* packet of metadata in the ring buffer.
*/
ret = lttng_metadata_output_channel(stream, chan);
if (ret < 0)
goto err;
break;
}
case RING_BUFFER_GET_METADATA_VERSION:
{
struct lttng_metadata_stream *stream = filp->private_data;
return put_u64(stream->version, arg);
}
case RING_BUFFER_SNAPSHOT:
{
/*
* Force the buffer to quiescent so the ring buffer
* don't attempt to perform a SWITCH_FLUSH, which would
* desynchronize the client accounting of the amount of
* data available in the buffer from the ring buffer
* view.
*/
buf->quiescent = true;
break;
}
default:
break;
}
/* PUT_SUBBUF is the one from lib ring buffer, unmodified. */
/* Performing lib ring buffer ioctl after our own. */
ret = lib_ring_buffer_compat_ioctl(filp, cmd, arg, buf);
if (ret < 0)
goto err;
switch (cmd) {
case RING_BUFFER_PUT_NEXT_SUBBUF:
{
lttng_metadata_ring_buffer_ioctl_put_next_subbuf(filp,
cmd, arg);
break;
}
default:
break;
}
err:
return ret;
}