4 files changed, 43 insertions, 11 deletions

diff --git a/drivers/serial/cpm_uart/cpm_uart.h b/drivers/serial/cpm_uart/cpm_uart.h
index aa5eb7ddeda9..3b35cb779539 100644
--- a/drivers/serial/cpm_uart/cpm_uart.h
+++ b/drivers/serial/cpm_uart/cpm_uart.h
@@ -5,6 +5,13 @@
  *
  *  Copyright (C) 2004 Freescale Semiconductor, Inc.
  *
+ *  2006 (c) MontaVista Software, Inc.
+ *      Vitaly Bordug <vbordug@ru.mvista.com>
+ *
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
+ * kind, whether express or implied.
+ *
  */
 #ifndef CPM_UART_H
 #define CPM_UART_H
@@ -101,12 +108,13 @@ static inline unsigned long cpu2cpm_addr(void* addr, struct uart_cpm_port *pinfo
         int offset;
         u32 val = (u32)addr;
         /* sane check */
-        if ((val >= (u32)pinfo->mem_addr) &&
+        if (likely((val >= (u32)pinfo->mem_addr)) &&
                         (val<((u32)pinfo->mem_addr + pinfo->mem_size))) {
                 offset = val - (u32)pinfo->mem_addr;
                 return pinfo->dma_addr+offset;
         }
-        printk("%s(): address %x to translate out of range!\n", __FUNCTION__, val);
+        /* something nasty happened */
+        BUG();
         return 0;
 }
 
@@ -115,12 +123,13 @@ static inline void *cpm2cpu_addr(unsigned long addr, struct uart_cpm_port *pinfo
         int offset;
         u32 val = addr;
         /* sane check */
-        if ((val >= pinfo->dma_addr) &&
-                        (val<(pinfo->dma_addr + pinfo->mem_size))) {
+        if (likely((val >= pinfo->dma_addr) &&
+                        (val<(pinfo->dma_addr + pinfo->mem_size)))) {
                 offset = val - (u32)pinfo->dma_addr;
                 return (void*)(pinfo->mem_addr+offset);
         }
-        printk("%s(): address %x to translate out of range!\n", __FUNCTION__, val);
+        /* something nasty happened */
+        BUG();
         return 0;
 }
 
diff --git a/drivers/serial/cpm_uart/cpm_uart_core.c b/drivers/serial/cpm_uart/cpm_uart_core.c
index b9d1185df20c..969f94900431 100644
--- a/drivers/serial/cpm_uart/cpm_uart_core.c
+++ b/drivers/serial/cpm_uart/cpm_uart_core.c
@@ -12,7 +12,8 @@
  *
  *  Copyright (C) 2004 Freescale Semiconductor, Inc.
  *            (C) 2004 Intracom, S.A.
- *            (C) 2005 MontaVista Software, Inc. by Vitaly Bordug <vbordug@ru.mvista.com>
+ *            (C) 2005-2006 MontaVista Software, Inc.
+ *              Vitaly Bordug <vbordug@ru.mvista.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -81,7 +82,7 @@ early_uart_get_pdev(int index)
 }
 
 
-void cpm_uart_count(void)
+static void cpm_uart_count(void)
 {
         cpm_uart_nr = 0;
 #ifdef CONFIG_SERIAL_CPM_SMC1
@@ -104,6 +105,21 @@ void cpm_uart_count(void)
 #endif
 }
 
+/* Get UART number by its id */
+static int cpm_uart_id2nr(int id)
+{
+        int i;
+        if (id < UART_NR) {
+                for (i=0; i<UART_NR; i++) {
+                        if (cpm_uart_port_map[i] == id)
+                                return i;
+                }
+        }
+
+        /* not found or invalid argument */
+        return -1;
+}
+
 /*
  * Check, if transmit buffers are processed
 */
@@ -1012,7 +1028,11 @@ int cpm_uart_drv_get_platform_data(struct platform_device *pdev, int is_con)
         int line;
         u32 mem, pram;
 
-        for (line=0; line<UART_NR && cpm_uart_port_map[line]!=pdata->fs_no; line++);
+        line = cpm_uart_id2nr(idx);
+        if(line < 0) {
+                printk(KERN_ERR"%s(): port %d is not registered", __FUNCTION__, idx);
+                return -1;
+        }
 
         pinfo = (struct uart_cpm_port *) &cpm_uart_ports[idx];
 
@@ -1245,8 +1265,7 @@ static int cpm_uart_drv_probe(struct device *dev)
         }
 
         pdata = pdev->dev.platform_data;
-        pr_debug("cpm_uart_drv_probe: Adding CPM UART %d\n",
-                        cpm_uart_port_map[pdata->fs_no]);
+        pr_debug("cpm_uart_drv_probe: Adding CPM UART %d\n", cpm_uart_id2nr(pdata->fs_no));
 
         if ((ret = cpm_uart_drv_get_platform_data(pdev, 0)))
                 return ret;
@@ -1265,7 +1284,7 @@ static int cpm_uart_drv_remove(struct device *dev)
         struct fs_uart_platform_info *pdata = pdev->dev.platform_data;
 
         pr_debug("cpm_uart_drv_remove: Removing CPM UART %d\n",
-                        cpm_uart_port_map[pdata->fs_no]);
+                        cpm_uart_id2nr(pdata->fs_no));
 
         uart_remove_one_port(&cpm_reg, &cpm_uart_ports[pdata->fs_no].port);
         return 0;
diff --git a/drivers/serial/cpm_uart/cpm_uart_cpm1.c b/drivers/serial/cpm_uart/cpm_uart_cpm1.c
index a5a30622637a..17406a05ce1f 100644
--- a/drivers/serial/cpm_uart/cpm_uart_cpm1.c
+++ b/drivers/serial/cpm_uart/cpm_uart_cpm1.c
@@ -8,6 +8,8 @@
  *
  *  Copyright (C) 2004 Freescale Semiconductor, Inc.
  *            (C) 2004 Intracom, S.A.
+ *            (C) 2006 MontaVista Software, Inc.
+ *              Vitaly Bordug <vbordug@ru.mvista.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
diff --git a/drivers/serial/cpm_uart/cpm_uart_cpm2.c b/drivers/serial/cpm_uart/cpm_uart_cpm2.c
index 7c6b07aeea92..4b2de08f46d0 100644
--- a/drivers/serial/cpm_uart/cpm_uart_cpm2.c
+++ b/drivers/serial/cpm_uart/cpm_uart_cpm2.c
@@ -8,6 +8,8 @@
  * 
  *  Copyright (C) 2004 Freescale Semiconductor, Inc.
  *            (C) 2004 Intracom, S.A.
+ *            (C) 2006 MontaVista Software, Inc.
+ *              Vitaly Bordug <vbordug@ru.mvista.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by