arch/s390/kernel/time.c

   1 /*
   2  *  arch/s390/kernel/time.c
   3  *    Time of day based timer functions.
   4  *
   5  *  S390 version
   6  *    Copyright (C) 1999 IBM Deutschland Entwicklung GmbH, IBM Corporation
   7  *    Author(s): Hartmut Penner (hp@de.ibm.com),
   8  *               Martin Schwidefsky (schwidefsky@de.ibm.com),
   9  *               Denis Joseph Barrow (djbarrow@de.ibm.com,barrow_dj@yahoo.com)
  10  *
  11  *  Derived from "arch/i386/kernel/time.c"
  12  *    Copyright (C) 1991, 1992, 1995  Linus Torvalds
  13  */
  14
  15 #include <linux/errno.h>
  16 #include <linux/module.h>
  17 #include <linux/sched.h>
  18 #include <linux/kernel.h>
  19 #include <linux/param.h>
  20 #include <linux/string.h>
  21 #include <linux/mm.h>
  22 #include <linux/interrupt.h>
  23 #include <linux/time.h>
  24 #include <linux/sysdev.h>
  25 #include <linux/delay.h>
  26 #include <linux/init.h>
  27 #include <linux/smp.h>
  28 #include <linux/types.h>
  29 #include <linux/profile.h>
  30 #include <linux/timex.h>
  31 #include <linux/notifier.h>
  32 #include <linux/clocksource.h>
  33 #include <linux/clockchips.h>
  34 #include <asm/uaccess.h>
  35 #include <asm/delay.h>
  36 #include <asm/s390_ext.h>
  37 #include <asm/div64.h>
  38 #include <asm/irq.h>
  39 #include <asm/irq_regs.h>
  40 #include <asm/timer.h>
  41 #include <asm/etr.h>
  42 #include <asm/cio.h>
  43
  44 /* change this if you have some constant time drift */
  45 #define USECS_PER_JIFFY     ((unsigned long) 1000000/HZ)
  46 #define CLK_TICKS_PER_JIFFY ((unsigned long) USECS_PER_JIFFY << 12)
  47
  48 /* The value of the TOD clock for 1.1.1970. */
  49 #define TOD_UNIX_EPOCH 0x7d91048bca000000ULL
  50
  51 /*
  52  * Create a small time difference between the timer interrupts
  53  * on the different cpus to avoid lock contention.
  54  */
  55 #define CPU_DEVIATION       (smp_processor_id() << 12)
  56
  57 #define TICK_SIZE tick
  58
  59 static ext_int_info_t ext_int_info_cc;
  60 static ext_int_info_t ext_int_etr_cc;
  61 static u64 jiffies_timer_cc;
  62
  63 static DEFINE_PER_CPU(struct clock_event_device, comparators);
  64
  65 /*
  66  * Scheduler clock - returns current time in nanosec units.
  67  */
  68 unsigned long long sched_clock(void)
  69 {
  70         return ((get_clock_xt() - jiffies_timer_cc) * 125) >> 9;
  71 }
  72
  73 /*
  74  * Monotonic_clock - returns # of nanoseconds passed since time_init()
  75  */
  76 unsigned long long monotonic_clock(void)
  77 {
  78         return sched_clock();
  79 }
  80 EXPORT_SYMBOL(monotonic_clock);
  81
  82 void tod_to_timeval(__u64 todval, struct timespec *xtime)
  83 {
  84         unsigned long long sec;
  85
  86         sec = todval >> 12;
  87         do_div(sec, 1000000);
  88         xtime->tv_sec = sec;
  89         todval -= (sec * 1000000) << 12;
  90         xtime->tv_nsec = ((todval * 1000) >> 12);
  91 }
  92
  93 #ifdef CONFIG_PROFILING
  94 #define s390_do_profile()       profile_tick(CPU_PROFILING)
  95 #else
  96 #define s390_do_profile()       do { ; } while(0)
  97 #endif /* CONFIG_PROFILING */
  98
  99 void clock_comparator_work(void)
 100 {
 101         struct clock_event_device *cd;
 102
 103         S390_lowcore.clock_comparator = -1ULL;
 104         set_clock_comparator(S390_lowcore.clock_comparator);
 105         cd = &__get_cpu_var(comparators);
 106         cd->event_handler(cd);
 107         s390_do_profile();
 108 }
 109
 110 /*
 111  * Fixup the clock comparator.
 112  */
 113 static void fixup_clock_comparator(unsigned long long delta)
 114 {
 115         /* If nobody is waiting there's nothing to fix. */
 116         if (S390_lowcore.clock_comparator == -1ULL)
 117                 return;
 118         S390_lowcore.clock_comparator += delta;
 119         set_clock_comparator(S390_lowcore.clock_comparator);
 120 }
 121
 122 static int s390_next_event(unsigned long delta,
 123                            struct clock_event_device *evt)
 124 {
 125         S390_lowcore.clock_comparator = get_clock() + delta;
 126         set_clock_comparator(S390_lowcore.clock_comparator);
 127         return 0;
 128 }
 129
 130 static void s390_set_mode(enum clock_event_mode mode,
 131                           struct clock_event_device *evt)
 132 {
 133 }
 134
 135 /*
 136  * Set up lowcore and control register of the current cpu to
 137  * enable TOD clock and clock comparator interrupts.
 138  */
 139 void init_cpu_timer(void)
 140 {
 141         struct clock_event_device *cd;
 142         int cpu;
 143
 144         S390_lowcore.clock_comparator = -1ULL;
 145         set_clock_comparator(S390_lowcore.clock_comparator);
 146
 147         cpu = smp_processor_id();
 148         cd = &per_cpu(comparators, cpu);
 149         cd->name                = "comparator";
 150         cd->features            = CLOCK_EVT_FEAT_ONESHOT;
 151         cd->mult                = 16777;
 152         cd->shift               = 12;
 153         cd->min_delta_ns        = 1;
 154         cd->max_delta_ns        = LONG_MAX;
 155         cd->rating              = 400;
 156         cd->cpumask             = cpumask_of_cpu(cpu);
 157         cd->set_next_event      = s390_next_event;
 158         cd->set_mode            = s390_set_mode;
 159
 160         clockevents_register_device(cd);
 161
 162         /* Enable clock comparator timer interrupt. */
 163         __ctl_set_bit(0,11);
 164
 165         /* Always allow ETR external interrupts, even without an ETR. */
 166         __ctl_set_bit(0, 4);
 167 }
 168
 169 static void clock_comparator_interrupt(__u16 code)
 170 {
 171 }
 172
 173 static void etr_reset(void);
 174 static void etr_ext_handler(__u16);
 175
 176 /*
 177  * Get the TOD clock running.
 178  */
 179 static u64 __init reset_tod_clock(void)
 180 {
 181         u64 time;
 182
 183         etr_reset();
 184         if (store_clock(&time) == 0)
 185                 return time;
 186         /* TOD clock not running. Set the clock to Unix Epoch. */
 187         if (set_clock(TOD_UNIX_EPOCH) != 0 || store_clock(&time) != 0)
 188                 panic("TOD clock not operational.");
 189
 190         return TOD_UNIX_EPOCH;
 191 }
 192
 193 static cycle_t read_tod_clock(void)
 194 {
 195         return get_clock();
 196 }
 197
 198 static struct clocksource clocksource_tod = {
 199         .name           = "tod",
 200         .rating         = 400,
 201         .read           = read_tod_clock,
 202         .mask           = -1ULL,
 203         .mult           = 1000,
 204         .shift          = 12,
 205         .flags          = CLOCK_SOURCE_IS_CONTINUOUS,
 206 };
 207
 208
 209 /*
 210  * Initialize the TOD clock and the CPU timer of
 211  * the boot cpu.
 212  */
 213 void __init time_init(void)
 214 {
 215         u64 init_timer_cc;
 216
 217         init_timer_cc = reset_tod_clock();
 218         jiffies_timer_cc = init_timer_cc - jiffies_64 * CLK_TICKS_PER_JIFFY;
 219
 220         /* set xtime */
 221         tod_to_timeval(init_timer_cc - TOD_UNIX_EPOCH, &xtime);
 222         set_normalized_timespec(&wall_to_monotonic,
 223                                 -xtime.tv_sec, -xtime.tv_nsec);
 224
 225         /* request the clock comparator external interrupt */
 226         if (register_early_external_interrupt(0x1004,
 227                                               clock_comparator_interrupt,
 228                                               &ext_int_info_cc) != 0)
 229                 panic("Couldn't request external interrupt 0x1004");
 230
 231         if (clocksource_register(&clocksource_tod) != 0)
 232                 panic("Could not register TOD clock source");
 233
 234         /* request the etr external interrupt */
 235         if (register_early_external_interrupt(0x1406, etr_ext_handler,
 236                                               &ext_int_etr_cc) != 0)
 237                 panic("Couldn't request external interrupt 0x1406");
 238
 239         /* Enable TOD clock interrupts on the boot cpu. */
 240         init_cpu_timer();
 241
 242 #ifdef CONFIG_VIRT_TIMER
 243         vtime_init();
 244 #endif
 245 }
 246
 247 /*
 248  * External Time Reference (ETR) code.
 249  */
 250 static int etr_port0_online;
 251 static int etr_port1_online;
 252
 253 static int __init early_parse_etr(char *p)
 254 {
 255         if (strncmp(p, "off", 3) == 0)
 256                 etr_port0_online = etr_port1_online = 0;
 257         else if (strncmp(p, "port0", 5) == 0)
 258                 etr_port0_online = 1;
 259         else if (strncmp(p, "port1", 5) == 0)
 260                 etr_port1_online = 1;
 261         else if (strncmp(p, "on", 2) == 0)
 262                 etr_port0_online = etr_port1_online = 1;
 263         return 0;
 264 }
 265 early_param("etr", early_parse_etr);
 266
 267 enum etr_event {
 268         ETR_EVENT_PORT0_CHANGE,
 269         ETR_EVENT_PORT1_CHANGE,
 270         ETR_EVENT_PORT_ALERT,
 271         ETR_EVENT_SYNC_CHECK,
 272         ETR_EVENT_SWITCH_LOCAL,
 273         ETR_EVENT_UPDATE,
 274 };
 275
 276 enum etr_flags {
 277         ETR_FLAG_ENOSYS,
 278         ETR_FLAG_EACCES,
 279         ETR_FLAG_STEAI,
 280 };
 281
 282 /*
 283  * Valid bit combinations of the eacr register are (x = don't care):
 284  * e0 e1 dp p0 p1 ea es sl
 285  *  0  0  x  0  0  0  0  0  initial, disabled state
 286  *  0  0  x  0  1  1  0  0  port 1 online
 287  *  0  0  x  1  0  1  0  0  port 0 online
 288  *  0  0  x  1  1  1  0  0  both ports online
 289  *  0  1  x  0  1  1  0  0  port 1 online and usable, ETR or PPS mode
 290  *  0  1  x  0  1  1  0  1  port 1 online, usable and ETR mode
 291  *  0  1  x  0  1  1  1  0  port 1 online, usable, PPS mode, in-sync
 292  *  0  1  x  0  1  1  1  1  port 1 online, usable, ETR mode, in-sync
 293  *  0  1  x  1  1  1  0  0  both ports online, port 1 usable
 294  *  0  1  x  1  1  1  1  0  both ports online, port 1 usable, PPS mode, in-sync
 295  *  0  1  x  1  1  1  1  1  both ports online, port 1 usable, ETR mode, in-sync
 296  *  1  0  x  1  0  1  0  0  port 0 online and usable, ETR or PPS mode
 297  *  1  0  x  1  0  1  0  1  port 0 online, usable and ETR mode
 298  *  1  0  x  1  0  1  1  0  port 0 online, usable, PPS mode, in-sync
 299  *  1  0  x  1  0  1  1  1  port 0 online, usable, ETR mode, in-sync
 300  *  1  0  x  1  1  1  0  0  both ports online, port 0 usable
 301  *  1  0  x  1  1  1  1  0  both ports online, port 0 usable, PPS mode, in-sync
 302  *  1  0  x  1  1  1  1  1  both ports online, port 0 usable, ETR mode, in-sync
 303  *  1  1  x  1  1  1  1  0  both ports online & usable, ETR, in-sync
 304  *  1  1  x  1  1  1  1  1  both ports online & usable, ETR, in-sync
 305  */
 306 static struct etr_eacr etr_eacr;
 307 static u64 etr_tolec;                   /* time of last eacr update */
 308 static unsigned long etr_flags;
 309 static struct etr_aib etr_port0;
 310 static int etr_port0_uptodate;
 311 static struct etr_aib etr_port1;
 312 static int etr_port1_uptodate;
 313 static unsigned long etr_events;
 314 static struct timer_list etr_timer;
 315 static DEFINE_PER_CPU(atomic_t, etr_sync_word);
 316
 317 static void etr_timeout(unsigned long dummy);
 318 static void etr_work_fn(struct work_struct *work);
 319 static DECLARE_WORK(etr_work, etr_work_fn);
 320
 321 /*
 322  * The etr get_clock function. It will write the current clock value
 323  * to the clock pointer and return 0 if the clock is in sync with the
 324  * external time source. If the clock mode is local it will return
 325  * -ENOSYS and -EAGAIN if the clock is not in sync with the external
 326  * reference. This function is what ETR is all about..
 327  */
 328 int get_sync_clock(unsigned long long *clock)
 329 {
 330         atomic_t *sw_ptr;
 331         unsigned int sw0, sw1;
 332
 333         sw_ptr = &get_cpu_var(etr_sync_word);
 334         sw0 = atomic_read(sw_ptr);
 335         *clock = get_clock();
 336         sw1 = atomic_read(sw_ptr);
 337         put_cpu_var(etr_sync_sync);
 338         if (sw0 == sw1 && (sw0 & 0x80000000U))
 339                 /* Success: time is in sync. */
 340                 return 0;
 341         if (test_bit(ETR_FLAG_ENOSYS, &etr_flags))
 342                 return -ENOSYS;
 343         if (test_bit(ETR_FLAG_EACCES, &etr_flags))
 344                 return -EACCES;
 345         return -EAGAIN;
 346 }
 347 EXPORT_SYMBOL(get_sync_clock);
 348
 349 /*
 350  * Make get_sync_clock return -EAGAIN.
 351  */
 352 static void etr_disable_sync_clock(void *dummy)
 353 {
 354         atomic_t *sw_ptr = &__get_cpu_var(etr_sync_word);
 355         /*
 356          * Clear the in-sync bit 2^31. All get_sync_clock calls will
 357          * fail until the sync bit is turned back on. In addition
 358          * increase the "sequence" counter to avoid the race of an
 359          * etr event and the complete recovery against get_sync_clock.
 360          */
 361         atomic_clear_mask(0x80000000, sw_ptr);
 362         atomic_inc(sw_ptr);
 363 }
 364
 365 /*
 366  * Make get_sync_clock return 0 again.
 367  * Needs to be called from a context disabled for preemption.
 368  */
 369 static void etr_enable_sync_clock(void)
 370 {
 371         atomic_t *sw_ptr = &__get_cpu_var(etr_sync_word);
 372         atomic_set_mask(0x80000000, sw_ptr);
 373 }
 374
 375 /*
 376  * Reset ETR attachment.
 377  */
 378 static void etr_reset(void)
 379 {
 380         etr_eacr =  (struct etr_eacr) {
 381                 .e0 = 0, .e1 = 0, ._pad0 = 4, .dp = 0,
 382                 .p0 = 0, .p1 = 0, ._pad1 = 0, .ea = 0,
 383                 .es = 0, .sl = 0 };
 384         if (etr_setr(&etr_eacr) == 0)
 385                 etr_tolec = get_clock();
 386         else {
 387                 set_bit(ETR_FLAG_ENOSYS, &etr_flags);
 388                 if (etr_port0_online || etr_port1_online) {
 389                         printk(KERN_WARNING "Running on non ETR capable "
 390                                "machine, only local mode available.\n");
 391                         etr_port0_online = etr_port1_online = 0;
 392                 }
 393         }
 394 }
 395
 396 static int __init etr_init(void)
 397 {
 398         struct etr_aib aib;
 399
 400         if (test_bit(ETR_FLAG_ENOSYS, &etr_flags))
 401                 return 0;
 402         /* Check if this machine has the steai instruction. */
 403         if (etr_steai(&aib, ETR_STEAI_STEPPING_PORT) == 0)
 404                 set_bit(ETR_FLAG_STEAI, &etr_flags);
 405         setup_timer(&etr_timer, etr_timeout, 0UL);
 406         if (!etr_port0_online && !etr_port1_online)
 407                 set_bit(ETR_FLAG_EACCES, &etr_flags);
 408         if (etr_port0_online) {
 409                 set_bit(ETR_EVENT_PORT0_CHANGE, &etr_events);
 410                 schedule_work(&etr_work);
 411         }
 412         if (etr_port1_online) {
 413                 set_bit(ETR_EVENT_PORT1_CHANGE, &etr_events);
 414                 schedule_work(&etr_work);
 415         }
 416         return 0;
 417 }
 418
 419 arch_initcall(etr_init);
 420
 421 /*
 422  * Two sorts of ETR machine checks. The architecture reads:
 423  * "When a machine-check niterruption occurs and if a switch-to-local or
 424  *  ETR-sync-check interrupt request is pending but disabled, this pending
 425  *  disabled interruption request is indicated and is cleared".
 426  * Which means that we can get etr_switch_to_local events from the machine
 427  * check handler although the interruption condition is disabled. Lovely..
 428  */
 429
 430 /*
 431  * Switch to local machine check. This is called when the last usable
 432  * ETR port goes inactive. After switch to local the clock is not in sync.
 433  */
 434 void etr_switch_to_local(void)
 435 {
 436         if (!etr_eacr.sl)
 437                 return;
 438         etr_disable_sync_clock(NULL);
 439         set_bit(ETR_EVENT_SWITCH_LOCAL, &etr_events);
 440         schedule_work(&etr_work);
 441 }
 442
 443 /*
 444  * ETR sync check machine check. This is called when the ETR OTE and the
 445  * local clock OTE are farther apart than the ETR sync check tolerance.
 446  * After a ETR sync check the clock is not in sync. The machine check
 447  * is broadcasted to all cpus at the same time.
 448  */
 449 void etr_sync_check(void)
 450 {
 451         if (!etr_eacr.es)
 452                 return;
 453         etr_disable_sync_clock(NULL);
 454         set_bit(ETR_EVENT_SYNC_CHECK, &etr_events);
 455         schedule_work(&etr_work);
 456 }
 457
 458 /*
 459  * ETR external interrupt. There are two causes:
 460  * 1) port state change, check the usability of the port
 461  * 2) port alert, one of the ETR-data-validity bits (v1-v2 bits of the
 462  *    sldr-status word) or ETR-data word 1 (edf1) or ETR-data word 3 (edf3)
 463  *    or ETR-data word 4 (edf4) has changed.
 464  */
 465 static void etr_ext_handler(__u16 code)
 466 {
 467         struct etr_interruption_parameter *intparm =
 468                 (struct etr_interruption_parameter *) &S390_lowcore.ext_params;
 469
 470         if (intparm->pc0)
 471                 /* ETR port 0 state change. */
 472                 set_bit(ETR_EVENT_PORT0_CHANGE, &etr_events);
 473         if (intparm->pc1)
 474                 /* ETR port 1 state change. */
 475                 set_bit(ETR_EVENT_PORT1_CHANGE, &etr_events);
 476         if (intparm->eai)
 477                 /*
 478                  * ETR port alert on either port 0, 1 or both.
 479                  * Both ports are not up-to-date now.
 480                  */
 481                 set_bit(ETR_EVENT_PORT_ALERT, &etr_events);
 482         schedule_work(&etr_work);
 483 }
 484
 485 static void etr_timeout(unsigned long dummy)
 486 {
 487         set_bit(ETR_EVENT_UPDATE, &etr_events);
 488         schedule_work(&etr_work);
 489 }
 490
 491 /*
 492  * Check if the etr mode is pss.
 493  */
 494 static inline int etr_mode_is_pps(struct etr_eacr eacr)
 495 {
 496         return eacr.es && !eacr.sl;
 497 }
 498
 499 /*
 500  * Check if the etr mode is etr.
 501  */
 502 static inline int etr_mode_is_etr(struct etr_eacr eacr)
 503 {
 504         return eacr.es && eacr.sl;
 505 }
 506
 507 /*
 508  * Check if the port can be used for TOD synchronization.
 509  * For PPS mode the port has to receive OTEs. For ETR mode
 510  * the port has to receive OTEs, the ETR stepping bit has to
 511  * be zero and the validity bits for data frame 1, 2, and 3
 512  * have to be 1.
 513  */
 514 static int etr_port_valid(struct etr_aib *aib, int port)
 515 {
 516         unsigned int psc;
 517
 518         /* Check that this port is receiving OTEs. */
 519         if (aib->tsp == 0)
 520                 return 0;
 521
 522         psc = port ? aib->esw.psc1 : aib->esw.psc0;
 523         if (psc == etr_lpsc_pps_mode)
 524                 return 1;
 525         if (psc == etr_lpsc_operational_step)
 526                 return !aib->esw.y && aib->slsw.v1 &&
 527                         aib->slsw.v2 && aib->slsw.v3;
 528         return 0;
 529 }
 530
 531 /*
 532  * Check if two ports are on the same network.
 533  */
 534 static int etr_compare_network(struct etr_aib *aib1, struct etr_aib *aib2)
 535 {
 536         // FIXME: any other fields we have to compare?
 537         return aib1->edf1.net_id == aib2->edf1.net_id;
 538 }
 539
 540 /*
 541  * Wrapper for etr_stei that converts physical port states
 542  * to logical port states to be consistent with the output
 543  * of stetr (see etr_psc vs. etr_lpsc).
 544  */
 545 static void etr_steai_cv(struct etr_aib *aib, unsigned int func)
 546 {
 547         BUG_ON(etr_steai(aib, func) != 0);
 548         /* Convert port state to logical port state. */
 549         if (aib->esw.psc0 == 1)
 550                 aib->esw.psc0 = 2;
 551         else if (aib->esw.psc0 == 0 && aib->esw.p == 0)
 552                 aib->esw.psc0 = 1;
 553         if (aib->esw.psc1 == 1)
 554                 aib->esw.psc1 = 2;
 555         else if (aib->esw.psc1 == 0 && aib->esw.p == 1)
 556                 aib->esw.psc1 = 1;
 557 }
 558
 559 /*
 560  * Check if the aib a2 is still connected to the same attachment as
 561  * aib a1, the etv values differ by one and a2 is valid.
 562  */
 563 static int etr_aib_follows(struct etr_aib *a1, struct etr_aib *a2, int p)
 564 {
 565         int state_a1, state_a2;
 566
 567         /* Paranoia check: e0/e1 should better be the same. */
 568         if (a1->esw.eacr.e0 != a2->esw.eacr.e0 ||
 569             a1->esw.eacr.e1 != a2->esw.eacr.e1)
 570                 return 0;
 571
 572         /* Still connected to the same etr ? */
 573         state_a1 = p ? a1->esw.psc1 : a1->esw.psc0;
 574         state_a2 = p ? a2->esw.psc1 : a2->esw.psc0;
 575         if (state_a1 == etr_lpsc_operational_step) {
 576                 if (state_a2 != etr_lpsc_operational_step ||
 577                     a1->edf1.net_id != a2->edf1.net_id ||
 578                     a1->edf1.etr_id != a2->edf1.etr_id ||
 579                     a1->edf1.etr_pn != a2->edf1.etr_pn)
 580                         return 0;
 581         } else if (state_a2 != etr_lpsc_pps_mode)
 582                 return 0;
 583
 584         /* The ETV value of a2 needs to be ETV of a1 + 1. */
 585         if (a1->edf2.etv + 1 != a2->edf2.etv)
 586                 return 0;
 587
 588         if (!etr_port_valid(a2, p))
 589                 return 0;
 590
 591         return 1;
 592 }
 593
 594 /*
 595  * The time is "clock". old is what we think the time is.
 596  * Adjust the value by a multiple of jiffies and add the delta to ntp.
 597  * "delay" is an approximation how long the synchronization took. If
 598  * the time correction is positive, then "delay" is subtracted from
 599  * the time difference and only the remaining part is passed to ntp.
 600  */
 601 static unsigned long long etr_adjust_time(unsigned long long old,
 602                                           unsigned long long clock,
 603                                           unsigned long long delay)
 604 {
 605         unsigned long long delta, ticks;
 606         struct timex adjust;
 607
 608         if (clock > old) {
 609                 /* It is later than we thought. */
 610                 delta = ticks = clock - old;
 611                 delta = ticks = (delta < delay) ? 0 : delta - delay;
 612                 delta -= do_div(ticks, CLK_TICKS_PER_JIFFY);
 613                 adjust.offset = ticks * (1000000 / HZ);
 614         } else {
 615                 /* It is earlier than we thought. */
 616                 delta = ticks = old - clock;
 617                 delta -= do_div(ticks, CLK_TICKS_PER_JIFFY);
 618                 delta = -delta;
 619                 adjust.offset = -ticks * (1000000 / HZ);
 620         }
 621         jiffies_timer_cc += delta;
 622         if (adjust.offset != 0) {
 623                 printk(KERN_NOTICE "etr: time adjusted by %li micro-seconds\n",
 624                        adjust.offset);
 625                 adjust.modes = ADJ_OFFSET_SINGLESHOT;
 626                 do_adjtimex(&adjust);
 627         }
 628         return delta;
 629 }
 630
 631 static struct {
 632         int in_sync;
 633         unsigned long long fixup_cc;
 634 } etr_sync;
 635
 636 static void etr_sync_cpu_start(void *dummy)
 637 {
 638         etr_enable_sync_clock();
 639         /*
 640          * This looks like a busy wait loop but it isn't. etr_sync_cpus
 641          * is called on all other cpus while the TOD clocks is stopped.
 642          * __udelay will stop the cpu on an enabled wait psw until the
 643          * TOD is running again.
 644          */
 645         while (etr_sync.in_sync == 0) {
 646                 __udelay(1);
 647                 /*
 648                  * A different cpu changes *in_sync. Therefore use
 649                  * barrier() to force memory access.
 650                  */
 651                 barrier();
 652         }
 653         if (etr_sync.in_sync != 1)
 654                 /* Didn't work. Clear per-cpu in sync bit again. */
 655                 etr_disable_sync_clock(NULL);
 656         /*
 657          * This round of TOD syncing is done. Set the clock comparator
 658          * to the next tick and let the processor continue.
 659          */
 660         fixup_clock_comparator(etr_sync.fixup_cc);
 661 }
 662
 663 static void etr_sync_cpu_end(void *dummy)
 664 {
 665 }
 666
 667 /*
 668  * Sync the TOD clock using the port refered to by aibp. This port
 669  * has to be enabled and the other port has to be disabled. The
 670  * last eacr update has to be more than 1.6 seconds in the past.
 671  */
 672 static int etr_sync_clock(struct etr_aib *aib, int port)
 673 {
 674         struct etr_aib *sync_port;
 675         unsigned long long clock, old_clock, delay, delta;
 676         int follows;
 677         int rc;
 678
 679         /* Check if the current aib is adjacent to the sync port aib. */
 680         sync_port = (port == 0) ? &etr_port0 : &etr_port1;
 681         follows = etr_aib_follows(sync_port, aib, port);
 682         memcpy(sync_port, aib, sizeof(*aib));
 683         if (!follows)
 684                 return -EAGAIN;
 685
 686         /*
 687          * Catch all other cpus and make them wait until we have
 688          * successfully synced the clock. smp_call_function will
 689          * return after all other cpus are in etr_sync_cpu_start.
 690          */
 691         memset(&etr_sync, 0, sizeof(etr_sync));
 692         preempt_disable();
 693         smp_call_function(etr_sync_cpu_start, NULL, 0, 0);
 694         local_irq_disable();
 695         etr_enable_sync_clock();
 696
 697         /* Set clock to next OTE. */
 698         __ctl_set_bit(14, 21);
 699         __ctl_set_bit(0, 29);
 700         clock = ((unsigned long long) (aib->edf2.etv + 1)) << 32;
 701         old_clock = get_clock();
 702         if (set_clock(clock) == 0) {
 703                 __udelay(1);    /* Wait for the clock to start. */
 704                 __ctl_clear_bit(0, 29);
 705                 __ctl_clear_bit(14, 21);
 706                 etr_stetr(aib);
 707                 /* Adjust Linux timing variables. */
 708                 delay = (unsigned long long)
 709                         (aib->edf2.etv - sync_port->edf2.etv) << 32;
 710                 delta = etr_adjust_time(old_clock, clock, delay);
 711                 etr_sync.fixup_cc = delta;
 712                 fixup_clock_comparator(delta);
 713                 /* Verify that the clock is properly set. */
 714                 if (!etr_aib_follows(sync_port, aib, port)) {
 715                         /* Didn't work. */
 716                         etr_disable_sync_clock(NULL);
 717                         etr_sync.in_sync = -EAGAIN;
 718                         rc = -EAGAIN;
 719                 } else {
 720                         etr_sync.in_sync = 1;
 721                         rc = 0;
 722                 }
 723         } else {
 724                 /* Could not set the clock ?!? */
 725                 __ctl_clear_bit(0, 29);
 726                 __ctl_clear_bit(14, 21);
 727                 etr_disable_sync_clock(NULL);
 728                 etr_sync.in_sync = -EAGAIN;
 729                 rc = -EAGAIN;
 730         }
 731         local_irq_enable();
 732         smp_call_function(etr_sync_cpu_end,NULL,0,0);
 733         preempt_enable();
 734         return rc;
 735 }
 736
 737 /*
 738  * Handle the immediate effects of the different events.
 739  * The port change event is used for online/offline changes.
 740  */
 741 static struct etr_eacr etr_handle_events(struct etr_eacr eacr)
 742 {
 743         if (test_and_clear_bit(ETR_EVENT_SYNC_CHECK, &etr_events))
 744                 eacr.es = 0;
 745         if (test_and_clear_bit(ETR_EVENT_SWITCH_LOCAL, &etr_events))
 746                 eacr.es = eacr.sl = 0;
 747         if (test_and_clear_bit(ETR_EVENT_PORT_ALERT, &etr_events))
 748                 etr_port0_uptodate = etr_port1_uptodate = 0;
 749
 750         if (test_and_clear_bit(ETR_EVENT_PORT0_CHANGE, &etr_events)) {
 751                 if (eacr.e0)
 752                         /*
 753                          * Port change of an enabled port. We have to
 754                          * assume that this can have caused an stepping
 755                          * port switch.
 756                          */
 757                         etr_tolec = get_clock();
 758                 eacr.p0 = etr_port0_online;
 759                 if (!eacr.p0)
 760                         eacr.e0 = 0;
 761                 etr_port0_uptodate = 0;
 762         }
 763         if (test_and_clear_bit(ETR_EVENT_PORT1_CHANGE, &etr_events)) {
 764                 if (eacr.e1)
 765                         /*
 766                          * Port change of an enabled port. We have to
 767                          * assume that this can have caused an stepping
 768                          * port switch.
 769                          */
 770                         etr_tolec = get_clock();
 771                 eacr.p1 = etr_port1_online;
 772                 if (!eacr.p1)
 773                         eacr.e1 = 0;
 774                 etr_port1_uptodate = 0;
 775         }
 776         clear_bit(ETR_EVENT_UPDATE, &etr_events);
 777         return eacr;
 778 }
 779
 780 /*
 781  * Set up a timer that expires after the etr_tolec + 1.6 seconds if
 782  * one of the ports needs an update.
 783  */
 784 static void etr_set_tolec_timeout(unsigned long long now)
 785 {
 786         unsigned long micros;
 787
 788         if ((!etr_eacr.p0 || etr_port0_uptodate) &&
 789             (!etr_eacr.p1 || etr_port1_uptodate))
 790                 return;
 791         micros = (now > etr_tolec) ? ((now - etr_tolec) >> 12) : 0;
 792         micros = (micros > 1600000) ? 0 : 1600000 - micros;
 793         mod_timer(&etr_timer, jiffies + (micros * HZ) / 1000000 + 1);
 794 }
 795
 796 /*
 797  * Set up a time that expires after 1/2 second.
 798  */
 799 static void etr_set_sync_timeout(void)
 800 {
 801         mod_timer(&etr_timer, jiffies + HZ/2);
 802 }
 803
 804 /*
 805  * Update the aib information for one or both ports.
 806  */
 807 static struct etr_eacr etr_handle_update(struct etr_aib *aib,
 808                                          struct etr_eacr eacr)
 809 {
 810         /* With both ports disabled the aib information is useless. */
 811         if (!eacr.e0 && !eacr.e1)
 812                 return eacr;
 813
 814         /* Update port0 or port1 with aib stored in etr_work_fn. */
 815         if (aib->esw.q == 0) {
 816                 /* Information for port 0 stored. */
 817                 if (eacr.p0 && !etr_port0_uptodate) {
 818                         etr_port0 = *aib;
 819                         if (etr_port0_online)
 820                                 etr_port0_uptodate = 1;
 821                 }
 822         } else {
 823                 /* Information for port 1 stored. */
 824                 if (eacr.p1 && !etr_port1_uptodate) {
 825                         etr_port1 = *aib;
 826                         if (etr_port0_online)
 827                                 etr_port1_uptodate = 1;
 828                 }
 829         }
 830
 831         /*
 832          * Do not try to get the alternate port aib if the clock
 833          * is not in sync yet.
 834          */
 835         if (!eacr.es)
 836                 return eacr;
 837
 838         /*
 839          * If steai is available we can get the information about
 840          * the other port immediately. If only stetr is available the
 841          * data-port bit toggle has to be used.
 842          */
 843         if (test_bit(ETR_FLAG_STEAI, &etr_flags)) {
 844                 if (eacr.p0 && !etr_port0_uptodate) {
 845                         etr_steai_cv(&etr_port0, ETR_STEAI_PORT_0);
 846                         etr_port0_uptodate = 1;
 847                 }
 848                 if (eacr.p1 && !etr_port1_uptodate) {
 849                         etr_steai_cv(&etr_port1, ETR_STEAI_PORT_1);
 850                         etr_port1_uptodate = 1;
 851                 }
 852         } else {
 853                 /*
 854                  * One port was updated above, if the other
 855                  * port is not uptodate toggle dp bit.
 856                  */
 857                 if ((eacr.p0 && !etr_port0_uptodate) ||
 858                     (eacr.p1 && !etr_port1_uptodate))
 859                         eacr.dp ^= 1;
 860                 else
 861                         eacr.dp = 0;
 862         }
 863         return eacr;
 864 }
 865
 866 /*
 867  * Write new etr control register if it differs from the current one.
 868  * Return 1 if etr_tolec has been updated as well.
 869  */
 870 static void etr_update_eacr(struct etr_eacr eacr)
 871 {
 872         int dp_changed;
 873
 874         if (memcmp(&etr_eacr, &eacr, sizeof(eacr)) == 0)
 875                 /* No change, return. */
 876                 return;
 877         /*
 878          * The disable of an active port of the change of the data port
 879          * bit can/will cause a change in the data port.
 880          */
 881         dp_changed = etr_eacr.e0 > eacr.e0 || etr_eacr.e1 > eacr.e1 ||
 882                 (etr_eacr.dp ^ eacr.dp) != 0;
 883         etr_eacr = eacr;
 884         etr_setr(&etr_eacr);
 885         if (dp_changed)
 886                 etr_tolec = get_clock();
 887 }
 888
 889 /*
 890  * ETR tasklet. In this function you'll find the main logic. In
 891  * particular this is the only function that calls etr_update_eacr(),
 892  * it "controls" the etr control register.
 893  */
 894 static void etr_work_fn(struct work_struct *work)
 895 {
 896         unsigned long long now;
 897         struct etr_eacr eacr;
 898         struct etr_aib aib;
 899         int sync_port;
 900
 901         /* Create working copy of etr_eacr. */
 902         eacr = etr_eacr;
 903
 904         /* Check for the different events and their immediate effects. */
 905         eacr = etr_handle_events(eacr);
 906
 907         /* Check if ETR is supposed to be active. */
 908         eacr.ea = eacr.p0 || eacr.p1;
 909         if (!eacr.ea) {
 910                 /* Both ports offline. Reset everything. */
 911                 eacr.dp = eacr.es = eacr.sl = 0;
 912                 on_each_cpu(etr_disable_sync_clock, NULL, 0, 1);
 913                 del_timer_sync(&etr_timer);
 914                 etr_update_eacr(eacr);
 915                 set_bit(ETR_FLAG_EACCES, &etr_flags);
 916                 return;
 917         }
 918
 919         /* Store aib to get the current ETR status word. */
 920         BUG_ON(etr_stetr(&aib) != 0);
 921         etr_port0.esw = etr_port1.esw = aib.esw;        /* Copy status word. */
 922         now = get_clock();
 923
 924         /*
 925          * Update the port information if the last stepping port change
 926          * or data port change is older than 1.6 seconds.
 927          */
 928         if (now >= etr_tolec + (1600000 << 12))
 929                 eacr = etr_handle_update(&aib, eacr);
 930
 931         /*
 932          * Select ports to enable. The prefered synchronization mode is PPS.
 933          * If a port can be enabled depends on a number of things:
 934          * 1) The port needs to be online and uptodate. A port is not
 935          *    disabled just because it is not uptodate, but it is only
 936          *    enabled if it is uptodate.
 937          * 2) The port needs to have the same mode (pps / etr).
 938          * 3) The port needs to be usable -> etr_port_valid() == 1
 939          * 4) To enable the second port the clock needs to be in sync.
 940          * 5) If both ports are useable and are ETR ports, the network id
 941          *    has to be the same.
 942          * The eacr.sl bit is used to indicate etr mode vs. pps mode.
 943          */
 944         if (eacr.p0 && aib.esw.psc0 == etr_lpsc_pps_mode) {
 945                 eacr.sl = 0;
 946                 eacr.e0 = 1;
 947                 if (!etr_mode_is_pps(etr_eacr))
 948                         eacr.es = 0;
 949                 if (!eacr.es || !eacr.p1 || aib.esw.psc1 != etr_lpsc_pps_mode)
 950                         eacr.e1 = 0;
 951                 // FIXME: uptodate checks ?
 952                 else if (etr_port0_uptodate && etr_port1_uptodate)
 953                         eacr.e1 = 1;
 954                 sync_port = (etr_port0_uptodate &&
 955                              etr_port_valid(&etr_port0, 0)) ? 0 : -1;
 956                 clear_bit(ETR_FLAG_EACCES, &etr_flags);
 957         } else if (eacr.p1 && aib.esw.psc1 == etr_lpsc_pps_mode) {
 958                 eacr.sl = 0;
 959                 eacr.e0 = 0;
 960                 eacr.e1 = 1;
 961                 if (!etr_mode_is_pps(etr_eacr))
 962                         eacr.es = 0;
 963                 sync_port = (etr_port1_uptodate &&
 964                              etr_port_valid(&etr_port1, 1)) ? 1 : -1;
 965                 clear_bit(ETR_FLAG_EACCES, &etr_flags);
 966         } else if (eacr.p0 && aib.esw.psc0 == etr_lpsc_operational_step) {
 967                 eacr.sl = 1;
 968                 eacr.e0 = 1;
 969                 if (!etr_mode_is_etr(etr_eacr))
 970                         eacr.es = 0;
 971                 if (!eacr.es || !eacr.p1 ||
 972                     aib.esw.psc1 != etr_lpsc_operational_alt)
 973                         eacr.e1 = 0;
 974                 else if (etr_port0_uptodate && etr_port1_uptodate &&
 975                          etr_compare_network(&etr_port0, &etr_port1))
 976                         eacr.e1 = 1;
 977                 sync_port = (etr_port0_uptodate &&
 978                              etr_port_valid(&etr_port0, 0)) ? 0 : -1;
 979                 clear_bit(ETR_FLAG_EACCES, &etr_flags);
 980         } else if (eacr.p1 && aib.esw.psc1 == etr_lpsc_operational_step) {
 981                 eacr.sl = 1;
 982                 eacr.e0 = 0;
 983                 eacr.e1 = 1;
 984                 if (!etr_mode_is_etr(etr_eacr))
 985                         eacr.es = 0;
 986                 sync_port = (etr_port1_uptodate &&
 987                              etr_port_valid(&etr_port1, 1)) ? 1 : -1;
 988                 clear_bit(ETR_FLAG_EACCES, &etr_flags);
 989         } else {
 990                 /* Both ports not usable. */
 991                 eacr.es = eacr.sl = 0;
 992                 sync_port = -1;
 993                 set_bit(ETR_FLAG_EACCES, &etr_flags);
 994         }
 995
 996         /*
 997          * If the clock is in sync just update the eacr and return.
 998          * If there is no valid sync port wait for a port update.
 999          */
1000         if (eacr.es || sync_port < 0) {
1001                 etr_update_eacr(eacr);
1002                 etr_set_tolec_timeout(now);
1003                 return;
1004         }
1005
1006         /*
1007          * Prepare control register for clock syncing
1008          * (reset data port bit, set sync check control.
1009          */
1010         eacr.dp = 0;
1011         eacr.es = 1;
1012
1013         /*
1014          * Update eacr and try to synchronize the clock. If the update
1015          * of eacr caused a stepping port switch (or if we have to
1016          * assume that a stepping port switch has occured) or the
1017          * clock syncing failed, reset the sync check control bit
1018          * and set up a timer to try again after 0.5 seconds
1019          */
1020         etr_update_eacr(eacr);
1021         if (now < etr_tolec + (1600000 << 12) ||
1022             etr_sync_clock(&aib, sync_port) != 0) {
1023                 /* Sync failed. Try again in 1/2 second. */
1024                 eacr.es = 0;
1025                 etr_update_eacr(eacr);
1026                 etr_set_sync_timeout();
1027         } else
1028                 etr_set_tolec_timeout(now);
1029 }
1030
1031 /*
1032  * Sysfs interface functions
1033  */
1034 static struct sysdev_class etr_sysclass = {
1035         .name   = "etr",
1036 };
1037
1038 static struct sys_device etr_port0_dev = {
1039         .id     = 0,
1040         .cls    = &etr_sysclass,
1041 };
1042
1043 static struct sys_device etr_port1_dev = {
1044         .id     = 1,
1045         .cls    = &etr_sysclass,
1046 };
1047
1048 /*
1049  * ETR class attributes
1050  */
1051 static ssize_t etr_stepping_port_show(struct sysdev_class *class, char *buf)
1052 {
1053         return sprintf(buf, "%i\n", etr_port0.esw.p);
1054 }
1055
1056 static SYSDEV_CLASS_ATTR(stepping_port, 0400, etr_stepping_port_show, NULL);
1057
1058 static ssize_t etr_stepping_mode_show(struct sysdev_class *class, char *buf)
1059 {
1060         char *mode_str;
1061
1062         if (etr_mode_is_pps(etr_eacr))
1063                 mode_str = "pps";
1064         else if (etr_mode_is_etr(etr_eacr))
1065                 mode_str = "etr";
1066         else
1067                 mode_str = "local";
1068         return sprintf(buf, "%s\n", mode_str);
1069 }
1070
1071 static SYSDEV_CLASS_ATTR(stepping_mode, 0400, etr_stepping_mode_show, NULL);
1072
1073 /*
1074  * ETR port attributes
1075  */
1076 static inline struct etr_aib *etr_aib_from_dev(struct sys_device *dev)
1077 {
1078         if (dev == &etr_port0_dev)
1079                 return etr_port0_online ? &etr_port0 : NULL;
1080         else
1081                 return etr_port1_online ? &etr_port1 : NULL;
1082 }
1083
1084 static ssize_t etr_online_show(struct sys_device *dev, char *buf)
1085 {
1086         unsigned int online;
1087
1088         online = (dev == &etr_port0_dev) ? etr_port0_online : etr_port1_online;
1089         return sprintf(buf, "%i\n", online);
1090 }
1091
1092 static ssize_t etr_online_store(struct sys_device *dev,
1093                               const char *buf, size_t count)
1094 {
1095         unsigned int value;
1096
1097         value = simple_strtoul(buf, NULL, 0);
1098         if (value != 0 && value != 1)
1099                 return -EINVAL;
1100         if (test_bit(ETR_FLAG_ENOSYS, &etr_flags))
1101                 return -ENOSYS;
1102         if (dev == &etr_port0_dev) {
1103                 if (etr_port0_online == value)
1104                         return count;   /* Nothing to do. */
1105                 etr_port0_online = value;
1106                 set_bit(ETR_EVENT_PORT0_CHANGE, &etr_events);
1107                 schedule_work(&etr_work);
1108         } else {
1109                 if (etr_port1_online == value)
1110                         return count;   /* Nothing to do. */
1111                 etr_port1_online = value;
1112                 set_bit(ETR_EVENT_PORT1_CHANGE, &etr_events);
1113                 schedule_work(&etr_work);
1114         }
1115         return count;
1116 }
1117
1118 static SYSDEV_ATTR(online, 0600, etr_online_show, etr_online_store);
1119
1120 static ssize_t etr_stepping_control_show(struct sys_device *dev, char *buf)
1121 {
1122         return sprintf(buf, "%i\n", (dev == &etr_port0_dev) ?
1123                        etr_eacr.e0 : etr_eacr.e1);
1124 }
1125
1126 static SYSDEV_ATTR(stepping_control, 0400, etr_stepping_control_show, NULL);
1127
1128 static ssize_t etr_mode_code_show(struct sys_device *dev, char *buf)
1129 {
1130         if (!etr_port0_online && !etr_port1_online)
1131                 /* Status word is not uptodate if both ports are offline. */
1132                 return -ENODATA;
1133         return sprintf(buf, "%i\n", (dev == &etr_port0_dev) ?
1134                        etr_port0.esw.psc0 : etr_port0.esw.psc1);
1135 }
1136
1137 static SYSDEV_ATTR(state_code, 0400, etr_mode_code_show, NULL);
1138
1139 static ssize_t etr_untuned_show(struct sys_device *dev, char *buf)
1140 {
1141         struct etr_aib *aib = etr_aib_from_dev(dev);
1142
1143         if (!aib || !aib->slsw.v1)
1144                 return -ENODATA;
1145         return sprintf(buf, "%i\n", aib->edf1.u);
1146 }
1147
1148 static SYSDEV_ATTR(untuned, 0400, etr_untuned_show, NULL);
1149
1150 static ssize_t etr_network_id_show(struct sys_device *dev, char *buf)
1151 {
1152         struct etr_aib *aib = etr_aib_from_dev(dev);
1153
1154         if (!aib || !aib->slsw.v1)
1155                 return -ENODATA;
1156         return sprintf(buf, "%i\n", aib->edf1.net_id);
1157 }
1158
1159 static SYSDEV_ATTR(network, 0400, etr_network_id_show, NULL);
1160
1161 static ssize_t etr_id_show(struct sys_device *dev, char *buf)
1162 {
1163         struct etr_aib *aib = etr_aib_from_dev(dev);
1164
1165         if (!aib || !aib->slsw.v1)
1166                 return -ENODATA;
1167         return sprintf(buf, "%i\n", aib->edf1.etr_id);
1168 }
1169
1170 static SYSDEV_ATTR(id, 0400, etr_id_show, NULL);
1171
1172 static ssize_t etr_port_number_show(struct sys_device *dev, char *buf)
1173 {
1174         struct etr_aib *aib = etr_aib_from_dev(dev);
1175
1176         if (!aib || !aib->slsw.v1)
1177                 return -ENODATA;
1178         return sprintf(buf, "%i\n", aib->edf1.etr_pn);
1179 }
1180
1181 static SYSDEV_ATTR(port, 0400, etr_port_number_show, NULL);
1182
1183 static ssize_t etr_coupled_show(struct sys_device *dev, char *buf)
1184 {
1185         struct etr_aib *aib = etr_aib_from_dev(dev);
1186
1187         if (!aib || !aib->slsw.v3)
1188                 return -ENODATA;
1189         return sprintf(buf, "%i\n", aib->edf3.c);
1190 }
1191
1192 static SYSDEV_ATTR(coupled, 0400, etr_coupled_show, NULL);
1193
1194 static ssize_t etr_local_time_show(struct sys_device *dev, char *buf)
1195 {
1196         struct etr_aib *aib = etr_aib_from_dev(dev);
1197
1198         if (!aib || !aib->slsw.v3)
1199                 return -ENODATA;
1200         return sprintf(buf, "%i\n", aib->edf3.blto);
1201 }
1202
1203 static SYSDEV_ATTR(local_time, 0400, etr_local_time_show, NULL);
1204
1205 static ssize_t etr_utc_offset_show(struct sys_device *dev, char *buf)
1206 {
1207         struct etr_aib *aib = etr_aib_from_dev(dev);
1208
1209         if (!aib || !aib->slsw.v3)
1210                 return -ENODATA;
1211         return sprintf(buf, "%i\n", aib->edf3.buo);
1212 }
1213
1214 static SYSDEV_ATTR(utc_offset, 0400, etr_utc_offset_show, NULL);
1215
1216 static struct sysdev_attribute *etr_port_attributes[] = {
1217         &attr_online,
1218         &attr_stepping_control,
1219         &attr_state_code,
1220         &attr_untuned,
1221         &attr_network,
1222         &attr_id,
1223         &attr_port,
1224         &attr_coupled,
1225         &attr_local_time,
1226         &attr_utc_offset,
1227         NULL
1228 };
1229
1230 static int __init etr_register_port(struct sys_device *dev)
1231 {
1232         struct sysdev_attribute **attr;
1233         int rc;
1234
1235         rc = sysdev_register(dev);
1236         if (rc)
1237                 goto out;
1238         for (attr = etr_port_attributes; *attr; attr++) {
1239                 rc = sysdev_create_file(dev, *attr);
1240                 if (rc)
1241                         goto out_unreg;
1242         }
1243         return 0;
1244 out_unreg:
1245         for (; attr >= etr_port_attributes; attr--)
1246                 sysdev_remove_file(dev, *attr);
1247         sysdev_unregister(dev);
1248 out:
1249         return rc;
1250 }
1251
1252 static void __init etr_unregister_port(struct sys_device *dev)
1253 {
1254         struct sysdev_attribute **attr;
1255
1256         for (attr = etr_port_attributes; *attr; attr++)
1257                 sysdev_remove_file(dev, *attr);
1258         sysdev_unregister(dev);
1259 }
1260
1261 static int __init etr_init_sysfs(void)
1262 {
1263         int rc;
1264
1265         rc = sysdev_class_register(&etr_sysclass);
1266         if (rc)
1267                 goto out;
1268         rc = sysdev_class_create_file(&etr_sysclass, &attr_stepping_port);
1269         if (rc)
1270                 goto out_unreg_class;
1271         rc = sysdev_class_create_file(&etr_sysclass, &attr_stepping_mode);
1272         if (rc)
1273                 goto out_remove_stepping_port;
1274         rc = etr_register_port(&etr_port0_dev);
1275         if (rc)
1276                 goto out_remove_stepping_mode;
1277         rc = etr_register_port(&etr_port1_dev);
1278         if (rc)
1279                 goto out_remove_port0;
1280         return 0;
1281
1282 out_remove_port0:
1283         etr_unregister_port(&etr_port0_dev);
1284 out_remove_stepping_mode:
1285         sysdev_class_remove_file(&etr_sysclass, &attr_stepping_mode);
1286 out_remove_stepping_port:
1287         sysdev_class_remove_file(&etr_sysclass, &attr_stepping_port);
1288 out_unreg_class:
1289         sysdev_class_unregister(&etr_sysclass);
1290 out:
1291         return rc;
1292 }
1293
1294 device_initcall(etr_init_sysfs);