-command_ret_t do_cpu_freq(cmd_tbl_t *cmdtp UNUSED, uint_fast8_t flag UNUSED, int argc, char * const argv[])
-{
-
-#define O_SILENT (1<<0)
-#define O_WENV (1<<1)
-#define O_LOAD_LOOP (1<<2)
-#define O_UNLOAD_LOOP (1<<3)
-
- uint_fast8_t options = O_LOAD_LOOP | O_UNLOAD_LOOP;
- uint8_t lcycles = 18;
- uint16_t timeout = 1000;
-
- uint8_t eicrb_save;
- uint8_t eimsk_save;
- uint8_t mem_save[cpuinfo_length];
-
- /* reset getopt() */
- optind = 0;
-
- int opt;
- while ((opt = getopt(argc, argv, PSTR("swnuc:t:"))) != -1) {
- switch (opt) {
- case 's':
- options |= O_SILENT;
- break;
- case 'w':
- options |= O_WENV;
- break;
- case 'n':
- options &= ~O_LOAD_LOOP;
- break;
- case 'u':
- options &= ~O_UNLOAD_LOOP;
- break;
- case 'c':
- lcycles = eval_arg(optarg, NULL);
- break;
- case 't':
- timeout = eval_arg(optarg, NULL);
- break;
- default: /* '?' */
- return CMD_RET_USAGE;
- }
- }
- if (argc - optind != 0)
- return CMD_RET_USAGE;
-
- if (z80_bus_state() & ZST_RUNNING) {
- if (!(options & O_SILENT))
- printf_P(PSTR("Frequency measuring failed. CPU allready running!\n"));
- return CMD_RET_FAILURE;
- }
-
- ATOMIC_BLOCK(ATOMIC_FORCEON) {
- /* Save state and disable INT6 */
- eimsk_save = EIMSK;
- EIMSK &= ~_BV(INT6);
- /* Save state and set INT6 for falling edge */
- eicrb_save = EICRB;
- EICRB = (eicrb_save & ~(0b11 << ISC60)) | (0b10 << ISC60);
- }
-
- z80_bus_cmd(Request);
- if (options & O_LOAD_LOOP) {
- z80_read_block(mem_save, 0, cpuinfo_length);
- z80_load_mem(0, cpuinfo,
- &cpuinfo_sections,
- cpuinfo_address,
- cpuinfo_length_of_sections);
- }
- Stat &= ~S_IO_0X40; /* Reset pending int */
- z80_bus_cmd(Release);
- z80_bus_cmd(Run);
-
- clear_ctrlc(); /* forget any previous Control C */
- ERRNUM err = 0;
-
- /* Wait for falling edge */
- do {
- /* check for ctrl-c to abort... */
- if (had_ctrlc() || ctrlc()) {
- err = EINTR;
- break;
- }
- } while ((Stat & S_IO_0X40) == 0);
-
- int32_t cpu_freq;
- if (!err)
- cpu_freq = z80_measure_phi(lcycles, false, timeout);
-
- z80_bus_cmd(Reset);
- if (options & O_UNLOAD_LOOP) {
- z80_bus_cmd(Request);
- z80_write_block(mem_save, 0, cpuinfo_length);
- z80_bus_cmd(Release);
- }
- ATOMIC_BLOCK(ATOMIC_FORCEON) {
- /* Restore INT6 */
- eicrb_save = EICRB;
- EICRB = (EICRB & ~(0b11 << ISC60)) | (eicrb_save & (0b11 << ISC60));
- if ((eimsk_save & _BV(INT6)) != 0)
- EIMSK |= _BV(INT6);
- /* Reset pending int */
- EIFR = _BV(INTF6);
- }
-
- if (err)
- cmd_error(CMD_RET_FAILURE, err, NULL);
-
- if (!(options & O_SILENT)) {
- printf_P(PSTR("%ld%S\n"), cpu_freq, cpu_freq < 0 ? PSTR("") : PSTR("Hz"));
-// if (cpu_freq != 0)
-// else
-// printf_P(PSTR("No CPU clock or input frequency to low!\n"));
- }
-#if 0
- if (options & O_WENV) {
- if (setenv_ulong(PSTR(ENV_CPU_FREQ), cpu_freq)) {
- if (!(options & O_SILENT))
- printf_P(PSTR("'SETENV (%S, %lu)' failed!\n"), PSTR(ENV_CPU_FREQ), cpu_freq);
- return CMD_RET_FAILURE;
- }
- }
-#endif
- return CMD_RET_SUCCESS;
-}
-