From 1f12cea75b3b4688e26554de5ff22eeb59d3da30 Mon Sep 17 00:00:00 2001 From: Bjorn Andersson Date: Sun, 7 Feb 2016 09:30:33 -0800 Subject: [PATCH] rmtfs: Initial rmtfs implementation The rmtfs tool pushlishes the two QMI services "rmtfs" and "rfsa", and implements the necessary requests for rmtfs that's needed to boot the modem subsystem on a Qualcomm based board. Signed-off-by: Bjorn Andersson --- Makefile | 17 ++ qmi_rmtfs.c | 600 ++++++++++++++++++++++++++++++++++++++++++++++++++ qmi_rmtfs.h | 196 +++++++++++++++++ qmi_rmtfs.qmi | 77 +++++++ qmi_tlv.c | 234 ++++++++++++++++++++ rmtfs.c | 511 ++++++++++++++++++++++++++++++++++++++++++ rmtfs.h | 28 +++ sharedmem.c | 88 ++++++++ storage.c | 140 ++++++++++++ util.c | 48 ++++ util.h | 9 + 11 files changed, 1948 insertions(+) create mode 100644 Makefile create mode 100644 qmi_rmtfs.c create mode 100644 qmi_rmtfs.h create mode 100644 qmi_rmtfs.qmi create mode 100644 qmi_tlv.c create mode 100644 rmtfs.c create mode 100644 rmtfs.h create mode 100644 sharedmem.c create mode 100644 storage.c create mode 100644 util.c create mode 100644 util.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..35c8a63 --- /dev/null +++ b/Makefile @@ -0,0 +1,17 @@ +OUT := rmtfs + +CFLAGS := -Wall -g -I../qrtr/lib +LDFLAGS := -L../qrtr -lqrtr + +SRCS := qmi_rmtfs.c qmi_tlv.c rmtfs.c sharedmem.c storage.c util.c +OBJS := $(SRCS:.c=.o) + +$(OUT): $(OBJS) + $(CC) -o $@ $^ $(LDFLAGS) + +test: $(OUT) + ./$(OUT) + +clean: + rm -f $(OUT) $(OBJS) + diff --git a/qmi_rmtfs.c b/qmi_rmtfs.c new file mode 100644 index 0000000..737b490 --- /dev/null +++ b/qmi_rmtfs.c @@ -0,0 +1,600 @@ +#include +#include +#include "qmi_rmtfs.h" + +struct rmtfs_open_req *rmtfs_open_req_alloc(unsigned txn) +{ + return (struct rmtfs_open_req*)qmi_tlv_init(txn, 1); +} + +struct rmtfs_open_req *rmtfs_open_req_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_open_req*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_open_req_encode(struct rmtfs_open_req *open_req, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)open_req, len); +} + +void rmtfs_open_req_free(struct rmtfs_open_req *open_req) +{ + qmi_tlv_free((struct qmi_tlv*)open_req); +} + +int rmtfs_open_req_set_path(struct rmtfs_open_req *open_req, char *buf, size_t len) +{ + return qmi_tlv_set((struct qmi_tlv*)open_req, 1, buf, len); +} + +int rmtfs_open_req_get_path(struct rmtfs_open_req *open_req, char *buf, size_t buflen) +{ + size_t len; + char *ptr; + + ptr = qmi_tlv_get((struct qmi_tlv*)open_req, 1, &len); + if (!ptr) + return -ENOENT; + + if (len >= buflen) + return -ENOMEM; + + memcpy(buf, ptr, len); + buf[len] = '\0'; + return len; +} + +struct rmtfs_open_resp *rmtfs_open_resp_alloc(unsigned txn) +{ + return (struct rmtfs_open_resp*)qmi_tlv_init(txn, 1); +} + +struct rmtfs_open_resp *rmtfs_open_resp_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_open_resp*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_open_resp_encode(struct rmtfs_open_resp *open_resp, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)open_resp, len); +} + +void rmtfs_open_resp_free(struct rmtfs_open_resp *open_resp) +{ + qmi_tlv_free((struct qmi_tlv*)open_resp); +} + +int rmtfs_open_resp_set_result(struct rmtfs_open_resp *open_resp, struct rmtfs_qmi_result *val) +{ + return qmi_tlv_set((struct qmi_tlv*)open_resp, 2, val, sizeof(struct rmtfs_qmi_result)); +} + +struct rmtfs_qmi_result *rmtfs_open_resp_get_result(struct rmtfs_open_resp *open_resp) +{ + size_t len; + void *ptr; + + ptr = qmi_tlv_get((struct qmi_tlv*)open_resp, 2, &len); + if (!ptr) + return NULL; + + if (len != sizeof(struct rmtfs_qmi_result)) + return NULL; + + return ptr; +} + +int rmtfs_open_resp_set_caller_id(struct rmtfs_open_resp *open_resp, uint32_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)open_resp, 16, &val, sizeof(uint32_t)); +} + +int rmtfs_open_resp_get_caller_id(struct rmtfs_open_resp *open_resp, uint32_t *val) +{ + uint32_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)open_resp, 16, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint32_t)) + return -EINVAL; + + *val = *(uint32_t*)ptr; + return 0; +} + +struct rmtfs_close_req *rmtfs_close_req_alloc(unsigned txn) +{ + return (struct rmtfs_close_req*)qmi_tlv_init(txn, 2); +} + +struct rmtfs_close_req *rmtfs_close_req_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_close_req*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_close_req_encode(struct rmtfs_close_req *close_req, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)close_req, len); +} + +void rmtfs_close_req_free(struct rmtfs_close_req *close_req) +{ + qmi_tlv_free((struct qmi_tlv*)close_req); +} + +int rmtfs_close_req_set_caller_id(struct rmtfs_close_req *close_req, uint32_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)close_req, 1, &val, sizeof(uint32_t)); +} + +int rmtfs_close_req_get_caller_id(struct rmtfs_close_req *close_req, uint32_t *val) +{ + uint32_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)close_req, 1, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint32_t)) + return -EINVAL; + + *val = *(uint32_t*)ptr; + return 0; +} + +struct rmtfs_close_resp *rmtfs_close_resp_alloc(unsigned txn) +{ + return (struct rmtfs_close_resp*)qmi_tlv_init(txn, 2); +} + +struct rmtfs_close_resp *rmtfs_close_resp_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_close_resp*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_close_resp_encode(struct rmtfs_close_resp *close_resp, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)close_resp, len); +} + +void rmtfs_close_resp_free(struct rmtfs_close_resp *close_resp) +{ + qmi_tlv_free((struct qmi_tlv*)close_resp); +} + +int rmtfs_close_resp_set_result(struct rmtfs_close_resp *close_resp, struct rmtfs_qmi_result *val) +{ + return qmi_tlv_set((struct qmi_tlv*)close_resp, 2, val, sizeof(struct rmtfs_qmi_result)); +} + +struct rmtfs_qmi_result *rmtfs_close_resp_get_result(struct rmtfs_close_resp *close_resp) +{ + size_t len; + void *ptr; + + ptr = qmi_tlv_get((struct qmi_tlv*)close_resp, 2, &len); + if (!ptr) + return NULL; + + if (len != sizeof(struct rmtfs_qmi_result)) + return NULL; + + return ptr; +} + +struct rmtfs_iovec_req *rmtfs_iovec_req_alloc(unsigned txn) +{ + return (struct rmtfs_iovec_req*)qmi_tlv_init(txn, 3); +} + +struct rmtfs_iovec_req *rmtfs_iovec_req_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_iovec_req*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_iovec_req_encode(struct rmtfs_iovec_req *iovec_req, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)iovec_req, len); +} + +void rmtfs_iovec_req_free(struct rmtfs_iovec_req *iovec_req) +{ + qmi_tlv_free((struct qmi_tlv*)iovec_req); +} + +int rmtfs_iovec_req_set_caller_id(struct rmtfs_iovec_req *iovec_req, uint32_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)iovec_req, 1, &val, sizeof(uint32_t)); +} + +int rmtfs_iovec_req_get_caller_id(struct rmtfs_iovec_req *iovec_req, uint32_t *val) +{ + uint32_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)iovec_req, 1, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint32_t)) + return -EINVAL; + + *val = *(uint32_t*)ptr; + return 0; +} + +int rmtfs_iovec_req_set_direction(struct rmtfs_iovec_req *iovec_req, uint8_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)iovec_req, 2, &val, sizeof(uint8_t)); +} + +int rmtfs_iovec_req_get_direction(struct rmtfs_iovec_req *iovec_req, uint8_t *val) +{ + uint8_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)iovec_req, 2, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint8_t)) + return -EINVAL; + + *val = *(uint8_t*)ptr; + return 0; +} + +int rmtfs_iovec_req_set_iovec(struct rmtfs_iovec_req *iovec_req, struct rmtfs_iovec_entry *val, size_t count) +{ + return qmi_tlv_set_array((struct qmi_tlv*)iovec_req, 3, 1, val, count, sizeof(struct rmtfs_iovec_entry)); +} + +struct rmtfs_iovec_entry *rmtfs_iovec_req_get_iovec(struct rmtfs_iovec_req *iovec_req, size_t *count) +{ + size_t size; + size_t len; + void *ptr; + + ptr = qmi_tlv_get_array((struct qmi_tlv*)iovec_req, 3, 1, &len, &size); + if (!ptr) + return NULL; + + if (size != sizeof(struct rmtfs_iovec_entry)) + return NULL; + + *count = len; + return ptr; +} + +int rmtfs_iovec_req_set_is_force_sync(struct rmtfs_iovec_req *iovec_req, uint8_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)iovec_req, 4, &val, sizeof(uint8_t)); +} + +int rmtfs_iovec_req_get_is_force_sync(struct rmtfs_iovec_req *iovec_req, uint8_t *val) +{ + uint8_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)iovec_req, 4, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint8_t)) + return -EINVAL; + + *val = *(uint8_t*)ptr; + return 0; +} + +struct rmtfs_iovec_resp *rmtfs_iovec_resp_alloc(unsigned txn) +{ + return (struct rmtfs_iovec_resp*)qmi_tlv_init(txn, 3); +} + +struct rmtfs_iovec_resp *rmtfs_iovec_resp_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_iovec_resp*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_iovec_resp_encode(struct rmtfs_iovec_resp *iovec_resp, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)iovec_resp, len); +} + +void rmtfs_iovec_resp_free(struct rmtfs_iovec_resp *iovec_resp) +{ + qmi_tlv_free((struct qmi_tlv*)iovec_resp); +} + +int rmtfs_iovec_resp_set_result(struct rmtfs_iovec_resp *iovec_resp, struct rmtfs_qmi_result *val) +{ + return qmi_tlv_set((struct qmi_tlv*)iovec_resp, 2, val, sizeof(struct rmtfs_qmi_result)); +} + +struct rmtfs_qmi_result *rmtfs_iovec_resp_get_result(struct rmtfs_iovec_resp *iovec_resp) +{ + size_t len; + void *ptr; + + ptr = qmi_tlv_get((struct qmi_tlv*)iovec_resp, 2, &len); + if (!ptr) + return NULL; + + if (len != sizeof(struct rmtfs_qmi_result)) + return NULL; + + return ptr; +} + +struct rmtfs_alloc_buf_req *rmtfs_alloc_buf_req_alloc(unsigned txn) +{ + return (struct rmtfs_alloc_buf_req*)qmi_tlv_init(txn, 4); +} + +struct rmtfs_alloc_buf_req *rmtfs_alloc_buf_req_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_alloc_buf_req*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_alloc_buf_req_encode(struct rmtfs_alloc_buf_req *alloc_buf_req, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)alloc_buf_req, len); +} + +void rmtfs_alloc_buf_req_free(struct rmtfs_alloc_buf_req *alloc_buf_req) +{ + qmi_tlv_free((struct qmi_tlv*)alloc_buf_req); +} + +int rmtfs_alloc_buf_req_set_caller_id(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)alloc_buf_req, 1, &val, sizeof(uint32_t)); +} + +int rmtfs_alloc_buf_req_get_caller_id(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t *val) +{ + uint32_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)alloc_buf_req, 1, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint32_t)) + return -EINVAL; + + *val = *(uint32_t*)ptr; + return 0; +} + +int rmtfs_alloc_buf_req_set_buff_size(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)alloc_buf_req, 2, &val, sizeof(uint32_t)); +} + +int rmtfs_alloc_buf_req_get_buff_size(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t *val) +{ + uint32_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)alloc_buf_req, 2, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint32_t)) + return -EINVAL; + + *val = *(uint32_t*)ptr; + return 0; +} + +struct rmtfs_alloc_buf_resp *rmtfs_alloc_buf_resp_alloc(unsigned txn) +{ + return (struct rmtfs_alloc_buf_resp*)qmi_tlv_init(txn, 4); +} + +struct rmtfs_alloc_buf_resp *rmtfs_alloc_buf_resp_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_alloc_buf_resp*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_alloc_buf_resp_encode(struct rmtfs_alloc_buf_resp *alloc_buf_resp, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)alloc_buf_resp, len); +} + +void rmtfs_alloc_buf_resp_free(struct rmtfs_alloc_buf_resp *alloc_buf_resp) +{ + qmi_tlv_free((struct qmi_tlv*)alloc_buf_resp); +} + +int rmtfs_alloc_buf_resp_set_result(struct rmtfs_alloc_buf_resp *alloc_buf_resp, struct rmtfs_qmi_result *val) +{ + return qmi_tlv_set((struct qmi_tlv*)alloc_buf_resp, 2, val, sizeof(struct rmtfs_qmi_result)); +} + +struct rmtfs_qmi_result *rmtfs_alloc_buf_resp_get_result(struct rmtfs_alloc_buf_resp *alloc_buf_resp) +{ + size_t len; + void *ptr; + + ptr = qmi_tlv_get((struct qmi_tlv*)alloc_buf_resp, 2, &len); + if (!ptr) + return NULL; + + if (len != sizeof(struct rmtfs_qmi_result)) + return NULL; + + return ptr; +} + +int rmtfs_alloc_buf_resp_set_buff_address(struct rmtfs_alloc_buf_resp *alloc_buf_resp, uint64_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)alloc_buf_resp, 16, &val, sizeof(uint64_t)); +} + +int rmtfs_alloc_buf_resp_get_buff_address(struct rmtfs_alloc_buf_resp *alloc_buf_resp, uint64_t *val) +{ + uint64_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)alloc_buf_resp, 16, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint64_t)) + return -EINVAL; + + *val = *(uint64_t*)ptr; + return 0; +} + +struct rmtfs_dev_error_req *rmtfs_dev_error_req_alloc(unsigned txn) +{ + return (struct rmtfs_dev_error_req*)qmi_tlv_init(txn, 5); +} + +struct rmtfs_dev_error_req *rmtfs_dev_error_req_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_dev_error_req*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_dev_error_req_encode(struct rmtfs_dev_error_req *dev_error_req, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)dev_error_req, len); +} + +void rmtfs_dev_error_req_free(struct rmtfs_dev_error_req *dev_error_req) +{ + qmi_tlv_free((struct qmi_tlv*)dev_error_req); +} + +int rmtfs_dev_error_req_set_caller_id(struct rmtfs_dev_error_req *dev_error_req, uint32_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)dev_error_req, 1, &val, sizeof(uint32_t)); +} + +int rmtfs_dev_error_req_get_caller_id(struct rmtfs_dev_error_req *dev_error_req, uint32_t *val) +{ + uint32_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)dev_error_req, 1, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint32_t)) + return -EINVAL; + + *val = *(uint32_t*)ptr; + return 0; +} + +struct rmtfs_dev_error_resp *rmtfs_dev_error_resp_alloc(unsigned txn) +{ + return (struct rmtfs_dev_error_resp*)qmi_tlv_init(txn, 5); +} + +struct rmtfs_dev_error_resp *rmtfs_dev_error_resp_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_dev_error_resp*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_dev_error_resp_encode(struct rmtfs_dev_error_resp *dev_error_resp, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)dev_error_resp, len); +} + +void rmtfs_dev_error_resp_free(struct rmtfs_dev_error_resp *dev_error_resp) +{ + qmi_tlv_free((struct qmi_tlv*)dev_error_resp); +} + +int rmtfs_dev_error_resp_set_result(struct rmtfs_dev_error_resp *dev_error_resp, struct rmtfs_qmi_result *val) +{ + return qmi_tlv_set((struct qmi_tlv*)dev_error_resp, 2, val, sizeof(struct rmtfs_qmi_result)); +} + +struct rmtfs_qmi_result *rmtfs_dev_error_resp_get_result(struct rmtfs_dev_error_resp *dev_error_resp) +{ + size_t len; + void *ptr; + + ptr = qmi_tlv_get((struct qmi_tlv*)dev_error_resp, 2, &len); + if (!ptr) + return NULL; + + if (len != sizeof(struct rmtfs_qmi_result)) + return NULL; + + return ptr; +} + +int rmtfs_dev_error_resp_set_status(struct rmtfs_dev_error_resp *dev_error_resp, uint8_t val) +{ + return qmi_tlv_set((struct qmi_tlv*)dev_error_resp, 16, &val, sizeof(uint8_t)); +} + +int rmtfs_dev_error_resp_get_status(struct rmtfs_dev_error_resp *dev_error_resp, uint8_t *val) +{ + uint8_t *ptr; + size_t len; + + ptr = qmi_tlv_get((struct qmi_tlv*)dev_error_resp, 16, &len); + if (!ptr) + return -ENOENT; + + if (len != sizeof(uint8_t)) + return -EINVAL; + + *val = *(uint8_t*)ptr; + return 0; +} + +struct rmtfs_force_sync *rmtfs_force_sync_alloc(unsigned txn) +{ + return (struct rmtfs_force_sync*)qmi_tlv_init(txn, 6); +} + +struct rmtfs_force_sync *rmtfs_force_sync_parse(void *buf, size_t len, unsigned *txn) +{ + return (struct rmtfs_force_sync*)qmi_tlv_decode(buf, len, txn); +} + +void *rmtfs_force_sync_encode(struct rmtfs_force_sync *force_sync, size_t *len) +{ + return qmi_tlv_encode((struct qmi_tlv*)force_sync, len); +} + +void rmtfs_force_sync_free(struct rmtfs_force_sync *force_sync) +{ + qmi_tlv_free((struct qmi_tlv*)force_sync); +} + +int rmtfs_force_sync_set_caller_id(struct rmtfs_force_sync *force_sync, uint32_t *val, size_t count) +{ + return qmi_tlv_set_array((struct qmi_tlv*)force_sync, 1, 1, val, count, sizeof(uint32_t)); +} + +uint32_t *rmtfs_force_sync_get_caller_id(struct rmtfs_force_sync *force_sync, size_t *count) +{ + uint32_t *ptr; + size_t size; + size_t len; + + ptr = qmi_tlv_get_array((struct qmi_tlv*)force_sync, 1, 1, &len, &size); + if (!ptr) + return NULL; + + if (size != sizeof(uint32_t)) + return NULL; + + *count = len; + return ptr; +} + diff --git a/qmi_rmtfs.h b/qmi_rmtfs.h new file mode 100644 index 0000000..ce03f63 --- /dev/null +++ b/qmi_rmtfs.h @@ -0,0 +1,196 @@ +#ifndef __QMI_RMTFS_H__ +#define __QMI_RMTFS_H__ + +#include +#include + +struct qmi_tlv; + +struct qmi_tlv *qmi_tlv_init(unsigned txn, unsigned msg_id); +struct qmi_tlv *qmi_tlv_decode(void *buf, size_t len, unsigned *txn); +void *qmi_tlv_encode(struct qmi_tlv *tlv, size_t *len); +void qmi_tlv_free(struct qmi_tlv *tlv); + +void *qmi_tlv_get(struct qmi_tlv *tlv, unsigned id, size_t *len); +void *qmi_tlv_get_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, size_t *len, size_t *size); +int qmi_tlv_set(struct qmi_tlv *tlv, unsigned id, void *buf, size_t len); +int qmi_tlv_set_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, void *buf, size_t len, size_t size); + +#define QMI_RMTFS_RESULT_SUCCESS 0 +#define QMI_RMTFS_RESULT_FAILURE 1 +#define QMI_RMTFS_ERR_NONE 0 +#define QMI_RMTFS_ERR_INTERNAL 1 +#define QMI_RMTFS_ERR_MALFORMED_MSG 2 +#define QMI_RMTFS_OPEN 1 +#define QMI_RMTFS_CLOSE 2 +#define QMI_RMTFS_RW_IOVEC 3 +#define QMI_RMTFS_ALLOC_BUFF 4 +#define QMI_RMTFS_GET_DEV_ERROR 5 +#define QMI_RMTFS_FORCE_SYNC_IND 6 + +struct rmtfs_qmi_result { + uint16_t result; + uint16_t error; +}; + +struct rmtfs_iovec_entry { + uint32_t sector_addr; + uint32_t phys_offset; + uint32_t num_sector; +}; + +struct rmtfs_open_req; +struct rmtfs_open_resp; +struct rmtfs_close_req; +struct rmtfs_close_resp; +struct rmtfs_iovec_req; +struct rmtfs_iovec_resp; +struct rmtfs_alloc_buf_req; +struct rmtfs_alloc_buf_resp; +struct rmtfs_dev_error_req; +struct rmtfs_dev_error_resp; +struct rmtfs_force_sync; + +/* + * rmtfs_open_req message + */ +struct rmtfs_open_req *rmtfs_open_req_alloc(unsigned txn); +struct rmtfs_open_req *rmtfs_open_req_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_open_req_encode(struct rmtfs_open_req *open_req, size_t *len); +void rmtfs_open_req_free(struct rmtfs_open_req *open_req); + +int rmtfs_open_req_set_path(struct rmtfs_open_req *open_req, char *buf, size_t len); +int rmtfs_open_req_get_path(struct rmtfs_open_req *open_req, char *buf, size_t buflen); + +/* + * rmtfs_open_resp message + */ +struct rmtfs_open_resp *rmtfs_open_resp_alloc(unsigned txn); +struct rmtfs_open_resp *rmtfs_open_resp_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_open_resp_encode(struct rmtfs_open_resp *open_resp, size_t *len); +void rmtfs_open_resp_free(struct rmtfs_open_resp *open_resp); + +int rmtfs_open_resp_set_result(struct rmtfs_open_resp *open_resp, struct rmtfs_qmi_result *val); +struct rmtfs_qmi_result *rmtfs_open_resp_get_result(struct rmtfs_open_resp *open_resp); + +int rmtfs_open_resp_set_caller_id(struct rmtfs_open_resp *open_resp, uint32_t val); +int rmtfs_open_resp_get_caller_id(struct rmtfs_open_resp *open_resp, uint32_t *val); + +/* + * rmtfs_close_req message + */ +struct rmtfs_close_req *rmtfs_close_req_alloc(unsigned txn); +struct rmtfs_close_req *rmtfs_close_req_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_close_req_encode(struct rmtfs_close_req *close_req, size_t *len); +void rmtfs_close_req_free(struct rmtfs_close_req *close_req); + +int rmtfs_close_req_set_caller_id(struct rmtfs_close_req *close_req, uint32_t val); +int rmtfs_close_req_get_caller_id(struct rmtfs_close_req *close_req, uint32_t *val); + +/* + * rmtfs_close_resp message + */ +struct rmtfs_close_resp *rmtfs_close_resp_alloc(unsigned txn); +struct rmtfs_close_resp *rmtfs_close_resp_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_close_resp_encode(struct rmtfs_close_resp *close_resp, size_t *len); +void rmtfs_close_resp_free(struct rmtfs_close_resp *close_resp); + +int rmtfs_close_resp_set_result(struct rmtfs_close_resp *close_resp, struct rmtfs_qmi_result *val); +struct rmtfs_qmi_result *rmtfs_close_resp_get_result(struct rmtfs_close_resp *close_resp); + +/* + * rmtfs_iovec_req message + */ +struct rmtfs_iovec_req *rmtfs_iovec_req_alloc(unsigned txn); +struct rmtfs_iovec_req *rmtfs_iovec_req_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_iovec_req_encode(struct rmtfs_iovec_req *iovec_req, size_t *len); +void rmtfs_iovec_req_free(struct rmtfs_iovec_req *iovec_req); + +int rmtfs_iovec_req_set_caller_id(struct rmtfs_iovec_req *iovec_req, uint32_t val); +int rmtfs_iovec_req_get_caller_id(struct rmtfs_iovec_req *iovec_req, uint32_t *val); + +int rmtfs_iovec_req_set_direction(struct rmtfs_iovec_req *iovec_req, uint8_t val); +int rmtfs_iovec_req_get_direction(struct rmtfs_iovec_req *iovec_req, uint8_t *val); + +int rmtfs_iovec_req_set_iovec(struct rmtfs_iovec_req *iovec_req, struct rmtfs_iovec_entry *val, size_t count); +struct rmtfs_iovec_entry *rmtfs_iovec_req_get_iovec(struct rmtfs_iovec_req *iovec_req, size_t *count); + +int rmtfs_iovec_req_set_is_force_sync(struct rmtfs_iovec_req *iovec_req, uint8_t val); +int rmtfs_iovec_req_get_is_force_sync(struct rmtfs_iovec_req *iovec_req, uint8_t *val); + +/* + * rmtfs_iovec_resp message + */ +struct rmtfs_iovec_resp *rmtfs_iovec_resp_alloc(unsigned txn); +struct rmtfs_iovec_resp *rmtfs_iovec_resp_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_iovec_resp_encode(struct rmtfs_iovec_resp *iovec_resp, size_t *len); +void rmtfs_iovec_resp_free(struct rmtfs_iovec_resp *iovec_resp); + +int rmtfs_iovec_resp_set_result(struct rmtfs_iovec_resp *iovec_resp, struct rmtfs_qmi_result *val); +struct rmtfs_qmi_result *rmtfs_iovec_resp_get_result(struct rmtfs_iovec_resp *iovec_resp); + +/* + * rmtfs_alloc_buf_req message + */ +struct rmtfs_alloc_buf_req *rmtfs_alloc_buf_req_alloc(unsigned txn); +struct rmtfs_alloc_buf_req *rmtfs_alloc_buf_req_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_alloc_buf_req_encode(struct rmtfs_alloc_buf_req *alloc_buf_req, size_t *len); +void rmtfs_alloc_buf_req_free(struct rmtfs_alloc_buf_req *alloc_buf_req); + +int rmtfs_alloc_buf_req_set_caller_id(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t val); +int rmtfs_alloc_buf_req_get_caller_id(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t *val); + +int rmtfs_alloc_buf_req_set_buff_size(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t val); +int rmtfs_alloc_buf_req_get_buff_size(struct rmtfs_alloc_buf_req *alloc_buf_req, uint32_t *val); + +/* + * rmtfs_alloc_buf_resp message + */ +struct rmtfs_alloc_buf_resp *rmtfs_alloc_buf_resp_alloc(unsigned txn); +struct rmtfs_alloc_buf_resp *rmtfs_alloc_buf_resp_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_alloc_buf_resp_encode(struct rmtfs_alloc_buf_resp *alloc_buf_resp, size_t *len); +void rmtfs_alloc_buf_resp_free(struct rmtfs_alloc_buf_resp *alloc_buf_resp); + +int rmtfs_alloc_buf_resp_set_result(struct rmtfs_alloc_buf_resp *alloc_buf_resp, struct rmtfs_qmi_result *val); +struct rmtfs_qmi_result *rmtfs_alloc_buf_resp_get_result(struct rmtfs_alloc_buf_resp *alloc_buf_resp); + +int rmtfs_alloc_buf_resp_set_buff_address(struct rmtfs_alloc_buf_resp *alloc_buf_resp, uint64_t val); +int rmtfs_alloc_buf_resp_get_buff_address(struct rmtfs_alloc_buf_resp *alloc_buf_resp, uint64_t *val); + +/* + * rmtfs_dev_error_req message + */ +struct rmtfs_dev_error_req *rmtfs_dev_error_req_alloc(unsigned txn); +struct rmtfs_dev_error_req *rmtfs_dev_error_req_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_dev_error_req_encode(struct rmtfs_dev_error_req *dev_error_req, size_t *len); +void rmtfs_dev_error_req_free(struct rmtfs_dev_error_req *dev_error_req); + +int rmtfs_dev_error_req_set_caller_id(struct rmtfs_dev_error_req *dev_error_req, uint32_t val); +int rmtfs_dev_error_req_get_caller_id(struct rmtfs_dev_error_req *dev_error_req, uint32_t *val); + +/* + * rmtfs_dev_error_resp message + */ +struct rmtfs_dev_error_resp *rmtfs_dev_error_resp_alloc(unsigned txn); +struct rmtfs_dev_error_resp *rmtfs_dev_error_resp_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_dev_error_resp_encode(struct rmtfs_dev_error_resp *dev_error_resp, size_t *len); +void rmtfs_dev_error_resp_free(struct rmtfs_dev_error_resp *dev_error_resp); + +int rmtfs_dev_error_resp_set_result(struct rmtfs_dev_error_resp *dev_error_resp, struct rmtfs_qmi_result *val); +struct rmtfs_qmi_result *rmtfs_dev_error_resp_get_result(struct rmtfs_dev_error_resp *dev_error_resp); + +int rmtfs_dev_error_resp_set_status(struct rmtfs_dev_error_resp *dev_error_resp, uint8_t val); +int rmtfs_dev_error_resp_get_status(struct rmtfs_dev_error_resp *dev_error_resp, uint8_t *val); + +/* + * rmtfs_force_sync message + */ +struct rmtfs_force_sync *rmtfs_force_sync_alloc(unsigned txn); +struct rmtfs_force_sync *rmtfs_force_sync_parse(void *buf, size_t len, unsigned *txn); +void *rmtfs_force_sync_encode(struct rmtfs_force_sync *force_sync, size_t *len); +void rmtfs_force_sync_free(struct rmtfs_force_sync *force_sync); + +int rmtfs_force_sync_set_caller_id(struct rmtfs_force_sync *force_sync, uint32_t *val, size_t count); +uint32_t *rmtfs_force_sync_get_caller_id(struct rmtfs_force_sync *force_sync, size_t *count); + +#endif diff --git a/qmi_rmtfs.qmi b/qmi_rmtfs.qmi new file mode 100644 index 0000000..ace996e --- /dev/null +++ b/qmi_rmtfs.qmi @@ -0,0 +1,77 @@ +package rmtfs; + +const QMI_RMTFS_RESULT_SUCCESS = 0; +const QMI_RMTFS_RESULT_FAILURE = 1; + +const QMI_RMTFS_ERR_NONE = 0; +const QMI_RMTFS_ERR_INTERNAL = 1; +const QMI_RMTFS_ERR_MALFORMED_MSG = 2; + +const QMI_RMTFS_OPEN = 1; +const QMI_RMTFS_CLOSE = 2; +const QMI_RMTFS_RW_IOVEC = 3; +const QMI_RMTFS_ALLOC_BUFF = 4; +const QMI_RMTFS_GET_DEV_ERROR = 5; +const QMI_RMTFS_FORCE_SYNC_IND = 6; + +struct qmi_result { + u16 result; + u16 error; +}; + +struct iovec_entry { + u32 sector_addr; + u32 phys_offset; + u32 num_sector; +}; + +message open_req { + required string path = 1; +} = 1; + +message open_resp { + required qmi_result result = 2; + optional u32 caller_id = 0x10; +} = 1; + +message close_req { + required u32 caller_id = 1; +} = 2; + +message close_resp { + required qmi_result result = 2; +} = 2; + +message iovec_req { + required u32 caller_id = 1; + required u8 direction = 2; + required iovec_entry iovec[] = 3; + required u8 is_force_sync = 4; +} = 3; + +message iovec_resp { + required qmi_result result = 2; +} = 3; + +message alloc_buf_req { + required u32 caller_id = 1; + required u32 buff_size = 2; +} = 4; + +message alloc_buf_resp { + required qmi_result result = 2; + optional u64 buff_address = 0x10; +} = 4; + +message dev_error_req { + required u32 caller_id = 1; +} = 5; + +message dev_error_resp { + required qmi_result result = 2; + optional u8 status = 0x10; +} = 5; + +message force_sync { + required u32 caller_id[] = 1; +} = 6; diff --git a/qmi_tlv.c b/qmi_tlv.c new file mode 100644 index 0000000..7972993 --- /dev/null +++ b/qmi_tlv.c @@ -0,0 +1,234 @@ +#include +#include +#include +#include +#include + +#include "qmi_rmtfs.h" + +#define QMI_PKT_TYPE_REQUEST 0 +#define QMI_PKT_TYPE_RESPONSE 2 +#define QMI_PKT_TYPE_CONTROL 4 + +struct qmi_packet { + uint8_t flags; + uint16_t txn_id; + uint16_t msg_id; + uint16_t msg_len; + uint8_t data[]; +} __attribute__((__packed__)); + +struct qmi_tlv { + void *allocated; + void *buf; + size_t size; + int error; +}; + +struct qmi_tlv_item { + uint8_t key; + uint16_t len; + uint8_t data[]; +} __attribute__((__packed__)); + +struct qmi_tlv *qmi_tlv_init(unsigned txn, unsigned msg_id) +{ + struct qmi_packet *pkt; + struct qmi_tlv *tlv; + + tlv = malloc(sizeof(struct qmi_tlv)); + memset(tlv, 0, sizeof(struct qmi_tlv)); + + tlv->size = sizeof(struct qmi_packet); + tlv->allocated = malloc(tlv->size); + tlv->buf = tlv->allocated; + + pkt = tlv->buf; + pkt->flags = QMI_PKT_TYPE_RESPONSE; + pkt->txn_id = txn; + pkt->msg_id = msg_id; + pkt->msg_len = 0; + + return tlv; +} + +struct qmi_tlv *qmi_tlv_decode(void *buf, size_t len, unsigned *txn) +{ + struct qmi_packet *pkt = buf; + struct qmi_tlv *tlv; + + tlv = malloc(sizeof(struct qmi_tlv)); + memset(tlv, 0, sizeof(struct qmi_tlv)); + + tlv->buf = buf; + tlv->size = len; + + if (txn) + *txn = pkt->txn_id; + + return tlv; +} + +void *qmi_tlv_encode(struct qmi_tlv *tlv, size_t *len) +{ + + struct qmi_packet *pkt; + + if (!tlv || tlv->error) + return NULL; + + pkt = tlv->buf; + pkt->msg_len = tlv->size - sizeof(struct qmi_packet); + + *len = tlv->size; + return tlv->buf; +} + +void qmi_tlv_free(struct qmi_tlv *tlv) +{ + free(tlv->allocated); + free(tlv); +} + +static struct qmi_tlv_item *qmi_tlv_get_item(struct qmi_tlv *tlv, unsigned id) +{ + struct qmi_tlv_item *item; + struct qmi_packet *pkt; + unsigned offset = 0; + void *pkt_data; + + pkt = tlv->buf; + pkt_data = pkt->data; + + while (offset < tlv->size) { + item = pkt_data + offset; + if (item->key == id) + return pkt_data + offset; + + offset += sizeof(struct qmi_tlv_item) + item->len; + } + return NULL; +} + +void *qmi_tlv_get(struct qmi_tlv *tlv, unsigned id, size_t *len) +{ + struct qmi_tlv_item *item; + + item = qmi_tlv_get_item(tlv, id); + if (!item) + return NULL; + + *len = item->len; + return item->data; +} + +void *qmi_tlv_get_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, size_t *len, size_t *size) +{ + struct qmi_tlv_item *item; + unsigned count; + void *ptr; + + item = qmi_tlv_get_item(tlv, id); + if (!item) + return NULL; + + ptr = item->data; + switch (len_size) { + case 4: + count = *(uint32_t*)ptr++; + break; + case 2: + count = *(uint16_t*)ptr++; + break; + case 1: + count = *(uint8_t*)ptr++; + break; + } + + *len = count; + *size = (item->len - len_size) / count; + + return ptr; +} + +static struct qmi_tlv_item *qmi_tlv_alloc_item(struct qmi_tlv *tlv, unsigned id, size_t len) +{ + struct qmi_tlv_item *item; + size_t new_size; + bool migrate; + void *newp; + + /* If using user provided buffer, migrate data */ + migrate = !tlv->allocated; + + new_size = tlv->size + sizeof(struct qmi_tlv_item) + len; + newp = realloc(tlv->allocated, new_size); + if (!newp) + return NULL; + + if (migrate) + memcpy(newp, tlv->buf, tlv->size); + + item = newp + tlv->size; + item->key = id; + item->len = len; + + tlv->buf = tlv->allocated = newp; + tlv->size = new_size; + + return item; +} + +int qmi_tlv_set(struct qmi_tlv *tlv, unsigned id, void *buf, size_t len) +{ + struct qmi_tlv_item *item; + + if (!tlv) + return -EINVAL; + + item = qmi_tlv_alloc_item(tlv, id, len); + if (!item) { + tlv->error = ENOMEM; + return -ENOMEM; + } + + memcpy(item->data, buf, len); + + return 0; +} + +int qmi_tlv_set_array(struct qmi_tlv *tlv, unsigned id, unsigned len_size, void *buf, size_t len, size_t size) +{ + struct qmi_tlv_item *item; + size_t array_size; + void *ptr; + + if (!tlv) + return -EINVAL; + + array_size = len * size; + item = qmi_tlv_alloc_item(tlv, id, len_size + array_size); + if (!item) { + tlv->error = ENOMEM; + return -ENOMEM; + } + + ptr = item->data; + + switch (len_size) { + case 4: + *(uint32_t*)ptr++ = len; + break; + case 2: + *(uint16_t*)ptr++ = len; + break; + case 1: + *(uint8_t*)ptr++ = len; + break; + } + memcpy(ptr, buf, array_size); + + return 0; +} + + diff --git a/rmtfs.c b/rmtfs.c new file mode 100644 index 0000000..c95cf8f --- /dev/null +++ b/rmtfs.c @@ -0,0 +1,511 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "qmi_rmtfs.h" +#include "util.h" +#include "rmtfs.h" + +#define RFSA_QMI_SERVICE 28 +#define RFSA_QMI_VERSION 1 +#define RFSA_QMI_INSTANCE 0 + +#define RMTFS_QMI_SERVICE 14 +#define RMTFS_QMI_VERSION 1 +#define RMTFS_QMI_INSTANCE 0 + +#define SECTOR_SIZE 512 + +/* TODO: include from kernel once it lands */ +struct sockaddr_qrtr { + unsigned short sq_family; + uint32_t sq_node; + uint32_t sq_port; +}; + +static bool dbgprintf_enabled; +static void dbgprintf(const char *fmt, ...) +{ + va_list ap; + + if (!dbgprintf_enabled) + return; + + va_start(ap, fmt); + vprintf(fmt, ap); + va_end(ap); +} + +static void qmi_result_error(struct rmtfs_qmi_result *result, unsigned error) +{ + /* Only propagate initial error */ + if (result->result == QMI_RMTFS_RESULT_FAILURE) + return; + + result->result = QMI_RMTFS_RESULT_FAILURE; + result->error = error; +} + +static void rmtfs_open(int sock, unsigned node, unsigned port, void *msg, size_t msg_len) +{ + struct rmtfs_qmi_result result = {}; + struct rmtfs_open_resp *resp; + struct rmtfs_open_req *req; + int caller_id; + unsigned txn; + size_t len; + void *ptr; + char path[256]; + int ret; + + req = rmtfs_open_req_parse(msg, msg_len, &txn); + if (!req) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_open_req_get_path(req, path, sizeof(path)); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + caller_id = storage_get(node, path); + if (caller_id < 0) + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + +respond: + dbgprintf("[RMTFS] open %s => %d (%d:%d)\n", path, caller_id, result.result, result.error); + + resp = rmtfs_open_resp_alloc(txn); + rmtfs_open_resp_set_result(resp, &result); + rmtfs_open_resp_set_caller_id(resp, caller_id); + ptr = rmtfs_open_resp_encode(resp, &len); + if (!ptr) + goto free_resp; + + ret = qrtr_sendto(sock, node, port, ptr, len); + if (ret < 0) + fprintf(stderr, "[RMTFS] failed to send open-response: %s\n", strerror(-ret)); + +free_resp: + rmtfs_open_resp_free(resp); + rmtfs_open_req_free(req); +} + +static void rmtfs_close(int sock, unsigned node, unsigned port, void *msg, size_t msg_len) +{ + struct rmtfs_qmi_result result = {}; + struct rmtfs_close_resp *resp; + struct rmtfs_close_req *req; + uint32_t caller_id; + unsigned txn; + size_t len; + void *ptr; + int ret; + + req = rmtfs_close_req_parse(msg, msg_len, &txn); + if (!req) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_close_req_get_caller_id(req, &caller_id); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = storage_put(node, caller_id); + if (ret < 0) + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + + rmtfs_mem_free(); + +respond: + dbgprintf("[RMTFS] close %d => (%d:%d)\n", caller_id, result.result, result.error); + + resp = rmtfs_close_resp_alloc(txn); + rmtfs_close_resp_set_result(resp, &result); + ptr = rmtfs_close_resp_encode(resp, &len); + if (!ptr) + goto free_resp; + + ret = qrtr_sendto(sock, node, port, ptr, len); + if (ret < 0) + fprintf(stderr, "[RMTFS] failed to send close-response: %s\n", strerror(-ret)); + +free_resp: + rmtfs_close_resp_free(resp); + rmtfs_close_req_free(req); +} + +static void rmtfs_iovec(int sock, unsigned node, unsigned port, void *msg, size_t msg_len) +{ + struct rmtfs_iovec_entry *entries; + struct rmtfs_qmi_result result = {}; + struct rmtfs_iovec_resp *resp; + struct rmtfs_iovec_req *req; + uint32_t caller_id; + size_t num_entries; + uint8_t is_write; + uint8_t force; + unsigned txn; + ssize_t n; + size_t len; + void *ptr; + int ret; + int fd; + int i; + int j; + + req = rmtfs_iovec_req_parse(msg, msg_len, &txn); + if (!req) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_iovec_req_get_caller_id(req, &caller_id); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_iovec_req_get_direction(req, &is_write); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + entries = rmtfs_iovec_req_get_iovec(req, &num_entries); + if (!entries) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_iovec_req_get_is_force_sync(req, &force); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + fd = storage_get_handle(node, caller_id); + if (fd < 0) { + fprintf(stderr, "[RMTFS] iovec request for non-existing caller\n"); + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + goto respond; + } + + for (i = 0; i < num_entries; i++) { + ptr = rmtfs_mem_ptr(entries[i].phys_offset, entries[i].num_sector * SECTOR_SIZE); + if (!ptr) { + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + goto respond; + } + + n = lseek(fd, entries[i].sector_addr * SECTOR_SIZE, SEEK_SET); + if (n < 0) { + fprintf(stderr, "[RMTFS] failed to seek sector %d\n", entries[i].sector_addr); + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + goto respond; + } + + for (j = 0; j < entries[i].num_sector; j++) { + if (is_write) + n = write(fd, ptr, SECTOR_SIZE); + else + n = read(fd, ptr, SECTOR_SIZE); + + if (n != SECTOR_SIZE) { + fprintf(stderr, "[RMTFS] failed to %s sector %d\n", + is_write ? "write" : "read", entries[i].sector_addr + j); + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + goto respond; + } + + ptr += SECTOR_SIZE; + } + } + +respond: + dbgprintf("[RMTFS] iovec %d, %sforced => (%d:%d)\n", caller_id, force ? "" : "not ", + result.result, result.error); + for (i = 0; i < num_entries; i++) { + dbgprintf("[RMTFS] %s %d:%d 0x%x\n", is_write ? "write" : "read", + entries[i].sector_addr, + entries[i].num_sector, + entries[i].phys_offset); + } + + resp = rmtfs_iovec_resp_alloc(txn); + rmtfs_iovec_resp_set_result(resp, &result); + ptr = rmtfs_iovec_resp_encode(resp, &len); + if (!ptr) + goto free_resp; + + ret = qrtr_sendto(sock, node, port, ptr, len); + if (ret < 0) + fprintf(stderr, "[RMTFS] failed to send iovec-response: %s\n", strerror(-ret)); + +free_resp: + rmtfs_iovec_resp_free(resp); + rmtfs_iovec_req_free(req); +} + +static void rmtfs_alloc_buf(int sock, unsigned node, unsigned port, void *msg, size_t msg_len) +{ + struct rmtfs_alloc_buf_resp *resp; + struct rmtfs_alloc_buf_req *req; + struct rmtfs_qmi_result result = {}; + uint32_t alloc_size; + uint32_t caller_id; + int64_t address; + unsigned txn; + size_t len; + void *ptr; + int ret; + + req = rmtfs_alloc_buf_req_parse(msg, msg_len, &txn); + if (!req) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_alloc_buf_req_get_caller_id(req, &caller_id); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_alloc_buf_req_get_buff_size(req, &alloc_size); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + address = rmtfs_mem_alloc(alloc_size); + if (address < 0) + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + +respond: + dbgprintf("[RMTFS] alloc %d, %d => 0x%lx (%d:%d)\n", caller_id, alloc_size, address, result.result, result.error); + + resp = rmtfs_alloc_buf_resp_alloc(txn); + rmtfs_alloc_buf_resp_set_result(resp, &result); + rmtfs_alloc_buf_resp_set_buff_address(resp, address); + ptr = rmtfs_alloc_buf_resp_encode(resp, &len); + if (!ptr) + goto free_resp; + + ret = qrtr_sendto(sock, node, port, ptr, len); + if (ret < 0) + fprintf(stderr, "[RMTFS] failed to send alloc-response: %s\n", strerror(-ret)); + +free_resp: + rmtfs_alloc_buf_resp_free(resp); + rmtfs_alloc_buf_req_free(req); +} + +static void rmtfs_get_dev_error(int sock, unsigned node, unsigned port, void *msg, size_t msg_len) +{ + struct rmtfs_dev_error_resp *resp; + struct rmtfs_dev_error_req *req; + struct rmtfs_qmi_result result = {}; + uint32_t caller_id; + int dev_error; + unsigned txn; + size_t len; + void *ptr; + int ret; + + req = rmtfs_dev_error_req_parse(msg, msg_len, &txn); + if (!req) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + ret = rmtfs_dev_error_req_get_caller_id(req, &caller_id); + if (ret < 0) { + qmi_result_error(&result, QMI_RMTFS_ERR_MALFORMED_MSG); + goto respond; + } + + dev_error = storage_get_error(node, caller_id); + if (dev_error < 0) + qmi_result_error(&result, QMI_RMTFS_ERR_INTERNAL); + +respond: + dbgprintf("[RMTFS] dev_error %d => %d (%d:%d)\n", caller_id, dev_error, result.result, result.error); + + resp = rmtfs_dev_error_resp_alloc(txn); + rmtfs_dev_error_resp_set_result(resp, &result); + rmtfs_dev_error_resp_set_status(resp, dev_error); + ptr = rmtfs_dev_error_resp_encode(resp, &len); + if (!ptr) + goto free_resp; + + ret = qrtr_sendto(sock, node, port, ptr, len); + if (ret < 0) + fprintf(stderr, "[RMTFS] failed to send error-response: %s\n", strerror(-ret)); + +free_resp: + rmtfs_dev_error_resp_free(resp); + rmtfs_dev_error_req_free(req); +} + +static int handle_rfsa(int sock) +{ + struct sockaddr_qrtr sq; + socklen_t sl; + char buf[4096]; + int ret; + + sl = sizeof(sq); + ret = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl); + if (ret < 0) { + fprintf(stderr, "[RMTFS] recvfrom failed: %d\n", ret); + return ret; + } + + printf("[RFSA] packet; from: %d:%d\n", sq.sq_node, sq.sq_port); + print_hex_dump("[RFSA <-]", buf, ret); + + return 0; +} + +static int handle_rmtfs(int sock) +{ + struct sockaddr_qrtr sq; + struct qmi_packet *qmi; + socklen_t sl; + char buf[4096]; + int ret; + + sl = sizeof(sq); + ret = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl); + if (ret < 0) { + fprintf(stderr, "[RMTFS] recvfrom failed: %d\n", ret); + return ret; + } + + dbgprintf("[RMTFS] packet; from: %d:%d\n", sq.sq_node, sq.sq_port); + + qmi = (struct qmi_packet*)buf; + if (qmi->msg_len != ret - sizeof(struct qmi_packet)) { + fprintf(stderr, "[RMTFS] Invalid length of incoming qmi request\n"); + return -EINVAL; + } + + switch (qmi->msg_id) { + case QMI_RMTFS_OPEN: + rmtfs_open(sock, sq.sq_node, sq.sq_port, qmi, qmi->msg_len); + break; + case QMI_RMTFS_CLOSE: + rmtfs_close(sock, sq.sq_node, sq.sq_port, qmi, qmi->msg_len); + break; + case QMI_RMTFS_RW_IOVEC: + rmtfs_iovec(sock, sq.sq_node, sq.sq_port, qmi, qmi->msg_len); + break; + case QMI_RMTFS_ALLOC_BUFF: + rmtfs_alloc_buf(sock, sq.sq_node, sq.sq_port, qmi, qmi->msg_len); + break; + case QMI_RMTFS_GET_DEV_ERROR: + rmtfs_get_dev_error(sock, sq.sq_node, sq.sq_port, qmi, qmi->msg_len); + break; + default: + fprintf(stderr, "[RMTFS] Unknown request: %d\n", qmi->msg_id); + ret = -EINVAL; + break; + } + + return ret; +} + +int main(int argc, char **argv) +{ + int rmtfs_fd; + int rfsa_fd; + fd_set rfds; + int nfds; + int ret; + + if (argc == 2 && strcmp(argv[1], "-v") == 0) + dbgprintf_enabled = true; + + ret = rmtfs_mem_open(); + if (ret) { + fprintf(stderr, "failed to initialize rmtfs shared memory"); + return 1; + } + + ret = storage_open(); + if (ret) { + fprintf(stderr, "failed to initialize storage system"); + goto close_rmtfs_mem; + } + + rfsa_fd = qrtr_open(0); + if (rfsa_fd < 0) { + fprintf(stderr, "failed to create qrtr socket"); + goto close_storage; + } + + rmtfs_fd = qrtr_open(0); + if (rmtfs_fd < 0) { + fprintf(stderr, "failed to create qrtr socket"); + goto close_storage; + } + + ret = qrtr_publish(rfsa_fd, RFSA_QMI_SERVICE, RFSA_QMI_VERSION, RFSA_QMI_INSTANCE); + if (ret < 0) { + fprintf(stderr, "failed to publish rfsa service"); + goto close_storage; + } + + ret = qrtr_publish(rmtfs_fd, RMTFS_QMI_SERVICE, RMTFS_QMI_VERSION, RMTFS_QMI_INSTANCE); + if (ret < 0) { + fprintf(stderr, "failed to publish misc ta service"); + goto unpublish_rfsa; + } + + for (;;) { + FD_ZERO(&rfds); + FD_SET(rfsa_fd, &rfds); + FD_SET(rmtfs_fd, &rfds); + + nfds = MAX(rfsa_fd, rmtfs_fd) + 1; + ret = select(nfds, &rfds, NULL, NULL, NULL); + if (ret < 0) { + fprintf(stderr, "select failed: %d\n", ret); + break; + } else if (ret == 0) { + continue; + } + + if (FD_ISSET(rfsa_fd, &rfds)) + handle_rfsa(rfsa_fd); + else if (FD_ISSET(rmtfs_fd, &rfds)) + handle_rmtfs(rmtfs_fd); + } + + qrtr_bye(rmtfs_fd, RMTFS_QMI_SERVICE, RMTFS_QMI_VERSION, RMTFS_QMI_INSTANCE); +unpublish_rfsa: + qrtr_bye(rfsa_fd, RFSA_QMI_SERVICE, RFSA_QMI_VERSION, RFSA_QMI_INSTANCE); +close_storage: + storage_close(); +close_rmtfs_mem: + rmtfs_mem_close(); + + return 0; +} diff --git a/rmtfs.h b/rmtfs.h new file mode 100644 index 0000000..c317b1d --- /dev/null +++ b/rmtfs.h @@ -0,0 +1,28 @@ +#ifndef __RMTFS_H__ +#define __RMTFS_H__ + +#include +#include "qmi_rmtfs.h" + +struct qmi_packet { + uint8_t flags; + uint16_t txn_id; + uint16_t msg_id; + uint16_t msg_len; + uint8_t data[]; +} __attribute__((__packed__)); + +int rmtfs_mem_open(void); +void rmtfs_mem_close(void); +int64_t rmtfs_mem_alloc(size_t size); +void *rmtfs_mem_ptr(unsigned phys_address, size_t len); +void rmtfs_mem_free(void); + +int storage_open(void); +int storage_get(unsigned node, const char *path); +int storage_put(unsigned node, int caller_id); +int storage_get_handle(unsigned node, int caller_id); +int storage_get_error(unsigned node, int caller_id); +void storage_close(void); + +#endif diff --git a/sharedmem.c b/sharedmem.c new file mode 100644 index 0000000..bc89d01 --- /dev/null +++ b/sharedmem.c @@ -0,0 +1,88 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include "rmtfs.h" + +#define SHAREDMEM_BASE 0x0fd80000 +#define SHAREDMEM_SIZE 0x180000 + +static uint64_t rmtfs_mem_address = SHAREDMEM_BASE; +static uint64_t rmtfs_mem_size = SHAREDMEM_SIZE; +static void *rmtfs_mem_base; +static bool rmtfs_mem_busy; +static int rmtfs_mem_fd; + +int rmtfs_mem_open(void) +{ + void *base; + int fd; + + fd = open("/dev/mem", O_RDWR|O_SYNC); + if (fd < 0) { + fprintf(stderr, "failed to open /dev/mem\n"); + return fd; + } + + base = mmap(0, SHAREDMEM_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, fd, SHAREDMEM_BASE); + if (base == MAP_FAILED) { + fprintf(stderr, "failed to mmap: %s\n", strerror(errno)); + return -errno; + } + + rmtfs_mem_base = base; + rmtfs_mem_fd = fd; + + return 0; +} + +int64_t rmtfs_mem_alloc(size_t alloc_size) +{ + if (rmtfs_mem_busy) { + fprintf(stderr, "[RMTFS] rmtfs shared memory already allocated\n"); + return -EBUSY; + } + + if (alloc_size > rmtfs_mem_size) { + fprintf(stderr, + "[RMTFS] rmtfs shared memory not large enough for allocation request 0x%zx vs 0x%lx\n", + alloc_size, rmtfs_mem_size); + return -EINVAL; + } + + rmtfs_mem_busy = true; + + return rmtfs_mem_address; +} + +void rmtfs_mem_free(void) +{ + rmtfs_mem_busy = false; +} + +void *rmtfs_mem_ptr(unsigned phys_address, size_t len) +{ + uint64_t start; + uint64_t end; + + start = phys_address; + end = start + len; + + if (start < rmtfs_mem_address || end > rmtfs_mem_address + rmtfs_mem_size) + return NULL; + + return rmtfs_mem_base + phys_address - rmtfs_mem_address; +} + +void rmtfs_mem_close(void) +{ + munmap(rmtfs_mem_base, rmtfs_mem_size); + close(rmtfs_mem_fd); + + rmtfs_mem_fd = -1; + rmtfs_mem_base = MAP_FAILED; +} diff --git a/storage.c b/storage.c new file mode 100644 index 0000000..2311e22 --- /dev/null +++ b/storage.c @@ -0,0 +1,140 @@ +#include +#include +#include +#include +#include +#include +#include +#include "rmtfs.h" + +#define MAX_CALLERS 10 + +struct partition { + const char *path; + const char *actual; +}; + +struct caller { + unsigned id; + unsigned node; + int fd; + unsigned dev_error; +}; + +static const struct partition partition_table[] = { + { "/boot/modem_fs1", "/boot/modem_fs1" }, + { "/boot/modem_fs2", "/boot/modem_fs2" }, + { "/boot/modem_fsg", "/boot/modem_fsg" }, + {} +}; + +static struct caller caller_handles[MAX_CALLERS]; + +int storage_open(void) +{ + int i; + + for (i = 0; i < MAX_CALLERS; i++) { + caller_handles[i].id = i; + caller_handles[i].fd = -1; + } + + return 0; +} + +int storage_get(unsigned node, const char *path) +{ + const struct partition *part; + struct caller *caller; + int saved_errno; + int fd; + int i; + + for (part = partition_table; part->path; part++) { + if (strcmp(part->path, path) == 0) + goto found; + } + + fprintf(stderr, "[RMTFS storage] request for unknown partition '%s', rejecting\n", path); + return -EPERM; + +found: + for (i = 0; i < MAX_CALLERS; i++) { + if (caller_handles[i].fd == -1) { + caller = &caller_handles[i]; + break; + } + } + if (!caller) { + fprintf(stderr, "[storage] out of free caller handles\n"); + return -EBUSY; + } + + fd = open(part->actual, O_RDWR); + if (fd < 0) { + saved_errno = errno; + fprintf(stderr, "[storage] failed to open '%s' (requested '%s'): %s\n", + part->actual, part->path, strerror(-errno)); + return -saved_errno; + } + + caller->node = node; + caller->fd = fd; + + return caller->id; +} + +int storage_put(unsigned node, int caller_id) +{ + struct caller *caller; + + if (caller_id >= MAX_CALLERS) + return -EINVAL; + + caller = &caller_handles[caller_id]; + if (caller->node != node) + return -EINVAL; + + close(caller->fd); + caller->fd = -1; + return 0; +} + +int storage_get_handle(unsigned node, int caller_id) +{ + struct caller *caller; + + if (caller_id >= MAX_CALLERS) + return -EINVAL; + + caller = &caller_handles[caller_id]; + if (caller->node != node) + return -EINVAL; + + return caller->fd; +} + +int storage_get_error(unsigned node, int caller_id) +{ + struct caller *caller; + + if (caller_id >= MAX_CALLERS) + return -EINVAL; + + caller = &caller_handles[caller_id]; + if (caller->node != node) + return -EINVAL; + + return caller->dev_error; +} + +void storage_close(void) +{ + int i; + + for (i = 0; i < MAX_CALLERS; i++) { + if (caller_handles[i].fd >= 0) + close(caller_handles[i].fd); + } +} + diff --git a/util.c b/util.c new file mode 100644 index 0000000..5ca5bba --- /dev/null +++ b/util.c @@ -0,0 +1,48 @@ +#include +#include +#include +#include "util.h" + +static uint8_t to_hex(uint8_t ch) +{ + ch &= 0xf; + return ch <= 9 ? '0' + ch : 'a' + ch - 10; +} + +void print_hex_dump(const char *prefix, const void *buf, size_t len) +{ + const uint8_t *ptr = buf; + size_t linelen; + uint8_t ch; + char line[16 * 3 + 16 + 1]; + int li; + int i; + int j; + + for (i = 0; i < len; i += 16) { + linelen = MIN(16, len - i); + li = 0; + + for (j = 0; j < linelen; j++) { + ch = ptr[i + j]; + line[li++] = to_hex(ch >> 4); + line[li++] = to_hex(ch); + line[li++] = ' '; + } + + for (; j < 16; j++) { + line[li++] = ' '; + line[li++] = ' '; + line[li++] = ' '; + } + + for (j = 0; j < linelen; j++) { + ch = ptr[i + j]; + line[li++] = isprint(ch) ? ch : '.'; + } + + line[li] = '\0'; + + printf("%s %04x: %s\n", prefix, i, line); + } +} diff --git a/util.h b/util.h new file mode 100644 index 0000000..3a9ec3e --- /dev/null +++ b/util.h @@ -0,0 +1,9 @@ +#ifndef __UTIL_H__ +#define __UTIL_H__ + +#define MIN(x, y) ((x) < (y) ? (x) : (y)) +#define MAX(x, y) ((x) > (y) ? (x) : (y)) + +void print_hex_dump(const char *prefix, const void *buf, size_t len); + +#endif