-static struct algorithm_steps *as_new(void)
-{
- struct algorithm_steps *as = calloc(1, sizeof(struct algorithm_steps));
- as->size = 8;
- as->steps = malloc(as->size * sizeof(as->steps[0]));
- return as;
-}
-
-static struct algorithm_steps *as_delete(struct algorithm_steps *as)
-{
- for (unsigned step = 0; step < as->used; step++) {
- free(as->steps[step]);
- as->steps[step] = NULL;
- }
- free(as->steps);
- free(as);
- return NULL;
-}
-
-static int as_empty(struct algorithm_steps *as)
-{
- for (unsigned s = 0; s < as->used; s++) {
- if (as->steps[s][0] != STEP_NOP)
- return 0;
- }
- return 1;
-}
-
-/* Return size of compiled program. */
-static unsigned as_compile(struct algorithm_steps *as, uint8_t *target,
- unsigned target_size)
-{
- unsigned offset = 0;
- bool finish_early = false;
- for (unsigned s = 0; s < as->used && !finish_early; s++) {
- unsigned bytes_left = target_size - offset;
- switch (as->steps[s][0]) {
- case STEP_NOP:
- break;
- case STEP_TX:
- {
- unsigned size = as->steps[s][1];
- if (size + 3 > bytes_left) {
- finish_early = true;
- break;
- }
- memcpy(target + offset, as->steps[s], size + 2);
- offset += size + 2;
- break;
- }
- case STEP_WRITE_REG:
- if (4 > bytes_left) {
- finish_early = true;
- break;
- }
- memcpy(target + offset, as->steps[s], 3);
- offset += 3;
- break;
- case STEP_SET_DIR:
- if (3 > bytes_left) {
- finish_early = true;
- break;
- }
- memcpy(target + offset, as->steps[s], 2);
- offset += 2;
- break;
- case STEP_TXWM_WAIT:
- case STEP_WIP_WAIT:
- if (2 > bytes_left) {
- finish_early = true;
- break;
- }
- memcpy(target + offset, as->steps[s], 1);
- offset += 1;
- break;
- default:
- assert(0);
- }
- if (!finish_early)
- as->steps[s][0] = STEP_NOP;
- }
- assert(offset + 1 <= target_size);
- target[offset++] = STEP_EXIT;
-
- LOG_DEBUG("%d-byte program:", offset);
- for (unsigned i = 0; i < offset;) {
- char buf[80];
- for (unsigned x = 0; i < offset && x < 16; x++, i++)
- sprintf(buf + x*3, "%02x ", target[i]);
- LOG_DEBUG("%s", buf);
- }
-
- return offset;
-}
-
-static void as_add_step(struct algorithm_steps *as, uint8_t *step)
-{
- if (as->used == as->size) {
- as->size *= 2;
- as->steps = realloc(as->steps, sizeof(as->steps[0]) * as->size);
- LOG_DEBUG("Increased size to 0x%x", as->size);
- }
- as->steps[as->used] = step;
- as->used++;
-}
-
-static void as_add_tx(struct algorithm_steps *as, unsigned count, const uint8_t *data)
-{
- LOG_DEBUG("count=%d", count);
- while (count > 0) {
- unsigned step_count = MIN(count, 255);
- uint8_t *step = malloc(step_count + 2);
- step[0] = STEP_TX;
- step[1] = step_count;
- memcpy(step + 2, data, step_count);
- as_add_step(as, step);
- data += step_count;
- count -= step_count;
- }
-}
-
-static void as_add_tx1(struct algorithm_steps *as, uint8_t byte)
-{
- uint8_t data[1];
- data[0] = byte;
- as_add_tx(as, 1, data);
-}
-
-static void as_add_write_reg(struct algorithm_steps *as, uint8_t offset, uint8_t data)
-{
- uint8_t *step = malloc(3);
- step[0] = STEP_WRITE_REG;
- step[1] = offset;
- step[2] = data;
- as_add_step(as, step);
-}
-
-static void as_add_txwm_wait(struct algorithm_steps *as)
-{
- uint8_t *step = malloc(1);
- step[0] = STEP_TXWM_WAIT;
- as_add_step(as, step);
-}
-
-static void as_add_wip_wait(struct algorithm_steps *as)
-{
- uint8_t *step = malloc(1);
- step[0] = STEP_WIP_WAIT;
- as_add_step(as, step);
-}
-
-static void as_add_set_dir(struct algorithm_steps *as, bool dir)
-{
- uint8_t *step = malloc(2);
- step[0] = STEP_SET_DIR;
- step[1] = FESPI_FMT_DIR(dir);
- as_add_step(as, step);
-}
-
-/* This should write something less than or equal to a page.*/
-static int steps_add_buffer_write(struct algorithm_steps *as,
- const uint8_t *buffer, uint32_t chip_offset, uint32_t len)
-{
- if (chip_offset & 0xFF000000) {
- LOG_ERROR("FESPI interface does not support greater than 3B addressing, can't write to offset 0x%" PRIx32,
- chip_offset);
- return ERROR_FAIL;
- }
-
- as_add_tx1(as, SPIFLASH_WRITE_ENABLE);
- as_add_txwm_wait(as);
- as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
-
- uint8_t setup[] = {
- SPIFLASH_PAGE_PROGRAM,
- chip_offset >> 16,
- chip_offset >> 8,
- chip_offset,
- };
- as_add_tx(as, sizeof(setup), setup);
-
- as_add_tx(as, len, buffer);
- as_add_txwm_wait(as);
- as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
-
- /* fespi_wip() */
- as_add_set_dir(as, FESPI_DIR_RX);
- as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_HOLD);
- as_add_wip_wait(as);
- as_add_write_reg(as, FESPI_REG_CSMODE, FESPI_CSMODE_AUTO);
- as_add_set_dir(as, FESPI_DIR_TX);
-
- return ERROR_OK;
-}
-
-static int steps_execute(struct algorithm_steps *as,
- struct flash_bank *bank, struct working_area *algorithm_wa,
- struct working_area *data_wa)
-{
- struct target *target = bank->target;
- struct fespi_flash_bank *fespi_info = bank->driver_priv;
- uint32_t ctrl_base = fespi_info->ctrl_base;
- int xlen = riscv_xlen(target);
-
- struct reg_param reg_params[2];
- init_reg_param(®_params[0], "a0", xlen, PARAM_OUT);
- init_reg_param(®_params[1], "a1", xlen, PARAM_OUT);
- buf_set_u64(reg_params[0].value, 0, xlen, ctrl_base);
- buf_set_u64(reg_params[1].value, 0, xlen, data_wa->address);
-
- int retval = ERROR_OK;
- while (!as_empty(as)) {
- keep_alive();
- uint8_t *data_buf = malloc(data_wa->size);
- unsigned bytes = as_compile(as, data_buf, data_wa->size);
- retval = target_write_buffer(target, data_wa->address, bytes,
- data_buf);
- free(data_buf);
- if (retval != ERROR_OK) {
- LOG_ERROR("Failed to write data to " TARGET_ADDR_FMT ": %d",
- data_wa->address, retval);
- goto exit;
- }
-
- retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
- algorithm_wa->address, algorithm_wa->address + 4,
- 10000, NULL);
- if (retval != ERROR_OK) {
- LOG_ERROR("Failed to execute algorithm at " TARGET_ADDR_FMT ": %d",
- algorithm_wa->address, retval);
- goto exit;
- }
- }
-
-exit:
- destroy_reg_param(®_params[1]);
- destroy_reg_param(®_params[0]);
- return retval;
-}