| /* |
| * Licensed to the Apache Software Foundation (ASF) under one |
| * or more contributor license agreements. See the NOTICE file |
| * distributed with this work for additional information |
| * regarding copyright ownership. The ASF licenses this file |
| * to you under the Apache License, Version 2.0 (the |
| * "License"); you may not use this file except in compliance |
| * with the License. You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, |
| * software distributed under the License is distributed on an |
| * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY |
| * KIND, either express or implied. See the License for the |
| * specific language governing permissions and limitations |
| * under the License. |
| */ |
| |
| #include <string.h> |
| #include "os/mynewt.h" |
| #include "cbmem/cbmem.h" |
| |
| typedef void (copy_data_func_t) (void *dst, const void *data, uint16_t len); |
| |
| int |
| cbmem_init(struct cbmem *cbmem, void *buf, uint32_t buf_len) |
| { |
| os_mutex_init(&cbmem->c_lock); |
| |
| memset(cbmem, 0, sizeof(*cbmem)); |
| cbmem->c_buf = buf; |
| cbmem->c_buf_end = buf + buf_len; |
| |
| return (0); |
| } |
| |
| int |
| cbmem_lock_acquire(struct cbmem *cbmem) |
| { |
| int rc; |
| |
| if (!os_started()) { |
| return (0); |
| } |
| |
| rc = os_mutex_pend(&cbmem->c_lock, OS_WAIT_FOREVER); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| return (0); |
| err: |
| return (rc); |
| } |
| |
| int |
| cbmem_lock_release(struct cbmem *cbmem) |
| { |
| int rc; |
| |
| if (!os_started()) { |
| return (0); |
| } |
| |
| rc = os_mutex_release(&cbmem->c_lock); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| return (0); |
| err: |
| return (rc); |
| } |
| |
| |
| static int |
| cbmem_append_internal(struct cbmem *cbmem, const void *data, uint16_t len, |
| copy_data_func_t *copy_func) |
| { |
| struct cbmem_entry_hdr *dst; |
| uint8_t *start; |
| uint8_t *end; |
| int rc; |
| |
| rc = cbmem_lock_acquire(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| if (cbmem->c_entry_end) { |
| dst = CBMEM_ENTRY_NEXT(cbmem->c_entry_end); |
| } else { |
| dst = (struct cbmem_entry_hdr *) cbmem->c_buf; |
| } |
| end = (uint8_t *) dst + len + sizeof(*dst); |
| |
| /* If this item would take us past the end of this buffer, then adjust |
| * the item to the beginning of the buffer. |
| */ |
| if (end > cbmem->c_buf_end) { |
| cbmem->c_buf_cur_end = (uint8_t *) dst; |
| dst = (struct cbmem_entry_hdr *) cbmem->c_buf; |
| end = (uint8_t *) dst + len + sizeof(*dst); |
| if ((uint8_t *) cbmem->c_entry_start >= cbmem->c_buf_cur_end) { |
| cbmem->c_entry_start = (struct cbmem_entry_hdr *) cbmem->c_buf; |
| } |
| } |
| |
| /* If the destination is prior to the start, and would overrwrite the |
| * start of the buffer, move start forward until you don't overwrite it |
| * anymore. |
| */ |
| start = (uint8_t *) cbmem->c_entry_start; |
| if (start && (uint8_t *) dst < start + CBMEM_ENTRY_SIZE(start) && |
| end > start) { |
| while (start < end) { |
| start = (uint8_t *) CBMEM_ENTRY_NEXT(start); |
| if (start == cbmem->c_buf_cur_end) { |
| start = cbmem->c_buf; |
| break; |
| } |
| } |
| cbmem->c_entry_start = (struct cbmem_entry_hdr *) start; |
| } |
| |
| /* Copy the entry into the log |
| */ |
| dst->ceh_len = len; |
| copy_func((uint8_t *) dst + sizeof(*dst), data, len); |
| |
| cbmem->c_entry_end = dst; |
| if (!cbmem->c_entry_start) { |
| cbmem->c_entry_start = dst; |
| } |
| |
| rc = cbmem_lock_release(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| return (0); |
| err: |
| return (-1); |
| } |
| |
| static void |
| copy_data_from_flat(void *dst, const void *data, uint16_t len) |
| { |
| memcpy(dst, data, len); |
| } |
| |
| static void |
| copy_data_from_mbuf(void *dst, const void *data, uint16_t len) |
| { |
| const struct os_mbuf *om = data; |
| |
| os_mbuf_copydata(om, 0, len, dst); |
| } |
| |
| static uint16_t |
| cbmem_scat_gath_entry_len(const struct cbmem_scat_gath_entry *entry) |
| { |
| if (entry->om != NULL) { |
| return os_mbuf_len(entry->om); |
| } else { |
| return entry->flat_len; |
| } |
| } |
| |
| static void |
| copy_data_from_scat_gath(void *dst, const void *data, uint16_t len) |
| { |
| const struct cbmem_scat_gath_entry *entry; |
| const struct cbmem_scat_gath *sg; |
| uint8_t *u8p; |
| uint16_t entry_len; |
| int i; |
| |
| u8p = dst; |
| |
| sg = data; |
| for (i = 0; i < sg->count; i++) { |
| entry = sg->entries + i; |
| |
| entry_len = cbmem_scat_gath_entry_len(entry); |
| if (entry->om != NULL) { |
| os_mbuf_copydata(entry->om, 0, entry_len, u8p); |
| } else { |
| memcpy(u8p, entry->flat_buf, entry_len); |
| } |
| |
| u8p += entry_len; |
| } |
| } |
| |
| int |
| cbmem_append(struct cbmem *cbmem, void *data, uint16_t len) |
| { |
| return cbmem_append_internal(cbmem, data, len, copy_data_from_flat); |
| } |
| |
| int |
| cbmem_append_mbuf(struct cbmem *cbmem, const struct os_mbuf *om) |
| { |
| return cbmem_append_internal(cbmem, om, os_mbuf_len(om), |
| copy_data_from_mbuf); |
| } |
| |
| int |
| cbmem_append_scat_gath(struct cbmem *cbmem, const struct cbmem_scat_gath *sg) |
| { |
| const struct cbmem_scat_gath_entry *entry; |
| uint16_t len; |
| int i; |
| |
| len = 0; |
| for (i = 0; i < sg->count; i++) { |
| entry = sg->entries + i; |
| len += cbmem_scat_gath_entry_len(entry); |
| } |
| |
| return cbmem_append_internal(cbmem, sg, len, copy_data_from_scat_gath); |
| } |
| |
| void |
| cbmem_iter_start(struct cbmem *cbmem, struct cbmem_iter *iter) |
| { |
| iter->ci_start = cbmem->c_entry_start; |
| iter->ci_cur = cbmem->c_entry_start; |
| iter->ci_end = cbmem->c_entry_end; |
| } |
| |
| struct cbmem_entry_hdr * |
| cbmem_iter_next(struct cbmem *cbmem, struct cbmem_iter *iter) |
| { |
| struct cbmem_entry_hdr *hdr; |
| |
| if (iter->ci_start > iter->ci_end) { |
| hdr = iter->ci_cur; |
| iter->ci_cur = CBMEM_ENTRY_NEXT(iter->ci_cur); |
| |
| if ((uint8_t *) iter->ci_cur >= cbmem->c_buf_cur_end) { |
| iter->ci_cur = (struct cbmem_entry_hdr *) cbmem->c_buf; |
| iter->ci_start = (struct cbmem_entry_hdr *) cbmem->c_buf; |
| } |
| } else { |
| hdr = iter->ci_cur; |
| if (!iter->ci_cur) { |
| goto err; |
| } |
| |
| if (hdr == CBMEM_ENTRY_NEXT(iter->ci_end)) { |
| hdr = NULL; |
| } else { |
| iter->ci_cur = CBMEM_ENTRY_NEXT(iter->ci_cur); |
| } |
| } |
| |
| err: |
| return (hdr); |
| } |
| |
| int |
| cbmem_flush(struct cbmem *cbmem) |
| { |
| int rc; |
| |
| rc = cbmem_lock_acquire(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| cbmem->c_entry_start = NULL; |
| cbmem->c_entry_end = NULL; |
| cbmem->c_buf_cur_end = NULL; |
| |
| rc = cbmem_lock_release(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| return (0); |
| err: |
| return (rc); |
| } |
| |
| int |
| cbmem_read(struct cbmem *cbmem, struct cbmem_entry_hdr *hdr, void *buf, |
| uint16_t off, uint16_t len) |
| { |
| int rc; |
| |
| rc = cbmem_lock_acquire(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| /* Only read the maximum number of bytes, if we exceed that, |
| * truncate the read. |
| */ |
| if (off + len > hdr->ceh_len) { |
| len = hdr->ceh_len - off; |
| } |
| |
| if (off > hdr->ceh_len) { |
| rc = -1; |
| cbmem_lock_release(cbmem); |
| goto err; |
| } |
| |
| memcpy(buf, (uint8_t *) hdr + sizeof(*hdr) + off, len); |
| |
| rc = cbmem_lock_release(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| return (len); |
| err: |
| return (-1); |
| } |
| |
| int cbmem_read_mbuf(struct cbmem *cbmem, struct cbmem_entry_hdr *hdr, |
| struct os_mbuf *om, uint16_t off, uint16_t len) |
| { |
| int rc; |
| |
| rc = cbmem_lock_acquire(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| /* Only read the maximum number of bytes, if we exceed that, |
| * truncate the read. |
| */ |
| if (off + len > hdr->ceh_len) { |
| len = hdr->ceh_len - off; |
| } |
| |
| if (off > hdr->ceh_len) { |
| rc = -1; |
| cbmem_lock_release(cbmem); |
| goto err; |
| } |
| |
| rc = os_mbuf_append(om, (uint8_t *) hdr + sizeof(*hdr) + off, len); |
| if (rc != 0) { |
| cbmem_lock_release(cbmem); |
| goto err; |
| } |
| |
| cbmem_lock_release(cbmem); |
| |
| return (len); |
| err: |
| return (-1); |
| |
| } |
| |
| int |
| cbmem_walk(struct cbmem *cbmem, cbmem_walk_func_t walk_func, void *arg) |
| { |
| struct cbmem_entry_hdr *hdr; |
| struct cbmem_iter iter; |
| int rc; |
| |
| rc = cbmem_lock_acquire(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| cbmem_iter_start(cbmem, &iter); |
| while (1) { |
| hdr = cbmem_iter_next(cbmem, &iter); |
| if (hdr == NULL) { |
| break; |
| } |
| |
| rc = walk_func(cbmem, hdr, arg); |
| if (rc == 1) { |
| break; |
| } |
| } |
| |
| rc = cbmem_lock_release(cbmem); |
| if (rc != 0) { |
| goto err; |
| } |
| |
| return (0); |
| err: |
| return (rc); |
| } |