+ /* Get relative drive number */
+ drv = msg[1];
+ drv_debug(START, PSTR("login"));
+
+ if (len != 5) {
+ return msg_cpm_result(subf, 0x01, res);
+ }
+
+ if ( drv >= CONFIG_CPM_MAX_DRIVE) {
+ /* invalid relative drive number */
+ return msg_cpm_result(subf, 0x02, res);
+ }
+
+ dp = &drv_table[drv];
+ dp->flags &= ~DRV_FLG_OPEN;
+ dp->dph = ((uint32_t)msg[4] << 16) + ((uint16_t)msg[3] << 8) + msg[2];
+
+ if (dp->img_name == NULL) {
+ /* no file attached */
+ return msg_cpm_result(subf, 0x06, res);
+ }
+
+ f_close(&dp->fd);
+ res = f_open(&dp->fd, dp->img_name,
+ FA_READ | (dp->opt&DRV_OPT_RO ? 0 : FA_WRITE));
+
+ dp->flags |= DRV_FLG_OPEN;
+
+ /* send result*/
+ msg_cpm_result(subf, 0x00, res);
+}
+
+
+/*
+ db 2 ; disk command
+ ds 1 ; subcommand (login/read/write)
+ ds 1 ; @adrv (8 bits) +0
+ ds 1 ; @rdrv (8 bits) +1
+ ds 2 ; @trk (16 bits) +2
+ ds 2 ; @sect(16 bits) +4
+ ds 1 ; @cnt (8 bits) +6
+ ds 3 ; phys. transfer addr +7
+*/
+
+#define ADRV 0
+#define RDRV 1
+#define TRK 2
+#define SEC 4
+#define CNT 6
+#define ADDR 7
+
+void do_msg_cpm_rw(uint8_t subf, int len, uint8_t * msg)
+{
+ struct cpm_drive_s *dp;
+ uint32_t addr;
+ uint32_t pos;
+ uint16_t track;
+ uint16_t sec;
+ uint8_t secs;
+ bool dowrite;
+ FRESULT res = 0;
+ uint8_t rc = 0;
+ bool buserr = 0;
+
+ drv = msg[RDRV];
+ dowrite = (subf == 2);
+
+ drv_debug(START, PSTR("%2S"), dowrite ? PSTR("W ") : PSTR(" R"));
+
+ if (len != 10) {
+ return msg_cpm_result(subf, 0x01, res);
+ }
+ if ( drv>= CONFIG_CPM_MAX_DRIVE) {
+ return msg_cpm_result(subf, 0x02, res);
+ }
+
+ dp = &drv_table[drv];
+ track = (uint16_t)(msg[TRK+1] << 8) + msg[TRK];
+ sec = (uint16_t)(msg[SEC+1] << 8) + msg[SEC];
+ secs = msg[CNT];
+ addr = ((uint32_t)msg[ADDR+2] << 16) + ((uint16_t)msg[ADDR+1] << 8) + msg[ADDR];
+
+ if (dp->img_name == NULL) {
+ /* no media */
+ return msg_cpm_result(subf, 0x06, res);
+ }
+
+ /* TODO: tracks per sector from dpb */
+ pos = (track * 8UL + sec) * CONFIG_CPM_BLOCK_SIZE;
+
+ drv_debug(MIDDLE, PSTR(" T:%4d, S:%2d, cnt:%2d, lba: %.8lx, addr: %.5lx"),
+ track, sec, secs, pos, addr);
+
+ if (addr == 0) {
+ return msg_cpm_result(subf, 0x07, res);
+ }
+
+ if (dowrite && dp->opt & DRV_OPT_RO) {
+ return msg_cpm_result(subf, 0x05, res);
+ }
+
+
+ if (pos + secs * CONFIG_CPM_BLOCK_SIZE > CONFIG_CPM_DISKSIZE) {
+ drv_debug(MIDDLE, PSTR(" access > DISKSIZE:%.8lx!"),
+ CONFIG_CPM_DISKSIZE);
+ return msg_cpm_result(subf, 0x04, res);
+ }
+
+ res = f_lseek(&dp->fd, pos);
+
+ while (!res && secs--) {
+ unsigned int brw;
+ if (dowrite) {
+ if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
+ buserr = 1;
+ break;
+ } else {
+ z80_read_block(disk_buffer, addr, CONFIG_CPM_BLOCK_SIZE);
+ z80_bus_cmd(Release);
+ }
+ res = f_write(&dp->fd, disk_buffer, CONFIG_CPM_BLOCK_SIZE, &brw);
+ } else {
+ res = f_read(&dp->fd, disk_buffer, CONFIG_CPM_BLOCK_SIZE, &brw);
+ if (res == FR_OK) {
+ if (!(z80_bus_cmd(Request) & ZST_ACQUIRED)) {
+ buserr = 1;
+ break;
+ } else {
+ z80_write_block(disk_buffer, addr, CONFIG_CPM_BLOCK_SIZE);
+ z80_bus_cmd(Release);
+ }
+ }
+ }
+ if (brw != CONFIG_CPM_BLOCK_SIZE) {
+ drv_debug(MIDDLE, PSTR(" short rd/wr: res: %d, brw: %u"),
+ res, brw);
+ res = 64;
+ }
+ addr += CONFIG_CPM_BLOCK_SIZE;
+ }
+
+ if (dowrite && !res) {
+ dp->flags |= DRV_FLG_DIRTY;
+ bg_setstat(handle_cpm_drv_to, 1);
+ }
+
+ if (buserr) {
+ /* Bus timeout. how can this happen? */
+ rc = 0x03;
+ }
+
+ /* send result*/
+ msg_cpm_result(subf, rc, res);